Simple quaternion library for Processing

From Create52 Cookbook

Below is the contents of a tab in a Processing application that allows Quaternion manipulation.

// some common quaternion values, for convenience

Quaternion QUATERNION_ZERO = new Quaternion(0, 0, 0, 0);
Quaternion QUATERNION_ONE = new Quaternion(1, 0, 0, 0);
Quaternion QUATERNION_I = new Quaternion(0, 1, 0, 0);
Quaternion QUATERNION_J = new Quaternion(0, 0, 1, 0);
Quaternion QUATERNION_K = new Quaternion(0, 0, 0, 1);


// acceptable error when checking if values are the same
float ACCEPTABLE_ERROR = 0.001;



/////////////////////////////////////////////////////////
//   'static' methods - these make a new Quaternion for
//   the result, and don't have any affect on the 
//   quaternion parameters given.
//
//  (For 'in place' operators that affect one of the
//  Quaternion parameters, use the instance methods in the
//  Quaternion class itself.)
//
//   NB. These methods have a '2' at the end of the name
//   due to a limitation in Processing: you can't have a
//   file-level method with same name as a method in a class
//   in that file (without some name-hiding occuring) - even
//   if the parameters to those methods are different!
//

public Quaternion add2(Quaternion q1, Quaternion q2) {
  return new Quaternion(q1.real + q2.real, 
      q1.i + q2.i, 
      q1.j + q2.j, 
      q1.k + q2.k);
}


public Quaternion sub2(Quaternion q1, Quaternion q2) {
  return new Quaternion(q1.real - q2.real, 
      q1.i - q2.i, 
      q1.j - q2.j, 
      q1.k - q2.k);
}

// multiplication by a scalar
public Quaternion multiply2(Quaternion q, float multiplier) {
  return new Quaternion(q.real * multiplier, 
      q.i * multiplier, 
      q.j * multiplier, 
      q.k * multiplier);
}


public Quaternion divide2(Quaternion q, float divisor) {
  return new Quaternion(q.real / divisor, 
      q.i / divisor, 
      q.j / divisor, 
      q.k / divisor);
}

// multiplication of two quaternions
public Quaternion multiply2(Quaternion q1, Quaternion q2) {
  // just use the instance method
  Quaternion result = new Quaternion(q1);
  result.multiply(q2);
  return result;
}

public Quaternion conjugate2(Quaternion q) {
  return new Quaternion(q.real, - q.i, - q.j, - q.k);
}  

public Quaternion intPower2(Quaternion q, int power) {
  Quaternion result = new Quaternion(q);
  for (int i = 0; i < power -1; i++) {
     result.multiply(q); 
  }
  return result;
}  

/////////////////////////////////////////////////////////////////


class Quaternion {
  public float real = 0;
  public float i = 0;
  public float j = 0;
  public float k = 0;
  
  // constructor for constructing the quaternion Q = 0
  public Quaternion() {
    // nothing
  }
  
  // Copying constructor
  public Quaternion(Quaternion copyFrom) {
     this.real = copyFrom.real;
     this.i = copyFrom.i;
     this.j = copyFrom.j;
     this.k = copyFrom.k;
  }
  
  public Quaternion(float real, float i, float j, float k) {
    this.real = real;    
    this.i = i;
    this.j = j;
    this.k = k;    
  }
  
  
  public void add(Quaternion q) {
    real += q.real;
    i += q.i;
    j += q.j;
    k += q.k;
  }

  public void sub(Quaternion q) {
    real -= q.real;
    i -= q.i;
    j -= q.j;
    k -= q.k;
  }


  // multiply by a scalar
  public void multiply(float multiplier) {
    real *= multiplier;
    i *= multiplier;
    j *= multiplier;
    k *= multiplier;
  }

  // divide by a scalar
  public void divide(float divisor) {  
    real /= divisor;
    i /= divisor;
    j /= divisor;
    k /= divisor;
  }

  //a[0]*b[0] - a[1]*b[1] - a[2]*b[2] - a[3]*b[3], 
  //      a[0]*b[1] + a[1]*b[0] +   a[2]*b[3] - a[3]*b[2], 
  //      a[0]*b[2] + a[2]*b[0] +   a[3]*b[1] - a[1]*b[3], 
  //      a[0]*b[3] + a[3]*b[0] +   a[1]*b[2] - a[2]*b[1] };
        
  public void multiply(Quaternion q) {
    float realResult = real * q.real - i * q.i - j * q.j - k * q.k;
    float iResult = real * q.i + i * q.real  +  j * q.k - k * q.j;
    float jResult = real * q.j + j * q.real  +  k * q.i - i * q.k;
    float kResult = real * q.k + k * q.real  +  i * q.j - j * q.i;
    
    real = realResult;
    i = iResult;
    j = jResult;
    k = kResult;
  }

  // divide
  public void divide(Quaternion q) {
    // uses fact that z*conj(z) is a real quantity and that a/b = a/b * (conj(b)/conj(b))
    
    Quaternion qConj = q.conjugate2();
    //println("q="+q+" conj=" + qConj);
    
    // calculate conjugate product of the divisor: q * conj(q) 
    float conjProduct = q.real * q.real + q.i * q.i + q.j * q.j + q.k * q.k;
    
    this.multiply(qConj);
      
    this.divide(conjProduct);
  }

  // returns the norm sqaured of this quaternion (i.e. square of Euclidean distance from origin)
  public float magnitudeSq() {
    return real*real + i*i + j*j + k*k;
  }
  

  // returns the norm of this quaternion (i.e. Euclidean distance from origin)
  public float magnitude() {
    return sqrt(real*real + i*i + j*j + k*k);
  }
    
  // makes this quaternion into a unit quaternion
  public void unit() {
    divide(magnitude());
  }
  
  // returns a quaternion that is the unit quaternion of 'this'
  public Quaternion unit2() {
    Quaternion q = new Quaternion(this);
    q.divide(q.magnitude());
    return q;
  }
  
  public void conjugate() {
    i = -i;
    j = -j;
    k = -k;    
  }
  
  public Quaternion conjugate2() {
    return new Quaternion(real, -i, -j, -k);
  }
  
  // Rotates this quaternion around the three-vector (imaginary part) of 'axis',
  // by theta degrees.
  // I know there are more efficient ways to do this by hand-cranking the
  // equation v' = r v r^-1  but this is for clarity...
  public Quaternion rotate2(Quaternion axis, float theta) {
    Quaternion target = new Quaternion(this);
    // half theta, since magic quaternion rotation rotates by theta*2
    theta /= 2;
    
    // axis quaternion is a vector-only (imaginary only) quaternion specifying
    // the axis. We need to  convert it into another form for rotation:
    //   (cos theta, sin theta * [Imag])
    

    axis.multiply(sin(theta));
    axis.real = cos(theta);
    
    // need axis to be unit vector
    
    Quaternion axisNorm = axis.unit2();
    Quaternion axisNormConj = axisNorm.conjugate2();

    axisNorm.multiply(target);
    axisNorm.multiply(axisNormConj);
    
    // we know the answer should have a zero real part, so set it to zero in case 
    // rounding errors etc. have crept in
    axisNorm.real = 0;
    return axisNorm;
  }
  
  public String toString() {
    return "(" + real +", " + i +", " + j +", "+ k +")";
  }
  
  public boolean equals(Object quat) {
    Quaternion q = (Quaternion)quat;


    // note use of OR to fail early (conditional evaluation)
    float ddr = (real - q.real);
    float ddi = (i - q.i);
    float ddj = (j - q.j);
    float ddk = (k - q.k);

//  if ((abs(ddr) + abs(ddi) + abs(ddj) + abs(ddk)) > ACCEPTABLE_ERROR) {
    if ((ddr*ddr + ddi*ddi + ddj*ddj + ddk*ddk) > ACCEPTABLE_ERROR) {
        return false;
    }
    return true;    
  }  
}