Merge branch 'features/r2_11_2/JAL-3821_reinstate_patch' into develop
[jalview.git] / unused / 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 }