00001 #ifndef H_MATH3D 00002 #define H_MATH3D 00003 00004 #include <math.h> 00005 00006 struct Vector3D; 00007 struct Matrix3; 00008 struct Matrix3D; 00009 struct Plane; 00010 00011 inline float flabs(float f) { return (f>=0.0f?f:-f); } 00012 const float epsilon=1e-8f; 00013 inline bool IsZero(float f) { return flabs(f)<epsilon; } 00014 00015 Vector3D operator*(float scalar, const Vector3D& v); 00016 float operator*(const Vector3D& v1, const Vector3D& v2); 00017 Vector3D operator+(const Vector3D& v1, const Vector3D& v2); 00018 Vector3D operator-(const Vector3D& v1, const Vector3D& v2); 00019 Vector3D CrossProduct(const Vector3D& v1, const Vector3D& v2); 00020 Matrix3D operator*(const Matrix3D& m1, const Matrix3D& m2); 00021 Matrix3D operator*(float scalar, const Matrix3D& m); 00022 00023 struct Vector3D 00024 { 00025 float x,y,z; 00026 static const Vector3D Zero; 00027 00028 Vector3D() {} 00029 Vector3D(float X, float Y, float Z) : x(X), y(Y), z(Z) {} 00030 Vector3D(const Vector3D& v) : x(v.x), y(v.y), z(v.z) {} 00031 00032 Vector3D& operator+=(const Vector3D& v) { x+=v.x; y+=v.y; z+=v.z; return *this; } 00033 Vector3D& operator*=(float s) { x*=s; y*=s; z*=s; return *this; } 00034 Vector3D& operator/=(float s) { return *this *= (1.0f/s); } 00035 bool operator==(const Vector3D& v) { return x==v.x && y==v.y && z==v.z; } 00036 00037 Vector3D operator- () const { return Vector3D(-x,-y,-z); } 00038 float SquareMagnitude () const { return x*x+y*y+z*z; } 00039 float Magnitude () const { return (float)sqrt(SquareMagnitude()); } 00040 Vector3D Normalized () const { return (1.0f/Magnitude())*(*this); } 00041 float operator[] (int i) const { return ((float*)&x)[i]; } 00042 float& operator[] (int i) { return ((float*)&x)[i]; } 00043 }; 00044 00045 #define _11 sclr.s11 00046 #define _12 sclr.s12 00047 #define _13 sclr.s13 00048 #define _14 sclr.s14 00049 #define _21 sclr.s21 00050 #define _22 sclr.s22 00051 #define _23 sclr.s23 00052 #define _24 sclr.s24 00053 #define _31 sclr.s31 00054 #define _32 sclr.s32 00055 #define _33 sclr.s33 00056 #define _34 sclr.s34 00057 #define _41 sclr.s41 00058 #define _42 sclr.s42 00059 #define _43 sclr.s43 00060 #define _44 sclr.s44 00061 00062 struct Matrix3 00063 { 00064 union { 00065 struct { float s11,s12,s13, 00066 s21,s22,s23, 00067 s31,s32,s33; } sclr; 00068 float m[3][3]; 00069 }; 00070 static const Matrix3 Identity; 00071 00072 Vector3D& baseRow(int i) { return *((Vector3D*)m[i]); } 00073 float operator() (int i, int j) const { return m[i][j]; } 00074 float& operator() (int i, int j) { return m[i][j]; } 00075 }; 00076 00077 struct Matrix3D 00078 { 00079 union { 00080 struct { float s11,s12,s13,s14, 00081 s21,s22,s23,s24, 00082 s31,s32,s33,s34, 00083 s41,s42,s43,s44; } sclr; 00084 float m[4][4]; 00085 }; 00086 static const Matrix3D Identity; 00087 00088 Matrix3D() {} 00089 00090 Matrix3D(float f11, float f12, float f13, float f14, 00091 float f21, float f22, float f23, float f24, 00092 float f31, float f32, float f33, float f34, 00093 float f41, float f42, float f43, float f44) 00094 { 00095 _11=f11; _12=f12; _13=f13; _14=f14; 00096 _21=f21; _22=f22; _23=f23; _24=f24; 00097 _31=f31; _32=f32; _33=f33; _34=f34; 00098 _41=f41; _42=f42; _43=f43; _44=f44; 00099 } 00100 00101 Matrix3D& operator*= (const Matrix3D& m) 00102 { 00103 return *this = *this * m; 00104 } 00105 00106 friend Matrix3D PitchMatrix3D(const float theta); 00107 friend Matrix3D YawMatrix3D(const float theta); 00108 friend Matrix3D RollMatrix3D(const float theta); 00109 void rotate(const Vector3D& v); 00110 00111 Matrix3D Inverse() const; 00112 Matrix3D Adjoint() const; 00113 float Determinant() const; 00114 00115 float operator() (int i, int j) const { return m[i][j]; } 00116 float& operator() (int i, int j) { return m[i][j]; } 00117 }; 00118 00119 struct Plane 00120 { 00121 Vector3D normal; 00122 float d; 00123 00124 Plane(const Vector3D& a, const Vector3D& b, const Vector3D& c) 00125 { 00126 normal = CrossProduct(b - a, c - a).Normalized(); 00127 d = -normal * a; 00128 } 00129 00130 float Classify(const Vector3D& v) 00131 { 00132 return v * normal + d; 00133 } 00134 }; 00135 00136 inline Vector3D operator* (float scalar, const Vector3D& v) 00137 { 00138 return Vector3D(scalar*v.x,scalar*v.y,scalar*v.z); 00139 } 00140 00141 inline Vector3D operator+ (const Vector3D& v1, const Vector3D& v2) 00142 { 00143 return Vector3D(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z); 00144 } 00145 00146 inline Vector3D operator- (const Vector3D& v1, const Vector3D& v2) 00147 { 00148 return Vector3D(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z); 00149 } 00150 00151 inline float operator* (const Vector3D& v1, const Vector3D& v2) 00152 { 00153 return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z; 00154 } 00155 00156 inline Vector3D CrossProduct(const Vector3D& v1, const Vector3D& v2) 00157 { 00158 return Vector3D(v1.y*v2.z-v2.y*v1.z, 00159 v1.z*v2.x-v2.z*v1.x, 00160 v1.x*v2.y-v2.x*v1.y); 00161 } 00162 00163 inline Vector3D Transform(const Vector3D& v, const Matrix3D& m) 00164 { 00165 return Vector3D(v.x*m._11 + v.y*m._21 + v.z*m._31 + m._41, 00166 v.x*m._12 + v.y*m._22 + v.z*m._32 + m._42, 00167 v.x*m._13 + v.y*m._23 + v.z*m._33 + m._43); 00168 } 00169 00170 inline Vector3D rotateVector(const Vector3D& v, const Matrix3D& m) 00171 { 00172 return Vector3D(v.x*m._11 + v.y*m._21 + v.z*m._31, 00173 v.x*m._12 + v.y*m._22 + v.z*m._32, 00174 v.x*m._13 + v.y*m._23 + v.z*m._33); 00175 } 00176 00177 inline Matrix3D operator*(float scalar, const Matrix3D& m) 00178 { 00179 return Matrix3D(scalar*m(0,0),scalar*m(0,1),scalar*m(0,2),scalar*m(0,3), 00180 scalar*m(1,0),scalar*m(1,1),scalar*m(1,2),scalar*m(1,3), 00181 scalar*m(2,0),scalar*m(2,1),scalar*m(2,2),scalar*m(2,3), 00182 scalar*m(3,0),scalar*m(3,1),scalar*m(3,2),scalar*m(3,3)); 00183 } 00184 00185 inline Matrix3D operator*(const Matrix3D& m1, const Matrix3D& m2) 00186 { 00187 return Matrix3D( 00188 m1._11*m2._11 + m1._12*m2._21 + m1._13*m2._31 + m1._14*m2._41, 00189 m1._11*m2._12 + m1._12*m2._22 + m1._13*m2._32 + m1._14*m2._42, 00190 m1._11*m2._13 + m1._12*m2._23 + m1._13*m2._33 + m1._14*m2._43, 00191 m1._11*m2._14 + m1._12*m2._24 + m1._13*m2._34 + m1._14*m2._44, 00192 m1._21*m2._11 + m1._22*m2._21 + m1._23*m2._31 + m1._24*m2._41, 00193 m1._21*m2._12 + m1._22*m2._22 + m1._23*m2._32 + m1._24*m2._42, 00194 m1._21*m2._13 + m1._22*m2._23 + m1._23*m2._33 + m1._24*m2._43, 00195 m1._21*m2._14 + m1._22*m2._24 + m1._23*m2._34 + m1._24*m2._44, 00196 m1._31*m2._11 + m1._32*m2._21 + m1._33*m2._31 + m1._34*m2._41, 00197 m1._31*m2._12 + m1._32*m2._22 + m1._33*m2._32 + m1._34*m2._42, 00198 m1._31*m2._13 + m1._32*m2._23 + m1._33*m2._33 + m1._34*m2._43, 00199 m1._31*m2._14 + m1._32*m2._24 + m1._33*m2._34 + m1._34*m2._44, 00200 m1._41*m2._11 + m1._42*m2._21 + m1._43*m2._31 + m1._44*m2._41, 00201 m1._41*m2._12 + m1._42*m2._22 + m1._43*m2._32 + m1._44*m2._42, 00202 m1._41*m2._13 + m1._42*m2._23 + m1._43*m2._33 + m1._44*m2._43, 00203 m1._41*m2._14 + m1._42*m2._24 + m1._43*m2._34 + m1._44*m2._44); 00204 } 00205 00206 inline void 00207 Matrix3D::rotate(const Vector3D& v) 00208 { 00209 if (v.x!=0.0f) *this = PitchMatrix3D(v.x) * (*this); 00210 if (v.y!=0.0f) *this = YawMatrix3D (v.y) * (*this); 00211 if (v.z!=0.0f) *this = RollMatrix3D (v.z) * (*this); 00212 } 00213 00214 inline Matrix3D 00215 TranslateMatrix3D(const Vector3D& v) 00216 { 00217 return Matrix3D(1.0f,0.0f,0.0f,0.0f, 00218 0.0f,1.0f,0.0f,0.0f, 00219 0.0f,0.0f,1.0f,0.0f, 00220 v.x, v.y, v.z,1.0f); 00221 } 00222 00223 00224 inline Matrix3D 00225 ScaleMatrix3D(const Vector3D& v) 00226 { 00227 return Matrix3D( v.x,0.0f,0.0f,0.0f, 00228 0.0f, v.y,0.0f,0.0f, 00229 0.0f,0.0f, v.z,0.0f, 00230 0.0f,0.0f,0.0f,1.0f); 00231 } 00232 00233 00234 inline Matrix3D 00235 ScaleMatrix3D(const float s) 00236 { 00237 return ScaleMatrix3D(Vector3D(s,s,s)); 00238 } 00239 00240 00241 inline Matrix3D 00242 PitchMatrix3D(const float c, const float s) 00243 { 00244 return Matrix3D(1.0f, 0.0f, 0.0f, 0.0f, 00245 0.0f, c, -s, 0.0f, 00246 0.0f, s, c, 0.0f, 00247 0.0f, 0.0f, 0.0f, 1.0f); 00248 } 00249 00250 00251 inline Matrix3D 00252 PitchMatrix3D(const float theta) 00253 { 00254 return PitchMatrix3D((float) cos(theta), (float) sin(theta)); 00255 } 00256 00257 00258 inline Matrix3D 00259 YawMatrix3D(const float c, const float s) 00260 { 00261 return Matrix3D( c, 0.0f, s, 0.0f, 00262 0.0f, 1.0f, 0.0f, 0.0f, 00263 -s, 0.0f, c, 0.0f, 00264 0.0f, 0.0f, 0.0f, 1.0f); 00265 } 00266 00267 00268 inline Matrix3D 00269 YawMatrix3D(const float theta) 00270 { 00271 return YawMatrix3D((float) cos(theta), (float) sin(theta)); 00272 } 00273 00274 00275 inline Matrix3D 00276 RollMatrix3D(const float c, const float s) 00277 { 00278 return Matrix3D(c, -s, 0.0f, 0.0f, 00279 s, c, 0.0f, 0.0f, 00280 0.0f, 0.0f, 1.0f, 0.0f, 00281 0.0f, 0.0f, 0.0f, 1.0f); 00282 } 00283 00284 00285 inline Matrix3D 00286 RollMatrix3D(const float theta) 00287 { 00288 return RollMatrix3D((float) cos(theta), (float) sin(theta)); 00289 } 00290 00291 00292 template<class T> 00293 inline T Max(T a, T b) 00294 { 00295 return (a>b ? a : b); 00296 } 00297 00298 template<class T> 00299 inline T Min(T a, T b) 00300 { 00301 return (a<b ? a : b); 00302 } 00303 00304 #endif // H_MATH3D