1 /* 2 Copyright (c) 2005-2007 Erin Catto http://www.gphysics.com 3 4 This software is provided 'as-is', without any express or implied 5 warranty. In no event will the authors be held liable for any damages 6 arising from the use of this software. 7 8 Permission is granted to anyone to use this software for any purpose, 9 including commercial applications, and to alter it and redistribute it 10 freely, subject to the following restrictions: 11 12 1. The origin of this software must not be misrepresented; you must not 13 claim that you wrote the original software. If you use this software 14 in a product, an acknowledgment in the product documentation would be 15 appreciated but is not required. 16 2. Altered source versions must be plainly marked as such, and must not be 17 misrepresented as being the original software. 18 3. This notice may not be removed or altered from any source distribution. 19 */ 20 21 #ifndef MATHUTILS_H 22 #define MATHUTILS_H 23 24 #include <math.h> 25 26 struct Vec3 27 { Vec3Vec328 Vec3() : x(0.f),y(0.f),z(0.f) {} Vec3Vec329 Vec3(float x, float y, float z) : x(x), y(y), z(z) {} 30 SetZeroVec331 void SetZero() { x = y = z = 0.0f; } 32 33 Vec3& operator *= (float s) 34 { 35 x *= s; 36 y *= s; 37 z *= s; 38 return *this; 39 } 40 41 Vec3& operator += (const Vec3& v) 42 { 43 x += v.x; 44 y += v.y; 45 z += v.z; 46 return *this; 47 } 48 LengthVec349 float Length() const 50 { 51 return sqrtf(x*x + y*y + z*z); 52 } 53 NormalizeVec354 void Normalize() 55 { 56 float norm = Length(); 57 if (norm) 58 { 59 float inv = 1.0f/norm; 60 x *= inv; y *= inv; z *= inv; 61 } 62 } 63 64 float x, y, z; 65 }; 66 67 68 inline Vec3 operator + (const Vec3& a, const Vec3& b) 69 { 70 return Vec3(a.x + b.x, a.y + b.y, a.z + b.z); 71 } 72 73 inline Vec3 operator - (const Vec3& a, const Vec3& b) 74 { 75 return Vec3(a.x - b.x, a.y - b.y, a.z - b.z); 76 } 77 78 inline Vec3 operator * (float s, const Vec3& a) 79 { 80 return Vec3(s*a.x, s*a.y, s*a.z); 81 } 82 83 // Dot product 84 inline float operator * (const Vec3& a, const Vec3& b) 85 { 86 return a.x*b.x + a.y*b.y + a.z*b.z; 87 } 88 89 // Cross product 90 inline Vec3 operator % (const Vec3& a, const Vec3& b) 91 { 92 return Vec3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x); 93 } 94 95 96 struct Quat 97 { QuatQuat98 Quat() : x(0.f),y(0.f),z(0.f),w(0.f) {} QuatQuat99 Quat(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {} 100 101 Quat& operator += (const Quat& q) 102 { 103 x += q.x; y += q.y; z += q.z; w += q.w; 104 return *this; 105 } 106 NormalizeQuat107 void Normalize() 108 { 109 float len = sqrtf(x*x + y*y + z*z + w*w); 110 if (len) 111 { 112 x /= len; y /= len; z /= len; w /= len; 113 } 114 else 115 { 116 x = 0; y = 0; z = 0; w = 1; 117 } 118 } 119 ConjugateQuat120 Quat Conjugate() const 121 { 122 return Quat(-x, -y, -z, w); 123 } 124 RotateQuat125 Vec3 Rotate(const Vec3& v) const 126 { 127 Vec3 s(x,y,z); 128 return v + 2.0f*(s % (s % v + w*v)); 129 } 130 131 float x, y, z, w; 132 }; 133 134 // Quat multiplication 135 inline Quat operator * (const Quat& a, const Quat& b) 136 { 137 Quat result; 138 139 float aW = a.w, aX = a.x, aY = a.y, aZ = a.z; 140 float bW = b.w, bX = b.x, bY = b.y, bZ = b.z; 141 142 result.x = aW*bX + bW*aX + aY*bZ - aZ*bY; 143 result.y = aW*bY + bW*aY + aZ*bX - aX*bZ; 144 result.z = aW*bZ + bW*aZ + aX*bY - aY*bX; 145 result.w = aW*bW - (aX*bX + aY*bY + aZ*bZ); 146 147 return result; 148 } 149 150 inline Quat operator * (float s, const Quat& q) 151 { 152 return Quat(s*q.x, s*q.y, s*q.z, s*q.w); 153 } 154 155 struct Mat33 156 { Mat33Mat33157 Mat33() {} Mat33Mat33158 Mat33(const Quat& q) 159 { 160 float x = q.x, y = q.y, z = q.z, w = q.w; 161 float x2 = x+x, y2 = y+y, z2 = z+z; 162 float xx = x*x2, xy = x*y2, xz = x*z2; 163 float yy = y*y2, yz = y*z2, zz = z*z2; 164 float wx = w*x2, wy = w*y2, wz = w*z2; 165 166 col1.x = 1 - (yy + zz); 167 col2.x = xy - wz; 168 col3.x = xz + wy; 169 170 col1.y = xy + wz; 171 col2.y = 1 - (xx + zz); 172 col3.y = yz - wx; 173 174 col1.z = xz - wy; 175 col2.z = yz + wx; 176 col3.z = 1 - (xx + yy); 177 } 178 operatorMat33179 float& operator ()(int row, int col) { return *( (float*)this + col*3 + row); } operatorMat33180 float operator ()(int row, int col) const { return *( (float*)this + col*3 + row); } 181 182 Vec3 col1, col2, col3; 183 }; 184 185 struct RigidBody 186 { 187 Vec3 x; // world position of center of mass 188 Quat q; // rotation 189 Vec3 v; // velocity of center of mass 190 Vec3 omega; // angular velocity 191 Vec3 F; // force at center of mass 192 Vec3 T; // torque 193 Vec3 inertia; // rotational inertia 194 float mass; 195 196 }; 197 198 #endif 199