├── README.md ├── appa.txt ├── appb.txt ├── appc.txt ├── ch01.txt ├── ch02.txt ├── ch04.txt ├── ch06.txt ├── ch07.txt ├── ch08.txt ├── ch09.txt ├── ch10.txt ├── ch11.txt ├── ch12.txt ├── ch13.txt ├── ch15.txt ├── ch18.txt ├── ch19.txt ├── ch20.txt ├── ch21.txt ├── ch22.txt ├── ch23.txt ├── ch25.txt └── ch26.txt /README.md: -------------------------------------------------------------------------------- 1 | physics_for_game_developers_2e 2 | ============================== 3 | 4 | This is the example code than accompanies Physics for Game Developers, 2E by David M. Bourg and Bryan Bywalec (9781449392512). 5 | 6 | Click the Download Zip button to the right to download example code. 7 | 8 | Visit the catalog page [here](http://shop.oreilly.com/product/0636920012221.do). 9 | 10 | See an error? Report it [here](http://oreilly.com/catalog/errata.csp?isbn=0636920012221), or simply fork and send us a pull request. 11 | -------------------------------------------------------------------------------- /appa.txt: -------------------------------------------------------------------------------- 1 | appendix: Vector Operations 2 | ================== 3 | //------------------------------------------------------------------------ 4 | // Vector Class and vector functions 5 | //------------------------------------------------------------------------ 6 | class Vector { 7 | public: 8 | float x; 9 | float y; 10 | float z; 11 | 12 | Vector(void); 13 | Vector(float xi, float yi, float zi); 14 | 15 | float Magnitude(void); 16 | void Normalize(void); 17 | void Reverse(void); 18 | 19 | Vector& operator+=(Vector u); 20 | Vector& operator-=(Vector u); 21 | Vector& operator*=(float s); 22 | Vector& operator/=(float s); 23 | 24 | Vector operator-(void); 25 | 26 | }; 27 | 28 | // Constructor 29 | inline Vector::Vector(void) 30 | { 31 | x = 0; 32 | y = 0; 33 | z = 0; 34 | } 35 | 36 | // Constructor 37 | inline Vector::Vector(float xi, float yi, float zi) 38 | { 39 | x = xi; 40 | y = yi; 41 | z = zi; 42 | } 43 | 44 | 45 | ==================================== 46 | inline float Vector::Magnitude(void) 47 | { 48 | return (float) sqrt(x*x + y*y + z*z); 49 | } 50 | 51 | 52 | ==================================== 53 | inline void Vector::Normalize(void) 54 | { 55 | float m = (float) sqrt(x*x + y*y + z*z); 56 | if(m <= tol) m = 1; 57 | x /= m; 58 | y /= m; 59 | z /= m; 60 | 61 | if (fabs(x) < tol) x = 0.0f; 62 | if (fabs(y) < tol) y = 0.0f; 63 | if (fabs(z) < tol) z = 0.0f; 64 | } 65 | 66 | 67 | ==================================== 68 | float const tol = 0.0001f; 69 | 70 | 71 | ==================================== 72 | inline void Vector::Reverse(void) 73 | { 74 | x = -x; 75 | y = -y; 76 | z = -z; 77 | } 78 | 79 | 80 | ==================================== 81 | inline Vector& Vector::operator+=(Vector u) 82 | { 83 | x += u.x; 84 | y += u.y; 85 | z += u.z; 86 | return *this; 87 | } 88 | 89 | 90 | ==================================== 91 | inline Vector& Vector::operator-=(Vector u) 92 | { 93 | x -= u.x; 94 | y -= u.y; 95 | z -= u.z; 96 | return *this; 97 | } 98 | 99 | 100 | ==================================== 101 | inline Vector& Vector::operator*=(float s) 102 | { 103 | x *= s; 104 | y *= s; 105 | z *= s; 106 | return *this; 107 | } 108 | 109 | 110 | ==================================== 111 | inline Vector& Vector::operator/=(float s) 112 | { 113 | x /= s; 114 | y /= s; 115 | z /= s; 116 | return *this; 117 | } 118 | 119 | 120 | ==================================== 121 | inline Vector Vector::operator-(void) 122 | { 123 | return Vector(-x, -y, -z); 124 | } 125 | 126 | 127 | ==================================== 128 | inline Vector operator+(Vector u, Vector v) 129 | { 130 | return Vector(u.x + v.x, u.y + v.y, u.z + v.z); 131 | } 132 | 133 | 134 | ==================================== 135 | inline Vector operator-(Vector u, Vector v) 136 | { 137 | return Vector(u.x - v.x, u.y - v.y, u.z - v.z); 138 | } 139 | 140 | 141 | ==================================== 142 | inline Vector operator^(Vector u, Vector v) 143 | { 144 | return Vector( u.y*v.z - u.z*v.y, 145 | -u.x*v.z + u.z*v.x, 146 | u.x*v.y - u.y*v.x ); 147 | } 148 | 149 | 150 | ==================================== 151 | // Vector dot product 152 | inline float operator*(Vector u, Vector v) 153 | { 154 | return (u.x*v.x + u.y*v.y + u.z*v.z); 155 | } 156 | 157 | 158 | ==================================== 159 | inline Vector operator*(float s, Vector u) 160 | { 161 | return Vector(u.x*s, u.y*s, u.z*s); 162 | } 163 | 164 | 165 | inline Vector operator*(Vector u, float s) 166 | { 167 | return Vector(u.x*s, u.y*s, u.z*s); 168 | } 169 | 170 | 171 | ==================================== 172 | inline Vector operator/(Vector u, float s) 173 | { 174 | return Vector(u.x/s, u.y/s, u.z/s); 175 | } 176 | 177 | 178 | ==================================== 179 | inline float TripleScalarProduct(Vector u, Vector v, Vector w) 180 | { 181 | return float( (u.x * (v.y*w.z - v.z*w.y)) + 182 | (u.y * (-v.x*w.z + v.z*w.x)) + 183 | (u.z * (v.x*w.y - v.y*w.x)) ); 184 | } 185 | 186 | 187 | ================== -------------------------------------------------------------------------------- /appb.txt: -------------------------------------------------------------------------------- 1 | appendix: Matrix Operations 2 | ================== 3 | class Matrix3x3 { 4 | public: 5 | // elements eij: i -> row, j -> column 6 | float e11, e12, e13, e21, e22, e23, e31, e32, e33; 7 | 8 | Matrix3x3(void); 9 | Matrix3x3(float r1c1, float r1c2, float r1c3, 10 | float r2c1, float r2c2, float r2c3, 11 | float r3c1, float r3c2, float r3c3 ); 12 | 13 | float det(void); 14 | Matrix3x3 Transpose(void); 15 | Matrix3x3 Inverse(void); 16 | 17 | Matrix3x3& operator+=(Matrix3x3 m); 18 | Matrix3x3& operator-=(Matrix3x3 m); 19 | Matrix3x3& operator*=(float s); 20 | Matrix3x3& operator/=(float s); 21 | }; 22 | 23 | // Constructor 24 | inline Matrix3x3::Matrix3x3(void) 25 | { 26 | e11 = 0; 27 | e12 = 0; 28 | e13 = 0; 29 | e21 = 0; 30 | e22 = 0; 31 | e23 = 0; 32 | e31 = 0; 33 | e32 = 0; 34 | e33 = 0; 35 | } 36 | 37 | // Constructor 38 | inline Matrix3x3::Matrix3x3(float r1c1, float r1c2, float r1c3, 39 | float r2c1, float r2c2, float r2c3, 40 | float r3c1, float r3c2, float r3c3 ) 41 | { 42 | e11 = r1c1; 43 | e12 = r1c2; 44 | e13 = r1c3; 45 | e21 = r2c1; 46 | e22 = r2c2; 47 | e23 = r2c3; 48 | e31 = r3c1; 49 | e32 = r3c2; 50 | e33 = r3c3; 51 | } 52 | 53 | 54 | ==================================== 55 | inline float Matrix3x3::det(void) 56 | { 57 | return e11*e22*e33 - 58 | e11*e32*e23 + 59 | e21*e32*e13 - 60 | e21*e12*e33 + 61 | e31*e12*e23 - 62 | e31*e22*e13; 63 | } 64 | 65 | 66 | ==================================== 67 | inline Matrix3x3 Matrix3x3::Transpose(void) 68 | { 69 | return Matrix3x3(e11,e21,e31,e12,e22,e32,e13,e23,e33); 70 | } 71 | 72 | 73 | ==================================== 74 | inline Matrix3x3 Matrix3x3::Inverse(void) 75 | { 76 | float d = e11*e22*e33 - 77 | e11*e32*e23 + 78 | e21*e32*e13 - 79 | e21*e12*e33 + 80 | e31*e12*e23 - 81 | e31*e22*e13; 82 | 83 | if (d == 0) d = 1; 84 | 85 | return Matrix3x3( (e22*e33-e23*e32)/d, 86 | -(e12*e33-e13*e32)/d, 87 | (e12*e23-e13*e22)/d, 88 | -(e21*e33-e23*e31)/d, 89 | (e11*e33-e13*e31)/d, 90 | -(e11*e23-e13*e21)/d, 91 | (e21*e32-e22*e31)/d, 92 | -(e11*e32-e12*e31)/d, 93 | (e11*e22-e12*e21)/d ); 94 | } 95 | 96 | 97 | ==================================== 98 | inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m) 99 | { 100 | e11 += m.e11; 101 | e12 += m.e12; 102 | e13 += m.e13; 103 | e21 += m.e21; 104 | e22 += m.e22; 105 | e23 += m.e23; 106 | e31 += m.e31; 107 | e32 += m.e32; 108 | e33 += m.e33; 109 | return *this; 110 | } 111 | 112 | 113 | ==================================== 114 | inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m) 115 | { 116 | e11 -= m.e11; 117 | e12 -= m.e12; 118 | e13 -= m.e13; 119 | e21 -= m.e21; 120 | e22 -= m.e22; 121 | e23 -= m.e23; 122 | e31 -= m.e31; 123 | e32 -= m.e32; 124 | e33 -= m.e33; 125 | return *this; 126 | } 127 | 128 | 129 | ==================================== 130 | inline Matrix3x3& Matrix3x3::operator*=(float s) 131 | { 132 | e11 *= s; 133 | e12 *= s; 134 | e13 *= s; 135 | e21 *= s; 136 | e22 *= s; 137 | e23 *= s; 138 | e31 *= s; 139 | e32 *= s; 140 | e33 *= s; 141 | return *this; 142 | } 143 | 144 | 145 | ==================================== 146 | inline Matrix3x3& Matrix3x3::operator/=(float s) 147 | { 148 | e11 /= s; 149 | e12 /= s; 150 | e13 /= s; 151 | e21 /= s; 152 | e22 /= s; 153 | e23 /= s; 154 | e31 /= s; 155 | e32 /= s; 156 | e33 /= s; 157 | return *this; 158 | } 159 | 160 | 161 | ==================================== 162 | inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2) 163 | { 164 | return Matrix3x3( m1.e11+m2.e11, 165 | m1.e12+m2.e12, 166 | m1.e13+m2.e13, 167 | m1.e21+m2.e21, 168 | m1.e22+m2.e22, 169 | m1.e23+m2.e23, 170 | m1.e31+m2.e31, 171 | m1.e32+m2.e32, 172 | m1.e33+m2.e33); 173 | } 174 | 175 | 176 | ==================================== 177 | inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2) 178 | { 179 | return Matrix3x3( m1.e11-m2.e11, 180 | m1.e12-m2.e12, 181 | m1.e13-m2.e13, 182 | m1.e21-m2.e21, 183 | m1.e22-m2.e22, 184 | m1.e23-m2.e23, 185 | m1.e31-m2.e31, 186 | m1.e32-m2.e32, 187 | m1.e33-m2.e33); 188 | } 189 | 190 | 191 | ==================================== 192 | inline Matrix3x3 operator/(Matrix3x3 m, float s) 193 | { 194 | return Matrix3x3( m.e11/s, 195 | m.e12/s, 196 | m.e13/s, 197 | m.e21/s, 198 | m.e22/s, 199 | m.e23/s, 200 | m.e31/s, 201 | m.e32/s, 202 | m.e33/s); 203 | } 204 | 205 | 206 | ==================================== 207 | inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2) 208 | { 209 | return Matrix3x3(m1.e11*m2.e11 + m1.e12*m2.e21 + m1.e13*m2.e31, 210 | m1.e11*m2.e12 + m1.e12*m2.e22 + m1.e13*m2.e32, 211 | m1.e11*m2.e13 + m1.e12*m2.e23 + m1.e13*m2.e33, 212 | m1.e21*m2.e11 + m1.e22*m2.e21 + m1.e23*m2.e31, 213 | m1.e21*m2.e12 + m1.e22*m2.e22 + m1.e23*m2.e32, 214 | m1.e21*m2.e13 + m1.e22*m2.e23 + m1.e23*m2.e33, 215 | m1.e31*m2.e11 + m1.e32*m2.e21 + m1.e33*m2.e31, 216 | m1.e31*m2.e12 + m1.e32*m2.e22 + m1.e33*m2.e32, 217 | m1.e31*m2.e13 + m1.e32*m2.e23 + m1.e33*m2.e33 ); 218 | } 219 | 220 | 221 | ==================================== 222 | inline Matrix3x3 operator*(Matrix3x3 m, float s) 223 | { 224 | return Matrix3x3( m.e11*s, 225 | m.e12*s, 226 | m.e13*s, 227 | m.e21*s, 228 | m.e22*s, 229 | m.e23*s, 230 | m.e31*s, 231 | m.e32*s, 232 | m.e33*s); 233 | } 234 | 235 | inline Matrix3x3 operator*(float s, Matrix3x3 m) 236 | { 237 | return Matrix3x3( m.e11*s, 238 | m.e12*s, 239 | m.e13*s, 240 | m.e21*s, 241 | m.e22*s, 242 | m.e23*s, 243 | m.e31*s, 244 | m.e32*s, 245 | m.e33*s); 246 | } 247 | 248 | 249 | ==================================== 250 | inline Vector operator*(Matrix3x3 m, Vector u) 251 | { 252 | return Vector( m.e11*u.x + m.e12*u.y + m.e13*u.z, 253 | m.e21*u.x + m.e22*u.y + m.e23*u.z, 254 | m.e31*u.x + m.e32*u.y + m.e33*u.z); 255 | } 256 | 257 | inline Vector operator*(Vector u, Matrix3x3 m) 258 | { 259 | return Vector( u.x*m.e11 + u.y*m.e21 + u.z*m.e31, 260 | u.x*m.e12 + u.y*m.e22 + u.z*m.e32, 261 | u.x*m.e13 + u.y*m.e23 + u.z*m.e33); 262 | } 263 | 264 | 265 | ================== -------------------------------------------------------------------------------- /appc.txt: -------------------------------------------------------------------------------- 1 | appendix: Quaternion Operations 2 | ================== 3 | class Quaternion { 4 | public: 5 | float n; // number (scalar) part 6 | Vector v; // vector part: v.x, v.y, v.z 7 | 8 | Quaternion(void); 9 | Quaternion(float e0, float e1, float e2, float e3); 10 | 11 | float Magnitude(void); 12 | Vector GetVector(void); 13 | float GetScalar(void); 14 | Quaternion operator+=(Quaternion q); 15 | Quaternion operator-=(Quaternion q); 16 | Quaternion operator*=(float s); 17 | Quaternion operator/=(float s); 18 | Quaternion operator~(void) const { return Quaternion( n, 19 | -v.x, 20 | -v.y, 21 | -v.z);} 22 | }; 23 | 24 | // Constructor 25 | inline Quaternion::Quaternion(void) 26 | { 27 | n = 0; 28 | v.x = 0; 29 | v.y = 0; 30 | v.z = 0; 31 | } 32 | 33 | // Constructor 34 | inline Quaternion::Quaternion(float e0, float e1, float e2, float e3) 35 | { 36 | n = e0; 37 | v.x = e1; 38 | v.y = e2; 39 | v.z = e3; 40 | } 41 | 42 | 43 | ==================================== 44 | inline float Quaternion::Magnitude(void) 45 | { 46 | return (float) sqrt(n*n + v.x*v.x + v.y*v.y + v.z*v.z); 47 | } 48 | 49 | 50 | ==================================== 51 | inline Vector Quaternion::GetVector(void) 52 | { 53 | return Vector(v.x, v.y, v.z); 54 | } 55 | 56 | 57 | ==================================== 58 | inline float Quaternion::GetScalar(void) 59 | { 60 | return n; 61 | } 62 | 63 | 64 | ==================================== 65 | inline Quaternion Quaternion::operator+=(Quaternion q) 66 | { 67 | n += q.n; 68 | v.x += q.v.x; 69 | v.y += q.v.y; 70 | v.z += q.v.z; 71 | return *this; 72 | } 73 | 74 | 75 | ==================================== 76 | inline Quaternion Quaternion::operator-=(Quaternion q) 77 | { 78 | n -= q.n; 79 | v.x -= q.v.x; 80 | v.y -= q.v.y; 81 | v.z -= q.v.z; 82 | return *this; 83 | } 84 | 85 | 86 | ==================================== 87 | inline Quaternion Quaternion::operator*=(float s) 88 | { 89 | n *= s; 90 | v.x *= s; 91 | v.y *= s; 92 | v.z *= s; 93 | return *this; 94 | } 95 | 96 | 97 | ==================================== 98 | inline Quaternion Quaternion::operator/=(float s) 99 | { 100 | n /= s; 101 | v.x /= s; 102 | v.y /= s; 103 | v.z /= s; 104 | return *this; 105 | } 106 | 107 | 108 | ==================================== 109 | Quaternion operator~(void) const { return Quaternion( n, 110 | -v.x, 111 | -v.y, 112 | -v.z);} 113 | 114 | 115 | ==================================== 116 | inline Quaternion operator+(Quaternion q1, Quaternion q2) 117 | { 118 | return Quaternion( q1.n + q2.n, 119 | q1.v.x + q2.v.x, 120 | q1.v.y + q2.v.y, 121 | q1.v.z + q2.v.z); 122 | } 123 | 124 | 125 | ==================================== 126 | inline Quaternion operator-(Quaternion q1, Quaternion q2) 127 | { 128 | return Quaternion( q1.n - q2.n, 129 | q1.v.x - q2.v.x, 130 | q1.v.y - q2.v.y, 131 | q1.v.z - q2.v.z); 132 | } 133 | 134 | 135 | ==================================== 136 | inline Quaternion operator*(Quaternion q1, Quaternion q2) 137 | { 138 | return Quaternion(q1.n*q2.n - q1.v.x*q2.v.x 139 | - q1.v.y*q2.v.y - q1.v.z*q2.v.z, 140 | q1.n*q2.v.x + q1.v.x*q2.n 141 | + q1.v.y*q2.v.z - q1.v.z*q2.v.y, 142 | q1.n*q2.v.y + q1.v.y*q2.n 143 | + q1.v.z*q2.v.x - q1.v.x*q2.v.z, 144 | q1.n*q2.v.z + q1.v.z*q2.n 145 | + q1.v.x*q2.v.y - q1.v.y*q2.v.x); 146 | } 147 | 148 | 149 | ==================================== 150 | inline Quaternion operator*(Quaternion q, float s) 151 | { 152 | return Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s); 153 | } 154 | 155 | inline Quaternion operator*(float s, Quaternion q) 156 | { 157 | return Quaternion(q.n*s, q.v.x*s, q.v.y*s, q.v.z*s); 158 | } 159 | 160 | 161 | ==================================== 162 | inline Quaternion operator*(Quaternion q, Vector v) 163 | { 164 | return Quaternion( -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z), 165 | q.n*v.x + q.v.y*v.z - q.v.z*v.y, 166 | q.n*v.y + q.v.z*v.x - q.v.x*v.z, 167 | q.n*v.z + q.v.x*v.y - q.v.y*v.x); 168 | } 169 | inline Quaternion operator*(Vector v, Quaternion q) 170 | { 171 | return Quaternion( -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z), 172 | q.n*v.x + q.v.z*v.y - q.v.y*v.z, 173 | q.n*v.y + q.v.x*v.z - q.v.z*v.x, 174 | q.n*v.z + q.v.y*v.x - q.v.x*v.y); 175 | } 176 | 177 | 178 | ==================================== 179 | inline Quaternion operator/(Quaternion q, float s) 180 | { 181 | return Quaternion(q.n/s, q.v.x/s, q.v.y/s, q.v.z/s); 182 | } 183 | 184 | 185 | ==================================== 186 | inline float QGetAngle(Quaternion q) 187 | { 188 | return (float) (2*acos(q.n)); 189 | } 190 | 191 | 192 | ==================================== 193 | inline Vector QGetAxis(Quaternion q) 194 | { 195 | Vector v; 196 | float m; 197 | 198 | v = q.GetVector(); 199 | m = v.Magnitude(); 200 | 201 | if (m <= tol) 202 | return Vector(); 203 | else 204 | return v/m; 205 | } 206 | 207 | 208 | ==================================== 209 | inline Quaternion QRotate(Quaternion q1, Quaternion q2) 210 | { 211 | return q1*q2*(~q1); 212 | } 213 | 214 | 215 | ==================================== 216 | inline Vector QVRotate(Quaternion q, Vector v) 217 | { 218 | Quaternion t; 219 | 220 | 221 | t = q*v*(~q); 222 | 223 | return t.GetVector(); 224 | } 225 | 226 | 227 | ==================================== 228 | inline Quaternion MakeQFromEulerAngles(float x, float y, float z) 229 | { 230 | Quaternion q; 231 | double roll = DegreesToRadians(x); 232 | double pitch = DegreesToRadians(y); 233 | double yaw = DegreesToRadians(z); 234 | 235 | double cyaw, cpitch, croll, syaw, spitch, sroll; 236 | double cyawcpitch, syawspitch, cyawspitch, syawcpitch; 237 | 238 | cyaw = cos(0.5f * yaw); 239 | cpitch = cos(0.5f * pitch); 240 | croll = cos(0.5f * roll); 241 | syaw = sin(0.5f * yaw); 242 | spitch = sin(0.5f * pitch); 243 | sroll = sin(0.5f * roll); 244 | 245 | cyawcpitch = cyaw*cpitch; 246 | syawspitch = syaw*spitch; 247 | cyawspitch = cyaw*spitch; 248 | syawcpitch = syaw*cpitch; 249 | 250 | q.n = (float) (cyawcpitch * croll + syawspitch * sroll); 251 | q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll); 252 | q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll); 253 | q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll); 254 | 255 | return q; 256 | } 257 | 258 | 259 | ==================================== 260 | inline Vector MakeEulerAnglesFromQ(Quaternion q) 261 | { 262 | double r11, r21, r31, r32, r33, r12, r13; 263 | double q00, q11, q22, q33; 264 | double tmp; 265 | Vector u; 266 | 267 | q00 = q.n * q.n; 268 | q11 = q.v.x * q.v.x; 269 | q22 = q.v.y * q.v.y; 270 | q33 = q.v.z * q.v.z; 271 | 272 | r11 = q00 + q11 - q22 - q33; 273 | r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z); 274 | r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y); 275 | r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x); 276 | r33 = q00 - q11 - q22 + q33; 277 | 278 | tmp = fabs(r31); 279 | if(tmp > 0.999999) 280 | { 281 | r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z); 282 | r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y); 283 | 284 | u.x = RadiansToDegrees(0.0f); //roll 285 | u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch 286 | u.z = RadiansToDegrees((float) atan2(-r12, -r31*r13)); // yaw 287 | return u; 288 | } 289 | 290 | u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll 291 | u.y = RadiansToDegrees((float) asin(-r31)); // pitch 292 | u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw 293 | return u; 294 | 295 | 296 | } 297 | 298 | 299 | ==================================== 300 | inline float DegreesToRadians(float deg) 301 | { 302 | return deg * pi / 180.0f; 303 | } 304 | 305 | inline float RadiansToDegrees(float rad) 306 | { 307 | return rad * 180.0f / pi; 308 | } 309 | 310 | 311 | ================== -------------------------------------------------------------------------------- /ch01.txt: -------------------------------------------------------------------------------- 1 | chapter: Basic Concepts 2 | ================== 3 | typedef struct _PointMass 4 | { 5 | float mass; 6 | Vector designPosition; 7 | Vector correctedPosition; 8 | } PointMass; 9 | 10 | // Assume that _NUMELEMENTS has been defined 11 | PointMassElements[_NUMELEMENTS]; 12 | 13 | 14 | ==================================== 15 | int i; 16 | float TotalMass; 17 | Vector CombinedCG; 18 | Vector FirstMoment; 19 | 20 | TotalMass = 0; 21 | for(i=0; i<_NUMELEMENTS; i++) 22 | TotalMass FAC+= Elements[i].mass; 23 | 24 | FirstMoment = Vector(0, 0, 0); 25 | for(i=0; i<_NUMELEMENTS; i++) 26 | { 27 | FirstMoment += Element[i].mass * Element[i].designPosition; 28 | } 29 | CombinedCG = FirstMoment / TotalMass; 30 | 31 | 32 | ==================================== 33 | for(i=0; i<_NUMELEMENTS; i++) 34 | { 35 | Element[i].correctedPosition = Element[i].designPosition - 36 | CombinedCG; 37 | } 38 | 39 | 40 | ==================================== 41 | typedef struct _PointMass 42 | { 43 | float mass; 44 | Vector designPosition; 45 | Vector correctedPosition; 46 | Vector localInertia; 47 | } PointMass; 48 | 49 | 50 | ==================================== 51 | float Ixx, Iyy, Izz, Ixy, Ixz, Iyz; 52 | Matrix3x3 InertiaTensor; 53 | 54 | Ixx = 0; Iyy = 0; Izz = 0; 55 | Ixy = 0; Ixz = 0; Iyz = 0; 56 | 57 | for (i = 0; i<_NUMELEMENTS; i++) 58 | { 59 | Ixx += Element[i].LocalInertia.x + 60 | Element[i].mass * (Element[i].correctedPosition.y * 61 | Element[i].correctedPosition.y + 62 | Element[i].correctedPosition.z * 63 | Element[i].correctedPosition.z); 64 | 65 | Iyy += Element[i].LocalInertia.y + 66 | Element[i].mass * (Element[i].correctedPosition.z * 67 | Element[i].correctedPosition.z + 68 | Element[i].correctedPosition.x * 69 | Element[i].correctedPosition.x); 70 | 71 | Izz += Element[i].LocalInertia.z + 72 | Element[i].mass * (Element[i].correctedPosition.x * 73 | Element[i].correctedPosition.x + 74 | Element[i].correctedPosition.y * 75 | Element[i].correctedPosition.y); 76 | 77 | Ixy += Element[i].mass * (Element[i].correctedPosition.x * 78 | Element[i].correctedPosition.y); 79 | 80 | Ixz += Element[i].mass * (Element[i].correctedPosition.x * 81 | Element[i].correctedPosition.z); 82 | 83 | Iyz += Element[i].mass * (Element[i].correctedPosition.y * 84 | Element[i].correctedPosition.z); 85 | } 86 | 87 | // e11 stands for element on row 1 column 1, e12 for row 1 column 2, etc. 88 | InertiaTensor.e11 = Ixx; 89 | InertiaTensor.e12 = -Ixy; 90 | InertiaTensor.e13 = -Ixz; 91 | 92 | InertiaTensor.e21 = -Ixy; 93 | InertiaTensor.e22 = Iyy; 94 | InertiaTensor.e23 = -Iyz; 95 | 96 | InertiaTensor.e31 = -Ixz; 97 | InertiaTensor.e32 = -Iyz; 98 | InertiaTensor.e33 = Izz; 99 | 100 | 101 | ================== -------------------------------------------------------------------------------- /ch02.txt: -------------------------------------------------------------------------------- 1 | chapter: Kinematics 2 | ================== 3 | FIRE BUTTON PRESSED EVENT: 4 | 5 | Fetch and store user input values for global variables, 6 | Vm, Alpha, Gamma, L, Yb, X, Y, Z, Length, Width, Height... 7 | 8 | Initialize the time and status variables... 9 | status = 0; 10 | time = 0; 11 | 12 | Start stepping through time for the simulation 13 | until the target is hit, the shell hits 14 | the ground, or the simulation times out... 15 | 16 | while(status == 0) 17 | { 18 | // do the next time step 19 | status = DoSimulation(); 20 | 21 | Update the display... 22 | 23 | } 24 | 25 | // Report results 26 | if (status == 1) 27 | Display DIRECT HIT message to the user... 28 | 29 | if (status == 2) 30 | Display MISSED TARGET message to the user... 31 | 32 | if (status == 3) 33 | Display SIMULATION TIMED OUT message to the user... 34 | 35 | 36 | ==================================== 37 | //---------------------------------------------------------------------------// 38 | // Define a custom type to represent 39 | // the three components of a 3D vector, where 40 | // i represents the x component, j represents 41 | // the y component, and k represents the z 42 | // component 43 | //---------------------------------------------------------------------------// 44 | typedef struct TVectorTag 45 | { 46 | double i; 47 | double j; 48 | double k; 49 | } TVector; 50 | 51 | //---------------------------------------------------------------------------// 52 | // Now define the variables required for this simulation 53 | //---------------------------------------------------------------------------// 54 | double Vm; // Magnitude of muzzle velocity, m/s 55 | double Alpha; // Angle from y-axis (upward) to the cannon. 56 | // When this angle is 0, the cannon is pointing 57 | // straight up, when it is 90 degrees, the cannon 58 | // is horizontal 59 | double Gamma; // Angle from x-axis, in the x-z plane to the cannon. 60 | // When this angle is 0, the cannon is pointing in 61 | // the positive x-direction, positive values of this angle 62 | // are toward the positive z-axis 63 | double L; // This is the length of the cannon, m 64 | double Yb; // This is the base elevation of the cannon, m 65 | 66 | double X; // The x-position of the center of the target, m 67 | double Y; // The y-position of the center of the target, m 68 | double Z; // The z-position of the center of the target, m 69 | double Length; // The length of the target measured along the x-axis, m 70 | double Width; // The width of the target measured along the z-axis, m 71 | double Height; // The height of the target measure along the y-axis, m 72 | 73 | TVector s; // The shell position (displacement) vector 74 | 75 | double time; // The time from the instant the shell leaves 76 | // the cannon, seconds 77 | double tInc; // The time increment to use when stepping through 78 | // the simulation, seconds 79 | 80 | double g; // acceleration due to gravity, m/s^2 81 | 82 | //-----------------------------------------------------------------------------// 83 | // This function steps the simulation ahead in time. This is where the kinematic 84 | // properties are calculated. The function will return 1 when the target is hit, 85 | // and 2 when the shell hits the ground (x-z plane) before hitting the target; 86 | // otherwise, the function returns 0. 87 | //-----------------------------------------------------------------------------// 88 | int DoSimulation(void) 89 | //-----------------------------------------------------------------------------// 90 | { 91 | double cosX; 92 | double cosY; 93 | double cosZ; 94 | double xe, ze; 95 | double b, Lx, Ly, Lz; 96 | double tx1, tx2, ty1, ty2, tz1, tz2; 97 | 98 | // step to the next time in the simulation 99 | time+=tInc; 100 | 101 | // First calculate the direction cosines for the cannon orientation. 102 | // In a real game, you would not want to put this calculation in this 103 | // function since it is a waste of CPU time to calculate these values 104 | // at each time step as they never change during the sim. We only put them 105 | // here in this case so you can see all the calculation steps in a single 106 | // function. 107 | b = L * cos((90-Alpha) *3.14/180); // projection of barrel onto x-z plane 108 | Lx = b * cos(Gamma * 3.14/180); // x-component of barrel length 109 | Ly = L * cos(Alpha * 3.14/180); // y-component of barrel length 110 | Lz = b * sin(Gamma * 3.14/180); // z-component of barrel length 111 | 112 | cosX = Lx/L; 113 | cosY = Ly/L; 114 | cosZ = Lz/L; 115 | 116 | // These are the x and z coordinates of the very end of the cannon barrel 117 | // we'll use these as the initial x and z displacements 118 | xe = L * cos((90-Alpha) *3.14/180) * cos(Gamma * 3.14/180); 119 | ze = L * cos((90-Alpha) *3.14/180) * sin(Gamma * 3.14/180); 120 | 121 | // Now we can calculate the position vector at this time 122 | s.i = Vm * cosX * time + xe; 123 | s.j = (Yb + L * cos(Alpha*3.14/180)) + (Vm * cosY * time) − 124 | (0.5 * g * time * time); 125 | s.k = Vm * cosZ * time + ze; 126 | 127 | // Check for collision with target 128 | // Get extents (bounding coordinates) of the target 129 | tx1 = X - Length/2; 130 | tx2 = X + Length/2; 131 | ty1 = Y - Height/2; 132 | ty2 = Y + Height/2; 133 | tz1 = Z - Width/2; 134 | tz2 = Z + Width/2; 135 | 136 | // Now check to see if the shell has passed through the target 137 | // We're using a rudimentary collision detection scheme here where 138 | // we simply check to see if the shell's coordinates are within the 139 | // bounding box of the target. This works for demo purposes, but 140 | // a practical problem is that you may miss a collision if for a given 141 | // time step the shell's change in position is large enough to allow 142 | // it to "skip" over the target. 143 | // A better approach is to look at the previous time step's position data 144 | // and to check the line from the previous position to the current position 145 | // to see if that line intersects the target bounding box. 146 | if( (s.i >= tx1 && s.i <= tx2) && 147 | (s.j >= ty1 && s.j <= ty2) && 148 | (s.k >= tz1 && s.k <= tz2) ) 149 | return 1; 150 | 151 | // Check for collision with ground (x-z plane) 152 | if(s.j <= 0) 153 | return 2; 154 | 155 | // Cut off the simulation if it's taking too long 156 | // This is so the program does not get stuck in the while loop 157 | if(time>3600) 158 | return 3; 159 | 160 | return 0; 161 | } 162 | 163 | 164 | ==================================== 165 | // Now we can calculate the position vector at this time 166 | s.i = Vm * cosX * time + xe; 167 | s.j = (Yb + L * cos(Alpha*3.14/180)) + (Vm * cosY * time) − 168 | (0.5 * g * time * time); 169 | s.k = Vm * cosZ * time + ze; 170 | 171 | 172 | ==================================== 173 | //---------------------------------------------------------------------------// 174 | // Define a custom type to represent each particle in the effect. 175 | //---------------------------------------------------------------------------// 176 | typedef struct _TParticle 177 | { 178 | float x; // x coordinate of the particle 179 | float y; // y coordinate of the particle 180 | float vi; // initial velocity 181 | float angle; // initial trajectory (direction) 182 | int life; // duration in milliseconds 183 | int r; // red component of particle's color 184 | int g; // green component of particle's color 185 | int b; // blue component of particle's color 186 | int time; // keeps track of the effect's time 187 | float gravity; // gravity factor 188 | BOOL Active; // indicates whether this particle 189 | // is active or dead 190 | } TParticle; 191 | 192 | #define _MAXPARTICLES 50 193 | 194 | typedef struct _TParticleExplosion 195 | { 196 | TParticle p[_MAXPARTICLES]; // list of particles 197 | // making up this effect 198 | int V0; // initial velocity, or strength, of the effect 199 | int x; // initial x location 200 | int y; // initial y location 201 | BOOL Active; // indicates whether this effect is 202 | //active or dead 203 | } TParticleExplosion; 204 | 205 | //---------------------------------------------------------------------------// 206 | // Now define the variables required for this simulation 207 | //---------------------------------------------------------------------------// 208 | TParticleExplosion Explosion; 209 | 210 | int xc; // x coordinate of the effect 211 | int yc; // y coordinate of the effect 212 | int V0; // initial velocity 213 | int Duration; // life in milliseconds 214 | float Gravity; // gravity factor (acceleration) 215 | float Angle; // indicates particles' direction 216 | 217 | 218 | ==================================== 219 | ///////////////////////////////////////////////////////////////////// 220 | /* This function creates a new particle explosion effect. 221 | 222 | x,y: starting point of the effect 223 | Vinit: a measure of how fast the particles will be sent flying 224 | (it's actually the initial velocity of the particles) 225 | life: life of the particles in milliseconds; particles will 226 | fade and die out as they approach 227 | their specified life 228 | gravity: the acceleration due to gravity, which controls the 229 | rate at which particles will fall 230 | as they fly 231 | angle: initial trajectory angle of the particles, 232 | specify 999 to create a particle explosion 233 | that emits particles in all directions; otherwise, 234 | 0 right, 90 up, 180 left, etc... 235 | */ 236 | void CreateParticleExplosion(int x, int y, int Vinit, int life, 237 | float gravity, float angle) 238 | { 239 | int i; 240 | int m; 241 | float f; 242 | 243 | Explosion.Active = TRUE; 244 | Explosion.x = x; 245 | Explosion.y = y; 246 | Explosion.V0 = Vinit; 247 | 248 | for(i=0; i<_MAXPARTICLES; i++) 249 | { 250 | Explosion.p[i].x = 0; 251 | Explosion.p[i].y = 0; 252 | Explosion.p[i].vi = tb_Rnd(Vinit/2, Vinit); 253 | 254 | if(angle < 999) 255 | { 256 | if(tb_Rnd(0,1) == 0) 257 | m = −1; 258 | else 259 | m = 1; 260 | Explosion.p[i].angle = -angle + m * tb_Rnd(0,10); 261 | } else 262 | Explosion.p[i].angle = tb_Rnd(0,360); 263 | 264 | f = (float) tb_Rnd(80, 100) / 100.0f; 265 | Explosion.p[i].life = tb_Round(life * f); 266 | Explosion.p[i].r = 255;//tb_Rnd(225, 255); 267 | Explosion.p[i].g = 255;//tb_Rnd(85, 115); 268 | Explosion.p[i].b = 255;//tb_Rnd(15, 45); 269 | Explosion.p[i].time = 0; 270 | Explosion.p[i].Active = TRUE; 271 | Explosion.p[i].gravity = gravity; 272 | } 273 | 274 | } 275 | 276 | 277 | ==================================== 278 | while(status) 279 | { 280 | Clear the off screen buffer... 281 | 282 | status = DrawParticleExplosion( ); 283 | 284 | Copy the off screen buffer to the screen... 285 | } 286 | 287 | 288 | ==================================== 289 | //---------------------------------------------------------------------------// 290 | // Draws the particle system and updates the state of each particle. 291 | // Returns false when all of the particles have died out. 292 | //---------------------------------------------------------------------------// 293 | 294 | BOOL DrawParticleExplosion(void) 295 | { 296 | int i; 297 | BOOL finished = TRUE; 298 | float r; 299 | 300 | if(Explosion.Active) 301 | for(i=0; i<_MAXPARTICLES; i++) 302 | { 303 | if(Explosion.p[i].Active) 304 | { 305 | finished = FALSE; 306 | 307 | // Calculate a color scale factor to fade the particle's color 308 | // as its life expires 309 | r = ((float)(Explosion.p[i].life- 310 | Explosion.p[i].time)/(float)(Explosion.p[i].life)); 311 | 312 | ... 313 | Draw the particle as a small circle... 314 | ... 315 | 316 | Explosion.p[i].Active = UpdateParticleState(&(Explosion.p[i]), 317 | 10); 318 | } 319 | } 320 | 321 | if(finished) 322 | Explosion.Active = FALSE; 323 | 324 | return !finished; 325 | } 326 | 327 | //---------------------------------------------------------------------------// 328 | /* This is generic function to update the state of a given particle. 329 | p: pointer to a particle structure 330 | dtime: time increment in milliseconds to 331 | advance the state of the particle 332 | 333 | If the total elapsed time for this particle has exceeded the particle's 334 | set life, then this function returns FALSE, indicating that the particle 335 | should expire. 336 | */ 337 | BOOL UpdateParticleState(TParticle* p, int dtime) 338 | { 339 | BOOL retval; 340 | float t; 341 | 342 | p->time+=dtime; 343 | t = (float)p->time/1000.0f; 344 | p->x = p->vi * cos(p->angle*PI/180.0f) * t; 345 | p->y = p->vi * sin(p->angle*PI/180.0f) * t + (p->gravity*t*t)/2.0f; 346 | 347 | if (p->time >= p->life) 348 | retval = FALSE; 349 | else 350 | retval = TRUE; 351 | 352 | return retval; 353 | } 354 | 355 | 356 | ================== -------------------------------------------------------------------------------- /ch04.txt: -------------------------------------------------------------------------------- 1 | chapter: Kinetics 2 | ================== 3 | //--------------------------------------------------------------------------------// 4 | int DoSimulation(void) 5 | //--------------------------------------------------------------------------------// 6 | { 7 | . 8 | . 9 | . 10 | 11 | // new local variables: 12 | double sx1, vx1; 13 | double sy1, vy1; 14 | double sz1, vz1; 15 | 16 | . 17 | . 18 | . 19 | 20 | // Now we can calculate the position vector at this time 21 | 22 | // Old position vector commented out: 23 | //s.i = Vm * cosX * time + xe; 24 | //s.j = (Yb + L * cos(Alpha*3.14/180)) + (Vm * cosY * time) − 25 | (0.5 * g * time * time); 26 | //s.k = Vm * cosZ * time + ze; 27 | 28 | // New position vector calculations: 29 | sx1 = xe; 30 | vx1 = Vm * cosX; 31 | 32 | sy1 = Yb + L * cos(Alpha * 3.14/180); 33 | vy1 = Vm * cosY; 34 | 35 | sz1 = ze; 36 | vz1 = Vm * cosZ; 37 | 38 | s.i =((m/Cd) * exp(-(Cd * time)/m) * ((-Cw * Vw * cos(GammaW * 3.14/180))/Cd − 39 | vx1) - (Cw * Vw * cos(GammaW * 3.14/180) * time) / Cd ) - 40 | ( (m/Cd) * ((-Cw * Vw * cos(GammaW * 3.14/180))/Cd - vx1) ) + sx1; 41 | 42 | s.j = sy1 + ( -(vy1 + (m * g)/Cd) * (m/Cd) * exp(-(Cd*time)/m) − 43 | (m * g * time) / Cd ) + ( (m/Cd) * (vy1 + (m * g)/Cd) ); 44 | 45 | s.k =((m/Cd) * exp(-(Cd * time)/m) * ((-Cw * Vw * sin(GammaW * 3.14/180))/Cd − 46 | vz1) - (Cw * Vw * sin(GammaW * 3.14/180) * time) / Cd ) - 47 | ( (m/Cd) * ((-Cw * Vw * sin(GammaW * 3.14/180))/Cd - vz1) ) + sz1; 48 | . 49 | . 50 | . 51 | } 52 | 53 | 54 | ==================================== 55 | //---------------------------------------------------------------------------------// 56 | LRESULT CALLBACK DemoDlgProc(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam) 57 | //---------------------------------------------------------------------------------// 58 | { 59 | . 60 | . 61 | . 62 | 63 | case WM_INITDIALOG: 64 | . 65 | . 66 | . 67 | // New variables: 68 | sprintf( str, "%f", m ); 69 | SetDlgItemText(hDlg, IDC_M, str); 70 | 71 | sprintf( str, "%f", Cd ); 72 | SetDlgItemText(hDlg, IDC_CD, str); 73 | 74 | sprintf( str, "%f", Vw ); 75 | SetDlgItemText(hDlg, IDC_VW, str); 76 | 77 | sprintf( str, "%f", GammaW ); 78 | SetDlgItemText(hDlg, IDC_GAMMAW, str); 79 | 80 | sprintf( str, "%f", Cw ); 81 | SetDlgItemText(hDlg, IDC_CW, str); 82 | . 83 | . 84 | . 85 | 86 | case IDC_REFRESH: 87 | . 88 | . 89 | . 90 | // New variables: 91 | GetDlgItemText(hDlg, IDC_M, str, 15); 92 | m = atof(str); 93 | 94 | GetDlgItemText(hDlg, IDC_CD, str, 15); 95 | Cd = atof(str); 96 | 97 | GetDlgItemText(hDlg, IDC_VW, str, 15); 98 | Vw = atof(str); 99 | 100 | GetDlgItemText(hDlg, IDC_GAMMAW, str, 15); 101 | GammaW = atof(str); 102 | 103 | GetDlgItemText(hDlg, IDC_CW, str, 15); 104 | Cw = atof(str); 105 | . 106 | . 107 | . 108 | 109 | case IDC_FIRE: 110 | . 111 | . 112 | . 113 | // New variables: 114 | GetDlgItemText(hDlg, IDC_M, str, 15); 115 | m = atof(str); 116 | 117 | GetDlgItemText(hDlg, IDC_CD, str, 15); 118 | Cd = atof(str); 119 | 120 | GetDlgItemText(hDlg, IDC_VW, str, 15); 121 | Vw = atof(str); 122 | 123 | GetDlgItemText(hDlg, IDC_GAMMAW, str, 15); 124 | GammaW = atof(str); 125 | 126 | GetDlgItemText(hDlg, IDC_CW, str, 15); 127 | Cw = atof(str); 128 | . 129 | . 130 | . 131 | } 132 | 133 | 134 | ================== -------------------------------------------------------------------------------- /ch06.txt: -------------------------------------------------------------------------------- 1 | chapter: Projectiles 2 | ================== 3 | //----------------------------------------------------------------------------// 4 | // Global variables required for this simulation 5 | //----------------------------------------------------------------------------// 6 | TVector V1; // Initial velocity (given), m/s 7 | TVector V2; // Velocity vector at time t, m/s 8 | double m; // Projectile mass (given), kg 9 | TVector s1; // Initial position (given), m 10 | TVector s2; // The projectile's position (displacement) vector, m 11 | double time; // The time from the instant the projectile 12 | // is launched, s 13 | double tInc; // The time increment to use when stepping 14 | // through the simulation, s 15 | double g; // acceleration due to gravity (given), m/s^2 16 | double spin; // spin in rpm (given) 17 | double omega; // spin in radians per second 18 | double radius; // radius of projectile (given), m 19 | 20 | #define PI 3.14159f 21 | #define RHO 1.225f // kg/m^3 22 | 23 | //----------------------------------------------------------------------------// 24 | int DoSimulation(void) 25 | //----------------------------------------------------------------------------// 26 | { 27 | double C = PI * PI * RHO * radius * radius * radius * omega; 28 | double t; 29 | 30 | // step to the next time in the simulation 31 | time+=tInc; 32 | t = time; 33 | 34 | // Calc. V2: 35 | V2.i = 1.0f/(1.0f-(t/m)*(t/m)*C*C) * (V1.i + C * V1.j * (t/m) − 36 | C * g * (t*t)/m); 37 | V2.j = V1.j + (t/m)*C*V2.i - g*t; 38 | 39 | // Calc. S2: 40 | s2.i = s1.i + V1.i * t + (1.0f/2.0f) * (C/m * V2.j) * (t*t); 41 | s2.j = s1.j + V1.j * t + (1.0f/2.0f) * ( ((C*V2.i) - m*g)/m ) * (t*t); 42 | 43 | 44 | // Check for collision with ground (x-z plane) 45 | if(s2.j <= 0) 46 | return 2; 47 | 48 | // Cut off the simulation if it's taking too long 49 | // This is so the program does not get stuck in the while loop 50 | if(time>60) 51 | return 3; 52 | 53 | return 0; 54 | } 55 | 56 | 57 | ================== -------------------------------------------------------------------------------- /ch07.txt: -------------------------------------------------------------------------------- 1 | chapter: Real-Time Simulations 2 | ================== 3 | // Global Variables 4 | float T; // thrust 5 | float C; // drag coefficient 6 | float V; // velocity 7 | float M; // mass 8 | float S; // displacement 9 | 10 | . 11 | . 12 | . 13 | 14 | // This function progresses the simulation by dt seconds using 15 | // Euler's basic method 16 | void StepSimulation(float dt) 17 | { 18 | float F; // total force 19 | float A; // acceleration 20 | float Vnew; // new velocity at time t + dt 21 | float Snew; // new position at time t + dt 22 | 23 | // Calculate the total force 24 | F = (T − (C * V)); 25 | 26 | // Calculate the acceleration 27 | A = F / M; 28 | 29 | // Calculate the new velocity at time t + dt 30 | // where V is the velocity at time t 31 | Vnew = V + A * dt; 32 | 33 | // Calculate the new displacement at time t + dt 34 | // where S is the displacement at time t 35 | Snew = S + Vnew * dt; 36 | 37 | // Update old velocity and displacement with the new ones 38 | V = Vnew; 39 | S = Snew; 40 | } 41 | 42 | 43 | ==================================== 44 | // New global variable 45 | float eto; // truncation error tolerance 46 | 47 | // This function progresses the simulation by dt seconds using 48 | // Euler's basic method with an adaptive step size 49 | void StepSimulation(float dt) 50 | { 51 | float F; // total force 52 | float A; // acceleration 53 | float Vnew; // new velocity at time t + dt 54 | float Snew; // new position at time t + dt 55 | float V1, V2; // temporary velocity variables 56 | float dtnew; // new time step 57 | float et; // truncation error 58 | 59 | // Take one step of size dt to estimate the new velocity 60 | F = (T − (C * V)); 61 | A = F / M; 62 | V1 = V + A * dt; 63 | 64 | // Take two steps of size dt/2 to estimate the new velocity 65 | F = (T − (C * V)); 66 | A = F / M; 67 | V2 = V + A * (dt/2); 68 | 69 | F = (T − (C * V2)); 70 | A = F / M; 71 | V2 = V2 + A * (dt/2); 72 | 73 | // Estimate the truncation error 74 | et = absf(V1 − V2); 75 | 76 | // Estimate a new step size 77 | dtnew = dt * SQRT(eto/et); 78 | 79 | 80 | if (dtnew < dt) 81 | { // take at step at the new smaller step size 82 | F = (T − (C * V)); 83 | A = F / M; 84 | Vnew = V + A * dtnew; 85 | Snew = S + Vnew * dtnew; 86 | } else 87 | { // original step size is okay 88 | Vnew = V1; 89 | Snew = S + Vnew * dt; 90 | } 91 | 92 | // Update old velocity and displacement with the new ones 93 | V = Vnew; 94 | S = Snew; 95 | } 96 | 97 | 98 | ==================================== 99 | // This function progresses the simulation by dt seconds using 100 | // the "improved" Euler method 101 | void StepSimulation(float dt) 102 | { 103 | float F; // total force 104 | float A; // acceleration 105 | float Vnew; // new velocity at time t + dt 106 | float Snew; // new position at time t + dt 107 | float k1, k2; 108 | 109 | 110 | F = (T - (C * V)); 111 | A = F/M; 112 | k1 = dt * A; 113 | 114 | F = (T - (C * (V + k1))); 115 | A = F/M; 116 | k2 = dt * A; 117 | 118 | // Calculate the new velocity at time t + dt 119 | // where V is the velocity at time t 120 | Vnew = V + (k1 + k2) / 2; 121 | 122 | // Calculate the new displacement at time t + dt 123 | // where S is the displacement at time t 124 | Snew = S + Vnew * dt; 125 | 126 | // Update old velocity and displacement with the new ones 127 | V = Vnew; 128 | S = Snew; 129 | } 130 | 131 | 132 | ==================================== 133 | // This function progresses the simulation by dt seconds using 134 | // the Runge-Kutta method 135 | void StepSimulation(float dt) 136 | { 137 | float F; // total force 138 | float A; // acceleration 139 | float Vnew; // new velocity at time t + dt 140 | float Snew; // new position at time t + dt 141 | float k1, k2, k3, k4; 142 | 143 | 144 | F = (T - (C * V)); 145 | A = F/M; 146 | k1 = dt * A; 147 | 148 | F = (T - (C * (V + k1/2))); 149 | A = F/M; 150 | k2 = dt * A; 151 | 152 | F = (T - (C * (V + k2/2))); 153 | A = F/M; 154 | k3 = dt * A; 155 | 156 | F = (T - (C * (V + k3))); 157 | A = F/M; 158 | k4 = dt * A; 159 | 160 | // Calculate the new velocity at time t + dt 161 | // where V is the velocity at time t 162 | Vnew = V + (k1 + 2*k2 + 2*k3 + k4) / 6; 163 | 164 | // Calculate the new displacement at time t + dt 165 | // where S is the displacement at time t 166 | Snew = S + Vnew * dt; 167 | 168 | // Update old velocity and displacement with the new ones 169 | V = Vnew; 170 | S = Snew; 171 | } 172 | 173 | 174 | ================== -------------------------------------------------------------------------------- /ch08.txt: -------------------------------------------------------------------------------- 1 | chapter: Particles 2 | ================== 3 | class Particle { 4 | public: 5 | float fMass; // Total mass 6 | Vector vPosition; // Position 7 | Vector vVelocity; // Velocity 8 | float fSpeed; // Speed (magnitude of the velocity) 9 | Vector vForces; // Total force acting on the particle 10 | float fRadius; // Particle radius used for collision detection 11 | Vector vGravity; // Gravity force vector 12 | 13 | Particle(void); // Constructor 14 | void CalcLoads(void); // Aggregates forces acting on the particle 15 | void UpdateBodyEuler(double dt); // Integrates one time step 16 | void Draw(void); // Draws the particle 17 | }; 18 | 19 | 20 | ==================================== 21 | Particle::Particle(void) 22 | { 23 | fMass = 1.0; 24 | vPosition.x = 0.0; 25 | vPosition.y = 0.0; 26 | vPosition.z = 0.0; 27 | vVelocity.x = 0.0; 28 | vVelocity.y = 0.0; 29 | vVelocity.z = 0.0; 30 | fSpeed = 0.0; 31 | vForces.x = 0.0; 32 | vForces.y = 0.0; 33 | vForces.z = 0.0; 34 | fRadius = 0.1; 35 | vGravity.x = 0; 36 | vGravity.y = fMass * _GRAVITYACCELERATION; 37 | } 38 | 39 | 40 | ==================================== 41 | #define _GRAVITYACCELERATION −9.8f 42 | 43 | 44 | ==================================== 45 | void Particle::CalcLoads(void) 46 | { 47 | // Reset forces: 48 | vForces.x = 0.0f; 49 | vForces.y = 0.0f; 50 | 51 | // Aggregate forces: 52 | vForces += vGravity; 53 | } 54 | 55 | 56 | ==================================== 57 | void Particle::UpdateBodyEuler(double dt) 58 | { 59 | Vector a; 60 | Vector dv; 61 | Vector ds; 62 | 63 | // Integrate equation of motion: 64 | a = vForces / fMass; 65 | 66 | dv = a * dt; 67 | vVelocity += dv; 68 | 69 | ds = vVelocity * dt; 70 | vPosition += ds; 71 | 72 | // Misc. calculations: 73 | fSpeed = vVelocity.Magnitude(); 74 | } 75 | 76 | 77 | ==================================== 78 | void Particle::Draw(void) 79 | { 80 | RECT r; 81 | float drawRadius = max(2, fRadius); 82 | 83 | SetRect(&r, vPosition.x − drawRadius, 84 | _WINHEIGHT − (vPosition.y − drawRadius), 85 | vPosition.x + drawRadius, 86 | _WINHEIGHT − (vPosition.y + drawRadius)); 87 | DrawEllipse(&r, 2, RGB(0,0,0)); 88 | } 89 | 90 | 91 | ==================================== 92 | // Global Variables: 93 | int FrameCounter = 0; 94 | Particle Units[_MAX_NUM_UNITS]; 95 | 96 | 97 | ==================================== 98 | bool Initialize(void) 99 | { 100 | int i; 101 | 102 | GetRandomNumber(0, _WINWIDTH, true); 103 | 104 | for(i=0; i<_MAX_NUM_UNITS; i++) 105 | { 106 | Units[i].vPosition.x = GetRandomNumber(_WINWIDTH/2-_SPAWN_AREA_R*2, 107 | _WINWIDTH/2+_SPAWN_AREA_R*2, false); 108 | Units[i].vPosition.y = _WINHEIGHT − 109 | GetRandomNumber(_WINHEIGHT/2-_SPAWN_AREA_R, 110 | _WINHEIGHT/2, false); 111 | } 112 | 113 | return true; 114 | } 115 | 116 | 117 | ==================================== 118 | void UpdateSimulation(void) 119 | { 120 | double dt = _TIMESTEP; 121 | int i; 122 | 123 | // initialize the back buffer 124 | if(FrameCounter >= _RENDER_FRAME_COUNT) 125 | { 126 | ClearBackBuffer(); 127 | } 128 | 129 | // update the particles (Units) 130 | for(i=0; i<_MAX_NUM_UNITS; i++) 131 | { 132 | Units[i].CalcLoads(); 133 | Units[i].UpdateBodyEuler(dt); 134 | 135 | if(FrameCounter >= _RENDER_FRAME_COUNT) 136 | { 137 | Units[i].Draw(); 138 | } 139 | 140 | if(Units[i].vPosition.x > _WINWIDTH) Units[i].vPosition.x = 0; 141 | if(Units[i].vPosition.x < 0) Units[i].vPosition.x = _WINWIDTH; 142 | if(Units[i].vPosition.y > _WINHEIGHT) Units[i].vPosition.y = 0; 143 | if(Units[i].vPosition.y < 0) Units[i].vPosition.y = _WINHEIGHT; 144 | } 145 | 146 | // Render the scene if required 147 | if(FrameCounter >= _RENDER_FRAME_COUNT) { 148 | CopyBackBufferToWindow(); 149 | FrameCounter = 0; 150 | } else 151 | FrameCounter++; 152 | } 153 | 154 | 155 | ==================================== 156 | void Particle::CalcLoads(void) 157 | { 158 | // Reset forces: 159 | vForces.x = 0.0f; 160 | vForces.y = 0.0f; 161 | 162 | // Aggregate forces: 163 | 164 | // Gravity 165 | vForces += vGravity; 166 | 167 | // Still air drag 168 | Vector vDrag; 169 | Float fDrag; 170 | 171 | vDrag-=vVelocity; 172 | vDrag.Normalize(); 173 | fDrag = 0.5 * _AIRDENSITY * fSpeed * fSpeed * 174 | (3.14159 * fRadius * fRadius) * _DRAGCOEFFICIENT; 175 | 176 | vDrag*=fDrag; 177 | 178 | vForces += vDrag; 179 | 180 | // Wind 181 | Vector vWind; 182 | vWind.x = 0.5 * _AIRDENSITY * _WINDSPEED * 183 | _WINDSPEED * (3.14159 * fRadius * fRadius) * 184 | _DRAGCOEFFICIENT; 185 | vForces += vWind; 186 | } 187 | 188 | 189 | ==================================== 190 | fDrag = 0.5 * _AIRDENSITY * fSpeed * fSpeed * 191 | (3.14159 * fRadius * fRadius) * _DRAGCOEFFICIENT; 192 | 193 | 194 | ==================================== 195 | class Particle { 196 | . 197 | . 198 | . 199 | Vector vPreviousPosition; 200 | Vector vImpactForces; 201 | bool bCollision; 202 | . 203 | . 204 | . 205 | }; 206 | 207 | 208 | ==================================== 209 | void Particle::CalcLoads(void) 210 | { 211 | // Reset forces: 212 | vForces.x = 0.0f; 213 | vForces.y = 0.0f; 214 | 215 | // Aggregate forces: 216 | if(bCollision) { 217 | // Add Impact forces (if any) 218 | vForces += vImpactForces; 219 | } else { 220 | // Gravity 221 | vForces += vGravity; 222 | 223 | // Still air drag 224 | Vector vDrag; 225 | float fDrag; 226 | 227 | vDrag -= vVelocity; 228 | vDrag.Normalize(); 229 | fDrag = 0.5 * _AIRDENSITY * fSpeed * fSpeed * 230 | (3.14159 * fRadius * fRadius) * _DRAGCOEFFICIENT; 231 | vDrag *= fDrag; 232 | vForces += vDrag; 233 | 234 | // Wind 235 | Vector vWind; 236 | vWind.x = 0.5 * _AIRDENSITY * _WINDSPEED * _WINDSPEED * 237 | (3.14159 * fRadius * fRadius) * _DRAGCOEFFICIENT; 238 | vForces += vWind; 239 | } 240 | } 241 | 242 | 243 | ==================================== 244 | void UpdateSimulation(void) 245 | { 246 | . 247 | . 248 | . 249 | 250 | // update computer controlled units: 251 | for(i=0; i<_MAX_NUM_UNITS; i++) 252 | { 253 | Units[i].bCollision = CheckForCollisions(&(Units[i])); 254 | Units[i].CalcLoads(); 255 | Units[i].UpdateBodyEuler(dt); 256 | . 257 | . 258 | . 259 | } // end i-loop 260 | 261 | . 262 | . 263 | . 264 | } 265 | 266 | 267 | ==================================== 268 | bool CheckForCollisions(Particle* p) 269 | { 270 | Vector n; 271 | Vector vr; 272 | float vrn; 273 | float J; 274 | Vector Fi; 275 | bool hasCollision = false; 276 | 277 | 278 | // Reset aggregate impact force 279 | p->vImpactForces.x = 0; 280 | p->vImpactForces.y = 0; 281 | 282 | // check for collisions with ground plane 283 | if(p->vPosition.y <= (_GROUND_PLANE+p->fRadius)) { 284 | n.x = 0; 285 | n.y = 1; 286 | vr = p->vVelocity; 287 | vrn = vr * n; 288 | // check to see if the particle is moving toward the ground 289 | if(vrn < 0.0) { 290 | J = -(vr*n) * (_RESTITUTION + 1) * p->fMass; 291 | Fi = n; 292 | Fi *= J/_TIMESTEP; 293 | p->vImpactForces += Fi; 294 | 295 | p->vPosition.y = _GROUND_PLANE + p->fRadius; 296 | p->vPosition.x = ((_GROUND_PLANE + p->fRadius − 297 | p->vPreviousPosition.y) / 298 | (p->vPosition.y - p->vPreviousPosition.y) * 299 | (p->vPosition.x - p->vPreviousPosition.x)) + 300 | p->vPreviousPosition.x; 301 | 302 | hasCollision = true; 303 | } 304 | } 305 | 306 | return hasCollision; 307 | } 308 | 309 | 310 | ==================================== 311 | J = -(vr*n) * (_RESTITUTION + 1) * p->fMass; 312 | Fi = n; 313 | Fi *= J/_TIMESTEP; 314 | p->vImpactForces += Fi; 315 | 316 | p->vPosition.y = _GROUND_PLANE + p->fRadius; 317 | p->vPosition.x = (_GROUND_PLANE + p->fRadius − 318 | p->vPreviousPosition.y) / 319 | (p->vPosition.y - p->vPreviousPosition.y) * 320 | (p->vPosition.x - p->vPreviousPosition.x) + 321 | p->vPreviousPosition.x; 322 | 323 | hasCollision = true; 324 | 325 | 326 | ==================================== 327 | Particle Obstacles[_NUM_OBSTACLES]; 328 | 329 | 330 | ==================================== 331 | bool Initialize(void) 332 | { 333 | . 334 | . 335 | . 336 | 337 | for(i=0; i<_NUM_OBSTACLES; i++) 338 | { 339 | Obstacles[i].vPosition.x = GetRandomNumber(_WINWIDTH/2 − 340 | _OBSTACLE_RADIUS*10, 341 | _WINWIDTH/2 + 342 | _OBSTACLE_RADIUS*10, false); 343 | Obstacles[i].vPosition.y = GetRandomNumber(_GROUND_PLANE + 344 | _OBSTACLE_RADIUS, _WINHEIGHT/2 − 345 | _OBSTACLE_RADIUS*4, false); 346 | Obstacles[i].fRadius = _OBSTACLE_RADIUS; 347 | Obstacles[i].fMass = 100; 348 | } 349 | . 350 | . 351 | . 352 | } 353 | 354 | 355 | ==================================== 356 | void DrawObstacles(void) 357 | { 358 | int i; 359 | 360 | for(i=0; i<_NUM_OBSTACLES; i++) 361 | { 362 | Obstacles[i].Draw(); 363 | } 364 | 365 | } 366 | 367 | 368 | ==================================== 369 | void UpdateSimulation(void) 370 | { 371 | . 372 | . 373 | . 374 | 375 | // initialize the back buffer 376 | if(FrameCounter >= _RENDER_FRAME_COUNT) 377 | { 378 | ClearBackBuffer(); 379 | // Draw ground plane 380 | DrawLine(0, _WINHEIGHT - _GROUND_PLANE, 381 | _WINWIDTH, _WINHEIGHT - _GROUND_PLANE, 382 | 3, RGB(0,0,0)); 383 | 384 | DrawObstacles(); 385 | } 386 | . 387 | . 388 | . 389 | } 390 | 391 | 392 | ==================================== 393 | bool CheckForCollisions(Particle* p) 394 | { 395 | . 396 | . 397 | . 398 | 399 | // Check for collisions with obstacles 400 | float r; 401 | Vector d; 402 | float s; 403 | 404 | for(i=0; i<_NUM_OBSTACLES; i++) 405 | { 406 | r = p->fRadius + Obstacles[i].fRadius; 407 | d = p->vPosition - Obstacles[i].vPosition; 408 | s = d.Magnitude() - r; 409 | 410 | if(s <= 0.0) 411 | { 412 | d.Normalize(); 413 | n = d; 414 | vr = p->vVelocity - Obstacles[i].vVelocity; 415 | vrn = vr*n; 416 | 417 | 418 | if(vrn < 0.0) 419 | { 420 | J = -(vr*n) * (_RESTITUTION + 1) / 421 | (1/p->fMass + 1/Obstacles[i].fMass); 422 | Fi = n; 423 | Fi *= J/_TIMESTEP; 424 | p->vImpactForces += Fi; 425 | 426 | p->vPosition -= n*s; 427 | hasCollision = true; 428 | } 429 | } 430 | } 431 | . 432 | . 433 | . 434 | } 435 | 436 | 437 | ================== -------------------------------------------------------------------------------- /ch09.txt: -------------------------------------------------------------------------------- 1 | chapter: 2D Rigid-Body Simulator 2 | ================== 3 | class RigidBody2D { 4 | public: 5 | float fMass; // total mass (constant) 6 | float fInertia; // mass moment of inertia 7 | float fInertiaInverse; // inverse of mass moment of inertia 8 | Vector vPosition; // position in earth coordinates 9 | Vector vVelocity; // velocity in earth coordinates 10 | Vector vVelocityBody; // velocity in body coordinates 11 | Vector vAngularVelocity; // angular velocity in body coordinates 12 | 13 | float fSpeed; // speed 14 | float fOrientation; // orientation 15 | 16 | Vector vForces; // total force on body 17 | Vector vMoment; // total moment on body 18 | 19 | float ThrustForce; // Magnitude of the thrust force 20 | Vector PThrust, SThrust; // bow thruster forces 21 | 22 | float fWidth; // bounding dimensions 23 | float fLength; 24 | float fHeight; 25 | 26 | Vector CD; // location of center of drag in body coordinates 27 | Vector CT; // location of center of propeller thrust in body coords. 28 | Vector CPT; // location of port bow thruster thrust in body coords. 29 | Vector CST; // location of starboard bow thruster thrust in body 30 | // coords. 31 | 32 | float ProjectedArea; // projected area of the body 33 | 34 | RigidBody2D(void); 35 | void CalcLoads(void); 36 | void UpdateBodyEuler(double dt); 37 | void SetThrusters(bool p, bool s); 38 | void ModulateThrust(bool up); 39 | }; 40 | 41 | 42 | ==================================== 43 | RigidBody2D::RigidBody2D(void) 44 | { 45 | fMass = 100; 46 | fInertia = 500; 47 | fInertiaInverse = 1/fInertia; 48 | vPosition.x = 0; 49 | vPosition.y = 0; 50 | fWidth = 10; 51 | fLength = 20; 52 | fHeight = 5; 53 | fOrientation = 0; 54 | 55 | CD.x = −0.25*fLength; 56 | CD.y = 0.0f; 57 | CD.z = 0.0f; 58 | 59 | CT.x = −0.5*fLength; 60 | CT.y = 0.0f; 61 | CT.z = 0.0f; 62 | 63 | CPT.x = 0.5*fLength; 64 | CPT.y = −0.5*fWidth; 65 | CPT.z = 0.0f; 66 | 67 | CST.x = 0.5*fLength; 68 | CST.y = 0.5*fWidth; 69 | CST.z = 0.0f; 70 | 71 | ProjectedArea = (fLength + fWidth)/2 * fHeight; // an approximation 72 | ThrustForce = _THRUSTFORCE; 73 | } 74 | 75 | 76 | ==================================== 77 | void RigidBody2D::CalcLoads(void) 78 | { 79 | Vector Fb; // stores the sum of forces 80 | Vector Mb; // stores the sum of moments 81 | Vector Thrust; // thrust vector 82 | 83 | // reset forces and moments: 84 | vForces.x = 0.0f; 85 | vForces.y = 0.0f; 86 | vForces.z = 0.0f; // always zero in 2D 87 | 88 | vMoment.x = 0.0f; // always zero in 2D 89 | vMoment.y = 0.0f; // always zero in 2D 90 | vMoment.z = 0.0f; 91 | 92 | Fb.x = 0.0f; 93 | Fb.y = 0.0f; 94 | Fb.z = 0.0f; 95 | 96 | Mb.x = 0.0f; 97 | Mb.y = 0.0f; 98 | Mb.z = 0.0f; 99 | 100 | // Define the thrust vector, which acts through the craft's CG 101 | Thrust.x = 1.0f; 102 | Thrust.y = 0.0f; 103 | Thrust.z = 0.0f; // zero in 2D 104 | Thrust *= ThrustForce; 105 | 106 | // Calculate forces and moments in body space: 107 | Vector vLocalVelocity; 108 | float fLocalSpeed; 109 | Vector vDragVector; 110 | float tmp; 111 | Vector vResultant; 112 | Vector vtmp; 113 | 114 | // Calculate the aerodynamic drag force: 115 | // Calculate local velocity: 116 | // The local velocity includes the velocity due to 117 | // linear motion of the craft, 118 | // plus the velocity at each element 119 | // due to the rotation of the craft. 120 | 121 | vtmp = vAngularVelocity^CD; // rotational part 122 | vLocalVelocity = vVelocityBody + vtmp; 123 | 124 | // Calculate local air speed 125 | fLocalSpeed = vLocalVelocity.Magnitude(); 126 | 127 | // Find the direction in which drag will act. 128 | // Drag always acts in line with the relative 129 | // velocity but in the opposing direction 130 | if(fLocalSpeed > tol) 131 | { 132 | vLocalVelocity.Normalize(); 133 | vDragVector = -vLocalVelocity; 134 | 135 | // Determine the resultant force on the element. 136 | tmp = 0.5f * rho * fLocalSpeed*fLocalSpeed 137 | * ProjectedArea; 138 | vResultant = vDragVector * _LINEARDRAGCOEFFICIENT * tmp; 139 | 140 | // Keep a running total of these resultant forces 141 | Fb += vResultant; 142 | 143 | // Calculate the moment about the CG 144 | // and keep a running total of these moments 145 | 146 | vtmp = CD^vResultant; 147 | Mb += vtmp; 148 | } 149 | 150 | // Calculate the Port & Starboard bow thruster forces: 151 | // Keep a running total of these resultant forces 152 | 153 | Fb += PThrust; 154 | 155 | 156 | // Calculate the moment about the CG of this element's force 157 | // and keep a running total of these moments (total moment) 158 | vtmp = CPT^PThrust; 159 | Mb += vtmp; 160 | 161 | // Keep a running total of these resultant forces (total force) 162 | Fb += SThrust; 163 | 164 | // Calculate the moment about the CG of this element's force 165 | // and keep a running total of these moments (total moment) 166 | vtmp = CST^SThrust; 167 | Mb += vtmp; 168 | 169 | // Now add the propulsion thrust 170 | Fb += Thrust; // no moment since line of action is through CG 171 | 172 | // Convert forces from model space to earth space 173 | vForces = VRotate2D(fOrientation, Fb); 174 | 175 | vMoment += Mb; 176 | } 177 | 178 | 179 | ==================================== 180 | vLocalVelocity.Normalize(); 181 | vDragVector = -vLocalVelocity; 182 | 183 | // Determine the resultant force on the element. 184 | tmp = 0.5f * rho * fLocalSpeed*fLocalSpeed 185 | * ProjectedArea; 186 | vResultant = vDragVector * _LINEARDRAGCOEFFICIENT * tmp; 187 | 188 | 189 | ==================================== 190 | #define LINEARDRAGCOEFFICIENT 1.25f 191 | 192 | 193 | ==================================== 194 | Fb += vResultant; 195 | 196 | 197 | ==================================== 198 | vtmp = CD^vResultant; 199 | Mb += vtmp; 200 | 201 | 202 | ==================================== 203 | Fb += PThrust; 204 | 205 | vtmp = CPT^PThrust; 206 | Mb += vtmp; 207 | 208 | 209 | ==================================== 210 | Fb += Thrust; // no moment since line of action is through CG 211 | 212 | 213 | ==================================== 214 | Vector VRotate2D( float angle, Vector u) 215 | { 216 | float x,y; 217 | 218 | x = u.x * cos(DegreesToRadians(-angle)) + 219 | u.y * sin(DegreesToRadians(-angle)); 220 | y = -u.x * sin(DegreesToRadians(-angle)) + 221 | u.y * cos(DegreesToRadians(-angle)); 222 | 223 | return Vector( x, y, 0); 224 | } 225 | 226 | 227 | ==================================== 228 | void RigidBody2D::UpdateBodyEuler(double dt) 229 | { 230 | Vector a; 231 | Vector dv; 232 | Vector ds; 233 | float aa; 234 | float dav; 235 | float dr; 236 | 237 | // Calculate forces and moments: 238 | CalcLoads(); 239 | 240 | // Integrate linear equation of motion: 241 | a = vForces / fMass; 242 | 243 | dv = a * dt; 244 | vVelocity += dv; 245 | 246 | ds = vVelocity * dt; 247 | vPosition += ds; 248 | 249 | // Integrate angular equation of motion: 250 | aa = vMoment.z / fInertia; 251 | 252 | dav = aa * dt; 253 | 254 | vAngularVelocity.z += dav; 255 | 256 | dr = RadiansToDegrees(vAngularVelocity.z * dt); 257 | fOrientation += dr; 258 | 259 | // Misc. calculations: 260 | fSpeed = vVelocity.Magnitude(); 261 | vVelocityBody = VRotate2D(-fOrientation, vVelocity); 262 | } 263 | 264 | 265 | ==================================== 266 | void DrawCraft(RigidBody2D craft, COLORREF clr) 267 | { 268 | Vector vList[5]; 269 | double wd, lg; 270 | int i; 271 | Vector v1; 272 | 273 | wd = craft.fWidth; 274 | lg = craft.fLength; 275 | vList[0].x = lg/2; vList[0].y = wd/2; 276 | vList[1].x = -lg/2; vList[1].y = wd/2; 277 | vList[2].x = -lg/2; vList[2].y = -wd/2; 278 | vList[3].x = lg/2; vList[3].y = -wd/2; 279 | vList[4].x = lg/2*1.5; vList[4].y = 0; 280 | for(i=0; i<5; i++) 281 | { 282 | v1 = VRotate2D(craft.fOrientation, vList[i]); 283 | vList[i] = v1 + craft.vPosition; 284 | } 285 | 286 | DrawLine(vList[0].x, vList[0].y, vList[1].x, vList[1].y, 2, clr); 287 | DrawLine(vList[1].x, vList[1].y, vList[2].x, vList[2].y, 2, clr); 288 | DrawLine(vList[2].x, vList[2].y, vList[3].x, vList[3].y, 2, clr); 289 | DrawLine(vList[3].x, vList[3].y, vList[4].x, vList[4].y, 2, clr); 290 | DrawLine(vList[4].x, vList[4].y, vList[0].x, vList[0].y, 2, clr); 291 | } 292 | 293 | 294 | ==================================== 295 | for(i=0; i<5; i++) 296 | { 297 | v1 = VRotate2D(craft.fOrientation, vList[i]); 298 | vList[i] = v1 + craft.vPosition; 299 | } 300 | 301 | 302 | ==================================== 303 | // Global Variables: 304 | int FrameCounter = 0; 305 | RigidBody2D Craft; 306 | 307 | 308 | ==================================== 309 | bool Initialize(void) 310 | { 311 | Craft.vPosition.x = _WINWIDTH/10; 312 | Craft.vPosition.y = _WINHEIGHT/2; 313 | Craft.fOrientation = 0; 314 | 315 | return true; 316 | } 317 | 318 | 319 | ==================================== 320 | void UpdateSimulation(void) 321 | { 322 | double dt = _TIMESTEP; 323 | RECT r; 324 | 325 | Craft.SetThrusters(false, false); 326 | 327 | if (IsKeyDown(VK_UP)) 328 | Craft.ModulateThrust(true); 329 | 330 | if (IsKeyDown(VK_DOWN)) 331 | Craft.ModulateThrust(false); 332 | 333 | if (IsKeyDown(VK_RIGHT)) 334 | Craft.SetThrusters(true, false); 335 | 336 | if (IsKeyDown(VK_LEFT)) 337 | Craft.SetThrusters(false, true); 338 | 339 | // update the simulation 340 | Craft.UpdateBodyEuler(dt); 341 | 342 | if(FrameCounter >= _RENDER_FRAME_COUNT) 343 | { 344 | // update the display 345 | ClearBackBuffer(); 346 | 347 | DrawCraft(Craft, RGB(0,0,255)); 348 | 349 | CopyBackBufferToWindow(); 350 | FrameCounter = 0; 351 | } else 352 | FrameCounter++; 353 | 354 | if(Craft.vPosition.x > _WINWIDTH) Craft.vPosition.x = 0; 355 | if(Craft.vPosition.x < 0) Craft.vPosition.x = _WINWIDTH; 356 | if(Craft.vPosition.y > _WINHEIGHT) Craft.vPosition.y = 0; 357 | if(Craft.vPosition.y < 0) Craft.vPosition.y = _WINHEIGHT; 358 | } 359 | 360 | 361 | ==================================== 362 | Craft.SetThrusters(false, false); 363 | 364 | 365 | ==================================== 366 | Craft.ModulateThrust(true); 367 | 368 | 369 | ==================================== 370 | void RigidBody2D::ModulateThrust(bool up) 371 | { 372 | double dT = up ? _DTHRUST:-_DTHRUST; 373 | 374 | ThrustForce += dT; 375 | 376 | if(ThrustForce > _MAXTHRUST) ThrustForce = _MAXTHRUST; 377 | if(ThrustForce < _MINTHRUST) ThrustForce = _MINTHRUST; 378 | } 379 | 380 | 381 | ==================================== 382 | void RigidBody2D::SetThrusters(bool p, bool s) 383 | { 384 | PThrust.x = 0; 385 | PThrust.y = 0; 386 | SThrust.x = 0; 387 | SThrust.y = 0; 388 | 389 | if(p) 390 | PThrust.y = _STEERINGFORCE; 391 | if(s) 392 | SThrust.y = -_STEERINGFORCE; 393 | } 394 | 395 | 396 | ==================================== 397 | Craft.UpdateBodyEuler(dt) 398 | 399 | 400 | ==================================== 401 | #define _THRUSTFORCE 5.0f 402 | #define _MAXTHRUST 10.0f 403 | #define _MINTHRUST 0.0f 404 | #define _DTHRUST 0.001f 405 | #define _STEERINGFORCE 3.0f 406 | #define _LINEARDRAGCOEFFICIENT 1.25f 407 | 408 | 409 | ================== -------------------------------------------------------------------------------- /ch10.txt: -------------------------------------------------------------------------------- 1 | chapter: Implementing Collision Response 2 | ================== 3 | RigidBody2D Craft2; 4 | 5 | 6 | ==================================== 7 | bool Initialize(void) 8 | { 9 | . 10 | . 11 | . 12 | Craft2.vPosition.x = _WINWIDTH/2; 13 | Craft2.vPosition.y = _WINHEIGHT/2; 14 | Craft2.fOrientation = 90; 15 | . 16 | . 17 | . 18 | } 19 | 20 | 21 | ==================================== 22 | if(Craft2.vPosition.x > _WINWIDTH) Craft2.vPosition.x = 0; 23 | if(Craft2.vPosition.x < 0) Craft2.vPosition.x = _WINWIDTH; 24 | if(Craft2.vPosition.y > _WINHEIGHT) Craft2.vPosition.y = 0; 25 | if(Craft2.vPosition.y < 0) Craft2.vPosition.y = _WINHEIGHT; 26 | 27 | 28 | ==================================== 29 | int CheckForCollision (pRigidBody2D body1, pRigidBody2D body2) 30 | { 31 | Vector d; 32 | float r; 33 | int retval = 0; 34 | float s; 35 | Vector v1, v2; 36 | float Vrn; 37 | 38 | r = body1->ColRadius + body2->ColRadius; 39 | d = body1->vPosition - body2->vPosition; 40 | s = d.Magnitude() - r; 41 | 42 | d.Normalize(); 43 | vCollisionNormal = d; 44 | 45 | v1 = body1->vVelocity; 46 | v2 = body2->vVelocity; 47 | vRelativeVelocity = v1 - v2; 48 | 49 | Vrn = vRelativeVelocity * vCollisionNormal; 50 | if((fabs(s) <= ctol) && (Vrn < 0.0)) 51 | { 52 | retval = 1; // collision; 53 | CollisionBody1 = body1; 54 | CollisionBody2 = body2; 55 | } else if(s < -ctol) 56 | { 57 | retval = −1; // interpenetrating 58 | } else 59 | retval = 0; // no collision 60 | 61 | return retval; 62 | } 63 | 64 | 65 | ==================================== 66 | ->ColRadius = SQRT(fLength*fLength + fWidth*fWidth); 67 | 68 | 69 | ==================================== 70 | void ApplyImpulse(pRigidBody2D body1, pRigidBody2D body2) 71 | { 72 | float j; 73 | 74 | 75 | j = (-(1+fCr) * (vRelativeVelocity*vCollisionNormal)) / 76 | ( (vCollisionNormal*vCollisionNormal) * 77 | (1/body1->fMass + 1/body2->fMass) ); 78 | 79 | body1->vVelocity += (j * vCollisionNormal) / body1->fMass; 80 | body2->vVelocity -= (j * vCollisionNormal) / body2->fMass; 81 | } 82 | 83 | 84 | ==================================== 85 | void UpdateSimulation(float dt) 86 | { 87 | float dtime = dt; 88 | bool tryAgain = true; 89 | int check=0; 90 | RigidBody2D craft1Copy, craft2Copy; 91 | bool didPen = false; 92 | int count = 0; 93 | 94 | 95 | Craft.SetThrusters(false, false); 96 | 97 | if (IsKeyDown(VK_UP)) 98 | Craft.ModulateThrust(true); 99 | 100 | if (IsKeyDown(VK_DOWN)) 101 | Craft.ModulateThrust(false); 102 | 103 | if (IsKeyDown(VK_RIGHT)) 104 | Craft.SetThrusters(true, false); 105 | 106 | if (IsKeyDown(VK_LEFT)) 107 | Craft.SetThrusters(false, true); 108 | 109 | 110 | while(tryAgain && dtime > tol) 111 | { 112 | tryAgain = false; 113 | memcpy(&craft1Copy, &Craft, sizeof(RigidBody2D)); 114 | memcpy(&craft2Copy, &Craft2, sizeof(RigidBody2D)); 115 | 116 | Craft.UpdateBodyEuler(dtime); 117 | Craft2.UpdateBodyEuler(dtime); 118 | 119 | CollisionBody1 = 0; 120 | CollisionBody2 = 0; 121 | check = CheckForCollision(&craft1Copy, &craft2Copy); 122 | 123 | if(check == PENETRATING) 124 | { 125 | dtime = dtime/2; 126 | tryAgain = true; 127 | didPen = true; 128 | } else if(check == COLLISION) 129 | { 130 | if(CollisionBody1 != 0 && CollisionBody2 != 0) 131 | ApplyImpulse(CollisionBody1, CollisionBody2); 132 | } 133 | } 134 | 135 | if(!didPen) 136 | { 137 | memcpy(&Craft, &craft1Copy, sizeof(RigidBody2D)); 138 | memcpy(&Craft2, &craft2Copy, sizeof(RigidBody2D)); 139 | } 140 | } 141 | 142 | 143 | ==================================== 144 | while(tryAgain && dtime > tol) 145 | { 146 | . 147 | . 148 | . 149 | } 150 | 151 | 152 | ==================================== 153 | if(CollisionBody1 != 0 && CollisionBody2 != 0) 154 | ApplyImpulse(CollisionBody1, CollisionBody2); 155 | 156 | 157 | ==================================== 158 | #define LINEARDRAGCOEFFICIENT 0.25f 159 | #define COEFFICIENTOFRESTITUTION 0.5f 160 | #define COLLISIONTOLERANCE 2.0f 161 | 162 | Vector vCollisionNormal; 163 | Vector vRelativeVelocity; 164 | float fCr = COEFFICIENTOFRESTITUTION; 165 | float const ctol = COLLISIONTOLERANCE; 166 | 167 | 168 | ==================================== 169 | int CheckForCollision(pRigidBody2D body1, pRigidBody2D body2) 170 | { 171 | Vector d; 172 | float r; 173 | int retval = 0; 174 | float s; 175 | Vector vList1[4], vList2[4]; 176 | float wd, lg; 177 | int i,j; 178 | bool haveNodeNode = false; 179 | bool interpenetrating = false; 180 | bool haveNodeEdge = false; 181 | Vector v1, v2, u; 182 | Vector edge, p, proj; 183 | float dist, dot; 184 | float Vrn; 185 | 186 | // First check to see if the bounding circles are colliding 187 | r = body1->fLength/2 + body2->fLength/2; 188 | d = body1->vPosition - body2->vPosition; 189 | s = d.Magnitude() - r; 190 | 191 | if(s <= ctol) 192 | { // We have a possible collision, check further 193 | // build vertex lists for each hovercraft 194 | wd = body1->fWidth; 195 | lg = body1->fLength; 196 | vList1[0].y = wd/2; vList1[0].x = lg/2; 197 | vList1[1].y = -wd/2; vList1[1].x = lg/2; 198 | vList1[2].y = -wd/2; vList1[2].x = -lg/2; 199 | vList1[3].y = wd/2; vList1[3].x = -lg/2; 200 | 201 | for(i=0; i<4; i++) 202 | { 203 | VRotate2D(body1->fOrientation, vList1[i]); 204 | vList1[i] = vList1[i] + body1->vPosition; 205 | } 206 | 207 | wd = body2->fWidth; 208 | lg = body2->fLength; 209 | vList2[0].y = wd/2; vList2[0].x = lg/2; 210 | vList2[1].y = -wd/2; vList2[1].x = lg/2; 211 | vList2[2].y = -wd/2; vList2[2].x = -lg/2; 212 | vList2[3].y = wd/2; vList2[3].x = -lg/2; 213 | 214 | for(i=0; i<4; i++) 215 | { 216 | VRotate2D(body2->fOrientation, vList2[i]); 217 | vList2[i] = vList2[i] + body2->vPosition; 218 | } 219 | 220 | // Check for vertex-vertex collision 221 | for(i=0; i<4 && !haveNodeNode; i++) 222 | { 223 | for(j=0; j<4 && !haveNodeNode; j++) 224 | { 225 | 226 | vCollisionPoint = vList1[i]; 227 | body1->vCollisionPoint = vCollisionPoint − 228 | body1->vPosition; 229 | 230 | body2->vCollisionPoint = vCollisionPoint − 231 | body2->vPosition; 232 | 233 | vCollisionNormal = body1->vPosition − 234 | body2->vPosition; 235 | 236 | vCollisionNormal.Normalize(); 237 | 238 | v1 = body1->vVelocityBody + 239 | (body1->vAngularVelocity^body1->vCollisionPoint); 240 | 241 | v2 = body2->vVelocityBody + 242 | (body2->vAngularVelocity^body2->vCollisionPoint); 243 | 244 | v1 = VRotate2D(body1->fOrientation, v1); 245 | v2 = VRotate2D(body2->fOrientation, v2); 246 | 247 | vRelativeVelocity = v1 - v2; 248 | Vrn = vRelativeVelocity * vCollisionNormal; 249 | 250 | if( ArePointsEqual(vList1[i], 251 | vList2[j]) && 252 | (Vrn < 0.0) ) 253 | haveNodeNode = true; 254 | 255 | } 256 | } 257 | 258 | // Check for vertex-edge collision 259 | if(!haveNodeNode) 260 | { 261 | for(i=0; i<4 && !haveNodeEdge; i++) 262 | { 263 | for(j=0; j<3 && !haveNodeEdge; j++) 264 | { 265 | if(j==2) 266 | edge = vList2[0] - vList2[j]; 267 | else 268 | edge = vList2[j+1] - vList2[j]; 269 | u = edge; 270 | u.Normalize(); 271 | 272 | p = vList1[i] - vList2[j]; 273 | proj = (p * u) * u; 274 | 275 | d = p^u; 276 | dist = d.Magnitude(); 277 | 278 | vCollisionPoint = vList1[i]; 279 | body1->vCollisionPoint = vCollisionPoint − 280 | body1->vPosition; 281 | 282 | body2->vCollisionPoint = vCollisionPoint − 283 | body2->vPosition; 284 | 285 | vCollisionNormal = ((u^p)^u); 286 | vCollisionNormal.Normalize(); 287 | 288 | v1 = body1->vVelocityBody + 289 | (body1->vAngularVelocity ^ 290 | body1->vCollisionPoint); 291 | 292 | v2 = body2->vVelocityBody + 293 | (body2->vAngularVelocity ^ 294 | body2->vCollisionPoint); 295 | 296 | v1 = VRotate2D(body1->fOrientation, v1); 297 | v2 = VRotate2D(body2->fOrientation, v2); 298 | 299 | vRelativeVelocity = (v1 - v2); 300 | Vrn = vRelativeVelocity * vCollisionNormal; 301 | 302 | if( (proj.Magnitude() > 0.0f) && 303 | (proj.Magnitude() <= edge.Magnitude()) && 304 | (dist <= ctol) && 305 | (Vrn < 0.0) ) 306 | haveNodeEdge = true; 307 | } 308 | } 309 | } 310 | 311 | // Check for penetration 312 | if(!haveNodeNode && !haveNodeEdge) 313 | { 314 | for(i=0; i<4 && !interpenetrating; i++) 315 | { 316 | for(j=0; j<4 && !interpenetrating; j++) 317 | { 318 | if(j==3) 319 | edge = vList2[0] - vList2[j]; 320 | else 321 | edge = vList2[j+1] - vList2[j]; 322 | 323 | p = vList1[i] - vList2[j]; 324 | dot = p * edge; 325 | if(dot < 0) 326 | { 327 | interpenetrating = true; 328 | } 329 | } 330 | } 331 | } 332 | 333 | if(interpenetrating) 334 | { 335 | retval = −1; 336 | } else if(haveNodeNode || haveNodeEdge) 337 | { 338 | retval = 1; 339 | } else 340 | retval = 0; 341 | 342 | } else 343 | { 344 | retval = 0; 345 | } 346 | 347 | return retval; 348 | } 349 | 350 | 351 | ==================================== 352 | r = body1->fLength/2 + body2->fLength/2; 353 | d = body1->vPosition - body2->vPosition; 354 | s = d.Magnitude() - r; 355 | 356 | if(s <= ctol) 357 | { 358 | . 359 | . 360 | . 361 | } else 362 | retval = 0; 363 | } 364 | 365 | 366 | ==================================== 367 | wd = body1->fWidth; 368 | lg = body1->fLength; 369 | vList1[0].y = wd/2; vList1[0].x = lg/2; 370 | vList1[1].y = -wd/2; vList1[1].x = lg/2; 371 | vList1[2].y = -wd/2; vList1[2].x = -lg/2; 372 | vList1[3].y = wd/2; vList1[3].x = -lg/2; 373 | 374 | for(i=0; i<4; i++) 375 | { 376 | VRotate2D(body1->fOrientation, vList1[i]); 377 | vList1[i] = vList1[i] + body1->vPosition; 378 | } 379 | 380 | wd = body2->fWidth; 381 | lg = body2->fLength; 382 | vList2[0].y = wd/2; vList2[0].x = lg/2; 383 | vList2[1].y = -wd/2; vList2[1].x = lg/2; 384 | vList2[2].y = -wd/2; vList2[2].x = -lg/2; 385 | vList2[3].y = wd/2; vList2[3].x = -lg/2; 386 | for(i=0; i<4; i++) 387 | { 388 | VRotate2D(body2->fOrientation, vList2[i]); 389 | vList2[i] = vList2[i] + body2->vPosition; 390 | } 391 | 392 | 393 | ==================================== 394 | // Check for vertex-vertex collision 395 | for(i=0; i<4 && !haveNodeNode; i++) 396 | { 397 | for(j=0; j<4 && !haveNodeNode; j++) 398 | { 399 | 400 | vCollisionPoint = vList1[i]; 401 | body1->vCollisionPoint = vCollisionPoint − 402 | body1->vPosition; 403 | 404 | body2->vCollisionPoint = vCollisionPoint − 405 | body2->vPosition; 406 | 407 | vCollisionNormal = body1->vPosition − 408 | body2->vPosition; 409 | 410 | vCollisionNormal.Normalize(); 411 | 412 | v1 = body1->vVelocityBody + 413 | (body1->vAngularVelocity^body1->vCollisionPoint); 414 | 415 | v2 = body2->vVelocityBody + 416 | (body2->vAngularVelocity^body2->vCollisionPoint); 417 | 418 | v1 = VRotate2D(body1->fOrientation, v1); 419 | v2 = VRotate2D(body2->fOrientation, v2); 420 | 421 | vRelativeVelocity = v1 - v2; 422 | Vrn = vRelativeVelocity * vCollisionNormal; 423 | 424 | if( ArePointsEqual(vList1[i], 425 | vList2[j]) && 426 | (Vrn < 0.0) ) 427 | haveNodeNode = true; 428 | 429 | } 430 | } 431 | 432 | 433 | ==================================== 434 | if( ArePointsEqual(vList1[i], 435 | vList2[j]) && 436 | (Vrn < 0.0) ) 437 | haveNodeNode = true; 438 | 439 | 440 | ==================================== 441 | bool ArePointsEqual(Vector p1, Vector p2) 442 | { 443 | // Points are equal if each component is within ctol of each other 444 | if( (fabs(p1.x - p2.x) <= ctol) && 445 | (fabs(p1.y - p2.y) <= ctol) && 446 | (fabs(p1.z - p2.z) <= ctol) ) 447 | return true; 448 | else 449 | return false; 450 | } 451 | 452 | 453 | ==================================== 454 | vCollisionPoint = vList1[i]; 455 | body1->vCollisionPoint = vCollisionPoint − 456 | body1->vPosition; 457 | 458 | body2->vCollisionPoint = vCollisionPoint − 459 | body2->vPosition; 460 | 461 | 462 | ==================================== 463 | vCollisionNormal = body1->vPosition − 464 | body2->vPosition; 465 | 466 | vCollisionNormal.Normalize(); 467 | 468 | 469 | ==================================== 470 | v1 = body1->vVelocityBody + 471 | (body1->vAngularVelocity^body1->vCollisionPoint); 472 | 473 | v2 = body2->vVelocityBody + 474 | (body2->vAngularVelocity^body2->vCollisionPoint); 475 | 476 | v1 = VRotate2D(body1->fOrientation, v1); 477 | v2 = VRotate2D(body2->fOrientation, v2); 478 | 479 | vRelativeVelocity = v1 - v2; 480 | Vrn = vRelativeVelocity * vCollisionNormal; 481 | 482 | 483 | ==================================== 484 | // Check for vertex-edge collision 485 | if(!haveNodeNode) 486 | { 487 | for(i=0; i<4 && !haveNodeEdge; i++) 488 | { 489 | for(j=0; j<3 && !haveNodeEdge; j++) 490 | { 491 | if(j==3) 492 | edge = vList2[0] - vList2[j]; 493 | else 494 | edge = vList2[j+1] - vList2[j]; 495 | u = edge; 496 | u.Normalize(); 497 | 498 | p = vList1[i] - vList2[j]; 499 | proj = (p * u) * u; 500 | 501 | d = p^u; 502 | dist = d.Magnitude(); 503 | 504 | vCollisionPoint = vList1[i]; 505 | body1->vCollisionPoint = vCollisionPoint − 506 | body1->vPosition; 507 | 508 | body2->vCollisionPoint = vCollisionPoint − 509 | body2->vPosition; 510 | 511 | vCollisionNormal = ((u^p)^u); 512 | vCollisionNormal.Normalize(); 513 | 514 | v1 = body1->vVelocityBody + 515 | (body1->vAngularVelocity ^ 516 | body1->vCollisionPoint); 517 | 518 | v2 = body2->vVelocityBody + 519 | (body2->vAngularVelocity ^ 520 | body2->vCollisionPoint); 521 | 522 | v1 = VRotate2D(body1->fOrientation, v1); 523 | v2 = VRotate2D(body2->fOrientation, v2); 524 | 525 | vRelativeVelocity = (v1 - v2); 526 | Vrn = vRelativeVelocity * vCollisionNormal; 527 | 528 | if( (proj.Magnitude() > 0.0f) && 529 | (proj.Magnitude() <= edge.Magnitude()) && 530 | (dist <= ctol) && 531 | (Vrn < 0.0) ) 532 | haveNodeEdge = true; 533 | } 534 | } 535 | } 536 | 537 | 538 | ==================================== 539 | if(j==3) 540 | edge = vList2[0] - vList2[j]; 541 | else 542 | edge = vList2[j+1] - vList2[j]; 543 | u = edge; 544 | u.Normalize(); 545 | 546 | 547 | ==================================== 548 | p = vList1[i] - vList2[j]; 549 | proj = (p * u) * u; 550 | 551 | d = p^u; 552 | dist = d.Magnitude(); 553 | 554 | 555 | ==================================== 556 | vCollisionPoint = vList1[i]; 557 | body1->vCollisionPoint = vCollisionPoint − 558 | body1->vPosition; 559 | 560 | body2->vCollisionPoint = vCollisionPoint − 561 | body2->vPosition; 562 | 563 | 564 | ==================================== 565 | vCollisionNormal = ((u^p)^u); 566 | vCollisionNormal.Normalize(); 567 | 568 | 569 | ==================================== 570 | v1 = body1->vVelocityBody + 571 | (body1->vAngularVelocity ^ 572 | body1->vCollisionPoint); 573 | 574 | v2 = body2->vVelocityBody + 575 | (body2->vAngularVelocity ^ 576 | body2->vCollisionPoint); 577 | 578 | v1 = VRotate2D(body1->fOrientation, v1); 579 | v2 = VRotate2D(body2->fOrientation, v2); 580 | 581 | vRelativeVelocity = (v1 - v2); 582 | Vrn = vRelativeVelocity * vCollisionNormal; 583 | 584 | 585 | ==================================== 586 | if( (proj.Magnitude() > 0.0f) && 587 | (proj.Magnitude() <= edge.Magnitude()) && 588 | (dist <= ctol) && 589 | (Vrn < 0.0) ) 590 | haveNodeEdge = true; 591 | 592 | 593 | ==================================== 594 | if(!haveNodeNode && !haveNodeEdge) 595 | { 596 | for(i=0; i<4 && !interpenetrating; i++) 597 | { 598 | for(j=0; j<4 && !interpenetrating; j++) 599 | { 600 | if(j==3) 601 | edge = vList2[0] - vList2[j]; 602 | else 603 | edge = vList2[j+1] - vList2[j]; 604 | 605 | p = vList1[i] - vList2[j]; 606 | dot = p * edge; 607 | if(dot < 0) 608 | { 609 | interpenetrating = true; 610 | } 611 | } 612 | } 613 | } 614 | 615 | 616 | ==================================== 617 | void ApplyImpulse(pRigidBody2D body1, pRigidBody2D body2) 618 | { 619 | float j; 620 | 621 | j = (-(1+fCr) * (vRelativeVelocity*vCollisionNormal)) / 622 | ( (1/body1->fMass + 1/body2->fMass) + 623 | (vCollisionNormal * (((body1->vCollisionPoint ^ 624 | vCollisionNormal)/body1->fInertia)^body1->vCollisionPoint)) + 625 | (vCollisionNormal * (((body2->vCollisionPoint ^ 626 | vCollisionNormal)/body2->fInertia)^body2->vCollisionPoint)) 627 | ); 628 | 629 | body1->vVelocity += (j * vCollisionNormal) / body1->fMass; 630 | body1->vAngularVelocity += (body1->vCollisionPoint ^ 631 | (j * vCollisionNormal)) / 632 | body1->fInertia; 633 | 634 | body2->vVelocity -= (j * vCollisionNormal) / body2->fMass; 635 | body2->vAngularVelocity -= (body2->vCollisionPoint ^ 636 | (j * vCollisionNormal)) / 637 | body2->fInertia; 638 | } 639 | 640 | 641 | ================== -------------------------------------------------------------------------------- /ch11.txt: -------------------------------------------------------------------------------- 1 | chapter: Rotation in 3D Rigid-Body Simulators 2 | ================== 3 | class Quaternion { 4 | public: 5 | float n; // number (scalar) part 6 | Vector v; // vector part: v.x, v.y, v.z 7 | 8 | Quaternion(void); 9 | Quaternion(float e0, float e1, float e2, float e3); 10 | 11 | . 12 | . 13 | . 14 | 15 | }; 16 | 17 | 18 | ==================================== 19 | inline float Quaternion::Magnitude(void) 20 | { 21 | return (float) sqrt(n*n + v.x*v.x + v.y*v.y + v.z*v.z); 22 | } 23 | 24 | 25 | ==================================== 26 | Quaternion operator~(void) const { return Quaternion( n, 27 | -v.x, 28 | -v.y, 29 | -v.z);} 30 | 31 | 32 | ==================================== 33 | inline Vector QVRotate(Quaternion q, Vector v) 34 | { 35 | Quaternion t; 36 | 37 | t = q*v*(~q); 38 | 39 | return t.GetVector(); 40 | } 41 | 42 | 43 | ==================================== 44 | inline Quaternion operator*(Quaternion q1, Quaternion q2) 45 | { 46 | return Quaternion(q1.n*q2.n - q1.v.x*q2.v.x 47 | - q1.v.y*q2.v.y - q1.v.z*q2.v.z, 48 | q1.n*q2.v.x + q1.v.x*q2.n 49 | + q1.v.y*q2.v.z - q1.v.z*q2.v.y, 50 | q1.n*q2.v.y + q1.v.y*q2.n 51 | + q1.v.z*q2.v.x - q1.v.x*q2.v.z, 52 | q1.n*q2.v.z + q1.v.z*q2.n 53 | + q1.v.x*q2.v.y - q1.v.y*q2.v.x); 54 | } 55 | 56 | 57 | ==================================== 58 | inline Quaternion operator*(Quaternion q, Vector v) 59 | { 60 | return Quaternion( -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z), 61 | q.n*v.x + q.v.y*v.z - q.v.z*v.y, 62 | q.n*v.y + q.v.z*v.x - q.v.x*v.z, 63 | q.n*v.z + q.v.x*v.y - q.v.y*v.x); 64 | } 65 | inline Quaternion operator*(Vector v, Quaternion q) 66 | { 67 | return Quaternion( -(q.v.x*v.x + q.v.y*v.y + q.v.z*v.z), 68 | q.n*v.x + q.v.z*v.y - q.v.y*v.z, 69 | q.n*v.y + q.v.x*v.z - q.v.z*v.x, 70 | q.n*v.z + q.v.y*v.x - q.v.x*v.y); 71 | } 72 | 73 | 74 | ==================================== 75 | inline Quaternion MakeQFromEulerAngles(float x, float y, float z) 76 | { 77 | Quaternion q; 78 | double roll = DegreesToRadians(x); 79 | double pitch = DegreesToRadians(y); 80 | double yaw = DegreesToRadians(z); 81 | 82 | double cyaw, cpitch, croll, syaw, spitch, sroll; 83 | double cyawcpitch, syawspitch, cyawspitch, syawcpitch; 84 | 85 | cyaw = cos(0.5f * yaw); 86 | cpitch = cos(0.5f * pitch); 87 | croll = cos(0.5f * roll); 88 | syaw = sin(0.5f * yaw); 89 | spitch = sin(0.5f * pitch); 90 | sroll = sin(0.5f * roll); 91 | 92 | cyawcpitch = cyaw*cpitch; 93 | syawspitch = syaw*spitch; 94 | cyawspitch = cyaw*spitch; 95 | syawcpitch = syaw*cpitch; 96 | 97 | q.n = (float) (cyawcpitch * croll + syawspitch * sroll); 98 | q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll); 99 | q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll); 100 | q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll); 101 | 102 | return q; 103 | } 104 | 105 | 106 | ==================================== 107 | inline Vector MakeEulerAnglesFromQ(Quaternion q) 108 | { 109 | double r11, r21, r31, r32, r33, r12, r13; 110 | double q00, q11, q22, q33; 111 | double tmp; 112 | Vector u; 113 | 114 | q00 = q.n * q.n; 115 | q11 = q.v.x * q.v.x; 116 | q22 = q.v.y * q.v.y; 117 | q33 = q.v.z * q.v.z; 118 | 119 | r11 = q00 + q11 - q22 - q33; 120 | r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z); 121 | r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y); 122 | r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x); 123 | r33 = q00 - q11 - q22 + q33; 124 | 125 | tmp = fabs(r31); 126 | if(tmp > 0.999999) 127 | { 128 | r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z); 129 | r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y); 130 | 131 | u.x = RadiansToDegrees(0.0f); //roll 132 | u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch 133 | u.z = RadiansToDegrees((float) atan2(-r12, -r31*r13)); // yaw 134 | return u; 135 | } 136 | 137 | u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll 138 | u.y = RadiansToDegrees((float) asin(-r31)); // pitch 139 | u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw 140 | return u; 141 | 142 | 143 | } 144 | 145 | 146 | ==================================== 147 | Airplane.qOrientation = MakeQFromEulerAngles(iRoll, iPitch, iYaw); 148 | 149 | 150 | ==================================== 151 | // get the Euler angles for our information 152 | Vector u; 153 | 154 | u = MakeEulerAnglesFromQ(Airplane.qOrientation); 155 | Airplane.vEulerAngles.x = u.x; // roll 156 | Airplane.vEulerAngles.y = u.y; // pitch 157 | Airplane.vEulerAngles.z = u.z; // yaw 158 | 159 | 160 | ==================================== 161 | void CalcAirplaneLoads(void) 162 | { 163 | . 164 | . 165 | . 166 | 167 | // Convert forces from model space to earth space 168 | Airplane.vForces = QVRotate(Airplane.qOrientation, Fb); 169 | . 170 | . 171 | . 172 | } 173 | 174 | 175 | ==================================== 176 | . 177 | . 178 | . 179 | // calculate the angular velocity of the airplane in body space: 180 | Airplane.vAngularVelocity += Airplane.mInertiaInverse * 181 | (Airplane.vMoments - 182 | (Airplane.vAngularVelocity^ 183 | (Airplane.mInertia * 184 | Airplane.vAngularVelocity))) 185 | * dt; 186 | 187 | // calculate the new rotation quaternion: 188 | Airplane.qOrientation += (Airplane.qOrientation * 189 | Airplane.vAngularVelocity) * 190 | (0.5f * dt); 191 | 192 | // now normalize the orientation quaternion: 193 | mag = Airplane.qOrientation.Magnitude(); 194 | if (mag != 0) 195 | Airplane.qOrientation /= mag; 196 | 197 | // calculate the velocity in body space: 198 | // (we'll need this to calculate lift and drag forces) 199 | Airplane.vVelocityBody = QVRotate(~Airplane.qOrientation, 200 | Airplane.vVelocity); 201 | . 202 | . 203 | . 204 | 205 | 206 | ================== -------------------------------------------------------------------------------- /ch12.txt: -------------------------------------------------------------------------------- 1 | chapter: 3D Rigid-Body Simulator 2 | ================== 3 | typedef struct _RigidBody { 4 | 5 | float fMass; // total mass 6 | Matrix3x3 mInertia; // mass moment of inertia 7 | // in body coordinates 8 | 9 | Matrix3x3 mInertiaInverse; // inverse of mass moment of inertia 10 | Vector vPosition; // position in earth coordinates 11 | Vector vVelocity; // velocity in earth coordinates 12 | Vector vVelocityBody; // velocity in body coordinates 13 | Vector vAngularVelocity;// angular velocity in body coordinates 14 | Vector vEulerAngles; // Euler angles in body coordinates 15 | float fSpeed; // speed (magnitude of the velocity) 16 | Quaternion qOrientation; // orientation in earth coordinates 17 | Vector vForces; // total force on body 18 | Vector vMoments; // total moment (torque) on body 19 | } RigidBody, *pRigidBody; 20 | 21 | 22 | ==================================== 23 | RigidBody Airplane; // global variable representing the airplane 24 | . 25 | . 26 | . 27 | 28 | void InitializeAirplane(void) 29 | { 30 | float iRoll, iPitch, iYaw; 31 | 32 | // Set initial position 33 | Airplane.vPosition.x = −5000.0f; 34 | Airplane.vPosition.y = 0.0f; 35 | Airplane.vPosition.z = 2000.0f; 36 | 37 | // Set initial velocity 38 | Airplane.vVelocity.x = 60.0f; 39 | Airplane.vVelocity.y = 0.0f; 40 | Airplane.vVelocity.z = 0.0f; 41 | Airplane.fSpeed = 60.0f; 42 | 43 | // Set initial angular velocity 44 | Airplane.vAngularVelocity.x = 0.0f; 45 | Airplane.vAngularVelocity.y = 0.0f; 46 | Airplane.vAngularVelocity.z = 0.0f; 47 | 48 | // Set the initial thrust, forces, and moments 49 | Airplane.vForces.x = 500.0f; 50 | Airplane.vForces.y = 0.0f; 51 | Airplane.vForces.z = 0.0f; 52 | ThrustForce = 500.0; 53 | 54 | Airplane.vMoments.x = 0.0f; 55 | Airplane.vMoments.y = 0.0f; 56 | Airplane.vMoments.z = 0.0f; 57 | 58 | // Zero the velocity in body space coordinates 59 | Airplane.vVelocityBody.x = 0.0f; 60 | Airplane.vVelocityBody.y = 0.0f; 61 | Airplane.vVelocityBody.z = 0.0f; 62 | 63 | // Set these to false at first, 64 | // you can control later using the keyboard 65 | Stalling = false; 66 | Flaps = false; 67 | 68 | // Set the initial orientation 69 | iRoll = 0.0f; 70 | iPitch = 0.0f; 71 | iYaw = 0.0f; 72 | Airplane.qOrientation = MakeQFromEulerAngles(iRoll, iPitch, iYaw); 73 | 74 | // Now go ahead and calculate the plane's mass properties 75 | CalcAirplaneMassProperties(); 76 | } 77 | 78 | 79 | ==================================== 80 | void CalcAirplaneMassProperties(void) 81 | { 82 | . 83 | . 84 | . 85 | 86 | // Now calculate the moments and products of inertia for the 87 | // combined elements. 88 | // (This inertia matrix (tensor) is in body coordinates) 89 | Ixx = 0; Iyy = 0; Izz = 0; 90 | Ixy = 0; Ixz = 0; Iyz = 0; 91 | for (i = 0; i< 8; i++) 92 | { 93 | Ixx += Element[i].vLocalInertia.x + Element[i].fMass * 94 | (Element[i].vCGCoords.y*Element[i].vCGCoords.y + 95 | Element[i].vCGCoords.z*Element[i].vCGCoords.z); 96 | Iyy += Element[i].vLocalInertia.y + Element[i].fMass * 97 | (Element[i].vCGCoords.z*Element[i].vCGCoords.z + 98 | Element[i].vCGCoords.x*Element[i].vCGCoords.x); 99 | Izz += Element[i].vLocalInertia.z + Element[i].fMass * 100 | (Element[i].vCGCoords.x*Element[i].vCGCoords.x + 101 | Element[i].vCGCoords.y*Element[i].vCGCoords.y); 102 | Ixy += Element[i].fMass * (Element[i].vCGCoords.x * 103 | Element[i].vCGCoords.y); 104 | Ixz += Element[i].fMass * (Element[i].vCGCoords.x * 105 | Element[i].vCGCoords.z); 106 | Iyz += Element[i].fMass * (Element[i].vCGCoords.y * 107 | Element[i].vCGCoords.z); 108 | } 109 | 110 | // Finally set up the airplane's mass and its inertia matrix and take the 111 | // inverse of the inertia matrix 112 | Airplane.fMass = mass; 113 | Airplane.mInertia.e11 = Ixx; 114 | Airplane.mInertia.e12 = -Ixy; 115 | Airplane.mInertia.e13 = -Ixz; 116 | Airplane.mInertia.e21 = -Ixy; 117 | Airplane.mInertia.e22 = Iyy; 118 | Airplane.mInertia.e23 = -Iyz; 119 | Airplane.mInertia.e31 = -Ixz; 120 | Airplane.mInertia.e32 = -Iyz; 121 | Airplane.mInertia.e33 = Izz; 122 | 123 | Airplane.mInertiaInverse = Airplane.mInertia.Inverse(); 124 | } 125 | 126 | 127 | ==================================== 128 | void CalcAirplaneLoads(void) 129 | { 130 | . 131 | . 132 | . 133 | 134 | // Convert forces from model space to earth space 135 | Airplane.vForces = QVRotate(Airplane.qOrientation, Fb); 136 | 137 | // Apply gravity (g is defined as −32.174 ft/s^2) 138 | Airplane.vForces.z += g * Airplane.fMass; 139 | 140 | . 141 | . 142 | . 143 | } 144 | 145 | 146 | ==================================== 147 | void StepSimulation(float dt) 148 | { 149 | // Take care of translation first: 150 | // (If this body were a particle, this is all you would need to do.) 151 | 152 | Vector Ae; 153 | 154 | // calculate all of the forces and moments on the airplane: 155 | CalcAirplaneLoads(); 156 | 157 | // calculate the acceleration of the airplane in earth space: 158 | Ae = Airplane.vForces / Airplane.fMass; 159 | 160 | // calculate the velocity of the airplane in earth space: 161 | Airplane.vVelocity += Ae * dt; 162 | 163 | // calculate the position of the airplane in earth space: 164 | Airplane.vPosition += Airplane.vVelocity * dt; 165 | 166 | 167 | // Now handle the rotations: 168 | float mag; 169 | 170 | // calculate the angular velocity of the airplane in body space: 171 | Airplane.vAngularVelocity += Airplane.mInertiaInverse * 172 | (Airplane.vMoments - 173 | (Airplane.vAngularVelocity^ 174 | (Airplane.mInertia * 175 | Airplane.vAngularVelocity))) 176 | * dt; 177 | 178 | // calculate the new rotation quaternion: 179 | Airplane.qOrientation += (Airplane.qOrientation * 180 | Airplane.vAngularVelocity) * 181 | (0.5f * dt); 182 | 183 | // now normalize the orientation quaternion: 184 | mag = Airplane.qOrientation.Magnitude(); 185 | if (mag != 0) 186 | Airplane.qOrientation /= mag; 187 | 188 | // calculate the velocity in body space: 189 | // (we'll need this to calculate lift and drag forces) 190 | Airplane.vVelocityBody = QVRotate(~Airplane.qOrientation, 191 | Airplane.vVelocity); 192 | 193 | // calculate the air speed: 194 | Airplane.fSpeed = Airplane.vVelocity.Magnitude(); 195 | 196 | // get the Euler angles for our information 197 | Vector u; 198 | 199 | u = MakeEulerAnglesFromQ(Airplane.qOrientation); 200 | Airplane.vEulerAngles.x = u.x; // roll 201 | Airplane.vEulerAngles.y = u.y; // pitch 202 | Airplane.vEulerAngles.z = u.z; // yaw 203 | 204 | } 205 | 206 | 207 | ==================================== 208 | void IncThrust(void) 209 | { 210 | ThrustForce += _DTHRUST; 211 | if(ThrustForce > _MAXTHRUST) 212 | ThrustForce = _MAXTHRUST; 213 | } 214 | 215 | void DecThrust(void) 216 | { 217 | ThrustForce -= _DTHRUST; 218 | if(ThrustForce < 0) 219 | ThrustForce = 0; 220 | } 221 | 222 | 223 | ==================================== 224 | #define _DTHRUST 100.0f 225 | #define _MAXTHRUST 3000.0f 226 | 227 | 228 | ==================================== 229 | void LeftRudder(void) 230 | { 231 | Element[6].fIncidence = 16; 232 | } 233 | 234 | void RightRudder(void) 235 | { 236 | Element[6].fIncidence = −16; 237 | } 238 | 239 | void ZeroRudder(void) 240 | { 241 | Element[6].fIncidence = 0; 242 | } 243 | 244 | 245 | ==================================== 246 | void RollLeft(void) 247 | { 248 | Element[0].iFlap = 1; 249 | Element[3].iFlap = −1; 250 | } 251 | 252 | void RollRight(void) 253 | { 254 | Element[0].iFlap = −1; 255 | Element[3].iFlap = 1; 256 | } 257 | 258 | void ZeroAilerons(void) 259 | { 260 | Element[0].iFlap = 0; 261 | Element[3].iFlap = 0; 262 | } 263 | 264 | 265 | ==================================== 266 | void PitchUp(void) 267 | { 268 | Element[4].iFlap = 1; 269 | Element[5].iFlap = 1; 270 | } 271 | 272 | void PitchDown(void) 273 | { 274 | Element[4].iFlap = −1; 275 | Element[5].iFlap = −1; 276 | } 277 | 278 | 279 | void ZeroElevators(void) 280 | { 281 | Element[4].iFlap = 0; 282 | Element[5].iFlap = 0; 283 | } 284 | 285 | 286 | ==================================== 287 | void FlapsDown(void) 288 | { 289 | Element[1].iFlap = −1; 290 | Element[2].iFlap = −1; 291 | Flaps = true; 292 | } 293 | 294 | void ZeroFlaps(void) 295 | { 296 | Element[1].iFlap = 0; 297 | Element[2].iFlap = 0; 298 | Flaps = false; 299 | } 300 | 301 | 302 | ==================================== 303 | . 304 | . 305 | . 306 | 307 | ZeroRudder(); 308 | ZeroAilerons(); 309 | ZeroElevators(); 310 | 311 | // pitch down 312 | if (IsKeyDown(VK_UP)) 313 | PitchDown(); 314 | 315 | // pitch up 316 | if (IsKeyDown(VK_DOWN)) 317 | PitchUp(); 318 | 319 | // roll left 320 | if (IsKeyDown(VK_LEFT)) 321 | RollLeft(); 322 | 323 | // roll right 324 | if (IsKeyDown(VK_RIGHT)) 325 | RollRight(); 326 | 327 | // Increase thrust 328 | if (IsKeyDown(0x41)) // A 329 | IncThrust(); 330 | 331 | // Decrease thrust 332 | if (IsKeyDown(0x5A)) // Z 333 | DecThrust(); 334 | 335 | // yaw left 336 | if (IsKeyDown(0x58)) // x 337 | LeftRudder(); 338 | 339 | // yaw right 340 | if (IsKeyDown(0x43)) // c 341 | RightRudder(); 342 | 343 | // landing flaps down 344 | if (IsKeyDown(0x46)) //f 345 | FlapsDown(); 346 | 347 | // landing flaps up 348 | if (IsKeyDown(0x44)) // d 349 | ZeroFlaps(); 350 | 351 | StepSimulation(dt); 352 | . 353 | . 354 | . 355 | 356 | 357 | ==================================== 358 | BOOL IsKeyDown(short KeyCode) 359 | { 360 | 361 | SHORT retval; 362 | 363 | retval = GetAsyncKeyState(KeyCode); 364 | 365 | if (HIBYTE(retval)) 366 | return TRUE; 367 | 368 | return FALSE; 369 | } 370 | 371 | 372 | ================== -------------------------------------------------------------------------------- /ch13.txt: -------------------------------------------------------------------------------- 1 | chapter: Connecting Objects 2 | ================== 3 | typedef struct _Spring { 4 | int End1; 5 | int End2; 6 | float k; 7 | float d; 8 | float InitialLength; 9 | } Spring, *pSpring; 10 | 11 | 12 | ==================================== 13 | #define _NUM_OBJECTS 10 14 | #define _NUM_SPRINGS 9 15 | #define _SPRING_K 1000 16 | #define _SPRING_D 100 17 | 18 | Particle Objects[_NUM_OBJECTS]; 19 | Spring Springs[_NUM_SPRINGS]; 20 | 21 | 22 | ==================================== 23 | bool Initialize(void) 24 | { 25 | Vector r; 26 | int i; 27 | 28 | Objects[0].bLocked = true; 29 | 30 | // Initialize particle locations from left to right. 31 | for(i=0; i<_NUM_OBJECTS; i++) 32 | { 33 | Objects[i].vPosition.x = _WINWIDTH/2 + Objects[0].fLength * i; 34 | Objects[i].vPosition.y = _WINHEIGHT/8; 35 | } 36 | 37 | // Initialize springs connecting particles from left to right. 38 | for(i=0; i<_NUM_SPRINGS; i++) 39 | { 40 | Springs[i].End1 = i; 41 | Springs[i].End2 = i+1; 42 | r = Objects[i+1].vPosition - Objects[i].vPosition; 43 | 44 | Springs[i].InitialLength = r.Magnitude(); 45 | Springs[i].k = _SPRING_K; 46 | Springs[i].d = _SPRING_D; 47 | } 48 | return true; 49 | } 50 | 51 | 52 | ==================================== 53 | void UpdateSimulation(void) 54 | { 55 | double dt = _TIMESTEP; 56 | int i; 57 | double f, dl; 58 | Vector pt1, pt2; 59 | int j; 60 | Vector r; 61 | Vector F; 62 | Vector v1, v2, vr; 63 | 64 | // Initialize the spring forces on each object to zero. 65 | for(i=0; i<_NUM_OBJECTS; i++) 66 | { 67 | Objects[i].vSprings.x = 0; 68 | Objects[i].vSprings.y = 0; 69 | Objects[i].vSprings.z = 0; 70 | } 71 | 72 | // Calculate all spring forces based on positions of connected objects. 73 | for(i=0; i<_NUM_SPRINGS; i++) 74 | { 75 | j = Springs[i].End1; 76 | pt1 = Objects[j].vPosition; 77 | v1 = Objects[j].vVelocity; 78 | 79 | j = Springs[i].End2; 80 | pt2 = Objects[j].vPosition; 81 | v2 = Objects[j].vVelocity; 82 | 83 | vr = v2 - v1; 84 | r = pt2 - pt1; 85 | dl = r.Magnitude() - Springs[i].InitialLength; 86 | f = Springs[i].k * dl; // - means compression, + means tension 87 | r.Normalize(); 88 | F = (r*f) + (Springs[i].d*(vr*r))*r; 89 | 90 | j = Springs[i].End1; 91 | Objects[j].vSprings += F; 92 | 93 | j = Springs[i].End2; 94 | Objects[j].vSprings -= F; 95 | } 96 | 97 | . 98 | . 99 | . 100 | // Integrate equations of motion as usual. 101 | . 102 | . 103 | . 104 | // Render the scene as usual. 105 | . 106 | . 107 | . 108 | } 109 | 110 | 111 | ==================================== 112 | dl = r.Magnitude() - Springs[i].InitialLength; 113 | 114 | 115 | ==================================== 116 | f = Springs[i].k * dl; 117 | 118 | 119 | ==================================== 120 | F = (r*f) + (Springs[i].d*(vr*r))*r; 121 | 122 | 123 | ==================================== 124 | r.Normalize(); 125 | 126 | 127 | ==================================== 128 | j = Springs[i].End1; 129 | Objects[j].vSprings += F; 130 | 131 | 132 | ==================================== 133 | j = Springs[i].End2; 134 | Objects[j].vSprings -= F; 135 | 136 | 137 | ==================================== 138 | typedef struct _Spring { 139 | EndPoint End1; 140 | EndPoint End2; 141 | float k; 142 | float d; 143 | float InitialLength; 144 | } Spring, *pSpring; 145 | 146 | 147 | ==================================== 148 | typedef struct _EndPointRef { 149 | int ref; 150 | Vector pt; 151 | } EndPoint; 152 | 153 | 154 | ==================================== 155 | #define _NUM_OBJECTS 10 156 | #define _NUM_SPRINGS 10 157 | #define _SPRING_K 1000 158 | #define _SPRING_D 100 159 | 160 | RigidBody2D Objects[_NUM_OBJECTS]; 161 | Spring Springs[_NUM_SPRINGS]; 162 | 163 | 164 | ==================================== 165 | bool Initialize(void) 166 | { 167 | Vector r; 168 | Vector pt; 169 | int i; 170 | 171 | // Initialize objects for linked chain. 172 | for(i=0; i<_NUM_LINKS; i++) 173 | { 174 | Objects[i].vPosition.x = _WINWIDTH/2 + Objects[0].fLength * i; 175 | Objects[i].vPosition.y = _WINHEIGHT/8; 176 | Objects[i].fOrientation = 0; 177 | } 178 | 179 | // Connect end of the first object to a fixed point in space. 180 | Springs[0].End1.ref = −1; 181 | Springs[0].End1.pt.x = _WINWIDTH/2-Objects[0].fLength/2; 182 | Springs[0].End1.pt.y = _WINHEIGHT/8; 183 | 184 | Springs[0].End2.ref = 0; 185 | Springs[0].End2.pt.x = -Objects[0].fLength/2; 186 | Springs[0].End2.pt.y = 0; 187 | 188 | pt = VRotate2D(Objects[0].fOrientation, Springs[0].End2.pt) 189 | + Objects[0].vPosition; 190 | r = pt - Springs[0].End1.pt; 191 | 192 | Springs[0].InitialLength = r.Magnitude(); 193 | Springs[0].k = _SPRING_K; 194 | Springs[0].d = _SPRING_D; 195 | 196 | // Connect end of all remaining springs. 197 | for(i=1; i<_NUM_LINKS; i++) 198 | { 199 | Springs[i].End1.ref = i-1; 200 | Springs[i].End1.pt.x = Objects[i-1].fLength/2; 201 | Springs[i].End1.pt.y = 0; 202 | 203 | Springs[i].End2.ref = i; 204 | Springs[i].End2.pt.x = -Objects[i].fLength/2; 205 | Springs[i].End2.pt.y = 0; 206 | 207 | pt = VRotate2D(Objects[i].fOrientation, Springs[i].End2.pt) 208 | + Objects[i].vPosition; 209 | r = pt - (VRotate2D(Objects[i-1].fOrientation, Springs[i].End1.pt) 210 | + Objects[i-1].vPosition); 211 | 212 | Springs[i].InitialLength = r.Magnitude(); 213 | Springs[i].k = _SPRING_K; 214 | Springs[i].d = _SPRING_D; 215 | } 216 | 217 | return true; 218 | } 219 | 220 | 221 | ==================================== 222 | // Connect end of the first object to a fixed point in space. 223 | Springs[0].End1.ref = −1; 224 | Springs[0].End1.pt.x = _WINWIDTH/2-Objects[0].fLength/2; 225 | Springs[0].End1.pt.y = _WINHEIGHT/8; 226 | 227 | Springs[0].End2.ref = 0; 228 | Springs[0].End2.pt.x = -Objects[0].fLength/2; 229 | Springs[0].End2.pt.y = 0; 230 | 231 | pt = VRotate2D(Objects[0].fOrientation, Springs[0].End2.pt) 232 | + Objects[0].vPosition; 233 | r = pt - Springs[0].End1.pt; 234 | 235 | Springs[0].InitialLength = r.Magnitude(); 236 | Springs[0].k = _SPRING_K; 237 | Springs[0].d = _SPRING_D; 238 | 239 | 240 | ==================================== 241 | Springs[0].End2.pt.x = -Objects[0].fLength/2; 242 | Springs[0].End2.pt.y = 0; 243 | 244 | 245 | ==================================== 246 | pt = VRotate2D(Objects[0].fOrientation, Springs[0].End2.pt) 247 | + Objects[0].vPosition; 248 | r = pt - Springs[0].End1.pt; 249 | 250 | 251 | ==================================== 252 | pt = VRotate2D(Objects[i].fOrientation, Springs[i].End2.pt) 253 | + Objects[i].vPosition; 254 | r = pt - (VRotate2D(Objects[i-1].fOrientation, Springs[i].End1.pt) 255 | + Objects[i-1].vPosition); 256 | 257 | 258 | ==================================== 259 | void UpdateSimulation(void) 260 | { 261 | . 262 | . 263 | . 264 | Vector M; 265 | Vector Fo; 266 | 267 | // Initialize the spring forces and moments on each object to zero. 268 | for(i=0; i<_NUM_OBJECTS; i++) 269 | { 270 | . 271 | . 272 | . 273 | 274 | Objects[i].vMSprings.x = 0; 275 | Objects[i].vMSprings.y = 0; 276 | Objects[i].vMSprings.z = 0; 277 | } 278 | 279 | // Calculate all spring forces based on positions of connected objects 280 | for(i=0; i<_NUM_SPRINGS; i++) 281 | { 282 | if(Springs[i].End1.ref == −1) 283 | { 284 | pt1 = Springs[i].End1.pt; 285 | v1.x = v1.y = v1.z = 0; // point is not moving 286 | } else { 287 | j = Springs[i].End1.ref; 288 | pt1 = Objects[j].vPosition + VRotate2D(Objects[j].fOrientation, 289 | Springs[i].End1.pt); 290 | v1 = Objects[j].vVelocity + VRotate2D(Objects[j].fOrientation, 291 | Objects[j].vAngularVelocity^Springs[i].End1.pt); 292 | } 293 | 294 | if(Springs[i].End2.ref == −1) 295 | { 296 | pt2 = Springs[i].End2.pt; 297 | v2.x = v2.y = v2.z = 0; 298 | } else { 299 | j = Springs[i].End2.ref; 300 | pt2 = Objects[j].vPosition + VRotate2D(Objects[j].fOrientation, 301 | Springs[i].End2.pt); 302 | v2 = Objects[j].vVelocity + VRotate2D(Objects[j].fOrientation, 303 | Objects[j].vAngularVelocity^Springs[i].End2.pt); 304 | } 305 | 306 | // Compute spring-damper force. 307 | vr = v2 - v1; 308 | r = pt2 - pt1; 309 | dl = r.Magnitude() - Springs[i].InitialLength; 310 | f = Springs[i].k * dl; 311 | r.Normalize(); 312 | F = (r*f) + (Springs[i].d*(vr*r))*r; 313 | 314 | // Aggregate the spring force on each connected object 315 | j = Springs[i].End1.ref; 316 | if(j != −1) 317 | Objects[j].vSprings += F; 318 | 319 | j = Springs[i].End2.ref; 320 | if(j != −1) 321 | Objects[j].vSprings −= F; 322 | 323 | 324 | // convert force to first ref local coords 325 | // Get local lever 326 | // calc moment 327 | 328 | // Compute and aggregate moments due to spring force 329 | // on each connected object. 330 | j = Springs[i].End1.ref; 331 | if(j != −1) 332 | { 333 | Fo = VRotate2D(-Objects[j].fOrientation, F); 334 | r = Springs[i].End1.pt; 335 | M = r^Fo; 336 | Objects[j].vMSprings += M; 337 | } 338 | 339 | j = Springs[i].End2.ref; 340 | if(j!= −1) 341 | { 342 | Fo = VRotate2D(-Objects[j].fOrientation, F); 343 | r = Springs[i].End2.pt; 344 | M = r^Fo; 345 | Objects[j].vMSprings −= M; 346 | } 347 | } 348 | . 349 | . 350 | . 351 | // Integrate equations of motion as usual. 352 | . 353 | . 354 | . 355 | // Render the scene as usual. 356 | . 357 | . 358 | . 359 | } 360 | 361 | 362 | ==================================== 363 | pt1 = Objects[j].vPosition + VRotate2D(Objects[j].fOrientation, 364 | Springs[i].End1.pt); 365 | 366 | 367 | ==================================== 368 | v1 = Objects[j].vVelocity + VRotate2D(Objects[j].fOrientation, 369 | Objects[j].vAngularVelocity^Springs[i].End1.pt); 370 | 371 | 372 | ==================================== 373 | Fo = VRotate2D(-Objects[j].fOrientation, F); 374 | r = Springs[i].End1.pt; 375 | M = r^Fo; 376 | Objects[j].vMSprings += M; 377 | 378 | 379 | ==================================== 380 | Fo = VRotate2D(-Objects[j].fOrientation, F); 381 | 382 | 383 | ==================================== 384 | bool Initialize(void) 385 | { 386 | Vector r; 387 | Vector pt; 388 | int i; 389 | 390 | // Position objects 391 | Objects[0].vPosition.x = _WINWIDTH/2; 392 | Objects[0].vPosition.y = _WINHEIGHT/8+Objects[0].fLength/2; 393 | Objects[0].fOrientation = 90; 394 | 395 | Objects[1].vPosition.x = _WINWIDTH/2+Objects[1].fLength/2; 396 | Objects[1].vPosition.y = _WINHEIGHT/8+Objects[0].fLength; 397 | Objects[1].fOrientation = 0; 398 | 399 | // Connect end of the first object to the earth: 400 | Springs[0].End1.ref = −1; 401 | Springs[0].End1.pt.x = _WINWIDTH/2; 402 | Springs[0].End1.pt.y = _WINHEIGHT/8; 403 | 404 | Springs[0].End2.ref = 0; 405 | Springs[0].End2.pt.x = -Objects[0].fLength/2; 406 | Springs[0].End2.pt.y = 0; 407 | 408 | pt = VRotate2D(Objects[0].fOrientation, Springs[0].End2.pt) + 409 | Objects[0].vPosition; 410 | r = pt - Springs[0].End1.pt; 411 | 412 | Springs[0].InitialLength = r.Magnitude(); 413 | Springs[0].k = _SPRING_K; 414 | Springs[0].d = _SPRING_D; 415 | 416 | // Connect other end of first object to end of second object 417 | i = 1; 418 | Springs[i].End1.ref = i-1; 419 | Springs[i].End1.pt.x = Objects[i-1].fLength/2; 420 | Springs[i].End1.pt.y = 0; 421 | 422 | Springs[i].End2.ref = i; 423 | Springs[i].End2.pt.x = -Objects[i].fLength/2; 424 | Springs[i].End2.pt.y = 0; 425 | 426 | pt = VRotate2D(Objects[i].fOrientation, Springs[i].End2.pt) + 427 | Objects[i].vPosition; 428 | r = pt - (VRotate2D(Objects[i-1].fOrientation, Springs[i].End1.pt) + 429 | Objects[i-1].vPosition); 430 | 431 | Springs[i].InitialLength = r.Magnitude(); 432 | Springs[i].k = _SPRING_K; 433 | Springs[i].d = _SPRING_D; 434 | 435 | // Connect CG of objects to each other 436 | Springs[2].End1.ref = 0; 437 | Springs[2].End1.pt.x = 0; 438 | Springs[2].End1.pt.y = 0; 439 | 440 | Springs[2].End2.ref = 1; 441 | Springs[2].End2.pt.x = 0; 442 | Springs[2].End2.pt.y = 0; 443 | 444 | r = Objects[1].vPosition - Objects[0].vPosition; 445 | 446 | Springs[2].InitialLength = r.Magnitude(); 447 | Springs[2].k = _SPRING_K; 448 | Springs[2].d = _SPRING_D; 449 | } 450 | 451 | 452 | ==================================== 453 | Objects[0].vPosition.x = _WINWIDTH/2; 454 | Objects[0].vPosition.y = _WINHEIGHT/8+Objects[0].fLength/2; 455 | Objects[0].fOrientation = 90; 456 | 457 | Objects[1].vPosition.x = _WINWIDTH/2+Objects[1].fLength/2; 458 | Objects[1].vPosition.y = _WINHEIGHT/8+Objects[0].fLength; 459 | Objects[1].fOrientation = 0; 460 | 461 | 462 | ==================================== 463 | // Connect end of the first object to the earth: 464 | Springs[0].End1.ref = −1; 465 | Springs[0].End1.pt.x = _WINWIDTH/2; 466 | Springs[0].End1.pt.y = _WINHEIGHT/8; 467 | 468 | Springs[0].End2.ref = 0; 469 | Springs[0].End2.pt.x = -Objects[0].fLength/2; 470 | Springs[0].End2.pt.y = 0; 471 | 472 | pt = VRotate2D(Objects[0].fOrientation, Springs[0].End2.pt) + 473 | Objects[0].vPosition; 474 | r = pt - Springs[0].End1.pt; 475 | 476 | Springs[0].InitialLength = r.Magnitude(); 477 | Springs[0].k = _SPRING_K; 478 | Springs[0].d = _SPRING_D; 479 | 480 | 481 | ==================================== 482 | // Connect other end of first object to end of second object 483 | i = 1; 484 | Springs[i].End1.ref = i-1; 485 | Springs[i].End1.pt.x = Objects[i-1].fLength/2; 486 | Springs[i].End1.pt.y = 0; 487 | 488 | Springs[i].End2.ref = i; 489 | Springs[i].End2.pt.x = -Objects[i].fLength/2; 490 | Springs[i].End2.pt.y = 0; 491 | 492 | pt = VRotate2D(Objects[i].fOrientation, Springs[i].End2.pt) + 493 | Objects[i].vPosition; 494 | r = pt - (VRotate2D(Objects[i-1].fOrientation, Springs[i].End1.pt) + 495 | Objects[i-1].vPosition); 496 | 497 | Springs[i].InitialLength = r.Magnitude(); 498 | Springs[i].k = _SPRING_K; 499 | Springs[i].d = _SPRING_D; 500 | 501 | 502 | ==================================== 503 | // Connect CG of objects to each other 504 | Springs[2].End1.ref = 0; 505 | Springs[2].End1.pt.x = 0; 506 | Springs[2].End1.pt.y = 0; 507 | 508 | Springs[2].End2.ref = 1; 509 | Springs[2].End2.pt.x = 0; 510 | Springs[2].End2.pt.y = 0; 511 | 512 | r = Objects[1].vPosition - Objects[0].vPosition; 513 | 514 | Springs[2].InitialLength = r.Magnitude(); 515 | Springs[2].k = _SPRING_K; 516 | Springs[2].d = _SPRING_D; 517 | 518 | 519 | ================== -------------------------------------------------------------------------------- /ch15.txt: -------------------------------------------------------------------------------- 1 | chapter: Aircraft 2 | ================== 3 | //------------------------------------------------------------------------// 4 | // This model uses a set of eight discrete elements to represent the 5 | // airplane. The elements are described below: 6 | // 7 | // Element 0: Outboard; port (left) wing section fitted with ailerons 8 | // Element 1: Inboard; port wing section fitted with landing flaps 9 | // Element 2: Inboard; starboard (right) wing section fitted with 10 | landing flaps 11 | // Element 3: Outboard; starboard wing section fitted with ailerons 12 | // Element 4: Port elevator fitted with flap 13 | // Element 5: Starboard elevator fitted with flap 14 | // Element 6: Vertical tail/rudder (no flap; the whole thing rotates) 15 | // Element 7: The fuselage 16 | // 17 | // This function first sets up each element and then goes on to calculate 18 | // the combined weight, center of gravity, and inertia tensor for the plane. 19 | // Some other properties of each element are also calculated, which you'll 20 | // need when calculating the lift and drag forces on the plane. 21 | //------------------------------------------------------------------------// 22 | void CalcAirplaneMassProperties(void) 23 | { 24 | float mass; 25 | Vector vMoment; 26 | Vector CG; 27 | int i; 28 | float Ixx, Iyy, Izz, Ixy, Ixz, Iyz; 29 | float in, di; 30 | 31 | // Initialize the elements here 32 | // Initially the coordinates of each element are referenced from 33 | // a design coordinates system located at the very tail end of the plane, 34 | // its baseline and center line. Later, these coordinates will be adjusted 35 | // so that each element is referenced to the combined center of gravity of 36 | // the airplane. 37 | Element[0].fMass = 6.56f; 38 | Element[0].vDCoords = Vector(14.5f,12.0f,2.5f); 39 | Element[0].vLocalInertia = Vector(13.92f,10.50f,24.00f); 40 | Element[0].fIncidence = −3.5f; 41 | Element[0].fDihedral = 0.0f; 42 | Element[0].fArea = 31.2f; 43 | Element[0].iFlap = 0; 44 | 45 | Element[1].fMass = 7.31f; 46 | Element[1].vDCoords = Vector(14.5f,5.5f,2.5f); 47 | Element[1].vLocalInertia = Vector(21.95f,12.22f,33.67f); 48 | Element[1].fIncidence = −3.5f; 49 | Element[1].fDihedral = 0.0f; 50 | Element[1].fArea = 36.4f; 51 | Element[1].iFlap = 0; 52 | 53 | Element[2].fMass = 7.31f; 54 | Element[2].vDCoords = Vector(14.5f,−5.5f,2.5f); 55 | Element[2].vLocalInertia = Vector(21.95f,12.22f,33.67f); 56 | Element[2].fIncidence = −3.5f; 57 | Element[2].fDihedral = 0.0f; 58 | Element[2].fArea = 36.4f; 59 | Element[2].iFlap = 0; 60 | 61 | Element[3].fMass = 6.56f; 62 | Element[3].vDCoords = Vector(14.5f,−12.0f,2.5f); 63 | Element[3].vLocalInertia = Vector(13.92f,10.50f,24.00f); 64 | Element[3].fIncidence = −3.5f; 65 | Element[3].fDihedral = 0.0f; 66 | Element[3].fArea = 31.2f; 67 | Element[3].iFlap = 0; 68 | 69 | Element[4].fMass = 2.62f; 70 | Element[4].vDCoords = Vector(3.03f,2.5f,3.0f); 71 | Element[4].vLocalInertia = Vector(0.837f,0.385f,1.206f); 72 | Element[4].fIncidence = 0.0f; 73 | Element[4].fDihedral = 0.0f; 74 | Element[4].fArea = 10.8f; 75 | Element[4].iFlap = 0; 76 | 77 | Element[5].fMass = 2.62f; 78 | Element[5].vDCoords = Vector(3.03f,−2.5f,3.0f); 79 | Element[5].vLocalInertia = Vector(0.837f,0.385f,1.206f); 80 | Element[5].fIncidence = 0.0f; 81 | Element[5].fDihedral = 0.0f; 82 | Element[5].fArea = 10.8f; 83 | Element[5].iFlap = 0; 84 | 85 | Element[6].fMass = 2.93f; 86 | Element[6].vDCoords = Vector(2.25f,0.0f,5.0f); 87 | Element[6].vLocalInertia = Vector(1.262f,1.942f,0.718f); 88 | Element[6].fIncidence = 0.0f; 89 | Element[6].fDihedral = 90.0f; 90 | Element[6].fArea = 12.0f; 91 | Element[6].iFlap = 0; 92 | 93 | Element[7].fMass = 31.8f; 94 | Element[7].vDCoords = Vector(15.25f,0.0f,1.5f); 95 | Element[7].vLocalInertia = Vector(66.30f,861.9f,861.9f); 96 | Element[7].fIncidence = 0.0f; 97 | Element[7].fDihedral = 0.0f; 98 | Element[7].fArea = 84.0f; 99 | Element[7].iFlap = 0; 100 | 101 | // Calculate the vector normal (perpendicular) to each lifting surface. 102 | // This is required when you are calculating the relative air velocity for 103 | // lift and drag calculations. 104 | for (i = 0; i< 8; i++) 105 | { 106 | in = DegreesToRadians(Element[i].fIncidence); 107 | di = DegreesToRadians(Element[i].fDihedral); 108 | Element[i].vNormal = Vector((float)sin(in), (float)(cos(in)*sin(di)), 109 | (float)(cos(in)*cos(di))); 110 | Element[i].vNormal.Normalize(); 111 | } 112 | 113 | // Calculate total mass 114 | mass = 0; 115 | for (i = 0; i< 8; i++) 116 | mass += Element[i].fMass; 117 | 118 | // Calculate combined center of gravity location 119 | vMoment = Vector(0.0f, 0.0f, 0.0f); 120 | for (i = 0; i< 8; i++) 121 | { 122 | vMoment += Element[i].fMass*Element[i].vDCoords; 123 | } 124 | CG = vMoment/mass; 125 | 126 | // Calculate coordinates of each element with respect to the combined CG 127 | for (i = 0; i< 8; i++) 128 | { 129 | Element[i].vCGCoords = Element[i].vDCoords − CG; 130 | } 131 | 132 | // Now calculate the moments and products of inertia for the 133 | // combined elements. 134 | // (This inertia matrix (tensor) is in body coordinates) 135 | Ixx = 0; Iyy = 0; Izz = 0; 136 | Ixy = 0; Ixz = 0; Iyz = 0; 137 | for (i = 0; i< 8; i++) 138 | { 139 | Ixx += Element[i].vLocalInertia.x + Element[i].fMass * 140 | (Element[i].vCGCoords.y*Element[i].vCGCoords.y + 141 | Element[i].vCGCoords.z*Element[i].vCGCoords.z); 142 | Iyy += Element[i].vLocalInertia.y + Element[i].fMass * 143 | (Element[i].vCGCoords.z*Element[i].vCGCoords.z + 144 | Element[i].vCGCoords.x*Element[i].vCGCoords.x); 145 | Izz += Element[i].vLocalInertia.z + Element[i].fMass * 146 | (Element[i].vCGCoords.x*Element[i].vCGCoords.x + 147 | Element[i].vCGCoords.y*Element[i].vCGCoords.y); 148 | Ixy += Element[i].fMass * (Element[i].vCGCoords.x * 149 | Element[i].vCGCoords.y); 150 | Ixz += Element[i].fMass * (Element[i].vCGCoords.x * 151 | Element[i].vCGCoords.z); 152 | Iyz += Element[i].fMass * (Element[i].vCGCoords.y * 153 | Element[i].vCGCoords.z); 154 | } 155 | 156 | // Finally, set up the airplane's mass and its inertia matrix and take the 157 | // inverse of the inertia matrix. 158 | Airplane.fMass = mass; 159 | Airplane.mInertia.e11 = Ixx; 160 | Airplane.mInertia.e12 = -Ixy; 161 | Airplane.mInertia.e13 = -Ixz; 162 | Airplane.mInertia.e21 = -Ixy; 163 | Airplane.mInertia.e22 = Iyy; 164 | Airplane.mInertia.e23 = -Iyz; 165 | Airplane.mInertia.e31 = -Ixz; 166 | Airplane.mInertia.e32 = -Iyz; 167 | Airplane.mInertia.e33 = Izz; 168 | 169 | Airplane.mInertiaInverse = Airplane.mInertia.Inverse(); 170 | } 171 | 172 | 173 | ==================================== 174 | //------------------------------------------------------------------------// 175 | // Given the attack angle and the status of the flaps, this function 176 | // returns the appropriate lift coefficient for a cambered airfoil with 177 | // a plain trailing-edge flap (+/- 15 degree deflection). 178 | //------------------------------------------------------------------------// 179 | float LiftCoefficient(float angle, int flaps) 180 | { 181 | float clf0[9] = {−0.54f, −0.2f, 0.2f, 0.57f, 0.92f, 1.21f, 1.43f, 1.4f, 182 | 1.0f}; 183 | float clfd[9] = {0.0f, 0.45f, 0.85f, 1.02f, 1.39f, 1.65f, 1.75f, 1.38f, 184 | 1.17f}; 185 | float clfu[9] = {−0.74f, −0.4f, 0.0f, 0.27f, 0.63f, 0.92f, 1.03f, 1.1f, 186 | 0.78f}; 187 | float a[9] = {−8.0f, −4.0f, 0.0f, 4.0f, 8.0f, 12.0f, 16.0f, 20.0f, 188 | 24.0f}; 189 | float cl; 190 | int i; 191 | 192 | cl = 0; 193 | for (i=0; i<8; i++) 194 | { 195 | if( (a[i] <= angle) && (a[i+1] > angle) ) 196 | { 197 | switch(flaps) 198 | { 199 | case 0:// flaps not deflected 200 | cl = clf0[i] - (a[i] - angle) * (clf0[i] - clf0[i+1]) / 201 | (a[i] - a[i+1]); 202 | break; 203 | case −1: // flaps down 204 | cl = clfd[i] - (a[i] - angle) * (clfd[i] - clfd[i+1]) / 205 | (a[i] - a[i+1]); 206 | break; 207 | case 1: // flaps up 208 | cl = clfu[i] - (a[i] - angle) * (clfu[i] - clfu[i+1]) / 209 | (a[i] - a[i+1]); 210 | break; 211 | } 212 | break; 213 | } 214 | } 215 | 216 | return cl; 217 | } 218 | 219 | //------------------------------------------------------------------------// 220 | // Given the attack angle and the status of the flaps, this function 221 | // returns the appropriate drag coefficient for a cambered airfoil with 222 | // a plain trailing-edge flap (+/- 15 degree deflection). 223 | //------------------------------------------------------------------------// 224 | float DragCoefficient(float angle, int flaps) 225 | { 226 | float cdf0[9] = {0.01f, 0.0074f, 0.004f, 0.009f, 0.013f, 0.023f, 0.05f, 227 | 0.12f, 0.21f}; 228 | float cdfd[9] = {0.0065f, 0.0043f, 0.0055f, 0.0153f, 0.0221f, 0.0391f, 0.1f, 229 | 0.195f, 0.3f}; 230 | float cdfu[9] = {0.005f, 0.0043f, 0.0055f, 0.02601f, 0.03757f, 0.06647f, 231 | 0.13f, 0.18f, 0.25f}; 232 | float a[9] = {−8.0f, −4.0f, 0.0f, 4.0f, 8.0f, 12.0f, 16.0f, 20.0f, 233 | 24.0f}; 234 | float cd; 235 | int i; 236 | 237 | cd = 0.5; 238 | for (i=0; i<8; i++) 239 | { 240 | if( (a[i] <= angle) && (a[i+1] > angle) ) 241 | { 242 | switch(flaps) 243 | { 244 | case 0:// flaps not deflected 245 | cd = cdf0[i] - (a[i] - angle) * (cdf0[i] - cdf0[i+1]) / 246 | (a[i] - a[i+1]); 247 | break; 248 | case −1: // flaps down 249 | cd = cdfd[i] - (a[i] - angle) * (cdfd[i] - cdfd[i+1]) / 250 | (a[i] - a[i+1]); 251 | break; 252 | case 1: // flaps up 253 | cd = cdfu[i] - (a[i] - angle) * (cdfu[i] - cdfu[i+1]) / 254 | (a[i] - a[i+1]); 255 | break; 256 | } 257 | break; 258 | } 259 | } 260 | 261 | return cd; 262 | 263 | } 264 | 265 | 266 | ==================================== 267 | //------------------------------------------------------------------------// 268 | // Given the attack angle, this function returns the proper lift coefficient 269 | // for a symmetric (no camber) airfoil without flaps. 270 | //------------------------------------------------------------------------// 271 | float RudderLiftCoefficient(float angle) 272 | { 273 | float clf0[7] = {0.16f, 0.456f, 0.736f, 0.968f, 1.144f, 1.12f, 0.8f}; 274 | float a[7] = {0.0f, 4.0f, 8.0f, 12.0f, 16.0f, 20.0f, 24.0f}; 275 | float cl; 276 | int i; 277 | float aa = (float) fabs(angle); 278 | 279 | cl = 0; 280 | for (i=0; i<8; i++) 281 | { 282 | if( (a[i] <= aa) && (a[i+1] > aa) ) 283 | { 284 | cl = clf0[i] - (a[i] - aa) * (clf0[i] - clf0[i+1]) / 285 | (a[i] - a[i+1]); 286 | if (angle < 0) cl = -cl; 287 | break; 288 | } 289 | } 290 | return cl; 291 | } 292 | 293 | //------------------------------------------------------------------------// 294 | // Given the attack angle, this function returns the proper drag coefficient 295 | // for a symmetric (no camber) airfoil without flaps. 296 | //------------------------------------------------------------------------// 297 | float RudderDragCoefficient(float angle) 298 | { 299 | float cdf0[7] = {0.0032f, 0.0072f, 0.0104f, 0.0184f, 0.04f, 0.096f, 0.168f}; 300 | float a[7] = {0.0f, 4.0f, 8.0f, 12.0f, 16.0f, 20.0f, 24.0f}; 301 | float cd; 302 | int i; 303 | float aa = (float) fabs(angle); 304 | 305 | cd = 0.5; 306 | for (i=0; i<8; i++) 307 | { 308 | if( (a[i] <= aa) && (a[i+1] > aa) ) 309 | { 310 | cd = cdf0[i] - (a[i] - aa) * (cdf0[i] - cdf0[i+1]) / 311 | (a[i] - a[i+1]); 312 | break; 313 | } 314 | } 315 | return cd; 316 | } 317 | 318 | 319 | ==================================== 320 | //------------------------------------------------------------------------// 321 | // This function calculates all of the forces and moments acting on the 322 | // plane at any given time. 323 | //------------------------------------------------------------------------// 324 | void CalcAirplaneLoads(void) 325 | { 326 | Vector Fb, Mb; 327 | 328 | // reset forces and moments: 329 | Airplane.vForces.x = 0.0f; 330 | Airplane.vForces.y = 0.0f; 331 | Airplane.vForces.z = 0.0f; 332 | 333 | Airplane.vMoments.x = 0.0f; 334 | Airplane.vMoments.y = 0.0f; 335 | Airplane.vMoments.z = 0.0f; 336 | 337 | Fb.x = 0.0f; Mb.x = 0.0f; 338 | Fb.y = 0.0f; Mb.y = 0.0f; 339 | Fb.z = 0.0f; Mb.z = 0.0f; 340 | 341 | // Define the thrust vector, which acts through the plane's CG 342 | Thrust.x = 1.0f; 343 | Thrust.y = 0.0f; 344 | Thrust.z = 0.0f; 345 | Thrust *= ThrustForce; 346 | 347 | // Calculate forces and moments in body space: 348 | Vector vLocalVelocity; 349 | float fLocalSpeed; 350 | Vector vDragVector; 351 | Vector vLiftVector; 352 | float fAttackAngle; 353 | float tmp; 354 | Vector vResultant; 355 | int i; 356 | Vector vtmp; 357 | 358 | Stalling = false; 359 | 360 | for(i=0; i<7; i++) // loop through the seven lifting elements 361 | // skipping the fuselage 362 | { 363 | if (i == 6) // The tail/rudder is a special case since it can rotate; 364 | { // thus, you have to recalculate the normal vector. 365 | float in, di; 366 | in = DegreesToRadians(Element[i].fIncidence); // incidence angle 367 | di = DegreesToRadians(Element[i].fDihedral); // dihedral angle 368 | Element[i].vNormal = Vector( (float)sin(in), 369 | (float)(cos(in)*sin(di)), 370 | (float)(cos(in)*cos(di))); 371 | Element[i].vNormal.Normalize(); 372 | } 373 | 374 | // Calculate local velocity at element 375 | // The local velocity includes the velocity due to linear 376 | // motion of the airplane, 377 | // plus the velocity at each element due to the 378 | // rotation of the airplane. 379 | 380 | // Here's the rotational part 381 | vtmp = Airplane.vAngularVelocity^Element[i].vCGCoords; 382 | 383 | vLocalVelocity = Airplane.vVelocityBody + vtmp; 384 | 385 | // Calculate local air speed 386 | fLocalSpeed = vLocalVelocity.Magnitude(); 387 | 388 | // Find the direction in which drag will act. 389 | // Drag always acts inline with the relative 390 | // velocity but in the opposing direction 391 | if(fLocalSpeed > 1.) 392 | vDragVector = -vLocalVelocity/fLocalSpeed; 393 | 394 | // Find the direction in which lift will act. 395 | // Lift is always perpendicular to the drag vector 396 | vLiftVector = (vDragVector^Element[i].vNormal)^vDragVector; 397 | tmp = vLiftVector.Magnitude(); 398 | vLiftVector.Normalize(); 399 | 400 | // Find the angle of attack. 401 | // The attack angle is the angle between the lift vector and the 402 | // element normal vector. Note, the sine of the attack angle 403 | // is equal to the cosine of the angle between the drag vector and 404 | // the normal vector. 405 | tmp = vDragVector*Element[i].vNormal; 406 | if(tmp > 1.) tmp = 1; 407 | if(tmp < −1) tmp = −1; 408 | fAttackAngle = RadiansToDegrees((float) asin(tmp)); 409 | 410 | // Determine the resultant force (lift and drag) on the element. 411 | tmp = 0.5f * rho * fLocalSpeed*fLocalSpeed * Element[i].fArea; 412 | if (i == 6) // Tail/rudder 413 | { 414 | vResultant = (vLiftVector*RudderLiftCoefficient(fAttackAngle) + 415 | vDragVector*RudderDragCoefficient(fAttackAngle)) 416 | * tmp; 417 | } else 418 | vResultant = (vLiftVector*LiftCoefficient(fAttackAngle, 419 | Element[i].iFlap) + 420 | vDragVector*DragCoefficient(fAttackAngle, 421 | Element[i].iFlap) ) * tmp; 422 | // Check for stall. 423 | // We can easily determine stall by noting when the coefficient 424 | // of lift is 0. In reality, stall warning devices give warnings well 425 | // before the lift goes to 0 to give the pilot time to correct. 426 | if (i<=0) 427 | { 428 | if (LiftCoefficient(fAttackAngle, Element[i].iFlap) == 0) 429 | Stalling = true; 430 | } 431 | 432 | // Keep a running total of these resultant forces (total force) 433 | Fb += vResultant; 434 | 435 | // Calculate the moment about the CG of this element's force 436 | // and keep a running total of these moments (total moment) 437 | vtmp = Element[i].vCGCoords^vResultant; 438 | Mb += vtmp; 439 | } 440 | 441 | // Now add the thrust 442 | Fb += Thrust; 443 | 444 | // Convert forces from model space to earth space 445 | Airplane.vForces = QVRotate(Airplane.qOrientation, Fb); 446 | 447 | // Apply gravity (g is defined as −32.174 ft/s^2) 448 | Airplane.vForces.z += g * Airplane.fMass; 449 | 450 | Airplane.vMoments += Mb; 451 | } 452 | 453 | 454 | ================== -------------------------------------------------------------------------------- /ch18.txt: -------------------------------------------------------------------------------- 1 | chapter: Guns and Explosions 2 | ================== 3 | alp = 90-Math.toDegrees(Math.atan(((200-aimY)*(targetH/(drawH)))/range)); 4 | gmm = Math.toDegrees(Math.atan(((200-aimX)*(targetH/(drawH)))/range)); 5 | 6 | 7 | ==================================== 8 | timer = new Timer(100, TargetPanel); 9 | timer.start(); 10 | 11 | 12 | ==================================== 13 | if (direction == true) { 14 | breathHeight = breathHeight + 1; 15 | if (breathHeight == 5) { 16 | direction = false; 17 | breathHeight = breathHeight + 1; 18 | } 19 | } 20 | 21 | if (direction == false) { 22 | breathHeight = breathHeight − 1; 23 | if (breathHeight == −5) { 24 | direction = true; 25 | } 26 | } 27 | 28 | if (breathing) { 29 | aimY = aimY + breathHeight; 30 | } 31 | 32 | 33 | ==================================== 34 | void CreateParticleExplosion(int x, int y, int Vinit, int life, 35 | float gravity, float angle) 36 | { 37 | int i; 38 | int m; 39 | float f; 40 | 41 | Explosion.Active = TRUE; 42 | Explosion.x = x; 43 | Explosion.y = y; 44 | Explosion.V0 = Vinit; 45 | 46 | for(i=0; i<_MAXPARTICLES; i++) 47 | { 48 | Explosion.p[i].x = 0; 49 | Explosion.p[i].y = 0; 50 | Explosion.p[i].vi = tb_Rnd(Vinit/2, Vinit); 51 | 52 | if(angle < 999) 53 | { 54 | if(tb_Rnd(0,1) == 0) 55 | m = −1; 56 | else 57 | m = 1; 58 | Explosion.p[i].angle = -angle + m * tb_Rnd(0,10); 59 | } else 60 | Explosion.p[i].angle = tb_Rnd(0,360); 61 | 62 | f = (float) tb_Rnd(80, 100) / 100.0f; 63 | Explosion.p[i].life = tb_Round(life * f); 64 | Explosion.p[i].r = 255;//tb_Rnd(225, 255); 65 | Explosion.p[i].g = 255;//tb_Rnd(85, 115); 66 | Explosion.p[i].b = 255;//tb_Rnd(15, 45); 67 | Explosion.p[i].time = 0; 68 | Explosion.p[i].Active = TRUE; 69 | Explosion.p[i].gravity = gravity; 70 | } 71 | 72 | } 73 | 74 | 75 | ==================================== 76 | typedef struct _TParticle 77 | { 78 | float x; // x-coordinate of the particle 79 | float y; // y-coordinate of the particle 80 | float vi; // initial velocity 81 | float angle; // initial trajectory (direction) 82 | int life; // duration in milliseconds 83 | int r; // red component of particle's color 84 | int g; // green component of particle's color 85 | int b; // blue component of particle's color 86 | int time; // keeps track of the effect's time 87 | float gravity; // gravity factor 88 | BOOL Active; // indicates whether this particle 89 | // is active or dead 90 | float mass; //for calculating the particle's energy 91 | } TParticle; 92 | 93 | #define _MAXPARTICLES 50 94 | #define _MASSOFPARTICLE .25 95 | 96 | typedef struct _TParticleExplosion 97 | { 98 | TParticle p[_MAXPARTICLES]; // list of particles 99 | // making up this effect 100 | int x; // initial x location 101 | int y; // initial y location 102 | float KE; //Available kinect energy 103 | float 104 | BOOL Active; // indicates whether this effect is 105 | //active or dead 106 | } TParticleExplosion; 107 | 108 | 109 | ==================================== 110 | void CreateParticleExplosion(int x, int y, int KEb, int life, 111 | float gravity, float angle) 112 | { 113 | int i; 114 | int m; 115 | float f; 116 | 117 | Explosion.Active = TRUE; 118 | Explosion.x = x; 119 | Explosion.y = y; 120 | Explosion.KE = KEb; 121 | 122 | for(i=0; i<_MAXPARTICLES; i++) 123 | { 124 | Explosion.p[i].x = 0; 125 | Explosion.p[i].y = 0; 126 | Explosion.p[i].m = _MASSOFPARTICLE; //Mass of a single gravel 127 | Explosion.p[i].vi = tb_Rnd(0, sqrt(Explosion.KE/(_MASSOFPARTICLE* 128 | _MAXPARTICLES)); 129 | Explosion.KE = Explosion.KE - ((1/2)*(Explosion.p[i].m)* 130 | (Explosion.p[i].vi)); 131 | 132 | if(angle < 999) 133 | { 134 | if(tb_Rnd(0,1) == 0) 135 | m = −1; 136 | else 137 | m = 1; 138 | Explosion.p[i].angle = -angle + m * tb_Rnd(0,10); 139 | } else 140 | Explosion.p[i].angle = tb_Rnd(0,360); 141 | 142 | f = (float) tb_Rnd(80, 100) / 100.0f; 143 | Explosion.p[i].life = tb_Round(life * f); 144 | Explosion.p[i].r = 255;//tb_Rnd(225, 255); 145 | Explosion.p[i].g = 255;//tb_Rnd(85, 115); 146 | Explosion.p[i].b = 255;//tb_Rnd(15, 45); 147 | Explosion.p[i].time = 0; 148 | Explosion.p[i].Active = TRUE; 149 | Explosion.p[i].gravity = gravity; 150 | } 151 | 152 | } 153 | 154 | 155 | ================== -------------------------------------------------------------------------------- /ch19.txt: -------------------------------------------------------------------------------- 1 | chapter: Sports 2 | ================== 3 | #define RADIANS(d) (d/180.0*3.14159) 4 | #define DEGREES(r) (r*180.0/3.14159) 5 | 6 | 7 | ==================================== 8 | // Variables 9 | double alpha = 0.0; 10 | double alpha_dot = 0.0; 11 | double alpha_dotdot = 0.0; 12 | double beta = RADIANS(120.0); 13 | double beta_dot = 0.0; 14 | double beta_dotdot = 0.0; 15 | 16 | double J = 1.15; // kg m^2 17 | double I = 0.08; // kg m^2 18 | double Mc = 0.4; // kg 19 | double R = 0.62; // m 20 | double L = 1.1; // m 21 | double S = 0.4*1.1*0.75; // kg m 22 | double g = 9.8; // m/s^2 23 | double gamma = RADIANS(135.0); 24 | double theta = gamma - alpha; 25 | double SA = 7.3*0.62*0.5; // kg m 26 | double Qalpha = 100; // N m 27 | double Qbeta = −10; // N m 28 | double a = 0.1*g; // m/s^2 29 | double dt = 0.0025; // s 30 | double time = 0; // s 31 | double Vc = 0; 32 | 33 | 34 | ==================================== 35 | double ComputeAlphaDotDot(void) 36 | { 37 | double A, B, C, D, F, G; 38 | double num, denom; 39 | 40 | A = (J + I + Mc * R * R + 2 * R * S * cos(beta)); 41 | B = -(I + R * S * cos(beta)); 42 | F = Qalpha - (beta_dot * beta_dot - 2 * alpha_dot * beta_dot) * R * S * 43 | sin(beta) + S * (g * sin(theta + beta) - a * cos(theta + beta)) 44 | + (SA + Mc * R) * (g * sin(theta) - a * cos(theta)); 45 | 46 | C = B; 47 | D = I; 48 | G = Qbeta - alpha_dot * alpha_dot * R * S * sin(beta) − 49 | S * (g * sin(theta + beta) - a * cos(theta + beta)); 50 | 51 | num = (F - (B * G / D)); 52 | denom = (A-(B*C/D)); 53 | return (F - (B * G / D)) / (A-(B*C/D)); 54 | } 55 | 56 | 57 | ==================================== 58 | double ComputeBetaDotDot(void) 59 | { 60 | double C, D, G; 61 | 62 | C = -(I + R * S * cos(beta)); 63 | D = I; 64 | G = Qbeta - alpha_dot * alpha_dot * R * S * sin(beta) − 65 | S * (g * sin(theta + beta) - a * cos(theta + beta)); 66 | 67 | return (G - C * alpha_dotdot) / D; 68 | } 69 | 70 | 71 | ==================================== 72 | int _tmain(int argc, _TCHAR* argv[]) 73 | { 74 | double a, at; 75 | double b, bt; 76 | int i; 77 | FILE* fp; 78 | double phi; 79 | double Vc2; 80 | 81 | double ak1, ak2, ak3, ak4; 82 | double bk1, bk2, bk3, bk4; 83 | 84 | FILE* fdebug; 85 | 86 | fp = fopen("results.txt", "w"); 87 | fdebug = fopen("debug.txt", "w"); 88 | 89 | for(i = 0; i<200; i++) 90 | { 91 | time += dt; 92 | 93 | if(time>=0.1) 94 | { 95 | Qbeta = 0; 96 | } 97 | 98 | // save results of previous time step 99 | a = alpha; 100 | b = beta; 101 | at = alpha_dot; 102 | bt = beta_dot; 103 | 104 | // integrate alpha'' and beta'' 105 | 106 | // The K1 Step: 107 | alpha_dotdot = ComputeAlphaDotDot(); 108 | beta_dotdot = ComputeBetaDotDot(); 109 | 110 | ak1 = alpha_dotdot * dt; 111 | bk1 = beta_dotdot * dt; 112 | 113 | alpha_dot = at + ak1/2; 114 | beta_dot = bt + bk1/2; 115 | 116 | // The K2 Step: 117 | alpha_dotdot = ComputeAlphaDotDot(); 118 | beta_dotdot = ComputeBetaDotDot(); 119 | 120 | ak2 = alpha_dotdot * dt; 121 | bk2 = beta_dotdot * dt; 122 | 123 | alpha_dot = at + ak2/2; 124 | beta_dot = bt + bk2/2; 125 | 126 | // The K3 Step: 127 | alpha_dotdot = ComputeAlphaDotDot(); 128 | beta_dotdot = ComputeBetaDotDot(); 129 | 130 | ak3 = alpha_dotdot * dt; 131 | bk3 = beta_dotdot * dt; 132 | 133 | alpha_dot = at + ak3; 134 | beta_dot = bt + bk3; 135 | 136 | // The K3 Step: 137 | alpha_dotdot = ComputeAlphaDotDot(); 138 | beta_dotdot = ComputeBetaDotDot(); 139 | 140 | ak4 = alpha_dotdot * dt; 141 | bk4 = beta_dotdot * dt; 142 | 143 | alpha_dot = at + (ak1 + 2*ak2 + 2*ak3 + ak4) / 6; 144 | beta_dot = bt + (bk1 + 2*bk2 + 2*bk3 + bk4) / 6; 145 | 146 | alpha = a + alpha_dot * dt; 147 | beta = b + beta_dot * dt; 148 | 149 | theta = gamma - alpha; 150 | Vc2 = (R*R + L*L + 2 * R * L * cos(beta)) * ( alpha_dot * alpha_dot) 151 | + L*L * beta_dot * beta_dot 152 | - 2 * (L*L + R * L * cos(beta)) * alpha_dot * beta_dot; 153 | 154 | Vc = sqrt(Vc2); 155 | 156 | phi = theta + beta; 157 | fprintf(fp, "%f, %f, %f, %f, %f, %f\n", time, DEGREES(theta), 158 | DEGREES(alpha), DEGREES(beta), DEGREES(phi), Vc); 159 | 160 | fprintf(fdebug, "%f, %f, %f, %f, %f, %f, %f\n", time, DEGREES(alpha), 161 | alpha_dot, alpha_dotdot, DEGREES(beta), beta_dot, beta_dotdot); 162 | } 163 | 164 | fclose(fp); 165 | fclose(fdebug); 166 | return 0; 167 | } 168 | 169 | 170 | ==================================== 171 | typedef struct _RigidBody { 172 | float fMass; // Total mass (constant) 173 | Matrix3x3 mInertia; // Mass moment of inertia in body coordinates 174 | Matrix3x3 mInertiaInverse;// Inverse of mass moment of inertia matrix 175 | Vector vPosition; // Position in earth coordinates 176 | Vector vVelocity; // Velocity in earth coordinates 177 | Vector vVelocityBody; // Velocity in body coordinates 178 | Vector vAcceleration; // Acceleration of cg in earth space 179 | Vector vAngularAcceleration; //Angular acceleration in body coordinates 180 | Vector vAngularAccelerationGlobal; // Angular acceleration 181 | // in global coordinates 182 | Vector vAngularVelocity; // Angular velocity in body coordinates 183 | Vector vAngularVelocityGlobal; // Angular velocity in global coordinates 184 | Vector vEulerAngles; // Euler angles in body coordinates 185 | float fSpeed; // Speed (magnitude of the velocity) 186 | Quaternion qOrientation; // Orientation in earth coordinates 187 | Vector vForces; // Total force on body 188 | Vector vMoments; // Total moment (torque) on body 189 | Matrix3x3 mIeInverse; // Inverse of moment of inertia in earth coordinates 190 | float fRadius; // Ball radius 191 | } RigidBody, *pRigidBody; 192 | 193 | 194 | ==================================== 195 | typedef struct _Collision { 196 | int body1; 197 | int body2; 198 | Vector vCollisionNormal; 199 | Vector vCollisionPoint; 200 | Vector vRelativeVelocity; 201 | Vector vRelativeAcceleration; 202 | Vector vCollisionTangent; 203 | } Collision, *pCollision; 204 | 205 | 206 | ==================================== 207 | #define BALLDIAMETER 0.05715f 208 | #define BALLWEIGHT 1.612f 209 | #define GRAVITY −9.87f 210 | #define LINEARDRAGCOEFFICIENT 0.5f 211 | #define ANGULARDRAGCOEFFICIENT 0.05f 212 | #define FRICTIONFACTOR 0.5f 213 | #define COEFFICIENTOFRESTITUTION 0.8f 214 | #define COEFFICIENTOFRESTITUTIONGROUND 0.1f 215 | #define FRICTIONCOEFFICIENTBALLS 0.1f 216 | #define FRICTIONCOEFFICIENTGROUND 0.1f 217 | #define ROLLINGRESISTANCECOEFFICIENT 0.025f 218 | 219 | 220 | ==================================== 221 | RigidBody Bodies[NUMBODIES]; 222 | Collision Collisions[NUMBODIES*8]; 223 | int NumCollisions = 0; 224 | 225 | 226 | ==================================== 227 | void InitializeObjects(int configuration) 228 | { 229 | float iRoll, iPitch, iYaw; 230 | int i; 231 | float Ixx, Iyy, Izz; 232 | float s; 233 | 234 | ///////////////////////////////////////////////////// 235 | // Initialize the cue ball: 236 | // Set initial position 237 | Bodies[0].vPosition.x = -BALLDIAMETER*50.0f; 238 | Bodies[0].vPosition.y = 0.0f; 239 | Bodies[0].vPosition.z = BALLDIAMETER/2.0f; 240 | 241 | // Set initial velocity 242 | s = 7.0; 243 | Bodies[0].vVelocity.x = s; 244 | Bodies[0].vVelocity.y = 0.0f; 245 | Bodies[0].vVelocity.z = 0.0f; 246 | Bodies[0].fSpeed = s; 247 | 248 | // Set initial angular velocity 249 | Bodies[0].vAngularVelocity.x = 0.0f; // rotate about long'l axis 250 | Bodies[0].vAngularVelocity.y = 0.0f; // rotate about transverse axis 251 | Bodies[0].vAngularVelocity.z = 0.0f; // rotate about vertical axis 252 | 253 | Bodies[0].vAngularAcceleration.x = 0.0f; 254 | Bodies[0].vAngularAcceleration.y = 0.0f; 255 | Bodies[0].vAngularAcceleration.z = 0.0f; 256 | 257 | Bodies[0].vAcceleration.x = 0.0f; 258 | Bodies[0].vAcceleration.y = 0.0f; 259 | Bodies[0].vAcceleration.z = 0.0f; 260 | 261 | // Set the initial forces and moments 262 | Bodies[0].vForces.x = 0.0f; 263 | Bodies[0].vForces.y = 0.0f; 264 | Bodies[0].vForces.z = 0.0f; 265 | 266 | Bodies[0].vMoments.x = 0.0f; 267 | Bodies[0].vMoments.y = 0.0f; 268 | Bodies[0].vMoments.z = 0.0f; 269 | 270 | // Zero the velocity in body space coordinates 271 | Bodies[0].vVelocityBody.x = 0.0f; 272 | Bodies[0].vVelocityBody.y = 0.0f; 273 | Bodies[0].vVelocityBody.z = 0.0f; 274 | 275 | // Set the initial orientation 276 | iRoll = 0.0f; 277 | iPitch = 0.0f; 278 | iYaw = 0.0f; 279 | Bodies[0].qOrientation = MakeQFromEulerAngles(iRoll, iPitch, iYaw); 280 | 281 | // Set the mass properties 282 | Bodies[0].fMass = BALLWEIGHT/(-g); 283 | 284 | Ixx = 2.0f * Bodies[0].fMass / 5.0f * (BALLDIAMETER/2*BALLDIAMETER/2); 285 | Izz = Iyy = Ixx; 286 | 287 | Bodies[0].mInertia.e11 = Ixx; 288 | Bodies[0].mInertia.e12 = 0; 289 | Bodies[0].mInertia.e13 = 0; 290 | Bodies[0].mInertia.e21 = 0; 291 | Bodies[0].mInertia.e22 = Iyy; 292 | Bodies[0].mInertia.e23 = 0; 293 | Bodies[0].mInertia.e31 = 0; 294 | Bodies[0].mInertia.e32 = 0; 295 | Bodies[0].mInertia.e33 = Izz; 296 | 297 | Bodies[0].mInertiaInverse = Bodies[0].mInertia.Inverse(); 298 | 299 | Bodies[0].fRadius = BALLDIAMETER/2; 300 | 301 | 302 | ///////////////////////////////////////////////////// 303 | // Initialize the other balls 304 | for(i=1; i VELOCITYTOLERANCE) 548 | { 549 | // Kinetic: 550 | FrictionForce = (ContactForce.Magnitude() * 551 | FRICTIONCOEFFICIENTGROUND) * 552 | Collisions[j].vCollisionTangent; 553 | } else { 554 | // Static: 555 | FrictionForce = (ContactForce.Magnitude() * 556 | FRICTIONCOEFFICIENTGROUND * 2 * 557 | vt/VELOCITYTOLERANCE) * 558 | Collisions[j].vCollisionTangent; 559 | } 560 | 561 | } else 562 | FrictionForce.x = FrictionForce.y = FrictionForce.z = 0; 563 | 564 | // Do rolling resistance: 565 | if(Bodies[i].vAngularVelocity.Magnitude() > VELOCITYTOLERANCE) 566 | { 567 | FRn = ContactForce.Magnitude() * 568 | Collisions[j].vCollisionNormal; 569 | Collisions[j].vCollisionTangent.Normalize(); 570 | Vector m = (Collisions[j].vCollisionTangent 571 | *(ROLLINGRESISTANCECOEFFICIENT * 572 | Bodies[i].fRadius))^FRn; 573 | double mag = m.Magnitude(); 574 | Vector a = Bodies[i].vAngularVelocity; 575 | a.Normalize(); 576 | Bodies[i].vMoments += -a * mag; 577 | } 578 | 579 | // accumlate contact and friction forces and moments 580 | Bodies[i].vForces += ContactForce; 581 | Bodies[i].vForces += FrictionForce; 582 | 583 | ContactForce = QVRotate(~Bodies[i].qOrientation, ContactForce); 584 | FrictionForce = QVRotate(~Bodies[i].qOrientation, 585 | FrictionForce); 586 | pt = Collisions[j].vCollisionPoint - Bodies[i].vPosition; 587 | pt = QVRotate(~Bodies[i].qOrientation, pt); 588 | Bodies[i].vMoments += pt^ContactForce; 589 | Bodies[i].vMoments += pt^FrictionForce; 590 | 591 | } 592 | } 593 | 594 | } 595 | } 596 | 597 | 598 | ==================================== 599 | Bodies[i].vAcceleration = Bodies[i].vForces / Bodies[i].fMass; 600 | 601 | ContactForce = (Bodies[i].fMass * (-Bodies[i].vAcceleration * 602 | Collisions[j].vCollisionNormal)) * 603 | Collisions[j].vCollisionNormal; 604 | 605 | 606 | ==================================== 607 | ContactForce = (Bodies[i].fMass * (-Bodies[i].vAcceleration * 608 | Collisions[j].vCollisionNormal)) * 609 | Collisions[j].vCollisionNormal; 610 | 611 | double vt = fabs(Collisions[j].vRelativeVelocity * 612 | Collisions[j].vCollisionTangent); 613 | if(vt > VELOCITYTOLERANCE) 614 | { 615 | // Kinetic: 616 | FrictionForce = (ContactForce.Magnitude() * 617 | FRICTIONCOEFFICIENTGROUND) * 618 | Collisions[j].vCollisionTangent; 619 | } else { 620 | // Static: 621 | FrictionForce = (ContactForce.Magnitude() * 622 | FRICTIONCOEFFICIENTGROUND * 2 * 623 | vt/VELOCITYTOLERANCE) * 624 | Collisions[j].vCollisionTangent; 625 | } 626 | 627 | 628 | ==================================== 629 | // accumlate contact and friction forces and moments 630 | Bodies[i].vForces += ContactForce; 631 | Bodies[i].vForces += FrictionForce; 632 | 633 | ContactForce = QVRotate(~Bodies[i].qOrientation, ContactForce); 634 | FrictionForce = QVRotate(~Bodies[i].qOrientation, 635 | FrictionForce); 636 | pt = Collisions[j].vCollisionPoint - Bodies[i].vPosition; 637 | pt = QVRotate(~Bodies[i].qOrientation, pt); 638 | Bodies[i].vMoments += pt^ContactForce; 639 | Bodies[i].vMoments += pt^FrictionForce; 640 | 641 | 642 | ==================================== 643 | // Do rolling resistance: 644 | if(Bodies[i].vAngularVelocity.Magnitude() > VELOCITYTOLERANCE) 645 | { 646 | FRn = ContactForce.Magnitude() * 647 | Collisions[j].vCollisionNormal; 648 | Collisions[j].vCollisionTangent.Normalize(); 649 | Vector m = (Collisions[j].vCollisionTangent 650 | *(ROLLINGRESISTANCECOEFFICIENT * 651 | Bodies[i].fRadius))^FRn; 652 | double mag = m.Magnitude(); 653 | Vector a = Bodies[i].vAngularVelocity; 654 | a.Normalize(); 655 | Bodies[i].vMoments += -a * mag; 656 | } 657 | 658 | 659 | ==================================== 660 | int CheckForCollisions(void) 661 | { 662 | int status = NOCOLLISION; 663 | int i, j; 664 | Vector d; 665 | pCollision pCollisionData; 666 | int check = NOCOLLISION; 667 | float r; 668 | float s; 669 | Vector tmp; 670 | 671 | FlushCollisionData(); 672 | pCollisionData = Collisions; 673 | NumCollisions = 0; 674 | 675 | // check object collisions with each other 676 | for(i=0; ibody1 = i; 714 | pCollisionData->body2 = j; 715 | pCollisionData->vCollisionNormal = n; 716 | pCollisionData->vCollisionPoint = tmp; 717 | pCollisionData->vRelativeVelocity = Vr; 718 | pCollisionData->vCollisionTangent = (n^Vr)^n; 719 | pCollisionData->vCollisionTangent.Normalize(); 720 | 721 | pCollisionData++; 722 | NumCollisions++; 723 | status = COLLISION; 724 | } } 725 | } 726 | } 727 | } 728 | 729 | for(i=0; ibody1 = body1; 801 | CollisionData->body2 = −1; 802 | CollisionData->vCollisionNormal = n; 803 | CollisionData->vCollisionPoint = tmp; 804 | CollisionData->vRelativeVelocity = Vr; 805 | 806 | CollisionData->vCollisionTangent = (n^Vr)^n; 807 | CollisionData->vCollisionTangent.Reverse(); 808 | 809 | CollisionData->vCollisionTangent.Normalize(); 810 | status = COLLISION; 811 | } 812 | } 813 | } 814 | 815 | return status; 816 | } 817 | 818 | 819 | ==================================== 820 | void ResolveCollisions(void) 821 | { 822 | int i; 823 | double j; 824 | Vector pt1, pt2, vB1V, vB2V, vB1AV, vB2AV; 825 | float fCr = COEFFICIENTOFRESTITUTION; 826 | int b1, b2; 827 | float Vrt; 828 | float muB = FRICTIONCOEFFICIENTBALLS; 829 | float muG = FRICTIONCOEFFICIENTGROUND; 830 | bool dofriction = DOFRICTION; 831 | 832 | for(i=0; i 0.0 && dofriction) { 859 | Bodies[b1].vVelocity += 860 | ((j * Collisions[i].vCollisionNormal) + 861 | ((muB * j) * Collisions[i].vCollisionTangent)) / 862 | Bodies[b1].fMass; 863 | Bodies[b1].vAngularVelocityGlobal += 864 | (pt1 ^ ((j * Collisions[i].vCollisionNormal) + 865 | ((muB * j) * Collisions[i].vCollisionTangent))) * 866 | Bodies[b1].mIeInverse; 867 | Bodies[b1].vAngularVelocity = 868 | QVRotate(~Bodies[b1].qOrientation, 869 | Bodies[b1].vAngularVelocityGlobal); 870 | 871 | Bodies[b2].vVelocity += 872 | ((-j * Collisions[i].vCollisionNormal) + ((muB * 873 | j) * Collisions[i].vCollisionTangent)) / 874 | Bodies[b2].fMass; 875 | Bodies[b2].vAngularVelocityGlobal += 876 | (pt2 ^ ((-j * Collisions[i].vCollisionNormal) + 877 | ((muB * j) * Collisions[i].vCollisionTangent))) 878 | * Bodies[b2].mIeInverse; 879 | 880 | Bodies[b2].vAngularVelocity = 881 | QVRotate(~Bodies[b2].qOrientation, 882 | Bodies[b2].vAngularVelocityGlobal); 883 | 884 | } else { 885 | // Apply impulse: 886 | Bodies[b1].vVelocity += 887 | (j * Collisions[i].vCollisionNormal) / 888 | Bodies[b1].fMass; 889 | Bodies[b1].vAngularVelocityGlobal += 890 | (pt1 ^ (j * Collisions[i].vCollisionNormal)) * 891 | Bodies[b1].mIeInverse; 892 | Bodies[b1].vAngularVelocity = 893 | QVRotate(~Bodies[b1].qOrientation, 894 | Bodies[b1].vAngularVelocityGlobal); 895 | 896 | Bodies[b2].vVelocity -= 897 | (j * Collisions[i].vCollisionNormal) / 898 | Bodies[b2].fMass; 899 | Bodies[b2].vAngularVelocityGlobal -= 900 | (pt2 ^ (j * Collisions[i].vCollisionNormal)) * 901 | Bodies[b2].mIeInverse; 902 | Bodies[b2].vAngularVelocity = 903 | QVRotate(~Bodies[b2].qOrientation, 904 | Bodies[b2].vAngularVelocityGlobal); 905 | } 906 | 907 | } else { // Ground plane: 908 | fCr = COEFFICIENTOFRESTITUTIONGROUND; 909 | pt1 = Collisions[i].vCollisionPoint - Bodies[b1].vPosition; 910 | 911 | // Calculate impulse: 912 | j = (-(1+fCr) * (Collisions[i].vRelativeVelocity * 913 | Collisions[i].vCollisionNormal)) / 914 | ( (1/Bodies[b1].fMass) + 915 | (Collisions[i].vCollisionNormal * 916 | ( ( (pt1 ^ Collisions[i].vCollisionNormal) * 917 | Bodies[b1].mIeInverse )^pt1))); 918 | 919 | Vrt = Collisions[i].vRelativeVelocity * 920 | Collisions[i].vCollisionTangent; 921 | 922 | if(fabs(Vrt) > 0.0 && dofriction) { 923 | Bodies[b1].vVelocity += 924 | ( (j * Collisions[i].vCollisionNormal) + ((muG * 925 | j) * Collisions[i].vCollisionTangent) ) / 926 | Bodies[b1].fMass; 927 | Bodies[b1].vAngularVelocityGlobal += 928 | (pt1 ^ ((j * Collisions[i].vCollisionNormal) + 929 | ((muG * j) * Collisions[i].vCollisionTangent))) * 930 | Bodies[b1].mIeInverse; 931 | Bodies[b1].vAngularVelocity = 932 | QVRotate(~Bodies[b1].qOrientation, 933 | Bodies[b1].vAngularVelocityGlobal); 934 | } else { 935 | // Apply impulse: 936 | Bodies[b1].vVelocity += 937 | (j * Collisions[i].vCollisionNormal) / 938 | Bodies[b1].fMass; 939 | Bodies[b1].vAngularVelocityGlobal += 940 | (pt1 ^ (j * Collisions[i].vCollisionNormal)) * 941 | Bodies[b1].mIeInverse; 942 | Bodies[b1].vAngularVelocity = 943 | QVRotate(~Bodies[b1].qOrientation, 944 | Bodies[b1].vAngularVelocityGlobal); 945 | } 946 | } 947 | } 948 | } 949 | } 950 | 951 | 952 | ==================================== 953 | int CheckGroundPlaneContacts(pCollision CollisionData, int body1) 954 | { 955 | 956 | Vector v1[8]; 957 | Vector tmp; 958 | Vector u, v; 959 | Vector f[4]; 960 | Vector vel1; 961 | Vector pt1; 962 | Vector Vr; 963 | float Vrn; 964 | Vector n; 965 | int status = NOCOLLISION; 966 | Vector Ar; 967 | float Arn; 968 | 969 | if(Bodies[body1].vPosition.z <= (Bodies[body1].fRadius + COLLISIONTOLERANCE)) 970 | { 971 | pt1 = Bodies[body1].vPosition; 972 | pt1.z = COLLISIONTOLERANCE; 973 | tmp = pt1; 974 | pt1 = pt1-Bodies[body1].vPosition; 975 | vel1 = Bodies[body1].vVelocity/*Body*/ + 976 | (Bodies[body1].vAngularVelocityGlobal^pt1); 977 | 978 | n.x = 0; 979 | n.y = 0; 980 | n.z = 1; 981 | 982 | Vr = vel1; 983 | Vrn = Vr * n; 984 | 985 | if(fabs(Vrn) <= VELOCITYTOLERANCE) // at rest 986 | { 987 | // Check the relative acceleration: 988 | Ar = Bodies[body1].vAcceleration + 989 | (Bodies[body1].vAngularVelocityGlobal ^ 990 | (Bodies[body1].vAngularVelocityGlobal^pt1)) + 991 | (Bodies[body1].vAngularAccelerationGlobal ^ pt1); 992 | 993 | Arn = Ar * n; 994 | 995 | if(Arn <= 0.0f) 996 | { 997 | // We have a contact so fill the data structure 998 | assert(NumCollisions < (NUMBODIES*8)); 999 | if(NumCollisions < (NUMBODIES*8)) 1000 | { 1001 | CollisionData->body1 = body1; 1002 | CollisionData->body2 = −1; 1003 | CollisionData->vCollisionNormal = n; 1004 | CollisionData->vCollisionPoint = tmp; 1005 | CollisionData->vRelativeVelocity = Vr; 1006 | CollisionData->vRelativeAcceleration = Ar; 1007 | 1008 | CollisionData->vCollisionTangent = (n^Vr)^n; 1009 | CollisionData->vCollisionTangent.Reverse(); 1010 | 1011 | CollisionData->vCollisionTangent.Normalize(); 1012 | CollisionData++; 1013 | NumCollisions++; 1014 | status = CONTACT; 1015 | } 1016 | } 1017 | } 1018 | } 1019 | 1020 | return status; 1021 | } 1022 | 1023 | 1024 | ================== -------------------------------------------------------------------------------- /ch20.txt: -------------------------------------------------------------------------------- 1 | chapter: Touch Screens 2 | ================== 3 | - (void)touchesBegan:(NSSet *)touches withEvent:(UIEvent *)event 4 | { 5 | UITouch* touch = [[event touchesForView:self] anyObject]; 6 | firstTouch = [touch locationInView:self]; 7 | self.status = YES; 8 | [self trigger]; 9 | } 10 | 11 | 12 | ==================================== 13 | - (void)storeTouchPoints:(NSSet *)touches{ 14 | if ([touches count] > 0) { 15 | for (UITouch *touch in touches) { 16 | CGPoint *point = (CGPoint 17 | *)CFDictionaryGetValue(touchBeginPoints, touch); 18 | if (point == NULL) { 19 | point = (CGPoint *)malloc(sizeof(CGPoint)); 20 | CFDictionarySetValue(touchBeginPoints, touch, point); 21 | } 22 | *point = [touch locationInView:view.superview]; 23 | } 24 | } 25 | 26 | 27 | ================== -------------------------------------------------------------------------------- /ch21.txt: -------------------------------------------------------------------------------- 1 | chapter: Accelerometers 2 | ================== 3 | #define PI 3.14159 4 | 5 | float find2dAngle(void){ 6 | 7 | //LOCAL VARIABLES 8 | float alpha, 9 | double ax, ay; 10 | 11 | //POLL ACCELEROMETER FOR ACCELERATIONS, API SPECIFIC 12 | ax = getXacceleration(); 13 | ay = getYacceleration(); 14 | 15 | //FIND ANGLE 16 | alpha = atan2(ay,ax); 17 | 18 | if (alpha >= 0){ 19 | alpha = alpha * (180/PI); 20 | else { 21 | alpha = alpha * (-180/PI) + 180; 22 | } 23 | 24 | return alpha; 25 | } 26 | 27 | 28 | ==================================== 29 | - (void)viewDidLoad 30 | { 31 | UIAccelerometer *accelerometer = [UIAccelerometer sharedAccelerometer]; 32 | accelerometer.delegate = self; 33 | accelerometer.updateInterval = kPollingRate; 34 | [super viewDidLoad]; 35 | } 36 | 37 | 38 | ==================================== 39 | - (void)accelerometer:(UIAccelerometer *)accelerometer 40 | didAccelerate:(UIAcceleration *)acceleration{ 41 | [(SpriteView *)self.view setAcceleration:acceleration]; 42 | [(SpriteView *)self.view draw]; 43 | } 44 | 45 | 46 | ==================================== 47 | -(id)initWithCoder:(NSCoder *)coder { 48 | if((self = [super initWithCoder:coder])){ 49 | self.sprite = [UIImage imageNamed:@"sprite.png"]; 50 | self.currentPos = CGPointMake((self.bounds.size.width / 2.0f) + 51 | (sprite.size.width / 2.0f), (self.bounds.size.height /2.0f)+(sprite.size.height /2.0f)); 52 | xVelocity = 0.0f; 53 | yVelcoity = 0.0f; 54 | 55 | convertX = self.bounds.size.width / kWorldWidth; 56 | convertY = self.bounds.size.height / kWorldHeight; 57 | 58 | } 59 | return self; 60 | } 61 | 62 | 63 | ==================================== 64 | - (void)setCurrentPos:(CGPoint)newPos { 65 | prevPos = currentPos; 66 | currentPos = newPos; 67 | 68 | if(currentPos.x <0){ 69 | currentPos.x = 0; 70 | xVelocity = 0.0f; 71 | } 72 | if(currentPos.y <0){ 73 | currentPos.y = 0; 74 | yVelcoity = 0.0f; 75 | } 76 | if(currentPos.x > self.bounds.size.width - sprite.size.width){ 77 | currentPos.x = self.bounds.size.width - sprite.size.width; 78 | xVelocity = 0.0f; 79 | } 80 | if(currentPos.y > self.bounds.size.height - sprite.size.height){ 81 | currentPos.y = self.bounds.size.height - sprite.size.height; 82 | yVelocity = 0.0f; 83 | } 84 | 85 | CGRect curSpriteRect = CGRectMake(currentPos.x, currentPos.y, 86 | currentPos.x+sprite.size.width, currentPos.y+sprite.size.height); 87 | CGRect prevSpriteRect = CGRectMake(prevPos.x, prevPos.y, 88 | prevPos.x+sprite.size.width, currentPos.y+sprite.size.height); 89 | [self setNeedsDisplayInRect:CGRectUnion(curSpriteRect, prevSpriteRect)]; 90 | 91 | } 92 | 93 | 94 | ==================================== 95 | - (void)draw { 96 | static NSDate *lastUpdateTime; 97 | 98 | if (lastUpdateTime != nil) { 99 | NSTimeInterval secondsSinceUpdate = -([lastUpdateTime 100 | timeIntervalSinceNow]); //calculates interval in seconds from last update 101 | 102 | //Calculate displacement 103 | CGFloat deltaX = xVelocity * secondsSinceUpdate + 104 | ((acceleration.x*g*secondsSinceUpdate*secondsSinceUpdate)/2); // METERS 105 | CGFloat deltaY = yVelocity * secondsSinceUpdate + 106 | ((acceleration.y*g*secondsSinceUpdate*secondsSinceUpdate)/2); // METERS 107 | 108 | //Converts from meters to pixels based on defined World size 109 | deltaX = deltaX * convertX; 110 | deltaY = deltaY * convertY; 111 | 112 | //Calculate new velocity at new current position 113 | xVelocity = xVelocity + acceleration.x * g * secondsSinceUpdate; //assumes 114 | acceleration was constant over last update interval 115 | yVelocity = yVelocity - (acceleration.y * g * secondsSinceUpdate); //assumes 116 | acceleration was constant over last update interval 117 | 118 | //Mutate currentPos which will update screen 119 | self.currentPos = CGPointMake(self.currentPos.x + deltaX, 120 | self.currentPos.y + deltaY); 121 | 122 | } 123 | 124 | [lastUpdateTime release]; 125 | lastUpdateTime = [[NSDate alloc] init]; 126 | 127 | } 128 | 129 | 130 | ==================================== 131 | CGFloat deltaX = xVelocity * secondsSinceUpdate + 132 | ((acceleration.x*g*secondsSinceUpdate*secondsSinceUpdate)/2); // METERS 133 | 134 | 135 | ==================================== 136 | xVelocity = xVelocity + acceleration.x * g * secondsSinceUpdate; 137 | 138 | 139 | ================== -------------------------------------------------------------------------------- /ch22.txt: -------------------------------------------------------------------------------- 1 | chapter: Gaming from One Place to Another 2 | ================== 3 | typedef enum { 4 | float lat; 5 | float lon; 6 | } Coordinate2D; 7 | 8 | 9 | ==================================== 10 | float distanceGreatCircle(Coordinate2D startPoint, Coordinate2D endPoint){ 11 | 12 | //Convert location from degrees to radians 13 | float lat1 = (M_PI/180.) * startPoint.lat; 14 | float lon1 = (M_PI/180.) * endPoint.longi; 15 | float lat2 = (M_PI/180.) * endPoint.lat; 16 | float lon2 = (M_PI/180.) * endPoint.longi; 17 | 18 | //Calculate deltas 19 | float dLat = lat2 - lat1; 20 | float dLon = lon2 - lon1; 21 | 22 | //Calculate half chord legnth 23 | float a = sin(dLat/2) * sin(dLat/2) + cos(lat1) * cos(lat2) * sin(dLon/2) 24 | * sin(dLon/2); 25 | 26 | //Calculate angular distance 27 | float C = 2 * atan(sqrt(a)/sqrt(1-a)); 28 | 29 | //Find arclength 30 | float distance = 6371 * C; //6371 is radius of earth in km 31 | return distance; 32 | } 33 | 34 | 35 | ==================================== 36 | float initialBearing (Coordinate2D startPoint, Coordinate2D endPoint){ 37 | //Convert location from degrees to radians 38 | float lat1 = (M_PI/180.) * startPoint.lat; 39 | float lon1 = (M_PI/180.) * startPoint.longi; 40 | float lat2 = (M_PI/180.) * endPoint.lat; 41 | float lon2 = (M_PI/180.) * endPoint.longi; 42 | 43 | //Calculate deltas 44 | float dLat = lat2 - lat1; 45 | float dLon = lon2 - lon1; 46 | 47 | // Calculate bearing in radians 48 | float theta = atan2f( sin(dlon) * cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2) 49 | *cos(dlon)); 50 | 51 | //Convert to compass bearing 52 | Float bearing = theta * (180 / M_PI); //radians to degrees 53 | bearing = ( bearing > 0 ? bearing : (360.0 + bearing)); //fix range 54 | return bearing; 55 | } 56 | 57 | 58 | ==================================== 59 | float finalBearing (Coordinate2D startPoint, Coordinate2D endPoint){ 60 | //Convert location from degrees to radians 61 | float lat1 = (M_PI/180.) * endPoint.lat; 62 | float lon1 = (M_PI/180.) * endPoint.longi; 63 | float lat2 = (M_PI/180.) * startPoint.lat; 64 | float lon2 = (M_PI/180.) * startPoint.longi; 65 | 66 | //Calculate deltas 67 | float dLat = lat2 - lat1; 68 | float dLon = lon2 - lon1; 69 | 70 | //Calculate bearing in radians 71 | float theta = atan2f( sin(dlon) * cos(lat2), cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2) 72 | *cos(dlon)); 73 | 74 | //Convert to compass bearing 75 | float bearing = theta * (180 / M_PI); //radians to degrees 76 | bearing = ( bearing > 0 ? bearing : (360.0 + bearing)); //fix range 77 | bearing = ((bearing + 180) % 360) //reverse heading 78 | return bearing; 79 | } 80 | 81 | 82 | ==================================== 83 | float rhumbBearing ( Coordinate2D startPoint, Coordinate2D endPoint){ 84 | //Convert location from degrees to radians 85 | float lat1 = (M_PI/180.) * startPoint.lat; 86 | float lon1 = (M_PI/180.) * startPoint.longi; 87 | float lat2 = (M_PI/180.) * endPoint.lat; 88 | float lon2 = (M_PI/180.) * endPoint.longi; 89 | 90 | //Calculate deltas 91 | float dLat = lat2 - lat1; 92 | float dLon = lon2 - lon1; 93 | 94 | //find delta phi 95 | float deltaPhi = log(tan(lat2/2+(M_PI)/4)/tan(lat1+M_PI/4)) 96 | float q=(deltaPhi==0 ? dlat/deltaPhi : cos(lat1); //avoids division by 0 97 | 98 | if (abs(dLon) > M_PI){ 99 | dLon = (dLon>0 ? −(2*(M_PI-dLon):(2*M_PI+dLon)); 100 | } 101 | 102 | float D = sqrt(dLat*dLat + q*q*dLon*dLo)* 6371; 103 | float theta = atan2f(dLon, deltaPhi); 104 | 105 | //now convert to compass heading 106 | float bearing = theta * (180 / M_PI); //radians to degrees 107 | bearing = ( bearing > 0 ? bearing : (360.0 + bearing)); //fix range 108 | 109 | return bearing; 110 | } 111 | 112 | 113 | ================== -------------------------------------------------------------------------------- /ch23.txt: -------------------------------------------------------------------------------- 1 | chapter: Pressure Sensors and Load Cells 2 | ================== 3 | float altitude_difference = 4 | getAltitude(SensorManager.PRESSURE_STANDARD_ATMOSPHERE, 5 | pressure_at_point2) - 6 | getAltitude(SensorManager.PRESSURE_STANDARD_ATMOSPHERE, 7 | pressure_at_point1); 8 | 9 | 10 | ================== -------------------------------------------------------------------------------- /ch25.txt: -------------------------------------------------------------------------------- 1 | chapter: Optical Tracking 2 | ================== 3 | {(x[i],y[i],z[i],t[i]),(x[i+1],y[i+1],z[i+1],t[i+1]) , 4 | (x[i+2],y[i+2],z[i+2],t[i+2]), ...} 5 | 6 | 7 | ==================================== 8 | Vector findVelocity (x[i-1], y[i-1], z[i-1], t[i-1], x[i], y[i], z[i], t[i]){ 9 | 10 | float vx, vy, vz; 11 | 12 | vx = (x[i] − x[i-1])/(t[i]-t[i-1]); 13 | vy = (y[i] − y[i-1])/(t[i]-t[i-1]); 14 | vz = (z[i] − z[i-1])/(t[i]-t[i-1]); 15 | 16 | vector velocity = {vx, vy, vz}; 17 | 18 | return velocity; 19 | 20 | } 21 | 22 | 23 | ==================================== 24 | Vector findAcceleration (x[i-2], y[i-2], z[i-2], t[i-2], x[i-1], y[i-1], z[i-1], 25 | t[i-1], x[i], y[i], z[i], t[i] ){ 26 | 27 | float ax, ay, az, h; 28 | vector acceleration; 29 | 30 | h = t[i]-t[i-1]; 31 | 32 | ax = (x[i] − 2*x[i-1] + x[i-2]) / h; 33 | ay = (y[i] − 2*y[i-1] + y[i-2]) / h; 34 | az = (z[i] − 2*z[i-1] + z[i-2]) / h; 35 | 36 | return acceleration = {ax, ay, az}; 37 | } 38 | 39 | 40 | ================== -------------------------------------------------------------------------------- /ch26.txt: -------------------------------------------------------------------------------- 1 | chapter: Sound 2 | ================== 3 | int main() 4 | { 5 | ALuint uiBuffer; 6 | ALuint uiSource; 7 | ALint iState; 8 | 9 | // Initialize Framework 10 | ALFWInit(); 11 | 12 | if (!ALFWInitOpenAL()) 13 | { 14 | ALFWprintf("Failed to initialize OpenAL\n"); 15 | ALFWShutdown(); 16 | return 0; 17 | } 18 | 19 | // Generate an AL Buffer 20 | alGenBuffers( 1, &uiBuffer ); 21 | 22 | // Load Wave file into OpenAL Buffer 23 | if (!ALFWLoadWaveToBuffer((char*)ALFWaddMediaPath(TEST_WAVE_FILE), uiBuffer)) 24 | { 25 | ALFWprintf("Failed to load %s\n", ALFWaddMediaPath(TEST_WAVE_FILE)); 26 | } 27 | 28 | // Specify the location of the Listener 29 | alListener3f(AL_POSITION, 0, 0, 0); 30 | 31 | // Generate a Source to playback the Buffer 32 | alGenSources( 1, &uiSource ); 33 | 34 | // Attach Source to Buffer 35 | alSourcei( uiSource, AL_BUFFER, uiBuffer ); 36 | 37 | // Set the Doppler effect factor 38 | alDopplerFactor(10); 39 | 40 | // Initialize variables used to reposition the source 41 | float x = 75; 42 | float y = 0; 43 | float z = −10; 44 | float dx = −1; 45 | float dy = 0.1; 46 | float dz = 0.25; 47 | 48 | // Set Initial Source properties 49 | alSourcei(uiSource, AL_LOOPING, AL_TRUE); 50 | alSource3f(uiSource, AL_POSITION, x, y, z); 51 | alSource3f(uiSource, AL_VELOCITY, dx, dy, dz); 52 | 53 | // Play Source 54 | alSourcePlay( uiSource ); 55 | 56 | do 57 | { 58 | Sleep(100); 59 | 60 | if(fabs(x) > 75) dx = -dx; 61 | if(fabs(y) > 5) dy = -dy; 62 | if(fabs(z) > 10) dz = -dz; 63 | alSource3f(uiSource, AL_VELOCITY, dx, dy, dz); 64 | 65 | x += dx; 66 | y += dy; 67 | z += dz; 68 | alSource3f(uiSource, AL_POSITION, x, y, z); 69 | 70 | // Get Source State 71 | alGetSourcei( uiSource, AL_SOURCE_STATE, &iState); 72 | } while (iState == AL_PLAYING); 73 | 74 | // Clean up by deleting Source(s) and Buffer(s) 75 | alSourceStop(uiSource); 76 | alDeleteSources(1, &uiSource); 77 | alDeleteBuffers(1, &uiBuffer); 78 | 79 | ALFWShutdownOpenAL(); 80 | 81 | ALFWShutdown(); 82 | 83 | return 0; 84 | } 85 | 86 | 87 | ================== --------------------------------------------------------------------------------