src/javajs/util/Quat.java

   1 /* $RCSfile$
   2  * $Author: hansonr $
   3  * $Date: 2007-04-05 09:07:28 -0500 (Thu, 05 Apr 2007) $
   4  * $Revision: 7326 $
   5  *
   6  * Copyright (C) 2003-2005  The Jmol Development Team
   7  *
   8  * Contact: jmol-developers@lists.sf.net
   9  *
  10  *  This library is free software; you can redistribute it and/or
  11  *  modify it under the terms of the GNU Lesser General Public
  12  *  License as published by the Free Software Foundation; either
  13  *  version 2.1 of the License, or (at your option) any later version.
  14  *
  15  *  This library is distributed in the hope that it will be useful,
  16  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18  *  Lesser General Public License for more details.
  19  *
  20  *  You should have received a copy of the GNU Lesser General Public
  21  *  License along with this library; if not, write to the Free Software
  22  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
  23  */
  24 package javajs.util;
  25
  26 /*
  27  * Standard UNIT quaternion math -- for rotation.
  28  *
  29  * All rotations can be represented as two identical quaternions.
  30  * This is because any rotation can be considered from either end of the
  31  * rotational axis -- either as a + rotation or a - rotation. This code
  32  * is designed to always maintain the quaternion with a rotation in the
  33  * [0, PI) range.
  34  *
  35  * This ensures that the reported theta is always positive, and the normal
  36  * reported is always associated with a positive theta.
  37  *
  38  * @author Bob Hanson, hansonr@stolaf.edu 6/2008
  39  *
  40  */
  41
  42 public class Quat {
  43   public float q0, q1, q2, q3;
  44   private M3 mat;
  45
  46   private final static P4 qZero = new P4();
  47   private static final double RAD_PER_DEG = Math.PI / 180;
  48
  49   public Quat() {
  50     q0 = 1;
  51   }
  52
  53   public static Quat newQ(Quat q) {
  54     Quat q1 = new Quat();
  55     q1.set(q);
  56     return q1;
  57   }
  58
  59   public static Quat newVA(T3 v, float theta) {
  60     Quat q = new Quat();
  61     q.setTA(v, theta);
  62     return q;
  63   }
  64
  65   public static Quat newM(M3 mat) {
  66     Quat q = new Quat();
  67     q.setM(M3.newM3(mat));
  68     return q;
  69   }
  70
  71   public static Quat newAA(A4 a) {
  72     Quat q = new Quat();
  73     q.setAA(a);
  74     return q;
  75   }
  76
  77   public static Quat newP4(P4 pt) {
  78     Quat q = new Quat();
  79     q.setP4(pt);
  80     return q;
  81   }
  82
  83   /**
  84    * Note that q0 is the last parameter here
  85    *
  86    * @param q1
  87    * @param q2
  88    * @param q3
  89    * @param q0
  90    * @return {q1 q2 q3 q0}
  91    */
  92   public static Quat new4(float q1, float q2, float q3, float q0) {
  93     Quat q = new Quat();
  94     if (q0 < -1) {
  95       q.q0 = -1;
  96       return q;
  97     }
  98     if (q0 > 1) {
  99       q.q0 = 1;
 100       return q;
 101     }
 102     q.q0 = q0;
 103     q.q1 = q1;
 104     q.q2 = q2;
 105     q.q3 = q3;
 106     return q;
 107   }
 108
 109   public void set(Quat q) {
 110     q0 = q.q0;
 111     q1 = q.q1;
 112     q2 = q.q2;
 113     q3 = q.q3;
 114   }
 115
 116   /**
 117    * {x y z w} --> {q1 q2 q3 q0} and factored
 118    *
 119    * @param pt
 120    */
 121   private void setP4(P4 pt) {
 122     float factor = (pt == null ? 0 : pt.distance4(qZero));
 123     if (factor == 0) {
 124       q0 = 1;
 125       return;
 126     }
 127     q0 = pt.w / factor;
 128     q1 = pt.x / factor;
 129     q2 = pt.y / factor;
 130     q3 = pt.z / factor;
 131   }
 132
 133   /**
 134    * q = (cos(theta/2), sin(theta/2) * n)
 135    *
 136    * @param pt
 137    * @param theta
 138    */
 139   public void setTA(T3 pt, float theta) {
 140     if (pt.x == 0 && pt.y == 0 && pt.z == 0) {
 141       q0 = 1;
 142       return;
 143     }
 144     double fact = (Math.sin(theta / 2 * RAD_PER_DEG) / Math.sqrt(pt.x
 145         * pt.x + pt.y * pt.y + pt.z * pt.z));
 146     q0 = (float) (Math.cos(theta / 2 * RAD_PER_DEG));
 147     q1 = (float) (pt.x * fact);
 148     q2 = (float) (pt.y * fact);
 149     q3 = (float) (pt.z * fact);
 150   }
 151
 152   public void setAA(A4 a) {
 153     A4 aa = A4.newAA(a);
 154     if (aa.angle == 0)
 155       aa.y = 1;
 156     setM(new M3().setAA(aa));
 157   }
 158
 159   private void setM(M3 mat) {
 160
 161     /*
 162      * Changed 7/16/2008 to double precision for 11.5.48.
 163      *
 164      * <quote>
 165      *
 166      * RayTrace Software Package, release 3.0.  May 3, 2006.
 167      *
 168      * Mathematics Subpackage (VrMath)
 169      *
 170      * Author: Samuel R. Buss
 171      *
 172      * Software is "as-is" and carries no warranty.  It may be used without
 173      *   restriction, but if you modify it, please change the filenames to
 174      *   prevent confusion between different versions.  Please acknowledge
 175      *   all use of the software in any publications or products based on it.
 176      *
 177      * Bug reports: Sam Buss, sbuss@ucsd.edu.
 178      * Web page: http://math.ucsd.edu/~sbuss/MathCG
 179
 180      // Use Shepperd's algorithm, which is stable, does not lose
 181      //    significant precision and uses only one sqrt.
 182      //   J. Guidance and Control, 1 (1978) 223-224.
 183
 184      * </quote>
 185      *
 186      * Except, that code has errors.
 187      *
 188      * CORRECTIONS (as noted below) of Quaternion.cpp. I have reported the bug.
 189      *
 190      * -- Bob Hanson
 191      *
 192      *  theory:
 193      *         cos(theta/2)^2 = (cos(theta) + 1)/2
 194      *  and
 195      *         trace = (1-x^2)ct + (1-y^2)ct + (1-z^2)ct + 1 = 2cos(theta) + 1
 196      *  or
 197      *         cos(theta) = (trace - 1)/2
 198      *
 199      *  so in general,
 200      *
 201      *       w = cos(theta/2)
 202      *         = sqrt((cos(theta)+1)/2)
 203      *         = sqrt((trace-1)/4+1/2)
 204      *         = sqrt((trace+1)/4)
 205      *         = sqrt(trace+1)/2
 206      *
 207      *  but there are precision issues, so we allow for other situations.
 208      *  note -- trace >= 0.5 when cos(theta) >= -0.25 (-104.48 <= theta <= 104.48).
 209      *  this code cleverly matches the precision in all four options.
 210      *
 211      */
 212
 213     this.mat = mat;
 214
 215     double trace = mat.m00 + mat.m11 + mat.m22;
 216     double temp;
 217     double w, x, y, z;
 218     if (trace >= 0.5) {
 219       w = Math.sqrt(1.0 + trace);
 220       x = (mat.m21 - mat.m12) / w;
 221       y = (mat.m02 - mat.m20) / w;
 222       z = (mat.m10 - mat.m01) / w;
 223     } else if ((temp = mat.m00 + mat.m00 - trace) >= 0.5) {
 224       x = Math.sqrt(1.0 + temp);
 225       w = (mat.m21 - mat.m12) / x;
 226       y = (mat.m10 + mat.m01) / x;
 227       z = (mat.m20 + mat.m02) / x;
 228     } else if ((temp = mat.m11 + mat.m11 - trace) >= 0.5
 229         || mat.m11 > mat.m22) {
 230       y = Math.sqrt(1.0 + temp);
 231       w = (mat.m02 - mat.m20) / y;
 232       x = (mat.m10 + mat.m01) / y;
 233       z = (mat.m21 + mat.m12) / y;
 234     } else {
 235       z = Math.sqrt(1.0 + mat.m22 + mat.m22 - trace);
 236       w = (mat.m10 - mat.m01) / z;
 237       x = (mat.m20 + mat.m02) / z; // was -
 238       y = (mat.m21 + mat.m12) / z; // was -
 239     }
 240
 241     q0 = (float) (w * 0.5);
 242     q1 = (float) (x * 0.5);
 243     q2 = (float) (y * 0.5);
 244     q3 = (float) (z * 0.5);
 245
 246     /*
 247      *  Originally from http://www.gamedev.net/community/forums/topic.asp?topic_id=448380
 248      *  later algorithm was adapted from Visualizing Quaternions, by Andrew J. Hanson
 249      *   (Morgan Kaufmann, 2006), page 446
 250      *
 251      *  HOWEVER, checking with AxisAngle4f and Quat4f equivalents, it was found that
 252      *  BOTH of these sources produce inverted quaternions. So here we do an inversion.
 253      *
 254      *  This correction was made in 11.5.42  6/19/2008  -- Bob Hanson
 255      *
 256      *  former algorithm used:
 257      * /
 258
 259      double tr = mat.m00 + mat.m11 + mat.m22; //Matrix trace
 260      double s;
 261      double[] q = new double[4];
 262      if (tr > 0) {
 263      s = Math.sqrt(tr + 1);
 264      q0 = (float) (0.5 * s);
 265      s = 0.5 / s; // = 1/q0
 266      q1 = (float) ((mat.m21 - mat.m12) * s);
 267      q2 = (float) ((mat.m02 - mat.m20) * s);
 268      q3 = (float) ((mat.m10 - mat.m01) * s);
 269      } else {
 270      float[][] m = new float[][] { new float[3], new float[3], new float[3] };
 271      mat.getRow(0, m[0]);
 272      mat.getRow(1, m[1]);
 273      mat.getRow(2, m[2]);
 274
 275      //Find out the biggest element along the diagonal
 276      float max = Math.max(mat.m11, mat.m00);
 277      int i = (mat.m22 > max ? 2 : max == mat.m11 ? 1 : 0);
 278      int j = (i + 1) % 3;
 279      int k = (j + 1) % 3;
 280      s = -Math.sqrt(1 + m[i][i] - m[j][j] - m[k][k]);
 281      // 0 = 1 + (1-x^2)ct + x^2 -(1-y^2)ct - y^2 - (1-z^2)ct - z^2
 282      // 0 = 1 - ct + (x^2 - y^2 - z^2) - (x^2 - y^2 - z^2)ct
 283      // 0 = 1 - ct + 2x^2 - 1 - (2x^2)ct + ct
 284      // 0 = 2x^2(1 - ct)
 285      // theta = 0 (but then trace = 1 + 1 + 1 = 3)
 286      // or x = 0.
 287      q[i] = s * 0.5;
 288      if (s != 0)
 289      s = 0.5 / s; // = 1/q[i]
 290      q[j] = (m[i][j] + m[j][i]) * s;
 291      q[k] = (m[i][k] + m[k][i]) * s;
 292      q0 = (float) ((m[k][j] - m[j][k]) * s);
 293      q1 = (float) q[0]; // x
 294      q2 = (float) q[1]; // y
 295      q3 = (float) q[2]; // z
 296      }
 297
 298      */
 299   }
 300
 301   /*
 302    * if qref is null, "fix" this quaternion
 303    * otherwise, return a quaternion that is CLOSEST to the given quaternion
 304    * that is, one that gives a positive dot product
 305    *
 306    */
 307   public void setRef(Quat qref) {
 308     if (qref == null) {
 309       mul(getFixFactor());
 310       return;
 311     }
 312     if (dot(qref) >= 0)
 313       return;
 314     q0 *= -1;
 315     q1 *= -1;
 316     q2 *= -1;
 317     q3 *= -1;
 318   }
 319
 320   /**
 321    * returns a quaternion frame based on three points (center, x, and any point in xy plane)
 322    * or two vectors (vA, vB).
 323    *
 324    * @param center  (null for vA/vB option)
 325    * @param x
 326    * @param xy
 327    * @return quaternion for frame
 328    */
 329   public static final Quat getQuaternionFrame(P3 center, T3 x,
 330                                                     T3 xy) {
 331     V3 vA = V3.newV(x);
 332     V3 vB = V3.newV(xy);
 333     if (center != null) {
 334       vA.sub(center);
 335       vB.sub(center);
 336     }
 337     return getQuaternionFrameV(vA, vB, null, false);
 338   }
 339
 340   /**
 341    * Create a quaternion based on a frame
 342    * @param vA
 343    * @param vB
 344    * @param vC
 345    * @param yBased
 346    * @return quaternion
 347    */
 348   public static final Quat getQuaternionFrameV(V3 vA, V3 vB,
 349                                                     V3 vC, boolean yBased) {
 350     if (vC == null) {
 351       vC = new V3();
 352       vC.cross(vA, vB);
 353       if (yBased)
 354         vA.cross(vB, vC);
 355     }
 356     V3 vBprime = new V3();
 357     vBprime.cross(vC, vA);
 358     vA.normalize();
 359     vBprime.normalize();
 360     vC.normalize();
 361     M3 mat = new M3();
 362     mat.setColumnV(0, vA);
 363     mat.setColumnV(1, vBprime);
 364     mat.setColumnV(2, vC);
 365
 366     /*
 367      *
 368      * Verification tests using Quat4f and AngleAxis4f:
 369      *
 370      System.out.println("quaternion frame matrix: " + mat);
 371
 372      Point3f pt2 = new Point3f();
 373      mat.transform(Point3f.new3(1, 0, 0), pt2);
 374      System.out.println("vA=" + vA + " M(100)=" + pt2);
 375      mat.transform(Point3f.new3(0, 1, 0), pt2);
 376      System.out.println("vB'=" + vBprime + " M(010)=" + pt2);
 377      mat.transform(Point3f.new3(0, 0, 1), pt2);
 378      System.out.println("vC=" + vC + " M(001)=" + pt2);
 379      Quat4f q4 = new Quat4f();
 380      q4.set(mat);
 381      System.out.println("----");
 382      System.out.println("Quat4f: {" + q4.w + " " + q4.x + " " + q4.y + " " + q4.z + "}");
 383      System.out.println("Quat4f: 2xy + 2wz = m10: " + (2 * q4.x * q4.y + 2 * q4.w * q4.z) + " = " + mat.m10);
 384
 385      */
 386
 387     Quat q = newM(mat);
 388
 389      /*
 390      System.out.println("Quaternion mat from q \n" + q.getMatrix());
 391      System.out.println("Quaternion: " + q.getNormal() + " " + q.getTheta());
 392      AxisAngle4f a = new AxisAngle4f();
 393      a.set(mat);
 394      Vector3f v = Vector3f.new3(a.x, a.y, a.z);
 395      v.normalize();
 396      System.out.println("angleAxis: " + v + " "+(a.angle/Math.PI * 180));
 397      */
 398
 399     return q;
 400   }
 401
 402   public M3 getMatrix() {
 403     if (mat == null)
 404       setMatrix();
 405     return mat;
 406   }
 407
 408   private void setMatrix() {
 409     mat = new M3();
 410     // q0 = w, q1 = x, q2 = y, q3 = z
 411     mat.m00 = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
 412     mat.m01 = 2 * q1 * q2 - 2 * q0 * q3;
 413     mat.m02 = 2 * q1 * q3 + 2 * q0 * q2;
 414     mat.m10 = 2 * q1 * q2 + 2 * q0 * q3;
 415     mat.m11 = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3;
 416     mat.m12 = 2 * q2 * q3 - 2 * q0 * q1;
 417     mat.m20 = 2 * q1 * q3 - 2 * q0 * q2;
 418     mat.m21 = 2 * q2 * q3 + 2 * q0 * q1;
 419     mat.m22 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
 420   }
 421
 422   public Quat add(float x) {
 423     // scalar theta addition (degrees)
 424    return newVA(getNormal(), getTheta() + x);
 425   }
 426
 427   public Quat mul(float x) {
 428     // scalar theta multiplication
 429     return (x == 1 ? new4(q1, q2, q3, q0) :
 430       newVA(getNormal(), getTheta() * x));
 431   }
 432
 433   public Quat mulQ(Quat p) {
 434     return new4(
 435         q0 * p.q1 + q1 * p.q0 + q2 * p.q3 - q3 * p.q2,
 436         q0 * p.q2 + q2 * p.q0 + q3 * p.q1 - q1 * p.q3,
 437         q0 * p.q3 + q3 * p.q0 + q1 * p.q2 - q2 * p.q1,
 438         q0 * p.q0 - q1 * p.q1 - q2 * p.q2 - q3 * p.q3);
 439   }
 440
 441   public Quat div(Quat p) {
 442     // unit quaternions assumed -- otherwise would scale by 1/p.dot(p)
 443     return mulQ(p.inv());
 444   }
 445
 446   public Quat divLeft(Quat p) {
 447     // unit quaternions assumed -- otherwise would scale by 1/p.dot(p)
 448     return this.inv().mulQ(p);
 449   }
 450
 451   public float dot(Quat q) {
 452     return this.q0 * q.q0 + this.q1 * q.q1 + this.q2 * q.q2 + this.q3 * q.q3;
 453   }
 454
 455   public Quat inv() {
 456     return new4(-q1, -q2, -q3, q0);
 457   }
 458
 459   public Quat negate() {
 460     return new4(-q1, -q2, -q3, -q0);
 461   }
 462
 463   /**
 464    * ensures
 465    *
 466    * 1) q0 > 0
 467    * or
 468    * 2) q0 = 0 and q1 > 0
 469    * or
 470    * 3) q0 = 0 and q1 = 0 and q2 > 0
 471    * or
 472    * 4) q0 = 0 and q1 = 0 and q2 = 0 and q3 > 0
 473    *
 474    * @return 1 or -1
 475    *
 476    */
 477
 478   private float getFixFactor() {
 479     return (q0 < 0 ||
 480         q0 == 0 && (q1 < 0 || q1 == 0 && (q2 < 0 || q2 == 0 && q3 < 0)) ? -1 : 1);
 481   }
 482
 483   public V3 getVector(int i) {
 484     return getVectorScaled(i, 1f);
 485   }
 486
 487   public V3 getVectorScaled(int i, float scale) {
 488     if (i == -1) {
 489       scale *= getFixFactor();
 490       return V3.new3(q1 * scale, q2 * scale, q3 * scale);
 491     }
 492     if (mat == null)
 493       setMatrix();
 494     V3 v = new V3();
 495     mat.getColumnV(i, v);
 496     if (scale != 1f)
 497       v.scale(scale);
 498     return v;
 499   }
 500
 501   /**
 502    *
 503    * @return  vector such that 0 <= angle <= 180
 504    */
 505   public V3 getNormal() {
 506     V3 v = getRawNormal(this);
 507     v.scale(getFixFactor());
 508     return v;
 509   }
 510
 511   private static V3 getRawNormal(Quat q) {
 512     V3 v = V3.new3(q.q1, q.q2, q.q3);
 513     if (v.length() == 0)
 514       return V3.new3(0, 0, 1);
 515     v.normalize();
 516     return v;
 517   }
 518
 519   /**
 520    *
 521    * @return 0 <= angle <= 180 in degrees
 522    */
 523   public float getTheta() {
 524     return (float) (Math.acos(Math.abs(q0)) * 2 * 180 / Math.PI);
 525   }
 526
 527   public float getThetaRadians() {
 528     return (float) (Math.acos(Math.abs(q0)) * 2);
 529   }
 530
 531   /**
 532    *
 533    * @param v0
 534    * @return    vector option closest to v0
 535    *
 536    */
 537   public V3 getNormalDirected(V3 v0) {
 538     V3 v = getNormal();
 539     if (v.x * v0.x + v.y * v0.y + v.z * v0.z < 0) {
 540       v.scale(-1);
 541     }
 542     return v;
 543   }
 544
 545   public V3 get3dProjection(V3 v3d) {
 546     v3d.set(q1, q2, q3);
 547     return v3d;
 548   }
 549
 550   /**
 551    *
 552    * @param axisAngle
 553    * @return   fill in theta of axisAngle such that
 554    */
 555   public P4 getThetaDirected(P4 axisAngle) {
 556     //fills in .w;
 557     float theta = getTheta();
 558     V3 v = getNormal();
 559     if (axisAngle.x * q1 + axisAngle.y * q2 + axisAngle.z * q3 < 0) {
 560       v.scale(-1);
 561       theta = -theta;
 562     }
 563     axisAngle.set4(v.x, v.y, v.z, theta);
 564     return axisAngle;
 565   }
 566
 567   /**
 568    *
 569    * @param vector  a vector, same as for getNormalDirected
 570    * @return   return theta
 571    */
 572   public float getThetaDirectedV(V3 vector) {
 573     //fills in .w;
 574     float theta = getTheta();
 575     V3 v = getNormal();
 576     if (vector.x * q1 + vector.y * q2 + vector.z * q3 < 0) {
 577       v.scale(-1);
 578       theta = -theta;
 579     }
 580     return theta;
 581   }
 582
 583   /**
 584    *   Quaternions are saved as {q1, q2, q3, q0}
 585    *
 586    * While this may seem odd, it is so that for any point4 --
 587    * planes, axisangles, and quaternions -- we can use the
 588    * first three coordinates to determine the relavent axis
 589    * the fourth then gives us offset to {0,0,0} (plane),
 590    * rotation angle (axisangle), and cos(theta/2) (quaternion).
 591    * @return {x y z w} (unnormalized)
 592    */
 593   public P4 toPoint4f() {
 594     return P4.new4(q1, q2, q3, q0); // x,y,z,w
 595   }
 596
 597   public A4 toAxisAngle4f() {
 598     double theta = 2 * Math.acos(Math.abs(q0));
 599     double sinTheta2 = Math.sin(theta/2);
 600     V3 v = getNormal();
 601     if (sinTheta2 < 0) {
 602       v.scale(-1);
 603       theta = Math.PI - theta;
 604     }
 605     return A4.newVA(v, (float) theta);
 606   }
 607
 608   public T3 transform2(T3 pt, T3 ptNew) {
 609     if (mat == null)
 610       setMatrix();
 611     mat.rotate2(pt, ptNew);
 612     return ptNew;
 613   }
 614
 615   public Quat leftDifference(Quat q2) {
 616     //dq = q.leftDifference(qnext);//q.inv().mul(qnext);
 617     Quat q2adjusted = (this.dot(q2) < 0 ? q2.negate() : q2);
 618     return inv().mulQ(q2adjusted);
 619   }
 620
 621   public Quat rightDifference(Quat q2) {
 622     //dq = qnext.rightDifference(q);//qnext.mul(q.inv());
 623     Quat q2adjusted = (this.dot(q2) < 0 ? q2.negate() : q2);
 624     return mulQ(q2adjusted.inv());
 625   }
 626
 627   /**
 628    *
 629    *  Java axisAngle / plane / Point4f format
 630    *  all have the format {x y z w}
 631    *  so we go with that here as well
 632    *
 633    * @return  "{q1 q2 q3 q0}"
 634    */
 635   @Override
 636   public String toString() {
 637     return "{" + q1 + " " + q2 + " " + q3 + " " + q0 + "}";
 638   }
 639
 640   /**
 641    *
 642    * @param data1
 643    * @param data2
 644    * @param nMax     > 0 --> limit to this number
 645    * @param isRelative
 646    *
 647    * @return       pairwise array of data1 / data2 or data1 \ data2
 648    */
 649   public static Quat[] div(Quat[] data1, Quat[] data2, int nMax, boolean isRelative) {
 650     int n;
 651     if (data1 == null || data2 == null || (n = Math.min(data1.length, data2.length)) == 0)
 652       return null;
 653     if (nMax > 0 && n > nMax)
 654       n = nMax;
 655     Quat[] dqs = new Quat[n];
 656     for (int i = 0; i < n; i++) {
 657       if (data1[i] == null || data2[i] == null)
 658         return null;
 659       dqs[i] = (isRelative ? data1[i].divLeft(data2[i]) : data1[i].div(data2[i]));
 660     }
 661     return dqs;
 662   }
 663
 664   public static Quat sphereMean(Quat[] data, float[] retStddev, float criterion) {
 665     // Samuel R. Buss, Jay P. Fillmore:
 666     // Spherical averages and applications to spherical splines and interpolation.
 667     // ACM Trans. Graph. 20(2): 95-126 (2001)
 668       if (data == null || data.length == 0)
 669         return new Quat();
 670       if (retStddev == null)
 671         retStddev = new float[1];
 672       if (data.length == 1) {
 673         retStddev[0] = 0;
 674         return newQ(data[0]);
 675       }
 676       float diff = Float.MAX_VALUE;
 677       float lastStddev = Float.MAX_VALUE;
 678       Quat qMean = simpleAverage(data);
 679       int maxIter = 100; // typically goes about 5 iterations
 680       int iter = 0;
 681       while (diff > criterion && lastStddev != 0 && iter < maxIter) {
 682         qMean = newMean(data, qMean);
 683         retStddev[0] = stdDev(data, qMean);
 684         diff = Math.abs(retStddev[0] - lastStddev);
 685         lastStddev = retStddev[0];
 686         //Logger.info(++iter + " sphereMean " + qMean + " stddev=" + lastStddev + " diff=" + diff);
 687       }
 688       return qMean;
 689   }
 690
 691   /**
 692    * Just a starting point.
 693    * get average normal vector
 694    * scale normal by average projection of vectors onto it
 695    * create quaternion from this 3D projection
 696    *
 697    * @param ndata
 698    * @return approximate average
 699    */
 700   private static Quat simpleAverage(Quat[] ndata) {
 701     V3 mean = V3.new3(0, 0, 1);
 702     // using the directed normal ensures that the mean is
 703     // continually added to and never subtracted from
 704     V3 v = ndata[0].getNormal();
 705     mean.add(v);
 706     for (int i = ndata.length; --i >= 0;)
 707       mean.add(ndata[i].getNormalDirected(mean));
 708     mean.sub(v);
 709     mean.normalize();
 710     float f = 0;
 711     // the 3D projection of the quaternion is [sin(theta/2)]*n
 712     // so dotted with the normalized mean gets us an approximate average for sin(theta/2)
 713     for (int i = ndata.length; --i >= 0;)
 714       f += Math.abs(ndata[i].get3dProjection(v).dot(mean));
 715     if (f != 0)
 716       mean.scale(f / ndata.length);
 717     // now convert f to the corresponding cosine instead of sine
 718     f = (float) Math.sqrt(1 - mean.lengthSquared());
 719     if (Float.isNaN(f))
 720       f = 0;
 721     return newP4(P4.new4(mean.x, mean.y, mean.z, f));
 722   }
 723
 724   private static Quat newMean(Quat[] data, Quat mean) {
 725     /* quaternion derivatives nicely take care of producing the necessary
 726      * metric. Since dq gives us the normal with the smallest POSITIVE angle,
 727      * we just scale by that -- using degrees.
 728      * No special normalization is required.
 729      *
 730      * The key is that the mean has been set up already, and dq.getTheta()
 731      * will always return a value between 0 and 180. True, for groupings
 732      * where dq swings wildly -- 178, 182, 178, for example -- there will
 733      * be problems, but the presumption here is that there is a REASONABLE
 734      * set of data. Clearly there are spherical data sets that simply cannot
 735      * be assigned a mean. (For example, where the three projected points
 736      * are equally distant on the sphere. We just can't worry about those
 737      * cases here. Rather, if there is any significance to the data,
 738      * there will be clusters of projected points, and the analysis will
 739      * be meaningful.
 740      *
 741      * Note that the hemisphere problem drops out because dq.getNormal() and
 742      * dq.getTheta() will never return (n, 182 degrees) but will
 743      * instead return (-n, 2 degrees). That's just what we want in that case.
 744      *
 745      *  Note that the projection in this case is to 3D -- a set of vectors
 746      *  in space with lengths proportional to theta (not the sin(theta/2)
 747      *  that is associated with a quaternion map).
 748      *
 749      *  This is officially an "exponential" or "hyperbolic" projection.
 750      *
 751      */
 752     V3 sum = new V3();
 753     V3 v;
 754     Quat q, dq;
 755     //System.out.println("newMean mean " + mean);
 756     for (int i = data.length; --i >= 0;) {
 757       q = data[i];
 758       dq = q.div(mean);
 759       v = dq.getNormal();
 760       v.scale(dq.getTheta());
 761       sum.add(v);
 762     }
 763     sum.scale(1f/data.length);
 764     Quat dqMean = newVA(sum, sum.length());
 765     //System.out.println("newMean dqMean " + dqMean + " " + dqMean.getNormal() + " " + dqMean.getTheta());
 766     return dqMean.mulQ(mean);
 767   }
 768
 769   /**
 770    * @param data
 771    * @param mean
 772    * @return     standard deviation in units of degrees
 773    */
 774   private static float stdDev(Quat[] data, Quat mean) {
 775     // the quaternion dot product gives q0 for dq (i.e. q / mean)
 776     // that is, cos(theta/2) for theta between them
 777     double sum2 = 0;
 778     int n = data.length;
 779     for (int i = n; --i >= 0;) {
 780       float theta = data[i].div(mean).getTheta();
 781       sum2 += theta * theta;
 782     }
 783     return (float) Math.sqrt(sum2 / n);
 784   }
 785
 786   public float[] getEulerZYZ() {
 787     // http://www.swarthmore.edu/NatSci/mzucker1/e27/diebel2006attitude.pdf
 788     double rA, rB, rG;
 789     if (q1 == 0 && q2 == 0) {
 790       float theta = getTheta();
 791       // pure Z rotation - ambiguous
 792       return new float[] { q3 < 0 ? -theta : theta , 0, 0 };
 793     }
 794     rA = Math.atan2(2 * (q2 * q3 + q0 * q1), 2 * (-q1 * q3 + q0 * q2 ));
 795     rB = Math.acos(q3 * q3 - q2 * q2 - q1 * q1 + q0 * q0);
 796     rG = Math.atan2( 2 * (q2 * q3 - q0 * q1), 2 * (q0 * q2 + q1 * q3));
 797     return new float[]  {(float) (rA / RAD_PER_DEG), (float) (rB / RAD_PER_DEG), (float) (rG / RAD_PER_DEG)};
 798   }
 799
 800   public float[] getEulerZXZ() {
 801     // NOT http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
 802     // http://www.swarthmore.edu/NatSci/mzucker1/e27/diebel2006attitude.pdf
 803     double rA, rB, rG;
 804     if (q1 == 0 && q2 == 0) {
 805       float theta = getTheta();
 806       // pure Z rotation - ambiguous
 807       return new float[] { q3 < 0 ? -theta : theta , 0, 0 };
 808     }
 809     rA = Math.atan2(2 * (q1 * q3 - q0 * q2), 2 * (q0 * q1 + q2 * q3 ));
 810     rB = Math.acos(q3 * q3 - q2 * q2 - q1 * q1 + q0 * q0);
 811     rG = Math.atan2( 2 * (q1 * q3 + q0 * q2), 2 * (-q2 * q3 + q0 * q1));
 812     return new float[]  {(float) (rA / RAD_PER_DEG), (float) (rB / RAD_PER_DEG), (float) (rG / RAD_PER_DEG)};
 813   }
 814
 815 }