Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "cbase.h"
- #include <math.h>
- //#include "vector.h"
- #include "Quaternion.h"
- void Quat::GetRotationTo(const Vector& start, const Vector& dest)
- {
- // Based on Stan Melax's article in Game Programming Gems
- Quat q;
- // Copy, since cannot modify local
- Vector v0 = start;
- Vector v1 = dest;
- v0.NormalizeInPlace();
- v1.NormalizeInPlace();
- float d = v0.Dot(v1);
- // If dot == 1, vectors are the same
- if (d >= 1.0f)
- {
- q = Quat(1,0,0,0);
- *this = q;
- return;
- }
- if (d < (1e-6f - 1.0f))
- {
- // Generate an axis
- Vector axis = Vector(1,0,0).Cross(start);
- if (axis.IsZero()) // pick another if colinear
- axis = Vector(0,1,0).Cross(start);
- axis.NormalizeInPlace();
- q.AxisAngle(axis, 180);
- }
- else
- {
- float s = sqrt( (1+d)*2 );
- float invs = 1 / s;
- Vector c = v0.Cross(v1);
- q.x = c.x * invs;
- q.y = c.y * invs;
- q.z = c.z * invs;
- q.w = s * 0.5;
- //q.NormalizeInPlace();
- }
- *this = q;
- }
- void Quat::GetRotationTo( const Vector& cross, float angle )
- {
- // Based on Stan Melax's article in Game Programming Gems
- Quat q;
- // Copy, since cannot modify local
- float d = angle;//cos(angle);//v0.Dot(v1);
- // If dot == 1, vectors are the same
- if (d >= 1.0f)
- {
- q = Quat(1,0,0,0);
- *this = q;
- return;
- }
- float s = sqrt( (1+d)*2 );
- float invs = 1 / s;
- Vector c = cross;//v0.Cross(v1);
- q.x = c.x * invs;
- q.y = c.y * invs;
- q.z = c.z * invs;
- q.w = s * 0.5;
- *this = q;
- }
- void Quat::FromRotationMatrix (const matrix3x4_t& kRot)
- {
- // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
- // article "Quat Calculus and Fast Animation".
- float fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
- float fRoot;
- if ( fTrace > 0.0 )
- {
- // |w| > 1/2, may as well choose w > 1/2
- fRoot = sqrt(fTrace + 1.0); // 2w
- w = 0.5*fRoot;
- fRoot = 0.5/fRoot; // 1/(4w)
- x = (kRot[2][1]-kRot[1][2])*fRoot;
- y = (kRot[0][2]-kRot[2][0])*fRoot;
- z = (kRot[1][0]-kRot[0][1])*fRoot;
- }
- else
- {
- // |w| <= 1/2
- static size_t s_iNext[3] = { 1, 2, 0 };
- size_t i = 0;
- if ( kRot[1][1] > kRot[0][0] )
- i = 1;
- if ( kRot[2][2] > kRot[i][i] )
- i = 2;
- size_t j = s_iNext[i];
- size_t k = s_iNext[j];
- fRoot = sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0);
- float* apkQuat[3] = { &x, &y, &z };
- *apkQuat[i] = 0.5*fRoot;
- fRoot = 0.5/fRoot;
- w = (kRot[k][j]-kRot[j][k])*fRoot;
- *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
- *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
- }
- }
- void Quat::ToRotationMatrix (matrix3x4_t& kRot) const
- {
- float fTx = 2.0*x;
- float fTy = 2.0*y;
- float fTz = 2.0*z;
- float fTwx = fTx*w;
- float fTwy = fTy*w;
- float fTwz = fTz*w;
- float fTxx = fTx*x;
- float fTxy = fTy*x;
- float fTxz = fTz*x;
- float fTyy = fTy*y;
- float fTyz = fTz*y;
- float fTzz = fTz*z;
- kRot[0][0] = 1.0-(fTyy+fTzz);
- kRot[0][1] = fTxy-fTwz;
- kRot[0][2] = fTxz+fTwy;
- kRot[1][0] = fTxy+fTwz;
- kRot[1][1] = 1.0-(fTxx+fTzz);
- kRot[1][2] = fTyz-fTwx;
- kRot[2][0] = fTxz-fTwy;
- kRot[2][1] = fTyz+fTwx;
- kRot[2][2] = 1.0-(fTxx+fTyy);
- }
- /*
- void Quat::GetAngles( RadEuler &angles )
- {
- // FIXME: doing it this way calculates too much data, needs to do an optimized version...
- matrix3x4_t matrix = GetMatrix();//QuatMatrix( *this, matrix );
- MatrixAngles( matrix, angles );
- Assert( angles.IsValid() );
- }
- */
- void Quat::GetAngles( QAngle &angles )
- {
- Assert( IsValid() );
- #if 1
- // FIXME: doing it this way calculates too much data, needs to do an optimized version...
- matrix3x4_t matrix = GetMatrix();
- MatrixAngles( matrix, angles );;
- //Assert( angles.IsValid() );
- #else
- /*
- float m11, m12, m13, m23, m33;
- Quat q = *this;
- m11 = ( 2.0f * q.w * q.w ) + ( 2.0f * q.x * q.x ) - 1.0f;
- m12 = ( 2.0f * q.x * q.y ) + ( 2.0f * q.w * q.z );
- m13 = ( 2.0f * q.x * q.z ) - ( 2.0f * q.w * q.y );
- m23 = ( 2.0f * q.y * q.z ) + ( 2.0f * q.w * q.x );
- m33 = ( 2.0f * q.w * q.w ) + ( 2.0f * q.z * q.z ) - 1.0f;
- // FIXME: this code has a singularity near PITCH +-90
- angles[YAW] = RAD2DEG( atan2(m12, m11) );
- angles[PITCH] = RAD2DEG( asin(-m13) );
- angles[ROLL] = RAD2DEG( atan2(m23, m33) );
- */
- Quat q1 = *this;
- //q1.NormalizeInPlace();
- float sqw = q1.w*q1.w;
- float sqx = q1.x*q1.x;
- float sqy = q1.y*q1.y;
- float sqz = q1.z*q1.z;
- float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
- float test = q1.x*q1.y + q1.z*q1.w;
- float hpi = M_PI/2.0f;
- if (test > 0.5*unit) { // singularity at north pole
- angles.x = RAD2DEG(2 * atan2(q1.x,q1.w));
- angles.y = RAD2DEG(hpi);
- angles.z = 0;
- return;
- }
- if (test < -0.5*unit) { // singularity at south pole
- angles.x = RAD2DEG(-2 * atan2(q1.x,q1.w));
- angles.y = RAD2DEG(-hpi);
- angles.z = 0;
- return;
- }
- angles.x = RAD2DEG(atan2(2*q1.y*q1.w-2*q1.x*q1.z , sqx - sqy - sqz + sqw));
- angles.y = RAD2DEG(asin(2*test/unit));
- angles.z = RAD2DEG(atan2(2*q1.x*q1.w-2*q1.y*q1.z , -sqx + sqy - sqz + sqw));
- #endif
- Assert( angles.IsValid() );
- }
- matrix3x4_t Quat::GetMatrix()
- {
- Quat q = *this;
- Assert( IsValid() );
- matrix3x4_t matrix;
- #ifdef _VPROF_MATHLIB
- VPROF_BUDGET( "QuatMatrix", "Mathlib" );
- #endif
- // Original code
- // This should produce the same code as below with optimization, but looking at the assmebly,
- // it doesn't. There are 7 extra multiplies in the release build of this, go figure.
- #if 0
- matrix[0][0] = 1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z;
- matrix[1][0] = 2.0 * q.x * q.y + 2.0 * q.w * q.z;
- matrix[2][0] = 2.0 * q.x * q.z - 2.0 * q.w * q.y;
- matrix[0][1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
- matrix[1][1] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.z * q.z;
- matrix[2][1] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;
- matrix[0][2] = 2.0f * q.x * q.z + 2.0f * q.w * q.y;
- matrix[1][2] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
- matrix[2][2] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.y * q.y;
- #else
- float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
- // precalculate common multiplitcations
- x2 = q.x + q.x;
- y2 = q.y + q.y;
- z2 = q.z + q.z;
- xx = q.x * x2;
- xy = q.x * y2;
- xz = q.x * z2;
- yy = q.y * y2;
- yz = q.y * z2;
- zz = q.z * z2;
- wx = q.w * x2;
- wy = q.w * y2;
- wz = q.w * z2;
- matrix[0][0] = 1.0 - (yy + zz);
- matrix[0][1] = xy - wz;
- matrix[0][2] = xz + wy;
- matrix[1][0] = xy + wz;
- matrix[1][1] = 1.0 - (xx + zz);
- matrix[1][2] = yz - wx;
- matrix[2][0] = xz - wy;
- matrix[2][1] = yz + wx;
- matrix[2][2] = 1.0 - (xx + yy);
- #endif
- return matrix;
- }
- //-----------------------------------------------------------------------------
- // Array access
- //-----------------------------------------------------------------------------
- inline vec_t& Quat::operator[](int i)
- {
- Assert( (i >= 0) && (i < 4) );
- return ((vec_t*)this)[i];
- }
- inline vec_t Quat::operator[](int i) const
- {
- Assert( (i >= 0) && (i < 4) );
- return ((vec_t*)this)[i];
- }
- //-----------------------------------------------------------------------------
- // Equality test
- //-----------------------------------------------------------------------------
- inline bool Quat::operator==( const Quat &src ) const
- {
- return ( x == src.x ) && ( y == src.y ) && ( z == src.z ) && ( w == src.w );
- }
- inline bool Quat::operator!=( const Quat &src ) const
- {
- return !operator==( src );
- }
- extern void AngleQuat( RadEuler const &angles, Quat &qt );
- extern void QuatAngles( Quat const &q, RadEuler &angles );
- inline Quat::Quat(RadEuler const &angle)
- {
- AngleQuat( angle, *this );
- }
- //-----------------------------------------------------------------------------
- // Array access
- //-----------------------------------------------------------------------------
- inline vec_t& RadEuler::operator[](int i)
- {
- Assert( (i >= 0) && (i < 3) );
- return ((vec_t*)this)[i];
- }
- inline vec_t RadEuler::operator[](int i) const
- {
- Assert( (i >= 0) && (i < 3) );
- return ((vec_t*)this)[i];
- }
- inline Quat Quat::operator*(const Quat& q) const
- {
- Quat qt;
- //QuatMult( this, q, res );
- Assert( IsValid() );
- Assert( q.IsValid() );
- if (&*this == &qt)
- {
- Quat p2 = *this;
- //QuatMult( p2, q, qt );
- return (p2 * q);
- }
- // decide if one of the Quats is backwards
- Quat q2;
- q2.Align( *this, q );
- qt.x = x * q2.w + y * q2.z - z * q2.y + w * q2.x;
- qt.y = -x * q2.z + y * q2.w + z * q2.x + w * q2.y;
- qt.z = x * q2.y - y * q2.x + z * q2.w + w * q2.z;
- qt.w = -x * q2.x - y * q2.y - z * q2.z + w * q2.w;
- return qt;
- }
- Vector Quat::operator* (const Vector& vec) const
- {
- // nVidia SDK implementation
- Vector uv, uuv;
- Vector qvec(x, y, z);
- uv = qvec.Cross(vec);
- uuv = qvec.Cross(uv);
- uv *= (2.0f * w);
- uuv *= 2.0f;
- return vec + uv + uuv;
- }
- void Quat::Slerp( Quat &from, Quat &to, float t )
- {
- Quat q2, qt;
- // 0.0 returns p, 1.0 return q.
- // decide if one of the Quats is backwards
- q2.Align( from, to );
- SlerpNoAlign( from, q2, t );
- }
- void Quat::SlerpNoAlign( const Quat &p, const Quat &q, float t )
- {
- Quat qt;
- float omega, cosom, sinom, sclp, sclq;
- int i;
- // 0.0 returns p, 1.0 return q.
- cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3];
- if ((1.0f + cosom) > 0.000001f) {
- if ((1.0f - cosom) > 0.000001f) {
- omega = acos( cosom );
- sinom = sin( omega );
- sclp = sin( (1.0f - t)*omega) / sinom;
- sclq = sin( t*omega ) / sinom;
- }
- else {
- // TODO: add short circuit for cosom == 1.0f?
- sclp = 1.0f - t;
- sclq = t;
- }
- for (i = 0; i < 4; i++) {
- qt[i] = sclp * p[i] + sclq * q[i];
- }
- }
- else {
- Assert( &qt != &q );
- qt[0] = -q[1];
- qt[1] = q[0];
- qt[2] = -q[3];
- qt[3] = q[2];
- sclp = sin( (1.0f - t) * (0.5f * M_PI));
- sclq = sin( t * (0.5f * M_PI));
- for (i = 0; i < 3; i++) {
- qt[i] = sclp * p[i] + sclq * qt[i];
- }
- }
- Assert( qt.IsValid() );
- *this = qt;
- }
- inline float Quat::NormalizeInPlace()
- {
- Quat q = *this;
- float radius, iradius;
- Assert( q.IsValid() );
- radius = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
- if ( radius ) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON))
- {
- radius = sqrt(radius);
- iradius = 1.0f/radius;
- q[3] *= iradius;
- q[2] *= iradius;
- q[1] *= iradius;
- q[0] *= iradius;
- }
- return radius;
- }
- void Quat::CreateFromYawPitchRoll(Vector up, float yaw, Vector right, float pitch, Vector forward, float roll)
- {
- // Create a Quat for each rotation, and multiply them
- // together. We normalize them to avoid using the conjugate
- Quat qyaw;
- qyaw.AxisAngle( up, yaw );
- qyaw.NormalizeInPlace();
- Quat qtilt;
- qtilt.AxisAngle( right, pitch );
- qtilt.NormalizeInPlace();
- Quat qroll;
- qroll.AxisAngle( forward, roll );
- qroll.NormalizeInPlace();
- Quat yawpitch = qyaw * qtilt * qroll;
- yawpitch.NormalizeInPlace();
- *this = yawpitch;
- }
- void Quat::Align( const Quat &p, const Quat &q )
- {
- Quat qt;
- int i;
- // decide if one of the Quats is backwards
- float a = 0;
- float b = 0;
- for (i = 0; i < 4; i++)
- {
- a += (p[i]-q[i])*(p[i]-q[i]);
- b += (p[i]+q[i])*(p[i]+q[i]);
- }
- if (a > b)
- {
- for (i = 0; i < 4; i++)
- {
- qt[i] = -q[i];
- }
- }
- else if (&qt != &q)
- {
- for (i = 0; i < 4; i++)
- {
- qt[i] = q[i];
- }
- }
- *this = qt;
- }
- void Quat::TransformVector( Vector &vec )
- {
- Quat qConj, qVec, qResult;
- qVec.x = vec.x;
- qVec.y = vec.y;
- qVec.z = vec.z;
- qVec.w = 0;
- qConj.x = -x;
- qConj.y = -y;
- qConj.z = -z;
- qConj.w = w;
- qResult = *this * qVec * qConj;
- vec = Vector( qVec.x, qVec.y, qVec.z );
- }
- #ifndef M_PI
- #define M_PI 3.14159265358979323846 // matches value in gcc v2 math.h
- #endif
- //#define M_PI_F ((float)(M_PI)) // Shouldn't collide with anything.
- #ifndef DEG2RAD
- #define DEG2RAD( x ) ( x * (M_PI / 180.f) )
- #endif
- void Quat::AxisAngle( const Vector &axis, float angles )
- {
- float sa, ca;
- SinCos( DEG2RAD( angles/2 ), &sa, &ca );
- x = axis.x * sa;
- y = axis.y * sa;
- z = axis.z * sa;
- w = ca;
- }
- void Quat::ToAxisAngle( Vector &axis, float &angle )//ToAxisAngle( Vector &axis, float &angle ) const
- {
- Quat q = *this;
- if (q.w > 1)
- q.NormalizeInPlace(); // if w>1 acos and sqrt will produce errors, this cant happen if Quat is normalised
- angle = 2 * acos(q.w);
- double s = sqrt(1-q.w*q.w); // assuming Quat normalised then w is less than 1, so term always positive.
- if (s < 0.001)
- { // test to avoid divide by zero, s is always positive due to sqrt
- // if s close to zero then direction of axis not important
- axis.x = q.x; // if it is important that axis is normalised then replace with x=1; y=z=0;
- axis.y = q.y;
- axis.z = q.z;
- }
- else
- {
- axis.x = q.x / s; // normalise axis
- axis.y = q.y / s;
- axis.z = q.z / s;
- }
- /*
- const float squareLength = x*x + y*y + z*z;
- const float epsilon = 0.00001f; ///< floating point epsilon for single precision. todo: verify epsilon value and usage
- const float epsilonSquared = epsilon * epsilon;
- if (squareLength>epsilonSquared)
- {
- angle = 2.0f * (float) acos(w);
- const float inverseLength = 1.0f / (float) pow(squareLength, 0.5f);
- axis.x = x * inverseLength;
- axis.y = y * inverseLength;
- axis.z = z * inverseLength;
- }
- else
- {
- angle = 0.0f;
- axis.x = 1.0f;
- axis.y = 0.0f;
- axis.z = 0.0f;
- }
- */
- }
- void Quat::ToAxes (Vector* akAxis) const
- {
- matrix3x4_t kRot;
- ToRotationMatrix(kRot);
- for (size_t iCol = 0; iCol < 3; iCol++)
- {
- akAxis[iCol].x = kRot[0][iCol];
- akAxis[iCol].y = kRot[1][iCol];
- akAxis[iCol].z = kRot[2][iCol];
- }
- }
- Quat Quat::operator/(const Quat & q)const
- //! @brief Overload / operator.
- {
- // Quat q = rhs;
- // q = *this * q.conjugate();
- // q = *this * q.i();
- // return q;
- return *this * q.i();
- }
- Quat Quat::operator*(const float c)const
- /*!
- @brief Overload * operator, multiplication by a scalar.
- \f$q = [s, v]\f$ and let \f$r \in R\f$. Then
- \f$rq = qr = [r, 0][s, v] = [rs, rv]\f$
- The result is not necessarily a unit Quat even if \f$q\f$
- is a unit Quats.
- */
- {
- Quat q;
- q.w = w * c;
- q.x = x * c;
- q.y = y * c;
- q.z = z * c;
- return q;
- }
- Quat Slerp2(const Quat & q0, const Quat & q1, const float t)
- /*!
- @brief Spherical Linear Interpolation.
- Cite_:Dam
- The Quat \f$q(t)\f$ interpolate the Quats \f$q_0\f$
- and \f$q_1\f$ given the parameter \f$t\f$ along the Quat sphere.
- \f[
- q(t) = c_0(t)q_0 + c_1(t)q_1
- \f]
- where \f$c_0\f$ and \f$c_1\f$ are real functions with \f$0\leq t \leq 1\f$.
- As \f$t\f$ varies between 0 and 1. the values \f$q(t)\f$ varies uniformly
- along the circular arc from \f$q_0\f$ and \f$q_1\f$. The angle between
- \f$q(t)\f$ and \f$q_0\f$ is \f$\cos(t\theta)\f$ and the angle between
- \f$q(t)\f$ and \f$q_1\f$ is \f$\cos((1-t)\theta)\f$. Taking the dot product
- of \f$q(t)\f$ and \f$q_0\f$ yields
- \cos(t\theta) = c_0(t) + \cos(\theta)c_1(t)
- and taking the dot product of \f$q(t)\f$ and \f$q_1\f$ yields
- \cos((1-t)\theta) = \cos(\theta)c_0(t) + c_1(t)
- These are two equations with \f$c_0\f$ and \f$c_1\f$. The solution is
- c_0 = \frac{\sin((1-t)\theta)}{\sin(\theta)}
- c_1 = \frac{\sin(t\theta)}{sin(\theta)}
- The interpolation is then
- Slerp(q_0, q_1, t) = \frac{q_0\sin((1-t)\theta)+q_1\sin(t\theta)}{\sin(\theta)}
- If \f$q_0\f$ and \f$q_1\f$ are unit Quats the \f$q(t)\f$ is also a unit
- Quats. For unit Quats we have
- Slerp(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t
- For t = 0 and t = 1 we have
- q_0 = Slerp(q_0, q_1, 0)
- q_1 = Slerp(q_0, q_1, 1)
- It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle
- between q0 ang Gq1 is acute). This choice avoids extra spinning caused
- by the interpolated rotations.
- */
- {
- if( (t < 0) || (t > 1) )
- Msg( "Slerp(q0, q1, t): t < 0 or t > 1. t is set to 0.\n" );
- float dotprod = q0.dot_prod(q1);
- Quat result;
- if(dotprod >= 0)
- result = q0.i() * q1;
- else
- {
- result = q0.i() * -1;
- result = result * q1;
- }
- result = result.power(t);
- result = q0 * result;
- return result;
- }
- Quat Slerp_Prime(const Quat & q0, const Quat & q1,
- const float t)
- /*!
- @brief Spherical Linear Interpolation derivative.
- Cite_: Dam
- The derivative of the function \f$q^t\f$ where \f$q\f$ is a constant
- unit Quat is
- \frac{d}{dt}q^t = q^t log(q)
- Using the preceding equation the Slerp derivative is then
- Slerp'(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t log(q_0^{-1}q_1)
- It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle
- between q0 ang Gq1 is acute). This choice avoids extra spinning caused
- by the interpolated rotations.
- The result is not necessary a unit Quat.
- */
- {
- if( (t < 0) || (t > 1) )
- Msg( "Slerp_prime(q0, q1, t): t < 0 or t > 1. t is set to 0.\n" );
- Quat result;
- float dotprod = q0.dot_prod(q1);
- if(dotprod >= 0)
- {
- result = q0.i() * q1;
- }
- else
- {
- result = q0.i() * -1;
- result = result * q1;
- }
- result = result.Logr();
- result = Slerp2(q0, q1, t) * result;
- return result;
- }
- Quat Quat::i()const
- /*!
- @brief Quat inverse.
- \f[
- q^{-1} = \frac{q^{*}}{N(q)}
- \f]
- where \f$q^{*}\f$ and \f$N(q)\f$ are the Quat
- conjugate and the Quat norm respectively.
- */
- {
- Quat norm(x,y,z,w);
- Quat conj(-x,-y,-z,w);
- norm.NormalizeInPlace();
- return conj/norm;
- }
- Quat Quat::expon() const
- /*!
- @brief Exponential of a Quat.
- Let a Quat of the form \f$q = [0, \theta v]\f$, q is not
- necessarily a unit Quat. Then the exponential function
- is defined by \f$q = [\cos(\theta),v \sin(\theta)]\f$.
- */
- {
- Quat q;
- float theta = sqrt(1.0f);
- float sin_theta = sin(theta);
- q.w = cos(theta);
- if ( fabs(sin_theta) > 0.0001f)
- {
- sin_theta /= theta;
- q.x = x / sin_theta;
- q.y = y / sin_theta;
- q.z = z / sin_theta;
- }
- else
- {
- q.x = x;
- q.y = y;
- q.z = z;
- }
- return q;
- }
- Quat Quat::power(const float t) const
- {
- Quat q;// = (Logr()*t).expon();
- q = Logr();
- q = q * t;
- q = q.expon();
- return q;
- }
- Quat Quat::Logr()const
- /*!
- @brief Logarithm of a unit Quat.
- The logarithm function of a unit Quat
- \f$q = [\cos(\theta), v \sin(\theta)]\f$ is defined as
- \f$log(q) = [0, v\theta]\f$. The result is not necessary
- a unit Quat.
- */
- {
- Quat q;
- q.w = 0;
- float theta = acos(w);
- float sin_theta = sin(theta);
- if ( fabs(sin_theta) > 0.0001f )
- {
- sin_theta *= theta;
- q.x = x / sin_theta;
- q.y = y / sin_theta;
- q.z = z / sin_theta;
- }
- else
- {
- q.x = x;
- q.y = y;
- q.z = z;
- }
- return q;
- }
- float Quat::dot_prod(const Quat & q)const
- /*!
- @brief Quat dot product.
- */
- {
- Quat temp = *this;
- return (temp.w*q.w + temp.x*q.x + temp.y*q.y + temp.z*q.z);//v_(1)*q.v_(1) + v_(2)*q.v_(2) + v_(3)*q.v_(3));
- }
Advertisement
Add Comment
Please, Sign In to add comment