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.constraint; 30 31 import std.math; 32 import std.algorithm; 33 34 import dlib.core.ownership; 35 import dlib.core.memory; 36 37 import dlib.math.vector; 38 import dlib.math.matrix; 39 import dlib.math.quaternion; 40 41 import dmech.world; 42 import dmech.rigidbody; 43 44 abstract class Constraint: Owner 45 { 46 RigidBody body1; 47 RigidBody body2; 48 49 this(Owner o) 50 { 51 super(o); 52 } 53 54 void prepare(double delta); 55 void step(); 56 } 57 58 /* 59 * Keeps bodies at some fixed (or max/min) distance from each other. 60 * Also works as a spring, if softness is set to a higher value. 61 */ 62 class DistanceConstraint: Constraint 63 { 64 enum DistanceBehavior 65 { 66 LimitDistance, 67 LimitMaximumDistance, 68 LimitMinimumDistance, 69 } 70 71 Vector3f r1, r2; 72 73 float biasFactor = 0.1f; 74 float softness = 0.01f; 75 float distance; 76 77 DistanceBehavior behavior = DistanceBehavior.LimitDistance; 78 79 this(Owner owner, RigidBody body1, RigidBody body2, float dist = 0.0f) 80 { 81 super(owner); 82 83 this.body1 = body1; 84 this.body2 = body2; 85 86 if (dist > 0.0f) 87 distance = dist; 88 else 89 distance = (body1.worldCenterOfMass - body2.worldCenterOfMass).length; 90 } 91 92 float effectiveMass = 0.0f; 93 float accumulatedImpulse = 0.0f; 94 float bias; 95 float softnessOverDt; 96 97 Vector3f[4] jacobian; 98 99 bool skipConstraint = false; 100 101 float myCounter = 0.0f; 102 103 override void prepare(double dt) 104 { 105 r1 = Vector3f(0.0f, 0.0f, 0.0f); 106 r2 = Vector3f(0.0f, 0.0f, 0.0f); 107 108 Vector3f dp = body2.worldCenterOfMass - body1.worldCenterOfMass; 109 110 float deltaLength = dp.length - distance; 111 112 if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= 0.0f) 113 skipConstraint = true; 114 else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= 0.0f) 115 skipConstraint = true; 116 else 117 { 118 skipConstraint = false; 119 120 Vector3f n = dp; 121 if (n.lengthsqr != 0.0f) 122 n.normalize(); 123 124 jacobian[0] = -n; 125 jacobian[1] = -cross(r1, n); 126 jacobian[2] = n; 127 jacobian[3] = cross(r2, n); 128 129 effectiveMass = 130 body1.invMass + body2.invMass 131 + dot(jacobian[1] * body1.invInertiaTensor, jacobian[1]) 132 + dot(jacobian[3] * body2.invInertiaTensor, jacobian[3]); 133 134 softnessOverDt = softness / dt; 135 effectiveMass += softnessOverDt; 136 137 if (effectiveMass != 0) 138 effectiveMass = 1.0f / effectiveMass; 139 140 bias = deltaLength * biasFactor * (1.0f / dt); 141 142 if (body1.dynamic) 143 { 144 body1.linearVelocity += jacobian[0] * accumulatedImpulse * body1.invMass; 145 body1.angularVelocity += jacobian[1] * accumulatedImpulse * body1.invInertiaTensor; 146 } 147 148 if (body2.dynamic) 149 { 150 body2.linearVelocity += jacobian[2] * accumulatedImpulse * body2.invMass; 151 body2.angularVelocity += jacobian[3] * accumulatedImpulse * body2.invInertiaTensor; 152 } 153 } 154 } 155 156 override void step() 157 { 158 if (skipConstraint) 159 return; 160 161 float jv = 162 dot(body1.linearVelocity, jacobian[0]) 163 + dot(body1.angularVelocity, jacobian[1]) 164 + dot(body2.linearVelocity, jacobian[2]) 165 + dot(body2.angularVelocity, jacobian[3]); 166 167 float softnessScalar = accumulatedImpulse * softnessOverDt; 168 169 float lambda = -effectiveMass * (jv + bias + softnessScalar); 170 171 if (behavior == DistanceBehavior.LimitMinimumDistance) 172 { 173 float previousAccumulatedImpulse = accumulatedImpulse; 174 accumulatedImpulse = max(accumulatedImpulse + lambda, 0); 175 lambda = accumulatedImpulse - previousAccumulatedImpulse; 176 } 177 else if (behavior == DistanceBehavior.LimitMaximumDistance) 178 { 179 float previousAccumulatedImpulse = accumulatedImpulse; 180 accumulatedImpulse = min(accumulatedImpulse + lambda, 0); 181 lambda = accumulatedImpulse - previousAccumulatedImpulse; 182 } 183 else 184 { 185 accumulatedImpulse += lambda; 186 } 187 188 if (body1.dynamic) 189 { 190 body1.linearVelocity += jacobian[0] * lambda * body1.invMass; 191 body1.angularVelocity += jacobian[1] * lambda * body1.invInertiaTensor; 192 } 193 if (body2.dynamic) 194 { 195 body2.linearVelocity += jacobian[2] * lambda * body2.invMass; 196 body2.angularVelocity += jacobian[3] * lambda * body2.invInertiaTensor; 197 } 198 } 199 } 200 201 /* 202 * Limits the translation so that the local anchor points of two rigid bodies 203 * match in world space. 204 */ 205 class BallConstraint: Constraint 206 { 207 Vector3f localAnchor1, localAnchor2; 208 Vector3f r1, r2; 209 210 Vector3f[4] jacobian; 211 212 float accumulatedImpulse = 0.0f; 213 214 float biasFactor = 0.1f; 215 float softness = 0.01f; //0.05f; 216 217 float softnessOverDt; 218 float effectiveMass; 219 float bias; 220 221 this(Owner owner, RigidBody body1, RigidBody body2, Vector3f anchor1, Vector3f anchor2) 222 { 223 super(owner); 224 225 this.body1 = body1; 226 this.body2 = body2; 227 228 localAnchor1 = anchor1; 229 localAnchor2 = anchor2; 230 } 231 232 override void prepare(double delta) 233 { 234 Vector3f r1 = body1.orientation.rotate(localAnchor1); 235 Vector3f r2 = body2.orientation.rotate(localAnchor2); 236 237 Vector3f p1, p2, dp; 238 p1 = body1.worldCenterOfMass + r1; 239 p2 = body2.worldCenterOfMass + r2; 240 241 dp = p2 - p1; 242 243 float deltaLength = dp.length; 244 Vector3f n = dp.normalized; 245 246 jacobian[0] = -n; 247 jacobian[1] = -cross(r1, n); 248 jacobian[2] = n; 249 jacobian[3] = cross(r2, n); 250 251 effectiveMass = 252 body1.invMass + 253 body2.invMass + 254 dot(jacobian[1] * body1.invInertiaTensor, jacobian[1]) + 255 dot(jacobian[3] * body2.invInertiaTensor, jacobian[3]); 256 257 softnessOverDt = softness / delta; 258 effectiveMass += softnessOverDt; 259 effectiveMass = 1.0f / effectiveMass; 260 261 bias = deltaLength * biasFactor * (1.0f / delta); 262 263 if (body1.dynamic) 264 { 265 body1.linearVelocity += jacobian[0] * body1.invMass * accumulatedImpulse; 266 body1.angularVelocity += jacobian[1] * body1.invInertiaTensor * accumulatedImpulse; 267 } 268 269 if (body2.dynamic) 270 { 271 body2.linearVelocity += jacobian[2] * body2.invMass * accumulatedImpulse; 272 body2.angularVelocity += jacobian[3] * body2.invInertiaTensor * accumulatedImpulse; 273 } 274 } 275 276 override void step() 277 { 278 float jv = 279 dot(body1.linearVelocity, jacobian[0]) + 280 dot(body1.angularVelocity, jacobian[1]) + 281 dot(body2.linearVelocity, jacobian[2]) + 282 dot(body2.angularVelocity, jacobian[3]); 283 284 float softnessScalar = accumulatedImpulse * softnessOverDt; 285 float lambda = -effectiveMass * (jv + bias + softnessScalar); 286 287 accumulatedImpulse += lambda; 288 289 if (body1.dynamic) 290 { 291 body1.linearVelocity += jacobian[0] * body1.invMass * lambda; 292 body1.angularVelocity += jacobian[1] * body1.invInertiaTensor * lambda; 293 } 294 295 if (body2.dynamic) 296 { 297 body2.linearVelocity += jacobian[2] * body2.invMass * lambda; 298 body2.angularVelocity += jacobian[3] * body2.invInertiaTensor * lambda; 299 } 300 } 301 } 302 303 /* 304 * Constraints a point on a body to be fixed on a line 305 * which is fixed on another body. 306 */ 307 class SliderConstraint: Constraint 308 { 309 Vector3f lineNormal; 310 311 Vector3f localAnchor1, localAnchor2; 312 Vector3f r1, r2; 313 314 Vector3f[4] jacobian; 315 316 float accumulatedImpulse = 0.0f; 317 318 float biasFactor = 0.5f; 319 float softness = 0.0f; 320 321 float softnessOverDt; 322 float effectiveMass; 323 float bias; 324 325 this(Owner owner, RigidBody body1, RigidBody body2, Vector3f lineStartPointBody1, Vector3f pointBody2) 326 { 327 super(owner); 328 329 this.body1 = body1; 330 this.body2 = body2; 331 332 localAnchor1 = lineStartPointBody1; 333 localAnchor2 = pointBody2; 334 335 lineNormal = (lineStartPointBody1 + body1.worldCenterOfMass) - 336 (pointBody2 + body2.worldCenterOfMass); 337 338 if (lineNormal.lengthsqr != 0.0f) 339 lineNormal.normalize(); 340 } 341 342 override void prepare(double delta) 343 { 344 Vector3f r1 = body1.orientation.rotate(localAnchor1); 345 Vector3f r2 = body2.orientation.rotate(localAnchor2); 346 347 Vector3f p1, p2, dp; 348 p1 = body1.worldCenterOfMass + r1; 349 p2 = body2.worldCenterOfMass + r2; 350 351 dp = p2 - p1; 352 353 Vector3f l = body1.orientation.rotate(lineNormal); 354 355 Vector3f t = cross((p1 - p2), l); 356 if (t.lengthsqr != 0.0f) 357 t.normalize(); 358 t = cross(t, l); 359 360 jacobian[0] = t; 361 jacobian[1] = cross((r1 + p2 - p1), t); 362 jacobian[2] = -t; 363 jacobian[3] = -cross(r2, t); 364 365 effectiveMass = 366 body1.invMass + 367 body2.invMass + 368 dot(jacobian[1] * body1.invInertiaTensor, jacobian[1]) + 369 dot(jacobian[3] * body2.invInertiaTensor, jacobian[3]); 370 371 softnessOverDt = softness / delta; 372 effectiveMass += softnessOverDt; 373 374 if (effectiveMass != 0) 375 effectiveMass = 1.0f / effectiveMass; 376 377 bias = -cross(l, (p2 - p1)).length * biasFactor * (1.0f / delta); 378 379 if (body1.dynamic) 380 { 381 body1.linearVelocity += body1.invMass * accumulatedImpulse * jacobian[0]; 382 body1.angularVelocity += accumulatedImpulse * jacobian[1] * body1.invInertiaTensor; 383 } 384 385 if (body2.dynamic) 386 { 387 body2.linearVelocity += body2.invMass * accumulatedImpulse * jacobian[2]; 388 body2.angularVelocity += accumulatedImpulse * jacobian[3] * body2.invInertiaTensor; 389 } 390 } 391 392 override void step() 393 { 394 float jv = 395 dot(body1.linearVelocity, jacobian[0]) + 396 dot(body1.angularVelocity, jacobian[1]) + 397 dot(body2.linearVelocity, jacobian[2]) + 398 dot(body2.angularVelocity, jacobian[3]); 399 400 float softnessScalar = accumulatedImpulse * softnessOverDt; 401 float lambda = -effectiveMass * (jv + bias + softnessScalar); 402 403 accumulatedImpulse += lambda; 404 405 if (body1.dynamic) 406 { 407 body1.linearVelocity += body1.invMass * lambda * jacobian[0]; 408 body1.angularVelocity += lambda * jacobian[1] * body1.invInertiaTensor; 409 } 410 411 if (body2.dynamic) 412 { 413 body2.linearVelocity += body2.invMass * lambda * jacobian[2]; 414 body2.angularVelocity += lambda * jacobian[3] * body2.invInertiaTensor; 415 } 416 } 417 } 418 419 /* 420 * Constraints bodies so that they always take the same rotation 421 * relative to each other. 422 */ 423 class AngleConstraint: Constraint 424 { 425 Vector3f[4] jacobian; 426 427 Vector3f accumulatedImpulse = Vector3f(0, 0, 0); 428 429 float biasFactor = 0.05f; 430 float softness = 0.0f; 431 432 float softnessOverDt; 433 Matrix3x3f effectiveMass; 434 Vector3f bias; 435 436 this(Owner owner, RigidBody body1, RigidBody body2) 437 { 438 super(owner); 439 this.body1 = body1; 440 this.body2 = body2; 441 } 442 443 override void prepare(double dt) 444 { 445 effectiveMass = body1.invInertiaTensor + body2.invInertiaTensor; 446 447 softnessOverDt = softness / dt; 448 449 effectiveMass.a11 += softnessOverDt; 450 effectiveMass.a22 += softnessOverDt; 451 effectiveMass.a33 += softnessOverDt; 452 453 effectiveMass = effectiveMass.inverse; 454 455 Quaternionf dq = body2.orientation * body1.orientation.conj; 456 Vector3f axis = dq.generator; 457 /* 458 // Matrix version 459 Matrix3x3f orientationDifference = Matrix3x3f.identity; 460 auto rot1 = body1.orientation.toMatrix3x3; 461 auto rot2 = body2.orientation.toMatrix3x3; 462 Matrix3x3f q = orientationDifference * rot2 * rot1.inverse; 463 464 Vector3f axis; 465 float x = q.a32 - q.a23; 466 float y = q.a13 - q.a31; 467 float z = q.a21 - q.a12; 468 float r = sqrt(x * x + y * y + z * z); 469 float t = q.a11 + q.a22 + q.a33; 470 float angle = atan2(r, t - 1); 471 axis = Vector3f(x, y, z) * angle; 472 */ 473 474 bias = axis * biasFactor * (-1.0f / dt); 475 476 if (body1.dynamic) 477 body1.angularVelocity += accumulatedImpulse * body1.invInertiaTensor; 478 if (body2.dynamic) 479 body2.angularVelocity += -accumulatedImpulse * body2.invInertiaTensor; 480 } 481 482 override void step() 483 { 484 Vector3f jv = body1.angularVelocity - body2.angularVelocity; 485 Vector3f softnessVector = accumulatedImpulse * softnessOverDt; 486 487 Vector3f lambda = -1.0f * (jv+bias+softnessVector) * effectiveMass; 488 accumulatedImpulse += lambda; 489 490 if (body1.dynamic) 491 body1.angularVelocity += lambda * body1.invInertiaTensor; 492 if (body2.dynamic) 493 body2.angularVelocity += -lambda * body2.invInertiaTensor; 494 } 495 } 496 497 /* 498 * Constrains two bodies to rotate only around a single axis in worldspace. 499 */ 500 class AxisAngleConstraint: Constraint 501 { 502 Vector3f axis; 503 504 Vector3f localAxis1; 505 Vector3f localAxis2; 506 Vector3f localConstrAxis1; 507 Vector3f localConstrAxis2; 508 Vector3f worldConstrAxis1; 509 Vector3f worldConstrAxis2; 510 511 Vector3f accumulatedImpulse = Vector3f(0, 0, 0); 512 513 float biasFactor = 0.4f; 514 float softness = 0.0f; 515 516 float softnessOverDt; 517 Matrix3x3f effectiveMass; 518 Vector3f bias; 519 520 this(Owner owner, RigidBody body1, RigidBody body2, Vector3f axis) 521 { 522 super(owner); 523 this.body1 = body1; 524 this.body2 = body2; 525 this.axis = axis; 526 527 // Axis in body space 528 this.localAxis1 = axis * body1.orientation.toMatrix3x3.transposed; 529 this.localAxis2 = axis * body2.orientation.toMatrix3x3.transposed; 530 531 localConstrAxis1 = cross(Vector3f(0, 1, 0), localAxis1); 532 if (localConstrAxis1.lengthsqr < 0.001f) 533 localConstrAxis1 = cross(Vector3f(1, 0, 0), localAxis1); 534 535 localConstrAxis2 = cross(localAxis1, localConstrAxis1); 536 localConstrAxis1.normalize(); 537 localConstrAxis2.normalize(); 538 } 539 540 override void prepare(double dt) 541 { 542 effectiveMass = body1.invInertiaTensor + body2.invInertiaTensor; 543 544 softnessOverDt = softness / dt; 545 546 effectiveMass.a11 += softnessOverDt; 547 effectiveMass.a22 += softnessOverDt; 548 effectiveMass.a33 += softnessOverDt; 549 550 effectiveMass = effectiveMass.inverse; 551 552 auto rot1 = body1.orientation.toMatrix3x3; 553 auto rot2 = body2.orientation.toMatrix3x3; 554 555 Vector3f worldAxis1 = localAxis1 * rot1; 556 Vector3f worldAxis2 = localAxis2 * rot2; 557 558 worldConstrAxis1 = localConstrAxis1 * rot1; 559 worldConstrAxis2 = localConstrAxis2 * rot2; 560 561 Vector3f error = cross(worldAxis1, worldAxis2); 562 563 Vector3f errorAxis = Vector3f(0, 0, 0); 564 errorAxis.x = dot(error, worldConstrAxis1); 565 errorAxis.y = dot(error, worldConstrAxis2); 566 567 bias = errorAxis * biasFactor * (-1.0f / dt); 568 569 Vector3f impulse; 570 impulse.x = worldConstrAxis1.x * accumulatedImpulse.x 571 + worldConstrAxis2.x * accumulatedImpulse.y; 572 impulse.y = worldConstrAxis1.y * accumulatedImpulse.x 573 + worldConstrAxis2.y * accumulatedImpulse.y; 574 impulse.z = worldConstrAxis1.z * accumulatedImpulse.x 575 + worldConstrAxis2.z * accumulatedImpulse.y; 576 577 if (body1.dynamic) 578 body1.angularVelocity += impulse * body1.invInertiaTensor; 579 if (body2.dynamic) 580 body2.angularVelocity += -impulse * body2.invInertiaTensor; 581 } 582 583 override void step() 584 { 585 Vector3f vd = body1.angularVelocity - body2.angularVelocity; 586 Vector3f jv = Vector3f(0, 0, 0); 587 jv.x = dot(vd, worldConstrAxis1); 588 jv.y = dot(vd, worldConstrAxis2); 589 590 Vector3f softnessVector = accumulatedImpulse * softnessOverDt; 591 592 Vector3f lambda = -(jv + bias + softnessVector) * effectiveMass; 593 accumulatedImpulse += lambda; 594 595 Vector3f impulse; 596 impulse.x = worldConstrAxis1.x * lambda.x + worldConstrAxis2.x * lambda.y; 597 impulse.y = worldConstrAxis1.y * lambda.x + worldConstrAxis2.y * lambda.y; 598 impulse.z = worldConstrAxis1.z * lambda.x + worldConstrAxis2.z * lambda.y; 599 600 if (body1.dynamic) 601 body1.angularVelocity += impulse * body1.invInertiaTensor; 602 if (body2.dynamic) 603 body2.angularVelocity += -impulse * body2.invInertiaTensor; 604 } 605 } 606 607 /* 608 * Combination of SliderConstraint and AngleConstraint. 609 * Restrics 5 degrees of freedom so that bodies can only move in one direction 610 * relative to each other. 611 */ 612 class PrismaticConstraint: Constraint 613 { 614 AngleConstraint ac; 615 SliderConstraint sc; 616 617 this(Owner owner, RigidBody body1, RigidBody body2) 618 { 619 super(owner); 620 this.body1 = body1; 621 this.body2 = body2; 622 623 ac = New!AngleConstraint(this, body1, body2); 624 sc = New!SliderConstraint(this, body1, body2, 625 Vector3f(0, 0, 0), Vector3f(0, 0, 0)); 626 } 627 628 override void prepare(double dt) 629 { 630 ac.prepare(dt); 631 sc.prepare(dt); 632 } 633 634 override void step() 635 { 636 ac.step(); 637 sc.step(); 638 } 639 } 640 641 /* 642 * Combination of BallConstraint and AxisAngleConstraint. 643 * Restricts 5 degrees of freedom, so the bodies are fixed relative to 644 * anchor point and can only rotate around one axis. 645 * This can be useful to represent doors or wheels. 646 */ 647 class HingeConstraint: Constraint 648 { 649 AxisAngleConstraint aac; 650 BallConstraint bc; 651 652 this(Owner owner, 653 RigidBody body1, 654 RigidBody body2, 655 Vector3f anchor1, 656 Vector3f anchor2, 657 Vector3f axis) 658 { 659 super(owner); 660 this.body1 = body1; 661 this.body2 = body2; 662 663 aac = New!AxisAngleConstraint(this, body1, body2, axis); 664 bc = New!BallConstraint(this, body1, body2, anchor1, anchor2); 665 } 666 667 override void prepare(double dt) 668 { 669 aac.prepare(dt); 670 bc.prepare(dt); 671 } 672 673 override void step() 674 { 675 aac.step(); 676 bc.step(); 677 } 678 }