1 |
/* |
2 |
* Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project |
3 |
* |
4 |
* Contact: oopse@oopse.org |
5 |
* |
6 |
* This program is free software; you can redistribute it and/or |
7 |
* modify it under the terms of the GNU Lesser General Public License |
8 |
* as published by the Free Software Foundation; either version 2.1 |
9 |
* of the License, or (at your option) any later version. |
10 |
* All we ask is that proper credit is given for our work, which includes |
11 |
* - but is not limited to - adding the above copyright notice to the beginning |
12 |
* of your source code files, and to any copyright notice that you may distribute |
13 |
* with programs based on this work. |
14 |
* |
15 |
* This program is distributed in the hope that it will be useful, |
16 |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
17 |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
18 |
* GNU Lesser General Public License for more details. |
19 |
* |
20 |
* You should have received a copy of the GNU Lesser General Public License |
21 |
* along with this program; if not, write to the Free Software |
22 |
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
23 |
* |
24 |
*/ |
25 |
|
26 |
/** |
27 |
* @file SquareMatrix3.hpp |
28 |
* @author Teng Lin |
29 |
* @date 10/11/2004 |
30 |
* @version 1.0 |
31 |
*/ |
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> |
42 |
class SquareMatrix3 : public SquareMatrix<Real, 3> { |
43 |
public: |
44 |
|
45 |
/** default constructor */ |
46 |
SquareMatrix3() : SquareMatrix<Real, 3>() { |
47 |
} |
48 |
|
49 |
/** copy constructor */ |
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(); |
246 |
|
247 |
void diagonalize(); |
248 |
|
249 |
}; |
250 |
|
251 |
typedef template SquareMatrix3<double> Mat3x3d |
252 |
typedef template SquareMatrix3<double> RotMat3x3d; |
253 |
|
254 |
} //namespace oopse |
255 |
#endif // MATH_SQUAREMATRIX_HPP |