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="http://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 }