001        package com.croftsoft.core.math.quat;
002    
003        import com.croftsoft.core.math.axis.AxisAngleImp;
004        import com.croftsoft.core.math.axis.AxisAngleMut;
005        import com.croftsoft.core.math.matrix.Matrix3x3Imp;
006        import com.croftsoft.core.math.matrix.Matrix3x3Mut;
007    
008        /***********************************************************************
009        * A library of static methods to manipulate Quat objects.
010        * 
011        * @version
012        *   $Id: QuatLib.java,v 1.10 2008/09/20 02:51:51 croft Exp $
013        * @since
014        *   2008-05-02
015        * @author
016        *   <a href="https://www.croftsoft.com/">David Wallace Croft</a>
017        ***********************************************************************/
018    
019        public final class  QuatLib
020        ////////////////////////////////////////////////////////////////////////
021        ////////////////////////////////////////////////////////////////////////
022        {
023          
024        public static double  dotProduct (
025          final Quat  quat0,
026          final Quat  quat1 )
027        ////////////////////////////////////////////////////////////////////////
028        {
029          return quat0.getW ( ) * quat1.getW ( )
030               * quat0.getX ( ) * quat1.getX ( )
031               * quat0.getY ( ) * quat1.getY ( )
032               * quat0.getZ ( ) * quat1.getZ ( );
033        }
034        
035        public static QuatMut  fromEulerAngles (
036          final double  degreesX,
037          final double  degreesY,
038          final double  degreesZ )
039        ////////////////////////////////////////////////////////////////////////
040        {
041          final Quat  quatX = new AxisAngleImp ( degreesX, 0, 0, 1 ).toQuat ( );
042          
043          final Quat  quatY = new AxisAngleImp ( degreesY, 0, 1, 0 ).toQuat ( );
044          
045          final Quat  quatZ = new AxisAngleImp ( degreesZ, 0, 0, 1 ).toQuat ( );
046          
047          return quatZ.multiply ( quatY ).multiply ( quatX );
048        }
049        
050        public static boolean  matches (
051          final Quat  quat0,
052          final Quat  quat1 )
053        ////////////////////////////////////////////////////////////////////////
054        {
055          return quat0.getW ( ) == quat1.getW ( )
056              && quat0.getX ( ) == quat1.getX ( )
057              && quat0.getY ( ) == quat1.getY ( )
058              && quat0.getZ ( ) == quat1.getZ ( );
059        }
060          
061        public static boolean  matches (
062          final Quat    quat0,
063          final Quat    quat1,
064          final double  tolerance )
065        ////////////////////////////////////////////////////////////////////////
066        {
067          if ( tolerance < 0 )
068          {
069            throw new IllegalArgumentException ( "tolerance < 0" );
070          }
071          
072          return Math.abs ( quat0.getW ( ) - quat1.getW ( ) ) <= tolerance
073              && Math.abs ( quat0.getX ( ) - quat1.getX ( ) ) <= tolerance
074              && Math.abs ( quat0.getY ( ) - quat1.getY ( ) ) <= tolerance
075              && Math.abs ( quat0.getZ ( ) - quat1.getZ ( ) ) <= tolerance;
076        }
077          
078        public static QuatMut  multiply (
079          final Quat  quat0,
080          final Quat  quat1 )
081        ////////////////////////////////////////////////////////////////////////
082        {
083          final double  w0 = quat0.getW ( );
084          
085          final double  x0 = quat0.getX ( );
086          
087          final double  y0 = quat0.getY ( );
088          
089          final double  z0 = quat0.getZ ( );
090          
091          final double  w1 = quat1.getW ( );
092          
093          final double  x1 = quat1.getX ( );
094          
095          final double  y1 = quat1.getY ( );
096          
097          final double  z1 = quat1.getZ ( );
098          
099          return new QuatImp (
100            w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1,
101            y0 * z1 - z0 * y1 + w0 * x1 + x0 * w1,
102            z0 * x1 - x0 * z1 + w0 * y1 + y0 * w1,
103            x0 * y1 - y0 * x1 + w0 * z1 + z0 * w1 );
104        }
105        
106        public static AxisAngleMut  toAxisAngle ( final Quat  quat )
107        ////////////////////////////////////////////////////////////////////////
108        {
109          final double  w = quat.getW ( );
110          
111          final double  sinThetaOver2Sq = 1 - w * w;
112          
113          if ( sinThetaOver2Sq <= 0 )
114          {
115            return new AxisAngleImp ( );
116          }
117          
118          final double  oneOverSinThetaOver2
119            = 1 / Math.sqrt ( sinThetaOver2Sq );
120          
121          return new AxisAngleImp (
122            Math.toDegrees ( 2 * Math.acos ( w ) ),
123            quat.getX ( ) * oneOverSinThetaOver2,
124            quat.getY ( ) * oneOverSinThetaOver2,
125            quat.getZ ( ) * oneOverSinThetaOver2 );
126        }
127        
128        public static Matrix3x3Mut  toRotationMatrix ( final Quat  quat )
129        ////////////////////////////////////////////////////////////////////////
130        {
131          final double  w = quat.getW ( );
132          
133          final double  x = quat.getX ( );
134          
135          final double  y = quat.getY ( );
136          
137          final double  z = quat.getZ ( );
138          
139          final double  wx = w * x;
140          
141          final double  wy = w * y;
142          
143          final double  wz = w * z;
144          
145          final double  xx = x * x;
146          
147          final double  xy = x * y;
148          
149          final double  xz = x * z;
150          
151          final double  yy = y * y;
152          
153          final double  yz = y * z;
154          
155          final double  zz = z * z;
156          
157          return new Matrix3x3Imp (
158            1 - 2 * ( yy + zz ),
159                2 * ( xy - wz ),
160                2 * ( wy + xz ),
161                2 * ( xy + wz ),
162            1 - 2 * ( xx + zz ),
163                2 * ( yz - wx ),
164                2 * ( xz - wy ),
165                2 * ( yz + wx ),
166            1 - 2 * ( xx + yy ) );
167        }
168        
169        public static String  toString ( final Quat  quat )
170        ////////////////////////////////////////////////////////////////////////
171        {
172          return String.format (
173            "%1$1.3f; %2$1.3f, %3$1.3f, %4$1.3f",
174            new Double ( quat.getW ( ) ),
175            new Double ( quat.getX ( ) ),
176            new Double ( quat.getY ( ) ),
177            new Double ( quat.getZ ( ) ) );
178        }
179        
180        ////////////////////////////////////////////////////////////////////////
181        // private methods
182        ////////////////////////////////////////////////////////////////////////
183        
184        private  QuatLib ( )
185        ////////////////////////////////////////////////////////////////////////
186        {
187          // empty
188        }
189        
190        ////////////////////////////////////////////////////////////////////////
191        ////////////////////////////////////////////////////////////////////////
192        }