1 /* 2 Copyright (c) 2013-2018 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.rigidbody; 30 31 import std.math; 32 33 import dlib.core.ownership; 34 import dlib.core.memory; 35 import dlib.container.array; 36 import dlib.math.vector; 37 import dlib.math.matrix; 38 import dlib.math.quaternion; 39 import dlib.math.transformation; 40 import dlib.math.utils; 41 42 import dmech.world; 43 import dmech.shape; 44 import dmech.contact; 45 46 interface CollisionDispatcher 47 { 48 void onNewContact(RigidBody rb, Contact c); 49 } 50 51 class RigidBody: Owner 52 { 53 Vector3f position; 54 Quaternionf orientation; 55 56 Vector3f linearVelocity; 57 Vector3f angularVelocity; 58 59 Vector3f pseudoLinearVelocity; 60 Vector3f pseudoAngularVelocity; 61 62 Vector3f linearAcceleration; 63 Vector3f angularAcceleration; 64 65 float mass; 66 float invMass; 67 68 Matrix3x3f inertiaTensor; 69 Matrix3x3f invInertiaTensor; //TODO: rename to invInertiaWorld 70 71 Vector3f centerOfMass; 72 73 Vector3f forceAccumulator; 74 Vector3f torqueAccumulator; 75 76 float bounce; 77 float friction; 78 79 bool active = true; 80 bool useGravity = true; 81 bool enableRotation = true; 82 83 DynamicArray!ShapeComponent shapes; 84 85 bool dynamic; 86 87 float damping = 0.5f; 88 float stopThreshold = 0.15f; //0.15f 89 float stopThresholdPV = 0.0f; //0.01f 90 float stopThresholdAngular = 0.2f; 91 92 bool useOwnGravity = false; 93 Vector3f gravity = Vector3f(0, 0, 0); 94 95 DynamicArray!CollisionDispatcher collisionDispatchers; 96 97 void contactEvent(Contact c) 98 { 99 foreach(d; collisionDispatchers.data) 100 { 101 d.onNewContact(this, c); 102 } 103 } 104 105 bool raycastable = true; 106 uint numContacts = 0; 107 bool useFriction = true; 108 109 float maxSpeed = float.max; 110 111 this(PhysicsWorld world) 112 { 113 super(world); 114 115 position = Vector3f(0.0f, 0.0f, 0.0f); 116 orientation = Quaternionf(0.0f, 0.0f, 0.0f, 1.0f); 117 118 linearVelocity = Vector3f(0.0f, 0.0f, 0.0f); 119 angularVelocity = Vector3f(0.0f, 0.0f, 0.0f); 120 121 pseudoLinearVelocity = Vector3f(0.0f, 0.0f, 0.0f); 122 pseudoAngularVelocity = Vector3f(0.0f, 0.0f, 0.0f); 123 124 linearAcceleration = Vector3f(0.0f, 0.0f, 0.0f); 125 angularAcceleration = Vector3f(0.0f, 0.0f, 0.0f); 126 127 mass = 1.0f; 128 invMass = 1.0f; 129 130 inertiaTensor = matrixf( 131 mass, 0, 0, 132 0, mass, 0, 133 0, 0, mass 134 ); 135 invInertiaTensor = matrixf( 136 0, 0, 0, 137 0, 0, 0, 138 0, 0, 0 139 ); 140 141 centerOfMass = Vector3f(0.0f, 0.0f, 0.0f); 142 143 forceAccumulator = Vector3f(0.0f, 0.0f, 0.0f); 144 torqueAccumulator = Vector3f(0.0f, 0.0f, 0.0f); 145 146 bounce = 0.0f; 147 friction = 0.5f; 148 149 dynamic = true; 150 } 151 152 Matrix4x4f transformation() 153 { 154 Matrix4x4f t; 155 t = translationMatrix(position); 156 t *= orientation.toMatrix4x4(); 157 return t; 158 } 159 160 void addShapeComponent(ShapeComponent shape) 161 { 162 shape.transformation = transformation() * translationMatrix(shape.centroid); 163 shapes.append(shape); 164 165 if (!dynamic) 166 return; 167 168 centerOfMass = Vector3f(0, 0, 0); 169 float m = 0.0f; 170 171 foreach (sh; shapes.data) 172 { 173 m += sh.mass; 174 centerOfMass += sh.mass * sh.centroid; 175 } 176 177 float invm = 1.0f / m; 178 centerOfMass *= invm; 179 180 mass += shape.mass; 181 invMass = 1.0f / mass; 182 183 // Compute inertia tensor using Huygens-Steiner theorem 184 inertiaTensor = Matrix3x3f.zero; 185 foreach (sh; shapes.data) 186 { 187 Vector3f r = centerOfMass - sh.centroid; 188 inertiaTensor += 189 sh.geometry.inertiaTensor(sh.mass) + 190 (Matrix3x3f.identity * dot(r, r) - tensorProduct(r, r)) * sh.mass; 191 } 192 193 invInertiaTensor = inertiaTensor.inverse; 194 } 195 196 void integrateForces(float dt) 197 { 198 if (!active || !dynamic) 199 return; 200 201 linearAcceleration = forceAccumulator * invMass; 202 if (enableRotation) 203 angularAcceleration = torqueAccumulator * invInertiaTensor; 204 205 linearVelocity += linearAcceleration * dt; 206 if (enableRotation) 207 angularVelocity += angularAcceleration * dt; 208 } 209 210 void integrateVelocities(float dt) 211 { 212 if (!active || !dynamic) 213 return; 214 215 float d = clamp(1.0f - dt * damping, 0.0f, 1.0f); 216 linearVelocity *= d; 217 angularVelocity *= d; 218 219 float speed = linearVelocity.length; 220 221 if (speed > maxSpeed) 222 linearVelocity = linearVelocity.normalized * maxSpeed; 223 224 if (speed > stopThreshold /* || numContacts < 3 */) 225 { 226 position += linearVelocity * dt; 227 } 228 229 if (enableRotation) 230 if (angularVelocity.length > stopThresholdAngular /* || numContacts < 3 */) //stopThreshold 231 { 232 orientation += 0.5f * Quaternionf(angularVelocity, 0.0f) * orientation * dt; 233 orientation.normalize(); 234 } 235 } 236 237 void integratePseudoVelocities(float dt) 238 { 239 if (!active || !dynamic) 240 return; 241 242 float d = clamp(1.0f - dt * damping, 0.0f, 1.0f); 243 244 pseudoLinearVelocity *= d; 245 pseudoAngularVelocity *= d; 246 247 if (pseudoLinearVelocity.length > stopThresholdPV) 248 { 249 position += pseudoLinearVelocity; 250 } 251 252 if (enableRotation) 253 if (pseudoAngularVelocity.length > stopThresholdPV) 254 { 255 orientation += 0.5f * Quaternionf(pseudoAngularVelocity, 0.0f) * orientation; 256 orientation.normalize(); 257 } 258 259 pseudoLinearVelocity = Vector3f(0.0f, 0.0f, 0.0f); 260 pseudoAngularVelocity = Vector3f(0.0f, 0.0f, 0.0f); 261 } 262 263 @property Vector3f worldCenterOfMass() 264 { 265 return position + orientation.rotate(centerOfMass); 266 } 267 268 void updateInertia() 269 { 270 if (active && dynamic) 271 { 272 auto rot = orientation.toMatrix3x3; 273 //invInertiaTensor = (rot * inertiaTensor * rot.transposed).inverse; 274 invInertiaTensor = (rot * inertiaTensor.inverse * rot.transposed); 275 } 276 } 277 278 void updateShapeComponents() 279 { 280 foreach (sh; shapes.data) 281 { 282 sh.transformation = transformation() * translationMatrix(sh.centroid); 283 } 284 } 285 286 void resetForces() 287 { 288 forceAccumulator = Vector3f(0, 0, 0); 289 torqueAccumulator = Vector3f(0, 0, 0); 290 } 291 292 void applyForce(Vector3f force) 293 { 294 if (!active || !dynamic) 295 return; 296 297 forceAccumulator += force; 298 } 299 300 void applyTorque(Vector3f torque) 301 { 302 if (!active || !dynamic) 303 return; 304 305 torqueAccumulator += torque; 306 } 307 308 void applyImpulse(Vector3f impulse, Vector3f point) 309 { 310 if (!active || !dynamic) 311 return; 312 313 linearVelocity += impulse * invMass; 314 Vector3f angularImpulse = cross(point - worldCenterOfMass, impulse); 315 angularVelocity += angularImpulse * invInertiaTensor; 316 } 317 318 void applyForceAtPos(Vector3f force, Vector3f pos) 319 { 320 if (!active || !dynamic) 321 return; 322 323 forceAccumulator += force; 324 torqueAccumulator += cross(pos - worldCenterOfMass, force); 325 } 326 327 void applyPseudoImpulse(Vector3f impulse, Vector3f point) 328 { 329 if (!active || !dynamic) 330 return; 331 332 pseudoLinearVelocity += impulse * invMass; 333 Vector3f angularImpulse = cross(point - worldCenterOfMass, impulse); 334 pseudoAngularVelocity += angularImpulse * invInertiaTensor; 335 } 336 337 @property float linearKineticEnergy() 338 { 339 if (!active || !dynamic) 340 return float.infinity; 341 342 // 0.5 * m * v^2 343 float v = linearVelocity.length; 344 return 0.5f * mass * v * v; 345 } 346 347 @property float angularKineticEnergy() 348 { 349 if (!active || !dynamic) 350 return float.infinity; 351 352 // 0.5 * w * I * w 353 Vector3f w = angularVelocity; 354 return 0.5f * dot(w * inertiaTensor, w); 355 } 356 357 ~this() 358 { 359 shapes.free(); 360 collisionDispatchers.free(); 361 } 362 }