4#ifndef OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED 
    5#define OPENVDB_MATH_QUAT_H_HAS_BEEN_INCLUDED 
   24template<
typename T> 
class Quat;
 
   30    T qdot, 
angle, sineAngle;
 
   34    if (fabs(qdot) >= 1.0) {
 
   39        sineAngle = sin(
angle);
 
   48    if (sineAngle <= tolerance) {
 
   51        Quat<T> qtemp(s * q1[0] + t * q2[0], s * q1[1] + t * q2[1],
 
   52                      s * q1[2] + t * q2[2], s * q1[3] + t * q2[3]);
 
   59        double lengthSquared = qtemp.
dot(qtemp);
 
   61        if (lengthSquared <= tolerance * tolerance) {
 
   62            qtemp = (t < 0.5) ? q1 : q2;
 
   64            qtemp *= 1.0 / sqrt(lengthSquared);
 
   69        T sine  = 1.0 / sineAngle;
 
   70        T a = sin((1.0 - t) * 
angle) * sine;
 
   71        T b = sin(t * 
angle) * sine;
 
   72        return Quat<T>(a * q1[0] + b * q2[0], a * q1[1] + b * q2[1],
 
   73                       a * q1[2] + b * q2[2], a * q1[3] + b * q2[3]);
 
 
  117        T s = T(sin(
angle*T(0.5)));
 
 
  130        T s = T(sin(
angle*T(0.5)));
 
 
  140    template<
typename T1>
 
  146                "A non-rotation matrix can not be used to construct a quaternion");
 
  150                "A reflection matrix can not be used to construct a quaternion");
 
  153        T trace(rot.
trace());
 
  156            T q_w = 0.5 * std::sqrt(trace+1);
 
  157            T factor = 0.25 / q_w;
 
  159            mm[0] = factor * (rot(1,2) - rot(2,1));
 
  160            mm[1] = factor * (rot(2,0) - rot(0,2));
 
  161            mm[2] = factor * (rot(0,1) - rot(1,0));
 
  163        }  
else if (rot(0,0) > rot(1,1) && rot(0,0) > rot(2,2)) {
 
  165            T q_x = 0.5 * sqrt(rot(0,0)- rot(1,1)-rot(2,2)+1);
 
  166            T factor = 0.25 / q_x;
 
  169            mm[1] = factor * (rot(0,1) + rot(1,0));
 
  170            mm[2] = factor * (rot(2,0) + rot(0,2));
 
  171            mm[3] = factor * (rot(1,2) - rot(2,1));
 
  172        } 
else if (rot(1,1) > rot(2,2)) {
 
  174            T q_y = 0.5 * sqrt(rot(1,1)-rot(0,0)-rot(2,2)+1);
 
  175            T factor = 0.25 / q_y;
 
  177            mm[0] =  factor * (rot(0,1) + rot(1,0));
 
  179            mm[2] = factor * (rot(1,2) + rot(2,1));
 
  180            mm[3] = factor * (rot(2,0) - rot(0,2));
 
  183            T q_z = 0.5 * sqrt(rot(2,2)-rot(0,0)-rot(1,1)+1);
 
  184            T factor = 0.25 / q_z;
 
  186            mm[0] = factor * (rot(2,0) + rot(0,2));
 
  187            mm[1] = factor * (rot(1,2) + rot(2,1));
 
  189            mm[3] = factor * (rot(0,1) - rot(1,0));
 
 
  200    T 
x()
 const { 
return mm[0]; }
 
  201    T 
y()
 const { 
return mm[1]; }
 
  202    T 
z()
 const { 
return mm[2]; }
 
  203    T 
w()
 const { 
return mm[3]; }
 
  215    operator T*() { 
return mm; }
 
  216    operator const T*() 
const { 
return mm; }
 
  229        if ( sqrLength > 1.0e-8 ) {
 
  231            return T(T(2.0) * acos(
mm[3]));
 
 
  244        if ( sqrLength > 1.0e-8 ) {
 
  246            T invLength = T(T(1)/sqrt(sqrLength));
 
  248            return Vec3<T>( 
mm[0]*invLength, 
mm[1]*invLength, 
mm[2]*invLength );
 
 
  271        T s = T(sin(
angle*T(0.5)));
 
 
  292        mm[0] = 
mm[1] = 
mm[2] = 0;
 
 
  311    bool eq(
const Quat &q, T eps=1.0e-7)
 const 
 
  386        return Quat<T>(
mm[0]*scalar, 
mm[1]*scalar, 
mm[2]*scalar, 
mm[3]*scalar);
 
 
  392        return Quat<T>(
mm[0]/scalar, 
mm[1]/scalar, 
mm[2]/scalar, 
mm[3]/scalar);
 
 
  403        mm[0] = q1.
mm[0] + q2.
mm[0];
 
  404        mm[1] = q1.
mm[1] + q2.
mm[1];
 
  405        mm[2] = q1.
mm[2] + q2.
mm[2];
 
  406        mm[3] = q1.
mm[3] + q2.
mm[3];
 
 
  415        mm[0] = q1.
mm[0] - q2.
mm[0];
 
  416        mm[1] = q1.
mm[1] - q2.
mm[1];
 
  417        mm[2] = q1.
mm[2] - q2.
mm[2];
 
  418        mm[3] = q1.
mm[3] - q2.
mm[3];
 
 
  427        mm[0] = q1.
mm[3]*q2.
mm[0] + q1.
mm[0]*q2.
mm[3] +
 
  428                q1.
mm[1]*q2.
mm[2] - q1.
mm[2]*q2.
mm[1];
 
  429        mm[1] = q1.
mm[3]*q2.
mm[1] + q1.
mm[1]*q2.
mm[3] +
 
  430                q1.
mm[2]*q2.
mm[0] - q1.
mm[0]*q2.
mm[2];
 
  431        mm[2] = q1.
mm[3]*q2.
mm[2] + q1.
mm[2]*q2.
mm[3] +
 
  432                q1.
mm[0]*q2.
mm[1] - q1.
mm[1]*q2.
mm[0];
 
  433        mm[3] = q1.
mm[3]*q2.
mm[3] - q1.
mm[0]*q2.
mm[0] -
 
  434                q1.
mm[1]*q2.
mm[1] - q1.
mm[2]*q2.
mm[2];
 
 
  461        return Quat<T>( +
w()*omega.
x() -
z()*omega.
y() +
y()*omega.
z() ,
 
  462                        +
z()*omega.
x() +
w()*omega.
y() -
x()*omega.
z() ,
 
  463                        -
y()*omega.
x() +
x()*omega.
y() +
w()*omega.
z() ,
 
  464                        -
x()*omega.
x() -
y()*omega.
y() -
z()*omega.
z() );
 
 
  482                "Normalizing degenerate quaternion");
 
 
  492                "Cannot invert degenerate quaternion");
 
  493        Quat result = *
this/-d;
 
  494        result.
mm[3] = -result.
mm[3];
 
 
  520        std::ostringstream buffer;
 
  525        for (
unsigned j(0); j < 4; j++) {
 
  526            if (j) buffer << 
", ";
 
 
  544    void write(std::ostream& os)
 const { os.write(
static_cast<char*
>(&
mm), 
sizeof(T) * 4); }
 
  545    void read(std::istream& is) { is.read(
static_cast<char*
>(&
mm), 
sizeof(T) * 4); }
 
 
  552template <
typename S, 
typename T>
 
  559template <
typename T, 
typename T0>
 
  567    if (q1.
dot(q2) < 0) q2 *= -1;
 
 
  584template <
typename T, 
typename T0>
 
  589    Mat3<T> m00, m01, m02, m10, m11;
 
  591    m00 = 
slerp(m1, m2, t);
 
  592    m01 = 
slerp(m2, m3, t);
 
  593    m02 = 
slerp(m3, m4, t);
 
  595    m10 = 
slerp(m00, m01, t);
 
  596    m11 = 
slerp(m01, m02, t);
 
  598    return slerp(m10, m11, t);
 
 
#define OPENVDB_ASSERT(X)
Definition Assert.h:41
General-purpose arithmetic and comparison routines, most of which accept arbitrary value types (or at...
#define OPENVDB_IS_POD(Type)
Definition Math.h:56
Definition Exceptions.h:56
3x3 matrix class.
Definition Mat3.h:29
T trace() const
Trace of matrix.
Definition Mat3.h:488
T det() const
Determinant of matrix.
Definition Mat3.h:479
Vec3< T0 > transform(const Vec3< T0 > &v) const
Definition Mat3.h:505
Real angle() const
Definition Quat.h:225
Real & x()
Definition Quat.h:194
Quat & sub(const Quat &q1, const Quat &q2)
Definition Quat.h:413
Quat conjugate() const
Definition Quat.h:501
Vec3< Real > axis() const
Definition Quat.h:240
Quat operator/(T scalar) const
Return (this/scalar), e.g. q = q1 / scalar;.
Definition Quat.h:390
Quat & operator*=(T scalar)
Scale "this" quaternion by scalar, e.g. q *= scalar;.
Definition Quat.h:340
Quat & setAxisAngle(const Vec3< T > &axis, T angle)
Definition Quat.h:268
Quat unit() const
this = normalized this
Definition Quat.h:477
void write(std::ostream &os) const
Definition Quat.h:544
Quat operator*(const Quat &q) const
Return (this*q), e.g. q = q1 * q2;.
Definition Quat.h:363
bool operator==(const Quat &q) const
Equality operator, does exact floating point comparisons.
Definition Quat.h:302
Quat(T *a)
Constructor with array argument, e.g. float a[4]; Quatf q(a);.
Definition Quat.h:102
Quat & add(const Quat &q1, const Quat &q2)
Definition Quat.h:401
friend std::ostream & operator<<(std::ostream &stream, const Quat &q)
Output to the stream, e.g. std::cout << q << std::endl;.
Definition Quat.h:536
T w() const
Definition Quat.h:203
Quat(math::Axis axis, T angle)
Constructor given rotation as axis and angle.
Definition Quat.h:128
Quat & operator+=(const Quat &q)
Add quaternion q to "this" quaternion, e.g. q += q1;.
Definition Quat.h:318
Quat operator-() const
Negation operator, e.g. q = -q;.
Definition Quat.h:396
T & operator()(int i)
Alternative indexed reference to the elements.
Definition Quat.h:219
Real & y()
Definition Quat.h:195
Real & z()
Definition Quat.h:196
Quat(const Vec3< T > &axis, T angle)
Definition Quat.h:113
Quat & scale(T scale, const Quat &q)
Definition Quat.h:441
T operator()(int i) const
Alternative indexed constant reference to the elements,.
Definition Quat.h:222
Quat(const Mat3< T1 > &rot)
Constructor given a rotation matrix.
Definition Quat.h:141
Quat & setIdentity()
Definition Quat.h:290
Quat operator+(const Quat &q) const
Return (this+q), e.g. q = q1 + q2;.
Definition Quat.h:351
Quat & init()
"this" quaternion gets initialized to identity, same as setIdentity()
Definition Quat.h:264
Quat operator*=(const Quat &q)
Assigns this to (this*q), e.g. q *= q1;.
Definition Quat.h:377
Quat(T x, T y, T z, T w)
Constructor with four arguments, e.g. Quatf q(1,2,3,4);.
Definition Quat.h:92
Vec3< T > rotateVector(const Vec3< T > &v) const
Return rotated vector by "this" quaternion.
Definition Quat.h:507
T x() const
Get the component, e.g. float f = q.w();.
Definition Quat.h:200
static const int size
Definition Quat.h:84
bool normalize(T eps=T(1.0e-8))
this = normalized this
Definition Quat.h:468
static Quat identity()
Definition Quat.h:515
Quat & init(T x, T y, T z, T w)
"this" quaternion gets initialized to [x, y, z, w]
Definition Quat.h:257
T z() const
Definition Quat.h:202
T y() const
Definition Quat.h:201
Vec3< T > eulerAngles(RotationOrder rotationOrder) const
Returns vector of x,y,z rotational components.
Definition Quat.h:298
T operator[](int i) const
Array style constant reference to the components, e.g. float f = q[1];.
Definition Quat.h:212
static unsigned numElements()
Definition Quat.h:206
Quat derivative(const Vec3< T > &omega) const
Definition Quat.h:459
T & operator[](int i)
Array style reference to the components, e.g. q[3] = 1.34f;.
Definition Quat.h:209
Quat operator*(T scalar) const
Return (this*scalar), e.g. q = q1 * scalar;.
Definition Quat.h:384
Real mm[4]
Definition Quat.h:548
std::string str() const
Definition Quat.h:518
friend Quat slerp(const Quat &q1, const Quat &q2, Real t, Real tolerance)
Quat & operator-=(const Quat &q)
Subtract quaternion q from "this" quaternion, e.g. q -= q1;.
Definition Quat.h:329
Real ValueType
Definition Quat.h:83
Quat & mult(const Quat &q1, const Quat &q2)
Definition Quat.h:425
static Quat zero()
Predefined constants, e.g. Quat q = Quat::identity();.
Definition Quat.h:514
Quat inverse(T tolerance=T(0)) const
returns inverse of this
Definition Quat.h:487
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:311
Quat & setZero()
Set "this" vector to zero.
Definition Quat.h:283
Quat operator-(const Quat &q) const
Return (this-q), e.g. q = q1 - q2;.
Definition Quat.h:357
Real value_type
Definition Quat.h:82
Real & w()
Definition Quat.h:197
void read(std::istream &is)
Definition Quat.h:545
T dot(const Quat &q) const
Dot product.
Definition Quat.h:452
T & x()
Reference to the component, e.g. v.x() = 4.5f;.
Definition Vec3.h:86
T & y()
Definition Vec3.h:87
T & z()
Definition Vec3.h:88
Mat3< typename promote< T0, T1 >::type > operator*(const Mat3< T0 > &m0, const Mat3< T1 > &m1)
Multiply m0 by m1 and return the resulting matrix.
Definition Mat3.h:597
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:333
bool isApproxEqual(const Type &a, const Type &b, const Type &tolerance)
Return true if a is equal to b to within the given tolerance.
Definition Math.h:406
bool isUnitary(const MatType &m)
Determine if a matrix is unitary (i.e., rotation or reflection).
Definition Mat.h:889
MatType rotation(const Quat< typename MatType::value_type > &q, typename MatType::value_type eps=static_cast< typename MatType::value_type >(1.0e-8))
Return the rotation matrix specified by the given quaternion.
Definition Mat.h:172
T angle(const Vec2< T > &v1, const Vec2< T > &v2)
Definition Vec2.h:446
bool isExactlyEqual(const T0 &a, const T1 &b)
Return true if a is exactly equal to b.
Definition Math.h:443
Quat< double > Quatd
Definition Quat.h:602
Axis
Definition Math.h:901
@ Z_AXIS
Definition Math.h:904
@ X_AXIS
Definition Math.h:902
@ Y_AXIS
Definition Math.h:903
Quat< float > Quats
Definition Quat.h:601
Mat3< T > bezLerp(const Mat3< T0 > &m1, const Mat3< T0 > &m2, const Mat3< T0 > &m3, const Mat3< T0 > &m4, T t)
Definition Quat.h:585
RotationOrder
Definition Math.h:908
Quat< T > slerp(const Quat< T > &q1, const Quat< T > &q2, T t, T tolerance=0.00001)
Linear interpolation between the two quaternions.
Definition Quat.h:28
constexpr T zeroVal()
Return the value of type T that corresponds to zero.
Definition Math.h:70
Definition Exceptions.h:13
#define OPENVDB_THROW(exception, message)
Definition Exceptions.h:74
#define OPENVDB_VERSION_NAME
The version namespace name for this library version.
Definition version.h.in:121
#define OPENVDB_USE_VERSION_NAMESPACE
Definition version.h.in:218