|
Blender
V2.59
|
00001 #include "MT_Optimize.h" 00002 00003 GEN_INLINE MT_Quaternion& MT_Quaternion::operator*=(const MT_Quaternion& q) { 00004 setValue(m_co[3] * q[0] + m_co[0] * q[3] + m_co[1] * q[2] - m_co[2] * q[1], 00005 m_co[3] * q[1] + m_co[1] * q[3] + m_co[2] * q[0] - m_co[0] * q[2], 00006 m_co[3] * q[2] + m_co[2] * q[3] + m_co[0] * q[1] - m_co[1] * q[0], 00007 m_co[3] * q[3] - m_co[0] * q[0] - m_co[1] * q[1] - m_co[2] * q[2]); 00008 return *this; 00009 } 00010 00011 GEN_INLINE void MT_Quaternion::conjugate() { 00012 m_co[0] = -m_co[0]; m_co[1] = -m_co[1]; m_co[2] = -m_co[2]; 00013 } 00014 00015 GEN_INLINE MT_Quaternion MT_Quaternion::conjugate() const { 00016 return MT_Quaternion(-m_co[0], -m_co[1], -m_co[2], m_co[3]); 00017 } 00018 00019 GEN_INLINE void MT_Quaternion::invert() { 00020 conjugate(); 00021 *this /= length2(); 00022 } 00023 00024 GEN_INLINE MT_Quaternion MT_Quaternion::inverse() const { 00025 return conjugate() / length2(); 00026 } 00027 00028 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 00029 // pg. 124-132 00030 GEN_INLINE MT_Quaternion MT_Quaternion::random() { 00031 MT_Scalar x0 = MT_random(); 00032 MT_Scalar r1 = sqrt(MT_Scalar(1.0) - x0), r2 = sqrt(x0); 00033 MT_Scalar t1 = MT_2_PI * MT_random(), t2 = MT_2_PI * MT_random(); 00034 MT_Scalar c1 = cos(t1), s1 = sin(t1); 00035 MT_Scalar c2 = cos(t2), s2 = sin(t2); 00036 return MT_Quaternion(s1 * r1, c1 * r1, s2 * r2, c2 * r2); 00037 } 00038 00039 GEN_INLINE MT_Quaternion operator*(const MT_Quaternion& q1, 00040 const MT_Quaternion& q2) { 00041 return MT_Quaternion(q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1], 00042 q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2], 00043 q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0], 00044 q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]); 00045 } 00046 00047 GEN_INLINE MT_Quaternion operator*(const MT_Quaternion& q, const MT_Vector3& w) 00048 { 00049 return MT_Quaternion( q[3] * w[0] + q[1] * w[2] - q[2] * w[1], 00050 q[3] * w[1] + q[2] * w[0] - q[0] * w[2], 00051 q[3] * w[2] + q[0] * w[1] - q[1] * w[0], 00052 -q[0] * w[0] - q[1] * w[1] - q[2] * w[2]); 00053 } 00054 00055 GEN_INLINE MT_Quaternion operator*(const MT_Vector3& w, const MT_Quaternion& q) 00056 { 00057 return MT_Quaternion( w[0] * q[3] + w[1] * q[2] - w[2] * q[1], 00058 w[1] * q[3] + w[2] * q[0] - w[0] * q[2], 00059 w[2] * q[3] + w[0] * q[1] - w[1] * q[0], 00060 -w[0] * q[0] - w[1] * q[1] - w[2] * q[2]); 00061 } 00062 00063 GEN_INLINE MT_Scalar MT_Quaternion::angle(const MT_Quaternion& q) const 00064 { 00065 MT_Scalar s = sqrt(length2() * q.length2()); 00066 assert(s != MT_Scalar(0.0)); 00067 00068 s = dot(q) / s; 00069 00070 s = MT_clamp(s, -1.0, 1.0); 00071 00072 return acos(s); 00073 } 00074 00075 GEN_INLINE MT_Quaternion MT_Quaternion::slerp(const MT_Quaternion& q, const MT_Scalar& t) const 00076 { 00077 MT_Scalar d, s0, s1; 00078 MT_Scalar s = dot(q); 00079 bool neg = (s < 0.0); 00080 00081 if (neg) 00082 s = -s; 00083 if ((1.0 - s) > 0.0001) 00084 { 00085 MT_Scalar theta = acos(s); 00086 d = MT_Scalar(1.0) / sin(theta); 00087 s0 = sin((MT_Scalar(1.0) - t) * theta); 00088 s1 = sin(t * theta); 00089 } 00090 else 00091 { 00092 d = MT_Scalar(1.0); 00093 s0 = MT_Scalar(1.0) - t; 00094 s1 = t; 00095 } 00096 if (neg) 00097 s1 = -s1; 00098 return d*(*this * s0 + q * s1); 00099 } 00100