31 #ifndef OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED 32 #define OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED 38 #include <openvdb/Exceptions.h> 50 template<
typename T>
class Quat;
56 T qdot,
angle, sineAngle;
60 if (fabs(qdot) >= 1.0) {
65 sineAngle = sin(angle);
74 if (sineAngle <= tolerance) {
77 Quat<T> qtemp(s * q1[0] + t * q2[0], s * q1[1] + t * q2[1],
78 s * q1[2] + t * q2[2], s * q1[3] + t * q2[3]);
85 double lengthSquared = qtemp.
dot(qtemp);
87 if (lengthSquared <= tolerance * tolerance) {
88 qtemp = (t < 0.5) ? q1 : q2;
90 qtemp *= 1.0 / sqrt(lengthSquared);
95 T sine = 1.0 / sineAngle;
96 T a = sin((1.0 - t) * angle) * sine;
97 T b = sin(t * angle) * sine;
98 return Quat<T>(a * q1[0] + b * q2[0], a * q1[1] + b * q2[1],
99 a * q1[2] + b * q2[2], a * q1[3] + b * q2[3]);
137 T s = T(sin(angle*T(0.5)));
139 mm[0] = axis.
x() * s;
140 mm[1] = axis.
y() * s;
141 mm[2] = axis.
z() * s;
143 mm[3] = T(cos(angle*T(0.5)));
150 T s = T(sin(angle*T(0.5)));
156 mm[3] = T(cos(angle*T(0.5)));
160 template<
typename T1>
166 "A non-rotation matrix can not be used to construct a quaternion");
170 "A reflection matrix can not be used to construct a quaternion");
173 T trace(rot.
trace());
176 T q_w = 0.5 * std::sqrt(trace+1);
177 T factor = 0.25 / q_w;
179 mm[0] = factor * (rot(1,2) - rot(2,1));
180 mm[1] = factor * (rot(2,0) - rot(0,2));
181 mm[2] = factor * (rot(0,1) - rot(1,0));
183 }
else if (rot(0,0) > rot(1,1) && rot(0,0) > rot(2,2)) {
185 T q_x = 0.5 * sqrt(rot(0,0)- rot(1,1)-rot(2,2)+1);
186 T factor = 0.25 / q_x;
189 mm[1] = factor * (rot(0,1) + rot(1,0));
190 mm[2] = factor * (rot(2,0) + rot(0,2));
191 mm[3] = factor * (rot(1,2) - rot(2,1));
192 }
else if (rot(1,1) > rot(2,2)) {
194 T q_y = 0.5 * sqrt(rot(1,1)-rot(0,0)-rot(2,2)+1);
195 T factor = 0.25 / q_y;
197 mm[0] = factor * (rot(0,1) + rot(1,0));
199 mm[2] = factor * (rot(1,2) + rot(2,1));
200 mm[3] = factor * (rot(2,0) - rot(0,2));
203 T q_z = 0.5 * sqrt(rot(2,2)-rot(0,0)-rot(1,1)+1);
204 T factor = 0.25 / q_z;
206 mm[0] = factor * (rot(2,0) + rot(0,2));
207 mm[1] = factor * (rot(1,2) + rot(2,1));
209 mm[3] = factor * (rot(0,1) - rot(1,0));
224 T&
x() {
return mm[0]; }
225 T&
y() {
return mm[1]; }
226 T&
z() {
return mm[2]; }
227 T&
w() {
return mm[3]; }
230 T
x()
const {
return mm[0]; }
231 T
y()
const {
return mm[1]; }
232 T
z()
const {
return mm[2]; }
233 T
w()
const {
return mm[3]; }
245 operator T*() {
return mm; }
246 operator const T*()
const {
return mm; }
257 T sqrLength = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2];
259 if ( sqrLength > 1.0e-8 ) {
261 return T(T(2.0) * acos(mm[3]));
272 T sqrLength = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2];
274 if ( sqrLength > 1.0e-8 ) {
276 T invLength = T(T(1)/sqrt(sqrLength));
278 return Vec3<T>( mm[0]*invLength, mm[1]*invLength, mm[2]*invLength );
289 mm[0] = x; mm[1] = y; mm[2] = z; mm[3] = w;
301 T s = T(sin(angle*T(0.5)));
303 mm[0] = axis.
x() * s;
304 mm[1] = axis.
y() * s;
305 mm[2] = axis.
z() * s;
307 mm[3] = T(cos(angle*T(0.5)));
315 mm[0] = mm[1] = mm[2] = mm[3] = 0;
322 mm[0] = mm[1] = mm[2] = 0;
352 bool eq(
const Quat &q, T eps=1.0e-7)
const 394 return Quat<T>(mm[0]+q.
mm[0], mm[1]+q.
mm[1], mm[2]+q.
mm[2], mm[3]+q.
mm[3]);
400 return Quat<T>(mm[0]-q.
mm[0], mm[1]-q.
mm[1], mm[2]-q.
mm[2], mm[3]-q.
mm[3]);
408 prod.
mm[0] = mm[3]*q.
mm[0] + mm[0]*q.
mm[3] + mm[1]*q.
mm[2] - mm[2]*q.
mm[1];
409 prod.
mm[1] = mm[3]*q.
mm[1] + mm[1]*q.
mm[3] + mm[2]*q.
mm[0] - mm[0]*q.
mm[2];
410 prod.
mm[2] = mm[3]*q.
mm[2] + mm[2]*q.
mm[3] + mm[0]*q.
mm[1] - mm[1]*q.
mm[0];
411 prod.
mm[3] = mm[3]*q.
mm[3] - mm[0]*q.
mm[0] - mm[1]*q.
mm[1] - mm[2]*q.
mm[2];
427 return Quat<T>(mm[0]*scalar, mm[1]*scalar, mm[2]*scalar, mm[3]*scalar);
433 return Quat<T>(mm[0]/scalar, mm[1]/scalar, mm[2]/scalar, mm[3]/scalar);
438 {
return Quat<T>(-mm[0], -mm[1], -mm[2], -mm[3]); }
444 mm[0] = q1.
mm[0] + q2.
mm[0];
445 mm[1] = q1.
mm[1] + q2.
mm[1];
446 mm[2] = q1.
mm[2] + q2.
mm[2];
447 mm[3] = q1.
mm[3] + q2.
mm[3];
456 mm[0] = q1.
mm[0] - q2.
mm[0];
457 mm[1] = q1.
mm[1] - q2.
mm[1];
458 mm[2] = q1.
mm[2] - q2.
mm[2];
459 mm[3] = q1.
mm[3] - q2.
mm[3];
468 mm[0] = q1.
mm[3]*q2.
mm[0] + q1.
mm[0]*q2.
mm[3] +
469 q1.
mm[1]*q2.
mm[2] - q1.
mm[2]*q2.
mm[1];
470 mm[1] = q1.
mm[3]*q2.
mm[1] + q1.
mm[1]*q2.
mm[3] +
471 q1.
mm[2]*q2.
mm[0] - q1.
mm[0]*q2.
mm[2];
472 mm[2] = q1.
mm[3]*q2.
mm[2] + q1.
mm[2]*q2.
mm[3] +
473 q1.
mm[0]*q2.
mm[1] - q1.
mm[1]*q2.
mm[0];
474 mm[3] = q1.
mm[3]*q2.
mm[3] - q1.
mm[0]*q2.
mm[0] -
475 q1.
mm[1]*q2.
mm[1] - q1.
mm[2]*q2.
mm[2];
484 mm[0] = scale * q.
mm[0];
485 mm[1] = scale * q.
mm[1];
486 mm[2] = scale * q.
mm[2];
487 mm[3] = scale * q.
mm[3];
495 return (mm[0]*q.
mm[0] + mm[1]*q.
mm[1] + mm[2]*q.
mm[2] + mm[3]*q.
mm[3]);
502 return Quat<T>( +w()*omega.
x() -z()*omega.
y() +y()*omega.
z() ,
503 +z()*omega.
x() +w()*omega.
y() -x()*omega.
z() ,
504 -y()*omega.
x() +x()*omega.
y() +w()*omega.
z() ,
505 -x()*omega.
x() -y()*omega.
y() -z()*omega.
z() );
511 T d = T(sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3]));
520 T d = sqrt(mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3]);
523 "Normalizing degenerate quaternion");
530 T d = mm[0]*mm[0] + mm[1]*mm[1] + mm[2]*mm[2] + mm[3]*mm[3];
533 "Cannot invert degenerate quaternion");
534 Quat result = *
this/-d;
535 result.
mm[3] = -result.
mm[3];
544 return Quat<T>(-mm[0], -mm[1], -mm[2], mm[3]);
561 std::ostringstream buffer;
566 for (
unsigned j(0); j < 4; j++) {
567 if (j) buffer <<
", ";
583 friend Quat slerp<>(
const Quat &q1,
const Quat &q2, T t, T tolerance);
585 void write(std::ostream& os)
const { os.write(static_cast<char*>(&mm),
sizeof(T) * 4); }
586 void read(std::istream& is) { is.read(static_cast<char*>(&mm),
sizeof(T) * 4); }
593 template <
typename S,
typename T>
600 template <
typename T,
typename T0>
608 if (q1.
dot(q2) < 0) q2 *= -1;
610 Quat<T> qslerp = slerp<T>(q1, q2,
static_cast<T
>(t));
611 MatType m = rotation<MatType>(qslerp);
625 template <
typename T,
typename T0>
630 Mat3<T> m00, m01, m02, m10, m11;
632 m00 =
slerp(m1, m2, t);
633 m01 =
slerp(m2, m3, t);
634 m02 =
slerp(m3, m4, t);
636 m10 =
slerp(m00, m01, t);
637 m11 =
slerp(m01, m02, t);
639 return slerp(m10, m11, t);
648 template<>
inline math::Quats zeroVal<math::Quats >() {
return math::Quats::zero(); }
649 template<>
inline math::Quatd zeroVal<math::Quatd >() {
return math::Quatd::zero(); }
654 #endif //OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED T & y()
Definition: Vec3.h:111
Quat & init()
"this" quaternion gets initialized to identity, same as setIdentity()
Definition: Quat.h:294
Quat & init(T x, T y, T z, T w)
"this" quaternion gets initialized to [x, y, z, w]
Definition: Quat.h:287
Quat & operator+=(const Quat &q)
Add quaternion q to "this" quaternion, e.g. q += q1;.
Definition: Quat.h:359
Quat & mult(const Quat &q1, const Quat &q2)
Definition: Quat.h:466
bool operator==(const Quat &q) const
Equality operator, does exact floating point comparisons.
Definition: Quat.h:343
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition: Math.h:395
void read(std::istream &is)
Definition: Quat.h:586
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
static unsigned numElements()
Definition: Quat.h:236
friend std::ostream & operator<<(std::ostream &stream, const Quat &q)
Output to the stream, e.g. std::cout << q << std::endl;.
Definition: Quat.h:577
#define OPENVDB_THROW(exception, message)
Definition: Exceptions.h:109
Quat & setAxisAngle(const Vec3< T > &axis, T angle)
Definition: Quat.h:298
Quat & operator*=(T scalar)
Scale "this" quaternion by scalar, e.g. q *= scalar;.
Definition: Quat.h:381
Quat(const Quat &q)
Copy constructor.
Definition: Quat.h:214
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition: Mat.h:921
Quat(const Mat3< T1 > &rot)
Constructor given a rotation matrix.
Definition: Quat.h:161
Quat & setIdentity()
Set "this" vector to identity.
Definition: Quat.h:320
T trace() const
Trace of matrix.
Definition: Mat3.h:536
bool eq(const Quat &q, T eps=1.0e-7) const
Test if "this" is equivalent to q with tolerance of eps value.
Definition: Quat.h:352
T & w()
Definition: Quat.h:227
static Quat zero()
Predefined constants, e.g. Quat q = Quat::identity();.
Definition: Quat.h:555
T angle() const
Return angle of rotation.
Definition: Quat.h:255
Quat operator*(T scalar) const
Return (this*scalar), e.g. q = q1 * scalar;.
Definition: Quat.h:425
T dot(const Quat &q) const
Dot product.
Definition: Quat.h:493
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
Definition: Quat.h:224
Quat operator+(const Quat &q) const
Return (this+q), e.g. q = q1 + q2;.
Definition: Quat.h:392
void write(std::ostream &os) const
Definition: Quat.h:585
T mm[4]
Definition: Quat.h:589
static Quat identity()
Definition: Quat.h:556
Quat(math::Axis axis, T angle)
Constructor given rotation as axis and angle.
Definition: Quat.h:148
Quat & setZero()
Set "this" vector to zero.
Definition: Quat.h:313
Quat unit() const
this = normalized this
Definition: Quat.h:518
T & operator[](int i)
Array style reference to the components, e.g. q[3] = 1.34f;.
Definition: Quat.h:239
Quat operator-() const
Negation operator, e.g. q = -q;.
Definition: Quat.h:437
T det() const
Determinant of matrix.
Definition: Mat3.h:527
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition: version.h:136
Vec3< T > rotateVector(const Vec3< T > &v) const
Return rotated vector by "this" quaternion.
Definition: Quat.h:548
Mat3< T > slerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, T t)
Interpolate between m1 and m2. Converts to quaternion form and uses slerp m1 and m2 must be rotation ...
Definition: Quat.h:601
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition: Vec2.h:472
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition: Vec3.h:110
Quat(T *a)
Constructor with array argument, e.g. float a[4]; Quatf q(a);.
Definition: Quat.h:122
MatType scale(const Vec3< typename MatType::value_type > &s)
Return a matrix that scales by s.
Definition: Mat.h:647
Vec3< typename MatType::value_type > eulerAngles(const MatType &mat, RotationOrder rotationOrder, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return the Euler angles composing the given rotation matrix.
Definition: Mat.h:365
Definition: Exceptions.h:40
T & operator()(int i)
Alternative indexed reference to the elements.
Definition: Quat.h:249
Axis
Definition: Math.h:856
3x3 matrix class.
Definition: Mat3.h:55
T operator[](int i) const
Array style constant reference to the components, e.g. float f = q[1];.
Definition: Quat.h:242
std::string str() const
Definition: Quat.h:559
Quat operator*(const Quat &q) const
Return (this*q), e.g. q = q1 * q2;.
Definition: Quat.h:404
Quat operator-(const Quat &q) const
Return (this-q), e.g. q = q1 - q2;.
Definition: Quat.h:398
Quat(T x, T y, T z, T w)
Constructor with four arguments, e.g. Quatf q(1,2,3,4);.
Definition: Quat.h:112
Vec3< T > axis() const
Return axis of rotation.
Definition: Quat.h:270
Mat3< T > bezLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, const Mat3< T0 > &m3, const Mat3< T0 > &m4, T t)
Definition: Quat.h:626
T y() const
Definition: Quat.h:231
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition: Mat3.h:553
Quat operator*=(const Quat &q)
Assigns this to (this*q), e.g. q *= q1;.
Definition: Quat.h:418
T z() const
Definition: Quat.h:232
Quat derivative(const Vec3< T > &omega) const
Definition: Quat.h:500
T x() const
Get the component, e.g. float f = q.w();.
Definition: Quat.h:230
bool isApproxEqual(const Type &a, const Type &b)
Return true if a is equal to b to within the default floating-point comparison tolerance.
Definition: Math.h:358
Quat & sub(const Quat &q1, const Quat &q2)
Definition: Quat.h:454
Quat operator/(T scalar) const
Return (this/scalar), e.g. q = q1 / scalar;.
Definition: Quat.h:431
RotationOrder
Definition: Math.h:863
#define OPENVDB_USE_VERSION_NAMESPACE
Definition: version.h:188
Quat & add(const Quat &q1, const Quat &q2)
Definition: Quat.h:442
Quat< T > operator*(S scalar, const Quat< T > &q)
Multiply each element of the given quaternion by scalar and return the result.
Definition: Quat.h:594
T operator()(int i) const
Alternative indexed constant reference to the elements,.
Definition: Quat.h:252
T w() const
Definition: Quat.h:233
Quat inverse(T tolerance=T(0))
returns inverse of this
Definition: Quat.h:528
T & z()
Definition: Quat.h:226
T & y()
Definition: Quat.h:225
Vec3< T > eulerAngles(RotationOrder rotationOrder) const
Returns vector of x,y,z rotational components.
Definition: Quat.h:328
Quat conjugate() const
Definition: Quat.h:542
Quat & operator-=(const Quat &q)
Subtract quaternion q from "this" quaternion, e.g. q -= q1;.
Definition: Quat.h:370
Definition: Exceptions.h:83
Quat()
Trivial constructor, the quaternion is NOT initialized.
Definition: Quat.h:109
Quat & scale(T scale, const Quat &q)
Definition: Quat.h:482
Quat(const Vec3< T > &axis, T angle)
Definition: Quat.h:133
T & z()
Definition: Vec3.h:112
Quat & operator=(const Quat &q)
Assignment operator.
Definition: Quat.h:332
bool normalize(T eps=T(1.0e-8))
this = normalized this
Definition: Quat.h:509