added gps project

[utils] / gps / src / main / java / org / wamblee / gps / geometry / Plane.java

diff --git a/gps/src/main/java/org/wamblee/gps/geometry/Plane.java b/gps/src/main/java/org/wamblee/gps/geometry/Plane.java
new file mode 100644 (file)
index 0000000..1be9b54
--- /dev/null
+++ b/gps/src/main/java/org/wamblee/gps/geometry/Plane.java
@@ -0,0 +1,73 @@
+/*
+ * Copyright 2006 the original author or authors.
+ * 
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * 
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ * 
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */ 
+
+package org.wamblee.gps.geometry;
+
+import org.wamblee.general.Pair;
+
+/**
+ * Represents a plane. Usually used to represent a tangent plane to the earth surface to
+ * locally approximate the earth as flat.  
+ */
+public class Plane {
+    
+    private static final double EPS = 1e-4;
+    
+    private Coordinates _point; 
+    private Coordinates _normal; 
+    private Coordinates _north;
+    private Coordinates _east;
+
+    /**
+     * Constructs a plane. 
+     * @param aPoint Point on the plane. 
+     * @param aNormal Normal, not necessarily normalized. 
+     */
+    public Plane(Point aPoint, Point aNormal) {
+        _point = aPoint.getReferenceCoordinates(); 
+        _normal = aNormal.getReferenceCoordinates().normalize();
+        Coordinates north = new Coordinates(0.0, 0.0, 1.0);
+        _north = north.subtract(_normal.scale(north.innerProduct(_normal))).normalize();
+        _east = _north.outerProduct(_normal);
+      
+        if (  _normal.innerProduct(_north) > EPS ) {
+            throw new IllegalArgumentException("North access is not within the plane");
+        }
+    }
+    
+    /**
+     * Projects a point onto the plane.  
+     * @param aPoint Point to project. 
+     * @return Projected point. 
+     */
+    private Coordinates project(Point aPoint) { 
+        Coordinates ref = aPoint.getReferenceCoordinates();
+        double lambda = _normal.innerProduct(
+                _point.subtract(ref));
+        return ref.add(_normal.scale(lambda));
+    }
+    
+    /**
+     * Returns normalized coordinates within the plane of the projection of a point. 
+     */
+    public Pair<Double,Double> normalizedProjection(Point aPoint) { 
+        Coordinates projection = project(aPoint);
+        Coordinates delta = projection.subtract(_point);
+        double x1 = delta.innerProduct(_north);
+        double x2 = delta.innerProduct(_east);
+        return new Pair<Double,Double>(x1, x2);
+    }
+}