134 |
|
ctheta = cos(theta); |
135 |
|
cpsi = cos(psi); |
136 |
|
|
137 |
< |
data_[0][0] = cpsi * cphi - ctheta * sphi * spsi; |
138 |
< |
data_[0][1] = cpsi * sphi + ctheta * cphi * spsi; |
139 |
< |
data_[0][2] = spsi * stheta; |
137 |
> |
this->data_[0][0] = cpsi * cphi - ctheta * sphi * spsi; |
138 |
> |
this->data_[0][1] = cpsi * sphi + ctheta * cphi * spsi; |
139 |
> |
this->data_[0][2] = spsi * stheta; |
140 |
|
|
141 |
< |
data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi; |
142 |
< |
data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi; |
143 |
< |
data_[1][2] = cpsi * stheta; |
141 |
> |
this->data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi; |
142 |
> |
this->data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi; |
143 |
> |
this->data_[1][2] = cpsi * stheta; |
144 |
|
|
145 |
< |
data_[2][0] = stheta * sphi; |
146 |
< |
data_[2][1] = -stheta * cphi; |
147 |
< |
data_[2][2] = ctheta; |
145 |
> |
this->data_[2][0] = stheta * sphi; |
146 |
> |
this->data_[2][1] = -stheta * cphi; |
147 |
> |
this->data_[2][2] = ctheta; |
148 |
|
} |
149 |
|
|
150 |
|
|
177 |
|
Quaternion<Real> q; |
178 |
|
Real t, s; |
179 |
|
Real ad1, ad2, ad3; |
180 |
< |
t = data_[0][0] + data_[1][1] + data_[2][2] + 1.0; |
180 |
> |
t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0; |
181 |
|
|
182 |
|
if( t > 0.0 ){ |
183 |
|
|
184 |
|
s = 0.5 / sqrt( t ); |
185 |
|
q[0] = 0.25 / s; |
186 |
< |
q[1] = (data_[1][2] - data_[2][1]) * s; |
187 |
< |
q[2] = (data_[2][0] - data_[0][2]) * s; |
188 |
< |
q[3] = (data_[0][1] - data_[1][0]) * s; |
186 |
> |
q[1] = (this->data_[1][2] - this->data_[2][1]) * s; |
187 |
> |
q[2] = (this->data_[2][0] - this->data_[0][2]) * s; |
188 |
> |
q[3] = (this->data_[0][1] - this->data_[1][0]) * s; |
189 |
|
} else { |
190 |
|
|
191 |
< |
ad1 = fabs( data_[0][0] ); |
192 |
< |
ad2 = fabs( data_[1][1] ); |
193 |
< |
ad3 = fabs( data_[2][2] ); |
191 |
> |
ad1 = fabs( this->data_[0][0] ); |
192 |
> |
ad2 = fabs( this->data_[1][1] ); |
193 |
> |
ad3 = fabs( this->data_[2][2] ); |
194 |
|
|
195 |
|
if( ad1 >= ad2 && ad1 >= ad3 ){ |
196 |
|
|
197 |
< |
s = 2.0 * sqrt( 1.0 + data_[0][0] - data_[1][1] - data_[2][2] ); |
198 |
< |
q[0] = (data_[1][2] + data_[2][1]) / s; |
197 |
> |
s = 2.0 * sqrt( 1.0 + this->data_[0][0] - this->data_[1][1] - this->data_[2][2] ); |
198 |
> |
q[0] = (this->data_[1][2] + this->data_[2][1]) / s; |
199 |
|
q[1] = 0.5 / s; |
200 |
< |
q[2] = (data_[0][1] + data_[1][0]) / s; |
201 |
< |
q[3] = (data_[0][2] + data_[2][0]) / s; |
200 |
> |
q[2] = (this->data_[0][1] + this->data_[1][0]) / s; |
201 |
> |
q[3] = (this->data_[0][2] + this->data_[2][0]) / s; |
202 |
|
} else if ( ad2 >= ad1 && ad2 >= ad3 ) { |
203 |
< |
s = sqrt( 1.0 + data_[1][1] - data_[0][0] - data_[2][2] ) * 2.0; |
204 |
< |
q[0] = (data_[0][2] + data_[2][0]) / s; |
205 |
< |
q[1] = (data_[0][1] + data_[1][0]) / s; |
203 |
> |
s = sqrt( 1.0 + this->data_[1][1] - this->data_[0][0] - this->data_[2][2] ) * 2.0; |
204 |
> |
q[0] = (this->data_[0][2] + this->data_[2][0]) / s; |
205 |
> |
q[1] = (this->data_[0][1] + this->data_[1][0]) / s; |
206 |
|
q[2] = 0.5 / s; |
207 |
< |
q[3] = (data_[1][2] + data_[2][1]) / s; |
207 |
> |
q[3] = (this->data_[1][2] + this->data_[2][1]) / s; |
208 |
|
} else { |
209 |
|
|
210 |
< |
s = sqrt( 1.0 + data_[2][2] - data_[0][0] - data_[1][1] ) * 2.0; |
211 |
< |
q[0] = (data_[0][1] + data_[1][0]) / s; |
212 |
< |
q[1] = (data_[0][2] + data_[2][0]) / s; |
213 |
< |
q[2] = (data_[1][2] + data_[2][1]) / s; |
210 |
> |
s = sqrt( 1.0 + this->data_[2][2] - this->data_[0][0] - this->data_[1][1] ) * 2.0; |
211 |
> |
q[0] = (this->data_[0][1] + this->data_[1][0]) / s; |
212 |
> |
q[1] = (this->data_[0][2] + this->data_[2][0]) / s; |
213 |
> |
q[2] = (this->data_[1][2] + this->data_[2][1]) / s; |
214 |
|
q[3] = 0.5 / s; |
215 |
|
} |
216 |
|
} |
239 |
|
|
240 |
|
// set the tolerance for Euler angles and rotation elements |
241 |
|
|
242 |
< |
theta = acos(std::min(1.0, std::max(-1.0,data_[2][2]))); |
243 |
< |
ctheta = data_[2][2]; |
242 |
> |
theta = acos(std::min(1.0, std::max(-1.0,this->data_[2][2]))); |
243 |
> |
ctheta = this->data_[2][2]; |
244 |
|
stheta = sqrt(1.0 - ctheta * ctheta); |
245 |
|
|
246 |
|
// when sin(theta) is close to 0, we need to consider singularity |
253 |
|
|
254 |
|
if (fabs(stheta) <= oopse::epsilon){ |
255 |
|
psi = 0.0; |
256 |
< |
phi = atan2(-data_[1][0], data_[0][0]); |
256 |
> |
phi = atan2(-this->data_[1][0], this->data_[0][0]); |
257 |
|
} |
258 |
|
// we only have one unique solution |
259 |
|
else{ |
260 |
< |
phi = atan2(data_[2][0], -data_[2][1]); |
261 |
< |
psi = atan2(data_[0][2], data_[1][2]); |
260 |
> |
phi = atan2(this->data_[2][0], -this->data_[2][1]); |
261 |
> |
psi = atan2(this->data_[0][2], this->data_[1][2]); |
262 |
|
} |
263 |
|
|
264 |
|
//wrap phi and psi, make sure they are in the range from 0 to 2*Pi |
279 |
|
Real determinant() const { |
280 |
|
Real x,y,z; |
281 |
|
|
282 |
< |
x = data_[0][0] * (data_[1][1] * data_[2][2] - data_[1][2] * data_[2][1]); |
283 |
< |
y = data_[0][1] * (data_[1][2] * data_[2][0] - data_[1][0] * data_[2][2]); |
284 |
< |
z = data_[0][2] * (data_[1][0] * data_[2][1] - data_[1][1] * data_[2][0]); |
282 |
> |
x = this->data_[0][0] * (this->data_[1][1] * this->data_[2][2] - this->data_[1][2] * this->data_[2][1]); |
283 |
> |
y = this->data_[0][1] * (this->data_[1][2] * this->data_[2][0] - this->data_[1][0] * this->data_[2][2]); |
284 |
> |
z = this->data_[0][2] * (this->data_[1][0] * this->data_[2][1] - this->data_[1][1] * this->data_[2][0]); |
285 |
|
|
286 |
|
return(x + y + z); |
287 |
|
} |
288 |
|
|
289 |
|
/** Returns the trace of this matrix. */ |
290 |
|
Real trace() const { |
291 |
< |
return data_[0][0] + data_[1][1] + data_[2][2]; |
291 |
> |
return this->data_[0][0] + this->data_[1][1] + this->data_[2][2]; |
292 |
|
} |
293 |
|
|
294 |
|
/** |
304 |
|
//"This is a runtime or a programming error in your application."); |
305 |
|
} |
306 |
|
|
307 |
< |
m(0, 0) = data_[1][1]*data_[2][2] - data_[1][2]*data_[2][1]; |
308 |
< |
m(1, 0) = data_[1][2]*data_[2][0] - data_[1][0]*data_[2][2]; |
309 |
< |
m(2, 0) = data_[1][0]*data_[2][1] - data_[1][1]*data_[2][0]; |
310 |
< |
m(0, 1) = data_[2][1]*data_[0][2] - data_[2][2]*data_[0][1]; |
311 |
< |
m(1, 1) = data_[2][2]*data_[0][0] - data_[2][0]*data_[0][2]; |
312 |
< |
m(2, 1) = data_[2][0]*data_[0][1] - data_[2][1]*data_[0][0]; |
313 |
< |
m(0, 2) = data_[0][1]*data_[1][2] - data_[0][2]*data_[1][1]; |
314 |
< |
m(1, 2) = data_[0][2]*data_[1][0] - data_[0][0]*data_[1][2]; |
315 |
< |
m(2, 2) = data_[0][0]*data_[1][1] - data_[0][1]*data_[1][0]; |
307 |
> |
m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1]; |
308 |
> |
m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2]; |
309 |
> |
m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0]; |
310 |
> |
m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1]; |
311 |
> |
m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2]; |
312 |
> |
m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0]; |
313 |
> |
m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1]; |
314 |
> |
m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2]; |
315 |
> |
m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0]; |
316 |
|
|
317 |
|
m /= det; |
318 |
|
return m; |