Sphere with mouse and rotate to Lat Lon with Apache Math3 library

Sphere-Lat-Lon.pde

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

Vector3D nl = latLonToVector3D(52.37, 4.91);
Vector3D ny = latLonToVector3D(40.79, -73.96);
Vector3D zero = latLonToVector3D(0, 0.0001);
Vector3D north = latLonToVector3D(90, 0);
Vector3D south = latLonToVector3D(-90, 0);
Vector3D india = latLonToVector3D(8.2, 77.45);
Vector3D points[] = {nl, ny, zero, north, south, india};
Vector3D targetPoint = nl;
Rotation qTo = getRotationToPoint(targetPoint);
Rotation qNow = new Rotation(zero, 0);

int currentPointIndex = 0;
PShape sphere;

void setup() {
  size(800, 800, P3D);
  noStroke();
  sphere = createShape(SPHERE, height/2);
  sphere.rotateY(HALF_PI);
  sphere.setTexture(loadImage("earth.jpg"));
  sphere.setStroke(false);
}

void draw() {
  background(0);
  lights();
  ortho();
  translate(width/2, height/2);

  qNow = interpolateRotations(qNow, qTo, .1);
  applyRotation(qNow);
  shape(sphere);

  for (Vector3D p : points) {
    pushMatrix();
    translate((float)p.getX() * height/2, (float)p.getY() * height/2, (float)p.getZ() * height/2);
    fill(p==targetPoint ? 255 : 0, p==targetPoint ? 0 : 255, 0);
    sphere(10);
    popMatrix();
  }
}

void applyRotation(Rotation rotation) {
  double[][] m = rotation.getMatrix();
  applyMatrix(
    (float) m[0][0], (float) m[1][0], (float) m[2][0], 0,
    (float) m[0][1], (float) m[1][1], (float) m[2][1], 0,
    (float) m[0][2], (float) m[1][2], (float) m[2][2], 0,
    0, 0, 0, 1);
}

void keyPressed() {
  if (key == ' ') {
    currentPointIndex = (currentPointIndex + 1) % points.length;
    targetPoint = points[currentPointIndex];
    qTo = getRotationToPoint(targetPoint);
  }
}

Rotation getRotationToPoint(Vector3D point) {
  if (point.equals(zero)) return new Rotation(zero, 0);
  Vector3D axis = Vector3D.crossProduct(zero, point).normalize();
  double angle = Math.acos(Vector3D.dotProduct(zero, point));
  return new Rotation(axis, angle);
}

void mouseDragged() {
  Vector3D from = getMouseOnSphere(pmouseX, width-pmouseY);
  Vector3D to = getMouseOnSphere(mouseX, width-mouseY);
  drag(from, to);
}

void drag(Vector3D from, Vector3D to) {
  Vector3D axis = Vector3D.crossProduct(from, to);
  if (axis.getNorm() > 0) {
    axis = axis.normalize();
    double angle = -Math.acos(Vector3D.dotProduct(from, to));
    qTo = qTo.compose(new Rotation(axis, angle), RotationConvention.VECTOR_OPERATOR);
  }
}

Vector3D getMouseOnSphere(float x, float y) {
  x = map(x, 0, width, -1, 1);
  y = map(y, 0, height, 1, -1);
  float r = x*x+y*y;
  return new Vector3D(x, y, r>1 ? 0 : sqrt(1-r)).normalize();
}

Rotation interpolateRotations(Rotation start, Rotation end, float t) {
  return new Rotation(start.getQ0() + t * (end.getQ0() - start.getQ0()),
    start.getQ1() + t * (end.getQ1() - start.getQ1()),
    start.getQ2() + t * (end.getQ2() - start.getQ2()),
    start.getQ3() + t * (end.getQ3() - start.getQ3()),
    true);
}

Vector3D latLonToVector3D(float lat, float lon) { //in degrees
  float x = cos(radians(lat)) * sin(radians(lon));
  float y = -sin(radians(lat));
  float z = cos(radians(lat)) * cos(radians(lon));
  return new Vector3D(x, y, z);
}