51 |
|
#include "Quaternion.hpp" |
52 |
|
#include "SquareMatrix.hpp" |
53 |
|
#include "Vector3.hpp" |
54 |
< |
|
54 |
> |
#include "utils/NumericConstant.hpp" |
55 |
|
namespace oopse { |
56 |
|
|
57 |
|
template<typename Real> |
179 |
|
Real ad1, ad2, ad3; |
180 |
|
t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0; |
181 |
|
|
182 |
< |
if( t > 0.0 ){ |
182 |
> |
if( t > NumericConstant::epsilon ){ |
183 |
|
|
184 |
|
s = 0.5 / sqrt( t ); |
185 |
|
q[0] = 0.25 / s; |
194 |
|
|
195 |
|
if( ad1 >= ad2 && ad1 >= ad3 ){ |
196 |
|
|
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] = (this->data_[0][1] + this->data_[1][0]) / s; |
201 |
< |
q[3] = (this->data_[0][2] + this->data_[2][0]) / s; |
197 |
> |
s = 0.5 / 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.25 / 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 + 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] = (this->data_[1][2] + this->data_[2][1]) / s; |
203 |
> |
s = 0.5 / sqrt( 1.0 + this->data_[1][1] - this->data_[0][0] - this->data_[2][2] ); |
204 |
> |
q[0] = (this->data_[2][0] - this->data_[0][2] ) * s; |
205 |
> |
q[1] = (this->data_[0][1] + this->data_[1][0]) * s; |
206 |
> |
q[2] = 0.25 / s; |
207 |
> |
q[3] = (this->data_[1][2] + this->data_[2][1]) * s; |
208 |
|
} else { |
209 |
|
|
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; |
210 |
> |
s = 0.5 / sqrt( 1.0 + this->data_[2][2] - this->data_[0][0] - this->data_[1][1] ); |
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.25 / s; |
215 |
|
} |
216 |
|
} |
217 |
|
|