--- /dev/null
+/*
+ * 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;
+
+
+/**
+ * Represents a coordinate system.
+ */
+public interface CoordinateSystem {
+
+ /**
+ * Conversion to a reference coordinate system.
+ * @param aCoordinates Coordinates.
+ * @return Coordinates in the reference system.
+ */
+ Coordinates toReferenceSystem(Coordinates aCoordinates);
+}
--- /dev/null
+/*
+ * 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;
+
+/**
+ * Coordinates in some 3-dimensional coordinate system.
+ */
+public class Coordinates {
+
+ private double _x1;
+ private double _x2;
+ private double _x3;
+
+ /**
+ * Constructs the coordinates.
+ * @param aX1 First coordinate.
+ * @param aX2 Second coordinate.
+ * @param aX3 Third coordinate.
+ */
+ public Coordinates(double aX1, double aX2, double aX3) {
+ _x1 = aX1;
+ _x2 = aX2;
+ _x3 = aX3;
+ }
+
+ public double getX1() {
+ return _x1;
+ }
+
+ public double getX2() {
+ return _x2;
+ }
+
+ public double getX3() {
+ return _x3;
+ }
+
+ /* (non-Javadoc)
+ * @see java.lang.Object#toString()
+ */
+ @Override
+ public String toString() {
+ return "(" + getX1() + ", " + getX2() + ", " + getX3() + ")";
+ }
+
+ public Coordinates add(Coordinates aC) {
+ return new Coordinates(_x1 + aC._x1, _x2 + aC._x2, _x3 + aC._x3);
+ }
+
+ public Coordinates subtract(Coordinates aC) {
+ return new Coordinates(_x1 - aC._x1, _x2 - aC._x2, _x3 - aC._x3);
+ }
+
+ public double innerProduct(Coordinates aC) {
+ return _x1 * aC._x1 + _x2 * aC._x2 + _x3 * aC._x3;
+ }
+
+ public Coordinates outerProduct(Coordinates aC) {
+ return new Coordinates(
+ _x2*aC._x3 - _x3*aC._x2,
+ -_x1*aC._x3 + _x3*aC._x1,
+ _x1*aC._x2 - _x2*aC._x1
+ );
+ }
+
+ public double norm() {
+ return Math.sqrt(innerProduct(this));
+ }
+
+ public Coordinates scale(double aMultiplier) {
+ return new Coordinates(_x1*aMultiplier, _x2*aMultiplier, _x3*aMultiplier);
+ }
+
+ public Coordinates normalize() {
+ return scale(1.0/norm());
+ }
+}
--- /dev/null
+/*
+ * 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);
+ }
+}
--- /dev/null
+/*
+ * 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;
+
+
+/**
+ * Represents a point in some coordinate system.
+ */
+public class Point {
+
+ private Coordinates _coordinates;
+ private CoordinateSystem _system;
+
+ /**
+ * Constructs the point.
+ * @param aCoordinates Coordinates of the point in its coordinate system.
+ * @param aSystem Coordinate system.
+ */
+ public Point(Coordinates aCoordinates, CoordinateSystem aSystem) {
+ _coordinates = aCoordinates;
+ _system = aSystem;
+ }
+
+ /**
+ * Gets the coordinates in the point's coordinate system.
+ * @return Coordinates.
+ */
+ public Coordinates getCoordinates() {
+ return _coordinates;
+ }
+
+ public Coordinates getReferenceCoordinates() {
+ return _system.toReferenceSystem(_coordinates);
+ }
+
+ /**
+ * Gets the coordinate system.
+ * @return Coordinate system.
+ */
+ public CoordinateSystem getCoordinateSystem() {
+ return _system;
+ }
+
+
+ /* (non-Javadoc)
+ * @see java.lang.Object#toString()
+ */
+ @Override
+ public String toString() {
+ return getCoordinates().toString();
+ }
+}
--- /dev/null
+/*
+ * 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;
+
+
+/**
+ * Reference coordinate system which is the basis for defining metrics.
+ * This is a Cartesian coordinate system.
+ */
+public class ReferenceCoordinateSystem implements CoordinateSystem {
+
+ /**
+ * Constructs the coordinate system.
+ *
+ */
+ public ReferenceCoordinateSystem() {
+ // Empty
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see org.wamblee.gpx.CoordinateSystem#toReferenceSystem(org.wamblee.gpx.Coordinates)
+ */
+ public Coordinates toReferenceSystem(Coordinates aCoordinates) {
+ return aCoordinates;
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see org.wamblee.gpx.CoordinateSystem#distance(org.wamblee.gpx.Coordinates,
+ * org.wamblee.gpx.Coordinates)
+ */
+ private static double distance(Coordinates aC1, Coordinates aC2) {
+ return Math.sqrt(square(aC1.getX1() - aC2.getX1())
+ + square(aC1.getX2() - aC2.getX2())
+ + square(aC1.getX3() - aC2.getX3()));
+ }
+
+ private static double square(double x) {
+ return x * x;
+ }
+
+ /**
+ * Computes the distance between two points in arbitrary coordinate systems.
+ * @param aP1 First point.
+ * @param aP2 Second point.
+ * @return Distance.
+ */
+ public static double distance(Point aP1, Point aP2) {
+ return distance( aP1.getCoordinateSystem().toReferenceSystem(aP1.getCoordinates()),
+ aP2.getCoordinateSystem().toReferenceSystem(aP2.getCoordinates()));
+ }
+
+}
--- /dev/null
+/*
+ * 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 static java.lang.Math.PI;
+import static java.lang.Math.cos;
+import static java.lang.Math.sin;
+
+
+/**
+ * Represents the coordinate system for a GPS measurement identified by
+ * <ul>
+ * <li> x1: latitude in degrees </li>
+ * <li> x2: longitude in degrees </li>
+ * <li> x3: elevation in meters </li>
+ * </ul>
+ * This coordinate system models the earth as a sphere of a specific radius.
+ */
+public class SphericalCoordinateSystem implements CoordinateSystem {
+ /**
+ * Earth radius in meters.
+ */
+ private static final double EARTH_RADIUS = 6371000;
+
+
+ /* (non-Javadoc)
+ * @see org.wamblee.gpx.CoordinateSystem#toReferenceSystem(org.wamblee.gpx.Coordinates)
+ */
+ public Coordinates toReferenceSystem(Coordinates aCoordinates) {
+ double latrad = radians(aCoordinates.getX1());
+ double lonrad = radians(aCoordinates.getX2());
+ double coslat = cos(latrad);
+ double sinlat = sin(latrad);
+ double coslon = cos(lonrad);
+ double sinlon = sin(lonrad);
+
+ double trueElevation = EARTH_RADIUS + aCoordinates.getX3();
+ return new Coordinates(trueElevation*coslat*coslon,
+ trueElevation*coslat*sinlon,
+ trueElevation*sinlat);
+
+ }
+
+ private double radians(double aDegrees) {
+ return aDegrees/180.0*PI;
+ }
+}
--- /dev/null
+/*
+ * 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 static java.lang.Math.PI;
+import static java.lang.Math.cos;
+import static java.lang.Math.sin;
+
+
+/**
+ * Represents the WGS 84 coordinate system for a GPS measurement identified by
+ * <ul>
+ * <li> x1: latitude in degrees </li>
+ * <li> x2: longitude in degrees </li>
+ * <li> x3: elevation in meters </li>
+ * </ul>
+ * WGS84 models the earth as an ellipse.
+ */
+public class Wgs84CoordinateSystem implements CoordinateSystem {
+ /*
+ * Ellipsoide parameters, where the ellipsoide is defined by
+ *
+ * (x^2 + y^2)/a^2 + z^2/b^2 = 1
+ */
+
+ /**
+ * The radius of the ellipse at the equator
+ */
+ private static final double A = 6378137.000;
+
+ /**
+ * The distance of the North and South poles to the center of the ellipsoide.
+ */
+ private static final double B = 6356752.314;
+
+
+ /* (non-Javadoc)
+ * @see org.wamblee.gpx.CoordinateSystem#toReferenceSystem(org.wamblee.gpx.Coordinates)
+ */
+ public Coordinates toReferenceSystem(Coordinates aCoordinates) {
+ double latrad = radians(aCoordinates.getX1());
+ double lonrad = radians(aCoordinates.getX2());
+ double coslat = cos(latrad);
+ double sinlat = sin(latrad);
+ double coslon = cos(lonrad);
+ double sinlon = sin(lonrad);
+
+ double r = A*B/Math.sqrt(B*B*coslat*coslat + A*A*sinlat*sinlat) + aCoordinates.getX3();
+
+ return new Coordinates(r*coslat*coslon,
+ r*coslat*sinlon,
+ r*sinlat);
+
+ }
+
+ private double radians(double aDegrees) {
+ return aDegrees/180.0*PI;
+ }
+}
git://wamblee.org / utils / commitdiff