ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE-3.0/src/math/SquareMatrix3.hpp
(Generate patch)

Comparing trunk/OOPSE-3.0/src/math/SquareMatrix3.hpp (file contents):
Revision 1563 by tim, Wed Oct 13 06:51:09 2004 UTC vs.
Revision 1592 by tim, Mon Oct 18 17:07:27 2004 UTC

# Line 29 | Line 29
29   * @date 10/11/2004
30   * @version 1.0
31   */
32 < #ifndef MATH_SQUAREMATRIX#_HPP
33 < #define  MATH_SQUAREMATRIX#_HPP
32 > #ifndef MATH_SQUAREMATRIX3_HPP
33 > #define  MATH_SQUAREMATRIX3_HPP
34  
35 + #include "Quaternion.hpp"
36   #include "SquareMatrix.hpp"
37 + #include "Vector3.hpp"
38 +
39   namespace oopse {
40  
41      template<typename Real>
# Line 47 | Line 50 | namespace oopse {
50              SquareMatrix3(const SquareMatrix<Real, 3>& m)  : SquareMatrix<Real, 3>(m) {
51              }
52  
53 +            SquareMatrix3( const Vector3<Real>& eulerAngles) {
54 +                setupRotMat(eulerAngles);
55 +            }
56 +            
57 +            SquareMatrix3(Real phi, Real theta, Real psi) {
58 +                setupRotMat(phi, theta, psi);
59 +            }
60 +
61 +            SquareMatrix3(const Quaternion<Real>& q) {
62 +                *this = q.toRotationMatrix3();
63 +            }
64 +
65 +            SquareMatrix3(Real w, Real x, Real y, Real z) {
66 +                Quaternion<Real> q(w, x, y, z);
67 +                *this = q.toRotationMatrix3();
68 +            }
69 +            
70              /** copy assignment operator */
71              SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) {
72                  if (this == &m)
73                      return *this;
74                   SquareMatrix<Real, 3>::operator=(m);
75              }
76 +
77 +            /**
78 +             * Sets this matrix to a rotation matrix by three euler angles
79 +             * @ param euler
80 +             */
81 +            void setupRotMat(const Vector3<Real>& eulerAngles) {
82 +                setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
83 +            }
84 +
85 +            /**
86 +             * Sets this matrix to a rotation matrix by three euler angles
87 +             * @param phi
88 +             * @param theta
89 +             * @psi theta
90 +             */
91 +            void setupRotMat(Real phi, Real theta, Real psi) {
92 +                Real sphi, stheta, spsi;
93 +                Real cphi, ctheta, cpsi;
94 +
95 +                sphi = sin(phi);
96 +                stheta = sin(theta);
97 +                spsi = sin(psi);
98 +                cphi = cos(phi);
99 +                ctheta = cos(theta);
100 +                cpsi = cos(psi);
101 +
102 +                data_[0][0] = cpsi * cphi - ctheta * sphi * spsi;
103 +                data_[0][1] = cpsi * sphi + ctheta * cphi * spsi;
104 +                data_[0][2] = spsi * stheta;
105 +                
106 +                data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi;
107 +                data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi;
108 +                data_[1][2] = cpsi * stheta;
109 +
110 +                data_[2][0] = stheta * sphi;
111 +                data_[2][1] = -stheta * cphi;
112 +                data_[2][2] = ctheta;
113 +            }
114 +
115 +
116 +            /**
117 +             * Sets this matrix to a rotation matrix by quaternion
118 +             * @param quat
119 +            */
120 +            void setupRotMat(const Quaternion<Real>& quat) {
121 +                *this = quat.toRotationMatrix3();
122 +            }
123 +
124 +            /**
125 +             * Sets this matrix to a rotation matrix by quaternion
126 +             * @param w the first element
127 +             * @param x the second element
128 +             * @param y the third element
129 +             * @parma z the fourth element
130 +            */
131 +            void setupRotMat(Real w, Real x, Real y, Real z) {
132 +                Quaternion<Real> q(w, x, y, z);
133 +                *this = q.toRotationMatrix3();
134 +            }
135 +
136 +            /**
137 +             * Returns the quaternion from this rotation matrix
138 +             * @return the quaternion from this rotation matrix
139 +             * @exception invalid rotation matrix
140 +            */            
141 +            Quaternion<Real> toQuaternion() {
142 +                Quaternion<Real> q;
143 +                Real t, s;
144 +                Real ad1, ad2, ad3;    
145 +                t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0;
146 +
147 +                if( t > 0.0 ){
148 +
149 +                    s = 0.5 / sqrt( t );
150 +                    q[0] = 0.25 / s;
151 +                    q[1] = (data_[1][2] - data_[2][1]) * s;
152 +                    q[2] = (data_[2][0] - data_[0][2]) * s;
153 +                    q[3] = (data_[0][1] - data_[1][0]) * s;
154 +                } else {
155 +
156 +                    ad1 = fabs( data_[0][0] );
157 +                    ad2 = fabs( data_[1][1] );
158 +                    ad3 = fabs( data_[2][2] );
159 +
160 +                    if( ad1 >= ad2 && ad1 >= ad3 ){
161 +
162 +                        s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] );
163 +                        q[0] = (data_[1][2] + data_[2][1]) / s;
164 +                        q[1] = 0.5 / s;
165 +                        q[2] = (data_[0][1] + data_[1][0]) / s;
166 +                        q[3] = (data_[0][2] + data_[2][0]) / s;
167 +                    } else if ( ad2 >= ad1 && ad2 >= ad3 ) {
168 +                        s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0;
169 +                        q[0] = (data_[0][2] + data_[2][0]) / s;
170 +                        q[1] = (data_[0][1] + data_[1][0]) / s;
171 +                        q[2] = 0.5 / s;
172 +                        q[3] = (data_[1][2] + data_[2][1]) / s;
173 +                    } else {
174 +
175 +                        s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0;
176 +                        q[0] = (data_[0][1] + data_[1][0]) / s;
177 +                        q[1] = (data_[0][2] + data_[2][0]) / s;
178 +                        q[2] = (data_[1][2] + data_[2][1]) / s;
179 +                        q[3] = 0.5 / s;
180 +                    }
181 +                }            
182 +
183 +                return q;
184 +                
185 +            }
186 +
187 +            /**
188 +             * Returns the euler angles from this rotation matrix
189 +             * @return the euler angles in a vector
190 +             * @exception invalid rotation matrix
191 +             * We use so-called "x-convention", which is the most common definition.
192 +             * In this convention, the rotation given by Euler angles (phi, theta, psi), where the first
193 +             * rotation is by an angle phi about the z-axis, the second is by an angle  
194 +             * theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the
195 +             * z-axis (again).
196 +            */            
197 +            Vector3<Real> toEulerAngles() {
198 +                Vector<Real> myEuler;
199 +                Real phi,theta,psi,eps;
200 +                Real ctheta,stheta;
201 +                
202 +                // set the tolerance for Euler angles and rotation elements
203 +
204 +                theta = acos(min(1.0,max(-1.0,data_[2][2])));
205 +                ctheta = data_[2][2];
206 +                stheta = sqrt(1.0 - ctheta * ctheta);
207 +
208 +                // when sin(theta) is close to 0, we need to consider singularity
209 +                // In this case, we can assign an arbitary value to phi (or psi), and then determine
210 +                // the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0
211 +                // in cases of singularity.  
212 +                // we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
213 +                // Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never
214 +                // change the sign of both of the parameters passed to atan2.
215 +
216 +                if (fabs(stheta) <= oopse::epsilon){
217 +                    psi = 0.0;
218 +                    phi = atan2(-data_[1][0], data_[0][0]);  
219 +                }
220 +                // we only have one unique solution
221 +                else{    
222 +                    phi = atan2(data_[2][0], -data_[2][1]);
223 +                    psi = atan2(data_[0][2], data_[1][2]);
224 +                }
225 +
226 +                //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
227 +                if (phi < 0)
228 +                  phi += M_PI;
229 +
230 +                if (psi < 0)
231 +                  psi += M_PI;
232 +
233 +                myEuler[0] = phi;
234 +                myEuler[1] = theta;
235 +                myEuler[2] = psi;
236 +
237 +                return myEuler;
238 +            }
239              
240              /**
241               * Sets the value of this matrix to  the inversion of itself.
242               * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the
243               * implementation of inverse in SquareMatrix class
244               */
245 <            void  inverse();
63 <            
64 <            /**
65 <             * Sets the value of this matrix to  the inversion of other matrix.
66 <             * @ param m the source matrix
67 <             */        
68 <            void inverse(const SquareMatrix<Real, Dim>& m);
245 >            void  inverse() {
246  
247 <    }
247 >            }
248  
249 +            void diagonalize() {
250 +
251 +            }
252      };
253  
254 < }
255 < #endif // MATH_SQUAREMATRIX#_HPP
254 >    typedef SquareMatrix3<double> Mat3x3d;
255 >    typedef SquareMatrix3<double> RotMat3x3d;
256 >
257 > } //namespace oopse
258 > #endif // MATH_SQUAREMATRIX_HPP

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines