00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00089 #include <math.h>
00090
00097 #ifndef CYCLONE_CORE_H
00098 #define CYCLONE_CORE_H
00099
00100 #include "precision.h"
00101
00107
00108 namespace cyclone {
00109
00111
00119 extern real sleepEpsilon;
00120
00137 void setSleepEpsilon(real value);
00138
00148 real getSleepEpsilon();
00149
00151
00155
00160
00161
00162 class Vector3
00163 {
00165 public:
00167 real x;
00168
00170 real y;
00171
00173 real z;
00174
00175 private:
00177 real pad;
00178
00179 public:
00181 Vector3() : x(0), y(0), z(0) {}
00182
00187 Vector3(const real x, const real y, const real z)
00188 : x(x), y(y), z(z) {}
00190
00191 const static Vector3 GRAVITY;
00192 const static Vector3 HIGH_GRAVITY;
00193 const static Vector3 UP;
00194 const static Vector3 RIGHT;
00195 const static Vector3 OUT;
00196 const static Vector3 X;
00197 const static Vector3 Y;
00198 const static Vector3 Z;
00199
00201
00202
00204
00205 real operator[](unsigned i) const
00206 {
00207 if (i == 0) return x;
00208 if (i == 1) return y;
00209 return z;
00210 }
00211
00212 real& operator[](unsigned i)
00213 {
00214 if (i == 0) return x;
00215 if (i == 1) return y;
00216 return z;
00217 }
00218
00220
00221 void operator+=(const Vector3& v)
00222 {
00223 x += v.x;
00224 y += v.y;
00225 z += v.z;
00226 }
00227
00231 Vector3 operator+(const Vector3& v) const
00232 {
00233 return Vector3(x+v.x, y+v.y, z+v.z);
00234 }
00236
00238
00239 void operator-=(const Vector3& v)
00240 {
00241 x -= v.x;
00242 y -= v.y;
00243 z -= v.z;
00244 }
00245
00249 Vector3 operator-(const Vector3& v) const
00250 {
00251 return Vector3(x-v.x, y-v.y, z-v.z);
00252 }
00254
00256
00257 void operator*=(const real value)
00258 {
00259 x *= value;
00260 y *= value;
00261 z *= value;
00262 }
00263
00265 Vector3 operator*(const real value) const
00266 {
00267 return Vector3(x*value, y*value, z*value);
00268 }
00270
00272
00276 Vector3 componentProduct(const Vector3 &vector) const
00277 {
00278 return Vector3(x * vector.x, y * vector.y, z * vector.z);
00279 }
00280
00285 void componentProductUpdate(const Vector3 &vector)
00286 {
00287 x *= vector.x;
00288 y *= vector.y;
00289 z *= vector.z;
00290 }
00292
00294
00298 Vector3 vectorProduct(const Vector3 &vector) const
00299 {
00300 return Vector3(y*vector.z-z*vector.y,
00301 z*vector.x-x*vector.z,
00302 x*vector.y-y*vector.x);
00303 }
00304
00309 void operator %=(const Vector3 &vector)
00310 {
00311 *this = vectorProduct(vector);
00312 }
00313
00318 Vector3 operator%(const Vector3 &vector) const
00319 {
00320 return Vector3(y*vector.z-z*vector.y,
00321 z*vector.x-x*vector.z,
00322 x*vector.y-y*vector.x);
00323 }
00325
00327
00331 real scalarProduct(const Vector3 &vector) const
00332 {
00333 return x*vector.x + y*vector.y + z*vector.z;
00334 }
00335
00340 real operator *(const Vector3 &vector) const
00341 {
00342 return x*vector.x + y*vector.y + z*vector.z;
00343 }
00345
00347
00350 void addScaledVector(const Vector3& vector, real scale)
00351 {
00352 x += vector.x * scale;
00353 y += vector.y * scale;
00354 z += vector.z * scale;
00355 }
00357
00359
00360 real magnitude() const
00361 {
00362 return real_sqrt(x*x+y*y+z*z);
00363 }
00364
00366 real squareMagnitude() const
00367 {
00368 return x*x+y*y+z*z;
00369 }
00370
00372 void normalise()
00373 {
00374 real l = magnitude();
00375 if (l > 0)
00376 {
00377 (*this) *= ((real)1)/l;
00378 }
00379 }
00381
00383 bool operator==(const Vector3& other) const
00384 {
00385 return x == other.x &&
00386 y == other.y &&
00387 z == other.z;
00388 }
00389
00391 bool operator!=(const Vector3& other) const
00392 {
00393 return !(*this == other);
00394 }
00395
00403 bool operator<(const Vector3& other) const
00404 {
00405 return x < other.x && y < other.y && z < other.z;
00406 }
00407
00415 bool operator>(const Vector3& other) const
00416 {
00417 return x > other.x && y > other.y && z > other.z;
00418 }
00419
00427 bool operator<=(const Vector3& other) const
00428 {
00429 return x <= other.x && y <= other.y && z <= other.z;
00430 }
00431
00439 bool operator>=(const Vector3& other) const
00440 {
00441 return x >= other.x && y >= other.y && z >= other.z;
00442 }
00443
00445 void clear()
00446 {
00447 x = y = z = 0;
00448 }
00450
00452 void invert()
00453 {
00454 x = -x;
00455 y = -y;
00456 x = -z;
00457 }
00458
00460 };
00462
00464
00467
00482
00483 class Quaternion
00484 {
00486 public:
00487 union {
00488 struct {
00492 real r;
00493
00498 real i;
00499
00504 real j;
00505
00510 real k;
00511 };
00512
00516 real data[4];
00517 };
00519
00521
00522
00524
00528 Quaternion() : r(1), i(0), j(0), k(0) {}
00529
00554 Quaternion(const real r, const real i, const real j, const real k)
00555 : r(r), i(i), j(j), k(k)
00556 {
00557 }
00558
00560
00564 void normalise()
00565 {
00566 real d = r*r+i*i+j*j+k*k;
00567
00568
00569
00570 if (d == 0) {
00571 r = 1;
00572 return;
00573 }
00574
00575 d = ((real)1.0)/real_sqrt(d);
00576 r *= d;
00577 i *= d;
00578 j *= d;
00579 k *= d;
00580 }
00582
00584
00589 void operator *=(const Quaternion &multiplier)
00590 {
00591 Quaternion q = *this;
00592 r = q.r*multiplier.r - q.i*multiplier.i -
00593 q.j*multiplier.j - q.k*multiplier.k;
00594 i = q.r*multiplier.i + q.i*multiplier.r +
00595 q.j*multiplier.k - q.k*multiplier.j;
00596 j = q.r*multiplier.j + q.j*multiplier.r +
00597 q.k*multiplier.i - q.i*multiplier.k;
00598 k = q.r*multiplier.k + q.k*multiplier.r +
00599 q.i*multiplier.j - q.j*multiplier.i;
00600 }
00602
00604
00613 void addScaledVector(const Vector3& vector, real scale)
00614 {
00615 Quaternion q(0,
00616 vector.x * scale,
00617 vector.y * scale,
00618 vector.z * scale);
00619 q *= *this;
00620 r += q.r * ((real)0.5);
00621 i += q.i * ((real)0.5);
00622 j += q.j * ((real)0.5);
00623 k += q.k * ((real)0.5);
00624 }
00626
00628 void rotateByVector(const Vector3& vector)
00629 {
00630 Quaternion q(0, vector.x, vector.y, vector.z);
00631 (*this) *= q;
00632 }
00635 };
00637
00639
00644 class Matrix4
00645 {
00647 public:
00651 real data[12];
00653
00655
00656
00658
00662 Matrix4()
00663 {
00664 data[1] = data[2] = data[3] = data[4] = data[6] =
00665 data[7] = data[8] = data[9] = data[11] = 0;
00666 data[0] = data[5] = data[10] = 1;
00667 }
00668
00672 void setDiagonal(real a, real b, real c)
00673 {
00674 data[0] = a;
00675 data[5] = b;
00676 data[10] = c;
00677 }
00678
00680
00684 Matrix4 operator*(const Matrix4 &o) const
00685 {
00686 Matrix4 result;
00687 result.data[0] = (o.data[0]*data[0]) + (o.data[4]*data[1]) + (o.data[8]*data[2]);
00688 result.data[4] = (o.data[0]*data[4]) + (o.data[4]*data[5]) + (o.data[8]*data[6]);
00689 result.data[8] = (o.data[0]*data[8]) + (o.data[4]*data[9]) + (o.data[8]*data[10]);
00690
00691 result.data[1] = (o.data[1]*data[0]) + (o.data[5]*data[1]) + (o.data[9]*data[2]);
00692 result.data[5] = (o.data[1]*data[4]) + (o.data[5]*data[5]) + (o.data[9]*data[6]);
00693 result.data[9] = (o.data[1]*data[8]) + (o.data[5]*data[9]) + (o.data[9]*data[10]);
00694
00695 result.data[2] = (o.data[2]*data[0]) + (o.data[6]*data[1]) + (o.data[10]*data[2]);
00696 result.data[6] = (o.data[2]*data[4]) + (o.data[6]*data[5]) + (o.data[10]*data[6]);
00697 result.data[10] = (o.data[2]*data[8]) + (o.data[6]*data[9]) + (o.data[10]*data[10]);
00698
00699 result.data[3] = (o.data[3]*data[0]) + (o.data[7]*data[1]) + (o.data[11]*data[2]) + data[3];
00700 result.data[7] = (o.data[3]*data[4]) + (o.data[7]*data[5]) + (o.data[11]*data[6]) + data[7];
00701 result.data[11] = (o.data[3]*data[8]) + (o.data[7]*data[9]) + (o.data[11]*data[10]) + data[11];
00702
00703 return result;
00704 }
00706
00708
00713 Vector3 operator*(const Vector3 &vector) const
00714 {
00715 return Vector3(
00716 vector.x * data[0] +
00717 vector.y * data[1] +
00718 vector.z * data[2] + data[3],
00719
00720 vector.x * data[4] +
00721 vector.y * data[5] +
00722 vector.z * data[6] + data[7],
00723
00724 vector.x * data[8] +
00725 vector.y * data[9] +
00726 vector.z * data[10] + data[11]
00727 );
00728 }
00730
00736 Vector3 transform(const Vector3 &vector) const
00737 {
00738 return (*this) * vector;
00739 }
00740
00742
00745 real getDeterminant() const;
00746
00752 void setInverse(const Matrix4 &m);
00753
00755 Matrix4 inverse() const
00756 {
00757 Matrix4 result;
00758 result.setInverse(*this);
00759 return result;
00760 }
00761
00765 void invert()
00766 {
00767 setInverse(*this);
00768 }
00770
00772
00775
00781
00782 Vector3 transformDirection(const Vector3 &vector) const
00783 {
00784 return Vector3(
00785 vector.x * data[0] +
00786 vector.y * data[1] +
00787 vector.z * data[2],
00788
00789 vector.x * data[4] +
00790 vector.y * data[5] +
00791 vector.z * data[6],
00792
00793 vector.x * data[8] +
00794 vector.y * data[9] +
00795 vector.z * data[10]
00796 );
00797 }
00798
00803
00816
00817 Vector3 transformInverseDirection(const Vector3 &vector) const
00818 {
00819 return Vector3(
00820 vector.x * data[0] +
00821 vector.y * data[4] +
00822 vector.z * data[8],
00823
00824 vector.x * data[1] +
00825 vector.y * data[5] +
00826 vector.z * data[9],
00827
00828 vector.x * data[2] +
00829 vector.y * data[6] +
00830 vector.z * data[10]
00831 );
00832 }
00834
00836
00840
00850
00851 Vector3 transformInverse(const Vector3 &vector) const
00852 {
00853 Vector3 tmp = vector;
00854 tmp.x -= data[3];
00855 tmp.y -= data[7];
00856 tmp.z -= data[11];
00857 return Vector3(
00858 tmp.x * data[0] +
00859 tmp.y * data[4] +
00860 tmp.z * data[8],
00861
00862 tmp.x * data[1] +
00863 tmp.y * data[5] +
00864 tmp.z * data[9],
00865
00866 tmp.x * data[2] +
00867 tmp.y * data[6] +
00868 tmp.z * data[10]
00869 );
00870 }
00872
00881 Vector3 getAxisVector(int i) const
00882 {
00883 return Vector3(data[i], data[i+4], data[i+8]);
00884 }
00885
00887
00891 void setOrientationAndPos(const Quaternion &q, const Vector3 &pos)
00892 {
00893 data[0] = 1 - (2*q.j*q.j + 2*q.k*q.k);
00894 data[1] = 2*q.i*q.j + 2*q.k*q.r;
00895 data[2] = 2*q.i*q.k - 2*q.j*q.r;
00896 data[3] = pos.x;
00897
00898 data[4] = 2*q.i*q.j - 2*q.k*q.r;
00899 data[5] = 1 - (2*q.i*q.i + 2*q.k*q.k);
00900 data[6] = 2*q.j*q.k + 2*q.i*q.r;
00901 data[7] = pos.y;
00902
00903 data[8] = 2*q.i*q.k + 2*q.j*q.r;
00904 data[9] = 2*q.j*q.k - 2*q.i*q.r;
00905 data[10] = 1 - (2*q.i*q.i + 2*q.j*q.j);
00906 data[11] = pos.z;
00907 }
00909
00916 void fillGLArray(float array[16]) const
00917 {
00918 array[0] = (float)data[0];
00919 array[1] = (float)data[4];
00920 array[2] = (float)data[8];
00921 array[3] = (float)0;
00922
00923 array[4] = (float)data[1];
00924 array[5] = (float)data[5];
00925 array[6] = (float)data[9];
00926 array[7] = (float)0;
00927
00928 array[8] = (float)data[2];
00929 array[9] = (float)data[6];
00930 array[10] = (float)data[10];
00931 array[11] = (float)0;
00932
00933 array[12] = (float)data[3];
00934 array[13] = (float)data[7];
00935 array[14] = (float)data[11];
00936 array[15] = (float)1;
00937 }
00939 };
00941
00943
00950 class Matrix3
00952 {
00953 public:
00957 real data[9];
00959
00961
00962
00964
00967 Matrix3()
00968 {
00969 data[0] = data[1] = data[2] = data[3] = data[4] = data[5] =
00970 data[6] = data[7] = data[8] = 0;
00971 }
00972
00977 Matrix3(const Vector3 &compOne, const Vector3 &compTwo,
00978 const Vector3 &compThree)
00979 {
00980 setComponents(compOne, compTwo, compThree);
00981 }
00982
00986 Matrix3(real c0, real c1, real c2, real c3, real c4, real c5,
00987 real c6, real c7, real c8)
00988 {
00989 data[0] = c0; data[1] = c1; data[2] = c2;
00990 data[3] = c3; data[4] = c4; data[5] = c5;
00991 data[6] = c6; data[7] = c7; data[8] = c8;
00992 }
00993
00998 void setDiagonal(real a, real b, real c)
00999 {
01000 setInertiaTensorCoeffs(a, b, c);
01001 }
01002
01006 void setInertiaTensorCoeffs(real ix, real iy, real iz,
01007 real ixy=0, real ixz=0, real iyz=0)
01008 {
01009 data[0] = ix;
01010 data[1] = data[3] = -ixy;
01011 data[2] = data[6] = -ixz;
01012 data[4] = iy;
01013 data[5] = data[7] = -iyz;
01014 data[8] = iz;
01015 }
01016
01022 void setBlockInertiaTensor(const Vector3 &halfSizes, real mass)
01023 {
01024 Vector3 squares = halfSizes.componentProduct(halfSizes);
01025 setInertiaTensorCoeffs(0.3f*mass*(squares.y + squares.z),
01026 0.3f*mass*(squares.x + squares.z),
01027 0.3f*mass*(squares.x + squares.y));
01028 }
01029
01031
01037 void setSkewSymmetric(const Vector3 vector)
01038 {
01039 data[0] = data[4] = data[8] = 0;
01040 data[1] = -vector.z;
01041 data[2] = vector.y;
01042 data[3] = vector.z;
01043 data[5] = -vector.x;
01044 data[6] = -vector.y;
01045 data[7] = vector.x;
01046 }
01048
01050
01054 void setComponents(const Vector3 &compOne, const Vector3 &compTwo,
01055 const Vector3 &compThree)
01056 {
01057 data[0] = compOne.x;
01058 data[1] = compTwo.x;
01059 data[2] = compThree.x;
01060 data[3] = compOne.y;
01061 data[4] = compTwo.y;
01062 data[5] = compThree.y;
01063 data[6] = compOne.z;
01064 data[7] = compTwo.z;
01065 data[8] = compThree.z;
01066
01067 }
01069
01071
01076 Vector3 operator*(const Vector3 &vector) const
01077 {
01078 return Vector3(
01079 vector.x * data[0] + vector.y * data[1] + vector.z * data[2],
01080 vector.x * data[3] + vector.y * data[4] + vector.z * data[5],
01081 vector.x * data[6] + vector.y * data[7] + vector.z * data[8]
01082 );
01083 }
01085
01091 Vector3 transform(const Vector3 &vector) const
01092 {
01093 return (*this) * vector;
01094 }
01095
01101 Vector3 transformTranspose(const Vector3 &vector) const
01102 {
01103 return Vector3(
01104 vector.x * data[0] + vector.y * data[3] + vector.z * data[6],
01105 vector.x * data[1] + vector.y * data[4] + vector.z * data[7],
01106 vector.x * data[2] + vector.y * data[5] + vector.z * data[8]
01107 );
01108 }
01109
01115 Vector3 getRowVector(int i) const
01116 {
01117 return Vector3(data[i*3], data[i*3+1], data[i*3+2]);
01118 }
01119
01127 Vector3 getAxisVector(int i) const
01128 {
01129 return Vector3(data[i], data[i+3], data[i+6]);
01130 }
01131
01133
01138 void setInverse(const Matrix3 &m)
01139 {
01140 real t4 = m.data[0]*m.data[4];
01141 real t6 = m.data[0]*m.data[5];
01142 real t8 = m.data[1]*m.data[3];
01143 real t10 = m.data[2]*m.data[3];
01144 real t12 = m.data[1]*m.data[6];
01145 real t14 = m.data[2]*m.data[6];
01146
01147
01148 real t16 = (t4*m.data[8] - t6*m.data[7] - t8*m.data[8]+
01149 t10*m.data[7] + t12*m.data[5] - t14*m.data[4]);
01150
01151
01152 if (t16 == (real)0.0f) return;
01153 real t17 = 1/t16;
01154
01155 data[0] = (m.data[4]*m.data[8]-m.data[5]*m.data[7])*t17;
01156 data[1] = -(m.data[1]*m.data[8]-m.data[2]*m.data[7])*t17;
01157 data[2] = (m.data[1]*m.data[5]-m.data[2]*m.data[4])*t17;
01158 data[3] = -(m.data[3]*m.data[8]-m.data[5]*m.data[6])*t17;
01159 data[4] = (m.data[0]*m.data[8]-t14)*t17;
01160 data[5] = -(t6-t10)*t17;
01161 data[6] = (m.data[3]*m.data[7]-m.data[4]*m.data[6])*t17;
01162 data[7] = -(m.data[0]*m.data[7]-t12)*t17;
01163 data[8] = (t4-t8)*t17;
01164 }
01165
01167 Matrix3 inverse() const
01168 {
01169 Matrix3 result;
01170 result.setInverse(*this);
01171 return result;
01172 }
01173
01177 void invert()
01178 {
01179 setInverse(*this);
01180 }
01182
01184
01189 void setTranspose(const Matrix3 &m)
01190 {
01191 data[0] = m.data[0];
01192 data[1] = m.data[3];
01193 data[2] = m.data[6];
01194 data[3] = m.data[1];
01195 data[4] = m.data[4];
01196 data[5] = m.data[7];
01197 data[6] = m.data[2];
01198 data[7] = m.data[5];
01199 data[8] = m.data[8];
01200 }
01201
01203 Matrix3 transpose() const
01204 {
01205 Matrix3 result;
01206 result.setTranspose(*this);
01207 return result;
01208 }
01210
01212
01216 Matrix3 operator*(const Matrix3 &o) const
01217 {
01218 return Matrix3(
01219 data[0]*o.data[0] + data[1]*o.data[3] + data[2]*o.data[6],
01220 data[0]*o.data[1] + data[1]*o.data[4] + data[2]*o.data[7],
01221 data[0]*o.data[2] + data[1]*o.data[5] + data[2]*o.data[8],
01222
01223 data[3]*o.data[0] + data[4]*o.data[3] + data[5]*o.data[6],
01224 data[3]*o.data[1] + data[4]*o.data[4] + data[5]*o.data[7],
01225 data[3]*o.data[2] + data[4]*o.data[5] + data[5]*o.data[8],
01226
01227 data[6]*o.data[0] + data[7]*o.data[3] + data[8]*o.data[6],
01228 data[6]*o.data[1] + data[7]*o.data[4] + data[8]*o.data[7],
01229 data[6]*o.data[2] + data[7]*o.data[5] + data[8]*o.data[8]
01230 );
01231 }
01233
01237 void operator*=(const Matrix3 &o)
01238 {
01239 real t1;
01240 real t2;
01241 real t3;
01242
01243 t1 = data[0]*o.data[0] + data[1]*o.data[3] + data[2]*o.data[6];
01244 t2 = data[0]*o.data[1] + data[1]*o.data[4] + data[2]*o.data[7];
01245 t3 = data[0]*o.data[2] + data[1]*o.data[5] + data[2]*o.data[8];
01246 data[0] = t1;
01247 data[1] = t2;
01248 data[2] = t3;
01249
01250 t1 = data[3]*o.data[0] + data[4]*o.data[3] + data[5]*o.data[6];
01251 t2 = data[3]*o.data[1] + data[4]*o.data[4] + data[5]*o.data[7];
01252 t3 = data[3]*o.data[2] + data[4]*o.data[5] + data[5]*o.data[8];
01253 data[3] = t1;
01254 data[4] = t2;
01255 data[5] = t3;
01256
01257 t1 = data[6]*o.data[0] + data[7]*o.data[3] + data[8]*o.data[6];
01258 t2 = data[6]*o.data[1] + data[7]*o.data[4] + data[8]*o.data[7];
01259 t3 = data[6]*o.data[2] + data[7]*o.data[5] + data[8]*o.data[8];
01260 data[6] = t1;
01261 data[7] = t2;
01262 data[8] = t3;
01263 }
01264
01268 void operator*=(const real scalar)
01269 {
01270 data[0] *= scalar; data[1] *= scalar; data[2] *= scalar;
01271 data[3] *= scalar; data[4] *= scalar; data[5] *= scalar;
01272 data[6] *= scalar; data[7] *= scalar; data[8] *= scalar;
01273 }
01274
01279 void operator+=(const Matrix3 &o)
01280 {
01281 data[0] += o.data[0]; data[1] += o.data[1]; data[2] += o.data[2];
01282 data[3] += o.data[3]; data[4] += o.data[4]; data[5] += o.data[5];
01283 data[6] += o.data[6]; data[7] += o.data[7]; data[8] += o.data[8];
01284 }
01285
01287
01291 void setOrientation(const Quaternion &q)
01292 {
01293 data[0] = 1 - (2*q.j*q.j + 2*q.k*q.k);
01294 data[1] = 2*q.i*q.j + 2*q.k*q.r;
01295 data[2] = 2*q.i*q.k - 2*q.j*q.r;
01296 data[3] = 2*q.i*q.j - 2*q.k*q.r;
01297 data[4] = 1 - (2*q.i*q.i + 2*q.k*q.k);
01298 data[5] = 2*q.j*q.k + 2*q.i*q.r;
01299 data[6] = 2*q.i*q.k + 2*q.j*q.r;
01300 data[7] = 2*q.j*q.k - 2*q.i*q.r;
01301 data[8] = 1 - (2*q.i*q.i + 2*q.j*q.j);
01302 }
01303
01307 static Matrix3 linearInterpolate(const Matrix3& a, const Matrix3& b, real prop);
01308 };
01309
01310 }
01311
01312 #endif // CYCLONE_CORE_H