/*
===========================================================================
Doom 3 GPL Source Code
Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").
Doom 3 Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Doom 3 Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
In addition, the Doom 3 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 Source Code. If not, please request a copy in writing from id Software at the address below.
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.
===========================================================================
*/
#ifndef __PHYSICS_AF_H__
#define __PHYSICS_AF_H__
#include "idlib/math/Lcp.h"
#include "physics/Physics_Base.h"
/*
===================================================================================
Articulated Figure physics
Employs a constraint force based dynamic simulation using a lagrangian
multiplier method to solve for the constraint forces.
===================================================================================
*/
class idAFConstraint;
class idAFConstraint_Fixed;
class idAFConstraint_BallAndSocketJoint;
class idAFConstraint_BallAndSocketJointFriction;
class idAFConstraint_UniversalJoint;
class idAFConstraint_UniversalJointFriction;
class idAFConstraint_CylindricalJoint;
class idAFConstraint_Hinge;
class idAFConstraint_HingeFriction;
class idAFConstraint_HingeSteering;
class idAFConstraint_Slider;
class idAFConstraint_Line;
class idAFConstraint_Plane;
class idAFConstraint_Spring;
class idAFConstraint_Contact;
class idAFConstraint_ContactFriction;
class idAFConstraint_ConeLimit;
class idAFConstraint_PyramidLimit;
class idAFConstraint_Suspension;
class idAFBody;
class idAFTree;
class idPhysics_AF;
typedef enum {
CONSTRAINT_INVALID,
CONSTRAINT_FIXED,
CONSTRAINT_BALLANDSOCKETJOINT,
CONSTRAINT_UNIVERSALJOINT,
CONSTRAINT_HINGE,
CONSTRAINT_HINGESTEERING,
CONSTRAINT_SLIDER,
CONSTRAINT_CYLINDRICALJOINT,
CONSTRAINT_LINE,
CONSTRAINT_PLANE,
CONSTRAINT_SPRING,
CONSTRAINT_CONTACT,
CONSTRAINT_FRICTION,
CONSTRAINT_CONELIMIT,
CONSTRAINT_PYRAMIDLIMIT,
CONSTRAINT_SUSPENSION
} constraintType_t;
//===============================================================
//
// idAFConstraint
//
//===============================================================
// base class for all constraints
class idAFConstraint {
friend class idPhysics_AF;
friend class idAFTree;
public:
idAFConstraint( void );
virtual ~idAFConstraint( void );
constraintType_t GetType( void ) const { return type; }
const idStr & GetName( void ) const { return name; }
idAFBody * GetBody1( void ) const { return body1; }
idAFBody * GetBody2( void ) const { return body2; }
void SetPhysics( idPhysics_AF *p ) { physics = p; }
const idVecX & GetMultiplier( void );
virtual void SetBody1( idAFBody *body );
virtual void SetBody2( idAFBody *body );
virtual void DebugDraw( void );
virtual void GetForce( idAFBody *body, idVec6 &force );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
constraintType_t type; // constraint type
idStr name; // name of constraint
idAFBody * body1; // first constrained body
idAFBody * body2; // second constrained body, NULL for world
idPhysics_AF * physics; // for adding additional constraints like limits
// simulation variables set by Evaluate
idMatX J1, J2; // matrix with left hand side of constraint equations
idVecX c1, c2; // right hand side of constraint equations
idVecX lo, hi, e; // low and high bounds and lcp epsilon
idAFConstraint * boxConstraint; // constraint the boxIndex refers to
int boxIndex[6]; // indexes for special box constrained variables
// simulation variables used during calculations
idMatX invI; // transformed inertia
idMatX J; // transformed constraint matrix
idVecX s; // temp solution
idVecX lm; // lagrange multipliers
int firstIndex; // index of the first constraint row in the lcp matrix
struct constraintFlags_s {
bool allowPrimary : 1; // true if the constraint can be used as a primary constraint
bool frameConstraint : 1; // true if this constraint is added to the frame constraints
bool noCollision : 1; // true if body1 and body2 never collide with each other
bool isPrimary : 1; // true if this is a primary constraint
bool isZero : 1; // true if 's' is zero during calculations
} fl;
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
void InitSize( int size );
};
// fixed or rigid joint which allows zero degrees of freedom
// constrains body1 to have a fixed position and orientation relative to body2
class idAFConstraint_Fixed : public idAFConstraint {
public:
idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFBody *body2 );
void SetRelativeOrigin( const idVec3 &origin ) { this->offset = origin; }
void SetRelativeAxis( const idMat3 &axis ) { this->relAxis = axis; }
virtual void SetBody1( idAFBody *body );
virtual void SetBody2( idAFBody *body );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 offset; // offset of body1 relative to body2 in body2 space
idMat3 relAxis; // rotation of body1 relative to body2
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
void InitOffset( void );
};
// ball and socket or spherical joint which allows 3 degrees of freedom
// constrains body1 relative to body2 with a ball and socket joint
class idAFConstraint_BallAndSocketJoint : public idAFConstraint {
public:
idAFConstraint_BallAndSocketJoint( const idStr &name, idAFBody *body1, idAFBody *body2 );
~idAFConstraint_BallAndSocketJoint( void );
void SetAnchor( const idVec3 &worldPosition );
idVec3 GetAnchor( void ) const;
void SetNoLimit( void );
void SetConeLimit( const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis );
void SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
const float angle1, const float angle2, const idVec3 &body1Axis );
void SetLimitEpsilon( const float e );
void SetFriction( const float f ) { friction = f; }
float GetFriction( void ) const;
virtual void DebugDraw( void );
virtual void GetForce( idAFBody *body, idVec6 &force );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 anchor1; // anchor in body1 space
idVec3 anchor2; // anchor in body2 space
float friction; // joint friction
idAFConstraint_ConeLimit *coneLimit; // cone shaped limit
idAFConstraint_PyramidLimit *pyramidLimit; // pyramid shaped limit
idAFConstraint_BallAndSocketJointFriction *fc; // friction constraint
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// ball and socket joint friction
class idAFConstraint_BallAndSocketJointFriction : public idAFConstraint {
public:
idAFConstraint_BallAndSocketJointFriction( void );
void Setup( idAFConstraint_BallAndSocketJoint *cc );
bool Add( idPhysics_AF *phys, float invTimeStep );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
protected:
idAFConstraint_BallAndSocketJoint *joint;
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// universal, Cardan or Hooke joint which allows 2 degrees of freedom
// like a ball and socket joint but also constrains the rotation about the cardan shafts
class idAFConstraint_UniversalJoint : public idAFConstraint {
public:
idAFConstraint_UniversalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 );
~idAFConstraint_UniversalJoint( void );
void SetAnchor( const idVec3 &worldPosition );
idVec3 GetAnchor( void ) const;
void SetShafts( const idVec3 &cardanShaft1, const idVec3 &cardanShaft2 );
void GetShafts( idVec3 &cardanShaft1, idVec3 &cardanShaft2 ) { cardanShaft1 = shaft1; cardanShaft2 = shaft2; }
void SetNoLimit( void );
void SetConeLimit( const idVec3 &coneAxis, const float coneAngle );
void SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis,
const float angle1, const float angle2 );
void SetLimitEpsilon( const float e );
void SetFriction( const float f ) { friction = f; }
float GetFriction( void ) const;
virtual void DebugDraw( void );
virtual void GetForce( idAFBody *body, idVec6 &force );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 anchor1; // anchor in body1 space
idVec3 anchor2; // anchor in body2 space
idVec3 shaft1; // body1 cardan shaft in body1 space
idVec3 shaft2; // body2 cardan shaft in body2 space
idVec3 axis1; // cardan axis in body1 space
idVec3 axis2; // cardan axis in body2 space
float friction; // joint friction
idAFConstraint_ConeLimit *coneLimit; // cone shaped limit
idAFConstraint_PyramidLimit *pyramidLimit; // pyramid shaped limit
idAFConstraint_UniversalJointFriction *fc; // friction constraint
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// universal joint friction
class idAFConstraint_UniversalJointFriction : public idAFConstraint {
public:
idAFConstraint_UniversalJointFriction( void );
void Setup( idAFConstraint_UniversalJoint *cc );
bool Add( idPhysics_AF *phys, float invTimeStep );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
protected:
idAFConstraint_UniversalJoint *joint; // universal joint
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// cylindrical joint which allows 2 degrees of freedom
// constrains body1 to lie on a line relative to body2 and allows only translation along and rotation about the line
class idAFConstraint_CylindricalJoint : public idAFConstraint {
public:
idAFConstraint_CylindricalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
protected:
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// hinge, revolute or pin joint which allows 1 degree of freedom
// constrains all motion of body1 relative to body2 except the rotation about the hinge axis
class idAFConstraint_Hinge : public idAFConstraint {
public:
idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFBody *body2 );
~idAFConstraint_Hinge( void );
void SetAnchor( const idVec3 &worldPosition );
idVec3 GetAnchor( void ) const;
void SetAxis( const idVec3 &axis );
void GetAxis( idVec3 &a1, idVec3 &a2 ) const { a1 = axis1; a2 = axis2; }
idVec3 GetAxis( void ) const;
void SetNoLimit( void );
void SetLimit( const idVec3 &axis, const float angle, const idVec3 &body1Axis );
void SetLimitEpsilon( const float e );
float GetAngle( void ) const;
void SetSteerAngle( const float degrees );
void SetSteerSpeed( const float speed );
void SetFriction( const float f ) { friction = f; }
float GetFriction( void ) const;
virtual void DebugDraw( void );
virtual void GetForce( idAFBody *body, idVec6 &force );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 anchor1; // anchor in body1 space
idVec3 anchor2; // anchor in body2 space
idVec3 axis1; // axis in body1 space
idVec3 axis2; // axis in body2 space
idMat3 initialAxis; // initial axis of body1 relative to body2
float friction; // hinge friction
idAFConstraint_ConeLimit *coneLimit; // cone limit
idAFConstraint_HingeSteering *steering; // steering
idAFConstraint_HingeFriction *fc; // friction constraint
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// hinge joint friction
class idAFConstraint_HingeFriction : public idAFConstraint {
public:
idAFConstraint_HingeFriction( void );
void Setup( idAFConstraint_Hinge *cc );
bool Add( idPhysics_AF *phys, float invTimeStep );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
protected:
idAFConstraint_Hinge * hinge; // hinge
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// constrains two bodies attached to each other with a hinge to get a specified relative orientation
class idAFConstraint_HingeSteering : public idAFConstraint {
public:
idAFConstraint_HingeSteering( void );
void Setup( idAFConstraint_Hinge *cc );
void SetSteerAngle( const float degrees ) { steerAngle = degrees; }
void SetSteerSpeed( const float speed ) { steerSpeed = speed; }
void SetEpsilon( const float e ) { epsilon = e; }
bool Add( idPhysics_AF *phys, float invTimeStep );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idAFConstraint_Hinge * hinge; // hinge
float steerAngle; // desired steer angle in degrees
float steerSpeed; // steer speed
float epsilon; // lcp epsilon
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// slider, prismatic or translational constraint which allows 1 degree of freedom
// constrains body1 to lie on a line relative to body2, the orientation is also fixed relative to body2
class idAFConstraint_Slider : public idAFConstraint {
public:
idAFConstraint_Slider( const idStr &name, idAFBody *body1, idAFBody *body2 );
void SetAxis( const idVec3 &ax );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 axis; // axis along which body1 slides in body2 space
idVec3 offset; // offset of body1 relative to body2
idMat3 relAxis; // rotation of body1 relative to body2
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// line constraint which allows 4 degrees of freedom
// constrains body1 to lie on a line relative to body2, does not constrain the orientation.
class idAFConstraint_Line : public idAFConstraint {
public:
idAFConstraint_Line( const idStr &name, idAFBody *body1, idAFBody *body2 );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
protected:
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// plane constraint which allows 5 degrees of freedom
// constrains body1 to lie in a plane relative to body2, does not constrain the orientation.
class idAFConstraint_Plane : public idAFConstraint {
public:
idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFBody *body2 );
void SetPlane( const idVec3 &normal, const idVec3 &anchor );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 anchor1; // anchor in body1 space
idVec3 anchor2; // anchor in body2 space
idVec3 planeNormal; // plane normal in body2 space
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// spring constraint which allows 6 or 5 degrees of freedom based on the spring limits
// constrains body1 relative to body2 with a spring
class idAFConstraint_Spring : public idAFConstraint {
public:
idAFConstraint_Spring( const idStr &name, idAFBody *body1, idAFBody *body2 );
void SetAnchor( const idVec3 &worldAnchor1, const idVec3 &worldAnchor2 );
void SetSpring( const float stretch, const float compress, const float damping, const float restLength );
void SetLimit( const float minLength, const float maxLength );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 anchor1; // anchor in body1 space
idVec3 anchor2; // anchor in body2 space
float kstretch; // spring constant when stretched
float kcompress; // spring constant when compressed
float damping; // spring damping
float restLength; // rest length of spring
float minLength; // minimum spring length
float maxLength; // maximum spring length
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// constrains body1 to either be in contact with or move away from body2
class idAFConstraint_Contact : public idAFConstraint {
public:
idAFConstraint_Contact( void );
~idAFConstraint_Contact( void );
void Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c );
const contactInfo_t & GetContact( void ) const { return contact; }
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void GetCenter( idVec3 ¢er );
protected:
contactInfo_t contact; // contact information
idAFConstraint_ContactFriction *fc; // contact friction
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// contact friction
class idAFConstraint_ContactFriction : public idAFConstraint {
public:
idAFConstraint_ContactFriction( void );
void Setup( idAFConstraint_Contact *cc );
bool Add( idPhysics_AF *phys, float invTimeStep );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
protected:
idAFConstraint_Contact *cc; // contact constraint
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// constrains an axis attached to body1 to be inside a cone relative to body2
class idAFConstraint_ConeLimit : public idAFConstraint {
public:
idAFConstraint_ConeLimit( void );
void Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis,
const float coneAngle, const idVec3 &body1Axis );
void SetAnchor( const idVec3 &coneAnchor );
void SetBody1Axis( const idVec3 &body1Axis );
void SetEpsilon( const float e ) { epsilon = e; }
bool Add( idPhysics_AF *phys, float invTimeStep );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 coneAnchor; // top of the cone in body2 space
idVec3 coneAxis; // cone axis in body2 space
idVec3 body1Axis; // axis in body1 space that should stay within the cone
float cosAngle; // cos( coneAngle / 2 )
float sinHalfAngle; // sin( coneAngle / 4 )
float cosHalfAngle; // cos( coneAngle / 4 )
float epsilon; // lcp epsilon
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// constrains an axis attached to body1 to be inside a pyramid relative to body2
class idAFConstraint_PyramidLimit : public idAFConstraint {
public:
idAFConstraint_PyramidLimit( void );
void Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor,
const idVec3 &pyramidAxis, const idVec3 &baseAxis,
const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis );
void SetAnchor( const idVec3 &pyramidAxis );
void SetBody1Axis( const idVec3 &body1Axis );
void SetEpsilon( const float e ) { epsilon = e; }
bool Add( idPhysics_AF *phys, float invTimeStep );
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
virtual void Save( idSaveGame *saveFile ) const;
virtual void Restore( idRestoreGame *saveFile );
protected:
idVec3 pyramidAnchor; // top of the pyramid in body2 space
idMat3 pyramidBasis; // pyramid basis in body2 space with base[2] being the pyramid axis
idVec3 body1Axis; // axis in body1 space that should stay within the cone
float cosAngle[2]; // cos( pyramidAngle / 2 )
float sinHalfAngle[2]; // sin( pyramidAngle / 4 )
float cosHalfAngle[2]; // cos( pyramidAngle / 4 )
float epsilon; // lcp epsilon
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
// vehicle suspension
class idAFConstraint_Suspension : public idAFConstraint {
public:
idAFConstraint_Suspension( void );
void Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel );
void SetSuspension( const float up, const float down, const float k, const float d, const float f );
void SetSteerAngle( const float degrees ) { steerAngle = degrees; }
void EnableMotor( const bool enable ) { motorEnabled = enable; }
void SetMotorForce( const float force ) { motorForce = force; }
void SetMotorVelocity( const float vel ) { motorVelocity = vel; }
void SetEpsilon( const float e ) { epsilon = e; }
const idVec3 GetWheelOrigin( void ) const;
virtual void DebugDraw( void );
virtual void Translate( const idVec3 &translation );
virtual void Rotate( const idRotation &rotation );
protected:
idVec3 localOrigin; // position of suspension relative to body1
idMat3 localAxis; // orientation of suspension relative to body1
float suspensionUp; // suspension up movement
float suspensionDown; // suspension down movement
float suspensionKCompress; // spring compress constant
float suspensionDamping; // spring damping
float steerAngle; // desired steer angle in degrees
float friction; // friction
bool motorEnabled; // whether the motor is enabled or not
float motorForce; // motor force
float motorVelocity; // desired velocity
idClipModel * wheelModel; // wheel model
idVec3 wheelOffset; // wheel position relative to body1
trace_t trace; // contact point with the ground
float epsilon; // lcp epsilon
protected:
virtual void Evaluate( float invTimeStep );
virtual void ApplyFriction( float invTimeStep );
};
//===============================================================
//
// idAFBody
//
//===============================================================
typedef struct AFBodyPState_s {
idVec3 worldOrigin; // position in world space
idMat3 worldAxis; // axis at worldOrigin
idVec6 spatialVelocity; // linear and rotational velocity of body
idVec6 externalForce; // external force and torque applied to body
} AFBodyPState_t;
class idAFBody {
friend class idPhysics_AF;
friend class idAFTree;
public:
idAFBody( void );
idAFBody( const idStr &name, idClipModel *clipModel, float density );
~idAFBody( void );
void Init( void );
const idStr & GetName( void ) const { return name; }
const idVec3 & GetWorldOrigin( void ) const { return current->worldOrigin; }
const idMat3 & GetWorldAxis( void ) const { return current->worldAxis; }
const idVec3 & GetLinearVelocity( void ) const { return current->spatialVelocity.SubVec3(0); }
const idVec3 & GetAngularVelocity( void ) const { return current->spatialVelocity.SubVec3(1); }
idVec3 GetPointVelocity( const idVec3 &point ) const;
const idVec3 & GetCenterOfMass( void ) const { return centerOfMass; }
void SetClipModel( idClipModel *clipModel );
idClipModel * GetClipModel( void ) const { return clipModel; }
void SetClipMask( const int mask ) { clipMask = mask; fl.clipMaskSet = true; }
int GetClipMask( void ) const { return clipMask; }
void SetSelfCollision( const bool enable ) { fl.selfCollision = enable; }
void SetWorldOrigin( const idVec3 &origin ) { current->worldOrigin = origin; }
void SetWorldAxis( const idMat3 &axis ) { current->worldAxis = axis; }
void SetLinearVelocity( const idVec3 &linear ) const { current->spatialVelocity.SubVec3(0) = linear; }
void SetAngularVelocity( const idVec3 &angular ) const { current->spatialVelocity.SubVec3(1) = angular; }
void SetFriction( float linear, float angular, float contact );
float GetContactFriction( void ) const { return contactFriction; }
void SetBouncyness( float bounce );
float GetBouncyness( void ) const { return bouncyness; }
void SetDensity( float density, const idMat3 &inertiaScale = mat3_identity );
float GetInverseMass( void ) const { return invMass; }
idMat3 GetInverseWorldInertia( void ) const { return current->worldAxis.Transpose() * inverseInertiaTensor * current->worldAxis; }
void SetFrictionDirection( const idVec3 &dir );
bool GetFrictionDirection( idVec3 &dir ) const;
void SetContactMotorDirection( const idVec3 &dir );
bool GetContactMotorDirection( idVec3 &dir ) const;
void SetContactMotorVelocity( float vel ) { contactMotorVelocity = vel; }
float GetContactMotorVelocity( void ) const { return contactMotorVelocity; }
void SetContactMotorForce( float force ) { contactMotorForce = force; }
float GetContactMotorForce( void ) const { return contactMotorForce; }
void AddForce( const idVec3 &point, const idVec3 &force );
void InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const;
idVec6 & GetResponseForce( int index ) { return reinterpret_cast<idVec6 &>(response[ index * 8 ]); }
void Save( idSaveGame *saveFile );
void Restore( idRestoreGame *saveFile );
private:
// properties
idStr name; // name of body
idAFBody * parent; // parent of this body
idList<idAFBody *> children; // children of this body
idClipModel * clipModel; // model used for collision detection
idAFConstraint * primaryConstraint; // primary constraint (this->constraint->body1 = this)
idList<idAFConstraint *>constraints; // all constraints attached to this body
idAFTree * tree; // tree structure this body is part of
float linearFriction; // translational friction
float angularFriction; // rotational friction
float contactFriction; // friction with contact surfaces
float bouncyness; // bounce
int clipMask; // contents this body collides with
idVec3 frictionDir; // specifies a single direction of friction in body space
idVec3 contactMotorDir; // contact motor direction
float contactMotorVelocity; // contact motor velocity
float contactMotorForce; // maximum force applied to reach the motor velocity
// derived properties
float mass; // mass of body
float invMass; // inverse mass
idVec3 centerOfMass; // center of mass of body
idMat3 inertiaTensor; // inertia tensor
idMat3 inverseInertiaTensor; // inverse inertia tensor
// physics state
AFBodyPState_t state[2];
AFBodyPState_t * current; // current physics state
AFBodyPState_t * next; // next physics state
AFBodyPState_t saved; // saved physics state
idVec3 atRestOrigin; // origin at rest
idMat3 atRestAxis; // axis at rest
// simulation variables used during calculations
idMatX inverseWorldSpatialInertia; // inverse spatial inertia in world space
idMatX I, invI; // transformed inertia
idMatX J; // transformed constraint matrix
idVecX s; // temp solution
idVecX totalForce; // total force acting on body
idVecX auxForce; // force from auxiliary constraints
idVecX acceleration; // acceleration
float * response; // forces on body in response to auxiliary constraint forces
int * responseIndex; // index to response forces
int numResponses; // number of response forces
int maxAuxiliaryIndex; // largest index of an auxiliary constraint constraining this body
int maxSubTreeAuxiliaryIndex; // largest index of an auxiliary constraint constraining this body or one of it's children
struct bodyFlags_s {
bool clipMaskSet : 1; // true if this body has a clip mask set
bool selfCollision : 1; // true if this body can collide with other bodies of this AF
bool spatialInertiaSparse: 1; // true if the spatial inertia matrix is sparse
bool useFrictionDir : 1; // true if a single friction direction should be used
bool useContactMotorDir : 1; // true if a contact motor should be used
bool isZero : 1; // true if 's' is zero during calculations
} fl;
};
//===============================================================
//
// idAFTree
//
//===============================================================
class idAFTree {
friend class idPhysics_AF;
public:
void Factor( void ) const;
void Solve( int auxiliaryIndex = 0 ) const;
void Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const;
void CalculateForces( float timeStep ) const;
void SetMaxSubTreeAuxiliaryIndex( void );
void SortBodies( void );
void SortBodies_r( idList<idAFBody*>&sortedList, idAFBody *body );
void DebugDraw( const idVec4 &color ) const;
private:
idList<idAFBody *> sortedBodies;
};
//===============================================================
//
// idPhysics_AF
//
//===============================================================
typedef struct AFPState_s {
int atRest; // >= 0 if articulated figure is at rest
float noMoveTime; // time the articulated figure is hardly moving
float activateTime; // time since last activation
float lastTimeStep; // last time step
idVec6 pushVelocity; // velocity with which the af is pushed
} AFPState_t;
typedef struct AFCollision_s {
trace_t trace;
idAFBody * body;
} AFCollision_t;
class idPhysics_AF : public idPhysics_Base {
public:
CLASS_PROTOTYPE( idPhysics_AF );
idPhysics_AF( void );
~idPhysics_AF( void );
void Save( idSaveGame *savefile ) const;
void Restore( idRestoreGame *savefile );
// initialisation
int AddBody( idAFBody *body ); // returns body id
void AddConstraint( idAFConstraint *constraint );
void AddFrameConstraint( idAFConstraint *constraint );
// force a body to have a certain id
void ForceBodyId( idAFBody *body, int newId );
// get body or constraint id
int GetBodyId( idAFBody *body ) const;
int GetBodyId( const char *bodyName ) const;
int GetConstraintId( idAFConstraint *constraint ) const;
int GetConstraintId( const char *constraintName ) const;
// number of bodies and constraints
int GetNumBodies( void ) const;
int GetNumConstraints( void ) const;
// retrieve body or constraint
idAFBody * GetBody( const char *bodyName ) const;
idAFBody * GetBody( const int id ) const;
idAFBody * GetMasterBody( void ) const { return masterBody; }
idAFConstraint * GetConstraint( const char *constraintName ) const;
idAFConstraint * GetConstraint( const int id ) const;
// delete body or constraint
void DeleteBody( const char *bodyName );
void DeleteBody( const int id );
void DeleteConstraint( const char *constraintName );
void DeleteConstraint( const int id );
// get all the contact constraints acting on the body
int GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const;
// set the default friction for bodies
void SetDefaultFriction( float linear, float angular, float contact );
// suspend settings
void SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration );
// set the time and tolerances used to determine if the simulation can be suspended when the figure hardly moves for a while
void SetSuspendTolerance( const float noMoveTime, const float translationTolerance, const float rotationTolerance );
// set minimum and maximum simulation time in seconds
void SetSuspendTime( const float minTime, const float maxTime );
// set the time scale value
void SetTimeScale( const float ts ) { timeScale = ts; }
// set time scale ramp
void SetTimeScaleRamp( const float start, const float end );
// set the joint friction scale
void SetJointFrictionScale( const float scale ) { jointFrictionScale = scale; }
// set joint friction dent
void SetJointFrictionDent( const float dent, const float start, const float end );
// get the current joint friction scale
float GetJointFrictionScale( void ) const;
// set the contact friction scale
void SetContactFrictionScale( const float scale ) { contactFrictionScale = scale; }
// set contact friction dent
void SetContactFrictionDent( const float dent, const float start, const float end );
// get the current contact friction scale
float GetContactFrictionScale( void ) const;
// enable or disable collision detection
void SetCollision( const bool enable ) { enableCollision = enable; }
// enable or disable self collision
void SetSelfCollision( const bool enable ) { selfCollision = enable; }
// enable or disable coming to a dead stop
void SetComeToRest( bool enable ) { comeToRest = enable; }
// call when structure of articulated figure changes
void SetChanged( void ) { changedAF = true; }
// enable/disable activation by impact
void EnableImpact( void );
void DisableImpact( void );
// lock of unlock the world constraints
void LockWorldConstraints( const bool lock ) { worldConstraintsLocked = lock; }
// set force pushable
void SetForcePushable( const bool enable ) { forcePushable = enable; }
// update the clip model positions
void UpdateClipModels( void );
public: // common physics interface
void SetClipModel( idClipModel *model, float density, int id = 0, bool freeOld = true );
idClipModel * GetClipModel( int id = 0 ) const;
int GetNumClipModels( void ) const;
void SetMass( float mass, int id = -1 );
float GetMass( int id = -1 ) const;
void SetContents( int contents, int id = -1 );
int GetContents( int id = -1 ) const;
const idBounds & GetBounds( int id = -1 ) const;
const idBounds & GetAbsBounds( int id = -1 ) const;
bool Evaluate( int timeStepMSec, int endTimeMSec );
void UpdateTime( int endTimeMSec );
int GetTime( void ) const;
void GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const;
void ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse );
void AddForce( const int id, const idVec3 &point, const idVec3 &force );
bool IsAtRest( void ) const;
int GetRestStartTime( void ) const;
void Activate( void );
void PutToRest( void );
bool IsPushable( void ) const;
void SaveState( void );
void RestoreState( void );
void SetOrigin( const idVec3 &newOrigin, int id = -1 );
void SetAxis( const idMat3 &newAxis, int id = -1 );
void Translate( const idVec3 &translation, int id = -1 );
void Rotate( const idRotation &rotation, int id = -1 );
const idVec3 & GetOrigin( int id = 0 ) const;
const idMat3 & GetAxis( int id = 0 ) const;
void SetLinearVelocity( const idVec3 &newLinearVelocity, int id = 0 );
void SetAngularVelocity( const idVec3 &newAngularVelocity, int id = 0 );
const idVec3 & GetLinearVelocity( int id = 0 ) const;
const idVec3 & GetAngularVelocity( int id = 0 ) const;
void ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const;
void ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const;
int ClipContents( const idClipModel *model ) const;
void DisableClip( void );
void EnableClip( void );
void UnlinkClip( void );
void LinkClip( void );
bool EvaluateContacts( void );
void SetPushed( int deltaTime );
const idVec3 & GetPushedLinearVelocity( const int id = 0 ) const;
const idVec3 & GetPushedAngularVelocity( const int id = 0 ) const;
void SetMaster( idEntity *master, const bool orientated = true );
void WriteToSnapshot( idBitMsgDelta &msg ) const;
void ReadFromSnapshot( const idBitMsgDelta &msg );
private:
// articulated figure
idList<idAFTree *> trees; // tree structures
idList<idAFBody *> bodies; // all bodies
idList<idAFConstraint *>constraints; // all frame independent constraints
idList<idAFConstraint *>primaryConstraints; // list with primary constraints
idList<idAFConstraint *>auxiliaryConstraints; // list with auxiliary constraints
idList<idAFConstraint *>frameConstraints; // constraints that only live one frame
idList<idAFConstraint_Contact *>contactConstraints; // contact constraints
idList<int> contactBodies; // body id for each contact
idList<AFCollision_t> collisions; // collisions
bool changedAF; // true when the articulated figure just changed
// properties
float linearFriction; // default translational friction
float angularFriction; // default rotational friction
float contactFriction; // default friction with contact surfaces
float bouncyness; // default bouncyness
float totalMass; // total mass of articulated figure
float forceTotalMass; // force this total mass
idVec2 suspendVelocity; // simulation may not be suspended if a body has more velocity
idVec2 suspendAcceleration; // simulation may not be suspended if a body has more acceleration
float noMoveTime; // suspend simulation if hardly any movement for this many seconds
float noMoveTranslation; // maximum translation considered no movement
float noMoveRotation; // maximum rotation considered no movement
float minMoveTime; // if > 0 the simulation is never suspended before running this many seconds
float maxMoveTime; // if > 0 the simulation is always suspeded after running this many seconds
float impulseThreshold; // threshold below which impulses are ignored to avoid continuous activation
float timeScale; // the time is scaled with this value for slow motion effects
float timeScaleRampStart; // start of time scale change
float timeScaleRampEnd; // end of time scale change
float jointFrictionScale; // joint friction scale
float jointFrictionDent; // joint friction dives from 1 to this value and goes up again
float jointFrictionDentStart; // start time of joint friction dent
float jointFrictionDentEnd; // end time of joint friction dent
float jointFrictionDentScale; // dent scale
float contactFrictionScale; // contact friction scale
float contactFrictionDent; // contact friction dives from 1 to this value and goes up again
float contactFrictionDentStart; // start time of contact friction dent
float contactFrictionDentEnd; // end time of contact friction dent
float contactFrictionDentScale; // dent scale
bool enableCollision; // if true collision detection is enabled
bool selfCollision; // if true the self collision is allowed
bool comeToRest; // if true the figure can come to rest
bool linearTime; // if true use the linear time algorithm
bool noImpact; // if true do not activate when another object collides
bool worldConstraintsLocked; // if true world constraints cannot be moved
bool forcePushable; // if true can be pushed even when bound to a master
// physics state
AFPState_t current;
AFPState_t saved;
idAFBody * masterBody; // master body
idLCP * lcp; // linear complementarity problem solver
private:
void BuildTrees( void );
bool IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const;
void PrimaryFactor( void );
void EvaluateBodies( float timeStep );
void EvaluateConstraints( float timeStep );
void AddFrameConstraints( void );
void RemoveFrameConstraints( void );
void ApplyFriction( float timeStep, float endTimeMSec );
void PrimaryForces( float timeStep );
void AuxiliaryForces( float timeStep );
void VerifyContactConstraints( void );
void SetupContactConstraints( void );
void ApplyContactForces( void );
void Evolve( float timeStep );
idEntity * SetupCollisionForBody( idAFBody *body ) const;
bool CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision );
bool ApplyCollisions( float timeStep );
void CheckForCollisions( float timeStep );
void ClearExternalForce( void );
void AddGravity( void );
void SwapStates( void );
bool TestIfAtRest( float timeStep );
void Rest( void );
void AddPushVelocity( const idVec6 &pushVelocity );
void DebugDraw( void );
};
#endif /* !__PHYSICS_AF_H__ */