45 #ifndef CERES_PUBLIC_ROTATION_H_
46 #define CERES_PUBLIC_ROTATION_H_
69 template <
typename T,
int row_str
ide,
int col_str
ide>
102 template <
typename T>
105 template <
typename T,
int row_str
ide,
int col_str
ide>
110 template <
typename T>
113 template <
typename T,
int row_str
ide,
int col_str
ide>
124 template <
typename T>
127 template <
typename T,
int row_str
ide,
int col_str
ide>
151 template <
typename T>
inline
154 template <
typename T,
int row_str
ide,
int col_str
ide>
inline
161 template <
typename T>
inline
164 template <
typename T,
int row_str
ide,
int col_str
ide>
inline
177 template <
typename T>
inline
182 template <
typename T>
inline
186 template<
typename T>
inline
190 template<
typename T>
inline
193 template<
typename T>
inline
197 template<
typename T>
inline
202 template<
typename T,
int row_str
ide,
int col_str
ide>
210 return pointer_[r * row_stride +
c * col_stride];
214 template <
typename T>
219 template <
typename T>
226 const T& a0 = angle_axis[0];
227 const T&
a1 = angle_axis[1];
228 const T&
a2 = angle_axis[2];
229 const T theta_squared = a0 * a0 +
a1 *
a1 +
a2 *
a2;
232 if (theta_squared >
T(0.0)) {
233 const T theta =
sqrt(theta_squared);
234 const T half_theta = theta *
T(0.5);
235 const T k =
sin(half_theta) / theta;
258 const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
262 if (sin_squared_theta >
T(0.0)) {
263 const T sin_theta =
sqrt(sin_squared_theta);
280 T(2.0) * ((cos_theta < 0.0)
281 ?
atan2(-sin_theta, -cos_theta)
282 :
atan2(sin_theta, cos_theta));
283 const T k = two_theta / sin_theta;
284 angle_axis[0] = q1 * k;
285 angle_axis[1] = q2 * k;
286 angle_axis[2] = q3 * k;
293 angle_axis[0] = q1 * k;
294 angle_axis[1] = q2 * k;
295 angle_axis[2] = q3 * k;
304 template <
typename T>
309 template <
typename T,
int row_str
ide,
int col_str
ide>
314 angle_axis[0] =
R(2, 1) -
R(1, 2);
315 angle_axis[1] =
R(0, 2) -
R(2, 0);
316 angle_axis[2] =
R(1, 0) -
R(0, 1);
318 static const T kOne =
T(1.0);
319 static const T kTwo =
T(2.0);
330 angle_axis[1] * angle_axis[1] +
331 angle_axis[2] * angle_axis[2]) / kTwo,
335 const T theta =
atan2(sintheta, costheta);
346 static const double kThreshold = 1
e-12;
347 if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
348 const T r = theta / (kTwo * sintheta);
349 for (
int i = 0;
i < 3; ++
i) {
357 if (costheta > 0.0) {
358 const T kHalf =
T(0.5);
359 for (
int i = 0;
i < 3; ++
i) {
360 angle_axis[
i] *= kHalf;
373 const T inv_one_minus_costheta =
kOne / (
kOne - costheta);
380 for (
int i = 0;
i < 3; ++
i) {
381 angle_axis[
i] = theta *
sqrt((
R(
i,
i) - costheta) * inv_one_minus_costheta);
382 if (((sintheta < 0.0) && (angle_axis[
i] > 0.0)) ||
383 ((sintheta > 0.0) && (angle_axis[
i] < 0.0))) {
384 angle_axis[
i] = -angle_axis[
i];
389 template <
typename T>
394 template <
typename T,
int row_str
ide,
int col_str
ide>
398 static const T kOne =
T(1.0);
399 const T theta2 =
DotProduct(angle_axis, angle_axis);
404 const T theta =
sqrt(theta2);
405 const T wx = angle_axis[0] / theta;
406 const T wy = angle_axis[1] / theta;
407 const T wz = angle_axis[2] / theta;
409 const T costheta =
cos(theta);
410 const T sintheta =
sin(theta);
412 R(0, 0) = costheta + wx*wx*(
kOne - costheta);
413 R(1, 0) = wz*sintheta + wx*wy*(
kOne - costheta);
414 R(2, 0) = -wy*sintheta + wx*wz*(
kOne - costheta);
415 R(0, 1) = wx*wy*(
kOne - costheta) - wz*sintheta;
416 R(1, 1) = costheta + wy*wy*(
kOne - costheta);
417 R(2, 1) = wx*sintheta + wy*wz*(
kOne - costheta);
418 R(0, 2) = wy*sintheta + wx*wz*(
kOne - costheta);
419 R(1, 2) = -wx*sintheta + wy*wz*(
kOne - costheta);
420 R(2, 2) = costheta + wz*wz*(
kOne - costheta);
424 R(1, 0) = angle_axis[2];
425 R(2, 0) = -angle_axis[1];
426 R(0, 1) = -angle_axis[2];
428 R(2, 1) = angle_axis[0];
429 R(0, 2) = angle_axis[1];
430 R(1, 2) = -angle_axis[0];
435 template <
typename T>
437 const int row_stride_parameter,
439 DCHECK(row_stride_parameter==3);
443 template <
typename T,
int row_str
ide,
int col_str
ide>
447 const double kPi = 3.14159265358979323846;
448 const T degrees_to_radians(kPi / 180.0);
450 const T pitch(euler[0] * degrees_to_radians);
451 const T roll(euler[1] * degrees_to_radians);
452 const T yaw(euler[2] * degrees_to_radians);
455 const T s1 =
sin(yaw);
457 const T s2 =
sin(roll);
458 const T c3 =
cos(pitch);
459 const T s3 =
sin(pitch);
462 R(0, 1) = -s1*c3 +
c1*s2*s3;
463 R(0, 2) = s1*s3 +
c1*s2*c3;
466 R(1, 1) =
c1*c3 + s1*s2*s3;
467 R(1, 2) = -
c1*s3 + s1*s2*c3;
474 template <
typename T>
inline
479 template <
typename T,
int row_str
ide,
int col_str
ide>
inline
501 R(0, 0) = aa + bb - cc - dd;
R(0, 1) =
T(2) * (bc - ad);
R(0, 2) =
T(2) * (ac + bd);
502 R(1, 0) =
T(2) * (ad + bc);
R(1, 1) = aa - bb + cc - dd;
R(1, 2) =
T(2) * (
cd - ab);
503 R(2, 0) =
T(2) * (bd - ac);
R(2, 1) =
T(2) * (ab +
cd);
R(2, 2) = aa - bb - cc + dd;
506 template <
typename T>
inline
511 template <
typename T,
int row_str
ide,
int col_str
ide>
inline
516 T normalizer =
q[0]*
q[0] +
q[1]*
q[1] +
q[2]*
q[2] +
q[3]*
q[3];
517 CHECK_NE(normalizer,
T(0));
518 normalizer =
T(1) / normalizer;
520 for (
int i = 0;
i < 3; ++
i) {
521 for (
int j = 0;
j < 3; ++
j) {
522 R(
i,
j) *= normalizer;
527 template <
typename T>
inline
529 const T t2 =
q[0] *
q[1];
530 const T t3 =
q[0] *
q[2];
531 const T t4 =
q[0] *
q[3];
532 const T t5 = -
q[1] *
q[1];
533 const T t6 =
q[1] *
q[2];
534 const T t7 =
q[1] *
q[3];
535 const T t8 = -
q[2] *
q[2];
536 const T t9 =
q[2] *
q[3];
537 const T t1 = -
q[3] *
q[3];
538 result[0] =
T(2) * ((t8 + t1) *
pt[0] + (t6 - t4) *
pt[1] + (t3 + t7) *
pt[2]) +
pt[0];
539 result[1] =
T(2) * ((t4 + t6) *
pt[0] + (t5 + t1) *
pt[1] + (t9 - t2) *
pt[2]) +
pt[1];
540 result[2] =
T(2) * ((t7 - t3) *
pt[0] + (t2 + t9) *
pt[1] + (t5 + t8) *
pt[2]) +
pt[2];
543 template <
typename T>
inline
562 template<
typename T>
inline
564 zw[0] =
z[0] *
w[0] -
z[1] *
w[1] -
z[2] *
w[2] -
z[3] *
w[3];
565 zw[1] =
z[0] *
w[1] +
z[1] *
w[0] +
z[2] *
w[3] -
z[3] *
w[2];
566 zw[2] =
z[0] *
w[2] -
z[1] *
w[3] +
z[2] *
w[0] +
z[3] *
w[1];
567 zw[3] =
z[0] *
w[3] +
z[1] *
w[2] -
z[2] *
w[1] +
z[3] *
w[0];
571 template<
typename T>
inline
573 x_cross_y[0] =
x[1] *
y[2] -
x[2] *
y[1];
574 x_cross_y[1] =
x[2] *
y[0] -
x[0] *
y[2];
575 x_cross_y[2] =
x[0] *
y[1] -
x[1] *
y[0];
578 template<
typename T>
inline
580 return (
x[0] *
y[0] +
x[1] *
y[1] +
x[2] *
y[2]);
583 template<
typename T>
inline
585 const T theta2 =
DotProduct(angle_axis, angle_axis);
597 const T theta =
sqrt(theta2);
598 const T costheta =
cos(theta);
599 const T sintheta =
sin(theta);
600 const T theta_inverse = 1.0 / theta;
602 const T w[3] = { angle_axis[0] * theta_inverse,
603 angle_axis[1] * theta_inverse,
604 angle_axis[2] * theta_inverse };
608 const T w_cross_pt[3] = {
w[1] *
pt[2] -
w[2] *
pt[1],
609 w[2] *
pt[0] -
w[0] *
pt[2],
610 w[0] *
pt[1] -
w[1] *
pt[0] };
612 (
w[0] *
pt[0] +
w[1] *
pt[1] +
w[2] *
pt[2]) * (
T(1.0) - costheta);
614 result[0] =
pt[0] * costheta + w_cross_pt[0] * sintheta +
w[0] * tmp;
615 result[1] =
pt[1] * costheta + w_cross_pt[1] * sintheta +
w[1] * tmp;
616 result[2] =
pt[2] * costheta + w_cross_pt[2] * sintheta +
w[2] * tmp;
635 const T w_cross_pt[3] = { angle_axis[1] *
pt[2] - angle_axis[2] *
pt[1],
636 angle_axis[2] *
pt[0] - angle_axis[0] *
pt[2],
637 angle_axis[0] *
pt[1] - angle_axis[1] *
pt[0] };
647 #endif // CERES_PUBLIC_ROTATION_H_