1 /* 2 Copyright (c) 2013-2017 Timur Gafarov 3 4 Boost Software License - Version 1.0 - August 17th, 2003 5 6 Permission is hereby granted, free of charge, to any person or organization 7 obtaining a copy of the software and accompanying documentation covered by 8 this license (the "Software") to use, reproduce, display, distribute, 9 execute, and transmit the Software, and to prepare derivative works of the 10 Software, and to permit third-parties to whom the Software is furnished to 11 do so, all subject to the following: 12 13 The copyright notices in the Software and this entire statement, including 14 the above license grant, this restriction and the following disclaimer, 15 must be included in all copies of the Software, in whole or in part, and 16 all derivative works of the Software, unless such copies or derivative 17 works are solely in the form of machine-executable object code generated by 18 a source language processor. 19 20 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 21 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 22 FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT 23 SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE 24 FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE, 25 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 26 DEALINGS IN THE SOFTWARE. 27 */ 28 29 module dmech.world; 30 31 import std.math; 32 import std.range; 33 34 import dlib.core.ownership; 35 import dlib.core.memory; 36 import dlib.container.array; 37 import dlib.math.vector; 38 import dlib.math.matrix; 39 import dlib.math.transformation; 40 import dlib.geometry.triangle; 41 import dlib.geometry.sphere; 42 import dlib.geometry.ray; 43 import dlib.geometry.plane; 44 45 import dmech.rigidbody; 46 import dmech.geometry; 47 import dmech.shape; 48 import dmech.contact; 49 import dmech.solver; 50 import dmech.pairhashtable; 51 import dmech.collision; 52 import dmech.pcm; 53 import dmech.constraint; 54 import dmech.bvh; 55 import dmech.mpr; 56 import dmech.raycast; 57 58 /* 59 * World object stores bodies and constraints 60 * and performs simulation cycles on them. 61 * It also provides a generalized raycast query for all bodies. 62 */ 63 64 alias PairHashTable!PersistentContactManifold ContactCache; 65 66 class PhysicsWorld: Owner 67 { 68 DynamicArray!ShapeComponent shapeComponents; 69 DynamicArray!RigidBody staticBodies; 70 DynamicArray!RigidBody dynamicBodies; 71 DynamicArray!Constraint constraints; 72 73 Vector3f gravity; 74 75 protected uint maxShapeId = 1; 76 77 ContactCache manifolds; 78 79 bool broadphase = false; 80 bool warmstart = false; 81 82 uint positionCorrectionIterations = 20; 83 uint constraintIterations = 40; 84 85 BVHNode!Triangle bvhRoot = null; 86 87 // Proxy triangle to deal with BVH data 88 RigidBody proxyTri; 89 ShapeComponent proxyTriShape; 90 GeomTriangle proxyTriGeom; 91 92 this(Owner owner, size_t maxCollisions = 1000) 93 { 94 super(owner); 95 96 gravity = Vector3f(0.0f, -9.80665f, 0.0f); // Earth conditions 97 98 manifolds = New!ContactCache(this, maxCollisions); 99 100 // Create proxy triangle 101 proxyTri = New!RigidBody(this); 102 proxyTri.position = Vector3f(0, 0, 0); 103 proxyTriGeom = New!GeomTriangle(this, 104 Vector3f(-1.0f, 0.0f, -1.0f), 105 Vector3f(+1.0f, 0.0f, 0.0f), 106 Vector3f(-1.0f, 0.0f, +1.0f)); 107 proxyTriShape = New!ShapeComponent(this, proxyTriGeom, Vector3f(0, 0, 0), 1); 108 proxyTriShape.id = maxShapeId; 109 maxShapeId++; 110 proxyTriShape.transformation = 111 proxyTri.transformation() * translationMatrix(proxyTriShape.centroid); 112 proxyTri.shapes.append(proxyTriShape); 113 proxyTri.mass = float.infinity; 114 proxyTri.invMass = 0.0f; 115 proxyTri.inertiaTensor = matrixf( 116 float.infinity, 0, 0, 117 0, float.infinity, 0, 118 0, 0, float.infinity 119 ); 120 proxyTri.invInertiaTensor = matrixf( 121 0, 0, 0, 122 0, 0, 0, 123 0, 0, 0 124 ); 125 proxyTri.dynamic = false; 126 } 127 128 RigidBody addDynamicBody(Vector3f pos, float mass = 0.0f) 129 { 130 auto b = New!RigidBody(this); 131 b.position = pos; 132 b.mass = mass; 133 b.invMass = 1.0f / mass; 134 b.inertiaTensor = matrixf( 135 mass, 0, 0, 136 0, mass, 0, 137 0, 0, mass 138 ); 139 b.invInertiaTensor = matrixf( 140 0, 0, 0, 141 0, 0, 0, 142 0, 0, 0 143 ); 144 b.dynamic = true; 145 dynamicBodies.append(b); 146 return b; 147 } 148 149 RigidBody addStaticBody(Vector3f pos) 150 { 151 auto b = New!RigidBody(this); 152 b.position = pos; 153 b.mass = float.infinity; 154 b.invMass = 0.0f; 155 b.inertiaTensor = matrixf( 156 float.infinity, 0, 0, 157 0, float.infinity, 0, 158 0, 0, float.infinity 159 ); 160 b.invInertiaTensor = matrixf( 161 0, 0, 0, 162 0, 0, 0, 163 0, 0, 0 164 ); 165 b.dynamic = false; 166 staticBodies.append(b); 167 return b; 168 } 169 170 ShapeComponent addShapeComponent(RigidBody b, Geometry geom, Vector3f position, float mass) 171 { 172 auto shape = New!ShapeComponent(this, geom, position, mass); 173 shapeComponents.append(shape); 174 shape.id = maxShapeId; 175 maxShapeId++; 176 b.addShapeComponent(shape); 177 return shape; 178 } 179 180 ShapeComponent addSensor(RigidBody b, Geometry geom, Vector3f position) 181 { 182 auto shape = New!ShapeComponent(this, geom, position, 0.0f); 183 shape.raycastable = false; 184 shape.solve = false; 185 shapeComponents.append(shape); 186 shape.id = maxShapeId; 187 maxShapeId++; 188 b.addShapeComponent(shape); 189 return shape; 190 } 191 192 Constraint addConstraint(Constraint c) 193 { 194 constraints.append(c); 195 return c; 196 } 197 198 void update(double dt) 199 { 200 auto dynamicBodiesArray = dynamicBodies.data; 201 202 if (dynamicBodiesArray.length == 0) 203 return; 204 205 foreach(ref m; manifolds) 206 { 207 m.update(); 208 } 209 210 for (size_t i = 0; i < dynamicBodiesArray.length; i++) 211 { 212 auto b = dynamicBodiesArray[i]; 213 b.updateInertia(); 214 if (b.useGravity) 215 { 216 if (b.useOwnGravity) 217 b.applyForce(b.gravity * b.mass); 218 else 219 b.applyForce(gravity * b.mass); 220 } 221 b.integrateForces(dt); 222 b.resetForces(); 223 } 224 225 if (broadphase) 226 findDynamicCollisionsBroadphase(); 227 else 228 findDynamicCollisionsBruteForce(); 229 230 findStaticCollisionsBruteForce(); 231 232 solveConstraints(dt); 233 234 for (size_t i = 0; i < dynamicBodiesArray.length; i++) 235 { 236 auto b = dynamicBodiesArray[i]; 237 b.integrateVelocities(dt); 238 } 239 240 foreach(iteration; 0..positionCorrectionIterations) 241 foreach(ref m; manifolds) 242 foreach(i; 0..m.numContacts) 243 { 244 auto c = &m.contacts[i]; 245 solvePositionError(c, m.numContacts); 246 } 247 248 for (size_t i = 0; i < dynamicBodiesArray.length; i++) 249 { 250 auto b = dynamicBodiesArray[i]; 251 b.integratePseudoVelocities(dt); 252 b.updateShapeComponents(); 253 } 254 } 255 256 bool raycast( 257 Vector3f rayStart, 258 Vector3f rayDir, 259 float maxRayDist, 260 out CastResult castResult, 261 bool checkAgainstBodies = true, 262 bool checkAgainstBVH = true) 263 { 264 bool res = false; 265 float bestParam = float.max; 266 267 CastResult cr; 268 269 if (checkAgainstBodies) 270 foreach(b; chain(staticBodies.data, dynamicBodies.data)) 271 if (b.active && b.raycastable) 272 foreach(shape; b.shapes.data) 273 if (shape.active && shape.raycastable) 274 { 275 bool hit = convexRayCast(shape, rayStart, rayDir, maxRayDist, cr); 276 if (hit) 277 { 278 if (cr.param < bestParam) 279 { 280 bestParam = cr.param; 281 castResult = cr; 282 castResult.rbody = b; 283 res = true; 284 } 285 } 286 } 287 288 if (!checkAgainstBVH) 289 return res; 290 291 Ray ray = Ray(rayStart, rayStart + rayDir * maxRayDist); 292 293 if (bvhRoot !is null) 294 foreach(tri; bvhRoot.traverseByRay(&ray)) 295 { 296 Vector3f ip; 297 bool hit = ray.intersectTriangle(tri.v[0], tri.v[1], tri.v[2], ip); 298 if (hit) 299 { 300 float param = distance(rayStart, ip); 301 if (param < bestParam) 302 { 303 bestParam = param; 304 castResult.fact = true; 305 castResult.param = param; 306 castResult.point = ip; 307 castResult.normal = tri.normal; 308 castResult.rbody = proxyTri; 309 castResult.shape = null; 310 res = true; 311 } 312 } 313 } 314 315 return res; 316 } 317 318 void findDynamicCollisionsBruteForce() 319 { 320 auto dynamicBodiesArray = dynamicBodies.data; 321 for (int i = 0; i < dynamicBodiesArray.length - 1; i++) 322 { 323 auto body1 = dynamicBodiesArray[i]; 324 if (body1.active) 325 foreach(shape1; body1.shapes.data) 326 if (shape1.active) 327 { 328 for (int j = i + 1; j < dynamicBodiesArray.length; j++) 329 { 330 auto body2 = dynamicBodiesArray[j]; 331 if (body2.active) 332 foreach(shape2; body2.shapes.data) 333 if (shape2.active) 334 { 335 Contact c; 336 c.body1 = body1; 337 c.body2 = body2; 338 checkCollisionPair(shape1, shape2, c); 339 } 340 } 341 } 342 } 343 } 344 345 void findDynamicCollisionsBroadphase() 346 { 347 auto dynamicBodiesArray = dynamicBodies.data; 348 for (int i = 0; i < dynamicBodiesArray.length - 1; i++) 349 { 350 auto body1 = dynamicBodiesArray[i]; 351 if (body1.active) 352 foreach(shape1; body1.shapes.data) 353 if (shape1.active) 354 { 355 for (int j = i + 1; j < dynamicBodiesArray.length; j++) 356 { 357 auto body2 = dynamicBodiesArray[j]; 358 if (body2.active) 359 foreach(shape2; body2.shapes.data) 360 if (shape2.active) 361 if (shape1.boundingBox.intersectsAABB(shape2.boundingBox)) 362 { 363 Contact c; 364 c.body1 = body1; 365 c.body2 = body2; 366 checkCollisionPair(shape1, shape2, c); 367 } 368 } 369 } 370 } 371 } 372 373 void findStaticCollisionsBruteForce() 374 { 375 auto dynamicBodiesArray = dynamicBodies.data; 376 auto staticBodiesArray = staticBodies.data; 377 foreach(body1; dynamicBodiesArray) 378 { 379 if (body1.active) 380 foreach(shape1; body1.shapes.data) 381 if (shape1.active) 382 { 383 foreach(body2; staticBodiesArray) 384 { 385 if (body2.active) 386 foreach(shape2; body2.shapes.data) 387 if (shape2.active) 388 { 389 Contact c; 390 c.body1 = body1; 391 c.body2 = body2; 392 c.shape2pos = shape2.position; 393 checkCollisionPair(shape1, shape2, c); 394 } 395 } 396 } 397 } 398 399 // Find collisions between dynamic bodies 400 // and the BVH world (static triangle mesh) 401 if (bvhRoot !is null) 402 { 403 checkCollisionBVH(); 404 } 405 } 406 407 void checkCollisionBVH() 408 { 409 foreach(rb; dynamicBodies.data) 410 if (rb.active) 411 foreach(shape; rb.shapes.data) 412 if (shape.active) 413 { 414 // There may be more than one contact at a time 415 Contact[5] contacts; 416 Triangle[5] contactTris; 417 uint numContacts = 0; 418 419 Contact c; 420 c.body1 = rb; 421 c.body2 = proxyTri; 422 c.fact = false; 423 424 Sphere sphere = shape.boundingSphere; 425 426 foreach(tri; bvhRoot.traverseBySphere(&sphere)) 427 { 428 // Update temporary triangle to check collision 429 proxyTriShape.transformation = translationMatrix(tri.barycenter); 430 proxyTriGeom.v[0] = tri.v[0] - tri.barycenter; 431 proxyTriGeom.v[1] = tri.v[1] - tri.barycenter; 432 proxyTriGeom.v[2] = tri.v[2] - tri.barycenter; 433 434 bool collided = checkCollision(shape, proxyTriShape, c); 435 436 if (collided) 437 { 438 if (numContacts < contacts.length) 439 { 440 c.shape1RelPoint = c.point - shape.position; 441 c.shape2RelPoint = c.point - tri.barycenter; 442 c.body1RelPoint = c.point - c.body1.worldCenterOfMass; 443 c.body2RelPoint = c.point - tri.barycenter; 444 c.shape1 = shape; 445 c.shape2 = proxyTriShape; 446 c.shape2pos = tri.barycenter; 447 contacts[numContacts] = c; 448 contactTris[numContacts] = tri; 449 numContacts++; 450 } 451 } 452 } 453 454 /* 455 * NOTE: 456 * There is a problem when rolling bodies over a triangle mesh. Instead of rolling 457 * straight it will get influenced when hitting triangle edges. 458 * Current solution is to solve only the contact with deepest penetration and 459 * throw out all others. Other possible approach is to merge all contacts that 460 * are within epsilon of each other. When merging the contacts, average and 461 * re-normalize the normals, and average the penetration depth value. 462 */ 463 464 int deepestContactIdx = -1; 465 float maxPen = 0.0f; 466 float bestGroundness = -1.0f; 467 foreach(i; 0..numContacts) 468 { 469 if (contacts[i].penetration > maxPen) 470 { 471 deepestContactIdx = i; 472 maxPen = contacts[i].penetration; 473 } 474 } 475 476 if (deepestContactIdx >= 0) 477 { 478 auto co = &contacts[deepestContactIdx]; 479 co.calcFDir(); 480 481 auto m = manifolds.get(shape.id, proxyTriShape.id); 482 if (m is null) 483 { 484 PersistentContactManifold m1; 485 m1.addContact(*co); 486 manifolds.set(shape.id, proxyTriShape.id, m1); 487 488 shape.numCollisions++; 489 } 490 else 491 { 492 m.addContact(*co); 493 } 494 495 c.body1.numContacts++; 496 c.body2.numContacts++; 497 498 c.body1.contactEvent(*co); 499 c.body2.contactEvent(*co); 500 } 501 else 502 { 503 //manifolds.remove(shape.id, proxyTriShape.id); 504 505 auto m = manifolds.get(shape.id, proxyTriShape.id); 506 if (m !is null) 507 { 508 manifolds.remove(shape.id, proxyTriShape.id); 509 510 c.body1.numContacts -= m.numContacts; 511 c.body2.numContacts -= m.numContacts; 512 513 shape.numCollisions--; 514 } 515 } 516 } 517 } 518 519 void checkCollisionPair(ShapeComponent shape1, ShapeComponent shape2, ref Contact c) 520 { 521 if (checkCollision(shape1, shape2, c)) 522 { 523 c.body1RelPoint = c.point - c.body1.worldCenterOfMass; 524 c.body2RelPoint = c.point - c.body2.worldCenterOfMass; 525 c.shape1RelPoint = c.point - shape1.position; 526 c.shape2RelPoint = c.point - shape2.position; 527 c.shape1 = shape1; 528 c.shape2 = shape2; 529 c.calcFDir(); 530 531 auto m = manifolds.get(shape1.id, shape2.id); 532 if (m is null) 533 { 534 PersistentContactManifold m1; 535 m1.addContact(c); 536 manifolds.set(shape1.id, shape2.id, m1); 537 538 c.body1.contactEvent(c); 539 c.body2.contactEvent(c); 540 541 shape1.numCollisions++; 542 shape2.numCollisions++; 543 } 544 else 545 { 546 m.addContact(c); 547 } 548 549 c.body1.numContacts++; 550 c.body2.numContacts++; 551 } 552 else 553 { 554 auto m = manifolds.get(shape1.id, shape2.id); 555 if (m !is null) 556 { 557 manifolds.remove(shape1.id, shape2.id); 558 559 c.body1.numContacts -= m.numContacts; 560 c.body2.numContacts -= m.numContacts; 561 562 shape1.numCollisions--; 563 shape2.numCollisions--; 564 } 565 } 566 } 567 568 void solveConstraints(double dt) 569 { 570 foreach(ref m; manifolds) 571 foreach(i; 0..m.numContacts) 572 { 573 auto c = &m.contacts[i]; 574 prepareContact(c); 575 } 576 577 auto constraintsData = constraints.data; 578 foreach(c; constraintsData) 579 { 580 c.prepare(dt); 581 } 582 583 foreach(iteration; 0..constraintIterations) 584 { 585 foreach(c; constraintsData) 586 c.step(); 587 588 foreach(ref m; manifolds) 589 foreach(i; 0..m.numContacts) 590 { 591 auto c = &m.contacts[i]; 592 solveContact(c, dt); 593 } 594 } 595 } 596 597 ~this() 598 { 599 //foreach(sh; shapeComponents.data) 600 // sh.free(); 601 shapeComponents.free(); 602 603 //foreach(b; dynamicBodies.data) 604 // b.free(); 605 dynamicBodies.free(); 606 607 //foreach(b; staticBodies.data) 608 // b.free(); 609 staticBodies.free(); 610 611 //foreach(c; constraints.data) 612 // c.free(); 613 constraints.free(); 614 615 //manifolds.free(); 616 617 //proxyTriGeom.free(); 618 //proxyTriShape.free(); 619 //proxyTri.free(); 620 } 621 }