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