001 package com.croftsoft.core.math.matrix;
002
003 /***********************************************************************
004 * A library of static methods to manipulate Matrix3x3 objects.
005 *
006 * @version
007 * $Id: Matrix3x3Lib.java,v 1.8 2008/05/09 19:48:45 croft Exp $
008 * @since
009 * 2008-04-25
010 * @author
011 * <a href="https://www.croftsoft.com/">David Wallace Croft</a>
012 ***********************************************************************/
013
014 public final class Matrix3x3Lib
015 ////////////////////////////////////////////////////////////////////////
016 ////////////////////////////////////////////////////////////////////////
017 {
018
019 public static Matrix3x3Mut createRotationMatrix (
020 final double degreesX,
021 final double degreesY,
022 final double degreesZ )
023 ////////////////////////////////////////////////////////////////////////
024 {
025 // Rotation matrices multiplied in this order: R = Rz * Ry * Rx
026
027 final double cx = Math.cos ( Math.toRadians ( degreesX ) );
028
029 final double sx = Math.sin ( Math.toRadians ( degreesX ) );
030
031 final double cy = Math.cos ( Math.toRadians ( degreesY ) );
032
033 final double sy = Math.sin ( Math.toRadians ( degreesY ) );
034
035 final double cz = Math.cos ( Math.toRadians ( degreesZ ) );
036
037 final double sz = Math.sin ( Math.toRadians ( degreesZ ) );
038
039 final Matrix3x3Mut matrix3x3Mut = new Matrix3x3Imp (
040 new double [ ] [ ] {
041 { cy * cz,
042 -cx * sz + sx * sy * cz,
043 sx * sz + cx * sy * cz },
044 { cy * sz,
045 cx * cz + sx * sy * sz,
046 -sx * cz + cx * sy * sz },
047 { -sy,
048 sx * cy,
049 cx * cy } } );
050
051 return matrix3x3Mut;
052 }
053
054 public static Matrix3x3Mut createRotationMatrixX (
055 final double degrees )
056 ////////////////////////////////////////////////////////////////////////
057 {
058 final double cos = Math.cos ( Math.toRadians ( degrees ) );
059
060 final double sin = Math.sin ( Math.toRadians ( degrees ) );
061
062 return new Matrix3x3Imp (
063 1, 0, 0,
064 0, cos, -sin,
065 0, sin, cos );
066 }
067
068 public static Matrix3x3Mut createRotationMatrixY (
069 final double degrees )
070 ////////////////////////////////////////////////////////////////////////
071 {
072 final double cos = Math.cos ( Math.toRadians ( degrees ) );
073
074 final double sin = Math.sin ( Math.toRadians ( degrees ) );
075
076 return new Matrix3x3Imp (
077 cos, 0, sin,
078 0, 1, 0,
079 -sin, 0, cos );
080 }
081
082 public static Matrix3x3Mut createRotationMatrixZ (
083 final double degrees )
084 ////////////////////////////////////////////////////////////////////////
085 {
086 final double cos = Math.cos ( Math.toRadians ( degrees ) );
087
088 final double sin = Math.sin ( Math.toRadians ( degrees ) );
089
090 return new Matrix3x3Imp (
091 cos, -sin, 0,
092 sin, cos, 0,
093 0, 0, 1 );
094 }
095
096 public static Matrix3x3Mut multiply3x3 (
097 final Matrix3x3 matrix3x3a,
098 final Matrix3x3 matrix3x3b )
099 ////////////////////////////////////////////////////////////////////////
100 {
101 return new Matrix3x3Imp (
102 MatrixLib.multiply ( matrix3x3a, matrix3x3b ) );
103 }
104
105 public static double [ ] toEulerAngles ( final Matrix3x3 matrix3x3 )
106 ////////////////////////////////////////////////////////////////////////
107 {
108 // Adapted from Dunn and Parberry, 3D Math Primer, 2002, page 204.
109
110 double sp = -matrix3x3.get ( 1, 2 );
111
112 double heading = 0.0;
113
114 double pitch = 0.0;
115
116 double bank = 0.0;
117
118 if ( Math.abs ( sp ) > 0.99999 )
119 {
120 heading = Math.atan2 (
121 -matrix3x3.get ( 2, 0 ),
122 matrix3x3.get ( 0, 0 ) );
123
124 pitch = ( Math.PI / 2.0 ) * sp;
125
126 bank = 0.0;
127 }
128 else
129 {
130 heading = Math.atan2 (
131 matrix3x3.get ( 0, 2 ),
132 matrix3x3.get ( 2, 2 ) );
133
134 pitch = Math.asin ( sp );
135
136 bank = Math.atan2 (
137 matrix3x3.get ( 1, 0 ),
138 matrix3x3.get ( 1, 1 ) );
139 }
140
141 // final double [ ] canonizedEulerAngles = canonizeEulerAngles (
142 // new double [ ] { heading, pitch, bank } );
143
144 // Change order from heading, pitch, bank to x, y, z
145
146 return new double [ ] {
147 Math.toDegrees ( pitch ),
148 Math.toDegrees ( heading ),
149 Math.toDegrees ( bank ) };
150
151 // return new double [ ] {
152 // Math.toDegrees ( canonizedEulerAngles [ 1 ] ),
153 // Math.toDegrees ( canonizedEulerAngles [ 0 ] ),
154 // Math.toDegrees ( canonizedEulerAngles [ 2 ] ) };
155 }
156
157 public static Matrix3x3Mut transpose3x3 ( final Matrix3x3 matrix3x3 )
158 ////////////////////////////////////////////////////////////////////////
159 {
160 return new Matrix3x3Imp (
161 matrix3x3.get ( 0, 0 ),
162 matrix3x3.get ( 1, 0 ),
163 matrix3x3.get ( 2, 0 ),
164 matrix3x3.get ( 0, 1 ),
165 matrix3x3.get ( 1, 1 ),
166 matrix3x3.get ( 2, 1 ),
167 matrix3x3.get ( 0, 2 ),
168 matrix3x3.get ( 1, 2 ),
169 matrix3x3.get ( 2, 2 ) );
170 }
171
172 ////////////////////////////////////////////////////////////////////////
173 // private methods
174 ////////////////////////////////////////////////////////////////////////
175
176 // private static double [ ] canonizeEulerAngles (
177 // final double [ ] eulerAngles )
178 // //////////////////////////////////////////////////////////////////////
179 // {
180 // // Adapted from Dunn and Parberry, 3D Math Primer, 2002, page 201.
181 //
182 // double heading = eulerAngles [ 0 ];
183 //
184 // double pitch = wrapPi ( eulerAngles [ 1 ] );
185 //
186 // double bank = eulerAngles [ 2 ];
187 //
188 // if ( pitch < -( Math.PI / 2 ) )
189 // {
190 // pitch = -Math.PI - pitch;
191 //
192 // heading += Math.PI;
193 //
194 // bank += Math.PI;
195 // }
196 // else if ( pitch > ( Math.PI / 2 ) )
197 // {
198 // pitch = Math.PI - pitch;
199 //
200 // heading += Math.PI;
201 //
202 // bank += Math.PI;
203 // }
204 //
205 // if ( Math.abs ( pitch ) > ( Math.PI / 2 ) - 1e-4 )
206 // {
207 // heading += bank;
208 //
209 // bank = 0;
210 // }
211 // else
212 // {
213 // bank = wrapPi ( bank );
214 // }
215 //
216 // heading = wrapPi ( heading );
217 //
218 // return new double [ ] { heading, pitch, bank };
219 // }
220 //
221 // private static double wrapPi ( double theta )
222 // //////////////////////////////////////////////////////////////////////
223 // {
224 // if ( theta > Math.PI )
225 // {
226 // while ( theta > Math.PI ) theta -= Math.PI;
227 // }
228 // else if ( theta < -Math.PI )
229 // {
230 // while ( theta < -Math.PI ) theta += Math.PI;
231 // }
232 //
233 // return theta;
234 // }
235
236 private Matrix3x3Lib ( )
237 ////////////////////////////////////////////////////////////////////////
238 {
239 // empty
240 }
241
242 ////////////////////////////////////////////////////////////////////////
243 ////////////////////////////////////////////////////////////////////////
244 }