- Timestamp:
- Dec 13, 2008, 11:45:51 PM (16 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/branches/physics/src/bullet/LinearMath/btQuaternion.h
r2192 r2430 18 18 #define SIMD__QUATERNION_H_ 19 19 20 20 21 #include "btVector3.h" 21 22 22 / //The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform.23 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */ 23 24 class btQuaternion : public btQuadWord { 24 25 public: 26 /**@brief No initialization constructor */ 25 27 btQuaternion() {} 26 28 27 29 // template <typename btScalar> 28 30 // explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {} 29 31 /**@brief Constructor from scalars */ 30 32 btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) 31 33 : btQuadWord(x, y, z, w) 32 34 {} 33 35 /**@brief Axis angle Constructor 36 * @param axis The axis which the rotation is around 37 * @param angle The magnitude of the rotation around the angle (Radians) */ 34 38 btQuaternion(const btVector3& axis, const btScalar& angle) 35 39 { 36 40 setRotation(axis, angle); 37 41 } 38 42 /**@brief Constructor from Euler angles 43 * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z 44 * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y 45 * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */ 39 46 btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 40 47 { 48 #ifndef BT_EULER_DEFAULT_ZYX 41 49 setEuler(yaw, pitch, roll); 42 } 43 50 #else 51 setEulerZYX(yaw, pitch, roll); 52 #endif 53 } 54 /**@brief Set the rotation using axis angle notation 55 * @param axis The axis around which to rotate 56 * @param angle The magnitude of the rotation in Radians */ 44 57 void setRotation(const btVector3& axis, const btScalar& angle) 45 58 { … … 50 63 btCos(angle * btScalar(0.5))); 51 64 } 52 65 /**@brief Set the quaternion using Euler angles 66 * @param yaw Angle around Y 67 * @param pitch Angle around X 68 * @param roll Angle around Z */ 53 69 void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 54 70 { … … 67 83 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); 68 84 } 69 70 btQuaternion& operator+=(const btQuaternion& q) 71 { 72 m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW; 85 /**@brief Set the quaternion using euler angles 86 * @param yaw Angle around Z 87 * @param pitch Angle around Y 88 * @param roll Angle around X */ 89 void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 90 { 91 btScalar halfYaw = btScalar(yaw) * btScalar(0.5); 92 btScalar halfPitch = btScalar(pitch) * btScalar(0.5); 93 btScalar halfRoll = btScalar(roll) * btScalar(0.5); 94 btScalar cosYaw = btCos(halfYaw); 95 btScalar sinYaw = btSin(halfYaw); 96 btScalar cosPitch = btCos(halfPitch); 97 btScalar sinPitch = btSin(halfPitch); 98 btScalar cosRoll = btCos(halfRoll); 99 btScalar sinRoll = btSin(halfRoll); 100 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x 101 cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y 102 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z 103 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx 104 } 105 /**@brief Add two quaternions 106 * @param q The quaternion to add to this one */ 107 SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) 108 { 109 m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3]; 73 110 return *this; 74 111 } 75 112 113 /**@brief Subtract out a quaternion 114 * @param q The quaternion to subtract from this one */ 76 115 btQuaternion& operator-=(const btQuaternion& q) 77 116 { 78 m_ x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;117 m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3]; 79 118 return *this; 80 119 } 81 120 121 /**@brief Scale this quaternion 122 * @param s The scalar to scale by */ 82 123 btQuaternion& operator*=(const btScalar& s) 83 124 { 84 m_ x *= s; m_y *= s; m_z *= s; m_unusedW*= s;125 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s; 85 126 return *this; 86 127 } 87 128 88 129 /**@brief Multiply this quaternion by q on the right 130 * @param q The other quaternion 131 * Equivilant to this = this * q */ 89 132 btQuaternion& operator*=(const btQuaternion& q) 90 133 { 91 setValue(m_ unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z* q.y(),92 m_ unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x* q.z(),93 m_ unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y* q.x(),94 m_ unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z* q.z());134 setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(), 135 m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(), 136 m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(), 137 m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z()); 95 138 return *this; 96 139 } 97 140 /**@brief Return the dot product between this quaternion and another 141 * @param q The other quaternion */ 98 142 btScalar dot(const btQuaternion& q) const 99 143 { 100 return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW; 101 } 102 144 return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3]; 145 } 146 147 /**@brief Return the length squared of the quaternion */ 103 148 btScalar length2() const 104 149 { … … 106 151 } 107 152 153 /**@brief Return the length of the quaternion */ 108 154 btScalar length() const 109 155 { … … 111 157 } 112 158 159 /**@brief Normalize the quaternion 160 * Such that x^2 + y^2 + z^2 +w^2 = 1 */ 113 161 btQuaternion& normalize() 114 162 { … … 116 164 } 117 165 166 /**@brief Return a scaled version of this quaternion 167 * @param s The scale factor */ 118 168 SIMD_FORCE_INLINE btQuaternion 119 169 operator*(const btScalar& s) const 120 170 { 121 return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s); 122 } 123 124 125 171 return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s); 172 } 173 174 175 /**@brief Return an inversely scaled versionof this quaternion 176 * @param s The inverse scale factor */ 126 177 btQuaternion operator/(const btScalar& s) const 127 178 { … … 130 181 } 131 182 132 183 /**@brief Inversely scale this quaternion 184 * @param s The scale factor */ 133 185 btQuaternion& operator/=(const btScalar& s) 134 186 { … … 137 189 } 138 190 139 191 /**@brief Return a normalized version of this quaternion */ 140 192 btQuaternion normalized() const 141 193 { 142 194 return *this / length(); 143 195 } 144 196 /**@brief Return the angle between this quaternion and the other 197 * @param q The other quaternion */ 145 198 btScalar angle(const btQuaternion& q) const 146 199 { … … 149 202 return btAcos(dot(q) / s); 150 203 } 151 204 /**@brief Return the angle of rotation represented by this quaternion */ 152 205 btScalar getAngle() const 153 206 { 154 btScalar s = btScalar(2.) * btAcos(m_ unusedW);207 btScalar s = btScalar(2.) * btAcos(m_floats[3]); 155 208 return s; 156 209 } 157 210 158 211 159 212 /**@brief Return the inverse of this quaternion */ 160 213 btQuaternion inverse() const 161 214 { 162 return btQuaternion(-m_x, -m_y, -m_z, m_unusedW); 163 } 164 215 return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]); 216 } 217 218 /**@brief Return the sum of this quaternion and the other 219 * @param q2 The other quaternion */ 165 220 SIMD_FORCE_INLINE btQuaternion 166 221 operator+(const btQuaternion& q2) const 167 222 { 168 223 const btQuaternion& q1 = *this; 169 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW); 170 } 171 224 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]); 225 } 226 227 /**@brief Return the difference between this quaternion and the other 228 * @param q2 The other quaternion */ 172 229 SIMD_FORCE_INLINE btQuaternion 173 230 operator-(const btQuaternion& q2) const 174 231 { 175 232 const btQuaternion& q1 = *this; 176 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW); 177 } 178 233 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]); 234 } 235 236 /**@brief Return the negative of this quaternion 237 * This simply negates each element */ 179 238 SIMD_FORCE_INLINE btQuaternion operator-() const 180 239 { 181 240 const btQuaternion& q2 = *this; 182 return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_ unusedW);183 } 184 241 return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]); 242 } 243 /**@todo document this and it's use */ 185 244 SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 186 245 { … … 193 252 } 194 253 254 /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion 255 * @param q The other quaternion to interpolate with 256 * @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. 257 * Slerp interpolates assuming constant velocity. */ 195 258 btQuaternion slerp(const btQuaternion& q, const btScalar& t) const 196 259 { … … 201 264 btScalar s0 = btSin((btScalar(1.0) - t) * theta); 202 265 btScalar s1 = btSin(t * theta); 203 return btQuaternion((m_ x* s0 + q.x() * s1) * d,204 (m_ y* s0 + q.y() * s1) * d,205 (m_ z* s0 + q.z() * s1) * d,206 (m_ unusedW * s0 + q.m_unusedW* s1) * d);266 return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, 267 (m_floats[1] * s0 + q.y() * s1) * d, 268 (m_floats[2] * s0 + q.z() * s1) * d, 269 (m_floats[3] * s0 + q.m_floats[3] * s1) * d); 207 270 } 208 271 else … … 212 275 } 213 276 214 SIMD_FORCE_INLINE const btScalar& getW() const { return m_ unusedW; }277 SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; } 215 278 216 279 … … 218 281 219 282 220 283 /**@brief Return the negative of a quaternion */ 221 284 SIMD_FORCE_INLINE btQuaternion 222 285 operator-(const btQuaternion& q) … … 227 290 228 291 229 292 /**@brief Return the product of two quaternions */ 230 293 SIMD_FORCE_INLINE btQuaternion 231 294 operator*(const btQuaternion& q1, const btQuaternion& q2) { … … 254 317 } 255 318 319 /**@brief Calculate the dot product between two quaternions */ 256 320 SIMD_FORCE_INLINE btScalar 257 321 dot(const btQuaternion& q1, const btQuaternion& q2) … … 261 325 262 326 327 /**@brief Return the length of a quaternion */ 263 328 SIMD_FORCE_INLINE btScalar 264 329 length(const btQuaternion& q) … … 267 332 } 268 333 334 /**@brief Return the angle between two quaternions*/ 269 335 SIMD_FORCE_INLINE btScalar 270 336 angle(const btQuaternion& q1, const btQuaternion& q2) … … 273 339 } 274 340 275 341 /**@brief Return the inverse of a quaternion*/ 276 342 SIMD_FORCE_INLINE btQuaternion 277 343 inverse(const btQuaternion& q) … … 280 346 } 281 347 348 /**@brief Return the result of spherical linear interpolation betwen two quaternions 349 * @param q1 The first quaternion 350 * @param q2 The second quaternion 351 * @param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2 352 * Slerp assumes constant velocity between positions. */ 282 353 SIMD_FORCE_INLINE btQuaternion 283 354 slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
Note: See TracChangeset
for help on using the changeset viewer.