src/javajs/util/Quat.java

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