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> |
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) |
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 |
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. |
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 |