-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
ULog v1 support, various fixes and cleanup, jmalib-math module added
- Loading branch information
Showing
9 changed files
with
266 additions
and
29 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,13 @@ | ||
| <?xml version="1.0" encoding="UTF-8"?> | ||
| <project xmlns="http://maven.apache.org/POM/4.0.0" | ||
| xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" | ||
| xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd"> | ||
| <parent> | ||
| <artifactId>jmalib</artifactId> | ||
| <groupId>com.microavia</groupId> | ||
| <version>0.1.0-SNAPSHOT</version> | ||
| </parent> | ||
| <modelVersion>4.0.0</modelVersion> | ||
|
|
||
| <artifactId>jmalib-math</artifactId> | ||
| </project> |
71 changes: 71 additions & 0 deletions
71
jmalib-math/src/main/java/com/microavia/jmalib/math/GlobalPositionProjector.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,71 @@ | ||
| package com.microavia.jmalib.math; | ||
|
|
||
| import static java.lang.Math.*; | ||
|
|
||
| /** | ||
| * User: ton Date: 11.07.13 Time: 22:11 | ||
| */ | ||
| public class GlobalPositionProjector { | ||
| private static double r_earth = 6371000.0; | ||
| private boolean inited; | ||
| private double lat0; | ||
| private double lon0; | ||
| private double alt0; | ||
| private double cos_lat0; | ||
| private double sin_lat0; | ||
|
|
||
| public void reset() { | ||
| inited = false; | ||
| } | ||
|
|
||
| public boolean isInited() { | ||
| return inited; | ||
| } | ||
|
|
||
| public void init(LatLonAlt ref) { | ||
| inited = true; | ||
| lat0 = ref.lat * PI / 180.0; | ||
| lon0 = ref.lon * PI / 180.0; | ||
| alt0 = ref.alt; | ||
| cos_lat0 = cos(lat0); | ||
| sin_lat0 = sin(lat0); | ||
| } | ||
|
|
||
| public double[] project(LatLonAlt p) { | ||
| if (!inited) { | ||
| throw new RuntimeException("Not initialized"); | ||
| } | ||
| double lat_rad = p.lat * PI / 180.0; | ||
| double lon_rad = p.lon * PI / 180.0; | ||
| double sin_lat = sin(lat_rad); | ||
| double cos_lat = cos(lat_rad); | ||
| double cos_d_lon = cos(lon_rad - lon0); | ||
| double c = acos(sin_lat0 * sin_lat + cos_lat0 * cos_lat * cos_d_lon); | ||
| double k = (c == 0.0) ? 1.0 : (c / sin(c)); | ||
| double y = k * cos_lat * sin(lon_rad - lon0) * r_earth; | ||
| double x = k * (cos_lat0 * sin_lat - sin_lat0 * cos_lat * cos_d_lon) * r_earth; | ||
| double z = alt0 - p.alt; | ||
| return new double[]{x, y, z}; | ||
| } | ||
|
|
||
| public LatLonAlt reproject(double[] v) { | ||
| if (!inited) { | ||
| throw new RuntimeException("Not initialized"); | ||
| } | ||
| double x_rad = v[0] / r_earth; | ||
| double y_rad = v[1] / r_earth; | ||
| double c = sqrt(x_rad * x_rad + y_rad * y_rad); | ||
| double sin_c = sin(c); | ||
| double cos_c = cos(c); | ||
| double lat_rad; | ||
| double lon_rad; | ||
| if (c != 0.0) { | ||
| lat_rad = asin(cos_c * sin_lat0 + (x_rad * sin_c * cos_lat0) / c); | ||
| lon_rad = (lon0 + atan2(y_rad * sin_c, c * cos_lat0 * cos_c - x_rad * sin_lat0 * sin_c)); | ||
| } else { | ||
| lat_rad = lat0; | ||
| lon_rad = lon0; | ||
| } | ||
| return new LatLonAlt(lat_rad * 180.0 / Math.PI, lon_rad * 180.0 / Math.PI, alt0 - v[2]); | ||
| } | ||
| } |
Oops, something went wrong.