lat/lon to Rotation and back

lat,lon to Rotation and back.pde

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

GeoPoint nl = new GeoPoint(52.37, 4.91);
GeoPoint ny = new GeoPoint(40.79, -73.96);
GeoPoint jp = new GeoPoint(36.14, 137.86);
GeoPoint north = new GeoPoint(90, 0);
GeoPoint south = new GeoPoint(-90, 0);
GeoPoint west = new GeoPoint(0,-90);
GeoPoint east = new GeoPoint(0,90);

GeoPoint india = new GeoPoint(22, 77);
GeoPoint front = new GeoPoint(0, 0);
GeoPoint places[] = {nl, ny, india, jp, north, south};

void setup() {
    
  for (GeoPoint p : places) {
    Rotation qNow = new Rotation(p, front); //rotate to this location

    GeoPoint p2 = GeoPoint.fromRotation(qNow, front); //get lat/lon at the current rotation

    println(p.lat,p.lon," >>>> ", p2.lat,p2.lon);
  }
  exit();
}


static class GeoPoint extends Vector3D {
  float lat, lon;

  GeoPoint(float lat, float lon) {
    super(
      cos(radians(lat)) * sin(radians(lon)), //X&Z swapped
      sin(radians(lat)),
      cos(radians(lat)) * cos(radians(lon))); //X&Z swapped
    this.lat = lat;
    this.lon = lon;
  }

  static GeoPoint fromRotation(Rotation qNow, Vector3D front) {
    Vector3D inverse = qNow.applyInverseTo(front);
    float lat = degrees(asin((float)inverse.getY()));
    float lon = degrees(atan2((float)inverse.getX(),(float)inverse.getZ())); //X&Z swapped
    return new GeoPoint(lat, lon%180);
  }
}