Globe controlled by mouse and trackballs with helicopter pointing in direction of movement (for TopoGame)

GlobeTrackballsHeli.pde

import hypermedia.net.*;
UDP udp;
Heli heli = new Heli();
Quaternion qRel = new Quaternion();
Quaternion qNow = new Quaternion();
PShape sphere;
float radius = 315;
float heading, toHeading;

void setup() {
  size(800, 800, P3D);
  udp = new UDP(this, 8888);
  udp.listen(true);
  sphereDetail(64);
  sphere = createShape(SPHERE, radius);
  sphere.setStroke(false);
  sphere.setTexture(loadImage("earth2k.jpg"));
  heli.setup();
}

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

  //sphere
  noStroke();
  fill(255);   
  pushMatrix();  
  qNow.mult(qRel);
  rotate(qNow);
  rotateY(HALF_PI); //rotate texture to latlon=0,0
  shape(sphere);
  popMatrix();

  //heli
  if (!qRel.isIdentity()) {
    toHeading = qRel.getHeading();
  }
  heading = angleLerp(heading, toHeading, .1);
  heli.heading = heading;
  heli.draw();
  
  //reset relative rotation in frame
  qRel.reset();
}

void mouseDragged() {
  PVector from = toSphere(float(pmouseX)/width-.5, float(pmouseY)/width-.5);
  PVector to = toSphere(float(mouseX)/width-.5, float(mouseY)/width-.5);
  qRel.mult(new Quaternion(from.dot(to), from.cross(to))); //w,xyz
  qRel.normalize();
}

void receive( byte[] data ) {
  String message = new String( data );
  String values[] = split(message, ',');

  if (values.length==7) {
    for (int i=0; i<3; i++) {
      float offset = .7;
      float speed = .075 / radius;
      float localRotation = radians(45);
      float globalRotation = radians(155) - i/3. * TWO_PI;
      float x = float(values[i*2+0]);
      float y = float(values[i*2+1]);
      PVector from = new PVector(offset, 0).rotate(globalRotation);      
      PVector to = new PVector(x, y).rotate(localRotation);
      to.y *= -1;
      to.rotate(globalRotation).mult(speed).add(from);
      from = toSphere(from);
      to = toSphere(to);      
      qRel.mult(new Quaternion(from.dot(to), from.cross(to))); //w,xyz
      qRel.normalize();
    }
  }
}

PVector toSphere(float x, float y) {  //-0.5 ... +0.5
  PVector v = new PVector(x, y);
  if (v.mag()>=1.0f) v.normalize();
  else v.z = sqrt(1.0 - v.mag());
  return v;
}

PVector toSphere(PVector v) {  //-0.5 ... +0.5
  return toSphere(v.x, v.y);
}

float angleLerp(float from, float to, float t) {
  float diff = (to - from) % TWO_PI;
  return from + (2 * diff % TWO_PI - diff) * t;
}

void rotate(Quaternion q) {
  rotate(q.getAngle(), q.getAxis().x, q.getAxis().y, q.getAxis().z);
}

Heli.pde

class Heli {
  PImage blades,body;
  int pmillis = 0;
  float heading;
  float bladeAngle;
  
  void setup() {
    blades = loadImage("blades.png");
    body = loadImage("body.png");
  }

  void draw() {

    if (millis()-pmillis > 10) {
      pmillis = millis();
      bladeAngle += TWO_PI/30;
      if (bladeAngle>TWO_PI) bladeAngle-=TWO_PI;
    }

    hint(DISABLE_DEPTH_TEST);
    imageMode(CENTER);
    
    pushMatrix();
    rotate(heading - HALF_PI);
    
    image(body,0,0);
    popMatrix();
    
    pushMatrix();
    rotate(bladeAngle); //heading + HALF_PI);
    image(blades,0,0);
    popMatrix();
        
    hint(ENABLE_DEPTH_TEST);
  }
}

Quaternion.pde

static class Quaternion {
  float W, X, Y, Z;

  Quaternion() {
    set(1, 0, 0, 0);
  }

  Quaternion(float w, float x, float y, float z) {
    set(w, x, y, z);
  }
  
  Quaternion(float w, PVector v) {
    set(w,v.x,v.y,v.z);
  }

  void set(float w, float x, float y, float z) {
    W = w;
    X = x;
    Y = y;
    Z = z;
  }

  Quaternion mult(Quaternion q) {
    float x = q.W * X + q.X * W + q.Y * Z - q.Z * Y;
    float y = q.W * Y - q.X * Z + q.Y * W + q.Z * X;
    float z = q.W * Z + q.X * Y - q.Y * X + q.Z * W;
    float w = q.W * W - q.X * X - q.Y * Y - q.Z * Z;
    W = w;
    X = x;
    Y = y;
    Z = z;
    return this;
  }

  Quaternion copy() {
    return new Quaternion(W, X, Y, Z);
  }

  void applyTo(PVector v) {
    // nVidia SDK implementation
    PVector uv, uuv;
    PVector qvec = new PVector(X, Y, Z); //_v.x, _v.y, _v.z);
    uv = qvec.cross(v);    //uv = qvec ^ v;
    uuv = qvec.cross(uv);    //uuv = qvec ^ uv;
    uv.mult(2.0f * W);
    uuv.mult(2.0f);
    v.add(uv);
    v.add(uuv);
  }

  Quaternion normalize() {
    float norme = sqrt(W*W + X*X + Y*Y + Z*Z);
    if (norme == 0.0) {
      W = 1.0; 
      X = Y = Z = 0.0;
    } else {
      float recip = 1.0/norme;
      W *= recip;
      X *= recip;
      Y *= recip;
      Z *= recip;
    }
    return this;
  }

  float getAngle() {
    float sinhalfangle = sqrt(X*X+Y*Y+Z*Z);
    return 2.0 * atan2(sinhalfangle, W);
  }

  PVector getAxis() {
    float sinhalfangle = sqrt(X*X+Y*Y+Z*Z);
    if (sinhalfangle>0) {
      PVector axis = new PVector(X, Y, Z);
      axis.div(sinhalfangle);
      return axis;
    } else return new PVector(0, 0, 1);
  }

  /// Set the elements of the Quat to represent a rotation of angle
  /// around the axis (x,y,z)
  static Quaternion fromRotate( float angle, float x, float y, float z ) {
    //angle in Radians!

    //const 
    float epsilon = 0.0000001f;

    float length = sqrt( x * x + y * y + z * z ); //was sqrtf
    if (length < epsilon) {
      // ~zero length axis, so reset rotation to zero.
      //*this = ofQuaternion();
      return new Quaternion(1, 0, 0, 0);
    }

    float inversenorm  = 1.0f / length;
    float coshalfangle = cos( 0.5f * angle );
    float sinhalfangle = sin( 0.5f * angle );

    float _x = x * sinhalfangle * inversenorm;
    float _y = y * sinhalfangle * inversenorm;
    float _z = z * sinhalfangle * inversenorm;
    float _w = coshalfangle;

    return new Quaternion(_x, _y, _z, _w);
  }

  /// Spherical Linear Interpolation
  /// As t goes from 0 to 1, the Quat object goes from "from" to "to"
  /// Reference: Shoemake at SIGGRAPH 89
  /// See also
  /// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
  static Quaternion slerp(Quaternion from, Quaternion to, float t) {
    float epsilon = 0.00001;
    float omega, cosomega, sinomega, scale_from, scale_to ;

    Quaternion quatTo = to.copy();

    // this is a dot product
    cosomega = from.X*to.X + from.Y*to.Y + from.Z*to.Z + from.W*to.W;

    if ( cosomega < 0.0 ) {
      cosomega = -cosomega;
      quatTo.X *= -1;
      quatTo.Y *= -1;
      quatTo.Z *= -1;
      quatTo.W *= -1;
    }

    if ( (1.0 - cosomega) > epsilon ) {
      omega = acos(cosomega) ; // 0 <= omega <= Pi (see man acos)
      sinomega = sin(omega) ;  // this sinomega should always be +ve so
      // could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
      scale_from = sin((1.0 - t) * omega) / sinomega ;
      scale_to = sin(t * omega) / sinomega ;
    } else {
      /* --------------------------------------------------
       The ends of the vectors are very close
       we can use simple linear interpolation - no need
       to worry about the "spherical" interpolation
       -------------------------------------------------- */
      scale_from = 1.0 - t ;
      scale_to = t ;
    }

    //add
    return new Quaternion(
      from.W * scale_from + quatTo.W * scale_to, 
      from.X * scale_from + quatTo.X * scale_to, 
      from.Y * scale_from + quatTo.Y * scale_to, 
      from.Z * scale_from + quatTo.Z * scale_to
      );
  }

  String toString() {
    return W + "," + X + "," + Y + "," + Z;
  }

  PVector toEulerAngle() { //const Quaterniond& q, double& roll, double& pitch, double& yaw) {
    PVector rollPitchYaw = new PVector();

    //roll (x-axis rotation)
    float sinr_cosp = +2.0 * (W*X + Y*Z);
    float cosr_cosp = +1.0 - 2.0 * (X*X + Y*Y);
    rollPitchYaw.x = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    float sinp = +2.0 * (W*Y - Z*X);
    if (abs(sinp) >= 1)
      rollPitchYaw.y = sinp<0 ? -HALF_PI : HALF_PI; //copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
      rollPitchYaw.y = asin(sinp);

    // yaw (z-axis rotation)
    float siny_cosp = +2.0 * (W*Z + X*Y);
    float cosy_cosp = +1.0 - 2.0 * (Y*Y + Z*Z);  
    rollPitchYaw.z = atan2(siny_cosp, cosy_cosp);

    return rollPitchYaw;
  }

  float getYaw() {
    float siny_cosp = +2.0 * (W*Z + X*Y);
    float cosy_cosp = +1.0 - 2.0 * (Y*Y + Z*Z);  
    return atan2(siny_cosp, cosy_cosp);
  }

  float getLength2() {
    return X*X + Y*Y + Z*Z + W*W;
  }

  PMatrix3D getRotationMatrix() {    
    //see also:
    //https://github.com/processing/processing/blob/master/core/src/processing/core/PMatrix3D.java#L465
    //and: openFrameworks: ofMatrix4x4::setRotate(const ofQuaternion& q)

    PMatrix3D m = new PMatrix3D(); //new identity matrix created

    float length2 = this.getLength2();

    if (abs(length2) < EPSILON) {
      // m = identity matrix
    } else {

      float rlength2 = length2 != 1.0 ? 2.0/length2 : 2.0;
      float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

      // calculate coefficients
      x2 = rlength2*X;
      y2 = rlength2*Y;
      z2 = rlength2*Z;

      xx = X * x2;
      xy = X * y2;
      xz = X * z2;

      yy = Y * y2;
      yz = Y * z2;
      zz = Z * z2;

      wx = W * x2;
      wy = W * y2;
      wz = W * z2;

      m.set( 1.0 - (yy + zz), xy + wz, xz - wy, 0, 
        xy - wz, 1.0 - (xx + zz), yz + wx, 0, 
        xz + wy, yz - wx, 1.0 - (xx + yy), 0, 
        0, 0, 0, 1);
    }
    return m;
  }

  PVector getCartesian() {
    PMatrix3D m = getRotationMatrix();
    PVector v = new PVector(0, 0, 1);
    return m.mult(v, null);
  }

  Quaternion div(Quaternion q) {
    mult(new Quaternion().invert(q));
    return this;
  }

  Quaternion invert(Quaternion in) {
    float dot = in.getLength2();
    dot = dot == 0.0 ? 0.0 : 1.0 / dot;
    X = -in.X * dot; 
    Y = -in.Y * dot; 
    Z = -in.Z * dot;
    W = in.W * dot;
    return this;
  }

  boolean isIdentity() {
    return W==1 && X==0 && Y==0 && Z==0;
  }

  void reset() {
    set(1, 0, 0, 0);
  }

  float getHeading() {
    PVector v = new PVector(0, 0, 1); 
    applyTo(v);
    return atan2(v.y, v.x);
  }
}