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

Comparing trunk/OOPSE-4/src/math/SquareMatrix3.hpp (file contents):
Revision 1569 by tim, Thu Oct 14 23:28:09 2004 UTC vs.
Revision 1586 by tim, Sun Oct 17 01:19:11 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_SQUAREMATRIX_HPP
33 > #define  MATH_SQUAREMATRIX_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)
# Line 58 | Line 78 | namespace oopse {
78               * Sets this matrix to a rotation matrix by three euler angles
79               * @ param euler
80               */
81 <            void setupRotMat(const Vector3d& euler);
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
# Line 66 | Line 88 | namespace oopse {
88               * @param theta
89               * @psi theta
90               */
91 <            void setupRotMat(double phi, double theta, double psi);
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 Vector4d& quat);
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 q0
127 <             * @param q1
128 <             * @param q2
129 <             * @parma q3
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(double q0, double q1, double q2, double q4);
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 rotMatToQuat();
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 quaternion 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 <            Vector3d rotMatToEuler();
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.
# Line 107 | Line 246 | namespace oopse {
246  
247              void diagonalize();
248  
110 –    }
111 –
249      };
250  
251 < }
252 < #endif // MATH_SQUAREMATRIX#_HPP
251 >    typedef template SquareMatrix3<double> Mat3x3d
252 >    typedef template SquareMatrix3<double> RotMat3x3d;
253 >
254 > } //namespace oopse
255 > #endif // MATH_SQUAREMATRIX_HPP

Diff Legend

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