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