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