Mercurial > projects > chipmunkd
annotate trunk/chipmunkd/cpArbiter.d @ 28:4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
| author | Extrawurst |
|---|---|
| date | Mon, 13 Dec 2010 21:40:56 +0100 |
| parents | 4ceef5833c8c |
| children |
| rev | line source |
|---|---|
| 4 | 1 |
| 2 // written in the D programming language | |
| 3 | |
| 4 module chipmunkd.cpArbiter; | |
| 5 | |
| 6 import chipmunkd.chipmunk; | |
| 7 import chipmunkd.constraints.util; | |
| 8 | |
| 9 // Determines how fast penetrations resolve themselves. | |
| 10 //extern cpFloat cp_bias_coef; | |
| 11 // Amount of allowed penetration. Used to reduce vibrating contacts. | |
| 12 //extern cpFloat cp_collision_slop; | |
| 13 | |
| 14 // Data structure for contact points. | |
| 15 struct cpContact { | |
| 16 // Contact point and normal. | |
| 17 cpVect p, n; | |
| 18 // Penetration distance. | |
| 19 cpFloat dist; | |
| 20 | |
| 21 // Calculated by cpArbiterPreStep(). | |
| 22 cpVect r1, r2; | |
| 23 cpFloat nMass, tMass, bounce; | |
| 24 | |
| 25 // Persistant contact information. | |
| 26 cpFloat jnAcc, jtAcc, jBias; | |
| 27 cpFloat bias; | |
| 28 | |
| 29 // Hash value used to (mostly) uniquely identify a contact. | |
| 30 cpHashValue hash; | |
| 31 } | |
| 32 | |
| 33 //// Contacts are always allocated in groups. | |
| 34 //cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash); | |
| 35 // | |
| 36 //// Sum the contact impulses. (Can be used after cpSpaceStep() returns) | |
| 37 //cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts); | |
| 38 //cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts); | |
| 39 | |
| 23 | 40 enum CP_MAX_CONTACTS_PER_ARBITER = 6; |
| 41 | |
| 4 | 42 enum cpArbiterState { |
| 43 cpArbiterStateNormal, | |
| 44 cpArbiterStateFirstColl, | |
| 45 cpArbiterStateIgnore, | |
| 46 cpArbiterStateSleep, | |
| 47 cpArbiterStateCached, | |
| 48 } | |
| 49 | |
| 50 // Data structure for tracking collisions between shapes. | |
| 51 struct cpArbiter { | |
| 52 // Information on the contact points between the objects. | |
| 53 int numContacts; | |
| 54 cpContact *contacts; | |
| 55 | |
| 56 // The two shapes and bodies involved in the collision. | |
| 57 // These variables are NOT in the order defined by the collision handler. | |
| 58 // Using CP_ARBITER_GET_SHAPES and CP_ARBITER_GET_BODIES will save you from | |
| 59 // many headaches | |
| 23 | 60 cpShape* a; |
| 61 cpShape* b; | |
| 4 | 62 |
| 63 // Calculated before calling the pre-solve collision handler | |
| 64 // Override them with custom values if you want specialized behavior | |
| 65 cpFloat e; | |
| 66 cpFloat u; | |
| 67 // Used for surface_v calculations, implementation may change | |
| 68 cpVect surface_vr; | |
| 69 | |
| 70 // Time stamp of the arbiter. (from cpSpace) | |
| 71 cpTimestamp stamp; | |
| 72 | |
| 73 cpCollisionHandler *handler; | |
| 74 | |
| 75 // Are the shapes swapped in relation to the collision handler? | |
| 76 cpBool swappedColl; | |
| 77 cpArbiterState state; | |
| 78 } | |
| 79 | |
| 80 //// Arbiters are allocated in large buffers by the space and don't require a destroy function | |
| 81 //cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b); | |
| 82 // | |
| 83 //// These functions are all intended to be used internally. | |
| 84 //// Inject new contact points into the arbiter while preserving contact history. | |
| 85 //void cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, struct cpCollisionHandler *handler, cpShape *a, cpShape *b); | |
| 86 //// Precalculate values used by the solver. | |
| 87 //void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv); | |
| 88 //void cpArbiterApplyCachedImpulse(cpArbiter *arb); | |
| 89 //// Run an iteration of the solver on the arbiter. | |
| 90 //void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef); | |
| 91 // | |
| 92 //// Arbiter Helper Functions | |
| 93 //cpVect cpArbiterTotalImpulse(cpArbiter *arb); | |
| 94 //cpVect cpArbiterTotalImpulseWithFriction(cpArbiter *arb); | |
| 95 //void cpArbiterIgnore(cpArbiter *arb); | |
| 96 | |
| 97 | |
| 98 static void | |
| 99 cpArbiterGetShapes(/+const+/ cpArbiter *arb, cpShape **a, cpShape **b) | |
| 100 { | |
| 101 if(arb.swappedColl){ | |
| 23 | 102 (*a) = arb.b, (*b) = arb.a; |
| 4 | 103 } else { |
| 23 | 104 (*a) = arb.a, (*b) = arb.b; |
| 4 | 105 } |
| 106 } | |
| 13 | 107 |
| 4 | 108 template CP_ARBITER_GET_SHAPES(string arb,string a,string b) |
| 109 { | |
| 110 enum CP_ARBITER_GET_SHAPES = "cpShape* "~a~", "~b~";"~ | |
| 111 "cpArbiterGetShapes("~arb~", &"~a~", &"~b~");"; | |
| 112 } | |
| 113 | |
| 114 static void | |
| 115 cpArbiterGetBodies(/+const+/ cpArbiter *arb, cpBody **a, cpBody **b) | |
| 116 { | |
| 117 //CP_ARBITER_GET_SHAPES(arb, shape_a, shape_b); | |
| 118 cpShape *shape_a, shape_b; cpArbiterGetShapes(arb, &shape_a, &shape_b); | |
| 119 (*a) = shape_a._body; | |
| 120 (*b) = shape_b._body; | |
| 121 } | |
| 122 //#define CP_ARBITER_GET_BODIES(arb, a, b) cpBody *a, *b; cpArbiterGetBodies(arb, &a, &b); | |
| 123 | |
| 124 static cpBool | |
| 125 cpArbiterIsFirstContact(const cpArbiter *arb) | |
| 126 { | |
| 127 return arb.state == cpArbiterState.cpArbiterStateFirstColl; | |
| 128 } | |
| 129 | |
| 23 | 130 static int |
| 131 cpArbiterGetCount(const cpArbiter *arb) | |
| 132 { | |
| 133 return arb.numContacts; | |
| 134 } | |
| 135 | |
| 4 | 136 static cpVect |
| 137 cpArbiterGetNormal(const cpArbiter *arb, int i) | |
| 138 { | |
| 139 cpVect n = arb.contacts[i].n; | |
| 140 return arb.swappedColl ? cpvneg(n) : n; | |
| 141 } | |
| 142 | |
| 143 static cpVect | |
| 144 cpArbiterGetPoint(const cpArbiter *arb, int i) | |
| 145 { | |
| 146 return arb.contacts[i].p; | |
| 147 } | |
| 148 | |
| 23 | 149 |
| 150 static cpFloat | |
| 151 cpArbiteGetDepth(const cpArbiter *arb, int i) | |
| 152 { | |
| 153 return arb.contacts[i].dist; | |
| 154 } | |
| 155 | |
| 156 struct cpContactPointSet { | |
| 157 int count; | |
| 158 | |
| 159 struct TPoint | |
| 160 { | |
| 161 cpVect point, normal; | |
| 162 cpFloat dist = 0; | |
| 163 } | |
| 164 | |
| 165 TPoint[CP_MAX_CONTACTS_PER_ARBITER] points; | |
| 166 } | |
| 167 | |
| 168 static cpContactPointSet | |
| 169 cpArbiterGetContactPointSet(const cpArbiter *arb) | |
| 170 { | |
| 171 cpContactPointSet set; | |
| 172 set.count = cpArbiterGetCount(arb); | |
| 173 | |
| 174 int i; | |
| 175 for(i=0; i<set.count; i++){ | |
| 176 set.points[i].point = arb.contacts[i].p; | |
| 177 set.points[i].normal = arb.contacts[i].p; | |
| 178 set.points[i].dist = arb.contacts[i].dist; | |
| 179 } | |
| 180 | |
| 181 return set; | |
| 182 } | |
| 183 | |
| 184 // cpArbiter.c -------------------------- | |
| 185 | |
|
28
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
23
diff
changeset
|
186 __gshared cpFloat cp_bias_coef = 0.1f; |
|
4541ca17975b
use __gshared as chipmunk heavily relies on globals and D would otherwise make them TLS
Extrawurst
parents:
23
diff
changeset
|
187 __gshared cpFloat cp_collision_slop = 0.1f; |
| 4 | 188 |
| 189 cpContact* | |
| 190 cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash) | |
| 191 { | |
| 192 con.p = p; | |
| 193 con.n = n; | |
| 194 con.dist = dist; | |
| 195 | |
| 196 con.jnAcc = 0.0f; | |
| 197 con.jtAcc = 0.0f; | |
| 198 con.jBias = 0.0f; | |
| 199 | |
| 200 con.hash = hash; | |
| 201 | |
| 202 return con; | |
| 203 } | |
| 204 | |
| 205 cpVect | |
| 206 cpArbiterTotalImpulse(cpArbiter *arb) | |
| 207 { | |
| 208 cpContact *contacts = arb.contacts; | |
| 209 cpVect sum = cpvzero; | |
| 210 | |
| 211 for(int i=0, count=arb.numContacts; i<count; i++){ | |
| 212 cpContact *con = &contacts[i]; | |
| 213 sum = cpvadd(sum, cpvmult(con.n, con.jnAcc)); | |
| 214 } | |
| 215 | |
| 216 return sum; | |
| 217 } | |
| 218 | |
| 219 cpVect | |
| 220 cpArbiterTotalImpulseWithFriction(cpArbiter *arb) | |
| 221 { | |
| 222 cpContact *contacts = arb.contacts; | |
| 223 cpVect sum = cpvzero; | |
| 224 | |
| 225 for(int i=0, count=arb.numContacts; i<count; i++){ | |
| 226 cpContact *con = &contacts[i]; | |
| 227 sum = cpvadd(sum, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc))); | |
| 228 } | |
| 229 | |
| 230 return sum; | |
| 231 } | |
| 232 | |
| 233 cpFloat | |
| 234 cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts) | |
| 235 { | |
| 236 cpFloat fsum = 0.0f; | |
| 237 cpVect vsum = cpvzero; | |
| 238 | |
| 239 for(int i=0; i<numContacts; i++){ | |
| 240 cpContact *con = &contacts[i]; | |
| 241 cpVect j = cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc)); | |
| 242 | |
| 243 fsum += cpvlength(j); | |
| 244 vsum = cpvadd(vsum, j); | |
| 245 } | |
| 246 | |
| 247 cpFloat vmag = cpvlength(vsum); | |
| 248 return (1.0f - vmag/fsum); | |
| 249 } | |
| 250 | |
| 251 void | |
| 252 cpArbiterIgnore(cpArbiter *arb) | |
| 253 { | |
| 254 arb.state = cpArbiterState.cpArbiterStateIgnore; | |
| 255 } | |
| 256 | |
| 257 cpArbiter* | |
| 258 cpArbiterAlloc() | |
| 259 { | |
| 260 return cast(cpArbiter *)cpcalloc(1, cpArbiter.sizeof); | |
| 261 } | |
| 262 | |
| 263 cpArbiter* | |
| 264 cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b) | |
| 265 { | |
| 23 | 266 arb.handler = null; |
| 267 arb.swappedColl = cpFalse; | |
| 268 | |
| 269 arb.e = 0.0f; | |
| 270 arb.u = 0.0f; | |
| 271 arb.surface_vr = cpvzero; | |
| 272 | |
| 4 | 273 arb.numContacts = 0; |
| 274 arb.contacts = null; | |
| 275 | |
| 23 | 276 arb.a = a; |
| 277 arb.b = b; | |
| 4 | 278 |
| 279 arb.stamp = 0; | |
| 280 arb.state = cpArbiterState.cpArbiterStateFirstColl; | |
| 281 | |
| 282 return arb; | |
| 283 } | |
| 284 | |
| 285 cpArbiter* | |
| 286 cpArbiterNew(cpShape *a, cpShape *b) | |
| 287 { | |
| 288 return cpArbiterInit(cpArbiterAlloc(), a, b); | |
| 289 } | |
| 290 | |
| 291 void | |
| 292 cpArbiterDestroy(cpArbiter *arb) | |
| 293 { | |
| 294 // if(arb.contacts) cpfree(arb.contacts); | |
| 295 } | |
| 296 | |
| 297 void | |
| 298 cpArbiterFree(cpArbiter *arb) | |
| 299 { | |
| 300 if(arb){ | |
| 301 cpArbiterDestroy(arb); | |
| 302 cpfree(arb); | |
| 303 } | |
| 304 } | |
| 305 | |
| 306 void | |
| 307 cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b) | |
| 308 { | |
| 309 // Arbiters without contact data may exist if a collision function rejected the collision. | |
| 310 if(arb.contacts){ | |
| 311 // Iterate over the possible pairs to look for hash value matches. | |
| 312 for(int i=0; i<arb.numContacts; i++){ | |
| 313 cpContact *old = &arb.contacts[i]; | |
| 314 | |
| 315 for(int j=0; j<numContacts; j++){ | |
| 316 cpContact *new_contact = &contacts[j]; | |
| 317 | |
| 318 // This could trigger false positives, but is fairly unlikely nor serious if it does. | |
| 319 if(new_contact.hash == old.hash){ | |
| 320 // Copy the persistant contact information. | |
| 321 new_contact.jnAcc = old.jnAcc; | |
| 322 new_contact.jtAcc = old.jtAcc; | |
| 323 } | |
| 324 } | |
| 325 } | |
| 326 } | |
| 327 | |
| 328 arb.contacts = contacts; | |
| 329 arb.numContacts = numContacts; | |
| 330 | |
| 331 arb.handler = handler; | |
| 332 arb.swappedColl = (a.collision_type != handler.a); | |
| 333 | |
| 334 arb.e = a.e * b.e; | |
| 335 arb.u = a.u * b.u; | |
| 336 arb.surface_vr = cpvsub(a.surface_v, b.surface_v); | |
| 337 | |
| 338 // For collisions between two similar primitive types, the order could have been swapped. | |
| 23 | 339 arb.a = a; |
| 340 arb.b = b; | |
| 4 | 341 |
| 342 // mark it as new if it's been cached | |
| 343 if(arb.state == cpArbiterState.cpArbiterStateCached) arb.state = cpArbiterState.cpArbiterStateFirstColl; | |
| 344 } | |
| 345 | |
| 346 void | |
| 347 cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv) | |
| 348 { | |
| 23 | 349 cpBody *a = arb.a._body; |
| 350 cpBody *b = arb.b._body; | |
| 4 | 351 |
| 352 for(int i=0; i<arb.numContacts; i++){ | |
| 353 cpContact *con = &arb.contacts[i]; | |
| 354 | |
| 355 // Calculate the offsets. | |
| 356 con.r1 = cpvsub(con.p, a.p); | |
| 357 con.r2 = cpvsub(con.p, b.p); | |
| 358 | |
| 359 // Calculate the mass normal and mass tangent. | |
| 360 con.nMass = 1.0f/k_scalar(a, b, con.r1, con.r2, con.n); | |
| 361 con.tMass = 1.0f/k_scalar(a, b, con.r1, con.r2, cpvperp(con.n)); | |
| 362 | |
| 363 // Calculate the target bias velocity. | |
| 364 con.bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con.dist + cp_collision_slop); | |
| 365 con.jBias = 0.0f; | |
| 366 | |
| 367 // Calculate the target bounce velocity. | |
| 368 con.bounce = normal_relative_velocity(a, b, con.r1, con.r2, con.n)*arb.e;//cpvdot(con.n, cpvsub(v2, v1))*e; | |
| 369 } | |
| 370 } | |
| 371 | |
| 372 void | |
| 373 cpArbiterApplyCachedImpulse(cpArbiter *arb) | |
| 374 { | |
| 23 | 375 cpShape *shapea = arb.a; |
| 376 cpShape *shapeb = arb.b; | |
| 4 | 377 |
| 378 arb.u = shapea.u * shapeb.u; | |
| 379 arb.surface_vr = cpvsub(shapeb.surface_v, shapea.surface_v); | |
| 380 | |
| 381 cpBody *a = shapea._body; | |
| 382 cpBody *b = shapeb._body; | |
| 383 | |
| 384 for(int i=0; i<arb.numContacts; i++){ | |
| 385 cpContact *con = &arb.contacts[i]; | |
| 386 apply_impulses(a, b, con.r1, con.r2, cpvrotate(con.n, cpv(con.jnAcc, con.jtAcc))); | |
| 387 } | |
| 388 } | |
| 389 | |
| 390 void | |
| 391 cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef) | |
| 392 { | |
| 23 | 393 cpBody *a = arb.a._body; |
| 394 cpBody *b = arb.b._body; | |
| 4 | 395 |
| 396 for(int i=0; i<arb.numContacts; i++){ | |
| 397 cpContact *con = &arb.contacts[i]; | |
| 398 cpVect n = con.n; | |
| 399 cpVect r1 = con.r1; | |
| 400 cpVect r2 = con.r2; | |
| 401 | |
| 402 // Calculate the relative bias velocities. | |
| 403 cpVect vb1 = cpvadd(a.v_bias, cpvmult(cpvperp(r1), a.w_bias)); | |
| 404 cpVect vb2 = cpvadd(b.v_bias, cpvmult(cpvperp(r2), b.w_bias)); | |
| 405 cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); | |
| 406 | |
| 407 // Calculate and clamp the bias impulse. | |
| 408 cpFloat jbn = (con.bias - vbn)*con.nMass; | |
| 409 cpFloat jbnOld = con.jBias; | |
| 410 con.jBias = cpfmax(jbnOld + jbn, 0.0f); | |
| 411 jbn = con.jBias - jbnOld; | |
| 412 | |
| 413 // Apply the bias impulse. | |
| 414 apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn)); | |
| 415 | |
| 416 // Calculate the relative velocity. | |
| 417 cpVect vr = relative_velocity(a, b, r1, r2); | |
| 418 cpFloat vrn = cpvdot(vr, n); | |
| 419 | |
| 420 // Calculate and clamp the normal impulse. | |
| 421 cpFloat jn = -(con.bounce*eCoef + vrn)*con.nMass; | |
| 422 cpFloat jnOld = con.jnAcc; | |
| 423 con.jnAcc = cpfmax(jnOld + jn, 0.0f); | |
| 424 jn = con.jnAcc - jnOld; | |
| 425 | |
| 426 // Calculate the relative tangent velocity. | |
| 427 cpFloat vrt = cpvdot(cpvadd(vr, arb.surface_vr), cpvperp(n)); | |
| 428 | |
| 429 // Calculate and clamp the friction impulse. | |
| 430 cpFloat jtMax = arb.u*con.jnAcc; | |
| 431 cpFloat jt = -vrt*con.tMass; | |
| 432 cpFloat jtOld = con.jtAcc; | |
| 433 con.jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); | |
| 434 jt = con.jtAcc - jtOld; | |
| 435 | |
| 436 // Apply the final impulse. | |
| 437 apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt))); | |
| 438 } | |
| 439 } |