Anchor point z-rotation. Working test.

Anchor point z-rotation. Working test.pde

import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;

class GeoPoint extends Vector3D {
  float lat, lon;

  GeoPoint(float lat, float lon) {
    super(
      cos(radians(lat)) * sin(-radians(lon)),
      sin(radians(lat)),
      cos(radians(lat)) * cos(-radians(lon))
      );
    this.lat = lat;
    this.lon = lon % 180;
  }

  GeoPoint(Rotation q, GeoPoint p) {
    this(degrees(asin((float) q.applyTo(p).getY())),
     degrees(-atan2((float) q.applyTo(p).getX(), (float) q.applyTo(p).getZ())));
  }

  String toString() {
    return String.format("latlon=(%.1f %.1f) xyz=(%.2f %.2f %.2f)", lat, lon, getX(), getY(), getZ());
  }
}

void setup() {
  Rotation r;
  GeoPoint anchor, new_york, p;

  anchor = new GeoPoint(0, 0);
  new_york = new GeoPoint(40.79, -73.96);
  println("new york Voor de rotatie: " + new_york); // Verwacht: latlon=40.8,-74.0

  r = new Rotation(anchor, new_york);
  p = new GeoPoint(r, anchor);
  println("geopoint van anchor: " + p); // Verwacht: latlon=40.8,-74.0

  r = r.applyTo(new Rotation(anchor, 0.1));
  p = new GeoPoint(r, anchor);
  println("0.1 rotatie toegepast op punt anchor: " + p); // Verwacht: latlon=40.8,-74.0
}