31 #ifndef OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED
32 #define OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED
41 #include <openvdb/Exceptions.h>
49 template<
typename T>
class Quat;
55 T qdot,
angle, sineAngle;
59 if (fabs(qdot) >= 1.0) {
64 sineAngle = sin(angle);
76 Quat<T> qtemp(s * q1[0] + t * q2[0], s * q1[1] + t * q2[1],
77 s * q1[2] + t * q2[2], s * q1[3] + t * q2[3]);
84 double lengthSquared = qtemp.
dot(qtemp);
87 qtemp = (t < 0.5) ? q1 : q2;
89 qtemp *= 1.0 / sqrt(lengthSquared);
94 T sine = 1.0 / sineAngle;
95 T a = sin((1.0 - t) * angle) * sine;
96 T b = sin(t * angle) * sine;
97 return Quat<T>(a * q1[0] + b * q2[0], a * q1[1] + b * q2[1],
98 a * q1[2] + b * q2[2], a * q1[3] + b * q2[3]);
136 T s = sin(angle*T(0.5));
138 mm[0] = axis.
x() * s;
139 mm[1] = axis.
y() * s;
140 mm[2] = axis.
z() * s;
142 mm[3] = cos(angle*T(0.5));
149 T s = sin(angle*T(0.5));
155 mm[3] = cos(angle*T(0.5));
159 template<
typename T1>
165 "A non-rotation matrix can not be used to construct a quaternion");
169 "A reflection matrix can not be used to construct a quaternion");
172 T trace = (T)rot.
trace();
175 T q_w = 0.5 * std::sqrt(trace+1);
176 T factor = 0.25 / q_w;
178 mm[0] = factor * (rot(1,2) - rot(2,1));
179 mm[1] = factor * (rot(2,0) - rot(0,2));
180 mm[2] = factor * (rot(0,1) - rot(1,0));
182 }
else if (rot(0,0) > rot(1,1) && rot(0,0) > rot(2,2)) {
184 T q_x = 0.5 * sqrt(rot(0,0)- rot(1,1)-rot(2,2)+1);
185 T factor = 0.25 / q_x;
188 mm[1] = factor * (rot(0,1) + rot(1,0));
189 mm[2] = factor * (rot(2,0) + rot(0,2));
190 mm[3] = factor * (rot(1,2) - rot(2,1));
191 }
else if (rot(1,1) > rot(2,2)) {
193 T q_y = 0.5 * sqrt(rot(1,1)-rot(0,0)-rot(2,2)+1);
194 T factor = 0.25 / q_y;
196 mm[0] = factor * (rot(0,1) + rot(1,0));
198 mm[2] = factor * (rot(1,2) + rot(2,1));
199 mm[3] = factor * (rot(2,0) - rot(0,2));
202 T q_z = 0.5 * sqrt(rot(2,2)-rot(0,0)-rot(1,1)+1);
203 T factor = 0.25 / q_z;
205 mm[0] = factor * (rot(2,0) + rot(0,2));
206 mm[1] = factor * (rot(1,2) + rot(2,1));
208 mm[3] = factor * (rot(0,1) - rot(1,0));
223 T&
x() {
return mm[0]; }
224 T&
y() {
return mm[1]; }
225 T&
z() {
return mm[2]; }
226 T&
w() {
return mm[3]; }
229 T
x()
const {
return mm[0]; }
230 T
y()
const {
return mm[1]; }
231 T
z()
const {
return mm[2]; }
232 T
w()
const {
return mm[3]; }
244 operator T*() {
return mm; }
245 operator const T*()
const {
return mm; }
256 T sqrLength = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2];
258 if ( sqrLength > 1.0e-8 ) {
260 return T(2.0) * acos(mm[3]);
271 T sqrLength = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2];
273 if ( sqrLength > 1.0e-8 ) {
275 T invLength = T(1)/sqrt(sqrLength);
277 return Vec3<T>( mm[0]*invLength, mm[1]*invLength, mm[2]*invLength );
288 mm[0] = x; mm[1] = y; mm[2] = z; mm[3] = w;
300 T s = sin(angle*T(0.5));
302 mm[0] = axis.
x() * s;
303 mm[1] = axis.
y() * s;
304 mm[2] = axis.
z() * s;
306 mm[3] = cos(angle*T(0.5));
314 mm[0] = mm[1] = mm[2] = mm[3] = 0;
321 mm[0] = mm[1] = mm[2] = 0;
351 bool eq(
const Quat &q, T eps=1.0e-7)
const
393 return Quat<T>(mm[0]+q.mm[0], mm[1]+q.mm[1], mm[2]+q.mm[2], mm[3]+q.mm[3]);
399 return Quat<T>(mm[0]-q.mm[0], mm[1]-q.mm[1], mm[2]-q.mm[2], mm[3]-q.mm[3]);
407 prod.mm[0] = mm[3]*q.mm[0] + mm[0]*q.mm[3] + mm[1]*q.mm[2] - mm[2]*q.mm[1];
408 prod.mm[1] = mm[3]*q.mm[1] + mm[1]*q.mm[3] + mm[2]*q.mm[0] - mm[0]*q.mm[2];
409 prod.mm[2] = mm[3]*q.mm[2] + mm[2]*q.mm[3] + mm[0]*q.mm[1] - mm[1]*q.mm[0];
410 prod.mm[3] = mm[3]*q.mm[3] - mm[0]*q.mm[0] - mm[1]*q.mm[1] - mm[2]*q.mm[2];
426 return Quat<T>(mm[0]*scalar, mm[1]*scalar, mm[2]*scalar, mm[3]*scalar);
432 return Quat<T>(mm[0]/scalar, mm[1]/scalar, mm[2]/scalar, mm[3]/scalar);
437 {
return Quat<T>(-mm[0], -mm[1], -mm[2], -mm[3]); }
443 mm[0] = q1.mm[0] + q2.mm[0];
444 mm[1] = q1.mm[1] + q2.mm[1];
445 mm[2] = q1.mm[2] + q2.mm[2];
446 mm[3] = q1.mm[3] + q2.mm[3];
455 mm[0] = q1.mm[0] - q2.mm[0];
456 mm[1] = q1.mm[1] - q2.mm[1];
457 mm[2] = q1.mm[2] - q2.mm[2];
458 mm[3] = q1.mm[3] - q2.mm[3];
467 mm[0] = q1.mm[3]*q2.mm[0] + q1.mm[0]*q2.mm[3] +
468 q1.mm[1]*q2.mm[2] - q1.mm[2]*q2.mm[1];
469 mm[1] = q1.mm[3]*q2.mm[1] + q1.mm[1]*q2.mm[3] +
470 q1.mm[2]*q2.mm[0] - q1.mm[0]*q2.mm[2];
471 mm[2] = q1.mm[3]*q2.mm[2] + q1.mm[2]*q2.mm[3] +
472 q1.mm[0]*q2.mm[1] - q1.mm[1]*q2.mm[0];
473 mm[3] = q1.mm[3]*q2.mm[3] - q1.mm[0]*q2.mm[0] -
474 q1.mm[1]*q2.mm[1] - q1.mm[2]*q2.mm[2];
483 mm[0] = scale * q.mm[0];
484 mm[1] = scale * q.mm[1];
485 mm[2] = scale * q.mm[2];
486 mm[3] = scale * q.mm[3];
494 return (mm[0]*q.mm[0] + mm[1]*q.mm[1] + mm[2]*q.mm[2] + mm[3]*q.mm[3]);
501 return Quat<T>( +w()*omega.
x() -z()*omega.
y() +y()*omega.
z() ,
502 +z()*omega.
x() +w()*omega.
y() -x()*omega.
z() ,
503 -y()*omega.
x() +x()*omega.
y() +w()*omega.
z() ,
504 -x()*omega.
x() -y()*omega.
y() -z()*omega.
z() );
510 T d = sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3]);
519 T d = sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3]);
522 "Normalizing degenerate quaternion");
529 T d = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3];
532 "Cannot invert degenerate quaternion");
533 Quat result = *
this/-d;
534 result.mm[3] = -result.mm[3];
543 return Quat<T>(-mm[0], -mm[1], -mm[2], mm[3]);
560 std::ostringstream buffer;
565 for (
unsigned j(0); j < 4; j++) {
566 if (j) buffer <<
", ";
585 void write(std::ostream& os)
const {
586 os.write((
char*)&mm,
sizeof(T)*4);
589 is.read((
char*)&mm,
sizeof(T)*4);
597 template <
typename S,
typename T>
604 template <
typename T,
typename T0>
612 if (q1.dot(q2) < 0) q2 *= -1;
614 Quat<T> qslerp = slerp<T>(q1, q2,
static_cast<T
>(t));
615 MatType m = rotation<MatType>(qslerp);
629 template <
typename T,
typename T0>
634 Mat3<T> m00, m01, m02, m10, m11;
636 m00 =
slerp(m1, m2, t);
637 m01 =
slerp(m2, m3, t);
638 m02 =
slerp(m3, m4, t);
640 m10 =
slerp(m00, m01, t);
641 m11 =
slerp(m01, m02, t);
643 return slerp(m10, m11, t);
653 #endif //OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED