Physics_AF.h (44288B)
1 /* 2 =========================================================================== 3 4 Doom 3 BFG Edition GPL Source Code 5 Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 6 7 This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code"). 8 9 Doom 3 BFG Edition Source Code is free software: you can redistribute it and/or modify 10 it under the terms of the GNU General Public License as published by 11 the Free Software Foundation, either version 3 of the License, or 12 (at your option) any later version. 13 14 Doom 3 BFG Edition Source Code is distributed in the hope that it will be useful, 15 but WITHOUT ANY WARRANTY; without even the implied warranty of 16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 GNU General Public License for more details. 18 19 You should have received a copy of the GNU General Public License 20 along with Doom 3 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>. 21 22 In addition, the Doom 3 BFG Edition Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 BFG Edition Source Code. If not, please request a copy in writing from id Software at the address below. 23 24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA. 25 26 =========================================================================== 27 */ 28 29 #ifndef __PHYSICS_AF_H__ 30 #define __PHYSICS_AF_H__ 31 32 /* 33 =================================================================================== 34 35 Articulated Figure physics 36 37 Employs a constraint force based dynamic simulation using a lagrangian 38 multiplier method to solve for the constraint forces. 39 40 =================================================================================== 41 */ 42 43 class idAFConstraint; 44 class idAFConstraint_Fixed; 45 class idAFConstraint_BallAndSocketJoint; 46 class idAFConstraint_BallAndSocketJointFriction; 47 class idAFConstraint_UniversalJoint; 48 class idAFConstraint_UniversalJointFriction; 49 class idAFConstraint_CylindricalJoint; 50 class idAFConstraint_Hinge; 51 class idAFConstraint_HingeFriction; 52 class idAFConstraint_HingeSteering; 53 class idAFConstraint_Slider; 54 class idAFConstraint_Line; 55 class idAFConstraint_Plane; 56 class idAFConstraint_Spring; 57 class idAFConstraint_Contact; 58 class idAFConstraint_ContactFriction; 59 class idAFConstraint_ConeLimit; 60 class idAFConstraint_PyramidLimit; 61 class idAFConstraint_Suspension; 62 class idAFBody; 63 class idAFTree; 64 class idPhysics_AF; 65 66 typedef enum { 67 CONSTRAINT_INVALID, 68 CONSTRAINT_FIXED, 69 CONSTRAINT_BALLANDSOCKETJOINT, 70 CONSTRAINT_UNIVERSALJOINT, 71 CONSTRAINT_HINGE, 72 CONSTRAINT_HINGESTEERING, 73 CONSTRAINT_SLIDER, 74 CONSTRAINT_CYLINDRICALJOINT, 75 CONSTRAINT_LINE, 76 CONSTRAINT_PLANE, 77 CONSTRAINT_SPRING, 78 CONSTRAINT_CONTACT, 79 CONSTRAINT_FRICTION, 80 CONSTRAINT_CONELIMIT, 81 CONSTRAINT_PYRAMIDLIMIT, 82 CONSTRAINT_SUSPENSION 83 } constraintType_t; 84 85 86 //=============================================================== 87 // 88 // idAFConstraint 89 // 90 //=============================================================== 91 92 // base class for all constraints 93 class idAFConstraint { 94 95 friend class idPhysics_AF; 96 friend class idAFTree; 97 98 public: 99 idAFConstraint(); 100 virtual ~idAFConstraint(); 101 constraintType_t GetType() const { return type; } 102 const idStr & GetName() const { return name; } 103 idAFBody * GetBody1() const { return body1; } 104 idAFBody * GetBody2() const { return body2; } 105 void SetPhysics( idPhysics_AF *p ) { physics = p; } 106 const idVecX & GetMultiplier(); 107 virtual void SetBody1( idAFBody *body ); 108 virtual void SetBody2( idAFBody *body ); 109 virtual void DebugDraw(); 110 virtual void GetForce( idAFBody *body, idVec6 &force ); 111 virtual void Translate( const idVec3 &translation ); 112 virtual void Rotate( const idRotation &rotation ); 113 virtual void GetCenter( idVec3 ¢er ); 114 virtual void Save( idSaveGame *saveFile ) const; 115 virtual void Restore( idRestoreGame *saveFile ); 116 117 protected: 118 constraintType_t type; // constraint type 119 idStr name; // name of constraint 120 idAFBody * body1; // first constrained body 121 idAFBody * body2; // second constrained body, NULL for world 122 idPhysics_AF * physics; // for adding additional constraints like limits 123 124 // simulation variables set by Evaluate 125 idMatX J1, J2; // matrix with left hand side of constraint equations 126 idVecX c1, c2; // right hand side of constraint equations 127 idVecX lo, hi, e; // low and high bounds and lcp epsilon 128 idAFConstraint * boxConstraint; // constraint the boxIndex refers to 129 int boxIndex[6]; // indexes for special box constrained variables 130 131 // simulation variables used during calculations 132 idMatX invI; // transformed inertia 133 idMatX J; // transformed constraint matrix 134 idVecX s; // temp solution 135 idVecX lm; // lagrange multipliers 136 int firstIndex; // index of the first constraint row in the lcp matrix 137 138 struct constraintFlags_s { 139 bool allowPrimary : 1; // true if the constraint can be used as a primary constraint 140 bool frameConstraint : 1; // true if this constraint is added to the frame constraints 141 bool noCollision : 1; // true if body1 and body2 never collide with each other 142 bool isPrimary : 1; // true if this is a primary constraint 143 bool isZero : 1; // true if 's' is zero during calculations 144 } fl; 145 146 protected: 147 virtual void Evaluate( float invTimeStep ); 148 virtual void ApplyFriction( float invTimeStep ); 149 void InitSize( int size ); 150 }; 151 152 // fixed or rigid joint which allows zero degrees of freedom 153 // constrains body1 to have a fixed position and orientation relative to body2 154 class idAFConstraint_Fixed : public idAFConstraint { 155 156 public: 157 idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFBody *body2 ); 158 void SetRelativeOrigin( const idVec3 &origin ) { this->offset = origin; } 159 void SetRelativeAxis( const idMat3 &axis ) { this->relAxis = axis; } 160 virtual void SetBody1( idAFBody *body ); 161 virtual void SetBody2( idAFBody *body ); 162 virtual void DebugDraw(); 163 virtual void Translate( const idVec3 &translation ); 164 virtual void Rotate( const idRotation &rotation ); 165 virtual void GetCenter( idVec3 ¢er ); 166 virtual void Save( idSaveGame *saveFile ) const; 167 virtual void Restore( idRestoreGame *saveFile ); 168 169 protected: 170 idVec3 offset; // offset of body1 relative to body2 in body2 space 171 idMat3 relAxis; // rotation of body1 relative to body2 172 173 protected: 174 virtual void Evaluate( float invTimeStep ); 175 virtual void ApplyFriction( float invTimeStep ); 176 void InitOffset(); 177 }; 178 179 // ball and socket or spherical joint which allows 3 degrees of freedom 180 // constrains body1 relative to body2 with a ball and socket joint 181 class idAFConstraint_BallAndSocketJoint : public idAFConstraint { 182 183 public: 184 idAFConstraint_BallAndSocketJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ); 185 ~idAFConstraint_BallAndSocketJoint(); 186 void SetAnchor( const idVec3 &worldPosition ); 187 idVec3 GetAnchor() const; 188 void SetNoLimit(); 189 void SetConeLimit( const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ); 190 void SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis, 191 const float angle1, const float angle2, const idVec3 &body1Axis ); 192 void SetLimitEpsilon( const float e ); 193 void SetFriction( const float f ) { friction = f; } 194 float GetFriction() const; 195 virtual void DebugDraw(); 196 virtual void GetForce( idAFBody *body, idVec6 &force ); 197 virtual void Translate( const idVec3 &translation ); 198 virtual void Rotate( const idRotation &rotation ); 199 virtual void GetCenter( idVec3 ¢er ); 200 virtual void Save( idSaveGame *saveFile ) const; 201 virtual void Restore( idRestoreGame *saveFile ); 202 203 protected: 204 idVec3 anchor1; // anchor in body1 space 205 idVec3 anchor2; // anchor in body2 space 206 float friction; // joint friction 207 idAFConstraint_ConeLimit *coneLimit; // cone shaped limit 208 idAFConstraint_PyramidLimit *pyramidLimit; // pyramid shaped limit 209 idAFConstraint_BallAndSocketJointFriction *fc; // friction constraint 210 211 protected: 212 virtual void Evaluate( float invTimeStep ); 213 virtual void ApplyFriction( float invTimeStep ); 214 }; 215 216 // ball and socket joint friction 217 class idAFConstraint_BallAndSocketJointFriction : public idAFConstraint { 218 219 public: 220 idAFConstraint_BallAndSocketJointFriction(); 221 void Setup( idAFConstraint_BallAndSocketJoint *cc ); 222 bool Add( idPhysics_AF *phys, float invTimeStep ); 223 virtual void Translate( const idVec3 &translation ); 224 virtual void Rotate( const idRotation &rotation ); 225 226 protected: 227 idAFConstraint_BallAndSocketJoint *joint; 228 229 protected: 230 virtual void Evaluate( float invTimeStep ); 231 virtual void ApplyFriction( float invTimeStep ); 232 }; 233 234 // universal, Cardan or Hooke joint which allows 2 degrees of freedom 235 // like a ball and socket joint but also constrains the rotation about the cardan shafts 236 class idAFConstraint_UniversalJoint : public idAFConstraint { 237 238 public: 239 idAFConstraint_UniversalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ); 240 ~idAFConstraint_UniversalJoint(); 241 void SetAnchor( const idVec3 &worldPosition ); 242 idVec3 GetAnchor() const; 243 void SetShafts( const idVec3 &cardanShaft1, const idVec3 &cardanShaft2 ); 244 void GetShafts( idVec3 &cardanShaft1, idVec3 &cardanShaft2 ) { cardanShaft1 = shaft1; cardanShaft2 = shaft2; } 245 void SetNoLimit(); 246 void SetConeLimit( const idVec3 &coneAxis, const float coneAngle ); 247 void SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis, 248 const float angle1, const float angle2 ); 249 void SetLimitEpsilon( const float e ); 250 void SetFriction( const float f ) { friction = f; } 251 float GetFriction() const; 252 virtual void DebugDraw(); 253 virtual void GetForce( idAFBody *body, idVec6 &force ); 254 virtual void Translate( const idVec3 &translation ); 255 virtual void Rotate( const idRotation &rotation ); 256 virtual void GetCenter( idVec3 ¢er ); 257 virtual void Save( idSaveGame *saveFile ) const; 258 virtual void Restore( idRestoreGame *saveFile ); 259 260 protected: 261 idVec3 anchor1; // anchor in body1 space 262 idVec3 anchor2; // anchor in body2 space 263 idVec3 shaft1; // body1 cardan shaft in body1 space 264 idVec3 shaft2; // body2 cardan shaft in body2 space 265 idVec3 axis1; // cardan axis in body1 space 266 idVec3 axis2; // cardan axis in body2 space 267 float friction; // joint friction 268 idAFConstraint_ConeLimit *coneLimit; // cone shaped limit 269 idAFConstraint_PyramidLimit *pyramidLimit; // pyramid shaped limit 270 idAFConstraint_UniversalJointFriction *fc; // friction constraint 271 272 protected: 273 virtual void Evaluate( float invTimeStep ); 274 virtual void ApplyFriction( float invTimeStep ); 275 }; 276 277 // universal joint friction 278 class idAFConstraint_UniversalJointFriction : public idAFConstraint { 279 280 public: 281 idAFConstraint_UniversalJointFriction(); 282 void Setup( idAFConstraint_UniversalJoint *cc ); 283 bool Add( idPhysics_AF *phys, float invTimeStep ); 284 virtual void Translate( const idVec3 &translation ); 285 virtual void Rotate( const idRotation &rotation ); 286 287 protected: 288 idAFConstraint_UniversalJoint *joint; // universal joint 289 290 protected: 291 virtual void Evaluate( float invTimeStep ); 292 virtual void ApplyFriction( float invTimeStep ); 293 }; 294 295 // cylindrical joint which allows 2 degrees of freedom 296 // constrains body1 to lie on a line relative to body2 and allows only translation along and rotation about the line 297 class idAFConstraint_CylindricalJoint : public idAFConstraint { 298 299 public: 300 idAFConstraint_CylindricalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ); 301 virtual void DebugDraw(); 302 virtual void Translate( const idVec3 &translation ); 303 virtual void Rotate( const idRotation &rotation ); 304 305 protected: 306 307 protected: 308 virtual void Evaluate( float invTimeStep ); 309 virtual void ApplyFriction( float invTimeStep ); 310 }; 311 312 // hinge, revolute or pin joint which allows 1 degree of freedom 313 // constrains all motion of body1 relative to body2 except the rotation about the hinge axis 314 class idAFConstraint_Hinge : public idAFConstraint { 315 316 public: 317 idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFBody *body2 ); 318 ~idAFConstraint_Hinge(); 319 void SetAnchor( const idVec3 &worldPosition ); 320 idVec3 GetAnchor() const; 321 void SetAxis( const idVec3 &axis ); 322 void GetAxis( idVec3 &a1, idVec3 &a2 ) const { a1 = axis1; a2 = axis2; } 323 idVec3 GetAxis() const; 324 void SetNoLimit(); 325 void SetLimit( const idVec3 &axis, const float angle, const idVec3 &body1Axis ); 326 void SetLimitEpsilon( const float e ); 327 float GetAngle() const; 328 void SetSteerAngle( const float degrees ); 329 void SetSteerSpeed( const float speed ); 330 void SetFriction( const float f ) { friction = f; } 331 float GetFriction() const; 332 virtual void DebugDraw(); 333 virtual void GetForce( idAFBody *body, idVec6 &force ); 334 virtual void Translate( const idVec3 &translation ); 335 virtual void Rotate( const idRotation &rotation ); 336 virtual void GetCenter( idVec3 ¢er ); 337 virtual void Save( idSaveGame *saveFile ) const; 338 virtual void Restore( idRestoreGame *saveFile ); 339 340 protected: 341 idVec3 anchor1; // anchor in body1 space 342 idVec3 anchor2; // anchor in body2 space 343 idVec3 axis1; // axis in body1 space 344 idVec3 axis2; // axis in body2 space 345 idMat3 initialAxis; // initial axis of body1 relative to body2 346 float friction; // hinge friction 347 idAFConstraint_ConeLimit *coneLimit; // cone limit 348 idAFConstraint_HingeSteering *steering; // steering 349 idAFConstraint_HingeFriction *fc; // friction constraint 350 351 protected: 352 virtual void Evaluate( float invTimeStep ); 353 virtual void ApplyFriction( float invTimeStep ); 354 }; 355 356 // hinge joint friction 357 class idAFConstraint_HingeFriction : public idAFConstraint { 358 359 public: 360 idAFConstraint_HingeFriction(); 361 void Setup( idAFConstraint_Hinge *cc ); 362 bool Add( idPhysics_AF *phys, float invTimeStep ); 363 virtual void Translate( const idVec3 &translation ); 364 virtual void Rotate( const idRotation &rotation ); 365 366 protected: 367 idAFConstraint_Hinge * hinge; // hinge 368 369 protected: 370 virtual void Evaluate( float invTimeStep ); 371 virtual void ApplyFriction( float invTimeStep ); 372 }; 373 374 // constrains two bodies attached to each other with a hinge to get a specified relative orientation 375 class idAFConstraint_HingeSteering : public idAFConstraint { 376 377 public: 378 idAFConstraint_HingeSteering(); 379 void Setup( idAFConstraint_Hinge *cc ); 380 void SetSteerAngle( const float degrees ) { steerAngle = degrees; } 381 void SetSteerSpeed( const float speed ) { steerSpeed = speed; } 382 void SetEpsilon( const float e ) { epsilon = e; } 383 bool Add( idPhysics_AF *phys, float invTimeStep ); 384 virtual void Translate( const idVec3 &translation ); 385 virtual void Rotate( const idRotation &rotation ); 386 387 virtual void Save( idSaveGame *saveFile ) const; 388 virtual void Restore( idRestoreGame *saveFile ); 389 390 protected: 391 idAFConstraint_Hinge * hinge; // hinge 392 float steerAngle; // desired steer angle in degrees 393 float steerSpeed; // steer speed 394 float epsilon; // lcp epsilon 395 396 protected: 397 virtual void Evaluate( float invTimeStep ); 398 virtual void ApplyFriction( float invTimeStep ); 399 }; 400 401 // slider, prismatic or translational constraint which allows 1 degree of freedom 402 // constrains body1 to lie on a line relative to body2, the orientation is also fixed relative to body2 403 class idAFConstraint_Slider : public idAFConstraint { 404 405 public: 406 idAFConstraint_Slider( const idStr &name, idAFBody *body1, idAFBody *body2 ); 407 void SetAxis( const idVec3 &ax ); 408 virtual void DebugDraw(); 409 virtual void Translate( const idVec3 &translation ); 410 virtual void Rotate( const idRotation &rotation ); 411 virtual void GetCenter( idVec3 ¢er ); 412 virtual void Save( idSaveGame *saveFile ) const; 413 virtual void Restore( idRestoreGame *saveFile ); 414 415 protected: 416 idVec3 axis; // axis along which body1 slides in body2 space 417 idVec3 offset; // offset of body1 relative to body2 418 idMat3 relAxis; // rotation of body1 relative to body2 419 420 protected: 421 virtual void Evaluate( float invTimeStep ); 422 virtual void ApplyFriction( float invTimeStep ); 423 }; 424 425 // line constraint which allows 4 degrees of freedom 426 // constrains body1 to lie on a line relative to body2, does not constrain the orientation. 427 class idAFConstraint_Line : public idAFConstraint { 428 429 public: 430 idAFConstraint_Line( const idStr &name, idAFBody *body1, idAFBody *body2 ); 431 virtual void DebugDraw(); 432 virtual void Translate( const idVec3 &translation ); 433 virtual void Rotate( const idRotation &rotation ); 434 435 protected: 436 437 protected: 438 virtual void Evaluate( float invTimeStep ); 439 virtual void ApplyFriction( float invTimeStep ); 440 }; 441 442 // plane constraint which allows 5 degrees of freedom 443 // constrains body1 to lie in a plane relative to body2, does not constrain the orientation. 444 class idAFConstraint_Plane : public idAFConstraint { 445 446 public: 447 idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFBody *body2 ); 448 void SetPlane( const idVec3 &normal, const idVec3 &anchor ); 449 virtual void DebugDraw(); 450 virtual void Translate( const idVec3 &translation ); 451 virtual void Rotate( const idRotation &rotation ); 452 virtual void Save( idSaveGame *saveFile ) const; 453 virtual void Restore( idRestoreGame *saveFile ); 454 455 protected: 456 idVec3 anchor1; // anchor in body1 space 457 idVec3 anchor2; // anchor in body2 space 458 idVec3 planeNormal; // plane normal in body2 space 459 460 protected: 461 virtual void Evaluate( float invTimeStep ); 462 virtual void ApplyFriction( float invTimeStep ); 463 }; 464 465 // spring constraint which allows 6 or 5 degrees of freedom based on the spring limits 466 // constrains body1 relative to body2 with a spring 467 class idAFConstraint_Spring : public idAFConstraint { 468 469 public: 470 idAFConstraint_Spring( const idStr &name, idAFBody *body1, idAFBody *body2 ); 471 void SetAnchor( const idVec3 &worldAnchor1, const idVec3 &worldAnchor2 ); 472 void SetSpring( const float stretch, const float compress, const float damping, const float restLength ); 473 void SetLimit( const float minLength, const float maxLength ); 474 virtual void DebugDraw(); 475 virtual void Translate( const idVec3 &translation ); 476 virtual void Rotate( const idRotation &rotation ); 477 virtual void GetCenter( idVec3 ¢er ); 478 virtual void Save( idSaveGame *saveFile ) const; 479 virtual void Restore( idRestoreGame *saveFile ); 480 481 protected: 482 idVec3 anchor1; // anchor in body1 space 483 idVec3 anchor2; // anchor in body2 space 484 float kstretch; // spring constant when stretched 485 float kcompress; // spring constant when compressed 486 float damping; // spring damping 487 float restLength; // rest length of spring 488 float minLength; // minimum spring length 489 float maxLength; // maximum spring length 490 491 protected: 492 virtual void Evaluate( float invTimeStep ); 493 virtual void ApplyFriction( float invTimeStep ); 494 }; 495 496 // constrains body1 to either be in contact with or move away from body2 497 class idAFConstraint_Contact : public idAFConstraint { 498 499 public: 500 idAFConstraint_Contact(); 501 ~idAFConstraint_Contact(); 502 void Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c ); 503 const contactInfo_t & GetContact() const { return contact; } 504 virtual void DebugDraw(); 505 virtual void Translate( const idVec3 &translation ); 506 virtual void Rotate( const idRotation &rotation ); 507 virtual void GetCenter( idVec3 ¢er ); 508 509 protected: 510 contactInfo_t contact; // contact information 511 idAFConstraint_ContactFriction *fc; // contact friction 512 513 protected: 514 virtual void Evaluate( float invTimeStep ); 515 virtual void ApplyFriction( float invTimeStep ); 516 }; 517 518 // contact friction 519 class idAFConstraint_ContactFriction : public idAFConstraint { 520 521 public: 522 idAFConstraint_ContactFriction(); 523 void Setup( idAFConstraint_Contact *cc ); 524 bool Add( idPhysics_AF *phys, float invTimeStep ); 525 virtual void DebugDraw(); 526 virtual void Translate( const idVec3 &translation ); 527 virtual void Rotate( const idRotation &rotation ); 528 529 protected: 530 idAFConstraint_Contact *cc; // contact constraint 531 532 protected: 533 virtual void Evaluate( float invTimeStep ); 534 virtual void ApplyFriction( float invTimeStep ); 535 }; 536 537 // constrains an axis attached to body1 to be inside a cone relative to body2 538 class idAFConstraint_ConeLimit : public idAFConstraint { 539 540 public: 541 idAFConstraint_ConeLimit(); 542 void Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis, 543 const float coneAngle, const idVec3 &body1Axis ); 544 void SetAnchor( const idVec3 &coneAnchor ); 545 void SetBody1Axis( const idVec3 &body1Axis ); 546 void SetEpsilon( const float e ) { epsilon = e; } 547 bool Add( idPhysics_AF *phys, float invTimeStep ); 548 virtual void DebugDraw(); 549 virtual void Translate( const idVec3 &translation ); 550 virtual void Rotate( const idRotation &rotation ); 551 virtual void Save( idSaveGame *saveFile ) const; 552 virtual void Restore( idRestoreGame *saveFile ); 553 554 protected: 555 idVec3 coneAnchor; // top of the cone in body2 space 556 idVec3 coneAxis; // cone axis in body2 space 557 idVec3 body1Axis; // axis in body1 space that should stay within the cone 558 float cosAngle; // cos( coneAngle / 2 ) 559 float sinHalfAngle; // sin( coneAngle / 4 ) 560 float cosHalfAngle; // cos( coneAngle / 4 ) 561 float epsilon; // lcp epsilon 562 563 protected: 564 virtual void Evaluate( float invTimeStep ); 565 virtual void ApplyFriction( float invTimeStep ); 566 }; 567 568 // constrains an axis attached to body1 to be inside a pyramid relative to body2 569 class idAFConstraint_PyramidLimit : public idAFConstraint { 570 571 public: 572 idAFConstraint_PyramidLimit(); 573 void Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor, 574 const idVec3 &pyramidAxis, const idVec3 &baseAxis, 575 const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis ); 576 void SetAnchor( const idVec3 &pyramidAxis ); 577 void SetBody1Axis( const idVec3 &body1Axis ); 578 void SetEpsilon( const float e ) { epsilon = e; } 579 bool Add( idPhysics_AF *phys, float invTimeStep ); 580 virtual void DebugDraw(); 581 virtual void Translate( const idVec3 &translation ); 582 virtual void Rotate( const idRotation &rotation ); 583 virtual void Save( idSaveGame *saveFile ) const; 584 virtual void Restore( idRestoreGame *saveFile ); 585 586 protected: 587 idVec3 pyramidAnchor; // top of the pyramid in body2 space 588 idMat3 pyramidBasis; // pyramid basis in body2 space with base[2] being the pyramid axis 589 idVec3 body1Axis; // axis in body1 space that should stay within the cone 590 float cosAngle[2]; // cos( pyramidAngle / 2 ) 591 float sinHalfAngle[2]; // sin( pyramidAngle / 4 ) 592 float cosHalfAngle[2]; // cos( pyramidAngle / 4 ) 593 float epsilon; // lcp epsilon 594 595 protected: 596 virtual void Evaluate( float invTimeStep ); 597 virtual void ApplyFriction( float invTimeStep ); 598 }; 599 600 // vehicle suspension 601 class idAFConstraint_Suspension : public idAFConstraint { 602 603 public: 604 idAFConstraint_Suspension(); 605 606 void Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel ); 607 void SetSuspension( const float up, const float down, const float k, const float d, const float f ); 608 609 void SetSteerAngle( const float degrees ) { steerAngle = degrees; } 610 void EnableMotor( const bool enable ) { motorEnabled = enable; } 611 void SetMotorForce( const float force ) { motorForce = force; } 612 void SetMotorVelocity( const float vel ) { motorVelocity = vel; } 613 void SetEpsilon( const float e ) { epsilon = e; } 614 const idVec3 GetWheelOrigin() const; 615 616 virtual void DebugDraw(); 617 virtual void Translate( const idVec3 &translation ); 618 virtual void Rotate( const idRotation &rotation ); 619 620 protected: 621 idVec3 localOrigin; // position of suspension relative to body1 622 idMat3 localAxis; // orientation of suspension relative to body1 623 float suspensionUp; // suspension up movement 624 float suspensionDown; // suspension down movement 625 float suspensionKCompress; // spring compress constant 626 float suspensionDamping; // spring damping 627 float steerAngle; // desired steer angle in degrees 628 float friction; // friction 629 bool motorEnabled; // whether the motor is enabled or not 630 float motorForce; // motor force 631 float motorVelocity; // desired velocity 632 idClipModel * wheelModel; // wheel model 633 idVec3 wheelOffset; // wheel position relative to body1 634 trace_t trace; // contact point with the ground 635 float epsilon; // lcp epsilon 636 637 protected: 638 virtual void Evaluate( float invTimeStep ); 639 virtual void ApplyFriction( float invTimeStep ); 640 }; 641 642 643 //=============================================================== 644 // 645 // idAFBody 646 // 647 //=============================================================== 648 649 typedef struct AFBodyPState_s { 650 idVec3 worldOrigin; // position in world space 651 idMat3 worldAxis; // axis at worldOrigin 652 idVec6 spatialVelocity; // linear and rotational velocity of body 653 idVec6 externalForce; // external force and torque applied to body 654 } AFBodyPState_t; 655 656 657 class idAFBody { 658 659 friend class idPhysics_AF; 660 friend class idAFTree; 661 662 public: 663 idAFBody(); 664 idAFBody( const idStr &name, idClipModel *clipModel, float density ); 665 ~idAFBody(); 666 667 void Init(); 668 const idStr & GetName() const { return name; } 669 const idVec3 & GetWorldOrigin() const { return current->worldOrigin; } 670 const idMat3 & GetWorldAxis() const { return current->worldAxis; } 671 const idVec3 & GetLinearVelocity() const { return current->spatialVelocity.SubVec3(0); } 672 const idVec3 & GetAngularVelocity() const { return current->spatialVelocity.SubVec3(1); } 673 idVec3 GetPointVelocity( const idVec3 &point ) const; 674 const idVec3 & GetCenterOfMass() const { return centerOfMass; } 675 void SetClipModel( idClipModel *clipModel ); 676 idClipModel * GetClipModel() const { return clipModel; } 677 void SetClipMask( const int mask ) { clipMask = mask; fl.clipMaskSet = true; } 678 int GetClipMask() const { return clipMask; } 679 void SetSelfCollision( const bool enable ) { fl.selfCollision = enable; } 680 void SetWorldOrigin( const idVec3 &origin ) { current->worldOrigin = origin; } 681 void SetWorldAxis( const idMat3 &axis ) { current->worldAxis = axis; } 682 void SetLinearVelocity( const idVec3 &linear ) const { current->spatialVelocity.SubVec3(0) = linear; } 683 void SetAngularVelocity( const idVec3 &angular ) const { current->spatialVelocity.SubVec3(1) = angular; } 684 void SetFriction( float linear, float angular, float contact ); 685 float GetContactFriction() const { return contactFriction; } 686 void SetBouncyness( float bounce ); 687 float GetBouncyness() const { return bouncyness; } 688 void SetDensity( float density, const idMat3 &inertiaScale = mat3_identity ); 689 float GetInverseMass() const { return invMass; } 690 idMat3 GetInverseWorldInertia() const { return current->worldAxis.Transpose() * inverseInertiaTensor * current->worldAxis; } 691 692 void SetFrictionDirection( const idVec3 &dir ); 693 bool GetFrictionDirection( idVec3 &dir ) const; 694 695 void SetContactMotorDirection( const idVec3 &dir ); 696 bool GetContactMotorDirection( idVec3 &dir ) const; 697 void SetContactMotorVelocity( float vel ) { contactMotorVelocity = vel; } 698 float GetContactMotorVelocity() const { return contactMotorVelocity; } 699 void SetContactMotorForce( float force ) { contactMotorForce = force; } 700 float GetContactMotorForce() const { return contactMotorForce; } 701 702 void AddForce( const idVec3 &point, const idVec3 &force ); 703 void InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const; 704 idVec6 & GetResponseForce( int index ) { return reinterpret_cast<idVec6 &>(response[ index * 8 ]); } 705 706 void Save( idSaveGame *saveFile ); 707 void Restore( idRestoreGame *saveFile ); 708 709 private: 710 // properties 711 idStr name; // name of body 712 idAFBody * parent; // parent of this body 713 idList<idAFBody *, TAG_IDLIB_LIST_PHYSICS> children; // children of this body 714 idClipModel * clipModel; // model used for collision detection 715 idAFConstraint * primaryConstraint; // primary constraint (this->constraint->body1 = this) 716 idList<idAFConstraint *, TAG_IDLIB_LIST_PHYSICS>constraints; // all constraints attached to this body 717 idAFTree * tree; // tree structure this body is part of 718 float linearFriction; // translational friction 719 float angularFriction; // rotational friction 720 float contactFriction; // friction with contact surfaces 721 float bouncyness; // bounce 722 int clipMask; // contents this body collides with 723 idVec3 frictionDir; // specifies a single direction of friction in body space 724 idVec3 contactMotorDir; // contact motor direction 725 float contactMotorVelocity; // contact motor velocity 726 float contactMotorForce; // maximum force applied to reach the motor velocity 727 728 // derived properties 729 float mass; // mass of body 730 float invMass; // inverse mass 731 idVec3 centerOfMass; // center of mass of body 732 idMat3 inertiaTensor; // inertia tensor 733 idMat3 inverseInertiaTensor; // inverse inertia tensor 734 735 // physics state 736 AFBodyPState_t state[2]; 737 AFBodyPState_t * current; // current physics state 738 AFBodyPState_t * next; // next physics state 739 AFBodyPState_t saved; // saved physics state 740 idVec3 atRestOrigin; // origin at rest 741 idMat3 atRestAxis; // axis at rest 742 743 // simulation variables used during calculations 744 idMatX inverseWorldSpatialInertia; // inverse spatial inertia in world space 745 idMatX I, invI; // transformed inertia 746 idMatX J; // transformed constraint matrix 747 idVecX s; // temp solution 748 idVecX totalForce; // total force acting on body 749 idVecX auxForce; // force from auxiliary constraints 750 idVecX acceleration; // acceleration 751 float * response; // forces on body in response to auxiliary constraint forces 752 int * responseIndex; // index to response forces 753 int numResponses; // number of response forces 754 int maxAuxiliaryIndex; // largest index of an auxiliary constraint constraining this body 755 int maxSubTreeAuxiliaryIndex; // largest index of an auxiliary constraint constraining this body or one of it's children 756 757 struct bodyFlags_s { 758 bool clipMaskSet : 1; // true if this body has a clip mask set 759 bool selfCollision : 1; // true if this body can collide with other bodies of this AF 760 bool spatialInertiaSparse: 1; // true if the spatial inertia matrix is sparse 761 bool useFrictionDir : 1; // true if a single friction direction should be used 762 bool useContactMotorDir : 1; // true if a contact motor should be used 763 bool isZero : 1; // true if 's' is zero during calculations 764 } fl; 765 }; 766 767 768 //=============================================================== 769 // 770 // idAFTree 771 // 772 //=============================================================== 773 774 class idAFTree { 775 friend class idPhysics_AF; 776 777 public: 778 void Factor() const; 779 void Solve( int auxiliaryIndex = 0 ) const; 780 void Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const; 781 void CalculateForces( float timeStep ) const; 782 void SetMaxSubTreeAuxiliaryIndex(); 783 void SortBodies(); 784 void SortBodies_r( idList<idAFBody*>&sortedList, idAFBody *body ); 785 void DebugDraw( const idVec4 &color ) const; 786 787 private: 788 idList<idAFBody *, TAG_IDLIB_LIST_PHYSICS> sortedBodies; 789 }; 790 791 792 //=============================================================== 793 // 794 // idPhysics_AF 795 // 796 //=============================================================== 797 798 typedef struct AFPState_s { 799 int atRest; // >= 0 if articulated figure is at rest 800 float noMoveTime; // time the articulated figure is hardly moving 801 float activateTime; // time since last activation 802 float lastTimeStep; // last time step 803 idVec6 pushVelocity; // velocity with which the af is pushed 804 } AFPState_t; 805 806 typedef struct AFCollision_s { 807 trace_t trace; 808 idAFBody * body; 809 } AFCollision_t; 810 811 812 class idPhysics_AF : public idPhysics_Base { 813 814 public: 815 CLASS_PROTOTYPE( idPhysics_AF ); 816 817 idPhysics_AF(); 818 ~idPhysics_AF(); 819 820 void Save( idSaveGame *savefile ) const; 821 void Restore( idRestoreGame *savefile ); 822 823 // initialisation 824 int AddBody( idAFBody *body ); // returns body id 825 void AddConstraint( idAFConstraint *constraint ); 826 void AddFrameConstraint( idAFConstraint *constraint ); 827 // force a body to have a certain id 828 void ForceBodyId( idAFBody *body, int newId ); 829 // get body or constraint id 830 int GetBodyId( idAFBody *body ) const; 831 int GetBodyId( const char *bodyName ) const; 832 int GetConstraintId( idAFConstraint *constraint ) const; 833 int GetConstraintId( const char *constraintName ) const; 834 // number of bodies and constraints 835 int GetNumBodies() const; 836 int GetNumConstraints() const; 837 // retrieve body or constraint 838 idAFBody * GetBody( const char *bodyName ) const; 839 idAFBody * GetBody( const int id ) const; 840 idAFBody * GetMasterBody() const { return masterBody; } 841 idAFConstraint * GetConstraint( const char *constraintName ) const; 842 idAFConstraint * GetConstraint( const int id ) const; 843 // delete body or constraint 844 void DeleteBody( const char *bodyName ); 845 void DeleteBody( const int id ); 846 void DeleteConstraint( const char *constraintName ); 847 void DeleteConstraint( const int id ); 848 // get all the contact constraints acting on the body 849 int GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const; 850 // set the default friction for bodies 851 void SetDefaultFriction( float linear, float angular, float contact ); 852 // suspend settings 853 void SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration ); 854 // set the time and tolerances used to determine if the simulation can be suspended when the figure hardly moves for a while 855 void SetSuspendTolerance( const float noMoveTime, const float translationTolerance, const float rotationTolerance ); 856 // set minimum and maximum simulation time in seconds 857 void SetSuspendTime( const float minTime, const float maxTime ); 858 // set the time scale value 859 void SetTimeScale( const float ts ) { timeScale = ts; } 860 // set time scale ramp 861 void SetTimeScaleRamp( const float start, const float end ); 862 // set the joint friction scale 863 void SetJointFrictionScale( const float scale ) { jointFrictionScale = scale; } 864 // set joint friction dent 865 void SetJointFrictionDent( const float dent, const float start, const float end ); 866 // get the current joint friction scale 867 float GetJointFrictionScale() const; 868 // set the contact friction scale 869 void SetContactFrictionScale( const float scale ) { contactFrictionScale = scale; } 870 // set contact friction dent 871 void SetContactFrictionDent( const float dent, const float start, const float end ); 872 // get the current contact friction scale 873 float GetContactFrictionScale() const; 874 // enable or disable collision detection 875 void SetCollision( const bool enable ) { enableCollision = enable; } 876 // enable or disable self collision 877 void SetSelfCollision( const bool enable ) { selfCollision = enable; } 878 // enable or disable coming to a dead stop 879 void SetComeToRest( bool enable ) { comeToRest = enable; } 880 // call when structure of articulated figure changes 881 void SetChanged() { changedAF = true; } 882 // enable/disable activation by impact 883 void EnableImpact(); 884 void DisableImpact(); 885 // lock of unlock the world constraints 886 void LockWorldConstraints( const bool lock ) { worldConstraintsLocked = lock; } 887 // set force pushable 888 void SetForcePushable( const bool enable ) { forcePushable = enable; } 889 // update the clip model positions 890 void UpdateClipModels(); 891 892 public: // common physics interface 893 void SetClipModel( idClipModel *model, float density, int id = 0, bool freeOld = true ); 894 idClipModel * GetClipModel( int id = 0 ) const; 895 int GetNumClipModels() const; 896 897 void SetMass( float mass, int id = -1 ); 898 float GetMass( int id = -1 ) const; 899 900 void SetContents( int contents, int id = -1 ); 901 int GetContents( int id = -1 ) const; 902 903 const idBounds & GetBounds( int id = -1 ) const; 904 const idBounds & GetAbsBounds( int id = -1 ) const; 905 906 bool Evaluate( int timeStepMSec, int endTimeMSec ); 907 void UpdateTime( int endTimeMSec ); 908 int GetTime() const; 909 910 void GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const; 911 void ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ); 912 void AddForce( const int id, const idVec3 &point, const idVec3 &force ); 913 bool IsAtRest() const; 914 int GetRestStartTime() const; 915 void Activate(); 916 void PutToRest(); 917 bool IsPushable() const; 918 919 void SaveState(); 920 void RestoreState(); 921 922 void SetOrigin( const idVec3 &newOrigin, int id = -1 ); 923 void SetAxis( const idMat3 &newAxis, int id = -1 ); 924 925 void Translate( const idVec3 &translation, int id = -1 ); 926 void Rotate( const idRotation &rotation, int id = -1 ); 927 928 const idVec3 & GetOrigin( int id = 0 ) const; 929 const idMat3 & GetAxis( int id = 0 ) const; 930 931 void SetLinearVelocity( const idVec3 &newLinearVelocity, int id = 0 ); 932 void SetAngularVelocity( const idVec3 &newAngularVelocity, int id = 0 ); 933 934 const idVec3 & GetLinearVelocity( int id = 0 ) const; 935 const idVec3 & GetAngularVelocity( int id = 0 ) const; 936 937 void ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const; 938 void ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const; 939 int ClipContents( const idClipModel *model ) const; 940 941 void DisableClip(); 942 void EnableClip(); 943 944 void UnlinkClip(); 945 void LinkClip(); 946 947 bool EvaluateContacts(); 948 949 void SetPushed( int deltaTime ); 950 const idVec3 & GetPushedLinearVelocity( const int id = 0 ) const; 951 const idVec3 & GetPushedAngularVelocity( const int id = 0 ) const; 952 953 void SetMaster( idEntity *master, const bool orientated = true ); 954 955 void WriteToSnapshot( idBitMsg &msg ) const; 956 void ReadFromSnapshot( const idBitMsg &msg ); 957 958 private: 959 // articulated figure 960 idList<idAFTree *, TAG_IDLIB_LIST_PHYSICS> trees; // tree structures 961 idList<idAFBody *, TAG_IDLIB_LIST_PHYSICS> bodies; // all bodies 962 idList<idAFConstraint *, TAG_IDLIB_LIST_PHYSICS>constraints; // all frame independent constraints 963 idList<idAFConstraint *, TAG_IDLIB_LIST_PHYSICS>primaryConstraints; // list with primary constraints 964 idList<idAFConstraint *, TAG_IDLIB_LIST_PHYSICS>auxiliaryConstraints; // list with auxiliary constraints 965 idList<idAFConstraint *, TAG_IDLIB_LIST_PHYSICS>frameConstraints; // constraints that only live one frame 966 idList<idAFConstraint_Contact *, TAG_IDLIB_LIST_PHYSICS>contactConstraints; // contact constraints 967 idList<int, TAG_IDLIB_LIST_PHYSICS> contactBodies; // body id for each contact 968 idList<AFCollision_t, TAG_IDLIB_LIST_PHYSICS> collisions; // collisions 969 bool changedAF; // true when the articulated figure just changed 970 971 // properties 972 float linearFriction; // default translational friction 973 float angularFriction; // default rotational friction 974 float contactFriction; // default friction with contact surfaces 975 float bouncyness; // default bouncyness 976 float totalMass; // total mass of articulated figure 977 float forceTotalMass; // force this total mass 978 979 idVec2 suspendVelocity; // simulation may not be suspended if a body has more velocity 980 idVec2 suspendAcceleration; // simulation may not be suspended if a body has more acceleration 981 float noMoveTime; // suspend simulation if hardly any movement for this many seconds 982 float noMoveTranslation; // maximum translation considered no movement 983 float noMoveRotation; // maximum rotation considered no movement 984 float minMoveTime; // if > 0 the simulation is never suspended before running this many seconds 985 float maxMoveTime; // if > 0 the simulation is always suspeded after running this many seconds 986 float impulseThreshold; // threshold below which impulses are ignored to avoid continuous activation 987 988 float timeScale; // the time is scaled with this value for slow motion effects 989 float timeScaleRampStart; // start of time scale change 990 float timeScaleRampEnd; // end of time scale change 991 992 float jointFrictionScale; // joint friction scale 993 float jointFrictionDent; // joint friction dives from 1 to this value and goes up again 994 float jointFrictionDentStart; // start time of joint friction dent 995 float jointFrictionDentEnd; // end time of joint friction dent 996 float jointFrictionDentScale; // dent scale 997 998 float contactFrictionScale; // contact friction scale 999 float contactFrictionDent; // contact friction dives from 1 to this value and goes up again 1000 float contactFrictionDentStart; // start time of contact friction dent 1001 float contactFrictionDentEnd; // end time of contact friction dent 1002 float contactFrictionDentScale; // dent scale 1003 1004 bool enableCollision; // if true collision detection is enabled 1005 bool selfCollision; // if true the self collision is allowed 1006 bool comeToRest; // if true the figure can come to rest 1007 bool linearTime; // if true use the linear time algorithm 1008 bool noImpact; // if true do not activate when another object collides 1009 bool worldConstraintsLocked; // if true world constraints cannot be moved 1010 bool forcePushable; // if true can be pushed even when bound to a master 1011 1012 // physics state 1013 AFPState_t current; 1014 AFPState_t saved; 1015 1016 idAFBody * masterBody; // master body 1017 idLCP * lcp; // linear complementarity problem solver 1018 1019 private: 1020 void BuildTrees(); 1021 bool IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const; 1022 void PrimaryFactor(); 1023 void EvaluateBodies( float timeStep ); 1024 void EvaluateConstraints( float timeStep ); 1025 void AddFrameConstraints(); 1026 void RemoveFrameConstraints(); 1027 void ApplyFriction( float timeStep, float endTimeMSec ); 1028 void PrimaryForces( float timeStep ); 1029 void AuxiliaryForces( float timeStep ); 1030 void VerifyContactConstraints(); 1031 void SetupContactConstraints(); 1032 void ApplyContactForces(); 1033 void Evolve( float timeStep ); 1034 idEntity * SetupCollisionForBody( idAFBody *body ) const; 1035 bool CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision ); 1036 bool ApplyCollisions( float timeStep ); 1037 void CheckForCollisions( float timeStep ); 1038 void ClearExternalForce(); 1039 void AddGravity(); 1040 void SwapStates(); 1041 bool TestIfAtRest( float timeStep ); 1042 void Rest(); 1043 void AddPushVelocity( const idVec6 &pushVelocity ); 1044 void DebugDraw(); 1045 }; 1046 1047 #endif /* !__PHYSICS_AF_H__ */