/* =========================================================================== 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 . 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. =========================================================================== */ #include "sys/platform.h" #include "idlib/math/Quat.h" #include "idlib/Timer.h" #include "gamesys/SysCvar.h" #include "Entity.h" #include "Player.h" #include "WorldSpawn.h" #include "physics/Physics_AF.h" CLASS_DECLARATION( idPhysics_Base, idPhysics_AF ) END_CLASS const float ERROR_REDUCTION = 0.5f; const float ERROR_REDUCTION_MAX = 256.0f; const float LIMIT_ERROR_REDUCTION = 0.3f; const float LCP_EPSILON = 1e-7f; const float LIMIT_LCP_EPSILON = 1e-4f; const float CONTACT_LCP_EPSILON = 1e-6f; const float CENTER_OF_MASS_EPSILON = 1e-4f; const float NO_MOVE_TIME = 1.0f; const float NO_MOVE_TRANSLATION_TOLERANCE = 10.0f; const float NO_MOVE_ROTATION_TOLERANCE = 10.0f; const float MIN_MOVE_TIME = -1.0f; const float MAX_MOVE_TIME = -1.0f; const float IMPULSE_THRESHOLD = 500.0f; const float SUSPEND_LINEAR_VELOCITY = 10.0f; const float SUSPEND_ANGULAR_VELOCITY = 15.0f; const float SUSPEND_LINEAR_ACCELERATION = 20.0f; const float SUSPEND_ANGULAR_ACCELERATION = 30.0f; const idVec6 vec6_lcp_epsilon = idVec6( LCP_EPSILON, LCP_EPSILON, LCP_EPSILON, LCP_EPSILON, LCP_EPSILON, LCP_EPSILON ); #define AF_TIMINGS #ifdef AF_TIMINGS static int lastTimerReset = 0; static int numArticulatedFigures = 0; static idTimer timer_total, timer_pc, timer_ac, timer_collision, timer_lcp; #endif //=============================================================== // // idAFConstraint // //=============================================================== /* ================ idAFConstraint::idAFConstraint ================ */ idAFConstraint::idAFConstraint( void ) { type = CONSTRAINT_INVALID; name = "noname"; body1 = NULL; body2 = NULL; physics = NULL; lo.Zero( 6 ); lo.SubVec6(0) = -vec6_infinity; hi.Zero( 6 ); hi.SubVec6(0) = vec6_infinity; e.SetSize( 6 ); e.SubVec6(0) = vec6_lcp_epsilon; boxConstraint = NULL; boxIndex[0] = -1; boxIndex[1] = -1; boxIndex[2] = -1; boxIndex[3] = -1; boxIndex[4] = -1; boxIndex[5] = -1; firstIndex = 0; memset( &fl, 0, sizeof( fl ) ); } /* ================ idAFConstraint::~idAFConstraint ================ */ idAFConstraint::~idAFConstraint( void ) { } /* ================ idAFConstraint::SetBody1 ================ */ void idAFConstraint::SetBody1( idAFBody *body ) { if ( body1 != body) { body1 = body; if ( physics ) { physics->SetChanged(); } } } /* ================ idAFConstraint::SetBody2 ================ */ void idAFConstraint::SetBody2( idAFBody *body ) { if ( body2 != body ) { body2 = body; if ( physics ) { physics->SetChanged(); } } } /* ================ idAFConstraint::GetMultiplier ================ */ const idVecX &idAFConstraint::GetMultiplier( void ) { return lm; } /* ================ idAFConstraint::Evaluate ================ */ void idAFConstraint::Evaluate( float invTimeStep ) { assert( 0 ); } /* ================ idAFConstraint::ApplyFriction ================ */ void idAFConstraint::ApplyFriction( float invTimeStep ) { } /* ================ idAFConstraint::GetForce ================ */ void idAFConstraint::GetForce( idAFBody *body, idVec6 &force ) { idVecX v; v.SetData( 6, VECX_ALLOCA( 6 ) ); if ( body == body1 ) { J1.TransposeMultiply( v, lm ); } else if ( body == body2 ) { J2.TransposeMultiply( v, lm ); } else { v.Zero(); } force[0] = v[0]; force[1] = v[1]; force[2] = v[2]; force[3] = v[3]; force[4] = v[4]; force[5] = v[5]; } /* ================ idAFConstraint::Translate ================ */ void idAFConstraint::Translate( const idVec3 &translation ) { assert( 0 ); } /* ================ idAFConstraint::Rotate ================ */ void idAFConstraint::Rotate( const idRotation &rotation ) { assert( 0 ); } /* ================ idAFConstraint::GetCenter ================ */ void idAFConstraint::GetCenter( idVec3 ¢er ) { center.Zero(); } /* ================ idAFConstraint::DebugDraw ================ */ void idAFConstraint::DebugDraw( void ) { } /* ================ idAFConstraint::InitSize ================ */ void idAFConstraint::InitSize( int size ) { J1.Zero( size, 6 ); J2.Zero( size, 6 ); c1.Zero( size ); c2.Zero( size ); s.Zero( size ); lm.Zero( size ); } /* ================ idAFConstraint::Save ================ */ void idAFConstraint::Save( idSaveGame *saveFile ) const { saveFile->WriteInt( type ); } /* ================ idAFConstraint::Restore ================ */ void idAFConstraint::Restore( idRestoreGame *saveFile ) { constraintType_t t; saveFile->ReadInt( (int &)t ); assert( t == type ); } //=============================================================== // // idAFConstraint_Fixed // //=============================================================== /* ================ idAFConstraint_Fixed::idAFConstraint_Fixed ================ */ idAFConstraint_Fixed::idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( body1 ); type = CONSTRAINT_FIXED; this->name = name; this->body1 = body1; this->body2 = body2; InitSize( 6 ); fl.allowPrimary = true; fl.noCollision = true; InitOffset(); } /* ================ idAFConstraint_Fixed::InitOffset ================ */ void idAFConstraint_Fixed::InitOffset( void ) { if ( body2 ) { offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose(); } else { offset = body1->GetWorldOrigin(); relAxis = body1->GetWorldAxis(); } } /* ================ idAFConstraint_Fixed::SetBody1 ================ */ void idAFConstraint_Fixed::SetBody1( idAFBody *body ) { if ( body1 != body) { body1 = body; InitOffset(); if ( physics ) { physics->SetChanged(); } } } /* ================ idAFConstraint_Fixed::SetBody2 ================ */ void idAFConstraint_Fixed::SetBody2( idAFBody *body ) { if ( body2 != body ) { body2 = body; InitOffset(); if ( physics ) { physics->SetChanged(); } } } /* ================ idAFConstraint_Fixed::Evaluate ================ */ void idAFConstraint_Fixed::Evaluate( float invTimeStep ) { idVec3 ofs, a2; idMat3 ax; idRotation r; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { a2 = offset * master->GetWorldAxis(); ofs = a2 + master->GetWorldOrigin(); ax = relAxis * master->GetWorldAxis(); } else { a2.Zero(); ofs = offset; ax = relAxis; } J1.Set( mat3_identity, mat3_zero, mat3_zero, mat3_identity ); if ( body2 ) { J2.Set( -mat3_identity, SkewSymmetric( a2 ), mat3_zero, -mat3_identity ); } else { J2.Zero( 6, 6 ); } c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( ofs - body1->GetWorldOrigin() ); r = ( body1->GetWorldAxis().Transpose() * ax ).ToRotation(); c1.SubVec3(1) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * -(float) DEG2RAD( r.GetAngle() ) ); c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); } /* ================ idAFConstraint_Fixed::ApplyFriction ================ */ void idAFConstraint_Fixed::ApplyFriction( float invTimeStep ) { // no friction } /* ================ idAFConstraint_Fixed::Translate ================ */ void idAFConstraint_Fixed::Translate( const idVec3 &translation ) { if ( !body2 ) { offset += translation; } } /* ================ idAFConstraint_Fixed::Rotate ================ */ void idAFConstraint_Fixed::Rotate( const idRotation &rotation ) { if ( !body2 ) { offset *= rotation; relAxis *= rotation.ToMat3(); } } /* ================ idAFConstraint_Fixed::GetCenter ================ */ void idAFConstraint_Fixed::GetCenter( idVec3 ¢er ) { center = body1->GetWorldOrigin(); } /* ================ idAFConstraint_Fixed::DebugDraw ================ */ void idAFConstraint_Fixed::DebugDraw( void ) { idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), master->GetWorldOrigin() ); } else { gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), vec3_origin ); } } /* ================ idAFConstraint_Fixed::Save ================ */ void idAFConstraint_Fixed::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( offset ); saveFile->WriteMat3( relAxis ); } /* ================ idAFConstraint_Fixed::Restore ================ */ void idAFConstraint_Fixed::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( offset ); saveFile->ReadMat3( relAxis ); } //=============================================================== // // idAFConstraint_BallAndSocketJoint // //=============================================================== /* ================ idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint ================ */ idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( body1 ); type = CONSTRAINT_BALLANDSOCKETJOINT; this->name = name; this->body1 = body1; this->body2 = body2; InitSize( 3 ); coneLimit = NULL; pyramidLimit = NULL; friction = 0.0f; fc = NULL; fl.allowPrimary = true; fl.noCollision = true; } /* ================ idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint ================ */ idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint( void ) { if ( coneLimit ) { delete coneLimit; } if ( pyramidLimit ) { delete pyramidLimit; } } /* ================ idAFConstraint_BallAndSocketJoint::SetAnchor ================ */ void idAFConstraint_BallAndSocketJoint::SetAnchor( const idVec3 &worldPosition ) { // get anchor relative to center of mass of body1 anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); if ( body2 ) { // get anchor relative to center of mass of body2 anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldPosition; } if ( coneLimit ) { coneLimit->SetAnchor( anchor2 ); } if ( pyramidLimit ) { pyramidLimit->SetAnchor( anchor2 ); } } /* ================ idAFConstraint_BallAndSocketJoint::GetAnchor ================ */ idVec3 idAFConstraint_BallAndSocketJoint::GetAnchor( void ) const { if ( body2 ) { return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2; } return anchor2; } /* ================ idAFConstraint_BallAndSocketJoint::SetNoLimit ================ */ void idAFConstraint_BallAndSocketJoint::SetNoLimit( void ) { if ( coneLimit ) { delete coneLimit; coneLimit = NULL; } if ( pyramidLimit ) { delete pyramidLimit; pyramidLimit = NULL; } } /* ================ idAFConstraint_BallAndSocketJoint::SetConeLimit ================ */ void idAFConstraint_BallAndSocketJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) { if ( pyramidLimit ) { delete pyramidLimit; pyramidLimit = NULL; } if ( !coneLimit ) { coneLimit = new idAFConstraint_ConeLimit; coneLimit->SetPhysics( physics ); } if ( body2 ) { coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, body1Axis * body1->GetWorldAxis().Transpose() ); } else { coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, body1Axis * body1->GetWorldAxis().Transpose() ); } } /* ================ idAFConstraint_BallAndSocketJoint::SetPyramidLimit ================ */ void idAFConstraint_BallAndSocketJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis, const float angle1, const float angle2, const idVec3 &body1Axis ) { if ( coneLimit ) { delete coneLimit; coneLimit = NULL; } if ( !pyramidLimit ) { pyramidLimit = new idAFConstraint_PyramidLimit; pyramidLimit->SetPhysics( physics ); } if ( body2 ) { pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(), baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, body1Axis * body1->GetWorldAxis().Transpose() ); } else { pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, body1Axis * body1->GetWorldAxis().Transpose() ); } } /* ================ idAFConstraint_BallAndSocketJoint::SetLimitEpsilon ================ */ void idAFConstraint_BallAndSocketJoint::SetLimitEpsilon( const float e ) { if ( coneLimit ) { coneLimit->SetEpsilon( e ); } if ( pyramidLimit ) { pyramidLimit->SetEpsilon( e ); } } /* ================ idAFConstraint_BallAndSocketJoint::GetFriction ================ */ float idAFConstraint_BallAndSocketJoint::GetFriction( void ) const { if ( af_forceFriction.GetFloat() > 0.0f ) { return af_forceFriction.GetFloat(); } return friction * physics->GetJointFrictionScale(); } /* ================ idAFConstraint_BallAndSocketJoint::Evaluate ================ */ void idAFConstraint_BallAndSocketJoint::Evaluate( float invTimeStep ) { idVec3 a1, a2; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); a1 = anchor1 * body1->GetWorldAxis(); if ( master ) { a2 = anchor2 * master->GetWorldAxis(); c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) ); } else { a2.Zero(); c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( anchor2 - ( a1 + body1->GetWorldOrigin() ) ); } c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); J1.Set( mat3_identity, -SkewSymmetric( a1 ) ); if ( body2 ) { J2.Set( -mat3_identity, SkewSymmetric( a2 ) ); } else { J2.Zero( 3, 6 ); } if ( coneLimit ) { coneLimit->Add( physics, invTimeStep ); } else if ( pyramidLimit ) { pyramidLimit->Add( physics, invTimeStep ); } } /* ================ idAFConstraint_BallAndSocketJoint::ApplyFriction ================ */ void idAFConstraint_BallAndSocketJoint::ApplyFriction( float invTimeStep ) { idVec3 angular; float invMass, currentFriction; currentFriction = GetFriction(); if ( currentFriction <= 0.0f ) { return; } if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) { angular = body1->GetAngularVelocity(); invMass = body1->GetInverseMass(); if ( body2 ) { angular -= body2->GetAngularVelocity(); invMass += body2->GetInverseMass(); } angular *= currentFriction / invMass; body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() ); if ( body2 ) { body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() ); } } else { if ( !fc ) { fc = new idAFConstraint_BallAndSocketJointFriction; fc->Setup( this ); } fc->Add( physics, invTimeStep ); } } /* ================ idAFConstraint_BallAndSocketJoint::GetForce ================ */ void idAFConstraint_BallAndSocketJoint::GetForce( idAFBody *body, idVec6 &force ) { idAFConstraint::GetForce( body, force ); // FIXME: add limit force } /* ================ idAFConstraint_BallAndSocketJoint::Translate ================ */ void idAFConstraint_BallAndSocketJoint::Translate( const idVec3 &translation ) { if ( !body2 ) { anchor2 += translation; } if ( coneLimit ) { coneLimit->Translate( translation ); } else if ( pyramidLimit ) { pyramidLimit->Translate( translation ); } } /* ================ idAFConstraint_BallAndSocketJoint::Rotate ================ */ void idAFConstraint_BallAndSocketJoint::Rotate( const idRotation &rotation ) { if ( !body2 ) { anchor2 *= rotation; } if ( coneLimit ) { coneLimit->Rotate( rotation ); } else if ( pyramidLimit ) { pyramidLimit->Rotate( rotation ); } } /* ================ idAFConstraint_BallAndSocketJoint::GetCenter ================ */ void idAFConstraint_BallAndSocketJoint::GetCenter( idVec3 ¢er ) { center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); } /* ================ idAFConstraint_BallAndSocketJoint::DebugDraw ================ */ void idAFConstraint_BallAndSocketJoint::DebugDraw( void ) { idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 5, 0, 0 ), a1 + idVec3( 5, 0, 0 ) ); gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 5, 0 ), a1 + idVec3( 0, 5, 0 ) ); gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 0, 5 ), a1 + idVec3( 0, 0, 5 ) ); if ( af_showLimits.GetBool() ) { if ( coneLimit ) { coneLimit->DebugDraw(); } if ( pyramidLimit ) { pyramidLimit->DebugDraw(); } } } /* ================ idAFConstraint_BallAndSocketJoint::Save ================ */ void idAFConstraint_BallAndSocketJoint::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( anchor1 ); saveFile->WriteVec3( anchor2 ); saveFile->WriteFloat( friction ); if ( coneLimit ) { coneLimit->Save( saveFile ); } if ( pyramidLimit ) { pyramidLimit->Save( saveFile ); } } /* ================ idAFConstraint_BallAndSocketJoint::Restore ================ */ void idAFConstraint_BallAndSocketJoint::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( anchor1 ); saveFile->ReadVec3( anchor2 ); saveFile->ReadFloat( friction ); if ( coneLimit ) { coneLimit->Restore( saveFile ); } if ( pyramidLimit ) { pyramidLimit->Restore( saveFile ); } } //=============================================================== // // idAFConstraint_BallAndSocketJointFriction // //=============================================================== /* ================ idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction ================ */ idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction( void ) { type = CONSTRAINT_FRICTION; name = "ballAndSocketJointFriction"; InitSize( 3 ); joint = NULL; fl.allowPrimary = false; fl.frameConstraint = true; } /* ================ idAFConstraint_BallAndSocketJointFriction::Setup ================ */ void idAFConstraint_BallAndSocketJointFriction::Setup( idAFConstraint_BallAndSocketJoint *bsj ) { this->joint = bsj; body1 = bsj->GetBody1(); body2 = bsj->GetBody2(); } /* ================ idAFConstraint_BallAndSocketJointFriction::Evaluate ================ */ void idAFConstraint_BallAndSocketJointFriction::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_BallAndSocketJointFriction::ApplyFriction ================ */ void idAFConstraint_BallAndSocketJointFriction::ApplyFriction( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_BallAndSocketJointFriction::Add ================ */ bool idAFConstraint_BallAndSocketJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) { float f; physics = phys; f = joint->GetFriction() * joint->GetMultiplier().Length(); if ( f == 0.0f ) { return false; } lo[0] = lo[1] = lo[2] = -f; hi[0] = hi[1] = hi[2] = f; J1.Zero( 3, 6 ); J1[0][3] = J1[1][4] = J1[2][5] = 1.0f; if ( body2 ) { J2.Zero( 3, 6 ); J2[0][3] = J2[1][4] = J2[2][5] = 1.0f; } physics->AddFrameConstraint( this ); return true; } /* ================ idAFConstraint_BallAndSocketJointFriction::Translate ================ */ void idAFConstraint_BallAndSocketJointFriction::Translate( const idVec3 &translation ) { } /* ================ idAFConstraint_BallAndSocketJointFriction::Rotate ================ */ void idAFConstraint_BallAndSocketJointFriction::Rotate( const idRotation &rotation ) { } //=============================================================== // // idAFConstraint_UniversalJoint // //=============================================================== /* ================ idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint ================ */ idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( body1 ); type = CONSTRAINT_UNIVERSALJOINT; this->name = name; this->body1 = body1; this->body2 = body2; InitSize( 4 ); coneLimit = NULL; pyramidLimit = NULL; friction = 0.0f; fc = NULL; fl.allowPrimary = true; fl.noCollision = true; } /* ================ idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint ================ */ idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint( void ) { if ( coneLimit ) { delete coneLimit; } if ( pyramidLimit ) { delete pyramidLimit; } if ( fc ) { delete fc; } } /* ================ idAFConstraint_UniversalJoint::SetAnchor ================ */ void idAFConstraint_UniversalJoint::SetAnchor( const idVec3 &worldPosition ) { // get anchor relative to center of mass of body1 anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); if ( body2 ) { // get anchor relative to center of mass of body2 anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldPosition; } if ( coneLimit ) { coneLimit->SetAnchor( anchor2 ); } if ( pyramidLimit ) { pyramidLimit->SetAnchor( anchor2 ); } } /* ================ idAFConstraint_UniversalJoint::GetAnchor ================ */ idVec3 idAFConstraint_UniversalJoint::GetAnchor( void ) const { if ( body2 ) { return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2; } return anchor2; } /* ================ idAFConstraint_UniversalJoint::SetShafts ================ */ void idAFConstraint_UniversalJoint::SetShafts( const idVec3 &cardanShaft1, const idVec3 &cardanShaft2 ) { idVec3 cardanAxis; float l id_attribute((unused)); shaft1 = cardanShaft1; l = shaft1.Normalize(); assert( l != 0.0f ); shaft2 = cardanShaft2; l = shaft2.Normalize(); assert( l != 0.0f ); // the cardan axis is a vector orthogonal to both cardan shafts cardanAxis = shaft1.Cross( shaft2 ); if ( cardanAxis.Normalize() == 0.0f ) { idVec3 vecY; shaft1.OrthogonalBasis( cardanAxis, vecY ); cardanAxis.Normalize(); } shaft1 *= body1->GetWorldAxis().Transpose(); axis1 = cardanAxis * body1->GetWorldAxis().Transpose(); if ( body2 ) { shaft2 *= body2->GetWorldAxis().Transpose(); axis2 = cardanAxis * body2->GetWorldAxis().Transpose(); } else { axis2 = cardanAxis; } if ( coneLimit ) { coneLimit->SetBody1Axis( shaft1 ); } if ( pyramidLimit ) { pyramidLimit->SetBody1Axis( shaft1 ); } } /* ================ idAFConstraint_UniversalJoint::SetNoLimit ================ */ void idAFConstraint_UniversalJoint::SetNoLimit( void ) { if ( coneLimit ) { delete coneLimit; coneLimit = NULL; } if ( pyramidLimit ) { delete pyramidLimit; pyramidLimit = NULL; } } /* ================ idAFConstraint_UniversalJoint::SetConeLimit ================ */ void idAFConstraint_UniversalJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle ) { if ( pyramidLimit ) { delete pyramidLimit; pyramidLimit = NULL; } if ( !coneLimit ) { coneLimit = new idAFConstraint_ConeLimit; coneLimit->SetPhysics( physics ); } if ( body2 ) { coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, shaft1 ); } else { coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, shaft1 ); } } /* ================ idAFConstraint_UniversalJoint::SetPyramidLimit ================ */ void idAFConstraint_UniversalJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis, const float angle1, const float angle2 ) { if ( coneLimit ) { delete coneLimit; coneLimit = NULL; } if ( !pyramidLimit ) { pyramidLimit = new idAFConstraint_PyramidLimit; pyramidLimit->SetPhysics( physics ); } if ( body2 ) { pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(), baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, shaft1 ); } else { pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, shaft1 ); } } /* ================ idAFConstraint_UniversalJoint::SetLimitEpsilon ================ */ void idAFConstraint_UniversalJoint::SetLimitEpsilon( const float e ) { if ( coneLimit ) { coneLimit->SetEpsilon( e ); } if ( pyramidLimit ) { pyramidLimit->SetEpsilon( e ); } } /* ================ idAFConstraint_UniversalJoint::GetFriction ================ */ float idAFConstraint_UniversalJoint::GetFriction( void ) const { if ( af_forceFriction.GetFloat() > 0.0f ) { return af_forceFriction.GetFloat(); } return friction * physics->GetJointFrictionScale(); } /* ================ idAFConstraint_UniversalJoint::Evaluate NOTE: this joint is homokinetic ================ */ void idAFConstraint_UniversalJoint::Evaluate( float invTimeStep ) { idVec3 a1, a2, s1, s2, d1, d2, v; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); a1 = anchor1 * body1->GetWorldAxis(); s1 = shaft1 * body1->GetWorldAxis(); d1 = s1.Cross( axis1 * body1->GetWorldAxis() ); if ( master ) { a2 = anchor2 * master->GetWorldAxis(); s2 = shaft2 * master->GetWorldAxis(); d2 = axis2 * master->GetWorldAxis(); c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) ); } else { a2 = anchor2; s2 = shaft2; d2 = axis2; c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) ); } J1.Set( mat3_identity, -SkewSymmetric( a1 ), mat3_zero, idMat3( s1[0], s1[1], s1[2], 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f ) ); J1.SetSize( 4, 6 ); if ( body2 ) { J2.Set( -mat3_identity, SkewSymmetric( a2 ), mat3_zero, idMat3( s2[0], s2[1], s2[2], 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f ) ); J2.SetSize( 4, 6 ); } else { J2.Zero( 4, 6 ); } v = s1.Cross( s2 ); if ( v.Normalize() != 0.0f ) { idMat3 m1, m2; m1[0] = s1; m1[1] = v; m1[2] = v.Cross( m1[0] ); m2[0] = -s2; m2[1] = v; m2[2] = v.Cross( m2[0] ); d2 *= m2.Transpose() * m1; } c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( d1 * d2 ); c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); if ( coneLimit ) { coneLimit->Add( physics, invTimeStep ); } else if ( pyramidLimit ) { pyramidLimit->Add( physics, invTimeStep ); } } /* ================ idAFConstraint_UniversalJoint::ApplyFriction ================ */ void idAFConstraint_UniversalJoint::ApplyFriction( float invTimeStep ) { idVec3 angular; float invMass, currentFriction; currentFriction = GetFriction(); if ( currentFriction <= 0.0f ) { return; } if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) { angular = body1->GetAngularVelocity(); invMass = body1->GetInverseMass(); if ( body2 ) { angular -= body2->GetAngularVelocity(); invMass += body2->GetInverseMass(); } angular *= currentFriction / invMass; body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() ); if ( body2 ) { body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() ); } } else { if ( !fc ) { fc = new idAFConstraint_UniversalJointFriction; fc->Setup( this ); } fc->Add( physics, invTimeStep ); } } /* ================ idAFConstraint_UniversalJoint::GetForce ================ */ void idAFConstraint_UniversalJoint::GetForce( idAFBody *body, idVec6 &force ) { idAFConstraint::GetForce( body, force ); // FIXME: add limit force } /* ================ idAFConstraint_UniversalJoint::Translate ================ */ void idAFConstraint_UniversalJoint::Translate( const idVec3 &translation ) { if ( !body2 ) { anchor2 += translation; } if ( coneLimit ) { coneLimit->Translate( translation ); } else if ( pyramidLimit ) { pyramidLimit->Translate( translation ); } } /* ================ idAFConstraint_UniversalJoint::Rotate ================ */ void idAFConstraint_UniversalJoint::Rotate( const idRotation &rotation ) { if ( !body2 ) { anchor2 *= rotation; shaft2 *= rotation.ToMat3(); axis2 *= rotation.ToMat3(); } if ( coneLimit ) { coneLimit->Rotate( rotation ); } else if ( pyramidLimit ) { pyramidLimit->Rotate( rotation ); } } /* ================ idAFConstraint_UniversalJoint::GetCenter ================ */ void idAFConstraint_UniversalJoint::GetCenter( idVec3 ¢er ) { center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); } /* ================ idAFConstraint_UniversalJoint::DebugDraw ================ */ void idAFConstraint_UniversalJoint::DebugDraw( void ) { idVec3 a1, a2, s1, s2, d1, d2, v; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); s1 = shaft1 * body1->GetWorldAxis(); d1 = axis1 * body1->GetWorldAxis(); if ( master ) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); s2 = shaft2 * master->GetWorldAxis(); d2 = axis2 * master->GetWorldAxis(); } else { a2 = anchor2; s2 = shaft2; d2 = axis2; } v = s1.Cross( s2 ); if ( v.Normalize() != 0.0f ) { idMat3 m1, m2; m1[0] = s1; m1[1] = v; m1[2] = v.Cross( m1[0] ); m2[0] = -s2; m2[1] = v; m2[2] = v.Cross( m2[0] ); d2 *= m2.Transpose() * m1; } gameRenderWorld->DebugArrow( colorCyan, a1, a1 + s1 * 5.0f, 1.0f ); gameRenderWorld->DebugArrow( colorBlue, a2, a2 + s2 * 5.0f, 1.0f ); gameRenderWorld->DebugLine( colorGreen, a1, a1 + d1 * 5.0f ); gameRenderWorld->DebugLine( colorGreen, a2, a2 + d2 * 5.0f ); if ( af_showLimits.GetBool() ) { if ( coneLimit ) { coneLimit->DebugDraw(); } if ( pyramidLimit ) { pyramidLimit->DebugDraw(); } } } /* ================ idAFConstraint_UniversalJoint::Save ================ */ void idAFConstraint_UniversalJoint::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( anchor1 ); saveFile->WriteVec3( anchor2 ); saveFile->WriteVec3( shaft1 ); saveFile->WriteVec3( shaft2 ); saveFile->WriteVec3( axis1 ); saveFile->WriteVec3( axis2 ); saveFile->WriteFloat( friction ); if ( coneLimit ) { coneLimit->Save( saveFile ); } if ( pyramidLimit ) { pyramidLimit->Save( saveFile ); } } /* ================ idAFConstraint_UniversalJoint::Restore ================ */ void idAFConstraint_UniversalJoint::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( anchor1 ); saveFile->ReadVec3( anchor2 ); saveFile->ReadVec3( shaft1 ); saveFile->ReadVec3( shaft2 ); saveFile->ReadVec3( axis1 ); saveFile->ReadVec3( axis2 ); saveFile->ReadFloat( friction ); if ( coneLimit ) { coneLimit->Restore( saveFile ); } if ( pyramidLimit ) { pyramidLimit->Restore( saveFile ); } } //=============================================================== // // idAFConstraint_UniversalJointFriction // //=============================================================== /* ================ idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction ================ */ idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction( void ) { type = CONSTRAINT_FRICTION; name = "universalJointFriction"; InitSize( 2 ); joint = NULL; fl.allowPrimary = false; fl.frameConstraint = true; } /* ================ idAFConstraint_UniversalJointFriction::Setup ================ */ void idAFConstraint_UniversalJointFriction::Setup( idAFConstraint_UniversalJoint *uj ) { this->joint = uj; body1 = uj->GetBody1(); body2 = uj->GetBody2(); } /* ================ idAFConstraint_UniversalJointFriction::Evaluate ================ */ void idAFConstraint_UniversalJointFriction::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_UniversalJointFriction::ApplyFriction ================ */ void idAFConstraint_UniversalJointFriction::ApplyFriction( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_UniversalJointFriction::Add ================ */ bool idAFConstraint_UniversalJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) { idVec3 s1, s2, dir1, dir2; float f; physics = phys; f = joint->GetFriction() * joint->GetMultiplier().Length(); if ( f == 0.0f ) { return false; } lo[0] = lo[1] = -f; hi[0] = hi[1] = f; joint->GetShafts( s1, s2 ); s1 *= body1->GetWorldAxis(); s1.NormalVectors( dir1, dir2 ); J1.SetSize( 2, 6 ); J1.SubVec6(0).SubVec3(0).Zero(); J1.SubVec6(0).SubVec3(1) = dir1; J1.SubVec6(1).SubVec3(0).Zero(); J1.SubVec6(1).SubVec3(1) = dir2; if ( body2 ) { J2.SetSize( 2, 6 ); J2.SubVec6(0).SubVec3(0).Zero(); J2.SubVec6(0).SubVec3(1) = -dir1; J2.SubVec6(1).SubVec3(0).Zero(); J2.SubVec6(1).SubVec3(1) = -dir2; } physics->AddFrameConstraint( this ); return true; } /* ================ idAFConstraint_UniversalJointFriction::Translate ================ */ void idAFConstraint_UniversalJointFriction::Translate( const idVec3 &translation ) { } /* ================ idAFConstraint_UniversalJointFriction::Rotate ================ */ void idAFConstraint_UniversalJointFriction::Rotate( const idRotation &rotation ) { } //=============================================================== // // idAFConstraint_CylindricalJoint // //=============================================================== /* ================ idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint ================ */ idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_CylindricalJoint::Evaluate ================ */ void idAFConstraint_CylindricalJoint::Evaluate( float invTimeStep ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_CylindricalJoint::ApplyFriction ================ */ void idAFConstraint_CylindricalJoint::ApplyFriction( float invTimeStep ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_CylindricalJoint::Translate ================ */ void idAFConstraint_CylindricalJoint::Translate( const idVec3 &translation ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_CylindricalJoint::Rotate ================ */ void idAFConstraint_CylindricalJoint::Rotate( const idRotation &rotation ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_CylindricalJoint::DebugDraw ================ */ void idAFConstraint_CylindricalJoint::DebugDraw( void ) { assert( 0 ); // FIXME: implement } //=============================================================== // // idAFConstraint_Hinge // //=============================================================== /* ================ idAFConstraint_Hinge::idAFConstraint_Hinge ================ */ idAFConstraint_Hinge::idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( body1 ); type = CONSTRAINT_HINGE; this->name = name; this->body1 = body1; this->body2 = body2; InitSize( 5 ); coneLimit = NULL; steering = NULL; friction = 0.0f; fc = NULL; fl.allowPrimary = true; fl.noCollision = true; initialAxis = body1->GetWorldAxis(); if ( body2 ) { initialAxis *= body2->GetWorldAxis().Transpose(); } } /* ================ idAFConstraint_Hinge::~idAFConstraint_Hinge ================ */ idAFConstraint_Hinge::~idAFConstraint_Hinge( void ) { if ( coneLimit ) { delete coneLimit; } if ( fc ) { delete fc; } if ( steering ) { delete steering; } } /* ================ idAFConstraint_Hinge::SetAnchor ================ */ void idAFConstraint_Hinge::SetAnchor( const idVec3 &worldPosition ) { // get anchor relative to center of mass of body1 anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); if ( body2 ) { // get anchor relative to center of mass of body2 anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldPosition; } if ( coneLimit ) { coneLimit->SetAnchor( anchor2 ); } } /* ================ idAFConstraint_Hinge::GetAnchor ================ */ idVec3 idAFConstraint_Hinge::GetAnchor( void ) const { if ( body2 ) { return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2; } return anchor2; } /* ================ idAFConstraint_Hinge::SetAxis ================ */ void idAFConstraint_Hinge::SetAxis( const idVec3 &axis ) { idVec3 normAxis; normAxis = axis; normAxis.Normalize(); // get axis relative to body1 axis1 = normAxis * body1->GetWorldAxis().Transpose(); if ( body2 ) { // get axis relative to body2 axis2 = normAxis * body2->GetWorldAxis().Transpose(); } else { axis2 = normAxis; } } /* ================ idAFConstraint_Hinge::GetAxis ================ */ idVec3 idAFConstraint_Hinge::GetAxis( void ) const { if ( body2 ) { return axis2 * body2->GetWorldAxis(); } return axis2; } /* ================ idAFConstraint_Hinge::SetNoLimit ================ */ void idAFConstraint_Hinge::SetNoLimit( void ) { if ( coneLimit ) { delete coneLimit; coneLimit = NULL; } } /* ================ idAFConstraint_Hinge::SetLimit ================ */ void idAFConstraint_Hinge::SetLimit( const idVec3 &axis, const float angle, const idVec3 &body1Axis ) { if ( !coneLimit ) { coneLimit = new idAFConstraint_ConeLimit; coneLimit->SetPhysics( physics ); } if ( body2 ) { coneLimit->Setup( body1, body2, anchor2, axis * body2->GetWorldAxis().Transpose(), angle, body1Axis * body1->GetWorldAxis().Transpose() ); } else { coneLimit->Setup( body1, body2, anchor2, axis, angle, body1Axis * body1->GetWorldAxis().Transpose() ); } } /* ================ idAFConstraint_Hinge::SetLimitEpsilon ================ */ void idAFConstraint_Hinge::SetLimitEpsilon( const float e ) { if ( coneLimit ) { coneLimit->SetEpsilon( e ); } } /* ================ idAFConstraint_Hinge::GetFriction ================ */ float idAFConstraint_Hinge::GetFriction( void ) const { if ( af_forceFriction.GetFloat() > 0.0f ) { return af_forceFriction.GetFloat(); } return friction * physics->GetJointFrictionScale(); } /* ================ idAFConstraint_Hinge::GetAngle ================ */ float idAFConstraint_Hinge::GetAngle( void ) const { idMat3 axis; idRotation rotation; float angle; axis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose() * initialAxis.Transpose(); rotation = axis.ToRotation(); angle = rotation.GetAngle(); if ( rotation.GetVec() * axis1 < 0.0f ) { return -angle; } return angle; } /* ================ idAFConstraint_Hinge::SetSteerAngle ================ */ void idAFConstraint_Hinge::SetSteerAngle( const float degrees ) { if ( coneLimit ) { delete coneLimit; coneLimit = NULL; } if ( !steering ) { steering = new idAFConstraint_HingeSteering(); steering->Setup( this ); } steering->SetSteerAngle( degrees ); } /* ================ idAFConstraint_Hinge::SetSteerSpeed ================ */ void idAFConstraint_Hinge::SetSteerSpeed( const float speed ) { if ( steering ) { steering->SetSteerSpeed( speed ); } } /* ================ idAFConstraint_Hinge::Evaluate ================ */ void idAFConstraint_Hinge::Evaluate( float invTimeStep ) { idVec3 a1, a2; idVec3 x1, x2, cross; idVec3 vecX, vecY; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); x1 = axis1 * body1->GetWorldAxis(); // axis in body1 space x1.OrthogonalBasis( vecX, vecY ); // basis for axis in body1 space a1 = anchor1 * body1->GetWorldAxis(); // anchor in body1 space if ( master ) { a2 = anchor2 * master->GetWorldAxis(); // anchor in master space x2 = axis2 * master->GetWorldAxis(); c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) ); } else { a2 = anchor2; x2 = axis2; c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) ); } J1.Set( mat3_identity, -SkewSymmetric( a1 ), mat3_zero, idMat3( vecX[0], vecX[1], vecX[2], vecY[0], vecY[1], vecY[2], 0.0f, 0.0f, 0.0f ) ); J1.SetSize( 5, 6 ); if ( body2 ) { J2.Set( -mat3_identity, SkewSymmetric( a2 ), mat3_zero, idMat3( -vecX[0], -vecX[1], -vecX[2], -vecY[0], -vecY[1], -vecY[2], 0.0f, 0.0f, 0.0f ) ); J2.SetSize( 5, 6 ); } else { J2.Zero( 5, 6 ); } cross = x1.Cross( x2 ); c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecX ); c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecY ); c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); if ( steering ) { steering->Add( physics, invTimeStep ); } else if ( coneLimit ) { coneLimit->Add( physics, invTimeStep ); } } /* ================ idAFConstraint_Hinge::ApplyFriction ================ */ void idAFConstraint_Hinge::ApplyFriction( float invTimeStep ) { idVec3 angular; float invMass, currentFriction; currentFriction = GetFriction(); if ( currentFriction <= 0.0f ) { return; } if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) { angular = body1->GetAngularVelocity(); invMass = body1->GetInverseMass(); if ( body2 ) { angular -= body2->GetAngularVelocity(); invMass += body2->GetInverseMass(); } angular *= currentFriction / invMass; body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() ); if ( body2 ) { body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() ); } } else { if ( !fc ) { fc = new idAFConstraint_HingeFriction; fc->Setup( this ); } fc->Add( physics, invTimeStep ); } } /* ================ idAFConstraint_Hinge::GetForce ================ */ void idAFConstraint_Hinge::GetForce( idAFBody *body, idVec6 &force ) { idAFConstraint::GetForce( body, force ); // FIXME: add limit force } /* ================ idAFConstraint_Hinge::Translate ================ */ void idAFConstraint_Hinge::Translate( const idVec3 &translation ) { if ( !body2 ) { anchor2 += translation; } if ( coneLimit ) { coneLimit->Translate( translation ); } } /* ================ idAFConstraint_Hinge::Rotate ================ */ void idAFConstraint_Hinge::Rotate( const idRotation &rotation ) { if ( !body2 ) { anchor2 *= rotation; axis2 *= rotation.ToMat3(); } if ( coneLimit ) { coneLimit->Rotate( rotation ); } } /* ================ idAFConstraint_Hinge::GetCenter ================ */ void idAFConstraint_Hinge::GetCenter( idVec3 ¢er ) { center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); } /* ================ idAFConstraint_Hinge::DebugDraw ================ */ void idAFConstraint_Hinge::DebugDraw( void ) { idVec3 vecX, vecY; idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); idVec3 x1 = axis1 * body1->GetWorldAxis(); x1.OrthogonalBasis( vecX, vecY ); gameRenderWorld->DebugArrow( colorBlue, a1 - 4.0f * x1, a1 + 4.0f * x1, 1 ); gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecX, a1 + 2.0f * vecX ); gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecY, a1 + 2.0f * vecY ); if ( af_showLimits.GetBool() ) { if ( coneLimit ) { coneLimit->DebugDraw(); } } } /* ================ idAFConstraint_Hinge::Save ================ */ void idAFConstraint_Hinge::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( anchor1 ); saveFile->WriteVec3( anchor2 ); saveFile->WriteVec3( axis1 ); saveFile->WriteVec3( axis2 ); saveFile->WriteMat3( initialAxis ); saveFile->WriteFloat( friction ); if ( coneLimit ) { saveFile->WriteBool( true ); coneLimit->Save( saveFile ); } else { saveFile->WriteBool( false ); } if ( steering ) { saveFile->WriteBool( true ); steering->Save( saveFile ); } else { saveFile->WriteBool( false ); } if ( fc ) { saveFile->WriteBool( true ); fc->Save( saveFile ); } else { saveFile->WriteBool( false ); } } /* ================ idAFConstraint_Hinge::Restore ================ */ void idAFConstraint_Hinge::Restore( idRestoreGame *saveFile ) { bool b; idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( anchor1 ); saveFile->ReadVec3( anchor2 ); saveFile->ReadVec3( axis1 ); saveFile->ReadVec3( axis2 ); saveFile->ReadMat3( initialAxis ); saveFile->ReadFloat( friction ); saveFile->ReadBool( b ); if ( b ) { if ( !coneLimit ) { coneLimit = new idAFConstraint_ConeLimit; } coneLimit->SetPhysics( physics ); coneLimit->Restore( saveFile ); } saveFile->ReadBool( b ); if ( b ) { if ( !steering ) { steering = new idAFConstraint_HingeSteering; } steering->Setup( this ); steering->Restore( saveFile ); } saveFile->ReadBool( b ); if ( b ) { if ( !fc ) { fc = new idAFConstraint_HingeFriction; } fc->Setup( this ); fc->Restore( saveFile ); } } //=============================================================== // // idAFConstraint_HingeFriction // //=============================================================== /* ================ idAFConstraint_HingeFriction::idAFConstraint_HingeFriction ================ */ idAFConstraint_HingeFriction::idAFConstraint_HingeFriction( void ) { type = CONSTRAINT_FRICTION; name = "hingeFriction"; InitSize( 1 ); hinge = NULL; fl.allowPrimary = false; fl.frameConstraint = true; } /* ================ idAFConstraint_HingeFriction::Setup ================ */ void idAFConstraint_HingeFriction::Setup( idAFConstraint_Hinge *h ) { this->hinge = h; body1 = h->GetBody1(); body2 = h->GetBody2(); } /* ================ idAFConstraint_HingeFriction::Evaluate ================ */ void idAFConstraint_HingeFriction::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_HingeFriction::ApplyFriction ================ */ void idAFConstraint_HingeFriction::ApplyFriction( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_HingeFriction::Add ================ */ bool idAFConstraint_HingeFriction::Add( idPhysics_AF *phys, float invTimeStep ) { idVec3 a1, a2; float f; physics = phys; f = hinge->GetFriction() * hinge->GetMultiplier().Length(); if ( f == 0.0f ) { return false; } lo[0] = -f; hi[0] = f; hinge->GetAxis( a1, a2 ); a1 *= body1->GetWorldAxis(); J1.SetSize( 1, 6 ); J1.SubVec6(0).SubVec3(0).Zero(); J1.SubVec6(0).SubVec3(1) = a1; if ( body2 ) { a2 *= body2->GetWorldAxis(); J2.SetSize( 1, 6 ); J2.SubVec6(0).SubVec3(0).Zero(); J2.SubVec6(0).SubVec3(1) = -a2; } physics->AddFrameConstraint( this ); return true; } /* ================ idAFConstraint_HingeFriction::Translate ================ */ void idAFConstraint_HingeFriction::Translate( const idVec3 &translation ) { } /* ================ idAFConstraint_HingeFriction::Rotate ================ */ void idAFConstraint_HingeFriction::Rotate( const idRotation &rotation ) { } //=============================================================== // // idAFConstraint_HingeSteering // //=============================================================== /* ================ idAFConstraint_HingeSteering::idAFConstraint_HingeSteering ================ */ idAFConstraint_HingeSteering::idAFConstraint_HingeSteering( void ) { type = CONSTRAINT_HINGESTEERING; name = "hingeFriction"; InitSize( 1 ); hinge = NULL; fl.allowPrimary = false; fl.frameConstraint = true; steerSpeed = 0.0f; epsilon = LCP_EPSILON; } /* ================ idAFConstraint_HingeSteering::Save ================ */ void idAFConstraint_HingeSteering::Save( idSaveGame *saveFile ) const { saveFile->WriteFloat(steerAngle); saveFile->WriteFloat(steerSpeed); saveFile->WriteFloat(epsilon); } /* ================ idAFConstraint_HingeSteering::Restore ================ */ void idAFConstraint_HingeSteering::Restore( idRestoreGame *saveFile ) { saveFile->ReadFloat(steerAngle); saveFile->ReadFloat(steerSpeed); saveFile->ReadFloat(epsilon); } /* ================ idAFConstraint_HingeSteering::Setup ================ */ void idAFConstraint_HingeSteering::Setup( idAFConstraint_Hinge *h ) { this->hinge = h; body1 = h->GetBody1(); body2 = h->GetBody2(); } /* ================ idAFConstraint_HingeSteering::Evaluate ================ */ void idAFConstraint_HingeSteering::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_HingeSteering::ApplyFriction ================ */ void idAFConstraint_HingeSteering::ApplyFriction( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_HingeSteering::Add ================ */ bool idAFConstraint_HingeSteering::Add( idPhysics_AF *phys, float invTimeStep ) { float angle, speed; idVec3 a1, a2; physics = phys; hinge->GetAxis( a1, a2 ); angle = hinge->GetAngle(); a1 *= body1->GetWorldAxis(); J1.SetSize( 1, 6 ); J1.SubVec6(0).SubVec3(0).Zero(); J1.SubVec6(0).SubVec3(1) = a1; if ( body2 ) { a2 *= body2->GetWorldAxis(); J2.SetSize( 1, 6 ); J2.SubVec6(0).SubVec3(0).Zero(); J2.SubVec6(0).SubVec3(1) = -a2; } speed = steerAngle - angle; if ( steerSpeed != 0.0f ) { if ( speed > steerSpeed ) { speed = steerSpeed; } else if ( speed < -steerSpeed ) { speed = -steerSpeed; } } c1[0] = DEG2RAD( speed ) * invTimeStep; physics->AddFrameConstraint( this ); return true; } /* ================ idAFConstraint_HingeSteering::Translate ================ */ void idAFConstraint_HingeSteering::Translate( const idVec3 &translation ) { } /* ================ idAFConstraint_HingeSteering::Rotate ================ */ void idAFConstraint_HingeSteering::Rotate( const idRotation &rotation ) { } //=============================================================== // // idAFConstraint_Slider // //=============================================================== /* ================ idAFConstraint_Slider::idAFConstraint_Slider ================ */ idAFConstraint_Slider::idAFConstraint_Slider( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( body1 ); type = CONSTRAINT_SLIDER; this->name = name; this->body1 = body1; this->body2 = body2; InitSize( 5 ); fl.allowPrimary = true; fl.noCollision = true; if ( body2 ) { offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose(); } else { offset = body1->GetWorldOrigin(); relAxis = body1->GetWorldAxis(); } } /* ================ idAFConstraint_Slider::SetAxis ================ */ void idAFConstraint_Slider::SetAxis( const idVec3 &ax ) { idVec3 normAxis; // get normalized axis relative to body1 normAxis = ax; normAxis.Normalize(); if ( body2 ) { axis = normAxis * body2->GetWorldAxis().Transpose(); } else { axis = normAxis; } } /* ================ idAFConstraint_Slider::Evaluate ================ */ void idAFConstraint_Slider::Evaluate( float invTimeStep ) { idVec3 vecX, vecY, ofs; idRotation r; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { (axis * master->GetWorldAxis()).OrthogonalBasis( vecX, vecY ); ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin(); r = ( body1->GetWorldAxis().Transpose() * (relAxis * master->GetWorldAxis()) ).ToRotation(); } else { axis.OrthogonalBasis( vecX, vecY ); ofs = offset - body1->GetWorldOrigin(); r = ( body1->GetWorldAxis().Transpose() * relAxis ).ToRotation(); } J1.Set( mat3_zero, mat3_identity, idMat3( vecX, vecY, vec3_origin ), mat3_zero ); J1.SetSize( 5, 6 ); if ( body2 ) { J2.Set( mat3_zero, -mat3_identity, idMat3( -vecX, -vecY, vec3_origin ), mat3_zero ); J2.SetSize( 5, 6 ); } else { J2.Zero( 5, 6 ); } c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * - (float) DEG2RAD( r.GetAngle() ) ); c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( vecX * ofs ); c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( vecY * ofs ); c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); } /* ================ idAFConstraint_Slider::ApplyFriction ================ */ void idAFConstraint_Slider::ApplyFriction( float invTimeStep ) { // no friction } /* ================ idAFConstraint_Slider::Translate ================ */ void idAFConstraint_Slider::Translate( const idVec3 &translation ) { if ( !body2 ) { offset += translation; } } /* ================ idAFConstraint_Slider::Rotate ================ */ void idAFConstraint_Slider::Rotate( const idRotation &rotation ) { if ( !body2 ) { offset *= rotation; } } /* ================ idAFConstraint_Slider::GetCenter ================ */ void idAFConstraint_Slider::GetCenter( idVec3 ¢er ) { idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { center = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin(); } else { center = offset - body1->GetWorldOrigin(); } } /* ================ idAFConstraint_Slider::DebugDraw ================ */ void idAFConstraint_Slider::DebugDraw( void ) { idVec3 ofs; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin(); } else { ofs = offset - body1->GetWorldOrigin(); } gameRenderWorld->DebugLine( colorGreen, ofs, ofs + axis * body1->GetWorldAxis() ); } /* ================ idAFConstraint_Slider::Save ================ */ void idAFConstraint_Slider::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( axis ); saveFile->WriteVec3( offset ); saveFile->WriteMat3( relAxis ); } /* ================ idAFConstraint_Slider::Restore ================ */ void idAFConstraint_Slider::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( axis ); saveFile->ReadVec3( offset ); saveFile->ReadMat3( relAxis ); } //=============================================================== // // idAFConstraint_Line // //=============================================================== /* ================ idAFConstraint_Line::idAFConstraint_Line ================ */ idAFConstraint_Line::idAFConstraint_Line( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_Line::Evaluate ================ */ void idAFConstraint_Line::Evaluate( float invTimeStep ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_Line::ApplyFriction ================ */ void idAFConstraint_Line::ApplyFriction( float invTimeStep ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_Line::Translate ================ */ void idAFConstraint_Line::Translate( const idVec3 &translation ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_Line::Rotate ================ */ void idAFConstraint_Line::Rotate( const idRotation &rotation ) { assert( 0 ); // FIXME: implement } /* ================ idAFConstraint_Line::DebugDraw ================ */ void idAFConstraint_Line::DebugDraw( void ) { assert( 0 ); // FIXME: implement } //=============================================================== // // idAFConstraint_Plane // //=============================================================== /* ================ idAFConstraint_Plane::idAFConstraint_Plane ================ */ idAFConstraint_Plane::idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( body1 ); type = CONSTRAINT_PLANE; this->name = name; this->body1 = body1; this->body2 = body2; InitSize( 1 ); fl.allowPrimary = true; fl.noCollision = true; } /* ================ idAFConstraint_Plane::SetPlane ================ */ void idAFConstraint_Plane::SetPlane( const idVec3 &normal, const idVec3 &anchor ) { // get anchor relative to center of mass of body1 anchor1 = ( anchor - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); if ( body2 ) { // get anchor relative to center of mass of body2 anchor2 = ( anchor - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); planeNormal = normal * body2->GetWorldAxis().Transpose(); } else { anchor2 = anchor; planeNormal = normal; } } /* ================ idAFConstraint_Plane::Evaluate ================ */ void idAFConstraint_Plane::Evaluate( float invTimeStep ) { idVec3 a1, a2, normal, p; idVec6 v; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); if ( master ) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); normal = planeNormal * master->GetWorldAxis(); } else { a2 = anchor2; normal = planeNormal; } p = a1 - body1->GetWorldOrigin(); v.SubVec3(0) = normal; v.SubVec3(1) = p.Cross( normal ); J1.Set( 1, 6, v.ToFloatPtr() ); if ( body2 ) { p = a1 - body2->GetWorldOrigin(); v.SubVec3(0) = -normal; v.SubVec3(1) = p.Cross( -normal ); J2.Set( 1, 6, v.ToFloatPtr() ); } c1[0] = -( invTimeStep * ERROR_REDUCTION ) * (a1 * normal - a2 * normal); c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); } /* ================ idAFConstraint_Plane::ApplyFriction ================ */ void idAFConstraint_Plane::ApplyFriction( float invTimeStep ) { // no friction } /* ================ idAFConstraint_Plane::Translate ================ */ void idAFConstraint_Plane::Translate( const idVec3 &translation ) { if ( !body2 ) { anchor2 += translation; } } /* ================ idAFConstraint_Plane::Rotate ================ */ void idAFConstraint_Plane::Rotate( const idRotation &rotation ) { if ( !body2 ) { anchor2 *= rotation; planeNormal *= rotation.ToMat3(); } } /* ================ idAFConstraint_Plane::DebugDraw ================ */ void idAFConstraint_Plane::DebugDraw( void ) { idVec3 a1, normal, right, up; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); if ( master ) { normal = planeNormal * master->GetWorldAxis(); } else { normal = planeNormal; } normal.NormalVectors( right, up ); normal *= 4.0f; right *= 4.0f; up *= 4.0f; gameRenderWorld->DebugLine( colorCyan, a1 - right, a1 + right ); gameRenderWorld->DebugLine( colorCyan, a1 - up, a1 + up ); gameRenderWorld->DebugArrow( colorCyan, a1, a1 + normal, 1 ); } /* ================ idAFConstraint_Plane::Save ================ */ void idAFConstraint_Plane::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( anchor1 ); saveFile->WriteVec3( anchor2 ); saveFile->WriteVec3( planeNormal ); } /* ================ idAFConstraint_Plane::Restore ================ */ void idAFConstraint_Plane::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( anchor1 ); saveFile->ReadVec3( anchor2 ); saveFile->ReadVec3( planeNormal ); } //=============================================================== // // idAFConstraint_Spring // //=============================================================== /* ================ idAFConstraint_Spring::idAFConstraint_Spring ================ */ idAFConstraint_Spring::idAFConstraint_Spring( const idStr &name, idAFBody *body1, idAFBody *body2 ) { assert( body1 ); type = CONSTRAINT_SPRING; this->name = name; this->body1 = body1; this->body2 = body2; InitSize( 1 ); fl.allowPrimary = false; kstretch = kcompress = damping = 1.0f; minLength = maxLength = restLength = 0.0f; } /* ================ idAFConstraint_Spring::SetAnchor ================ */ void idAFConstraint_Spring::SetAnchor( const idVec3 &worldAnchor1, const idVec3 &worldAnchor2 ) { // get anchor relative to center of mass of body1 anchor1 = ( worldAnchor1 - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); if ( body2 ) { // get anchor relative to center of mass of body2 anchor2 = ( worldAnchor2 - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldAnchor2; } } /* ================ idAFConstraint_Spring::SetSpring ================ */ void idAFConstraint_Spring::SetSpring( const float stretch, const float compress, const float damping, const float restLength ) { assert( stretch >= 0.0f && compress >= 0.0f && restLength >= 0.0f ); this->kstretch = stretch; this->kcompress = compress; this->damping = damping; this->restLength = restLength; } /* ================ idAFConstraint_Spring::SetLimit ================ */ void idAFConstraint_Spring::SetLimit( const float minLength, const float maxLength ) { assert( minLength >= 0.0f && maxLength >= 0.0f && maxLength >= minLength ); this->minLength = minLength; this->maxLength = maxLength; } /* ================ idAFConstraint_Spring::Evaluate ================ */ void idAFConstraint_Spring::Evaluate( float invTimeStep ) { idVec3 a1, a2, velocity1, velocity2, force; idVec6 v1, v2; float d, dampingForce, length, error; bool limit; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); velocity1 = body1->GetPointVelocity( a1 ); if ( master ) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); velocity2 = master->GetPointVelocity( a2 ); } else { a2 = anchor2; velocity2.Zero(); } force = a2 - a1; d = force * force; if ( d != 0.0f ) { dampingForce = damping * idMath::Fabs( (velocity2 - velocity1) * force ) / d; } else { dampingForce = 0.0f; } length = force.Normalize(); if ( length > restLength ) { if ( kstretch > 0.0f ) { idVec3 springForce = force * ( Square( length - restLength ) * kstretch - dampingForce ); body1->AddForce( a1, springForce ); if ( master ) { master->AddForce( a2, -springForce ); } } } else { if ( kcompress > 0.0f ) { idVec3 springForce = force * -( Square( restLength - length ) * kcompress - dampingForce ); body1->AddForce( a1, springForce ); if ( master ) { master->AddForce( a2, -springForce ); } } } // check for spring limits if ( length < minLength ) { force = -force; error = minLength - length; limit = true; } else if ( maxLength > 0.0f && length > maxLength ) { error = length - maxLength; limit = true; } else { error = 0.0f; limit = false; } if ( limit ) { a1 -= body1->GetWorldOrigin(); v1.SubVec3(0) = force; v1.SubVec3(1) = a1.Cross( force ); J1.Set( 1, 6, v1.ToFloatPtr() ); if ( body2 ) { a2 -= body2->GetWorldOrigin(); v2.SubVec3(0) = -force; v2.SubVec3(1) = a2.Cross( -force ); J2.Set( 1, 6, v2.ToFloatPtr() ); } c1[0] = -( invTimeStep * ERROR_REDUCTION ) * error; lo[0] = 0.0f; } else { J1.Zero( 0, 0 ); J2.Zero( 0, 0 ); } c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); } /* ================ idAFConstraint_Spring::ApplyFriction ================ */ void idAFConstraint_Spring::ApplyFriction( float invTimeStep ) { // no friction } /* ================ idAFConstraint_Spring::Translate ================ */ void idAFConstraint_Spring::Translate( const idVec3 &translation ) { if ( !body2 ) { anchor2 += translation; } } /* ================ idAFConstraint_Spring::Rotate ================ */ void idAFConstraint_Spring::Rotate( const idRotation &rotation ) { if ( !body2 ) { anchor2 *= rotation; } } /* ================ idAFConstraint_Spring::GetCenter ================ */ void idAFConstraint_Spring::GetCenter( idVec3 ¢er ) { idAFBody *master; idVec3 a1, a2; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); if ( master ) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); } else { a2 = anchor2; } center = ( a1 + a2 ) * 0.5f; } /* ================ idAFConstraint_Spring::DebugDraw ================ */ void idAFConstraint_Spring::DebugDraw( void ) { idAFBody *master; float length; idVec3 a1, a2, dir, mid, p; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); if ( master ) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); } else { a2 = anchor2; } dir = a2 - a1; mid = a1 + 0.5f * dir; length = dir.Normalize(); // draw spring gameRenderWorld->DebugLine( colorGreen, a1, a2 ); // draw rest length p = restLength * 0.5f * dir; gameRenderWorld->DebugCircle( colorWhite, mid + p, dir, 1.0f, 10 ); gameRenderWorld->DebugCircle( colorWhite, mid - p, dir, 1.0f, 10 ); if ( restLength > length ) { gameRenderWorld->DebugLine( colorWhite, a2, mid + p ); gameRenderWorld->DebugLine( colorWhite, a1, mid - p ); } if ( minLength > 0.0f ) { // draw min length gameRenderWorld->DebugCircle( colorBlue, mid + minLength * 0.5f * dir, dir, 2.0f, 10 ); gameRenderWorld->DebugCircle( colorBlue, mid - minLength * 0.5f * dir, dir, 2.0f, 10 ); } if ( maxLength > 0.0f ) { // draw max length gameRenderWorld->DebugCircle( colorRed, mid + maxLength * 0.5f * dir, dir, 2.0f, 10 ); gameRenderWorld->DebugCircle( colorRed, mid - maxLength * 0.5f * dir, dir, 2.0f, 10 ); } } /* ================ idAFConstraint_Spring::Save ================ */ void idAFConstraint_Spring::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( anchor1 ); saveFile->WriteVec3( anchor2 ); saveFile->WriteFloat( kstretch ); saveFile->WriteFloat( kcompress ); saveFile->WriteFloat( damping ); saveFile->WriteFloat( restLength ); saveFile->WriteFloat( minLength ); saveFile->WriteFloat( maxLength ); } /* ================ idAFConstraint_Spring::Restore ================ */ void idAFConstraint_Spring::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( anchor1 ); saveFile->ReadVec3( anchor2 ); saveFile->ReadFloat( kstretch ); saveFile->ReadFloat( kcompress ); saveFile->ReadFloat( damping ); saveFile->ReadFloat( restLength ); saveFile->ReadFloat( minLength ); saveFile->ReadFloat( maxLength ); } //=============================================================== // // idAFConstraint_Contact // //=============================================================== /* ================ idAFConstraint_Contact::idAFConstraint_Contact ================ */ idAFConstraint_Contact::idAFConstraint_Contact( void ) { name = "contact"; type = CONSTRAINT_CONTACT; InitSize( 1 ); fc = NULL; fl.allowPrimary = false; fl.frameConstraint = true; } /* ================ idAFConstraint_Contact::~idAFConstraint_Contact ================ */ idAFConstraint_Contact::~idAFConstraint_Contact( void ) { if ( fc ) { delete fc; } } /* ================ idAFConstraint_Contact::Setup ================ */ void idAFConstraint_Contact::Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c ) { idVec3 p; idVec6 v; float vel; float minBounceVelocity = 2.0f; assert( b1 ); body1 = b1; body2 = b2; contact = c; p = c.point - body1->GetWorldOrigin(); v.SubVec3(0) = c.normal; v.SubVec3(1) = p.Cross( c.normal ); J1.Set( 1, 6, v.ToFloatPtr() ); vel = v.SubVec3(0) * body1->GetLinearVelocity() + v.SubVec3(1) * body1->GetAngularVelocity(); if ( body2 ) { p = c.point - body2->GetWorldOrigin(); v.SubVec3(0) = -c.normal; v.SubVec3(1) = p.Cross( -c.normal ); J2.Set( 1, 6, v.ToFloatPtr() ); vel += v.SubVec3(0) * body2->GetLinearVelocity() + v.SubVec3(1) * body2->GetAngularVelocity(); c2[0] = 0.0f; } if ( body1->GetBouncyness() > 0.0f && -vel > minBounceVelocity ) { c1[0] = body1->GetBouncyness() * vel; } else { c1[0] = 0.0f; } e[0] = CONTACT_LCP_EPSILON; lo[0] = 0.0f; hi[0] = idMath::INFINITY; boxConstraint = NULL; boxIndex[0] = -1; } /* ================ idAFConstraint_Contact::Evaluate ================ */ void idAFConstraint_Contact::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_Contact::ApplyFriction ================ */ void idAFConstraint_Contact::ApplyFriction( float invTimeStep ) { idVec3 r, velocity, normal, dir1, dir2; float friction, magnitude, forceNumerator, forceDenominator; idVecX impulse, dv; friction = body1->GetContactFriction(); if ( body2 && body2->GetContactFriction() < friction ) { friction = body2->GetContactFriction(); } friction *= physics->GetContactFrictionScale(); if ( friction <= 0.0f ) { return; } // seperate friction per contact is silly but it's fast and often looks close enough if ( af_useImpulseFriction.GetBool() ) { impulse.SetData( 6, VECX_ALLOCA( 6 ) ); dv.SetData( 6, VECX_ALLOCA( 6 ) ); // calculate velocity in the contact plane r = contact.point - body1->GetWorldOrigin(); velocity = body1->GetLinearVelocity() + body1->GetAngularVelocity().Cross( r ); velocity -= contact.normal * velocity * contact.normal; // get normalized direction of friction and magnitude of velocity normal = -velocity; magnitude = normal.Normalize(); forceNumerator = friction * magnitude; forceDenominator = body1->GetInverseMass() + ( ( body1->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal ); impulse.SubVec3(0) = (forceNumerator / forceDenominator) * normal; impulse.SubVec3(1) = r.Cross( impulse.SubVec3(0) ); body1->InverseWorldSpatialInertiaMultiply( dv, impulse.ToFloatPtr() ); // modify velocity with friction force body1->SetLinearVelocity( body1->GetLinearVelocity() + dv.SubVec3(0) ); body1->SetAngularVelocity( body1->GetAngularVelocity() + dv.SubVec3(1) ); } else { if ( !fc ) { fc = new idAFConstraint_ContactFriction; } // call setup each frame because contact constraints are re-used for different bodies fc->Setup( this ); fc->Add( physics, invTimeStep ); } } /* ================ idAFConstraint_Contact::Translate ================ */ void idAFConstraint_Contact::Translate( const idVec3 &translation ) { assert( 0 ); // contact should never be translated } /* ================ idAFConstraint_Contact::Rotate ================ */ void idAFConstraint_Contact::Rotate( const idRotation &rotation ) { assert( 0 ); // contact should never be rotated } /* ================ idAFConstraint_Contact::GetCenter ================ */ void idAFConstraint_Contact::GetCenter( idVec3 ¢er ) { center = contact.point; } /* ================ idAFConstraint_Contact::DebugDraw ================ */ void idAFConstraint_Contact::DebugDraw( void ) { idVec3 x, y; contact.normal.NormalVectors( x, y ); gameRenderWorld->DebugLine( colorWhite, contact.point, contact.point + 6.0f * contact.normal ); gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * x, contact.point + 2.0f * x ); gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * y, contact.point + 2.0f * y ); } //=============================================================== // // idAFConstraint_ContactFriction // //=============================================================== /* ================ idAFConstraint_ContactFriction::idAFConstraint_ContactFriction ================ */ idAFConstraint_ContactFriction::idAFConstraint_ContactFriction( void ) { type = CONSTRAINT_FRICTION; name = "contactFriction"; InitSize( 2 ); cc = NULL; fl.allowPrimary = false; fl.frameConstraint = true; } /* ================ idAFConstraint_ContactFriction::Setup ================ */ void idAFConstraint_ContactFriction::Setup( idAFConstraint_Contact *cc ) { this->cc = cc; body1 = cc->GetBody1(); body2 = cc->GetBody2(); } /* ================ idAFConstraint_ContactFriction::Evaluate ================ */ void idAFConstraint_ContactFriction::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_ContactFriction::ApplyFriction ================ */ void idAFConstraint_ContactFriction::ApplyFriction( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_ContactFriction::Add ================ */ bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep ) { idVec3 r, dir1, dir2; float friction; int newRow; physics = phys; friction = body1->GetContactFriction() * physics->GetContactFrictionScale(); // if the body only has friction in one direction if ( body1->GetFrictionDirection( dir1 ) ) { // project the friction direction into the contact plane dir1 -= dir1 * cc->GetContact().normal * dir1; dir1.Normalize(); r = cc->GetContact().point - body1->GetWorldOrigin(); J1.SetSize( 1, 6 ); J1.SubVec6(0).SubVec3(0) = dir1; J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 ); c1.SetSize( 1 ); c1[0] = 0.0f; if ( body2 ) { r = cc->GetContact().point - body2->GetWorldOrigin(); J2.SetSize( 1, 6 ); J2.SubVec6(0).SubVec3(0) = -dir1; J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 ); c2.SetSize( 1 ); c2[0] = 0.0f; } lo[0] = -friction; hi[0] = friction; boxConstraint = cc; boxIndex[0] = 0; } else { // get two friction directions orthogonal to contact normal cc->GetContact().normal.NormalVectors( dir1, dir2 ); r = cc->GetContact().point - body1->GetWorldOrigin(); J1.SetSize( 2, 6 ); J1.SubVec6(0).SubVec3(0) = dir1; J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 ); J1.SubVec6(1).SubVec3(0) = dir2; J1.SubVec6(1).SubVec3(1) = r.Cross( dir2 ); c1.SetSize( 2 ); c1[0] = c1[1] = 0.0f; if ( body2 ) { r = cc->GetContact().point - body2->GetWorldOrigin(); J2.SetSize( 2, 6 ); J2.SubVec6(0).SubVec3(0) = -dir1; J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 ); J2.SubVec6(1).SubVec3(0) = -dir2; J2.SubVec6(1).SubVec3(1) = r.Cross( -dir2 ); c2.SetSize( 2 ); c2[0] = c2[1] = 0.0f; if ( body2->GetContactFriction() < friction ) { friction = body2->GetContactFriction(); } } lo[0] = -friction; hi[0] = friction; boxConstraint = cc; boxIndex[0] = 0; lo[1] = -friction; hi[1] = friction; boxIndex[1] = 0; } if ( body1->GetContactMotorDirection( dir1 ) && body1->GetContactMotorForce() > 0.0f ) { // project the motor force direction into the contact plane dir1 -= dir1 * cc->GetContact().normal * dir1; dir1.Normalize(); r = cc->GetContact().point - body1->GetWorldOrigin(); newRow = J1.GetNumRows(); J1.ChangeSize( newRow+1, J1.GetNumColumns() ); J1.SubVec6(newRow).SubVec3(0) = -dir1; J1.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 ); c1.ChangeSize( newRow+1 ); c1[newRow] = body1->GetContactMotorVelocity(); if ( body2 ) { r = cc->GetContact().point - body2->GetWorldOrigin(); J2.ChangeSize( newRow+1, J2.GetNumColumns() ); J2.SubVec6(newRow).SubVec3(0) = -dir1; J2.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 ); c2.ChangeSize( newRow+1 ); c2[newRow] = 0.0f; } lo[newRow] = -body1->GetContactMotorForce(); hi[newRow] = body1->GetContactMotorForce(); boxIndex[newRow] = -1; } physics->AddFrameConstraint( this ); return true; } /* ================ idAFConstraint_ContactFriction::Translate ================ */ void idAFConstraint_ContactFriction::Translate( const idVec3 &translation ) { } /* ================ idAFConstraint_ContactFriction::Rotate ================ */ void idAFConstraint_ContactFriction::Rotate( const idRotation &rotation ) { } /* ================ idAFConstraint_ContactFriction::DebugDraw ================ */ void idAFConstraint_ContactFriction::DebugDraw( void ) { } //=============================================================== // // idAFConstraint_ConeLimit // //=============================================================== /* ================ idAFConstraint_ConeLimit::idAFConstraint_ConeLimit ================ */ idAFConstraint_ConeLimit::idAFConstraint_ConeLimit( void ) { type = CONSTRAINT_CONELIMIT; name = "coneLimit"; InitSize( 1 ); fl.allowPrimary = false; fl.frameConstraint = true; } /* ================ idAFConstraint_ConeLimit::Setup the coneAnchor is the top of the cone in body2 space the coneAxis is the axis of the cone in body2 space the coneAngle is the angle the cone hull makes at the top the body1Axis is the axis in body1 space that should stay within the cone ================ */ void idAFConstraint_ConeLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) { this->body1 = b1; this->body2 = b2; this->coneAxis = coneAxis; this->coneAxis.Normalize(); this->coneAnchor = coneAnchor; this->body1Axis = body1Axis; this->body1Axis.Normalize(); this->cosAngle = (float) cos( DEG2RAD( coneAngle * 0.5f ) ); this->sinHalfAngle = (float) sin( DEG2RAD( coneAngle * 0.25f ) ); this->cosHalfAngle = (float) cos( DEG2RAD( coneAngle * 0.25f ) ); } /* ================ idAFConstraint_ConeLimit::SetAnchor ================ */ void idAFConstraint_ConeLimit::SetAnchor( const idVec3 &coneAnchor ) { this->coneAnchor = coneAnchor; } /* ================ idAFConstraint_ConeLimit::SetBody1Axis ================ */ void idAFConstraint_ConeLimit::SetBody1Axis( const idVec3 &body1Axis ) { this->body1Axis = body1Axis; } /* ================ idAFConstraint_ConeLimit::Evaluate ================ */ void idAFConstraint_ConeLimit::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_ConeLimit::ApplyFriction ================ */ void idAFConstraint_ConeLimit::ApplyFriction( float invTimeStep ) { } /* ================ idAFConstraint_ConeLimit::Add ================ */ bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) { float a; idVec6 J1row, J2row; idVec3 ax, anchor, body1ax, normal, coneVector, p1, p2; idQuat q; idAFBody *master; if ( af_skipLimits.GetBool() ) { lm.Zero(); // constraint exerts no force return false; } physics = phys; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { ax = coneAxis * master->GetWorldAxis(); anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis(); } else { ax = coneAxis; anchor = coneAnchor; } body1ax = body1Axis * body1->GetWorldAxis(); a = ax * body1ax; // if the body1 axis is inside the cone if ( a > cosAngle ) { lm.Zero(); // constraint exerts no force return false; } // calculate the inward cone normal for the position the body1 axis went outside the cone normal = body1ax.Cross( ax ); normal.Normalize(); q.x = normal.x * sinHalfAngle; q.y = normal.y * sinHalfAngle; q.z = normal.z * sinHalfAngle; q.w = cosHalfAngle; coneVector = ax * q.ToMat3(); normal = coneVector.Cross( ax ).Cross( coneVector ); normal.Normalize(); p1 = anchor + 32.0f * coneVector - body1->GetWorldOrigin(); J1row.SubVec3(0) = normal; J1row.SubVec3(1) = p1.Cross( normal ); J1.Set( 1, 6, J1row.ToFloatPtr() ); c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) ); if ( body2 ) { p2 = anchor + 32.0f * coneVector - master->GetWorldOrigin(); J2row.SubVec3(0) = -normal; J2row.SubVec3(1) = p2.Cross( -normal ); J2.Set( 1, 6, J2row.ToFloatPtr() ); c2[0] = 0.0f; } lo[0] = 0.0f; e[0] = LIMIT_LCP_EPSILON; physics->AddFrameConstraint( this ); return true; } /* ================ idAFConstraint_ConeLimit::Translate ================ */ void idAFConstraint_ConeLimit::Translate( const idVec3 &translation ) { if ( !body2 ) { coneAnchor += translation; } } /* ================ idAFConstraint_ConeLimit::Rotate ================ */ void idAFConstraint_ConeLimit::Rotate( const idRotation &rotation ) { if ( !body2 ) { coneAnchor *= rotation; coneAxis *= rotation.ToMat3(); } } /* ================ idAFConstraint_ConeLimit::DebugDraw ================ */ void idAFConstraint_ConeLimit::DebugDraw( void ) { idVec3 ax, anchor, x, y, z, start, end; float sinAngle, a, size = 10.0f; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { ax = coneAxis * master->GetWorldAxis(); anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis(); } else { ax = coneAxis; anchor = coneAnchor; } // draw body1 axis gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) ); // draw cone ax.NormalVectors( x, y ); sinAngle = idMath::Sqrt( 1.0f - cosAngle * cosAngle ); x *= size * sinAngle; y *= size * sinAngle; z = anchor + ax * size * cosAngle; start = x + z; for ( a = 0.0f; a < 360.0f; a += 45.0f ) { end = x * (float) cos( DEG2RAD(a + 45.0f) ) + y * (float) sin( DEG2RAD(a + 45.0f) ) + z; gameRenderWorld->DebugLine( colorMagenta, anchor, start ); gameRenderWorld->DebugLine( colorMagenta, start, end ); start = end; } } /* ================ idAFConstraint_ConeLimit::Save ================ */ void idAFConstraint_ConeLimit::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( coneAnchor ); saveFile->WriteVec3( coneAxis ); saveFile->WriteVec3( body1Axis ); saveFile->WriteFloat( cosAngle ); saveFile->WriteFloat( sinHalfAngle ); saveFile->WriteFloat( cosHalfAngle ); saveFile->WriteFloat( epsilon ); } /* ================ idAFConstraint_ConeLimit::Restore ================ */ void idAFConstraint_ConeLimit::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( coneAnchor ); saveFile->ReadVec3( coneAxis ); saveFile->ReadVec3( body1Axis ); saveFile->ReadFloat( cosAngle ); saveFile->ReadFloat( sinHalfAngle ); saveFile->ReadFloat( cosHalfAngle ); saveFile->ReadFloat( epsilon ); } //=============================================================== // // idAFConstraint_PyramidLimit // //=============================================================== /* ================ idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit ================ */ idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit( void ) { type = CONSTRAINT_PYRAMIDLIMIT; name = "pyramidLimit"; InitSize( 1 ); fl.allowPrimary = false; fl.frameConstraint = true; } /* ================ idAFConstraint_PyramidLimit::Setup ================ */ void idAFConstraint_PyramidLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor, const idVec3 &pyramidAxis, const idVec3 &baseAxis, const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis ) { body1 = b1; body2 = b2; // setup the base and make sure the basis is orthonormal pyramidBasis[2] = pyramidAxis; pyramidBasis[2].Normalize(); pyramidBasis[0] = baseAxis; pyramidBasis[0] -= pyramidBasis[2] * baseAxis * pyramidBasis[2]; pyramidBasis[0].Normalize(); pyramidBasis[1] = pyramidBasis[0].Cross( pyramidBasis[2] ); // pyramid top this->pyramidAnchor = pyramidAnchor; // angles cosAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.5f ) ); cosAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.5f ) ); sinHalfAngle[0] = (float) sin( DEG2RAD( pyramidAngle1 * 0.25f ) ); sinHalfAngle[1] = (float) sin( DEG2RAD( pyramidAngle2 * 0.25f ) ); cosHalfAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.25f ) ); cosHalfAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.25f ) ); this->body1Axis = body1Axis; } /* ================ idAFConstraint_PyramidLimit::SetAnchor ================ */ void idAFConstraint_PyramidLimit::SetAnchor( const idVec3 &pyramidAnchor ) { this->pyramidAnchor = pyramidAnchor; } /* ================ idAFConstraint_PyramidLimit::SetBody1Axis ================ */ void idAFConstraint_PyramidLimit::SetBody1Axis( const idVec3 &body1Axis ) { this->body1Axis = body1Axis; } /* ================ idAFConstraint_PyramidLimit::Evaluate ================ */ void idAFConstraint_PyramidLimit::Evaluate( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_PyramidLimit::ApplyFriction ================ */ void idAFConstraint_PyramidLimit::ApplyFriction( float invTimeStep ) { } /* ================ idAFConstraint_PyramidLimit::Add ================ */ bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) { int i; float a[2]; idVec6 J1row, J2row; idMat3 worldBase; idVec3 anchor, body1ax, ax[2], v, normal, pyramidVector, p1, p2; idQuat q; idAFBody *master; if ( af_skipLimits.GetBool() ) { lm.Zero(); // constraint exerts no force return false; } physics = phys; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { worldBase[0] = pyramidBasis[0] * master->GetWorldAxis(); worldBase[1] = pyramidBasis[1] * master->GetWorldAxis(); worldBase[2] = pyramidBasis[2] * master->GetWorldAxis(); anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis(); } else { worldBase = pyramidBasis; anchor = pyramidAnchor; } body1ax = body1Axis * body1->GetWorldAxis(); for ( i = 0; i < 2; i++ ) { ax[i] = body1ax - worldBase[!i] * body1ax * worldBase[!i]; ax[i].Normalize(); a[i] = worldBase[2] * ax[i]; } // if the body1 axis is inside the pyramid if ( a[0] > cosAngle[0] && a[1] > cosAngle[1] ) { lm.Zero(); // constraint exerts no force return false; } // calculate the inward pyramid normal for the position the body1 axis went outside the pyramid pyramidVector = worldBase[2]; for ( i = 0; i < 2; i++ ) { if ( a[i] <= cosAngle[i] ) { v = ax[i].Cross( worldBase[2] ); v.Normalize(); q.x = v.x * sinHalfAngle[i]; q.y = v.y * sinHalfAngle[i]; q.z = v.z * sinHalfAngle[i]; q.w = cosHalfAngle[i]; pyramidVector *= q.ToMat3(); } } normal = pyramidVector.Cross( worldBase[2] ).Cross( pyramidVector ); normal.Normalize(); p1 = anchor + 32.0f * pyramidVector - body1->GetWorldOrigin(); J1row.SubVec3(0) = normal; J1row.SubVec3(1) = p1.Cross( normal ); J1.Set( 1, 6, J1row.ToFloatPtr() ); c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) ); if ( body2 ) { p2 = anchor + 32.0f * pyramidVector - master->GetWorldOrigin(); J2row.SubVec3(0) = -normal; J2row.SubVec3(1) = p2.Cross( -normal ); J2.Set( 1, 6, J2row.ToFloatPtr() ); c2[0] = 0.0f; } lo[0] = 0.0f; e[0] = LIMIT_LCP_EPSILON; physics->AddFrameConstraint( this ); return true; } /* ================ idAFConstraint_PyramidLimit::Translate ================ */ void idAFConstraint_PyramidLimit::Translate( const idVec3 &translation ) { if ( !body2 ) { pyramidAnchor += translation; } } /* ================ idAFConstraint_PyramidLimit::Rotate ================ */ void idAFConstraint_PyramidLimit::Rotate( const idRotation &rotation ) { if ( !body2 ) { pyramidAnchor *= rotation; pyramidBasis[0] *= rotation.ToMat3(); pyramidBasis[1] *= rotation.ToMat3(); pyramidBasis[2] *= rotation.ToMat3(); } } /* ================ idAFConstraint_PyramidLimit::DebugDraw ================ */ void idAFConstraint_PyramidLimit::DebugDraw( void ) { int i; float size = 10.0f; idVec3 anchor, dir, p[4]; idMat3 worldBase, m[2]; idQuat q; idAFBody *master; master = body2 ? body2 : physics->GetMasterBody(); if ( master ) { worldBase[0] = pyramidBasis[0] * master->GetWorldAxis(); worldBase[1] = pyramidBasis[1] * master->GetWorldAxis(); worldBase[2] = pyramidBasis[2] * master->GetWorldAxis(); anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis(); } else { worldBase = pyramidBasis; anchor = pyramidAnchor; } // draw body1 axis gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) ); // draw the pyramid for ( i = 0; i < 2; i++ ) { q.x = worldBase[!i].x * sinHalfAngle[i]; q.y = worldBase[!i].y * sinHalfAngle[i]; q.z = worldBase[!i].z * sinHalfAngle[i]; q.w = cosHalfAngle[i]; m[i] = q.ToMat3(); } dir = worldBase[2] * size; p[0] = anchor + m[0] * (m[1] * dir); p[1] = anchor + m[0] * (m[1].Transpose() * dir); p[2] = anchor + m[0].Transpose() * (m[1].Transpose() * dir); p[3] = anchor + m[0].Transpose() * (m[1] * dir); for ( i = 0; i < 4; i++ ) { gameRenderWorld->DebugLine( colorMagenta, anchor, p[i] ); gameRenderWorld->DebugLine( colorMagenta, p[i], p[(i+1)&3] ); } } /* ================ idAFConstraint_PyramidLimit::Save ================ */ void idAFConstraint_PyramidLimit::Save( idSaveGame *saveFile ) const { idAFConstraint::Save( saveFile ); saveFile->WriteVec3( pyramidAnchor ); saveFile->WriteMat3( pyramidBasis ); saveFile->WriteVec3( body1Axis ); saveFile->WriteFloat( cosAngle[0] ); saveFile->WriteFloat( cosAngle[1] ); saveFile->WriteFloat( sinHalfAngle[0] ); saveFile->WriteFloat( sinHalfAngle[1] ); saveFile->WriteFloat( cosHalfAngle[0] ); saveFile->WriteFloat( cosHalfAngle[1] ); saveFile->WriteFloat( epsilon ); } /* ================ idAFConstraint_PyramidLimit::Restore ================ */ void idAFConstraint_PyramidLimit::Restore( idRestoreGame *saveFile ) { idAFConstraint::Restore( saveFile ); saveFile->ReadVec3( pyramidAnchor ); saveFile->ReadMat3( pyramidBasis ); saveFile->ReadVec3( body1Axis ); saveFile->ReadFloat( cosAngle[0] ); saveFile->ReadFloat( cosAngle[1] ); saveFile->ReadFloat( sinHalfAngle[0] ); saveFile->ReadFloat( sinHalfAngle[1] ); saveFile->ReadFloat( cosHalfAngle[0] ); saveFile->ReadFloat( cosHalfAngle[1] ); saveFile->ReadFloat( epsilon ); } //=============================================================== // // idAFConstraint_Suspension // //=============================================================== /* ================ idAFConstraint_Suspension::idAFConstraint_Suspension ================ */ idAFConstraint_Suspension::idAFConstraint_Suspension( void ) { type = CONSTRAINT_SUSPENSION; name = "suspension"; InitSize( 3 ); fl.allowPrimary = false; fl.frameConstraint = true; localOrigin.Zero(); localAxis.Identity(); suspensionUp = 0.0f; suspensionDown = 0.0f; suspensionKCompress = 0.0f; suspensionDamping = 0.0f; steerAngle = 0.0f; friction = 2.0f; motorEnabled = false; motorForce = 0.0f; motorVelocity = 0.0f; wheelModel = NULL; memset( &trace, 0, sizeof( trace ) ); epsilon = LCP_EPSILON; } /* ================ idAFConstraint_Suspension::Setup ================ */ void idAFConstraint_Suspension::Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel ) { this->name = name; body1 = body; body2 = NULL; localOrigin = ( origin - body->GetWorldOrigin() ) * body->GetWorldAxis().Transpose(); localAxis = axis * body->GetWorldAxis().Transpose(); wheelModel = clipModel; } /* ================ idAFConstraint_Suspension::SetSuspension ================ */ void idAFConstraint_Suspension::SetSuspension( const float up, const float down, const float k, const float d, const float f ) { suspensionUp = up; suspensionDown = down; suspensionKCompress = k; suspensionDamping = d; friction = f; } /* ================ idAFConstraint_Suspension::GetWheelOrigin ================ */ const idVec3 idAFConstraint_Suspension::GetWheelOrigin( void ) const { return body1->GetWorldOrigin() + wheelOffset * body1->GetWorldAxis(); } /* ================ idAFConstraint_Suspension::Evaluate ================ */ void idAFConstraint_Suspension::Evaluate( float invTimeStep ) { float velocity, suspensionLength, springLength, compression, dampingForce, springForce; idVec3 origin, start, end, vel1, vel2, springDir, r, frictionDir, motorDir; idMat3 axis; idRotation rotation; axis = localAxis * body1->GetWorldAxis(); origin = body1->GetWorldOrigin() + localOrigin * body1->GetWorldAxis(); start = origin + suspensionUp * axis[2]; end = origin - suspensionDown * axis[2]; rotation.SetVec( axis[2] ); rotation.SetAngle( steerAngle ); axis *= rotation.ToMat3(); gameLocal.clip.Translation( trace, start, end, wheelModel, axis, MASK_SOLID, NULL ); wheelOffset = ( trace.endpos - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); if ( trace.fraction >= 1.0f ) { J1.SetSize( 0, 6 ); if ( body2 ) { J2.SetSize( 0, 6 ); } return; } // calculate and add spring force vel1 = body1->GetPointVelocity( start ); if ( body2 ) { vel2 = body2->GetPointVelocity( trace.c.point ); } else { vel2.Zero(); } suspensionLength = suspensionUp + suspensionDown; springDir = trace.endpos - start; springLength = trace.fraction * suspensionLength; dampingForce = suspensionDamping * idMath::Fabs( ( vel2 - vel1 ) * springDir ) / ( 1.0f + springLength * springLength ); compression = suspensionLength - springLength; springForce = compression * compression * suspensionKCompress - dampingForce; r = trace.c.point - body1->GetWorldOrigin(); J1.SetSize( 2, 6 ); J1.SubVec6(0).SubVec3(0) = trace.c.normal; J1.SubVec6(0).SubVec3(1) = r.Cross( trace.c.normal ); c1.SetSize( 2 ); c1[0] = 0.0f; velocity = J1.SubVec6(0).SubVec3(0) * body1->GetLinearVelocity() + J1.SubVec6(0).SubVec3(1) * body1->GetAngularVelocity(); if ( body2 ) { r = trace.c.point - body2->GetWorldOrigin(); J2.SetSize( 2, 6 ); J2.SubVec6(0).SubVec3(0) = -trace.c.normal; J2.SubVec6(0).SubVec3(1) = r.Cross( -trace.c.normal ); c2.SetSize( 2 ); c2[0] = 0.0f; velocity += J2.SubVec6(0).SubVec3(0) * body2->GetLinearVelocity() + J2.SubVec6(0).SubVec3(1) * body2->GetAngularVelocity(); } c1[0] = -compression; // + 0.5f * -velocity; e[0] = 1e-4f; lo[0] = 0.0f; hi[0] = springForce; boxConstraint = NULL; boxIndex[0] = -1; // project the friction direction into the contact plane frictionDir = axis[1] - axis[1] * trace.c.normal * axis[1]; frictionDir.Normalize(); r = trace.c.point - body1->GetWorldOrigin(); J1.SubVec6(1).SubVec3(0) = frictionDir; J1.SubVec6(1).SubVec3(1) = r.Cross( frictionDir ); c1[1] = 0.0f; if ( body2 ) { r = trace.c.point - body2->GetWorldOrigin(); J2.SubVec6(1).SubVec3(0) = -frictionDir; J2.SubVec6(1).SubVec3(1) = r.Cross( -frictionDir ); c2[1] = 0.0f; } lo[1] = -friction * physics->GetContactFrictionScale(); hi[1] = friction * physics->GetContactFrictionScale(); boxConstraint = this; boxIndex[1] = 0; if ( motorEnabled ) { // project the motor force direction into the contact plane motorDir = axis[0] - axis[0] * trace.c.normal * axis[0]; motorDir.Normalize(); r = trace.c.point - body1->GetWorldOrigin(); J1.ChangeSize( 3, J1.GetNumColumns() ); J1.SubVec6(2).SubVec3(0) = -motorDir; J1.SubVec6(2).SubVec3(1) = r.Cross( -motorDir ); c1.ChangeSize( 3 ); c1[2] = motorVelocity; if ( body2 ) { r = trace.c.point - body2->GetWorldOrigin(); J2.ChangeSize( 3, J2.GetNumColumns() ); J2.SubVec6(2).SubVec3(0) = -motorDir; J2.SubVec6(2).SubVec3(1) = r.Cross( -motorDir ); c2.ChangeSize( 3 ); c2[2] = 0.0f; } lo[2] = -motorForce; hi[2] = motorForce; boxIndex[2] = -1; } } /* ================ idAFConstraint_Suspension::ApplyFriction ================ */ void idAFConstraint_Suspension::ApplyFriction( float invTimeStep ) { // do nothing } /* ================ idAFConstraint_Suspension::Translate ================ */ void idAFConstraint_Suspension::Translate( const idVec3 &translation ) { } /* ================ idAFConstraint_Suspension::Rotate ================ */ void idAFConstraint_Suspension::Rotate( const idRotation &rotation ) { } /* ================ idAFConstraint_Suspension::DebugDraw ================ */ void idAFConstraint_Suspension::DebugDraw( void ) { idVec3 origin; idMat3 axis; idRotation rotation; axis = localAxis * body1->GetWorldAxis(); rotation.SetVec( axis[2] ); rotation.SetAngle( steerAngle ); axis *= rotation.ToMat3(); if ( trace.fraction < 1.0f ) { origin = trace.c.point; gameRenderWorld->DebugLine( colorWhite, origin, origin + 6.0f * axis[2] ); gameRenderWorld->DebugLine( colorWhite, origin - 4.0f * axis[0], origin + 4.0f * axis[0] ); gameRenderWorld->DebugLine( colorWhite, origin - 2.0f * axis[1], origin + 2.0f * axis[1] ); } } //=============================================================== // // idAFBody // //=============================================================== /* ================ idAFBody::idAFBody ================ */ idAFBody::idAFBody( void ) { Init(); } /* ================ idAFBody::idAFBody ================ */ idAFBody::idAFBody( const idStr &name, idClipModel *clipModel, float density ) { assert( clipModel ); assert( clipModel->IsTraceModel() ); Init(); this->name = name; this->clipModel = NULL; SetClipModel( clipModel ); SetDensity( density ); current->worldOrigin = clipModel->GetOrigin(); current->worldAxis = clipModel->GetAxis(); *next = *current; } /* ================ idAFBody::~idAFBody ================ */ idAFBody::~idAFBody( void ) { delete clipModel; } /* ================ idAFBody::Init ================ */ void idAFBody::Init( void ) { name = "noname"; parent = NULL; clipModel = NULL; primaryConstraint = NULL; tree = NULL; linearFriction = -1.0f; angularFriction = -1.0f; contactFriction = -1.0f; bouncyness = -1.0f; clipMask = 0; frictionDir = vec3_zero; contactMotorDir = vec3_zero; contactMotorVelocity = 0.0f; contactMotorForce = 0.0f; mass = 1.0f; invMass = 1.0f; centerOfMass = vec3_zero; inertiaTensor = mat3_identity; inverseInertiaTensor = mat3_identity; current = &state[0]; next = &state[1]; current->worldOrigin = vec3_zero; current->worldAxis = mat3_identity; current->spatialVelocity = vec6_zero; current->externalForce = vec6_zero; *next = *current; saved = *current; atRestOrigin = vec3_zero; atRestAxis = mat3_identity; s.Zero( 6 ); totalForce.Zero( 6 ); auxForce.Zero( 6 ); acceleration.Zero( 6 ); response = NULL; responseIndex = NULL; numResponses = 0; maxAuxiliaryIndex = 0; maxSubTreeAuxiliaryIndex = 0; memset( &fl, 0, sizeof( fl ) ); fl.selfCollision = true; fl.isZero = true; } /* ================ idAFBody::SetClipModel ================ */ void idAFBody::SetClipModel( idClipModel *clipModel ) { if ( this->clipModel && this->clipModel != clipModel ) { delete this->clipModel; } this->clipModel = clipModel; } /* ================ idAFBody::SetFriction ================ */ void idAFBody::SetFriction( float linear, float angular, float contact ) { if ( linear < 0.0f || linear > 1.0f || angular < 0.0f || angular > 1.0f || contact < 0.0f ) { gameLocal.Warning( "idAFBody::SetFriction: friction out of range, linear = %.1f, angular = %.1f, contact = %.1f", linear, angular, contact ); return; } linearFriction = linear; angularFriction = angular; contactFriction = contact; } /* ================ idAFBody::SetBouncyness ================ */ void idAFBody::SetBouncyness( float bounce ) { if ( bounce < 0.0f || bounce > 1.0f ) { gameLocal.Warning( "idAFBody::SetBouncyness: bouncyness out of range, bounce = %.1f", bounce ); return; } bouncyness = bounce; } /* ================ idAFBody::SetDensity ================ */ void idAFBody::SetDensity( float density, const idMat3 &inertiaScale ) { // get the body mass properties clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor ); // make sure we have a valid mass if ( mass <= 0.0f || FLOAT_IS_NAN( mass ) ) { gameLocal.Warning( "idAFBody::SetDensity: invalid mass for body '%s'", name.c_str() ); mass = 1.0f; centerOfMass.Zero(); inertiaTensor.Identity(); } // make sure the center of mass is at the body origin if ( !centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) { gameLocal.Warning( "idAFBody::SetDentity: center of mass not at origin for body '%s'", name.c_str() ); } centerOfMass.Zero(); // calculate the inverse mass and inverse inertia tensor invMass = 1.0f / mass; if ( inertiaScale != mat3_identity ) { inertiaTensor *= inertiaScale; } if ( inertiaTensor.IsDiagonal( 1e-3f ) ) { inertiaTensor[0][1] = inertiaTensor[0][2] = 0.0f; inertiaTensor[1][0] = inertiaTensor[1][2] = 0.0f; inertiaTensor[2][0] = inertiaTensor[2][1] = 0.0f; inverseInertiaTensor.Identity(); inverseInertiaTensor[0][0] = 1.0f / inertiaTensor[0][0]; inverseInertiaTensor[1][1] = 1.0f / inertiaTensor[1][1]; inverseInertiaTensor[2][2] = 1.0f / inertiaTensor[2][2]; } else { inverseInertiaTensor = inertiaTensor.Inverse(); } } /* ================ idAFBody::SetFrictionDirection ================ */ void idAFBody::SetFrictionDirection( const idVec3 &dir ) { frictionDir = dir * current->worldAxis.Transpose(); fl.useFrictionDir = true; } /* ================ idAFBody::GetFrictionDirection ================ */ bool idAFBody::GetFrictionDirection( idVec3 &dir ) const { if ( fl.useFrictionDir ) { dir = frictionDir * current->worldAxis; return true; } return false; } /* ================ idAFBody::SetContactMotorDirection ================ */ void idAFBody::SetContactMotorDirection( const idVec3 &dir ) { contactMotorDir = dir * current->worldAxis.Transpose(); fl.useContactMotorDir = true; } /* ================ idAFBody::GetContactMotorDirection ================ */ bool idAFBody::GetContactMotorDirection( idVec3 &dir ) const { if ( fl.useContactMotorDir ) { dir = contactMotorDir * current->worldAxis; return true; } return false; } /* ================ idAFBody::GetPointVelocity ================ */ idVec3 idAFBody::GetPointVelocity( const idVec3 &point ) const { idVec3 r = point - current->worldOrigin; return current->spatialVelocity.SubVec3(0) + current->spatialVelocity.SubVec3(1).Cross( r ); } /* ================ idAFBody::AddForce ================ */ void idAFBody::AddForce( const idVec3 &point, const idVec3 &force ) { current->externalForce.SubVec3(0) += force; current->externalForce.SubVec3(1) += (point - current->worldOrigin).Cross( force ); } /* ================ idAFBody::InverseWorldSpatialInertiaMultiply dst = this->inverseWorldSpatialInertia * v; ================ */ ID_INLINE void idAFBody::InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const { const float *mPtr = inverseWorldSpatialInertia.ToFloatPtr(); const float *vPtr = v; float *dstPtr = dst.ToFloatPtr(); if ( fl.spatialInertiaSparse ) { dstPtr[0] = mPtr[0*6+0] * vPtr[0]; dstPtr[1] = mPtr[1*6+1] * vPtr[1]; dstPtr[2] = mPtr[2*6+2] * vPtr[2]; dstPtr[3] = mPtr[3*6+3] * vPtr[3] + mPtr[3*6+4] * vPtr[4] + mPtr[3*6+5] * vPtr[5]; dstPtr[4] = mPtr[4*6+3] * vPtr[3] + mPtr[4*6+4] * vPtr[4] + mPtr[4*6+5] * vPtr[5]; dstPtr[5] = mPtr[5*6+3] * vPtr[3] + mPtr[5*6+4] * vPtr[4] + mPtr[5*6+5] * vPtr[5]; } else { gameLocal.Warning( "spatial inertia is not sparse for body %s", name.c_str() ); } } /* ================ idAFBody::Save ================ */ void idAFBody::Save( idSaveGame *saveFile ) { saveFile->WriteFloat( linearFriction ); saveFile->WriteFloat( angularFriction ); saveFile->WriteFloat( contactFriction ); saveFile->WriteFloat( bouncyness ); saveFile->WriteInt( clipMask ); saveFile->WriteVec3( frictionDir ); saveFile->WriteVec3( contactMotorDir ); saveFile->WriteFloat( contactMotorVelocity ); saveFile->WriteFloat( contactMotorForce ); saveFile->WriteFloat( mass ); saveFile->WriteFloat( invMass ); saveFile->WriteVec3( centerOfMass ); saveFile->WriteMat3( inertiaTensor ); saveFile->WriteMat3( inverseInertiaTensor ); saveFile->WriteVec3( current->worldOrigin ); saveFile->WriteMat3( current->worldAxis ); saveFile->WriteVec6( current->spatialVelocity ); saveFile->WriteVec6( current->externalForce ); saveFile->WriteVec3( atRestOrigin ); saveFile->WriteMat3( atRestAxis ); } /* ================ idAFBody::Restore ================ */ void idAFBody::Restore( idRestoreGame *saveFile ) { saveFile->ReadFloat( linearFriction ); saveFile->ReadFloat( angularFriction ); saveFile->ReadFloat( contactFriction ); saveFile->ReadFloat( bouncyness ); saveFile->ReadInt( clipMask ); saveFile->ReadVec3( frictionDir ); saveFile->ReadVec3( contactMotorDir ); saveFile->ReadFloat( contactMotorVelocity ); saveFile->ReadFloat( contactMotorForce ); saveFile->ReadFloat( mass ); saveFile->ReadFloat( invMass ); saveFile->ReadVec3( centerOfMass ); saveFile->ReadMat3( inertiaTensor ); saveFile->ReadMat3( inverseInertiaTensor ); saveFile->ReadVec3( current->worldOrigin ); saveFile->ReadMat3( current->worldAxis ); saveFile->ReadVec6( current->spatialVelocity ); saveFile->ReadVec6( current->externalForce ); saveFile->ReadVec3( atRestOrigin ); saveFile->ReadMat3( atRestAxis ); } //=============================================================== // M // idAFTree MrE // E //=============================================================== /* ================ idAFTree::Factor factor matrix for the primary constraints in the tree ================ */ void idAFTree::Factor( void ) const { int i, j; idAFBody *body; idAFConstraint *child = NULL; idMatX childI; childI.SetData( 6, 6, MATX_ALLOCA( 6 * 6 ) ); // from the leaves up towards the root for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) { body = sortedBodies[i]; if ( body->children.Num() ) { for ( j = 0; j < body->children.Num(); j++ ) { child = body->children[j]->primaryConstraint; // child->I = - child->body1->J.Transpose() * child->body1->I * child->body1->J; childI.SetSize( child->J1.GetNumRows(), child->J1.GetNumRows() ); child->body1->J.TransposeMultiply( child->body1->I ).Multiply( childI, child->body1->J ); childI.Negate(); child->invI = childI; if ( !child->invI.InverseFastSelf() ) { gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for constraint '%s'", child->invI.GetNumRows(), child->invI.GetNumColumns(), child->GetName().c_str() ); } child->J = child->invI * child->J; body->I -= child->J.TransposeMultiply( childI ) * child->J; } body->invI = body->I; if ( !body->invI.InverseFastSelf() ) { gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for body %s", child->invI.GetNumRows(), child->invI.GetNumColumns(), body->GetName().c_str() ); } if ( body->primaryConstraint ) { body->J = body->invI * body->J; } } else if ( body->primaryConstraint ) { body->J = body->inverseWorldSpatialInertia * body->J; } } } /* ================ idAFTree::Solve solve for primary constraints in the tree ================ */ void idAFTree::Solve( int auxiliaryIndex ) const { int i, j; idAFBody *body, *child; idAFConstraint *primaryConstraint; // from the leaves up towards the root for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) { body = sortedBodies[i]; for ( j = 0; j < body->children.Num(); j++ ) { child = body->children[j]; primaryConstraint = child->primaryConstraint; if ( !child->fl.isZero ) { child->J.TransposeMultiplySub( primaryConstraint->s, child->s ); primaryConstraint->fl.isZero = false; } if ( !primaryConstraint->fl.isZero ) { primaryConstraint->J.TransposeMultiplySub( body->s, primaryConstraint->s ); body->fl.isZero = false; } } } bool useSymmetry = af_useSymmetry.GetBool(); // from the root down towards the leaves for ( i = 0; i < sortedBodies.Num(); i++ ) { body = sortedBodies[i]; primaryConstraint = body->primaryConstraint; if ( primaryConstraint ) { if ( useSymmetry && body->parent->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) { continue; } if ( !primaryConstraint->fl.isZero ) { primaryConstraint->s = primaryConstraint->invI * primaryConstraint->s; } primaryConstraint->J.MultiplySub( primaryConstraint->s, primaryConstraint->body2->s ); primaryConstraint->lm = primaryConstraint->s; if ( useSymmetry && body->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) { continue; } if ( body->children.Num() ) { if ( !body->fl.isZero ) { body->s = body->invI * body->s; } body->J.MultiplySub( body->s, primaryConstraint->s ); } } else if ( body->children.Num() ) { body->s = body->invI * body->s; } } } /* ================ idAFTree::Response calculate body forces in the tree in response to a constraint force ================ */ void idAFTree::Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const { int i, j; idAFBody *body; idAFConstraint *child, *primaryConstraint; idVecX v; // if a single body don't waste time because there aren't any primary constraints if ( sortedBodies.Num() == 1 ) { body = constraint->body1; if ( body->tree == this ) { body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row ); body->responseIndex[body->numResponses++] = auxiliaryIndex; } else { body = constraint->body2; body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row ); body->responseIndex[body->numResponses++] = auxiliaryIndex; } return; } v.SetData( 6, VECX_ALLOCA( 6 ) ); // initialize right hand side to zero for ( i = 0; i < sortedBodies.Num(); i++ ) { body = sortedBodies[i]; primaryConstraint = body->primaryConstraint; if ( primaryConstraint ) { primaryConstraint->s.Zero(); primaryConstraint->fl.isZero = true; } body->s.Zero(); body->fl.isZero = true; body->GetResponseForce( body->numResponses ).Zero(); } // set right hand side for first constrained body body = constraint->body1; if ( body->tree == this ) { body->InverseWorldSpatialInertiaMultiply( v, constraint->J1[row] ); primaryConstraint = body->primaryConstraint; if ( primaryConstraint ) { primaryConstraint->J1.Multiply( primaryConstraint->s, v ); primaryConstraint->fl.isZero = false; } for ( i = 0; i < body->children.Num(); i++ ) { child = body->children[i]->primaryConstraint; child->J2.Multiply( child->s, v ); child->fl.isZero = false; } body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row ); } // set right hand side for second constrained body body = constraint->body2; if ( body && body->tree == this ) { body->InverseWorldSpatialInertiaMultiply( v, constraint->J2[row] ); primaryConstraint = body->primaryConstraint; if ( primaryConstraint ) { primaryConstraint->J1.MultiplyAdd( primaryConstraint->s, v ); primaryConstraint->fl.isZero = false; } for ( i = 0; i < body->children.Num(); i++ ) { child = body->children[i]->primaryConstraint; child->J2.MultiplyAdd( child->s, v ); child->fl.isZero = false; } body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row ); } // solve for primary constraints Solve( auxiliaryIndex ); bool useSymmetry = af_useSymmetry.GetBool(); // store body forces in response to the constraint force idVecX force; for ( i = 0; i < sortedBodies.Num(); i++ ) { body = sortedBodies[i]; if ( useSymmetry && body->maxAuxiliaryIndex < auxiliaryIndex ) { continue; } force.SetData( 6, body->response + body->numResponses * 8 ); // add forces of all primary constraints acting on this body primaryConstraint = body->primaryConstraint; if ( primaryConstraint ) { primaryConstraint->J1.TransposeMultiplyAdd( force, primaryConstraint->lm ); } for ( j = 0; j < body->children.Num(); j++ ) { child = body->children[j]->primaryConstraint; child->J2.TransposeMultiplyAdd( force, child->lm ); } body->responseIndex[body->numResponses++] = auxiliaryIndex; } } /* ================ idAFTree::CalculateForces calculate forces on the bodies in the tree ================ */ void idAFTree::CalculateForces( float timeStep ) const { int i, j; float invStep; idAFBody *body; idAFConstraint *child, *c, *primaryConstraint; // forces on bodies for ( i = 0; i < sortedBodies.Num(); i++ ) { body = sortedBodies[i]; body->totalForce.SubVec6(0) = body->current->externalForce + body->auxForce.SubVec6(0); } // if a single body don't waste time because there aren't any primary constraints if ( sortedBodies.Num() == 1 ) { return; } invStep = 1.0f / timeStep; // initialize right hand side for ( i = 0; i < sortedBodies.Num(); i++ ) { body = sortedBodies[i]; body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() ); body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep; primaryConstraint = body->primaryConstraint; if ( primaryConstraint ) { // b = ( J * acc + c ) c = primaryConstraint; c->s = c->J1 * c->body1->acceleration + c->J2 * c->body2->acceleration + invStep * ( c->c1 + c->c2 ); c->fl.isZero = false; } body->s.Zero(); body->fl.isZero = true; } // solve for primary constraints Solve(); // calculate forces on bodies after applying primary constraints for ( i = 0; i < sortedBodies.Num(); i++ ) { body = sortedBodies[i]; // add forces of all primary constraints acting on this body primaryConstraint = body->primaryConstraint; if ( primaryConstraint ) { primaryConstraint->J1.TransposeMultiplyAdd( body->totalForce, primaryConstraint->lm ); } for ( j = 0; j < body->children.Num(); j++ ) { child = body->children[j]->primaryConstraint; child->J2.TransposeMultiplyAdd( body->totalForce, child->lm ); } } } /* ================ idAFTree::SetMaxSubTreeAuxiliaryIndex ================ */ void idAFTree::SetMaxSubTreeAuxiliaryIndex( void ) { int i, j; idAFBody *body, *child; // from the leaves up towards the root for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) { body = sortedBodies[i]; body->maxSubTreeAuxiliaryIndex = body->maxAuxiliaryIndex; for ( j = 0; j < body->children.Num(); j++ ) { child = body->children[j]; if ( child->maxSubTreeAuxiliaryIndex > body->maxSubTreeAuxiliaryIndex ) { body->maxSubTreeAuxiliaryIndex = child->maxSubTreeAuxiliaryIndex; } } } } /* ================ idAFTree::SortBodies_r ================ */ void idAFTree::SortBodies_r( idList&sortedList, idAFBody *body ) { int i; for ( i = 0; i < body->children.Num(); i++ ) { sortedList.Append( body->children[i] ); } for ( i = 0; i < body->children.Num(); i++ ) { SortBodies_r( sortedList, body->children[i] ); } } /* ================ idAFTree::SortBodies sort body list to make sure parents come first ================ */ void idAFTree::SortBodies( void ) { int i; idAFBody *body; // find the root for ( i = 0; i < sortedBodies.Num(); i++ ) { if ( !sortedBodies[i]->parent ) { break; } } if ( i >= sortedBodies.Num() ) { gameLocal.Error( "Articulated figure tree has no root." ); } body = sortedBodies[i]; sortedBodies.Clear(); sortedBodies.Append( body ); SortBodies_r( sortedBodies, body ); } /* ================ idAFTree::DebugDraw ================ */ void idAFTree::DebugDraw( const idVec4 &color ) const { int i; idAFBody *body; for ( i = 1; i < sortedBodies.Num(); i++ ) { body = sortedBodies[i]; gameRenderWorld->DebugArrow( color, body->parent->current->worldOrigin, body->current->worldOrigin, 1 ); } } //=============================================================== // M // idPhysics_AF MrE // E //=============================================================== /* ================ idPhysics_AF::EvaluateConstraints ================ */ void idPhysics_AF::EvaluateConstraints( float timeStep ) { int i; float invTimeStep; idAFBody *body; idAFConstraint *c; invTimeStep = 1.0f / timeStep; // setup the constraint equations for the current position and orientation of the bodies for ( i = 0; i < primaryConstraints.Num(); i++ ) { c = primaryConstraints[i]; c->Evaluate( invTimeStep ); c->J = c->J2; } for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { auxiliaryConstraints[i]->Evaluate( invTimeStep ); } // add contact constraints to the list with frame constraints for ( i = 0; i < contactConstraints.Num(); i++ ) { AddFrameConstraint( contactConstraints[i] ); } // setup body primary constraint matrix for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body->primaryConstraint ) { body->J = body->primaryConstraint->J1.Transpose(); } } } /* ================ idPhysics_AF::EvaluateBodies ================ */ void idPhysics_AF::EvaluateBodies( float timeStep ) { int i; idAFBody *body; idMat3 axis; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; // we transpose the axis before using it because idMat3 is column-major axis = body->current->worldAxis.Transpose(); // if the center of mass is at the body point of reference if ( body->centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) { // spatial inertia in world space body->I.Set( body->mass * mat3_identity, mat3_zero, mat3_zero, axis * body->inertiaTensor * axis.Transpose() ); // inverse spatial inertia in world space body->inverseWorldSpatialInertia.Set( body->invMass * mat3_identity, mat3_zero, mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose() ); body->fl.spatialInertiaSparse = true; } else { idMat3 massMoment = body->mass * SkewSymmetric( body->centerOfMass ); // spatial inertia in world space body->I.Set( body->mass * mat3_identity, massMoment, massMoment.Transpose(), axis * body->inertiaTensor * axis.Transpose() ); // inverse spatial inertia in world space body->inverseWorldSpatialInertia = body->I.InverseFast(); body->fl.spatialInertiaSparse = false; } // initialize auxiliary constraint force to zero body->auxForce.Zero(); } } /* ================ idPhysics_AF::AddFrameConstraints ================ */ void idPhysics_AF::AddFrameConstraints( void ) { int i; // add frame constraints to auxiliary constraints for ( i = 0; i < frameConstraints.Num(); i++ ) { auxiliaryConstraints.Append( frameConstraints[i] ); } } /* ================ idPhysics_AF::RemoveFrameConstraints ================ */ void idPhysics_AF::RemoveFrameConstraints( void ) { // remove all the frame constraints from the auxiliary constraints auxiliaryConstraints.SetNum( auxiliaryConstraints.Num() - frameConstraints.Num(), false ); frameConstraints.SetNum( 0, false ); } /* ================ idPhysics_AF::ApplyFriction ================ */ void idPhysics_AF::ApplyFriction( float timeStep, float endTimeMSec ) { int i; float invTimeStep; if ( af_skipFriction.GetBool() ) { return; } if ( jointFrictionDentStart < MS2SEC( endTimeMSec ) && jointFrictionDentEnd > MS2SEC( endTimeMSec ) ) { float halfTime = ( jointFrictionDentEnd - jointFrictionDentStart ) * 0.5f; if ( jointFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) { jointFrictionDentScale = 1.0f - ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart ) / halfTime; } else { jointFrictionDentScale = jointFrictionDent + ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart - halfTime ) / halfTime; } } else { jointFrictionDentScale = 0.0f; } if ( contactFrictionDentStart < MS2SEC( endTimeMSec ) && contactFrictionDentEnd > MS2SEC( endTimeMSec ) ) { float halfTime = ( contactFrictionDentEnd - contactFrictionDentStart ) * 0.5f; if ( contactFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) { contactFrictionDentScale = 1.0f - ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart ) / halfTime; } else { contactFrictionDentScale = contactFrictionDent + ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart - halfTime ) / halfTime; } } else { contactFrictionDentScale = 0.0f; } invTimeStep = 1.0f / timeStep; for ( i = 0; i < primaryConstraints.Num(); i++ ) { primaryConstraints[i]->ApplyFriction( invTimeStep ); } for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { auxiliaryConstraints[i]->ApplyFriction( invTimeStep ); } for ( i = 0; i < frameConstraints.Num(); i++ ) { frameConstraints[i]->ApplyFriction( invTimeStep ); } } /* ================ idPhysics_AF::PrimaryFactor ================ */ void idPhysics_AF::PrimaryFactor( void ) { int i; for ( i = 0; i < trees.Num(); i++ ) { trees[i]->Factor(); } } /* ================ idPhysics_AF::PrimaryForces ================ */ void idPhysics_AF::PrimaryForces( float timeStep ) { int i; for ( i = 0; i < trees.Num(); i++ ) { trees[i]->CalculateForces( timeStep ); } } /* ================ idPhysics_AF::AuxiliaryForces ================ */ void idPhysics_AF::AuxiliaryForces( float timeStep ) { int i, j, k, l, n, m, s, numAuxConstraints, *index, *boxIndex; float *ptr, *j1, *j2, *dstPtr, *forcePtr; float invStep, u; idAFBody *body; idAFConstraint *constraint; idVecX tmp; idMatX jmk; idVecX rhs, w, lm, lo, hi; // get the number of one dimensional auxiliary constraints for ( numAuxConstraints = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { numAuxConstraints += auxiliaryConstraints[i]->J1.GetNumRows(); } if ( numAuxConstraints == 0 ) { return; } // allocate memory to store the body response to auxiliary constraint forces forcePtr = (float *) _alloca16( bodies.Num() * numAuxConstraints * 8 * sizeof( float ) ); index = (int *) _alloca16( bodies.Num() * numAuxConstraints * sizeof( int ) ); for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; body->response = forcePtr; body->responseIndex = index; body->numResponses = 0; body->maxAuxiliaryIndex = 0; forcePtr += numAuxConstraints * 8; index += numAuxConstraints; } // set on each body the largest index of an auxiliary constraint constraining the body if ( af_useSymmetry.GetBool() ) { for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { constraint = auxiliaryConstraints[i]; for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { if ( k > constraint->body1->maxAuxiliaryIndex ) { constraint->body1->maxAuxiliaryIndex = k; } if ( constraint->body2 && k > constraint->body2->maxAuxiliaryIndex ) { constraint->body2->maxAuxiliaryIndex = k; } } } for ( i = 0; i < trees.Num(); i++ ) { trees[i]->SetMaxSubTreeAuxiliaryIndex(); } } // calculate forces of primary constraints in response to the auxiliary constraint forces for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { constraint = auxiliaryConstraints[i]; for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { // calculate body forces in the tree in response to the constraint force constraint->body1->tree->Response( constraint, j, k ); // if there is a second body which is part of a different tree if ( constraint->body2 && constraint->body2->tree != constraint->body1->tree ) { // calculate body forces in the second tree in response to the constraint force constraint->body2->tree->Response( constraint, j, k ); } } } // NOTE: the rows are 16 byte padded jmk.SetData( numAuxConstraints, ((numAuxConstraints+3)&~3), MATX_ALLOCA( numAuxConstraints * ((numAuxConstraints+3)&~3) ) ); tmp.SetData( 6, VECX_ALLOCA( 6 ) ); // create constraint matrix for auxiliary constraints using a mass matrix adjusted for the primary constraints for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { constraint = auxiliaryConstraints[i]; for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { constraint->body1->InverseWorldSpatialInertiaMultiply( tmp, constraint->J1[j] ); j1 = tmp.ToFloatPtr(); ptr = constraint->body1->response; index = constraint->body1->responseIndex; dstPtr = jmk[k]; s = af_useSymmetry.GetBool() ? k + 1 : numAuxConstraints; for ( l = n = 0, m = index[n]; n < constraint->body1->numResponses && m < s; n++, m = index[n] ) { while( l < m ) { dstPtr[l++] = 0.0f; } dstPtr[l++] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] + j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5]; ptr += 8; } while( l < s ) { dstPtr[l++] = 0.0f; } if ( constraint->body2 ) { constraint->body2->InverseWorldSpatialInertiaMultiply( tmp, constraint->J2[j] ); j2 = tmp.ToFloatPtr(); ptr = constraint->body2->response; index = constraint->body2->responseIndex; for ( n = 0, m = index[n]; n < constraint->body2->numResponses && m < s; n++, m = index[n] ) { dstPtr[m] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] + j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5]; ptr += 8; } } } } if ( af_useSymmetry.GetBool() ) { n = jmk.GetNumColumns(); for ( i = 0; i < numAuxConstraints; i++ ) { ptr = jmk.ToFloatPtr() + ( i + 1 ) * n + i; dstPtr = jmk.ToFloatPtr() + i * n + i + 1; for ( j = i+1; j < numAuxConstraints; j++ ) { *dstPtr++ = *ptr; ptr += n; } } } invStep = 1.0f / timeStep; // calculate body acceleration for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() ); body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep; } rhs.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); lo.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); hi.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); lm.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); boxIndex = (int *) _alloca16( numAuxConstraints * sizeof( int ) ); // set first index for special box constrained variables for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { auxiliaryConstraints[i]->firstIndex = k; k += auxiliaryConstraints[i]->J1.GetNumRows(); } // initialize right hand side and low and high bounds for auxiliary constraints for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { constraint = auxiliaryConstraints[i]; n = k; for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { j1 = constraint->J1[j]; ptr = constraint->body1->acceleration.ToFloatPtr(); rhs[k] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] + j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5]; rhs[k] += constraint->c1[j] * invStep; if ( constraint->body2 ) { j2 = constraint->J2[j]; ptr = constraint->body2->acceleration.ToFloatPtr(); rhs[k] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] + j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5]; rhs[k] += constraint->c2[j] * invStep; } rhs[k] = -rhs[k]; lo[k] = constraint->lo[j]; hi[k] = constraint->hi[j]; if ( constraint->boxIndex[j] >= 0 ) { if ( constraint->boxConstraint->fl.isPrimary ) { gameLocal.Error( "cannot reference primary constraints for the box index" ); } boxIndex[k] = constraint->boxConstraint->firstIndex + constraint->boxIndex[j]; } else { boxIndex[k] = -1; } jmk[k][k] += constraint->e[j] * invStep; } } #ifdef AF_TIMINGS timer_lcp.Start(); #endif // calculate lagrange multipliers for auxiliary constraints if ( !lcp->Solve( jmk, lm, rhs, lo, hi, boxIndex ) ) { return; // bad monkey! } #ifdef AF_TIMINGS timer_lcp.Stop(); #endif // calculate auxiliary constraint forces for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { constraint = auxiliaryConstraints[i]; for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { constraint->lm[j] = u = lm[k]; j1 = constraint->J1[j]; ptr = constraint->body1->auxForce.ToFloatPtr(); ptr[0] += j1[0] * u; ptr[1] += j1[1] * u; ptr[2] += j1[2] * u; ptr[3] += j1[3] * u; ptr[4] += j1[4] * u; ptr[5] += j1[5] * u; if ( constraint->body2 ) { j2 = constraint->J2[j]; ptr = constraint->body2->auxForce.ToFloatPtr(); ptr[0] += j2[0] * u; ptr[1] += j2[1] * u; ptr[2] += j2[2] * u; ptr[3] += j2[3] * u; ptr[4] += j2[4] * u; ptr[5] += j2[5] * u; } } } // recalculate primary constraint forces in response to auxiliary constraint forces PrimaryForces( timeStep ); // clear pointers pointing to stack space so tools don't get confused for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; body->response = NULL; body->responseIndex = NULL; } } /* ================ idPhysics_AF::VerifyContactConstraints ================ */ void idPhysics_AF::VerifyContactConstraints( void ) { #if 0 int i; float impulseNumerator, impulseDenominator; idVec3 r, velocity, normalVelocity, normal, impulse; idAFBody *body; for ( i = 0; i < contactConstraints.Num(); i++ ) { body = contactConstraints[i]->body1; const contactInfo_t &contact = contactConstraints[i]->GetContact(); r = contact.point - body->GetCenterOfMass(); // calculate velocity at contact point velocity = body->GetLinearVelocity() + body->GetAngularVelocity().Cross( r ); // velocity along normal vector normalVelocity = ( velocity * contact.normal ) * contact.normal; // if moving towards the surface at the contact point if ( normalVelocity * contact.normal < 0.0f ) { // calculate impulse normal = -normalVelocity; impulseNumerator = normal.Normalize(); impulseDenominator = body->GetInverseMass() + ( ( body->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal ); impulse = (impulseNumerator / impulseDenominator) * normal * 1.0001f; // apply impulse body->SetLinearVelocity( body->GetLinearVelocity() + impulse ); body->SetAngularVelocity( body->GetAngularVelocity() + r.Cross( impulse ) ); } } #else int i; idAFBody *body; idVec3 normal; for ( i = 0; i < contactConstraints.Num(); i++ ) { body = contactConstraints[i]->body1; normal = contactConstraints[i]->GetContact().normal; if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) { body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal; } body = contactConstraints[i]->body2; if ( !body ) { continue; } normal = -normal; if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) { body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal; } } #endif } /* ================ idPhysics_AF::Evolve ================ */ void idPhysics_AF::Evolve( float timeStep ) { int i; float angle; idVec3 vec; idAFBody *body; idVec6 force; idRotation rotation; float vSqr, maxLinearVelocity, maxAngularVelocity; maxLinearVelocity = af_maxLinearVelocity.GetFloat() / timeStep; maxAngularVelocity = af_maxAngularVelocity.GetFloat() / timeStep; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; // calculate the spatial velocity for the next physics state body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() ); body->next->spatialVelocity = body->current->spatialVelocity + timeStep * body->acceleration.SubVec6(0); if ( maxLinearVelocity > 0.0f ) { // cap the linear velocity vSqr = body->next->spatialVelocity.SubVec3(0).LengthSqr(); if ( vSqr > Square( maxLinearVelocity ) ) { body->next->spatialVelocity.SubVec3(0) *= idMath::InvSqrt( vSqr ) * maxLinearVelocity; } } if ( maxAngularVelocity > 0.0f ) { // cap the angular velocity vSqr = body->next->spatialVelocity.SubVec3(1).LengthSqr(); if ( vSqr > Square( maxAngularVelocity ) ) { body->next->spatialVelocity.SubVec3(1) *= idMath::InvSqrt( vSqr ) * maxAngularVelocity; } } } // make absolutely sure all contact constraints are satisfied VerifyContactConstraints(); // calculate the position of the bodies for the next physics state for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; // translate world origin body->next->worldOrigin = body->current->worldOrigin + timeStep * body->next->spatialVelocity.SubVec3( 0 ); // convert angular velocity to a rotation matrix vec = body->next->spatialVelocity.SubVec3( 1 ); angle = -timeStep * (float) RAD2DEG( vec.Normalize() ); rotation = idRotation( vec3_origin, vec, angle ); rotation.Normalize180(); // rotate world axis body->next->worldAxis = body->current->worldAxis * rotation.ToMat3(); body->next->worldAxis.OrthoNormalizeSelf(); // linear and angular friction body->next->spatialVelocity.SubVec3(0) -= body->linearFriction * body->next->spatialVelocity.SubVec3(0); body->next->spatialVelocity.SubVec3(1) -= body->angularFriction * body->next->spatialVelocity.SubVec3(1); } } /* ================ idPhysics_AF::CollisionImpulse apply impulse to the colliding bodies the current state of the body should be set to the moment of impact this is silly as it doesn't take the AF structure into account ================ */ bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision ) { idVec3 r, velocity, impulse; idMat3 inverseWorldInertiaTensor; float impulseNumerator, impulseDenominator; impactInfo_t info; idEntity *ent; ent = gameLocal.entities[collision.c.entityNum]; if ( ent == self ) { return false; } // get info from other entity involved ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info ); // collision point relative to the body center of mass r = collision.c.point - (body->current->worldOrigin + body->centerOfMass * body->current->worldAxis); // the velocity at the collision point velocity = body->current->spatialVelocity.SubVec3(0) + body->current->spatialVelocity.SubVec3(1).Cross(r); // subtract velocity of other entity velocity -= info.velocity; // never stick if ( velocity * collision.c.normal > 0.0f ) { velocity = collision.c.normal; } inverseWorldInertiaTensor = body->current->worldAxis.Transpose() * body->inverseInertiaTensor * body->current->worldAxis; impulseNumerator = -( 1.0f + body->bouncyness ) * ( velocity * collision.c.normal ); impulseDenominator = body->invMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal ); if ( info.invMass ) { impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal ); } impulse = (impulseNumerator / impulseDenominator) * collision.c.normal; // apply impact to other entity ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse ); // callback to self to let the entity know about the impact return self->Collide( collision, velocity ); } /* ================ idPhysics_AF::ApplyCollisions ================ */ bool idPhysics_AF::ApplyCollisions( float timeStep ) { int i; for ( i = 0; i < collisions.Num(); i++ ) { if ( CollisionImpulse( timeStep, collisions[i].body, collisions[i].trace ) ) { return true; } } return false; } /* ================ idPhysics_AF::SetupCollisionForBody ================ */ idEntity *idPhysics_AF::SetupCollisionForBody( idAFBody *body ) const { int i; idAFBody *b; idEntity *passEntity; passEntity = NULL; if ( !selfCollision || !body->fl.selfCollision || af_skipSelfCollision.GetBool() ) { // disable all bodies for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->clipModel->Disable(); } // don't collide with world collision model if attached to the world for ( i = 0; i < body->constraints.Num(); i++ ) { if ( !body->constraints[i]->fl.noCollision ) { continue; } // if this constraint attaches the body to the world if ( body->constraints[i]->body2 == NULL ) { // don't collide with the world collision model passEntity = gameLocal.world; } } } else { // enable all bodies that have self collision for ( i = 0; i < bodies.Num(); i++ ) { if ( bodies[i]->fl.selfCollision ) { bodies[i]->clipModel->Enable(); } else { bodies[i]->clipModel->Disable(); } } // don't let the body collide with itself body->clipModel->Disable(); // disable any bodies attached with constraints for ( i = 0; i < body->constraints.Num(); i++ ) { if ( !body->constraints[i]->fl.noCollision ) { continue; } // if this constraint attaches the body to the world if ( body->constraints[i]->body2 == NULL ) { // don't collide with the world collision model passEntity = gameLocal.world; } else { if ( body->constraints[i]->body1 == body ) { b = body->constraints[i]->body2; } else if ( body->constraints[i]->body2 == body ) { b = body->constraints[i]->body1; } else { continue; } // don't collide with this body b->clipModel->Disable(); } } } return passEntity; } /* ================ idPhysics_AF::CheckForCollisions check for collisions between the current and next state if there is a collision the next state is set to the state at the moment of impact assumes all bodies are linked for collision detection and relinks all bodies after moving them ================ */ void idPhysics_AF::CheckForCollisions( float timeStep ) { // #define TEST_COLLISION_DETECTION int i, index; idAFBody *body; idMat3 axis; idRotation rotation; trace_t collision; idEntity *passEntity; // clear list with collisions collisions.SetNum( 0, false ); if ( !enableCollision ) { return; } for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body->clipMask != 0 ) { passEntity = SetupCollisionForBody( body ); #ifdef TEST_COLLISION_DETECTION bool startsolid = false; if ( gameLocal.clip.Contents( body->current->worldOrigin, body->clipModel, body->current->worldAxis, body->clipMask, passEntity ) ) { startsolid = true; } #endif TransposeMultiply( body->current->worldAxis, body->next->worldAxis, axis ); rotation = axis.ToRotation(); rotation.SetOrigin( body->current->worldOrigin ); // if there was a collision if ( gameLocal.clip.Motion( collision, body->current->worldOrigin, body->next->worldOrigin, rotation, body->clipModel, body->current->worldAxis, body->clipMask, passEntity ) ) { // set the next state to the state at the moment of impact body->next->worldOrigin = collision.endpos; body->next->worldAxis = collision.endAxis; // add collision to the list index = collisions.Num(); collisions.SetNum( index + 1, false ); collisions[index].trace = collision; collisions[index].body = body; } #ifdef TEST_COLLISION_DETECTION if ( gameLocal.clip.Contents( body->next->worldOrigin, body->clipModel, body->next->worldAxis, body->clipMask, passEntity ) ) { if ( !startsolid ) { int bah = 1; } } #endif } body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->next->worldOrigin, body->next->worldAxis ); } } /* ================ idPhysics_AF::EvaluateContacts ================ */ bool idPhysics_AF::EvaluateContacts( void ) { int i, j, k, numContacts, numBodyContacts; idAFBody *body; contactInfo_t contactInfo[10]; idEntity *passEntity; idVecX dir( 6, VECX_ALLOCA( 6 ) ); // evaluate bodies EvaluateBodies( current.lastTimeStep ); // remove all existing contacts ClearContacts(); contactBodies.SetNum( 0, false ); if ( !enableCollision ) { return false; } // find all the contacts for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body->clipMask == 0 ) { continue; } passEntity = SetupCollisionForBody( body ); body->InverseWorldSpatialInertiaMultiply( dir, body->current->externalForce.ToFloatPtr() ); dir.SubVec6(0) = body->current->spatialVelocity + current.lastTimeStep * dir.SubVec6(0); dir.SubVec3(0).Normalize(); dir.SubVec3(1).Normalize(); numContacts = gameLocal.clip.Contacts( contactInfo, 10, body->current->worldOrigin, dir.SubVec6(0), 2.0f, //CONTACT_EPSILON, body->clipModel, body->current->worldAxis, body->clipMask, passEntity ); #if 1 // merge nearby contacts between the same bodies // and assure there are at most three planar contacts between any pair of bodies for ( j = 0; j < numContacts; j++ ) { numBodyContacts = 0; for ( k = 0; k < contacts.Num(); k++ ) { if ( contacts[k].entityNum == contactInfo[j].entityNum ) { if ( ( contacts[k].id == i && contactInfo[j].id == contactBodies[k] ) || ( contactBodies[k] == i && contacts[k].id == contactInfo[j].id ) ) { if ( ( contacts[k].point - contactInfo[j].point ).LengthSqr() < Square( 2.0f ) ) { break; } if ( idMath::Fabs( contacts[k].normal * contactInfo[j].normal ) > 0.9f ) { numBodyContacts++; } } } } if ( k >= contacts.Num() && numBodyContacts < 3 ) { contacts.Append( contactInfo[j] ); contactBodies.Append( i ); } } #else for ( j = 0; j < numContacts; j++ ) { contacts.Append( contactInfo[j] ); contactBodies.Append( i ); } #endif } AddContactEntitiesForContacts(); return ( contacts.Num() != 0 ); } /* ================ idPhysics_AF::SetupContactConstraints ================ */ void idPhysics_AF::SetupContactConstraints( void ) { int i; // make sure enough contact constraints are allocated contactConstraints.AssureSizeAlloc( contacts.Num(), idListNewElement ); contactConstraints.SetNum( contacts.Num(), false ); // setup contact constraints for ( i = 0; i < contacts.Num(); i++ ) { // add contact constraint contactConstraints[i]->physics = this; if ( contacts[i].entityNum == self->entityNumber ) { contactConstraints[i]->Setup( bodies[contactBodies[i]], bodies[ contacts[i].id ], contacts[i] ); } else { contactConstraints[i]->Setup( bodies[contactBodies[i]], NULL, contacts[i] ); } } } /* ================ idPhysics_AF::ApplyContactForces ================ */ void idPhysics_AF::ApplyContactForces( void ) { #if 0 int i; idEntity *ent; idVec3 force; for ( i = 0; i < contactConstraints.Num(); i++ ) { if ( contactConstraints[i]->body2 != NULL ) { continue; } const contactInfo_t &contact = contactConstraints[i]->GetContact(); ent = gameLocal.entities[contact.entityNum]; if ( !ent ) { continue; } force.Zero(); ent->AddForce( self, contact.id, contact.point, force ); } #endif } /* ================ idPhysics_AF::ClearExternalForce ================ */ void idPhysics_AF::ClearExternalForce( void ) { int i; idAFBody *body; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; // clear external force body->current->externalForce.Zero(); body->next->externalForce.Zero(); } } /* ================ idPhysics_AF::AddGravity ================ */ void idPhysics_AF::AddGravity( void ) { int i; idAFBody *body; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; // add gravitational force body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector; } } /* ================ idPhysics_AF::SwapStates ================ */ void idPhysics_AF::SwapStates( void ) { int i; idAFBody *body; AFBodyPState_t *swap; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; // swap the current and next state for next simulation step swap = body->current; body->current = body->next; body->next = swap; } } /* ================ idPhysics_AF::UpdateClipModels ================ */ void idPhysics_AF::UpdateClipModels( void ) { int i; idAFBody *body; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->current->worldOrigin, body->current->worldAxis ); } } /* ================ idPhysics_AF::SetSuspendSpeed ================ */ void idPhysics_AF::SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration ) { this->suspendVelocity = velocity; this->suspendAcceleration = acceleration; } /* ================ idPhysics_AF::SetSuspendTime ================ */ void idPhysics_AF::SetSuspendTime( const float minTime, const float maxTime ) { this->minMoveTime = minTime; this->maxMoveTime = maxTime; } /* ================ idPhysics_AF::SetSuspendTolerance ================ */ void idPhysics_AF::SetSuspendTolerance( const float noMoveTime, const float noMoveTranslation, const float noMoveRotation ) { this->noMoveTime = noMoveTime; this->noMoveTranslation = noMoveTranslation; this->noMoveRotation = noMoveRotation; } /* ================ idPhysics_AF::SetTimeScaleRamp ================ */ void idPhysics_AF::SetTimeScaleRamp( const float start, const float end ) { timeScaleRampStart = start; timeScaleRampEnd = end; } /* ================ idPhysics_AF::SetJointFrictionDent ================ */ void idPhysics_AF::SetJointFrictionDent( const float dent, const float start, const float end ) { jointFrictionDent = dent; jointFrictionDentStart = start; jointFrictionDentEnd = end; } /* ================ idPhysics_AF::GetJointFrictionScale ================ */ float idPhysics_AF::GetJointFrictionScale( void ) const { if ( jointFrictionDentScale > 0.0f ) { return jointFrictionDentScale; } else if ( jointFrictionScale > 0.0f ) { return jointFrictionScale; } else if ( af_jointFrictionScale.GetFloat() > 0.0f ) { return af_jointFrictionScale.GetFloat(); } return 1.0f; } /* ================ idPhysics_AF::SetContactFrictionDent ================ */ void idPhysics_AF::SetContactFrictionDent( const float dent, const float start, const float end ) { contactFrictionDent = dent; contactFrictionDentStart = start; contactFrictionDentEnd = end; } /* ================ idPhysics_AF::GetContactFrictionScale ================ */ float idPhysics_AF::GetContactFrictionScale( void ) const { if ( contactFrictionDentScale > 0.0f ) { return contactFrictionDentScale; } else if ( contactFrictionScale > 0.0f ) { return contactFrictionScale; } else if ( af_contactFrictionScale.GetFloat() > 0.0f ) { return af_contactFrictionScale.GetFloat(); } return 1.0f; } /* ================ idPhysics_AF::TestIfAtRest ================ */ bool idPhysics_AF::TestIfAtRest( float timeStep ) { int i; float translationSqr, maxTranslationSqr, rotation, maxRotation; idAFBody *body; if ( current.atRest >= 0 ) { return true; } current.activateTime += timeStep; // if the simulation should never be suspended before a certaint amount of time passed if ( minMoveTime > 0.0f && current.activateTime < minMoveTime ) { return false; } // if the simulation should always be suspended after a certain amount time passed if ( maxMoveTime > 0.0f && current.activateTime > maxMoveTime ) { return true; } // test if all bodies hardly moved over a period of time if ( current.noMoveTime == 0.0f ) { for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; body->atRestOrigin = body->current->worldOrigin; body->atRestAxis = body->current->worldAxis; } current.noMoveTime += timeStep; } else if ( current.noMoveTime > noMoveTime ) { current.noMoveTime = 0.0f; maxTranslationSqr = 0.0f; maxRotation = 0.0f; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; translationSqr = ( body->current->worldOrigin - body->atRestOrigin ).LengthSqr(); if ( translationSqr > maxTranslationSqr ) { maxTranslationSqr = translationSqr; } rotation = ( body->atRestAxis.Transpose() * body->current->worldAxis ).ToRotation().GetAngle(); if ( rotation > maxRotation ) { maxRotation = rotation; } } if ( maxTranslationSqr < Square( noMoveTranslation ) && maxRotation < noMoveRotation ) { // hardly moved over a period of time so the articulated figure may come to rest return true; } } else { current.noMoveTime += timeStep; } // test if the velocity or acceleration of any body is still too large to come to rest for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body->current->spatialVelocity.SubVec3(0).LengthSqr() > Square( suspendVelocity[0] ) ) { return false; } if ( body->current->spatialVelocity.SubVec3(1).LengthSqr() > Square( suspendVelocity[1] ) ) { return false; } if ( body->acceleration.SubVec3(0).LengthSqr() > Square( suspendAcceleration[0] ) ) { return false; } if ( body->acceleration.SubVec3(1).LengthSqr() > Square( suspendAcceleration[1] ) ) { return false; } } // all bodies have a velocity and acceleration small enough to come to rest return true; } /* ================ idPhysics_AF::Rest ================ */ void idPhysics_AF::Rest( void ) { int i; current.atRest = gameLocal.time; for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->current->spatialVelocity.Zero(); bodies[i]->current->externalForce.Zero(); } self->BecomeInactive( TH_PHYSICS ); } /* ================ idPhysics_AF::Activate ================ */ void idPhysics_AF::Activate( void ) { // if the articulated figure was at rest if ( current.atRest >= 0 ) { // normally gravity is added at the end of a simulation frame // if the figure was at rest add gravity here so it is applied this simulation frame AddGravity(); // reset the active time for the max move time current.activateTime = 0.0f; } current.atRest = -1; current.noMoveTime = 0.0f; self->BecomeActive( TH_PHYSICS ); } /* ================ idPhysics_AF::PutToRest put to rest untill something collides with this physics object ================ */ void idPhysics_AF::PutToRest( void ) { Rest(); } /* ================ idPhysics_AF::EnableImpact ================ */ void idPhysics_AF::EnableImpact( void ) { noImpact = false; } /* ================ idPhysics_AF::DisableImpact ================ */ void idPhysics_AF::DisableImpact( void ) { noImpact = true; } /* ================ idPhysics_AF::AddPushVelocity ================ */ void idPhysics_AF::AddPushVelocity( const idVec6 &pushVelocity ) { int i; if ( pushVelocity != vec6_origin ) { for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->current->spatialVelocity += pushVelocity; } } } /* ================ idPhysics_AF::SetClipModel ================ */ void idPhysics_AF::SetClipModel( idClipModel *model, float density, int id, bool freeOld ) { } /* ================ idPhysics_AF::GetClipModel ================ */ idClipModel *idPhysics_AF::GetClipModel( int id ) const { if ( id >= 0 && id < bodies.Num() ) { return bodies[id]->GetClipModel(); } return NULL; } /* ================ idPhysics_AF::GetNumClipModels ================ */ int idPhysics_AF::GetNumClipModels( void ) const { return bodies.Num(); } /* ================ idPhysics_AF::SetMass ================ */ void idPhysics_AF::SetMass( float mass, int id ) { if ( id >= 0 && id < bodies.Num() ) { } else { forceTotalMass = mass; } SetChanged(); } /* ================ idPhysics_AF::GetMass ================ */ float idPhysics_AF::GetMass( int id ) const { if ( id >= 0 && id < bodies.Num() ) { return bodies[id]->mass; } return totalMass; } /* ================ idPhysics_AF::SetContents ================ */ void idPhysics_AF::SetContents( int contents, int id ) { int i; if ( id >= 0 && id < bodies.Num() ) { bodies[id]->GetClipModel()->SetContents( contents ); } else { for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->GetClipModel()->SetContents( contents ); } } } /* ================ idPhysics_AF::GetContents ================ */ int idPhysics_AF::GetContents( int id ) const { int i, contents; if ( id >= 0 && id < bodies.Num() ) { return bodies[id]->GetClipModel()->GetContents(); } else { contents = 0; for ( i = 0; i < bodies.Num(); i++ ) { contents |= bodies[i]->GetClipModel()->GetContents(); } return contents; } } /* ================ idPhysics_AF::GetBounds ================ */ const idBounds &idPhysics_AF::GetBounds( int id ) const { int i; static idBounds relBounds; if ( id >= 0 && id < bodies.Num() ) { return bodies[id]->GetClipModel()->GetBounds(); } else if ( !bodies.Num() ) { relBounds.Zero(); return relBounds; } else { relBounds = bodies[0]->GetClipModel()->GetBounds(); for ( i = 1; i < bodies.Num(); i++ ) { idBounds bounds; idVec3 origin = ( bodies[i]->GetWorldOrigin() - bodies[0]->GetWorldOrigin() ) * bodies[0]->GetWorldAxis().Transpose(); idMat3 axis = bodies[i]->GetWorldAxis() * bodies[0]->GetWorldAxis().Transpose(); bounds.FromTransformedBounds( bodies[i]->GetClipModel()->GetBounds(), origin, axis ); relBounds += bounds; } return relBounds; } } /* ================ idPhysics_AF::GetAbsBounds ================ */ const idBounds &idPhysics_AF::GetAbsBounds( int id ) const { int i; static idBounds absBounds; if ( id >= 0 && id < bodies.Num() ) { return bodies[id]->GetClipModel()->GetAbsBounds(); } else if ( !bodies.Num() ) { absBounds.Zero(); return absBounds; } else { absBounds = bodies[0]->GetClipModel()->GetAbsBounds(); for ( i = 1; i < bodies.Num(); i++ ) { absBounds += bodies[i]->GetClipModel()->GetAbsBounds(); } return absBounds; } } /* ================ idPhysics_AF::Evaluate ================ */ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { float timeStep; if ( timeScaleRampStart < MS2SEC( endTimeMSec ) && timeScaleRampEnd > MS2SEC( endTimeMSec ) ) { timeStep = MS2SEC( timeStepMSec ) * ( MS2SEC( endTimeMSec ) - timeScaleRampStart ) / ( timeScaleRampEnd - timeScaleRampStart ); } else if ( af_timeScale.GetFloat() != 1.0f ) { timeStep = MS2SEC( timeStepMSec ) * af_timeScale.GetFloat(); } else { timeStep = MS2SEC( timeStepMSec ) * timeScale; } current.lastTimeStep = timeStep; // if the articulated figure changed if ( changedAF || ( linearTime != af_useLinearTime.GetBool() ) ) { BuildTrees(); changedAF = false; linearTime = af_useLinearTime.GetBool(); } // get the new master position if ( masterBody ) { idVec3 masterOrigin; idMat3 masterAxis; self->GetMasterPosition( masterOrigin, masterAxis ); if ( current.atRest >= 0 && ( masterBody->current->worldOrigin != masterOrigin || masterBody->current->worldAxis != masterAxis ) ) { Activate(); } masterBody->current->worldOrigin = masterOrigin; masterBody->current->worldAxis = masterAxis; } // if the simulation is suspended because the figure is at rest if ( current.atRest >= 0 || timeStep <= 0.0f ) { DebugDraw(); return false; } // move the af velocity into the frame of a pusher AddPushVelocity( -current.pushVelocity ); #ifdef AF_TIMINGS timer_total.Start(); #endif #ifdef AF_TIMINGS timer_collision.Start(); #endif // evaluate contacts EvaluateContacts(); // setup contact constraints SetupContactConstraints(); #ifdef AF_TIMINGS timer_collision.Stop(); #endif // evaluate constraint equations EvaluateConstraints( timeStep ); // apply friction ApplyFriction( timeStep, endTimeMSec ); // add frame constraints AddFrameConstraints(); #ifdef AF_TIMINGS int i, numPrimary = 0, numAuxiliary = 0; for ( i = 0; i < primaryConstraints.Num(); i++ ) { numPrimary += primaryConstraints[i]->J1.GetNumRows(); } for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { numAuxiliary += auxiliaryConstraints[i]->J1.GetNumRows(); } timer_pc.Start(); #endif // factor matrices for primary constraints PrimaryFactor(); // calculate forces on bodies after applying primary constraints PrimaryForces( timeStep ); #ifdef AF_TIMINGS timer_pc.Stop(); timer_ac.Start(); #endif // calculate and apply auxiliary constraint forces AuxiliaryForces( timeStep ); #ifdef AF_TIMINGS timer_ac.Stop(); #endif // evolve current state to next state Evolve( timeStep ); // debug graphics DebugDraw(); // clear external forces on all bodies ClearExternalForce(); // apply contact force to other entities ApplyContactForces(); // remove all frame constraints RemoveFrameConstraints(); #ifdef AF_TIMINGS timer_collision.Start(); #endif // check for collisions between current and next state CheckForCollisions( timeStep ); #ifdef AF_TIMINGS timer_collision.Stop(); #endif // swap the current and next state SwapStates(); // make sure all clip models are disabled in case they were enabled for self collision if ( selfCollision && !af_skipSelfCollision.GetBool() ) { DisableClip(); } // apply collision impulses if ( ApplyCollisions( timeStep ) ) { current.atRest = gameLocal.time; comeToRest = true; } // test if the simulation can be suspended because the whole figure is at rest if ( comeToRest && TestIfAtRest( timeStep ) ) { Rest(); } else { ActivateContactEntities(); } // add gravitational force AddGravity(); // move the af velocity back into the world frame AddPushVelocity( current.pushVelocity ); current.pushVelocity.Zero(); if ( IsOutsideWorld() ) { gameLocal.Warning( "articulated figure moved outside world bounds for entity '%s' type '%s' at (%s)", self->name.c_str(), self->GetType()->classname, bodies[0]->current->worldOrigin.ToString(0) ); Rest(); } #ifdef AF_TIMINGS timer_total.Stop(); if ( af_showTimings.GetInteger() == 1 ) { gameLocal.Printf( "%12s: t %u pc %2d, %u ac %2d %u lcp %u cd %u\n", self->name.c_str(), timer_total.Milliseconds(), numPrimary, timer_pc.Milliseconds(), numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(), timer_lcp.Milliseconds(), timer_collision.Milliseconds() ); } else if ( af_showTimings.GetInteger() == 2 ) { numArticulatedFigures++; if ( endTimeMSec > lastTimerReset ) { gameLocal.Printf( "af %d: t %u pc %2d, %u ac %2d %u lcp %u cd %u\n", numArticulatedFigures, timer_total.Milliseconds(), numPrimary, timer_pc.Milliseconds(), numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(), timer_lcp.Milliseconds(), timer_collision.Milliseconds() ); } } if ( endTimeMSec > lastTimerReset ) { lastTimerReset = endTimeMSec; numArticulatedFigures = 0; timer_total.Clear(); timer_pc.Clear(); timer_ac.Clear(); timer_collision.Clear(); timer_lcp.Clear(); } #endif return true; } /* ================ idPhysics_AF::UpdateTime ================ */ void idPhysics_AF::UpdateTime( int endTimeMSec ) { } /* ================ idPhysics_AF::GetTime ================ */ int idPhysics_AF::GetTime( void ) const { return gameLocal.time; } /* ================ DrawTraceModelSilhouette ================ */ void DrawTraceModelSilhouette( const idVec3 &projectionOrigin, const idClipModel *clipModel ) { int i, numSilEdges; int silEdges[MAX_TRACEMODEL_EDGES]; idVec3 v1, v2; const idTraceModel *trm = clipModel->GetTraceModel(); const idVec3 &origin = clipModel->GetOrigin(); const idMat3 &axis = clipModel->GetAxis(); numSilEdges = trm->GetProjectionSilhouetteEdges( ( projectionOrigin - origin ) * axis.Transpose(), silEdges ); for ( i = 0; i < numSilEdges; i++ ) { v1 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITSET( silEdges[i] ) ] ]; v2 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITNOTSET( silEdges[i] ) ] ]; gameRenderWorld->DebugArrow( colorRed, origin + v1 * axis, origin + v2 * axis, 1 ); } } /* ================ idPhysics_AF::DebugDraw ================ */ void idPhysics_AF::DebugDraw( void ) { int i; idAFBody *body, *highlightBody = NULL, *constrainedBody1 = NULL, *constrainedBody2 = NULL; idAFConstraint *constraint; idVec3 center; idMat3 axis; if ( af_highlightConstraint.GetString()[0] ) { constraint = GetConstraint( af_highlightConstraint.GetString() ); if ( constraint ) { constraint->GetCenter( center ); axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3(); gameRenderWorld->DebugCone( colorYellow, center, (axis[2] - axis[1]) * 4.0f, 0.0f, 1.0f, 0 ); if ( af_showConstrainedBodies.GetBool() ) { cvarSystem->SetCVarString( "cm_drawColor", colorCyan.ToString( 0 ) ); constrainedBody1 = constraint->body1; if ( constrainedBody1 ) { collisionModelManager->DrawModel( constrainedBody1->clipModel->Handle(), constrainedBody1->clipModel->GetOrigin(), constrainedBody1->clipModel->GetAxis(), vec3_origin, 0.0f ); } cvarSystem->SetCVarString( "cm_drawColor", colorBlue.ToString( 0 ) ); constrainedBody2 = constraint->body2; if ( constrainedBody2 ) { collisionModelManager->DrawModel( constrainedBody2->clipModel->Handle(), constrainedBody2->clipModel->GetOrigin(), constrainedBody2->clipModel->GetAxis(), vec3_origin, 0.0f ); } cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) ); } } } if ( af_highlightBody.GetString()[0] ) { highlightBody = GetBody( af_highlightBody.GetString() ); if ( highlightBody ) { cvarSystem->SetCVarString( "cm_drawColor", colorYellow.ToString( 0 ) ); collisionModelManager->DrawModel( highlightBody->clipModel->Handle(), highlightBody->clipModel->GetOrigin(), highlightBody->clipModel->GetAxis(), vec3_origin, 0.0f ); cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) ); } } if ( af_showBodies.GetBool() ) { for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body == constrainedBody1 || body == constrainedBody2 ) { continue; } if ( body == highlightBody ) { continue; } collisionModelManager->DrawModel( body->clipModel->Handle(), body->clipModel->GetOrigin(), body->clipModel->GetAxis(), vec3_origin, 0.0f ); //DrawTraceModelSilhouette( gameLocal.GetLocalPlayer()->GetEyePosition(), body->clipModel ); } } if ( af_showBodyNames.GetBool() ) { for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; gameRenderWorld->DrawText( body->GetName().c_str(), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); } } if ( af_showMass.GetBool() ) { for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; gameRenderWorld->DrawText( va( "\n%1.2f", 1.0f / body->GetInverseMass() ), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); } } if ( af_showTotalMass.GetBool() ) { axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3(); gameRenderWorld->DrawText( va( "\n%1.2f", totalMass ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 ); } if ( af_showInertia.GetBool() ) { for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; idMat3 &I = body->inertiaTensor; gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )", I[0].x, I[0].y, I[0].z, I[1].x, I[1].y, I[1].z, I[2].x, I[2].y, I[2].z ), body->GetWorldOrigin(), 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); } } if ( af_showVelocity.GetBool() ) { for ( i = 0; i < bodies.Num(); i++ ) { DrawVelocity( bodies[i]->clipModel->GetId(), 0.1f, 4.0f ); } } if ( af_showConstraints.GetBool() ) { for ( i = 0; i < primaryConstraints.Num(); i++ ) { constraint = primaryConstraints[i]; constraint->DebugDraw(); } if ( !af_showPrimaryOnly.GetBool() ) { for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { constraint = auxiliaryConstraints[i]; constraint->DebugDraw(); } } } if ( af_showConstraintNames.GetBool() ) { for ( i = 0; i < primaryConstraints.Num(); i++ ) { constraint = primaryConstraints[i]; constraint->GetCenter( center ); gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); } if ( !af_showPrimaryOnly.GetBool() ) { for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { constraint = auxiliaryConstraints[i]; constraint->GetCenter( center ); gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); } } } if ( af_showTrees.GetBool() || ( af_showActive.GetBool() && current.atRest < 0 ) ) { for ( i = 0; i < trees.Num(); i++ ) { trees[i]->DebugDraw( idStr::ColorForIndex( i+3 ) ); } } } /* ================ idPhysics_AF::idPhysics_AF ================ */ idPhysics_AF::idPhysics_AF( void ) { trees.Clear(); bodies.Clear(); constraints.Clear(); primaryConstraints.Clear(); auxiliaryConstraints.Clear(); frameConstraints.Clear(); contacts.Clear(); collisions.Clear(); changedAF = true; masterBody = NULL; lcp = idLCP::AllocSymmetric(); memset( ¤t, 0, sizeof( current ) ); current.atRest = -1; current.lastTimeStep = USERCMD_MSEC; saved = current; linearFriction = 0.005f; angularFriction = 0.005f; contactFriction = 0.8f; bouncyness = 0.4f; totalMass = 0.0f; forceTotalMass = -1.0f; suspendVelocity.Set( SUSPEND_LINEAR_VELOCITY, SUSPEND_ANGULAR_VELOCITY ); suspendAcceleration.Set( SUSPEND_LINEAR_ACCELERATION, SUSPEND_LINEAR_ACCELERATION ); noMoveTime = NO_MOVE_TIME; noMoveTranslation = NO_MOVE_TRANSLATION_TOLERANCE; noMoveRotation = NO_MOVE_ROTATION_TOLERANCE; minMoveTime = MIN_MOVE_TIME; maxMoveTime = MAX_MOVE_TIME; impulseThreshold = IMPULSE_THRESHOLD; timeScale = 1.0f; timeScaleRampStart = 0.0f; timeScaleRampEnd = 0.0f; jointFrictionScale = 0.0f; jointFrictionDent = 0.0f; jointFrictionDentStart = 0.0f; jointFrictionDentEnd = 0.0f; jointFrictionDentScale = 0.0f; contactFrictionScale = 0.0f; contactFrictionDent = 0.0f; contactFrictionDentStart = 0.0f; contactFrictionDentEnd = 0.0f; contactFrictionDentScale = 0.0f; enableCollision = true; selfCollision = true; comeToRest = true; linearTime = true; noImpact = false; worldConstraintsLocked = false; forcePushable = false; #ifdef AF_TIMINGS lastTimerReset = 0; #endif } /* ================ idPhysics_AF::~idPhysics_AF ================ */ idPhysics_AF::~idPhysics_AF( void ) { int i; trees.DeleteContents( true ); for ( i = 0; i < bodies.Num(); i++ ) { delete bodies[i]; } for ( i = 0; i < constraints.Num(); i++ ) { delete constraints[i]; } contactConstraints.SetNum( contactConstraints.NumAllocated(), false ); for ( i = 0; i < contactConstraints.NumAllocated(); i++ ) { delete contactConstraints[i]; } delete lcp; if ( masterBody ) { delete masterBody; } } /* ================ idPhysics_AF_SavePState ================ */ void idPhysics_AF_SavePState( idSaveGame *saveFile, const AFPState_t &state ) { saveFile->WriteInt( state.atRest ); saveFile->WriteFloat( state.noMoveTime ); saveFile->WriteFloat( state.activateTime ); saveFile->WriteFloat( state.lastTimeStep ); saveFile->WriteVec6( state.pushVelocity ); } /* ================ idPhysics_AF_RestorePState ================ */ void idPhysics_AF_RestorePState( idRestoreGame *saveFile, AFPState_t &state ) { saveFile->ReadInt( state.atRest ); saveFile->ReadFloat( state.noMoveTime ); saveFile->ReadFloat( state.activateTime ); saveFile->ReadFloat( state.lastTimeStep ); saveFile->ReadVec6( state.pushVelocity ); } /* ================ idPhysics_AF::Save ================ */ void idPhysics_AF::Save( idSaveGame *saveFile ) const { int i; // the articulated figure structure is handled by the owner idPhysics_AF_SavePState( saveFile, current ); idPhysics_AF_SavePState( saveFile, saved ); saveFile->WriteInt( bodies.Num() ); for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->Save( saveFile ); } if ( masterBody ) { saveFile->WriteBool( true ); masterBody->Save( saveFile ); } else { saveFile->WriteBool( false ); } saveFile->WriteInt( constraints.Num() ); for ( i = 0; i < constraints.Num(); i++ ) { constraints[i]->Save( saveFile ); } saveFile->WriteBool( changedAF ); saveFile->WriteFloat( linearFriction ); saveFile->WriteFloat( angularFriction ); saveFile->WriteFloat( contactFriction ); saveFile->WriteFloat( bouncyness ); saveFile->WriteFloat( totalMass ); saveFile->WriteFloat( forceTotalMass ); saveFile->WriteVec2( suspendVelocity ); saveFile->WriteVec2( suspendAcceleration ); saveFile->WriteFloat( noMoveTime ); saveFile->WriteFloat( noMoveTranslation ); saveFile->WriteFloat( noMoveRotation ); saveFile->WriteFloat( minMoveTime ); saveFile->WriteFloat( maxMoveTime ); saveFile->WriteFloat( impulseThreshold ); saveFile->WriteFloat( timeScale ); saveFile->WriteFloat( timeScaleRampStart ); saveFile->WriteFloat( timeScaleRampEnd ); saveFile->WriteFloat( jointFrictionScale ); saveFile->WriteFloat( jointFrictionDent ); saveFile->WriteFloat( jointFrictionDentStart ); saveFile->WriteFloat( jointFrictionDentEnd ); saveFile->WriteFloat( jointFrictionDentScale ); saveFile->WriteFloat( contactFrictionScale ); saveFile->WriteFloat( contactFrictionDent ); saveFile->WriteFloat( contactFrictionDentStart ); saveFile->WriteFloat( contactFrictionDentEnd ); saveFile->WriteFloat( contactFrictionDentScale ); saveFile->WriteBool( enableCollision ); saveFile->WriteBool( selfCollision ); saveFile->WriteBool( comeToRest ); saveFile->WriteBool( linearTime ); saveFile->WriteBool( noImpact ); saveFile->WriteBool( worldConstraintsLocked ); saveFile->WriteBool( forcePushable ); } /* ================ idPhysics_AF::Restore ================ */ void idPhysics_AF::Restore( idRestoreGame *saveFile ) { int i, num; bool hasMaster; // the articulated figure structure should have already been restored idPhysics_AF_RestorePState( saveFile, current ); idPhysics_AF_RestorePState( saveFile, saved ); saveFile->ReadInt( num ); assert( num == bodies.Num() ); for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->Restore( saveFile ); } saveFile->ReadBool( hasMaster ); if ( hasMaster ) { masterBody = new idAFBody(); masterBody->Restore( saveFile ); } saveFile->ReadInt( num ); assert( num == constraints.Num() ); for ( i = 0; i < constraints.Num(); i++ ) { constraints[i]->Restore( saveFile ); } saveFile->ReadBool( changedAF ); saveFile->ReadFloat( linearFriction ); saveFile->ReadFloat( angularFriction ); saveFile->ReadFloat( contactFriction ); saveFile->ReadFloat( bouncyness ); saveFile->ReadFloat( totalMass ); saveFile->ReadFloat( forceTotalMass ); saveFile->ReadVec2( suspendVelocity ); saveFile->ReadVec2( suspendAcceleration ); saveFile->ReadFloat( noMoveTime ); saveFile->ReadFloat( noMoveTranslation ); saveFile->ReadFloat( noMoveRotation ); saveFile->ReadFloat( minMoveTime ); saveFile->ReadFloat( maxMoveTime ); saveFile->ReadFloat( impulseThreshold ); saveFile->ReadFloat( timeScale ); saveFile->ReadFloat( timeScaleRampStart ); saveFile->ReadFloat( timeScaleRampEnd ); saveFile->ReadFloat( jointFrictionScale ); saveFile->ReadFloat( jointFrictionDent ); saveFile->ReadFloat( jointFrictionDentStart ); saveFile->ReadFloat( jointFrictionDentEnd ); saveFile->ReadFloat( jointFrictionDentScale ); saveFile->ReadFloat( contactFrictionScale ); saveFile->ReadFloat( contactFrictionDent ); saveFile->ReadFloat( contactFrictionDentStart ); saveFile->ReadFloat( contactFrictionDentEnd ); saveFile->ReadFloat( contactFrictionDentScale ); saveFile->ReadBool( enableCollision ); saveFile->ReadBool( selfCollision ); saveFile->ReadBool( comeToRest ); saveFile->ReadBool( linearTime ); saveFile->ReadBool( noImpact ); saveFile->ReadBool( worldConstraintsLocked ); saveFile->ReadBool( forcePushable ); changedAF = true; UpdateClipModels(); } /* ================ idPhysics_AF::IsClosedLoop ================ */ bool idPhysics_AF::IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const { const idAFBody *b1, *b2; for ( b1 = body1; b1->parent; b1 = b1->parent ) { } for ( b2 = body2; b2->parent; b2 = b2->parent ) { } return ( b1 == b2 ); } /* ================ idPhysics_AF::BuildTrees ================ */ void idPhysics_AF::BuildTrees( void ) { int i; float scale; idAFBody *b; idAFConstraint *c; idAFTree *tree; primaryConstraints.Clear(); auxiliaryConstraints.Clear(); trees.DeleteContents( true ); totalMass = 0.0f; for ( i = 0; i < bodies.Num(); i++ ) { b = bodies[i]; b->parent = NULL; b->primaryConstraint = NULL; b->constraints.SetNum( 0, false ); b->children.Clear(); b->tree = NULL; totalMass += b->mass; } if ( forceTotalMass > 0.0f ) { scale = forceTotalMass / totalMass; for ( i = 0; i < bodies.Num(); i++ ) { b = bodies[i]; b->mass *= scale; b->invMass = 1.0f / b->mass; b->inertiaTensor *= scale; b->inverseInertiaTensor = b->inertiaTensor.Inverse(); } totalMass = forceTotalMass; } if ( af_useLinearTime.GetBool() ) { for ( i = 0; i < constraints.Num(); i++ ) { c = constraints[i]; c->body1->constraints.Append( c ); if ( c->body2 ) { c->body2->constraints.Append( c ); } // only bilateral constraints between two non-world bodies that do not // create loops can be used as primary constraints if ( !c->body1->primaryConstraint && c->fl.allowPrimary && c->body2 != NULL && !IsClosedLoop( c->body1, c->body2 ) ) { c->body1->primaryConstraint = c; c->body1->parent = c->body2; c->body2->children.Append( c->body1 ); c->fl.isPrimary = true; c->firstIndex = 0; primaryConstraints.Append( c ); } else { c->fl.isPrimary = false; auxiliaryConstraints.Append( c ); } } // create trees for all parent bodies for ( i = 0; i < bodies.Num(); i++ ) { if ( !bodies[i]->parent ) { tree = new idAFTree(); tree->sortedBodies.Clear(); tree->sortedBodies.Append( bodies[i] ); bodies[i]->tree = tree; trees.Append( tree ); } } // add each child body to the appropriate tree for ( i = 0; i < bodies.Num(); i++ ) { if ( bodies[i]->parent ) { for ( b = bodies[i]->parent; !b->tree; b = b->parent ) { } b->tree->sortedBodies.Append( bodies[i] ); bodies[i]->tree = b->tree; } } if ( trees.Num() > 1 ) { gameLocal.Warning( "Articulated figure has multiple seperate tree structures for entity '%s' type '%s'.", self->name.c_str(), self->GetType()->classname ); } // sort bodies in each tree to make sure parents come first for ( i = 0; i < trees.Num(); i++ ) { trees[i]->SortBodies(); } } else { // create a tree for each body for ( i = 0; i < bodies.Num(); i++ ) { tree = new idAFTree(); tree->sortedBodies.Clear(); tree->sortedBodies.Append( bodies[i] ); bodies[i]->tree = tree; trees.Append( tree ); } for ( i = 0; i < constraints.Num(); i++ ) { c = constraints[i]; c->body1->constraints.Append( c ); if ( c->body2 ) { c->body2->constraints.Append( c ); } c->fl.isPrimary = false; auxiliaryConstraints.Append( c ); } } } /* ================ idPhysics_AF::AddBody bodies get an id in the order they are added starting at zero as such the first body added will get id zero ================ */ int idPhysics_AF::AddBody( idAFBody *body ) { int id = 0; if ( !body->clipModel ) { gameLocal.Error( "idPhysics_AF::AddBody: body '%s' has no clip model.", body->name.c_str() ); } if ( bodies.Find( body ) ) { gameLocal.Error( "idPhysics_AF::AddBody: body '%s' added twice.", body->name.c_str() ); } if ( GetBody( body->name ) ) { gameLocal.Error( "idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str() ); } id = bodies.Num(); body->clipModel->SetId( id ); if ( body->linearFriction < 0.0f ) { body->linearFriction = linearFriction; body->angularFriction = angularFriction; body->contactFriction = contactFriction; } if ( body->bouncyness < 0.0f ) { body->bouncyness = bouncyness; } if ( !body->fl.clipMaskSet ) { body->clipMask = clipMask; } bodies.Append( body ); changedAF = true; return id; } /* ================ idPhysics_AF::AddConstraint ================ */ void idPhysics_AF::AddConstraint( idAFConstraint *constraint ) { if ( constraints.Find( constraint ) ) { gameLocal.Error( "idPhysics_AF::AddConstraint: constraint '%s' added twice.", constraint->name.c_str() ); } if ( GetConstraint( constraint->name ) ) { gameLocal.Error( "idPhysics_AF::AddConstraint: a constraint with the name '%s' already exists.", constraint->name.c_str() ); } if ( !constraint->body1 ) { gameLocal.Error( "idPhysics_AF::AddConstraint: body1 == NULL on constraint '%s'.", constraint->name.c_str() ); } if ( !bodies.Find( constraint->body1 ) ) { gameLocal.Error( "idPhysics_AF::AddConstraint: body1 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() ); } if ( constraint->body2 && !bodies.Find( constraint->body2 ) ) { gameLocal.Error( "idPhysics_AF::AddConstraint: body2 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() ); } if ( constraint->body1 == constraint->body2 ) { gameLocal.Error( "idPhysics_AF::AddConstraint: body1 and body2 of constraint '%s' are the same.", constraint->name.c_str() ); } constraints.Append( constraint ); constraint->physics = this; changedAF = true; } /* ================ idPhysics_AF::AddFrameConstraint ================ */ void idPhysics_AF::AddFrameConstraint( idAFConstraint *constraint ) { frameConstraints.Append( constraint ); constraint->physics = this; } /* ================ idPhysics_AF::ForceBodyId ================ */ void idPhysics_AF::ForceBodyId( idAFBody *body, int newId ) { int id; id = bodies.FindIndex( body ); if ( id == -1 ) { gameLocal.Error( "ForceBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() ); } if ( id != newId ) { idAFBody *b = bodies[newId]; bodies[newId] = bodies[id]; bodies[id] = b; changedAF = true; } } /* ================ idPhysics_AF::GetBodyId ================ */ int idPhysics_AF::GetBodyId( idAFBody *body ) const { int id; id = bodies.FindIndex( body ); if ( id == -1 && body ) { gameLocal.Error( "GetBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() ); } return id; } /* ================ idPhysics_AF::GetBodyId ================ */ int idPhysics_AF::GetBodyId( const char *bodyName ) const { int i; for ( i = 0; i < bodies.Num(); i++ ) { if ( !bodies[i]->name.Icmp( bodyName ) ) { return i; } } gameLocal.Error( "GetBodyId: no body with the name '%s' is not part of the articulated figure.\n", bodyName ); return 0; } /* ================ idPhysics_AF::GetConstraintId ================ */ int idPhysics_AF::GetConstraintId( idAFConstraint *constraint ) const { int id; id = constraints.FindIndex( constraint ); if ( id == -1 && constraint ) { gameLocal.Error( "GetConstraintId: constraint '%s' is not part of the articulated figure.\n", constraint->name.c_str() ); } return id; } /* ================ idPhysics_AF::GetConstraintId ================ */ int idPhysics_AF::GetConstraintId( const char *constraintName ) const { int i; for ( i = 0; i < constraints.Num(); i++ ) { if ( constraints[i]->name.Icmp( constraintName ) == 0 ) { return i; } } gameLocal.Error( "GetConstraintId: no constraint with the name '%s' is not part of the articulated figure.\n", constraintName ); return 0; } /* ================ idPhysics_AF::GetNumBodies ================ */ int idPhysics_AF::GetNumBodies( void ) const { return bodies.Num(); } /* ================ idPhysics_AF::GetNumConstraints ================ */ int idPhysics_AF::GetNumConstraints( void ) const { return constraints.Num(); } /* ================ idPhysics_AF::GetBody ================ */ idAFBody *idPhysics_AF::GetBody( const char *bodyName ) const { int i; for ( i = 0; i < bodies.Num(); i++ ) { if ( !bodies[i]->name.Icmp( bodyName ) ) { return bodies[i]; } } return NULL; } /* ================ idPhysics_AF::GetBody ================ */ idAFBody *idPhysics_AF::GetBody( const int id ) const { if ( id < 0 || id >= bodies.Num() ) { gameLocal.Error( "GetBody: no body with id %d exists\n", id ); return NULL; } return bodies[id]; } /* ================ idPhysics_AF::GetConstraint ================ */ idAFConstraint *idPhysics_AF::GetConstraint( const char *constraintName ) const { int i; for ( i = 0; i < constraints.Num(); i++ ) { if ( constraints[i]->name.Icmp( constraintName ) == 0 ) { return constraints[i]; } } return NULL; } /* ================ idPhysics_AF::GetConstraint ================ */ idAFConstraint *idPhysics_AF::GetConstraint( const int id ) const { if ( id < 0 || id >= constraints.Num() ) { gameLocal.Error( "GetConstraint: no constraint with id %d exists\n", id ); return NULL; } return constraints[id]; } /* ================ idPhysics_AF::DeleteBody ================ */ void idPhysics_AF::DeleteBody( const char *bodyName ) { int i; // find the body with the given name for ( i = 0; i < bodies.Num(); i++ ) { if ( !bodies[i]->name.Icmp( bodyName ) ) { break; } } if ( i >= bodies.Num() ) { gameLocal.Warning( "DeleteBody: no body found in the articulated figure with the name '%s' for entity '%s' type '%s'.", bodyName, self->name.c_str(), self->GetType()->classname ); return; } DeleteBody( i ); } /* ================ idPhysics_AF::DeleteBody ================ */ void idPhysics_AF::DeleteBody( const int id ) { int j; if ( id < 0 || id > bodies.Num() ) { gameLocal.Error( "DeleteBody: no body with id %d.", id ); return; } // remove any constraints attached to this body for ( j = 0; j < constraints.Num(); j++ ) { if ( constraints[j]->body1 == bodies[id] || constraints[j]->body2 == bodies[id] ) { delete constraints[j]; constraints.RemoveIndex( j ); j--; } } // remove the body delete bodies[id]; bodies.RemoveIndex( id ); // set new body ids for ( j = 0; j < bodies.Num(); j++ ) { bodies[j]->clipModel->SetId( j ); } changedAF = true; } /* ================ idPhysics_AF::DeleteConstraint ================ */ void idPhysics_AF::DeleteConstraint( const char *constraintName ) { int i; // find the constraint with the given name for ( i = 0; i < constraints.Num(); i++ ) { if ( !constraints[i]->name.Icmp( constraintName ) ) { break; } } if ( i >= constraints.Num() ) { gameLocal.Warning( "DeleteConstraint: no constriant found in the articulated figure with the name '%s' for entity '%s' type '%s'.", constraintName, self->name.c_str(), self->GetType()->classname ); return; } DeleteConstraint( i ); } /* ================ idPhysics_AF::DeleteConstraint ================ */ void idPhysics_AF::DeleteConstraint( const int id ) { if ( id < 0 || id >= constraints.Num() ) { gameLocal.Error( "DeleteConstraint: no constraint with id %d.", id ); return; } // remove the constraint delete constraints[id]; constraints.RemoveIndex( id ); changedAF = true; } /* ================ idPhysics_AF::GetBodyContactConstraints ================ */ int idPhysics_AF::GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const { int i, numContacts; idAFBody *body; idAFConstraint_Contact *contact; if ( id < 0 || id >= bodies.Num() || maxContacts <= 0 ) { return 0; } numContacts = 0; body = bodies[id]; for ( i = 0; i < contactConstraints.Num(); i++ ) { contact = contactConstraints[i]; if ( contact->body1 == body || contact->body2 == body ) { contacts[numContacts++] = contact; if ( numContacts >= maxContacts ) { return numContacts; } } } return numContacts; } /* ================ idPhysics_AF::SetDefaultFriction ================ */ void idPhysics_AF::SetDefaultFriction( float linear, float angular, float contact ) { if ( linear < 0.0f || linear > 1.0f || angular < 0.0f || angular > 1.0f || contact < 0.0f || contact > 1.0f ) { return; } linearFriction = linear; angularFriction = angular; contactFriction = contact; } /* ================ idPhysics_AF::GetImpactInfo ================ */ void idPhysics_AF::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const { if ( id < 0 || id >= bodies.Num() ) { memset( info, 0, sizeof( *info ) ); return; } info->invMass = 1.0f / bodies[id]->mass; info->invInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis; info->position = point - bodies[id]->current->worldOrigin; info->velocity = bodies[id]->current->spatialVelocity.SubVec3(0) + bodies[id]->current->spatialVelocity.SubVec3(1).Cross( info->position ); } /* ================ idPhysics_AF::ApplyImpulse ================ */ void idPhysics_AF::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) { if ( id < 0 || id >= bodies.Num() ) { return; } if ( noImpact || impulse.LengthSqr() < Square( impulseThreshold ) ) { return; } idMat3 invWorldInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis; bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invMass * impulse; bodies[id]->current->spatialVelocity.SubVec3(1) += invWorldInertiaTensor * (point - bodies[id]->current->worldOrigin).Cross( impulse ); Activate(); } /* ================ idPhysics_AF::AddForce ================ */ void idPhysics_AF::AddForce( const int id, const idVec3 &point, const idVec3 &force ) { if ( noImpact ) { return; } if ( id < 0 || id >= bodies.Num() ) { return; } bodies[id]->current->externalForce.SubVec3( 0 ) += force; bodies[id]->current->externalForce.SubVec3( 1 ) += (point - bodies[id]->current->worldOrigin).Cross( force ); Activate(); } /* ================ idPhysics_AF::IsAtRest ================ */ bool idPhysics_AF::IsAtRest( void ) const { return current.atRest >= 0; } /* ================ idPhysics_AF::GetRestStartTime ================ */ int idPhysics_AF::GetRestStartTime( void ) const { return current.atRest; } /* ================ idPhysics_AF::IsPushable ================ */ bool idPhysics_AF::IsPushable( void ) const { return ( !noImpact && ( masterBody == NULL || forcePushable ) ); } /* ================ idPhysics_AF::SaveState ================ */ void idPhysics_AF::SaveState( void ) { int i; saved = current; for ( i = 0; i < bodies.Num(); i++ ) { memcpy( &bodies[i]->saved, bodies[i]->current, sizeof( AFBodyPState_t ) ); } } /* ================ idPhysics_AF::RestoreState ================ */ void idPhysics_AF::RestoreState( void ) { int i; current = saved; for ( i = 0; i < bodies.Num(); i++ ) { *(bodies[i]->current) = bodies[i]->saved; } EvaluateContacts(); } /* ================ idPhysics_AF::SetOrigin ================ */ void idPhysics_AF::SetOrigin( const idVec3 &newOrigin, int id ) { if ( masterBody ) { Translate( masterBody->current->worldOrigin + masterBody->current->worldAxis * newOrigin - bodies[0]->current->worldOrigin ); } else { Translate( newOrigin - bodies[0]->current->worldOrigin ); } } /* ================ idPhysics_AF::SetAxis ================ */ void idPhysics_AF::SetAxis( const idMat3 &newAxis, int id ) { idMat3 axis; idRotation rotation; if ( masterBody ) { axis = bodies[0]->current->worldAxis.Transpose() * ( newAxis * masterBody->current->worldAxis ); } else { axis = bodies[0]->current->worldAxis.Transpose() * newAxis; } rotation = axis.ToRotation(); rotation.SetOrigin( bodies[0]->current->worldOrigin ); Rotate( rotation ); } /* ================ idPhysics_AF::Translate ================ */ void idPhysics_AF::Translate( const idVec3 &translation, int id ) { int i; idAFBody *body; if ( !worldConstraintsLocked ) { // translate constraints attached to the world for ( i = 0; i < constraints.Num(); i++ ) { constraints[i]->Translate( translation ); } } // translate all the bodies for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; body->current->worldOrigin += translation; } Activate(); UpdateClipModels(); } /* ================ idPhysics_AF::Rotate ================ */ void idPhysics_AF::Rotate( const idRotation &rotation, int id ) { int i; idAFBody *body; if ( !worldConstraintsLocked ) { // rotate constraints attached to the world for ( i = 0; i < constraints.Num(); i++ ) { constraints[i]->Rotate( rotation ); } } // rotate all the bodies for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; body->current->worldOrigin *= rotation; body->current->worldAxis *= rotation.ToMat3(); } Activate(); UpdateClipModels(); } /* ================ idPhysics_AF::GetOrigin ================ */ const idVec3 &idPhysics_AF::GetOrigin( int id ) const { if ( id < 0 || id >= bodies.Num() ) { return vec3_origin; } else { return bodies[id]->current->worldOrigin; } } /* ================ idPhysics_AF::GetAxis ================ */ const idMat3 &idPhysics_AF::GetAxis( int id ) const { if ( id < 0 || id >= bodies.Num() ) { return mat3_identity; } else { return bodies[id]->current->worldAxis; } } /* ================ idPhysics_AF::SetLinearVelocity ================ */ void idPhysics_AF::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) { if ( id < 0 || id >= bodies.Num() ) { return; } bodies[id]->current->spatialVelocity.SubVec3( 0 ) = newLinearVelocity; Activate(); } /* ================ idPhysics_AF::SetAngularVelocity ================ */ void idPhysics_AF::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) { if ( id < 0 || id >= bodies.Num() ) { return; } bodies[id]->current->spatialVelocity.SubVec3( 1 ) = newAngularVelocity; Activate(); } /* ================ idPhysics_AF::GetLinearVelocity ================ */ const idVec3 &idPhysics_AF::GetLinearVelocity( int id ) const { if ( id < 0 || id >= bodies.Num() ) { return vec3_origin; } else { return bodies[id]->current->spatialVelocity.SubVec3( 0 ); } } /* ================ idPhysics_AF::GetAngularVelocity ================ */ const idVec3 &idPhysics_AF::GetAngularVelocity( int id ) const { if ( id < 0 || id >= bodies.Num() ) { return vec3_origin; } else { return bodies[id]->current->spatialVelocity.SubVec3( 1 ); } } /* ================ idPhysics_AF::ClipTranslation ================ */ void idPhysics_AF::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const { int i; idAFBody *body; trace_t bodyResults; results.fraction = 1.0f; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body->clipModel->IsTraceModel() ) { if ( model ) { gameLocal.clip.TranslationModel( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation, body->clipModel, body->current->worldAxis, body->clipMask, model->Handle(), model->GetOrigin(), model->GetAxis() ); } else { gameLocal.clip.Translation( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation, body->clipModel, body->current->worldAxis, body->clipMask, self ); } if ( bodyResults.fraction < results.fraction ) { results = bodyResults; } } } results.endpos = bodies[0]->current->worldOrigin + results.fraction * translation; results.endAxis = bodies[0]->current->worldAxis; } /* ================ idPhysics_AF::ClipRotation ================ */ void idPhysics_AF::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const { int i; idAFBody *body; trace_t bodyResults; idRotation partialRotation; results.fraction = 1.0f; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body->clipModel->IsTraceModel() ) { if ( model ) { gameLocal.clip.RotationModel( bodyResults, body->current->worldOrigin, rotation, body->clipModel, body->current->worldAxis, body->clipMask, model->Handle(), model->GetOrigin(), model->GetAxis() ); } else { gameLocal.clip.Rotation( bodyResults, body->current->worldOrigin, rotation, body->clipModel, body->current->worldAxis, body->clipMask, self ); } if ( bodyResults.fraction < results.fraction ) { results = bodyResults; } } } partialRotation = rotation * results.fraction; results.endpos = bodies[0]->current->worldOrigin * partialRotation; results.endAxis = bodies[0]->current->worldAxis * partialRotation.ToMat3(); } /* ================ idPhysics_AF::ClipContents ================ */ int idPhysics_AF::ClipContents( const idClipModel *model ) const { int i, contents; idAFBody *body; contents = 0; for ( i = 0; i < bodies.Num(); i++ ) { body = bodies[i]; if ( body->clipModel->IsTraceModel() ) { if ( model ) { contents |= gameLocal.clip.ContentsModel( body->current->worldOrigin, body->clipModel, body->current->worldAxis, -1, model->Handle(), model->GetOrigin(), model->GetAxis() ); } else { contents |= gameLocal.clip.Contents( body->current->worldOrigin, body->clipModel, body->current->worldAxis, -1, NULL ); } } } return contents; } /* ================ idPhysics_AF::DisableClip ================ */ void idPhysics_AF::DisableClip( void ) { int i; for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->clipModel->Disable(); } } /* ================ idPhysics_AF::EnableClip ================ */ void idPhysics_AF::EnableClip( void ) { int i; for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->clipModel->Enable(); } } /* ================ idPhysics_AF::UnlinkClip ================ */ void idPhysics_AF::UnlinkClip( void ) { int i; for ( i = 0; i < bodies.Num(); i++ ) { bodies[i]->clipModel->Unlink(); } } /* ================ idPhysics_AF::LinkClip ================ */ void idPhysics_AF::LinkClip( void ) { UpdateClipModels(); } /* ================ idPhysics_AF::SetPushed ================ */ void idPhysics_AF::SetPushed( int deltaTime ) { idAFBody *body; idRotation rotation; if ( bodies.Num() ) { body = bodies[0]; rotation = ( body->saved.worldAxis.Transpose() * body->current->worldAxis ).ToRotation(); // velocity with which the af is pushed current.pushVelocity.SubVec3(0) += ( body->current->worldOrigin - body->saved.worldOrigin ) / ( deltaTime * idMath::M_MS2SEC ); current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC ); } } /* ================ idPhysics_AF::GetPushedLinearVelocity ================ */ const idVec3 &idPhysics_AF::GetPushedLinearVelocity( const int id ) const { return current.pushVelocity.SubVec3(0); } /* ================ idPhysics_AF::GetPushedAngularVelocity ================ */ const idVec3 &idPhysics_AF::GetPushedAngularVelocity( const int id ) const { return current.pushVelocity.SubVec3(1); } /* ================ idPhysics_AF::SetMaster the binding is orientated based on the constraints being used ================ */ void idPhysics_AF::SetMaster( idEntity *master, const bool orientated ) { int i; idVec3 masterOrigin; idMat3 masterAxis; idRotation rotation; if ( master ) { self->GetMasterPosition( masterOrigin, masterAxis ); if ( !masterBody ) { masterBody = new idAFBody(); // translate and rotate all the constraints with body2 == NULL from world space to master space rotation = masterAxis.Transpose().ToRotation(); for ( i = 0; i < constraints.Num(); i++ ) { if ( constraints[i]->GetBody2() == NULL ) { constraints[i]->Translate( -masterOrigin ); constraints[i]->Rotate( rotation ); } } Activate(); } masterBody->current->worldOrigin = masterOrigin; masterBody->current->worldAxis = masterAxis; } else { if ( masterBody ) { // translate and rotate all the constraints with body2 == NULL from master space to world space rotation = masterBody->current->worldAxis.ToRotation(); for ( i = 0; i < constraints.Num(); i++ ) { if ( constraints[i]->GetBody2() == NULL ) { constraints[i]->Rotate( rotation ); constraints[i]->Translate( masterBody->current->worldOrigin ); } } delete masterBody; masterBody = NULL; Activate(); } } } const float AF_VELOCITY_MAX = 16000; const int AF_VELOCITY_TOTAL_BITS = 16; const int AF_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( AF_VELOCITY_MAX ) ) + 1; const int AF_VELOCITY_MANTISSA_BITS = AF_VELOCITY_TOTAL_BITS - 1 - AF_VELOCITY_EXPONENT_BITS; //const float AF_FORCE_MAX = 1e20f; //const int AF_FORCE_TOTAL_BITS = 16; //const int AF_FORCE_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( AF_FORCE_MAX ) ) + 1; //const int AF_FORCE_MANTISSA_BITS = AF_FORCE_TOTAL_BITS - 1 - AF_FORCE_EXPONENT_BITS; /* ================ idPhysics_AF::WriteToSnapshot ================ */ void idPhysics_AF::WriteToSnapshot( idBitMsgDelta &msg ) const { int i; idCQuat quat; msg.WriteInt( current.atRest ); msg.WriteFloat( current.noMoveTime ); msg.WriteFloat( current.activateTime ); msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, current.pushVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, current.pushVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, current.pushVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteByte( bodies.Num() ); for ( i = 0; i < bodies.Num(); i++ ) { AFBodyPState_t *state = bodies[i]->current; quat = state->worldAxis.ToCQuat(); msg.WriteFloat( state->worldOrigin[0] ); msg.WriteFloat( state->worldOrigin[1] ); msg.WriteFloat( state->worldOrigin[2] ); msg.WriteFloat( quat.x ); msg.WriteFloat( quat.y ); msg.WriteFloat( quat.z ); msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); /* msg.WriteDeltaFloat( 0.0f, state->externalForce[0], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->externalForce[1], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->externalForce[2], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->externalForce[3], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->externalForce[4], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); msg.WriteDeltaFloat( 0.0f, state->externalForce[5], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); */ } } /* ================ idPhysics_AF::ReadFromSnapshot ================ */ void idPhysics_AF::ReadFromSnapshot( const idBitMsgDelta &msg ) { int i, num id_attribute((unused)); idCQuat quat; current.atRest = msg.ReadInt(); current.noMoveTime = msg.ReadFloat(); current.activateTime = msg.ReadFloat(); current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); current.pushVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); current.pushVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); current.pushVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); num = msg.ReadByte(); assert( num == bodies.Num() ); for ( i = 0; i < bodies.Num(); i++ ) { AFBodyPState_t *state = bodies[i]->current; state->worldOrigin[0] = msg.ReadFloat(); state->worldOrigin[1] = msg.ReadFloat(); state->worldOrigin[2] = msg.ReadFloat(); quat.x = msg.ReadFloat(); quat.y = msg.ReadFloat(); quat.z = msg.ReadFloat(); state->spatialVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); state->spatialVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); state->spatialVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); state->spatialVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); state->spatialVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); state->spatialVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); /* state->externalForce[0] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); state->externalForce[1] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); state->externalForce[2] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); state->externalForce[3] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); state->externalForce[4] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); state->externalForce[5] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); */ state->worldAxis = quat.ToMat3(); } UpdateClipModels(); }