4 #ifndef OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED
5 #define OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED
23 template<
typename T>
class Quat;
33 if (fabs(qdot) >= 1.0) {
38 sineAngle =
sin(angle);
47 if (sineAngle <= tolerance) {
50 Quat<T> qtemp(s * q1[0] + t * q2[0], s * q1[1] + t * q2[1],
51 s * q1[2] + t * q2[2], s * q1[3] + t * q2[3]);
58 double lengthSquared = qtemp.
dot(qtemp);
60 if (lengthSquared <= tolerance * tolerance) {
61 qtemp = (t < 0.5) ? q1 : q2;
63 qtemp *= 1.0 /
sqrt(lengthSquared);
68 T sine = 1.0 / sineAngle;
69 T a =
sin((1.0 - t) * angle) * sine;
70 T b =
sin(t * angle) * sine;
71 return Quat<T>(a * q1[0] + b * q2[0], a * q1[1] + b * q2[1],
72 a * q1[2] + b * q2[2], a * q1[3] + b * q2[3]);
86 #if OPENVDB_ABI_VERSION_NUMBER >= 8
143 mm[0] = axis.
x() *
s;
144 mm[1] = axis.
y() *
s;
145 mm[2] = axis.
z() *
s;
164 template<
typename T1>
170 "A non-rotation matrix can not be used to construct a quaternion");
174 "A reflection matrix can not be used to construct a quaternion");
181 T factor = 0.25 / q_w;
183 mm[0] = factor * (
rot(1,2) -
rot(2,1));
184 mm[1] = factor * (
rot(2,0) -
rot(0,2));
185 mm[2] = factor * (
rot(0,1) -
rot(1,0));
187 }
else if (
rot(0,0) >
rot(1,1) &&
rot(0,0) >
rot(2,2)) {
190 T factor = 0.25 / q_x;
193 mm[1] = factor * (
rot(0,1) +
rot(1,0));
194 mm[2] = factor * (
rot(2,0) +
rot(0,2));
195 mm[3] = factor * (
rot(1,2) -
rot(2,1));
196 }
else if (
rot(1,1) >
rot(2,2)) {
199 T factor = 0.25 / q_y;
201 mm[0] = factor * (
rot(0,1) +
rot(1,0));
203 mm[2] = factor * (
rot(1,2) +
rot(2,1));
204 mm[3] = factor * (
rot(2,0) -
rot(0,2));
208 T factor = 0.25 / q_z;
210 mm[0] = factor * (
rot(2,0) +
rot(0,2));
211 mm[1] = factor * (
rot(1,2) +
rot(2,1));
213 mm[3] = factor * (
rot(0,1) -
rot(1,0));
239 operator T*() {
return mm; }
240 operator const T*()
const {
return mm; }
253 if ( sqrLength > 1.0e-8 ) {
255 return T(
T(2.0) *
acos(mm[3]));
268 if ( sqrLength > 1.0e-8 ) {
270 T invLength =
T(
T(1)/
sqrt(sqrLength));
272 return Vec3<T>( mm[0]*invLength, mm[1]*invLength, mm[2]*invLength );
297 mm[0] = axis.
x() *
s;
298 mm[1] = axis.
y() *
s;
299 mm[2] = axis.
z() *
s;
316 mm[0] =
mm[1] =
mm[2] = 0;
410 return Quat<T>(
mm[0]*scalar,
mm[1]*scalar,
mm[2]*scalar,
mm[3]*scalar);
416 return Quat<T>(
mm[0]/scalar,
mm[1]/scalar,
mm[2]/scalar,
mm[3]/scalar);
421 {
return Quat<T>(-
mm[0], -mm[1], -mm[2], -mm[3]); }
427 mm[0] = q1.
mm[0] + q2.
mm[0];
428 mm[1] = q1.
mm[1] + q2.
mm[1];
429 mm[2] = q1.
mm[2] + q2.
mm[2];
430 mm[3] = q1.
mm[3] + q2.
mm[3];
439 mm[0] = q1.
mm[0] - q2.
mm[0];
440 mm[1] = q1.
mm[1] - q2.
mm[1];
441 mm[2] = q1.
mm[2] - q2.
mm[2];
442 mm[3] = q1.
mm[3] - q2.
mm[3];
451 mm[0] = q1.
mm[3]*q2.
mm[0] + q1.
mm[0]*q2.
mm[3] +
452 q1.
mm[1]*q2.
mm[2] - q1.
mm[2]*q2.
mm[1];
453 mm[1] = q1.
mm[3]*q2.
mm[1] + q1.
mm[1]*q2.
mm[3] +
454 q1.
mm[2]*q2.
mm[0] - q1.
mm[0]*q2.
mm[2];
455 mm[2] = q1.
mm[3]*q2.
mm[2] + q1.
mm[2]*q2.
mm[3] +
456 q1.
mm[0]*q2.
mm[1] - q1.
mm[1]*q2.
mm[0];
457 mm[3] = q1.
mm[3]*q2.
mm[3] - q1.
mm[0]*q2.
mm[0] -
458 q1.
mm[1]*q2.
mm[1] - q1.
mm[2]*q2.
mm[2];
467 mm[0] = scale * q.
mm[0];
468 mm[1] = scale * q.
mm[1];
469 mm[2] = scale * q.
mm[2];
470 mm[3] = scale * q.
mm[3];
485 return Quat<T>( +
w()*omega.
x() -
z()*omega.
y() +
y()*omega.
z() ,
486 +
z()*omega.
x() +
w()*omega.
y() -
x()*omega.
z() ,
487 -
y()*omega.
x() +
x()*omega.
y() +
w()*omega.
z() ,
488 -
x()*omega.
x() -
y()*omega.
y() -
z()*omega.
z() );
506 "Normalizing degenerate quaternion");
516 "Cannot invert degenerate quaternion");
518 result.
mm[3] = -result.
mm[3];
527 return Quat<T>(-
mm[0], -mm[1], -mm[2], mm[3]);
544 std::ostringstream
buffer;
549 for (
unsigned j(0); j < 4; j++) {
550 if (j) buffer <<
", ";
568 void write(std::ostream& os)
const { os.write(static_cast<char*>(&
mm),
sizeof(
T) * 4); }
569 void read(std::istream& is) { is.read(static_cast<char*>(&
mm),
sizeof(
T) * 4); }
576 template <
typename S,
typename T>
583 template <
typename T,
typename T0>
591 if (q1.
dot(q2) < 0) q2 *= -1;
593 Quat<T> qslerp = slerp<T>(q1, q2,
static_cast<T>(
t));
594 MatType
m = rotation<MatType>(qslerp);
608 template <
typename T,
typename T0>
613 Mat3<T> m00, m01, m02, m10, m11;
615 m00 =
slerp(m1, m2, t);
616 m01 =
slerp(m2, m3, t);
617 m02 =
slerp(m3, m4, t);
619 m10 =
slerp(m00, m01, t);
620 m11 =
slerp(m01, m02, t);
622 return slerp(m10, m11, t);
628 #if OPENVDB_ABI_VERSION_NUMBER >= 8
642 #endif //OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED
Quat operator*(T scalar) const
Return (this*scalar), e.g. q = q1 * scalar;.
SYS_API double cos(double x)
Quat(T x, T y, T z, T w)
Constructor with four arguments, e.g. Quatf q(1,2,3,4);.
GLboolean GLboolean GLboolean b
Vec3< T > axis() const
Return axis of rotation.
Quat & sub(const Quat &q1, const Quat &q2)
GLenum GLenum GLenum GLenum GLenum scale
T operator[](int i) const
Array style constant reference to the components, e.g. float f = q[1];.
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Quat derivative(const Vec3< T > &omega) const
Quat & scale(T scale, const Quat &q)
Quat operator*(const Quat &q) const
Return (this*q), e.g. q = q1 * q2;.
GA_API const UT_StringHolder rot
vfloat4 sqrt(const vfloat4 &a)
Mat3< typename promote< T0, T1 >::type > operator*(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Multiply m0 by m1 and return the resulting matrix.
bool operator==(const Quat &q) const
Equality operator, does exact floating point comparisons.
#define OPENVDB_USE_VERSION_NAMESPACE
Quat unit() const
this = normalized this
Quat inverse(T tolerance=T(0)) const
returns inverse of this
Quat & init(T x, T y, T z, T w)
"this" quaternion gets initialized to [x, y, z, w]
Quat & operator=(const Quat &q)
Assignment operator.
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.
Quat operator-(const Quat &q) const
Return (this-q), e.g. q = q1 - q2;.
GLubyte GLubyte GLubyte GLubyte w
GLdouble GLdouble GLdouble GLdouble q
Quat operator+(const Quat &q) const
Return (this+q), e.g. q = q1 + q2;.
Vec3< T0 > transform(const Vec3< T0 > &v) const
Quat operator*=(const Quat &q)
Assigns this to (this*q), e.g. q *= q1;.
Quat operator/(T scalar) const
Return (this/scalar), e.g. q = q1 / scalar;.
#define OPENVDB_IS_POD(Type)
Mat3< T > bezLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, const Mat3< T0 > &m3, const Mat3< T0 > &m4, T t)
Quat(T *a)
Constructor with array argument, e.g. float a[4]; Quatf q(a);.
Quat(const Mat3< T1 > &rot)
Constructor given a rotation matrix.
Quat & operator-=(const Quat &q)
Subtract quaternion q from "this" quaternion, e.g. q -= q1;.
bool isApproxEqual(const Type &a, const Type &b, const Type &tolerance)
Return true if a is equal to b to within the given tolerance.
bool eq(const Quat &q, T eps=1.0e-7) const
Test if "this" is equivalent to q with tolerance of eps value.
void read(std::istream &is)
Quat(math::Axis axis, T angle)
Constructor given rotation as axis and angle.
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
bool normalize(T eps=T(1.0e-8))
this = normalized this
GLboolean GLboolean GLboolean GLboolean a
Vec3< T > rotateVector(const Vec3< T > &v) const
Return rotated vector by "this" quaternion.
GLsizei const GLchar *const * string
GLdouble GLdouble GLdouble z
Quat(const Quat &q)
Copy constructor.
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Quat operator-() const
Negation operator, e.g. q = -q;.
Vec3< T > eulerAngles(RotationOrder rotationOrder) const
Returns vector of x,y,z rotational components.
T det() const
Determinant of matrix.
SYS_API double acos(double x)
static unsigned numElements()
Quat & setIdentity()
Set "this" vector to identity.
Quat()
Trivial constructor, the quaternion is NOT initialized.
Quat & init()
"this" quaternion gets initialized to identity, same as setIdentity()
T dot(const Quat &q) const
Dot product.
T & operator()(int i)
Alternative indexed reference to the elements.
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
T & operator[](int i)
Array style reference to the components, e.g. q[3] = 1.34f;.
T trace() const
Trace of matrix.
T & x()
Reference to the component, e.g. q.x() = 4.5f;.
T angle() const
Return angle of rotation.
void write(std::ostream &os) const
Quat & setAxisAngle(const Vec3< T > &axis, T angle)
T x() const
Get the component, e.g. float f = q.w();.
Quat & operator*=(T scalar)
Scale "this" quaternion by scalar, e.g. q *= scalar;.
Quat & add(const Quat &q1, const Quat &q2)
Quat< T > slerp(const Quat< T > &q1, const Quat< T > &q2, T t, T tolerance=0.00001)
Linear interpolation between the two quaternions.
static Quat zero()
Predefined constants, e.g. Quat q = Quat::identity();.
Quat & mult(const Quat &q1, const Quat &q2)
Quat & setZero()
Set "this" vector to zero.
Quat(const Vec3< T > &axis, T angle)
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
SYS_API double sin(double x)
Quat & operator+=(const Quat &q)
Add quaternion q to "this" quaternion, e.g. q += q1;.
#define OPENVDB_THROW(exception, message)
T operator()(int i) const
Alternative indexed constant reference to the elements,.
friend std::ostream & operator<<(std::ostream &stream, const Quat &q)
Output to the stream, e.g. std::cout << q << std::endl;.