2012-11-26 18:58:24 +00:00
|
|
|
/*
|
|
|
|
===========================================================================
|
|
|
|
|
|
|
|
Doom 3 BFG Edition GPL Source Code
|
2012-11-28 15:47:07 +00:00
|
|
|
Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.
|
2012-11-26 18:58:24 +00:00
|
|
|
|
2012-11-28 15:47:07 +00:00
|
|
|
This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").
|
2012-11-26 18:58:24 +00:00
|
|
|
|
|
|
|
Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
|
|
|
|
In addition, the Doom 3 BFG Edition Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 BFG Edition Source Code. If not, please request a copy in writing from id Software at the address below.
|
|
|
|
|
|
|
|
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.
|
|
|
|
|
|
|
|
===========================================================================
|
|
|
|
*/
|
|
|
|
|
|
|
|
#pragma hdrstop
|
2012-12-22 15:18:19 +00:00
|
|
|
#include "precompiled.h"
|
2012-11-26 18:58:24 +00:00
|
|
|
|
|
|
|
#include "../Game_local.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,
|
2012-11-28 15:47:07 +00:00
|
|
|
LCP_EPSILON, LCP_EPSILON, LCP_EPSILON );
|
2012-11-26 18:58:24 +00:00
|
|
|
|
|
|
|
#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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint::idAFConstraint()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_INVALID;
|
|
|
|
name = "noname";
|
|
|
|
body1 = NULL;
|
|
|
|
body2 = NULL;
|
|
|
|
physics = NULL;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo.Zero( 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
lo.SubVec6( 0 ) = -vec6_infinity;
|
2012-11-26 18:58:24 +00:00
|
|
|
hi.Zero( 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
hi.SubVec6( 0 ) = vec6_infinity;
|
2012-11-26 18:58:24 +00:00
|
|
|
e.SetSize( 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
e.SubVec6( 0 ) = vec6_lcp_epsilon;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
boxConstraint = NULL;
|
|
|
|
boxIndex[0] = -1;
|
|
|
|
boxIndex[1] = -1;
|
|
|
|
boxIndex[2] = -1;
|
|
|
|
boxIndex[3] = -1;
|
|
|
|
boxIndex[4] = -1;
|
|
|
|
boxIndex[5] = -1;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
firstIndex = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
memset( &fl, 0, sizeof( fl ) );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::~idAFConstraint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint::~idAFConstraint()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::SetBody1
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::SetBody1( idAFBody* body )
|
|
|
|
{
|
|
|
|
if( body1 != body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body1 = body;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( physics )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->SetChanged();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::SetBody2
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::SetBody2( idAFBody* body )
|
|
|
|
{
|
|
|
|
if( body2 != body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body2 = body;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( physics )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->SetChanged();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::GetMultiplier
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idVecX& idAFConstraint::GetMultiplier()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return lm;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::GetForce
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::GetForce( idAFBody* body, idVec6& force )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVecX v;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
v.SetData( 6, VECX_ALLOCA( 6 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body == body1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.TransposeMultiply( v, lm );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( body == body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.TransposeMultiply( v, lm );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
v.Zero();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
force[0] = v[0];
|
|
|
|
force[1] = v[1];
|
|
|
|
force[2] = v[2];
|
|
|
|
force[3] = v[3];
|
|
|
|
force[4] = v[4];
|
|
|
|
force[5] = v[5];
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::GetCenter( idVec3& center )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center.Zero();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::InitSize
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::InitSize( int size )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Zero( size, 6 );
|
|
|
|
J2.Zero( size, 6 );
|
|
|
|
c1.Zero( size );
|
|
|
|
c2.Zero( size );
|
|
|
|
s.Zero( size );
|
|
|
|
lm.Zero( size );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteInt( type );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraintType_t t;
|
2012-11-28 15:47:07 +00:00
|
|
|
saveFile->ReadInt( ( int& )t );
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( t == type );
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_Fixed
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::idAFConstraint_Fixed
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Fixed::idAFConstraint_Fixed( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( body1 );
|
|
|
|
type = CONSTRAINT_FIXED;
|
|
|
|
this->name = name;
|
|
|
|
this->body1 = body1;
|
|
|
|
this->body2 = body2;
|
|
|
|
InitSize( 6 );
|
|
|
|
fl.allowPrimary = true;
|
|
|
|
fl.noCollision = true;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
InitOffset();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::InitOffset
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::InitOffset()
|
|
|
|
{
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
|
|
|
|
relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset = body1->GetWorldOrigin();
|
|
|
|
relAxis = body1->GetWorldAxis();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::SetBody1
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::SetBody1( idAFBody* body )
|
|
|
|
{
|
|
|
|
if( body1 != body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body1 = body;
|
|
|
|
InitOffset();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( physics )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->SetChanged();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::SetBody2
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::SetBody2( idAFBody* body )
|
|
|
|
{
|
|
|
|
if( body2 != body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body2 = body;
|
|
|
|
InitOffset();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( physics )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->SetChanged();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 ofs, a2;
|
|
|
|
idMat3 ax;
|
|
|
|
idRotation r;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = offset * master->GetWorldAxis();
|
|
|
|
ofs = a2 + master->GetWorldOrigin();
|
|
|
|
ax = relAxis * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2.Zero();
|
|
|
|
ofs = offset;
|
|
|
|
ax = relAxis;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( mat3_identity, mat3_zero,
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, mat3_identity );
|
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( -mat3_identity, SkewSymmetric( a2 ),
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, -mat3_identity );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Zero( 6, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( ofs - body1->GetWorldOrigin() );
|
2012-11-26 18:58:24 +00:00
|
|
|
r = ( body1->GetWorldAxis().Transpose() * ax ).ToRotation();
|
2012-11-28 15:47:07 +00:00
|
|
|
c1.SubVec3( 1 ) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * -( float ) DEG2RAD( r.GetAngle() ) );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// no friction
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset += translation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset *= rotation;
|
|
|
|
relAxis *= rotation.ToMat3();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::GetCenter( idVec3& center )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center = body1->GetWorldOrigin();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::DebugDraw()
|
|
|
|
{
|
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), master->GetWorldOrigin() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), vec3_origin );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Save( saveFile );
|
|
|
|
saveFile->WriteVec3( offset );
|
|
|
|
saveFile->WriteMat3( relAxis );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Fixed::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Fixed::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Restore( saveFile );
|
|
|
|
saveFile->ReadVec3( offset );
|
|
|
|
saveFile->ReadMat3( relAxis );
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_BallAndSocketJoint
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint()
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete pyramidLimit;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::SetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::SetAnchor( const idVec3& worldPosition )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
|
|
|
|
// get anchor relative to center of mass of body1
|
|
|
|
anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body2
|
|
|
|
anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 = worldPosition;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetAnchor( anchor2 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->SetAnchor( anchor2 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::GetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idVec3 idAFConstraint_BallAndSocketJoint::GetAnchor() const
|
|
|
|
{
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
|
|
|
|
}
|
|
|
|
return anchor2;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::SetNoLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::SetNoLimit()
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
coneLimit = NULL;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete pyramidLimit;
|
|
|
|
pyramidLimit = NULL;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::SetConeLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::SetConeLimit( const idVec3& coneAxis, const float coneAngle, const idVec3& body1Axis )
|
|
|
|
{
|
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete pyramidLimit;
|
|
|
|
pyramidLimit = NULL;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !coneLimit )
|
|
|
|
{
|
|
|
|
coneLimit = new( TAG_PHYSICS_AF ) idAFConstraint_ConeLimit;
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetPhysics( physics );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, body1Axis * body1->GetWorldAxis().Transpose() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, body1Axis * body1->GetWorldAxis().Transpose() );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::SetPyramidLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::SetPyramidLimit( const idVec3& pyramidAxis, const idVec3& baseAxis,
|
|
|
|
const float angle1, const float angle2, const idVec3& body1Axis )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
coneLimit = NULL;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !pyramidLimit )
|
|
|
|
{
|
|
|
|
pyramidLimit = new( TAG_PHYSICS_AF ) idAFConstraint_PyramidLimit;
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->SetPhysics( physics );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(),
|
2012-11-28 15:47:07 +00:00
|
|
|
baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2,
|
|
|
|
body1Axis * body1->GetWorldAxis().Transpose() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2,
|
2012-11-28 15:47:07 +00:00
|
|
|
body1Axis * body1->GetWorldAxis().Transpose() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::SetLimitEpsilon
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::SetLimitEpsilon( const float e )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetEpsilon( e );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->SetEpsilon( e );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::GetFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
float idAFConstraint_BallAndSocketJoint::GetFriction() const
|
|
|
|
{
|
|
|
|
if( af_forceFriction.GetFloat() > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return af_forceFriction.GetFloat();
|
|
|
|
}
|
|
|
|
return friction * physics->GetJointFrictionScale();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 = anchor1 * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2 * master->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( anchor2 - ( a1 + body1->GetWorldOrigin() ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( mat3_identity, -SkewSymmetric( a1 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( -mat3_identity, SkewSymmetric( a2 ) );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Zero( 3, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Add( physics, invTimeStep );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Add( physics, invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 angular;
|
|
|
|
float invMass, currentFriction;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
currentFriction = GetFriction();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( currentFriction <= 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
angular = body1->GetAngularVelocity();
|
|
|
|
invMass = body1->GetInverseMass();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
angular -= body2->GetAngularVelocity();
|
|
|
|
invMass += body2->GetInverseMass();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
angular *= currentFriction / invMass;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
if( !fc )
|
|
|
|
{
|
|
|
|
fc = new( TAG_PHYSICS_AF ) idAFConstraint_BallAndSocketJointFriction;
|
2012-11-26 18:58:24 +00:00
|
|
|
fc->Setup( this );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
fc->Add( physics, invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::GetForce
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::GetForce( idAFBody* body, idVec6& force )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::GetForce( body, force );
|
|
|
|
// FIXME: add limit force
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 += translation;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Translate( translation );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Translate( translation );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 *= rotation;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Rotate( rotation );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Rotate( rotation );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::GetCenter( idVec3& center )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showLimits.GetBool() )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->DebugDraw();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->DebugDraw();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Save( saveFile );
|
|
|
|
saveFile->WriteVec3( anchor1 );
|
|
|
|
saveFile->WriteVec3( anchor2 );
|
|
|
|
saveFile->WriteFloat( friction );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Save( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Save( saveFile );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJoint::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJoint::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Restore( saveFile );
|
|
|
|
saveFile->ReadVec3( anchor1 );
|
|
|
|
saveFile->ReadVec3( anchor2 );
|
|
|
|
saveFile->ReadFloat( friction );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Restore( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Restore( saveFile );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_BallAndSocketJointFriction
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_FRICTION;
|
|
|
|
name = "ballAndSocketJointFriction";
|
|
|
|
InitSize( 3 );
|
|
|
|
joint = NULL;
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJointFriction::Setup
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJointFriction::Setup( idAFConstraint_BallAndSocketJoint* bsj )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->joint = bsj;
|
|
|
|
body1 = bsj->GetBody1();
|
|
|
|
body2 = bsj->GetBody2();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJointFriction::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJointFriction::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJointFriction::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJointFriction::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJointFriction::Add
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFConstraint_BallAndSocketJointFriction::Add( idPhysics_AF* phys, float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
float f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics = phys;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
f = joint->GetFriction() * joint->GetMultiplier().Length();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( f == 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[0] = lo[1] = lo[2] = -f;
|
|
|
|
hi[0] = hi[1] = hi[2] = f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Zero( 3, 6 );
|
|
|
|
J1[0][3] = J1[1][4] = J1[2][5] = 1.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Zero( 3, 6 );
|
|
|
|
J2[0][3] = J2[1][4] = J2[2][5] = 1.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->AddFrameConstraint( this );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJointFriction::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJointFriction::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_BallAndSocketJointFriction::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_BallAndSocketJointFriction::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_UniversalJoint
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint()
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete pyramidLimit;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( fc )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete fc;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::SetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::SetAnchor( const idVec3& worldPosition )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
|
|
|
|
// get anchor relative to center of mass of body1
|
|
|
|
anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body2
|
|
|
|
anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 = worldPosition;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetAnchor( anchor2 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->SetAnchor( anchor2 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::GetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idVec3 idAFConstraint_UniversalJoint::GetAnchor() const
|
|
|
|
{
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
|
|
|
|
}
|
|
|
|
return anchor2;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::SetShafts
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::SetShafts( const idVec3& cardanShaft1, const idVec3& cardanShaft2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 cardanAxis;
|
|
|
|
float l;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
shaft1 = cardanShaft1;
|
|
|
|
l = shaft1.Normalize();
|
|
|
|
assert( l != 0.0f );
|
|
|
|
shaft2 = cardanShaft2;
|
|
|
|
l = shaft2.Normalize();
|
|
|
|
assert( l != 0.0f );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// the cardan axis is a vector orthogonal to both cardan shafts
|
|
|
|
cardanAxis = shaft1.Cross( shaft2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( cardanAxis.Normalize() == 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 vecY;
|
|
|
|
shaft1.OrthogonalBasis( cardanAxis, vecY );
|
|
|
|
cardanAxis.Normalize();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
shaft1 *= body1->GetWorldAxis().Transpose();
|
|
|
|
axis1 = cardanAxis * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
shaft2 *= body2->GetWorldAxis().Transpose();
|
|
|
|
axis2 = cardanAxis * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis2 = cardanAxis;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetBody1Axis( shaft1 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->SetBody1Axis( shaft1 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::SetNoLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::SetNoLimit()
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
coneLimit = NULL;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete pyramidLimit;
|
|
|
|
pyramidLimit = NULL;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::SetConeLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::SetConeLimit( const idVec3& coneAxis, const float coneAngle )
|
|
|
|
{
|
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete pyramidLimit;
|
|
|
|
pyramidLimit = NULL;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !coneLimit )
|
|
|
|
{
|
|
|
|
coneLimit = new( TAG_PHYSICS_AF ) idAFConstraint_ConeLimit;
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetPhysics( physics );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, shaft1 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, shaft1 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::SetPyramidLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::SetPyramidLimit( const idVec3& pyramidAxis, const idVec3& baseAxis,
|
|
|
|
const float angle1, const float angle2 )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
coneLimit = NULL;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !pyramidLimit )
|
|
|
|
{
|
|
|
|
pyramidLimit = new( TAG_PHYSICS_AF ) idAFConstraint_PyramidLimit;
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->SetPhysics( physics );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(),
|
2012-11-28 15:47:07 +00:00
|
|
|
baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, shaft1 );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, shaft1 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::SetLimitEpsilon
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::SetLimitEpsilon( const float e )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetEpsilon( e );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->SetEpsilon( e );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::GetFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
float idAFConstraint_UniversalJoint::GetFriction() const
|
|
|
|
{
|
|
|
|
if( af_forceFriction.GetFloat() > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return af_forceFriction.GetFloat();
|
|
|
|
}
|
|
|
|
return friction * physics->GetJointFrictionScale();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::Evaluate
|
|
|
|
|
|
|
|
NOTE: this joint is homokinetic
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2, s1, s2, d1, d2, v;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 = anchor1 * body1->GetWorldAxis();
|
|
|
|
s1 = shaft1 * body1->GetWorldAxis();
|
|
|
|
d1 = s1.Cross( axis1 * body1->GetWorldAxis() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2 * master->GetWorldAxis();
|
|
|
|
s2 = shaft2 * master->GetWorldAxis();
|
|
|
|
d2 = axis2 * master->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2;
|
|
|
|
s2 = shaft2;
|
|
|
|
d2 = axis2;
|
2012-11-28 15:47:07 +00:00
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( mat3_identity, -SkewSymmetric( a1 ),
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, idMat3( s1[0], s1[1], s1[2],
|
|
|
|
0.0f, 0.0f, 0.0f,
|
|
|
|
0.0f, 0.0f, 0.0f ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 4, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( -mat3_identity, SkewSymmetric( a2 ),
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, idMat3( s2[0], s2[1], s2[2],
|
|
|
|
0.0f, 0.0f, 0.0f,
|
|
|
|
0.0f, 0.0f, 0.0f ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 4, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Zero( 4, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
v = s1.Cross( s2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( v.Normalize() != 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3 m1, m2;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
m1[0] = s1;
|
|
|
|
m1[1] = v;
|
|
|
|
m1[2] = v.Cross( m1[0] );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
m2[0] = -s2;
|
|
|
|
m2[1] = v;
|
|
|
|
m2[2] = v.Cross( m2[0] );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
d2 *= m2.Transpose() * m1;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( d1 * d2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Add( physics, invTimeStep );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Add( physics, invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 angular;
|
|
|
|
float invMass, currentFriction;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
currentFriction = GetFriction();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( currentFriction <= 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
angular = body1->GetAngularVelocity();
|
|
|
|
invMass = body1->GetInverseMass();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
angular -= body2->GetAngularVelocity();
|
|
|
|
invMass += body2->GetInverseMass();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
angular *= currentFriction / invMass;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
if( !fc )
|
|
|
|
{
|
|
|
|
fc = new( TAG_PHYSICS_AF ) idAFConstraint_UniversalJointFriction;
|
2012-11-26 18:58:24 +00:00
|
|
|
fc->Setup( this );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
fc->Add( physics, invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::GetForce
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::GetForce( idAFBody* body, idVec6& force )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::GetForce( body, force );
|
|
|
|
// FIXME: add limit force
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 += translation;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Translate( translation );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Translate( translation );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 *= rotation;
|
|
|
|
shaft2 *= rotation.ToMat3();
|
|
|
|
axis2 *= rotation.ToMat3();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Rotate( rotation );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Rotate( rotation );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::GetCenter( idVec3& center )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2, s1, s2, d1, d2, v;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
|
|
|
s1 = shaft1 * body1->GetWorldAxis();
|
|
|
|
d1 = axis1 * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
|
|
|
a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
|
2012-11-26 18:58:24 +00:00
|
|
|
s2 = shaft2 * master->GetWorldAxis();
|
|
|
|
d2 = axis2 * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
a2 = anchor2;
|
2012-11-26 18:58:24 +00:00
|
|
|
s2 = shaft2;
|
|
|
|
d2 = axis2;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
v = s1.Cross( s2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( v.Normalize() != 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3 m1, m2;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
m1[0] = s1;
|
|
|
|
m1[1] = v;
|
|
|
|
m1[2] = v.Cross( m1[0] );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
m2[0] = -s2;
|
|
|
|
m2[1] = v;
|
|
|
|
m2[2] = v.Cross( m2[0] );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
d2 *= m2.Transpose() * m1;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showLimits.GetBool() )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->DebugDraw();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->DebugDraw();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Save( saveFile );
|
|
|
|
saveFile->WriteVec3( anchor1 );
|
|
|
|
saveFile->WriteVec3( anchor2 );
|
|
|
|
saveFile->WriteVec3( shaft1 );
|
|
|
|
saveFile->WriteVec3( shaft2 );
|
|
|
|
saveFile->WriteVec3( axis1 );
|
|
|
|
saveFile->WriteVec3( axis2 );
|
|
|
|
saveFile->WriteFloat( friction );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Save( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Save( saveFile );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJoint::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJoint::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Restore( saveFile );
|
|
|
|
saveFile->ReadVec3( anchor1 );
|
|
|
|
saveFile->ReadVec3( anchor2 );
|
|
|
|
saveFile->ReadVec3( shaft1 );
|
|
|
|
saveFile->ReadVec3( shaft2 );
|
|
|
|
saveFile->ReadVec3( axis1 );
|
|
|
|
saveFile->ReadVec3( axis2 );
|
|
|
|
saveFile->ReadFloat( friction );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Restore( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( pyramidLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidLimit->Restore( saveFile );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_UniversalJointFriction
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_FRICTION;
|
|
|
|
name = "universalJointFriction";
|
|
|
|
InitSize( 2 );
|
|
|
|
joint = NULL;
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJointFriction::Setup
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJointFriction::Setup( idAFConstraint_UniversalJoint* uj )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->joint = uj;
|
|
|
|
body1 = uj->GetBody1();
|
|
|
|
body2 = uj->GetBody2();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJointFriction::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJointFriction::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJointFriction::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJointFriction::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJointFriction::Add
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFConstraint_UniversalJointFriction::Add( idPhysics_AF* phys, float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 s1, s2, dir1, dir2;
|
|
|
|
float f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics = phys;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
f = joint->GetFriction() * joint->GetMultiplier().Length();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( f == 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[0] = lo[1] = -f;
|
|
|
|
hi[0] = hi[1] = f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
joint->GetShafts( s1, s2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
s1 *= body1->GetWorldAxis();
|
|
|
|
s1.NormalVectors( dir1, dir2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 2, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
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 )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 2, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J2.SubVec6( 0 ).SubVec3( 0 ).Zero();
|
|
|
|
J2.SubVec6( 0 ).SubVec3( 1 ) = -dir1;
|
|
|
|
J2.SubVec6( 1 ).SubVec3( 0 ).Zero();
|
|
|
|
J2.SubVec6( 1 ).SubVec3( 1 ) = -dir2;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->AddFrameConstraint( this );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJointFriction::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJointFriction::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_UniversalJointFriction::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_UniversalJointFriction::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_CylindricalJoint
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_CylindricalJoint::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_CylindricalJoint::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_CylindricalJoint::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_CylindricalJoint::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_CylindricalJoint::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_CylindricalJoint::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_CylindricalJoint::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_CylindricalJoint::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_CylindricalJoint::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_CylindricalJoint::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_Hinge
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::idAFConstraint_Hinge
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Hinge::idAFConstraint_Hinge( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
initialAxis *= body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::~idAFConstraint_Hinge
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Hinge::~idAFConstraint_Hinge()
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( fc )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete fc;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( steering )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete steering;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::SetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::SetAnchor( const idVec3& worldPosition )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body1
|
|
|
|
anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body2
|
|
|
|
anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 = worldPosition;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetAnchor( anchor2 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::GetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idVec3 idAFConstraint_Hinge::GetAnchor() const
|
|
|
|
{
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2;
|
|
|
|
}
|
|
|
|
return anchor2;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::SetAxis
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::SetAxis( const idVec3& axis )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 normAxis;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
normAxis = axis;
|
|
|
|
normAxis.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// get axis relative to body1
|
|
|
|
axis1 = normAxis * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get axis relative to body2
|
|
|
|
axis2 = normAxis * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis2 = normAxis;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::GetAxis
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idVec3 idAFConstraint_Hinge::GetAxis() const
|
|
|
|
{
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return axis2 * body2->GetWorldAxis();
|
|
|
|
}
|
|
|
|
return axis2;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::SetNoLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::SetNoLimit()
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
coneLimit = NULL;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::SetLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::SetLimit( const idVec3& axis, const float angle, const idVec3& body1Axis )
|
|
|
|
{
|
|
|
|
if( !coneLimit )
|
|
|
|
{
|
|
|
|
coneLimit = new( TAG_PHYSICS_AF ) idAFConstraint_ConeLimit;
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetPhysics( physics );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Setup( body1, body2, anchor2, axis * body2->GetWorldAxis().Transpose(), angle, body1Axis * body1->GetWorldAxis().Transpose() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Setup( body1, body2, anchor2, axis, angle, body1Axis * body1->GetWorldAxis().Transpose() );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::SetLimitEpsilon
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::SetLimitEpsilon( const float e )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->SetEpsilon( e );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::GetFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
float idAFConstraint_Hinge::GetFriction() const
|
|
|
|
{
|
|
|
|
if( af_forceFriction.GetFloat() > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return af_forceFriction.GetFloat();
|
|
|
|
}
|
|
|
|
return friction * physics->GetJointFrictionScale();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::GetAngle
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
float idAFConstraint_Hinge::GetAngle() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3 axis;
|
|
|
|
idRotation rotation;
|
|
|
|
float angle;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose() * initialAxis.Transpose();
|
|
|
|
rotation = axis.ToRotation();
|
|
|
|
angle = rotation.GetAngle();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( rotation.GetVec() * axis1 < 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return -angle;
|
|
|
|
}
|
|
|
|
return angle;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::SetSteerAngle
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::SetSteerAngle( const float degrees )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete coneLimit;
|
|
|
|
coneLimit = NULL;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !steering )
|
|
|
|
{
|
|
|
|
steering = new( TAG_PHYSICS_AF ) idAFConstraint_HingeSteering();
|
2012-11-26 18:58:24 +00:00
|
|
|
steering->Setup( this );
|
|
|
|
}
|
|
|
|
steering->SetSteerAngle( degrees );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::SetSteerSpeed
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::SetSteerSpeed( const float speed )
|
|
|
|
{
|
|
|
|
if( steering )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
steering->SetSteerSpeed( speed );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2;
|
|
|
|
idVec3 x1, x2, cross;
|
|
|
|
idVec3 vecX, vecY;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
x1 = axis1 * body1->GetWorldAxis(); // axis in body1 space
|
|
|
|
x1.OrthogonalBasis( vecX, vecY ); // basis for axis in body1 space
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 = anchor1 * body1->GetWorldAxis(); // anchor in body1 space
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2 * master->GetWorldAxis(); // anchor in master space
|
|
|
|
x2 = axis2 * master->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2;
|
|
|
|
x2 = axis2;
|
2012-11-28 15:47:07 +00:00
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( mat3_identity, -SkewSymmetric( a1 ),
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, idMat3( vecX[0], vecX[1], vecX[2],
|
|
|
|
vecY[0], vecY[1], vecY[2],
|
|
|
|
0.0f, 0.0f, 0.0f ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 5, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( -mat3_identity, SkewSymmetric( a2 ),
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, idMat3( -vecX[0], -vecX[1], -vecX[2],
|
|
|
|
-vecY[0], -vecY[1], -vecY[2],
|
|
|
|
0.0f, 0.0f, 0.0f ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 5, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Zero( 5, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
cross = x1.Cross( x2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecX );
|
|
|
|
c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecY );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( steering )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
steering->Add( physics, invTimeStep );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Add( physics, invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 angular;
|
|
|
|
float invMass, currentFriction;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
currentFriction = GetFriction();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( currentFriction <= 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
angular = body1->GetAngularVelocity();
|
|
|
|
invMass = body1->GetInverseMass();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
angular -= body2->GetAngularVelocity();
|
|
|
|
invMass += body2->GetInverseMass();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
angular *= currentFriction / invMass;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
if( !fc )
|
|
|
|
{
|
|
|
|
fc = new( TAG_PHYSICS_AF ) idAFConstraint_HingeFriction;
|
2012-11-26 18:58:24 +00:00
|
|
|
fc->Setup( this );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
fc->Add( physics, invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::GetForce
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::GetForce( idAFBody* body, idVec6& force )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::GetForce( body, force );
|
|
|
|
// FIXME: add limit force
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 += translation;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Translate( translation );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 *= rotation;
|
|
|
|
axis2 *= rotation.ToMat3();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->Rotate( rotation );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::GetCenter( idVec3& center )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 vecX, vecY;
|
|
|
|
idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
|
|
|
idVec3 x1 = axis1 * body1->GetWorldAxis();
|
|
|
|
x1.OrthogonalBasis( vecX, vecY );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showLimits.GetBool() )
|
|
|
|
{
|
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneLimit->DebugDraw();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Save( saveFile );
|
|
|
|
saveFile->WriteVec3( anchor1 );
|
|
|
|
saveFile->WriteVec3( anchor2 );
|
|
|
|
saveFile->WriteVec3( axis1 );
|
|
|
|
saveFile->WriteVec3( axis2 );
|
|
|
|
saveFile->WriteMat3( initialAxis );
|
|
|
|
saveFile->WriteFloat( friction );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( coneLimit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( true );
|
|
|
|
coneLimit->Save( saveFile );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( false );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( steering )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( true );
|
|
|
|
steering->Save( saveFile );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( false );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( fc )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( true );
|
|
|
|
fc->Save( saveFile );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( false );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Hinge::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Hinge::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bool b;
|
|
|
|
idAFConstraint::Restore( saveFile );
|
|
|
|
saveFile->ReadVec3( anchor1 );
|
|
|
|
saveFile->ReadVec3( anchor2 );
|
|
|
|
saveFile->ReadVec3( axis1 );
|
|
|
|
saveFile->ReadVec3( axis2 );
|
|
|
|
saveFile->ReadMat3( initialAxis );
|
|
|
|
saveFile->ReadFloat( friction );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadBool( b );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( b )
|
|
|
|
{
|
|
|
|
if( !coneLimit )
|
|
|
|
{
|
|
|
|
coneLimit = new( TAG_PHYSICS_AF ) idAFConstraint_ConeLimit;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
coneLimit->SetPhysics( physics );
|
|
|
|
coneLimit->Restore( saveFile );
|
|
|
|
}
|
|
|
|
saveFile->ReadBool( b );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( b )
|
|
|
|
{
|
|
|
|
if( !steering )
|
|
|
|
{
|
|
|
|
steering = new( TAG_PHYSICS_AF ) idAFConstraint_HingeSteering;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
steering->Setup( this );
|
|
|
|
steering->Restore( saveFile );
|
|
|
|
}
|
|
|
|
saveFile->ReadBool( b );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( b )
|
|
|
|
{
|
|
|
|
if( !fc )
|
|
|
|
{
|
|
|
|
fc = new( TAG_PHYSICS_AF ) idAFConstraint_HingeFriction;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
fc->Setup( this );
|
|
|
|
fc->Restore( saveFile );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_HingeFriction
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeFriction::idAFConstraint_HingeFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_HingeFriction::idAFConstraint_HingeFriction()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_FRICTION;
|
|
|
|
name = "hingeFriction";
|
|
|
|
InitSize( 1 );
|
|
|
|
hinge = NULL;
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeFriction::Setup
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeFriction::Setup( idAFConstraint_Hinge* h )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->hinge = h;
|
|
|
|
body1 = h->GetBody1();
|
|
|
|
body2 = h->GetBody2();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeFriction::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeFriction::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeFriction::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeFriction::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeFriction::Add
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFConstraint_HingeFriction::Add( idPhysics_AF* phys, float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2;
|
|
|
|
float f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics = phys;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
f = hinge->GetFriction() * hinge->GetMultiplier().Length();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( f == 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[0] = -f;
|
|
|
|
hi[0] = f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
hinge->GetAxis( a1, a2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 *= body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 1, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J1.SubVec6( 0 ).SubVec3( 0 ).Zero();
|
|
|
|
J1.SubVec6( 0 ).SubVec3( 1 ) = a1;
|
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 *= body2->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 1, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J2.SubVec6( 0 ).SubVec3( 0 ).Zero();
|
|
|
|
J2.SubVec6( 0 ).SubVec3( 1 ) = -a2;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->AddFrameConstraint( this );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeFriction::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeFriction::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeFriction::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeFriction::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_HingeSteering
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::idAFConstraint_HingeSteering
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_HingeSteering::idAFConstraint_HingeSteering()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_HINGESTEERING;
|
|
|
|
name = "hingeFriction";
|
|
|
|
InitSize( 1 );
|
|
|
|
hinge = NULL;
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
|
|
|
steerSpeed = 0.0f;
|
|
|
|
epsilon = LCP_EPSILON;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeSteering::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
|
|
|
saveFile->WriteFloat( steerAngle );
|
|
|
|
saveFile->WriteFloat( steerSpeed );
|
|
|
|
saveFile->WriteFloat( epsilon );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeSteering::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
|
|
|
saveFile->ReadFloat( steerAngle );
|
|
|
|
saveFile->ReadFloat( steerSpeed );
|
|
|
|
saveFile->ReadFloat( epsilon );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::Setup
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeSteering::Setup( idAFConstraint_Hinge* h )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->hinge = h;
|
|
|
|
body1 = h->GetBody1();
|
|
|
|
body2 = h->GetBody2();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeSteering::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeSteering::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::Add
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFConstraint_HingeSteering::Add( idPhysics_AF* phys, float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
float angle, speed;
|
|
|
|
idVec3 a1, a2;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics = phys;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
hinge->GetAxis( a1, a2 );
|
|
|
|
angle = hinge->GetAngle();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 *= body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 1, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J1.SubVec6( 0 ).SubVec3( 0 ).Zero();
|
|
|
|
J1.SubVec6( 0 ).SubVec3( 1 ) = a1;
|
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 *= body2->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 1, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J2.SubVec6( 0 ).SubVec3( 0 ).Zero();
|
|
|
|
J2.SubVec6( 0 ).SubVec3( 1 ) = -a2;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
speed = steerAngle - angle;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( steerSpeed != 0.0f )
|
|
|
|
{
|
|
|
|
if( speed > steerSpeed )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
speed = steerSpeed;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( speed < -steerSpeed )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
speed = -steerSpeed;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[0] = DEG2RAD( speed ) * invTimeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->AddFrameConstraint( this );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeSteering::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_HingeSteering::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_HingeSteering::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_Slider
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::idAFConstraint_Slider
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Slider::idAFConstraint_Slider( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( body1 );
|
|
|
|
type = CONSTRAINT_SLIDER;
|
|
|
|
this->name = name;
|
|
|
|
this->body1 = body1;
|
|
|
|
this->body2 = body2;
|
|
|
|
InitSize( 5 );
|
|
|
|
fl.allowPrimary = true;
|
|
|
|
fl.noCollision = true;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
|
|
|
|
relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset = body1->GetWorldOrigin();
|
|
|
|
relAxis = body1->GetWorldAxis();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::SetAxis
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::SetAxis( const idVec3& ax )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 normAxis;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// get normalized axis relative to body1
|
|
|
|
normAxis = ax;
|
|
|
|
normAxis.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = normAxis * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = normAxis;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 vecX, vecY, ofs;
|
|
|
|
idRotation r;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
|
|
|
( axis * master->GetWorldAxis() ).OrthogonalBasis( vecX, vecY );
|
2012-11-26 18:58:24 +00:00
|
|
|
ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
r = ( body1->GetWorldAxis().Transpose() * ( relAxis * master->GetWorldAxis() ) ).ToRotation();
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis.OrthogonalBasis( vecX, vecY );
|
|
|
|
ofs = offset - body1->GetWorldOrigin();
|
|
|
|
r = ( body1->GetWorldAxis().Transpose() * relAxis ).ToRotation();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( mat3_zero, mat3_identity,
|
|
|
|
idMat3( vecX, vecY, vec3_origin ), mat3_zero );
|
|
|
|
J1.SetSize( 5, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
|
|
|
|
|
|
|
J2.Set( mat3_zero, -mat3_identity,
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3( -vecX, -vecY, vec3_origin ), mat3_zero );
|
|
|
|
J2.SetSize( 5, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Zero( 5, 6 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
c1.SubVec3( 0 ) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * - ( float ) DEG2RAD( r.GetAngle() ) );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( vecX * ofs );
|
|
|
|
c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( vecY * ofs );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// no friction
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset += translation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
offset *= rotation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::GetCenter( idVec3& center )
|
|
|
|
{
|
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center = offset - body1->GetWorldOrigin();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 ofs;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ofs = offset - body1->GetWorldOrigin();
|
|
|
|
}
|
|
|
|
gameRenderWorld->DebugLine( colorGreen, ofs, ofs + axis * body1->GetWorldAxis() );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Save( saveFile );
|
|
|
|
saveFile->WriteVec3( axis );
|
|
|
|
saveFile->WriteVec3( offset );
|
|
|
|
saveFile->WriteMat3( relAxis );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Slider::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Slider::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Restore( saveFile );
|
|
|
|
saveFile->ReadVec3( axis );
|
|
|
|
saveFile->ReadVec3( offset );
|
|
|
|
saveFile->ReadMat3( relAxis );
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_Line
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Line::idAFConstraint_Line
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Line::idAFConstraint_Line( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Line::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Line::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Line::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Line::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Line::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Line::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Line::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Line::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Line::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Line::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // FIXME: implement
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_Plane
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::idAFConstraint_Plane
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Plane::idAFConstraint_Plane( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::SetPlane( const idVec3& normal, const idVec3& anchor )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body1
|
|
|
|
anchor1 = ( anchor - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body2
|
|
|
|
anchor2 = ( anchor - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
|
|
|
|
planeNormal = normal * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 = anchor;
|
|
|
|
planeNormal = normal;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2, normal, p;
|
|
|
|
idVec6 v;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
|
|
|
|
normal = planeNormal * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2;
|
|
|
|
normal = planeNormal;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
p = a1 - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
v.SubVec3( 0 ) = normal;
|
|
|
|
v.SubVec3( 1 ) = p.Cross( normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( 1, 6, v.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
p = a1 - body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
v.SubVec3( 0 ) = -normal;
|
|
|
|
v.SubVec3( 1 ) = p.Cross( -normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( 1, 6, v.ToFloatPtr() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
c1[0] = -( invTimeStep * ERROR_REDUCTION ) * ( a1 * normal - a2 * normal );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// no friction
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 += translation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 *= rotation;
|
|
|
|
planeNormal *= rotation.ToMat3();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, normal, right, up;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
normal = planeNormal * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
normal = planeNormal;
|
|
|
|
}
|
|
|
|
normal.NormalVectors( right, up );
|
|
|
|
normal *= 4.0f;
|
|
|
|
right *= 4.0f;
|
|
|
|
up *= 4.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DebugLine( colorCyan, a1 - right, a1 + right );
|
|
|
|
gameRenderWorld->DebugLine( colorCyan, a1 - up, a1 + up );
|
|
|
|
gameRenderWorld->DebugArrow( colorCyan, a1, a1 + normal, 1 );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Save( saveFile );
|
|
|
|
saveFile->WriteVec3( anchor1 );
|
|
|
|
saveFile->WriteVec3( anchor2 );
|
|
|
|
saveFile->WriteVec3( planeNormal );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Plane::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Plane::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idAFConstraint::Restore( saveFile );
|
|
|
|
saveFile->ReadVec3( anchor1 );
|
|
|
|
saveFile->ReadVec3( anchor2 );
|
|
|
|
saveFile->ReadVec3( planeNormal );
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_Spring
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::idAFConstraint_Spring
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Spring::idAFConstraint_Spring( const idStr& name, idAFBody* body1, idAFBody* body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::SetAnchor( const idVec3& worldAnchor1, const idVec3& worldAnchor2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body1
|
|
|
|
anchor1 = ( worldAnchor1 - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get anchor relative to center of mass of body2
|
|
|
|
anchor2 = ( worldAnchor2 - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 = worldAnchor2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::SetSpring
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::SetSpring( const float stretch, const float compress, const float damping, const float restLength )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::SetLimit( const float minLength, const float maxLength )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( minLength >= 0.0f && maxLength >= 0.0f && maxLength >= minLength );
|
|
|
|
this->minLength = minLength;
|
|
|
|
this->maxLength = maxLength;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2, velocity1, velocity2, force;
|
|
|
|
idVec6 v1, v2;
|
|
|
|
float d, dampingForce, length, error;
|
|
|
|
bool limit;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
|
|
|
velocity1 = body1->GetPointVelocity( a1 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
|
|
|
|
velocity2 = master->GetPointVelocity( a2 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2;
|
|
|
|
velocity2.Zero();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
force = a2 - a1;
|
|
|
|
d = force * force;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( d != 0.0f )
|
|
|
|
{
|
|
|
|
dampingForce = damping * idMath::Fabs( ( velocity2 - velocity1 ) * force ) / d;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
dampingForce = 0.0f;
|
|
|
|
}
|
|
|
|
length = force.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( length > restLength )
|
|
|
|
{
|
|
|
|
if( kstretch > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 springForce = force * ( Square( length - restLength ) * kstretch - dampingForce );
|
|
|
|
body1->AddForce( a1, springForce );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
master->AddForce( a2, -springForce );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
if( kcompress > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 springForce = force * -( Square( restLength - length ) * kcompress - dampingForce );
|
|
|
|
body1->AddForce( a1, springForce );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
master->AddForce( a2, -springForce );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// check for spring limits
|
2012-11-28 15:47:07 +00:00
|
|
|
if( length < minLength )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
force = -force;
|
|
|
|
error = minLength - length;
|
|
|
|
limit = true;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( maxLength > 0.0f && length > maxLength )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
error = length - maxLength;
|
|
|
|
limit = true;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
error = 0.0f;
|
|
|
|
limit = false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( limit )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a1 -= body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
v1.SubVec3( 0 ) = force;
|
|
|
|
v1.SubVec3( 1 ) = a1.Cross( force );
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( 1, 6, v1.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 -= body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
v2.SubVec3( 0 ) = -force;
|
|
|
|
v2.SubVec3( 1 ) = a2.Cross( -force );
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( 1, 6, v2.ToFloatPtr() );
|
|
|
|
}
|
|
|
|
c1[0] = -( invTimeStep * ERROR_REDUCTION ) * error;
|
|
|
|
lo[0] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Zero( 0, 0 );
|
|
|
|
J2.Zero( 0, 0 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// no friction
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 += translation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
anchor2 *= rotation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::GetCenter( idVec3& center )
|
|
|
|
{
|
|
|
|
idAFBody* master;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 a1, a2;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
|
|
|
a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2;
|
|
|
|
}
|
|
|
|
center = ( a1 + a2 ) * 0.5f;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Spring::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::DebugDraw()
|
|
|
|
{
|
|
|
|
idAFBody* master;
|
2012-11-26 18:58:24 +00:00
|
|
|
float length;
|
|
|
|
idVec3 a1, a2, dir, mid, p;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
|
|
|
a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
a2 = anchor2;
|
|
|
|
}
|
|
|
|
dir = a2 - a1;
|
|
|
|
mid = a1 + 0.5f * dir;
|
|
|
|
length = dir.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// draw spring
|
|
|
|
gameRenderWorld->DebugLine( colorGreen, a1, a2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( restLength > length )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DebugLine( colorWhite, a2, mid + p );
|
|
|
|
gameRenderWorld->DebugLine( colorWhite, a1, mid - p );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( minLength > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( maxLength > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Spring::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Contact::idAFConstraint_Contact()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
name = "contact";
|
|
|
|
type = CONSTRAINT_CONTACT;
|
|
|
|
InitSize( 1 );
|
|
|
|
fc = NULL;
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::~idAFConstraint_Contact
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Contact::~idAFConstraint_Contact()
|
|
|
|
{
|
|
|
|
if( fc )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete fc;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::Setup
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Contact::Setup( idAFBody* b1, idAFBody* b2, contactInfo_t& c )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 p;
|
|
|
|
idVec6 v;
|
|
|
|
float vel;
|
|
|
|
float minBounceVelocity = 2.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( b1 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body1 = b1;
|
|
|
|
body2 = b2;
|
|
|
|
contact = c;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
p = c.point - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
v.SubVec3( 0 ) = c.normal;
|
|
|
|
v.SubVec3( 1 ) = p.Cross( c.normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( 1, 6, v.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
vel = v.SubVec3( 0 ) * body1->GetLinearVelocity() + v.SubVec3( 1 ) * body1->GetAngularVelocity();
|
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
p = c.point - body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
v.SubVec3( 0 ) = -c.normal;
|
|
|
|
v.SubVec3( 1 ) = p.Cross( -c.normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( 1, 6, v.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
vel += v.SubVec3( 0 ) * body2->GetLinearVelocity() + v.SubVec3( 1 ) * body2->GetAngularVelocity();
|
2012-11-26 18:58:24 +00:00
|
|
|
c2[0] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body1->GetBouncyness() > 0.0f && -vel > minBounceVelocity )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[0] = body1->GetBouncyness() * vel;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[0] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
e[0] = CONTACT_LCP_EPSILON;
|
|
|
|
lo[0] = 0.0f;
|
|
|
|
hi[0] = idMath::INFINITY;
|
|
|
|
boxConstraint = NULL;
|
|
|
|
boxIndex[0] = -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Contact::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Contact::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 r, velocity, normal, dir1, dir2;
|
|
|
|
float friction, magnitude, forceNumerator, forceDenominator;
|
|
|
|
idVecX impulse, dv;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( !body1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
friction = body1->GetContactFriction();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 && body2->GetContactFriction() < friction )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
friction = body2->GetContactFriction();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
friction *= physics->GetContactFrictionScale();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( friction <= 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// seperate friction per contact is silly but it's fast and often looks close enough
|
2012-11-28 15:47:07 +00:00
|
|
|
if( af_useImpulseFriction.GetBool() )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
impulse.SetData( 6, VECX_ALLOCA( 6 ) );
|
|
|
|
dv.SetData( 6, VECX_ALLOCA( 6 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate velocity in the contact plane
|
|
|
|
r = contact.point - body1->GetWorldOrigin();
|
|
|
|
velocity = body1->GetLinearVelocity() + body1->GetAngularVelocity().Cross( r );
|
|
|
|
velocity -= contact.normal * velocity * contact.normal;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// get normalized direction of friction and magnitude of velocity
|
|
|
|
normal = -velocity;
|
|
|
|
magnitude = normal.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
forceNumerator = friction * magnitude;
|
|
|
|
forceDenominator = body1->GetInverseMass() + ( ( body1->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
|
2012-11-28 15:47:07 +00:00
|
|
|
impulse.SubVec3( 0 ) = ( forceNumerator / forceDenominator ) * normal;
|
|
|
|
impulse.SubVec3( 1 ) = r.Cross( impulse.SubVec3( 0 ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
body1->InverseWorldSpatialInertiaMultiply( dv, impulse.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// modify velocity with friction force
|
2012-11-28 15:47:07 +00:00
|
|
|
body1->SetLinearVelocity( body1->GetLinearVelocity() + dv.SubVec3( 0 ) );
|
|
|
|
body1->SetAngularVelocity( body1->GetAngularVelocity() + dv.SubVec3( 1 ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
|
|
|
|
if( !fc )
|
|
|
|
{
|
|
|
|
fc = new( TAG_PHYSICS_AF ) idAFConstraint_ContactFriction;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
// call setup each frame because contact constraints are re-used for different bodies
|
|
|
|
fc->Setup( this );
|
|
|
|
fc->Add( physics, invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Contact::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // contact should never be translated
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Contact::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
assert( 0 ); // contact should never be rotated
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::GetCenter
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Contact::GetCenter( idVec3& center )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
center = contact.point;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Contact::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Contact::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_ContactFriction::idAFConstraint_ContactFriction()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_FRICTION;
|
|
|
|
name = "contactFriction";
|
|
|
|
InitSize( 2 );
|
|
|
|
cc = NULL;
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ContactFriction::Setup
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ContactFriction::Setup( idAFConstraint_Contact* cc )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->cc = cc;
|
|
|
|
body1 = cc->GetBody1();
|
|
|
|
body2 = cc->GetBody2();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ContactFriction::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ContactFriction::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ContactFriction::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ContactFriction::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ContactFriction::Add
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFConstraint_ContactFriction::Add( idPhysics_AF* phys, float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 r, dir1, dir2;
|
|
|
|
float friction;
|
|
|
|
int newRow;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics = phys;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
friction = body1->GetContactFriction() * physics->GetContactFrictionScale();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the body only has friction in one direction
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body1->GetFrictionDirection( dir1 ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// project the friction direction into the contact plane
|
|
|
|
dir1 -= dir1 * cc->GetContact().normal * dir1;
|
|
|
|
dir1.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
r = cc->GetContact().point - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 1, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J1.SubVec6( 0 ).SubVec3( 0 ) = dir1;
|
|
|
|
J1.SubVec6( 0 ).SubVec3( 1 ) = r.Cross( dir1 );
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.SetSize( 1 );
|
|
|
|
c1[0] = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
r = cc->GetContact().point - body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 1, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J2.SubVec6( 0 ).SubVec3( 0 ) = -dir1;
|
|
|
|
J2.SubVec6( 0 ).SubVec3( 1 ) = r.Cross( -dir1 );
|
2012-11-26 18:58:24 +00:00
|
|
|
c2.SetSize( 1 );
|
|
|
|
c2[0] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[0] = -friction;
|
|
|
|
hi[0] = friction;
|
|
|
|
boxConstraint = cc;
|
|
|
|
boxIndex[0] = 0;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// get two friction directions orthogonal to contact normal
|
|
|
|
cc->GetContact().normal.NormalVectors( dir1, dir2 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
r = cc->GetContact().point - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 2, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
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 );
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.SetSize( 2 );
|
|
|
|
c1[0] = c1[1] = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
r = cc->GetContact().point - body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 2, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
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 );
|
2012-11-26 18:58:24 +00:00
|
|
|
c2.SetSize( 2 );
|
|
|
|
c2[0] = c2[1] = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2->GetContactFriction() < friction )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
friction = body2->GetContactFriction();
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[0] = -friction;
|
|
|
|
hi[0] = friction;
|
|
|
|
boxConstraint = cc;
|
|
|
|
boxIndex[0] = 0;
|
|
|
|
lo[1] = -friction;
|
|
|
|
hi[1] = friction;
|
|
|
|
boxIndex[1] = 0;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body1->GetContactMotorDirection( dir1 ) && body1->GetContactMotorForce() > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// project the motor force direction into the contact plane
|
|
|
|
dir1 -= dir1 * cc->GetContact().normal * dir1;
|
|
|
|
dir1.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
r = cc->GetContact().point - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
newRow = J1.GetNumRows();
|
2012-11-28 15:47:07 +00:00
|
|
|
J1.ChangeSize( newRow + 1, J1.GetNumColumns() );
|
|
|
|
J1.SubVec6( newRow ).SubVec3( 0 ) = -dir1;
|
|
|
|
J1.SubVec6( newRow ).SubVec3( 1 ) = r.Cross( -dir1 );
|
|
|
|
c1.ChangeSize( newRow + 1 );
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[newRow] = body1->GetContactMotorVelocity();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
r = cc->GetContact().point - body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
J2.ChangeSize( newRow + 1, J2.GetNumColumns() );
|
|
|
|
J2.SubVec6( newRow ).SubVec3( 0 ) = -dir1;
|
|
|
|
J2.SubVec6( newRow ).SubVec3( 1 ) = r.Cross( -dir1 );
|
|
|
|
c2.ChangeSize( newRow + 1 );
|
2012-11-26 18:58:24 +00:00
|
|
|
c2[newRow] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[newRow] = -body1->GetContactMotorForce();
|
|
|
|
hi[newRow] = body1->GetContactMotorForce();
|
|
|
|
boxIndex[newRow] = -1;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->AddFrameConstraint( this );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ContactFriction::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ContactFriction::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ContactFriction::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ContactFriction::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ContactFriction::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ContactFriction::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
//
|
|
|
|
// idAFConstraint_ConeLimit
|
|
|
|
//
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::idAFConstraint_ConeLimit
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_ConeLimit::idAFConstraint_ConeLimit()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::Setup( idAFBody* b1, idAFBody* b2, const idVec3& coneAnchor, const idVec3& coneAxis, const float coneAngle, const idVec3& body1Axis )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->body1 = b1;
|
|
|
|
this->body2 = b2;
|
|
|
|
this->coneAxis = coneAxis;
|
|
|
|
this->coneAxis.Normalize();
|
|
|
|
this->coneAnchor = coneAnchor;
|
|
|
|
this->body1Axis = body1Axis;
|
|
|
|
this->body1Axis.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
this->cosAngle = ( float ) cos( DEG2RAD( coneAngle * 0.5f ) );
|
|
|
|
this->sinHalfAngle = ( float ) sin( DEG2RAD( coneAngle * 0.25f ) );
|
|
|
|
this->cosHalfAngle = ( float ) cos( DEG2RAD( coneAngle * 0.25f ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::SetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::SetAnchor( const idVec3& coneAnchor )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->coneAnchor = coneAnchor;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::SetBody1Axis
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::SetBody1Axis( const idVec3& body1Axis )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->body1Axis = body1Axis;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::Add
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFConstraint_ConeLimit::Add( idPhysics_AF* phys, float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
float a;
|
|
|
|
idVec6 J1row, J2row;
|
|
|
|
idVec3 ax, anchor, body1ax, normal, coneVector, p1, p2;
|
|
|
|
idQuat q;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
|
|
|
if( af_skipLimits.GetBool() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
lm.Zero(); // constraint exerts no force
|
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics = phys;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ax = coneAxis * master->GetWorldAxis();
|
|
|
|
anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ax = coneAxis;
|
|
|
|
anchor = coneAnchor;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body1ax = body1Axis * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
a = ax * body1ax;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the body1 axis is inside the cone
|
2012-11-28 15:47:07 +00:00
|
|
|
if( a > cosAngle )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
lm.Zero(); // constraint exerts no force
|
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
p1 = anchor + 32.0f * coneVector - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
J1row.SubVec3( 0 ) = normal;
|
|
|
|
J1row.SubVec3( 1 ) = p1.Cross( normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( 1, 6, J1row.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
c1[0] = ( invTimeStep * LIMIT_ERROR_REDUCTION ) * ( normal * ( 32.0f * body1ax ) );
|
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
p2 = anchor + 32.0f * coneVector - master->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
J2row.SubVec3( 0 ) = -normal;
|
|
|
|
J2row.SubVec3( 1 ) = p2.Cross( -normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( 1, 6, J2row.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c2[0] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[0] = 0.0f;
|
|
|
|
e[0] = LIMIT_LCP_EPSILON;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->AddFrameConstraint( this );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneAnchor += translation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
coneAnchor *= rotation;
|
|
|
|
coneAxis *= rotation.ToMat3();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 ax, anchor, x, y, z, start, end;
|
|
|
|
float sinAngle, a, size = 10.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ax = coneAxis * master->GetWorldAxis();
|
|
|
|
anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ax = coneAxis;
|
|
|
|
anchor = coneAnchor;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// draw body1 axis
|
2012-11-28 15:47:07 +00:00
|
|
|
gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * ( body1Axis * body1->GetWorldAxis() ) );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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;
|
2012-11-28 15:47:07 +00:00
|
|
|
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;
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DebugLine( colorMagenta, anchor, start );
|
|
|
|
gameRenderWorld->DebugLine( colorMagenta, start, end );
|
|
|
|
start = end;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_ConeLimit::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_ConeLimit::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_PYRAMIDLIMIT;
|
|
|
|
name = "pyramidLimit";
|
|
|
|
InitSize( 1 );
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::Setup
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
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 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
2012-11-28 15:47:07 +00:00
|
|
|
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 ) );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
this->body1Axis = body1Axis;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::SetAnchor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::SetAnchor( const idVec3& pyramidAnchor )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->pyramidAnchor = pyramidAnchor;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::SetBody1Axis
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::SetBody1Axis( const idVec3& body1Axis )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->body1Axis = body1Axis;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::Add
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFConstraint_PyramidLimit::Add( idPhysics_AF* phys, float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
float a[2];
|
|
|
|
idVec6 J1row, J2row;
|
|
|
|
idMat3 worldBase;
|
|
|
|
idVec3 anchor, body1ax, ax[2], v, normal, pyramidVector, p1, p2;
|
|
|
|
idQuat q;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
|
|
|
if( af_skipLimits.GetBool() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
lm.Zero(); // constraint exerts no force
|
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics = phys;
|
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
|
|
|
|
worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
|
|
|
|
worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
|
|
|
|
anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
worldBase = pyramidBasis;
|
|
|
|
anchor = pyramidAnchor;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body1ax = body1Axis * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < 2; i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ax[i] = body1ax - worldBase[!i] * body1ax * worldBase[!i];
|
|
|
|
ax[i].Normalize();
|
|
|
|
a[i] = worldBase[2] * ax[i];
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the body1 axis is inside the pyramid
|
2012-11-28 15:47:07 +00:00
|
|
|
if( a[0] > cosAngle[0] && a[1] > cosAngle[1] )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
lm.Zero(); // constraint exerts no force
|
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate the inward pyramid normal for the position the body1 axis went outside the pyramid
|
|
|
|
pyramidVector = worldBase[2];
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < 2; i++ )
|
|
|
|
{
|
|
|
|
if( a[i] <= cosAngle[i] )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
p1 = anchor + 32.0f * pyramidVector - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
J1row.SubVec3( 0 ) = normal;
|
|
|
|
J1row.SubVec3( 1 ) = p1.Cross( normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.Set( 1, 6, J1row.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
c1[0] = ( invTimeStep * LIMIT_ERROR_REDUCTION ) * ( normal * ( 32.0f * body1ax ) );
|
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
p2 = anchor + 32.0f * pyramidVector - master->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
J2row.SubVec3( 0 ) = -normal;
|
|
|
|
J2row.SubVec3( 1 ) = p2.Cross( -normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.Set( 1, 6, J2row.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c2[0] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[0] = 0.0f;
|
|
|
|
e[0] = LIMIT_LCP_EPSILON;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
physics->AddFrameConstraint( this );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::Translate( const idVec3& translation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidAnchor += translation;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::Rotate( const idRotation& rotation )
|
|
|
|
{
|
|
|
|
if( !body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
pyramidAnchor *= rotation;
|
|
|
|
pyramidBasis[0] *= rotation.ToMat3();
|
|
|
|
pyramidBasis[1] *= rotation.ToMat3();
|
|
|
|
pyramidBasis[2] *= rotation.ToMat3();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
float size = 10.0f;
|
|
|
|
idVec3 anchor, dir, p[4];
|
|
|
|
idMat3 worldBase, m[2];
|
|
|
|
idQuat q;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* master;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
master = body2 ? body2 : physics->GetMasterBody();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
worldBase[0] = pyramidBasis[0] * master->GetWorldAxis();
|
|
|
|
worldBase[1] = pyramidBasis[1] * master->GetWorldAxis();
|
|
|
|
worldBase[2] = pyramidBasis[2] * master->GetWorldAxis();
|
|
|
|
anchor = master->GetWorldOrigin() + pyramidAnchor * master->GetWorldAxis();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
worldBase = pyramidBasis;
|
|
|
|
anchor = pyramidAnchor;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// draw body1 axis
|
2012-11-28 15:47:07 +00:00
|
|
|
gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * ( body1Axis * body1->GetWorldAxis() ) );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// draw the pyramid
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < 2; i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
dir = worldBase[2] * size;
|
2012-11-28 15:47:07 +00:00
|
|
|
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++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DebugLine( colorMagenta, anchor, p[i] );
|
2012-11-28 15:47:07 +00:00
|
|
|
gameRenderWorld->DebugLine( colorMagenta, p[i], p[( i + 1 ) & 3] );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_PyramidLimit::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_PyramidLimit::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint_Suspension::idAFConstraint_Suspension()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
type = CONSTRAINT_SUSPENSION;
|
|
|
|
name = "suspension";
|
|
|
|
InitSize( 3 );
|
|
|
|
fl.allowPrimary = false;
|
|
|
|
fl.frameConstraint = true;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Suspension::Setup( const char* name, idAFBody* body, const idVec3& origin, const idMat3& axis, idClipModel* clipModel )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->name = name;
|
|
|
|
body1 = body;
|
|
|
|
body2 = NULL;
|
|
|
|
localOrigin = ( origin - body->GetWorldOrigin() ) * body->GetWorldAxis().Transpose();
|
|
|
|
localAxis = axis * body->GetWorldAxis().Transpose();
|
|
|
|
wheelModel = clipModel;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Suspension::SetSuspension
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Suspension::SetSuspension( const float up, const float down, const float k, const float d, const float f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
suspensionUp = up;
|
|
|
|
suspensionDown = down;
|
|
|
|
suspensionKCompress = k;
|
|
|
|
suspensionDamping = d;
|
|
|
|
friction = f;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Suspension::GetWheelOrigin
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idVec3 idAFConstraint_Suspension::GetWheelOrigin() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return body1->GetWorldOrigin() + wheelOffset * body1->GetWorldAxis();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Suspension::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Suspension::Evaluate( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
float velocity, suspensionLength, springLength, compression, dampingForce, springForce;
|
|
|
|
idVec3 origin, start, end, vel1, vel2, springDir, r, frictionDir, motorDir;
|
|
|
|
idMat3 axis;
|
|
|
|
idRotation rotation;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = localAxis * body1->GetWorldAxis();
|
|
|
|
origin = body1->GetWorldOrigin() + localOrigin * body1->GetWorldAxis();
|
|
|
|
start = origin + suspensionUp * axis[2];
|
|
|
|
end = origin - suspensionDown * axis[2];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
rotation.SetVec( axis[2] );
|
|
|
|
rotation.SetAngle( steerAngle );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
axis *= rotation.ToMat3();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.clip.Translation( trace, start, end, wheelModel, axis, MASK_SOLID, NULL );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
wheelOffset = ( trace.endpos - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( trace.fraction >= 1.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.SetSize( 0, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.SetSize( 0, 6 );
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate and add spring force
|
|
|
|
vel1 = body1->GetPointVelocity( start );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
vel2 = body2->GetPointVelocity( trace.c.point );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
vel2.Zero();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
r = trace.c.point - body1->GetWorldOrigin();
|
|
|
|
J1.SetSize( 2, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J1.SubVec6( 0 ).SubVec3( 0 ) = trace.c.normal;
|
|
|
|
J1.SubVec6( 0 ).SubVec3( 1 ) = r.Cross( trace.c.normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.SetSize( 2 );
|
|
|
|
c1[0] = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
velocity = J1.SubVec6( 0 ).SubVec3( 0 ) * body1->GetLinearVelocity() + J1.SubVec6( 0 ).SubVec3( 1 ) * body1->GetAngularVelocity();
|
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
r = trace.c.point - body2->GetWorldOrigin();
|
|
|
|
J2.SetSize( 2, 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
J2.SubVec6( 0 ).SubVec3( 0 ) = -trace.c.normal;
|
|
|
|
J2.SubVec6( 0 ).SubVec3( 1 ) = r.Cross( -trace.c.normal );
|
2012-11-26 18:58:24 +00:00
|
|
|
c2.SetSize( 2 );
|
|
|
|
c2[0] = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
velocity += J2.SubVec6( 0 ).SubVec3( 0 ) * body2->GetLinearVelocity() + J2.SubVec6( 0 ).SubVec3( 1 ) * body2->GetAngularVelocity();
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[0] = -compression; // + 0.5f * -velocity;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
e[0] = 1e-4f;
|
|
|
|
lo[0] = 0.0f;
|
|
|
|
hi[0] = springForce;
|
|
|
|
boxConstraint = NULL;
|
|
|
|
boxIndex[0] = -1;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// project the friction direction into the contact plane
|
|
|
|
frictionDir = axis[1] - axis[1] * trace.c.normal * axis[1];
|
|
|
|
frictionDir.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
r = trace.c.point - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
J1.SubVec6( 1 ).SubVec3( 0 ) = frictionDir;
|
|
|
|
J1.SubVec6( 1 ).SubVec3( 1 ) = r.Cross( frictionDir );
|
2012-11-26 18:58:24 +00:00
|
|
|
c1[1] = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
r = trace.c.point - body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
J2.SubVec6( 1 ).SubVec3( 0 ) = -frictionDir;
|
|
|
|
J2.SubVec6( 1 ).SubVec3( 1 ) = r.Cross( -frictionDir );
|
2012-11-26 18:58:24 +00:00
|
|
|
c2[1] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[1] = -friction * physics->GetContactFrictionScale();
|
|
|
|
hi[1] = friction * physics->GetContactFrictionScale();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
boxConstraint = this;
|
|
|
|
boxIndex[1] = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
|
|
|
|
if( motorEnabled )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// project the motor force direction into the contact plane
|
|
|
|
motorDir = axis[0] - axis[0] * trace.c.normal * axis[0];
|
|
|
|
motorDir.Normalize();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
r = trace.c.point - body1->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J1.ChangeSize( 3, J1.GetNumColumns() );
|
2012-11-28 15:47:07 +00:00
|
|
|
J1.SubVec6( 2 ).SubVec3( 0 ) = -motorDir;
|
|
|
|
J1.SubVec6( 2 ).SubVec3( 1 ) = r.Cross( -motorDir );
|
2012-11-26 18:58:24 +00:00
|
|
|
c1.ChangeSize( 3 );
|
|
|
|
c1[2] = motorVelocity;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
r = trace.c.point - body2->GetWorldOrigin();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
J2.ChangeSize( 3, J2.GetNumColumns() );
|
2012-11-28 15:47:07 +00:00
|
|
|
J2.SubVec6( 2 ).SubVec3( 0 ) = -motorDir;
|
|
|
|
J2.SubVec6( 2 ).SubVec3( 1 ) = r.Cross( -motorDir );
|
2012-11-26 18:58:24 +00:00
|
|
|
c2.ChangeSize( 3 );
|
|
|
|
c2[2] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lo[2] = -motorForce;
|
|
|
|
hi[2] = motorForce;
|
|
|
|
boxIndex[2] = -1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Suspension::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Suspension::ApplyFriction( float invTimeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// do nothing
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Suspension::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Suspension::Translate( const idVec3& translation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Suspension::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Suspension::Rotate( const idRotation& rotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFConstraint_Suspension::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFConstraint_Suspension::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 origin;
|
|
|
|
idMat3 axis;
|
|
|
|
idRotation rotation;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = localAxis * body1->GetWorldAxis();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
rotation.SetVec( axis[2] );
|
|
|
|
rotation.SetAngle( steerAngle );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
axis *= rotation.ToMat3();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( trace.fraction < 1.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
origin = trace.c.point;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody::idAFBody()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
Init();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::idAFBody
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody::idAFBody( const idStr& name, idClipModel* clipModel, float density )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
|
|
|
|
assert( clipModel );
|
|
|
|
assert( clipModel->IsTraceModel() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
Init();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
this->name = name;
|
|
|
|
this->clipModel = NULL;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
SetClipModel( clipModel );
|
|
|
|
SetDensity( density );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
current->worldOrigin = clipModel->GetOrigin();
|
|
|
|
current->worldAxis = clipModel->GetAxis();
|
|
|
|
*next = *current;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::~idAFBody
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody::~idAFBody()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete clipModel;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::Init
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::Init()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
name = "noname";
|
|
|
|
parent = NULL;
|
|
|
|
clipModel = NULL;
|
|
|
|
primaryConstraint = NULL;
|
|
|
|
tree = NULL;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
linearFriction = -1.0f;
|
|
|
|
angularFriction = -1.0f;
|
|
|
|
contactFriction = -1.0f;
|
|
|
|
bouncyness = -1.0f;
|
|
|
|
clipMask = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
frictionDir = vec3_zero;
|
|
|
|
contactMotorDir = vec3_zero;
|
|
|
|
contactMotorVelocity = 0.0f;
|
|
|
|
contactMotorForce = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
mass = 1.0f;
|
|
|
|
invMass = 1.0f;
|
|
|
|
centerOfMass = vec3_zero;
|
|
|
|
inertiaTensor = mat3_identity;
|
|
|
|
inverseInertiaTensor = mat3_identity;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
s.Zero( 6 );
|
|
|
|
totalForce.Zero( 6 );
|
|
|
|
auxForce.Zero( 6 );
|
|
|
|
acceleration.Zero( 6 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
response = NULL;
|
|
|
|
responseIndex = NULL;
|
|
|
|
numResponses = 0;
|
|
|
|
maxAuxiliaryIndex = 0;
|
|
|
|
maxSubTreeAuxiliaryIndex = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
memset( &fl, 0, sizeof( fl ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
fl.selfCollision = true;
|
|
|
|
fl.isZero = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::SetClipModel
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::SetClipModel( idClipModel* clipModel )
|
|
|
|
{
|
|
|
|
if( this->clipModel && this->clipModel != clipModel )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete this->clipModel;
|
|
|
|
}
|
|
|
|
this->clipModel = clipModel;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::SetFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::SetFriction( float linear, float angular, float contact )
|
|
|
|
{
|
|
|
|
if( linear < 0.0f || linear > 1.0f ||
|
2012-11-26 18:58:24 +00:00
|
|
|
angular < 0.0f || angular > 1.0f ||
|
2012-11-28 15:47:07 +00:00
|
|
|
contact < 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::SetBouncyness( float bounce )
|
|
|
|
{
|
|
|
|
if( bounce < 0.0f || bounce > 1.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "idAFBody::SetBouncyness: bouncyness out of range, bounce = %.1f", bounce );
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
bouncyness = bounce;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::SetDensity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::SetDensity( float density, const idMat3& inertiaScale )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
|
|
|
|
// get the body mass properties
|
|
|
|
clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// make sure we have a valid mass
|
2012-11-28 15:47:07 +00:00
|
|
|
if( mass <= 0.0f || IEEE_FLT_IS_NAN( mass ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "idAFBody::SetDensity: invalid mass for body '%s'", name.c_str() );
|
|
|
|
mass = 1.0f;
|
|
|
|
centerOfMass.Zero();
|
|
|
|
inertiaTensor.Identity();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// make sure the center of mass is at the body origin
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "idAFBody::SetDentity: center of mass not at origin for body '%s'", name.c_str() );
|
|
|
|
}
|
|
|
|
centerOfMass.Zero();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate the inverse mass and inverse inertia tensor
|
|
|
|
invMass = 1.0f / mass;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( inertiaScale != mat3_identity )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
inertiaTensor *= inertiaScale;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( inertiaTensor.IsDiagonal( 1e-3f ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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];
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
inverseInertiaTensor = inertiaTensor.Inverse();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::SetFrictionDirection
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::SetFrictionDirection( const idVec3& dir )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
frictionDir = dir * current->worldAxis.Transpose();
|
|
|
|
fl.useFrictionDir = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::GetFrictionDirection
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFBody::GetFrictionDirection( idVec3& dir ) const
|
|
|
|
{
|
|
|
|
if( fl.useFrictionDir )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
dir = frictionDir * current->worldAxis;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::SetContactMotorDirection
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::SetContactMotorDirection( const idVec3& dir )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contactMotorDir = dir * current->worldAxis.Transpose();
|
|
|
|
fl.useContactMotorDir = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::GetContactMotorDirection
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idAFBody::GetContactMotorDirection( idVec3& dir ) const
|
|
|
|
{
|
|
|
|
if( fl.useContactMotorDir )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
dir = contactMotorDir * current->worldAxis;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::GetPointVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idVec3 idAFBody::GetPointVelocity( const idVec3& point ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 r = point - current->worldOrigin;
|
2012-11-28 15:47:07 +00:00
|
|
|
return current->spatialVelocity.SubVec3( 0 ) + current->spatialVelocity.SubVec3( 1 ).Cross( r );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::AddForce
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::AddForce( const idVec3& point, const idVec3& force )
|
|
|
|
{
|
|
|
|
current->externalForce.SubVec3( 0 ) += force;
|
|
|
|
current->externalForce.SubVec3( 1 ) += ( point - current->worldOrigin ).Cross( force );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::InverseWorldSpatialInertiaMultiply
|
|
|
|
|
|
|
|
dst = this->inverseWorldSpatialInertia * v;
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
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
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "spatial inertia is not sparse for body %s", name.c_str() );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::Save( idSaveGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteFloat( mass );
|
|
|
|
saveFile->WriteFloat( invMass );
|
|
|
|
saveFile->WriteVec3( centerOfMass );
|
|
|
|
saveFile->WriteMat3( inertiaTensor );
|
|
|
|
saveFile->WriteMat3( inverseInertiaTensor );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteVec3( current->worldOrigin );
|
|
|
|
saveFile->WriteMat3( current->worldAxis );
|
|
|
|
saveFile->WriteVec6( current->spatialVelocity );
|
|
|
|
saveFile->WriteVec6( current->externalForce );
|
|
|
|
saveFile->WriteVec3( atRestOrigin );
|
|
|
|
saveFile->WriteMat3( atRestAxis );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFBody::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFBody::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadFloat( mass );
|
|
|
|
saveFile->ReadFloat( invMass );
|
|
|
|
saveFile->ReadVec3( centerOfMass );
|
|
|
|
saveFile->ReadMat3( inertiaTensor );
|
|
|
|
saveFile->ReadMat3( inverseInertiaTensor );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::Factor() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, j;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
idAFConstraint* child = NULL;
|
2012-11-26 18:58:24 +00:00
|
|
|
idMatX childI;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
childI.SetData( 6, 6, MATX_ALLOCA( 6 * 6 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// from the leaves up towards the root
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = sortedBodies.Num() - 1; i >= 0; i-- )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->children.Num() )
|
|
|
|
{
|
|
|
|
|
|
|
|
for( j = 0; j < body->children.Num(); j++ )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
child = body->children[j]->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
child->invI = childI;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !child->invI.InverseFastSelf() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for constraint '%s'",
|
2012-11-28 15:47:07 +00:00
|
|
|
child->invI.GetNumRows(), child->invI.GetNumColumns(), child->GetName().c_str() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
child->J = child->invI * child->J;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->I -= child->J.TransposeMultiply( childI ) * child->J;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->invI = body->I;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !body->invI.InverseFastSelf() && child != NULL )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for body %s",
|
2012-11-28 15:47:07 +00:00
|
|
|
child->invI.GetNumRows(), child->invI.GetNumColumns(), body->GetName().c_str() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->J = body->invI * body->J;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( body->primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->J = body->inverseWorldSpatialInertia * body->J;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFTree::Solve
|
|
|
|
|
|
|
|
solve for primary constraints in the tree
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::Solve( int auxiliaryIndex ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, j;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body, *child;
|
|
|
|
idAFConstraint* primaryConstraint;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// from the leaves up towards the root
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = sortedBodies.Num() - 1; i >= 0; i-- )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( j = 0; j < body->children.Num(); j++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
child = body->children[j];
|
|
|
|
primaryConstraint = child->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( !child->fl.isZero )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
child->J.TransposeMultiplySub( primaryConstraint->s, child->s );
|
|
|
|
primaryConstraint->fl.isZero = false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !primaryConstraint->fl.isZero )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->J.TransposeMultiplySub( body->s, primaryConstraint->s );
|
|
|
|
body->fl.isZero = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
bool useSymmetry = af_useSymmetry.GetBool();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// from the root down towards the leaves
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
|
|
|
primaryConstraint = body->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( primaryConstraint )
|
|
|
|
{
|
|
|
|
|
|
|
|
if( useSymmetry && body->parent->maxSubTreeAuxiliaryIndex < auxiliaryIndex )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( !primaryConstraint->fl.isZero )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->s = primaryConstraint->invI * primaryConstraint->s;
|
|
|
|
}
|
|
|
|
primaryConstraint->J.MultiplySub( primaryConstraint->s, primaryConstraint->body2->s );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->lm = primaryConstraint->s;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( useSymmetry && body->maxSubTreeAuxiliaryIndex < auxiliaryIndex )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->children.Num() )
|
|
|
|
{
|
|
|
|
if( !body->fl.isZero )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->s = body->invI * body->s;
|
|
|
|
}
|
|
|
|
body->J.MultiplySub( body->s, primaryConstraint->s );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else if( body->children.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->s = body->invI * body->s;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFTree::Response
|
|
|
|
|
|
|
|
calculate body forces in the tree in response to a constraint force
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::Response( const idAFConstraint* constraint, int row, int auxiliaryIndex ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, j;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
idAFConstraint* child, *primaryConstraint;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVecX v;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if a single body don't waste time because there aren't any primary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
if( sortedBodies.Num() == 1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = constraint->body1;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->tree == this )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
|
|
|
|
body->responseIndex[body->numResponses++] = auxiliaryIndex;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = constraint->body2;
|
|
|
|
body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
|
|
|
|
body->responseIndex[body->numResponses++] = auxiliaryIndex;
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
v.SetData( 6, VECX_ALLOCA( 6 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// initialize right hand side to zero
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
|
|
|
primaryConstraint = body->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->s.Zero();
|
|
|
|
primaryConstraint->fl.isZero = true;
|
|
|
|
}
|
|
|
|
body->s.Zero();
|
|
|
|
body->fl.isZero = true;
|
|
|
|
body->GetResponseForce( body->numResponses ).Zero();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// set right hand side for first constrained body
|
|
|
|
body = constraint->body1;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->tree == this )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->InverseWorldSpatialInertiaMultiply( v, constraint->J1[row] );
|
|
|
|
primaryConstraint = body->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->J1.Multiply( primaryConstraint->s, v );
|
|
|
|
primaryConstraint->fl.isZero = false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < body->children.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
child = body->children[i]->primaryConstraint;
|
|
|
|
child->J2.Multiply( child->s, v );
|
|
|
|
child->fl.isZero = false;
|
|
|
|
}
|
|
|
|
body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// set right hand side for second constrained body
|
|
|
|
body = constraint->body2;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body && body->tree == this )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->InverseWorldSpatialInertiaMultiply( v, constraint->J2[row] );
|
|
|
|
primaryConstraint = body->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->J1.MultiplyAdd( primaryConstraint->s, v );
|
|
|
|
primaryConstraint->fl.isZero = false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < body->children.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
child = body->children[i]->primaryConstraint;
|
|
|
|
child->J2.MultiplyAdd( child->s, v );
|
|
|
|
child->fl.isZero = false;
|
|
|
|
}
|
|
|
|
body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// solve for primary constraints
|
|
|
|
Solve( auxiliaryIndex );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
bool useSymmetry = af_useSymmetry.GetBool();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// store body forces in response to the constraint force
|
|
|
|
idVecX force;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( useSymmetry && body->maxAuxiliaryIndex < auxiliaryIndex )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
force.SetData( 6, body->response + body->numResponses * 8 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add forces of all primary constraints acting on this body
|
|
|
|
primaryConstraint = body->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->J1.TransposeMultiplyAdd( force, primaryConstraint->lm );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = 0; j < body->children.Num(); j++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
child = body->children[j]->primaryConstraint;
|
|
|
|
child->J2.TransposeMultiplyAdd( force, child->lm );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->responseIndex[body->numResponses++] = auxiliaryIndex;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFTree::CalculateForces
|
|
|
|
|
|
|
|
calculate forces on the bodies in the tree
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::CalculateForces( float timeStep ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, j;
|
|
|
|
float invStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
idAFConstraint* child, *c, *primaryConstraint;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// forces on bodies
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
body->totalForce.SubVec6( 0 ) = body->current->externalForce + body->auxForce.SubVec6( 0 );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if a single body don't waste time because there aren't any primary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
if( sortedBodies.Num() == 1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
invStep = 1.0f / timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// initialize right hand side
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
body->acceleration.SubVec6( 0 ) += body->current->spatialVelocity * invStep;
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint = body->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// solve for primary constraints
|
|
|
|
Solve();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate forces on bodies after applying primary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add forces of all primary constraints acting on this body
|
|
|
|
primaryConstraint = body->primaryConstraint;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraint->J1.TransposeMultiplyAdd( body->totalForce, primaryConstraint->lm );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = 0; j < body->children.Num(); j++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
child = body->children[j]->primaryConstraint;
|
|
|
|
child->J2.TransposeMultiplyAdd( body->totalForce, child->lm );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFTree::SetMaxSubTreeAuxiliaryIndex
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::SetMaxSubTreeAuxiliaryIndex()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, j;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body, *child;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// from the leaves up towards the root
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = sortedBodies.Num() - 1; i >= 0; i-- )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->maxSubTreeAuxiliaryIndex = body->maxAuxiliaryIndex;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = 0; j < body->children.Num(); j++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
child = body->children[j];
|
2012-11-28 15:47:07 +00:00
|
|
|
if( child->maxSubTreeAuxiliaryIndex > body->maxSubTreeAuxiliaryIndex )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->maxSubTreeAuxiliaryIndex = child->maxSubTreeAuxiliaryIndex;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFTree::SortBodies_r
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::SortBodies_r( idList<idAFBody*>& sortedList, idAFBody* body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < body->children.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
sortedList.Append( body->children[i] );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < body->children.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
SortBodies_r( sortedList, body->children[i] );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFTree::SortBodies
|
|
|
|
|
|
|
|
sort body list to make sure parents come first
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::SortBodies()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// find the root
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !sortedBodies[i]->parent )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( i >= sortedBodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "Articulated figure tree has no root." );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
|
|
|
sortedBodies.Clear();
|
|
|
|
sortedBodies.Append( body );
|
|
|
|
SortBodies_r( sortedBodies, body );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idAFTree::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idAFTree::DebugDraw( const idVec4& color ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
for( i = 1; i < sortedBodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = sortedBodies[i];
|
|
|
|
gameRenderWorld->DebugArrow( color, body->parent->current->worldOrigin, body->current->worldOrigin, 1 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//===============================================================
|
|
|
|
// M
|
|
|
|
// idPhysics_AF MrE
|
|
|
|
// E
|
|
|
|
//===============================================================
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::EvaluateConstraints
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::EvaluateConstraints( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
float invTimeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
idAFConstraint* c;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
invTimeStep = 1.0f / timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// setup the constraint equations for the current position and orientation of the bodies
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < primaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c = primaryConstraints[i];
|
|
|
|
c->Evaluate( invTimeStep );
|
|
|
|
c->J = c->J2;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
auxiliaryConstraints[i]->Evaluate( invTimeStep );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add contact constraints to the list with frame constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < contactConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
AddFrameConstraint( contactConstraints[i] );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// setup body primary constraint matrix
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->primaryConstraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->J = body->primaryConstraint->J1.Transpose();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::EvaluateBodies
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::EvaluateBodies( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3 axis;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// we transpose the axis before using it because idMat3 is column-major
|
|
|
|
axis = body->current->worldAxis.Transpose();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the center of mass is at the body point of reference
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// spatial inertia in world space
|
|
|
|
body->I.Set( body->mass * mat3_identity, mat3_zero,
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, axis * body->inertiaTensor * axis.Transpose() );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// inverse spatial inertia in world space
|
|
|
|
body->inverseWorldSpatialInertia.Set( body->invMass * mat3_identity, mat3_zero,
|
2012-11-28 15:47:07 +00:00
|
|
|
mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose() );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->fl.spatialInertiaSparse = true;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3 massMoment = body->mass * SkewSymmetric( body->centerOfMass );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// spatial inertia in world space
|
|
|
|
body->I.Set( body->mass * mat3_identity, massMoment,
|
2012-11-28 15:47:07 +00:00
|
|
|
massMoment.Transpose(), axis * body->inertiaTensor * axis.Transpose() );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// inverse spatial inertia in world space
|
|
|
|
body->inverseWorldSpatialInertia = body->I.InverseFast();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->fl.spatialInertiaSparse = false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// initialize auxiliary constraint force to zero
|
|
|
|
body->auxForce.Zero();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::AddFrameConstraints
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::AddFrameConstraints()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add frame constraints to auxiliary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < frameConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
auxiliaryConstraints.Append( frameConstraints[i] );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::RemoveFrameConstraints
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::RemoveFrameConstraints()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// remove all the frame constraints from the auxiliary constraints
|
|
|
|
auxiliaryConstraints.SetNum( auxiliaryConstraints.Num() - frameConstraints.Num() );
|
|
|
|
frameConstraints.SetNum( 0 );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ApplyFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ApplyFriction( float timeStep, float endTimeMSec )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
float invTimeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_skipFriction.GetBool() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( jointFrictionDentStart < MS2SEC( endTimeMSec ) && jointFrictionDentEnd > MS2SEC( endTimeMSec ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
float halfTime = ( jointFrictionDentEnd - jointFrictionDentStart ) * 0.5f;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( jointFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
jointFrictionDentScale = 1.0f - ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart ) / halfTime;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
jointFrictionDentScale = jointFrictionDent + ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart - halfTime ) / halfTime;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
jointFrictionDentScale = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( contactFrictionDentStart < MS2SEC( endTimeMSec ) && contactFrictionDentEnd > MS2SEC( endTimeMSec ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
float halfTime = ( contactFrictionDentEnd - contactFrictionDentStart ) * 0.5f;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( contactFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contactFrictionDentScale = 1.0f - ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart ) / halfTime;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contactFrictionDentScale = contactFrictionDent + ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart - halfTime ) / halfTime;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contactFrictionDentScale = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
invTimeStep = 1.0f / timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < primaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraints[i]->ApplyFriction( invTimeStep );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
auxiliaryConstraints[i]->ApplyFriction( invTimeStep );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < frameConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
frameConstraints[i]->ApplyFriction( invTimeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::PrimaryFactor
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::PrimaryFactor()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < trees.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
trees[i]->Factor();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::PrimaryForces
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::PrimaryForces( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < trees.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
trees[i]->CalculateForces( timeStep );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::AuxiliaryForces
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::AuxiliaryForces( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, j, k, l, n, m, s, numAuxConstraints, *index, *boxIndex;
|
2012-11-28 15:47:07 +00:00
|
|
|
float* ptr, *j1, *j2, *dstPtr, *forcePtr;
|
2012-11-26 18:58:24 +00:00
|
|
|
float invStep, u;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
idAFConstraint* constraint;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVecX tmp;
|
|
|
|
idMatX jmk;
|
|
|
|
idVecX rhs, w, lm, lo, hi;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// get the number of one dimensional auxiliary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( numAuxConstraints = 0, i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
numAuxConstraints += auxiliaryConstraints[i]->J1.GetNumRows();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( numAuxConstraints == 0 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// allocate memory to store the body response to auxiliary constraint forces
|
2012-11-28 15:47:07 +00:00
|
|
|
forcePtr = ( float* ) _alloca16( bodies.Num() * numAuxConstraints * 8 * sizeof( float ) );
|
|
|
|
index = ( int* ) _alloca16( bodies.Num() * numAuxConstraints * sizeof( int ) );
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
body->response = forcePtr;
|
|
|
|
body->responseIndex = index;
|
|
|
|
body->numResponses = 0;
|
|
|
|
body->maxAuxiliaryIndex = 0;
|
|
|
|
forcePtr += numAuxConstraints * 8;
|
|
|
|
index += numAuxConstraints;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// set on each body the largest index of an auxiliary constraint constraining the body
|
2012-11-28 15:47:07 +00:00
|
|
|
if( af_useSymmetry.GetBool() )
|
|
|
|
{
|
|
|
|
for( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = auxiliaryConstraints[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = 0; j < constraint->J1.GetNumRows(); j++, k++ )
|
|
|
|
{
|
|
|
|
if( k > constraint->body1->maxAuxiliaryIndex )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint->body1->maxAuxiliaryIndex = k;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constraint->body2 && k > constraint->body2->maxAuxiliaryIndex )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint->body2->maxAuxiliaryIndex = k;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < trees.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
trees[i]->SetMaxSubTreeAuxiliaryIndex();
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate forces of primary constraints in response to the auxiliary constraint forces
|
2012-11-28 15:47:07 +00:00
|
|
|
for( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = auxiliaryConstraints[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( j = 0; j < constraint->J1.GetNumRows(); j++, k++ )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constraint->body2 && constraint->body2->tree != constraint->body1->tree )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate body forces in the second tree in response to the constraint force
|
|
|
|
constraint->body2->tree->Response( constraint, j, k );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// NOTE: the rows are 16 byte padded
|
2012-11-28 15:47:07 +00:00
|
|
|
jmk.SetData( numAuxConstraints, ( ( numAuxConstraints + 3 )&~3 ), MATX_ALLOCA( numAuxConstraints * ( ( numAuxConstraints + 3 )&~3 ) ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
tmp.SetData( 6, VECX_ALLOCA( 6 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// create constraint matrix for auxiliary constraints using a mass matrix adjusted for the primary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = auxiliaryConstraints[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( j = 0; j < constraint->J1.GetNumRows(); j++, k++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( l = n = 0, m = index[n]; n < constraint->body1->numResponses && m < s; n++, m = index[n] )
|
|
|
|
{
|
|
|
|
while( l < m )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
dstPtr[l++] = 0.0f;
|
|
|
|
}
|
|
|
|
dstPtr[l++] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] +
|
2012-11-28 15:47:07 +00:00
|
|
|
j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5];
|
2012-11-26 18:58:24 +00:00
|
|
|
ptr += 8;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
while( l < s )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
dstPtr[l++] = 0.0f;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( constraint->body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint->body2->InverseWorldSpatialInertiaMultiply( tmp, constraint->J2[j] );
|
|
|
|
j2 = tmp.ToFloatPtr();
|
|
|
|
ptr = constraint->body2->response;
|
|
|
|
index = constraint->body2->responseIndex;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( n = 0, m = index[n]; n < constraint->body2->numResponses && m < s; n++, m = index[n] )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
dstPtr[m] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] +
|
2012-11-28 15:47:07 +00:00
|
|
|
j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5];
|
2012-11-26 18:58:24 +00:00
|
|
|
ptr += 8;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_useSymmetry.GetBool() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
n = jmk.GetNumColumns();
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < numAuxConstraints; i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ptr = jmk.ToFloatPtr() + ( i + 1 ) * n + i;
|
|
|
|
dstPtr = jmk.ToFloatPtr() + i * n + i + 1;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = i + 1; j < numAuxConstraints; j++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
*dstPtr++ = *ptr;
|
|
|
|
ptr += n;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
invStep = 1.0f / timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate body acceleration
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
body->acceleration.SubVec6( 0 ) += body->current->spatialVelocity * invStep;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
rhs.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
|
|
|
|
lo.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
|
|
|
|
hi.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
|
|
|
|
lm.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
boxIndex = ( int* ) _alloca16( numAuxConstraints * sizeof( int ) );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// set first index for special box constrained variables
|
2012-11-28 15:47:07 +00:00
|
|
|
for( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
auxiliaryConstraints[i]->firstIndex = k;
|
|
|
|
k += auxiliaryConstraints[i]->J1.GetNumRows();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// initialize right hand side and low and high bounds for auxiliary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = auxiliaryConstraints[i];
|
|
|
|
n = k;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( j = 0; j < constraint->J1.GetNumRows(); j++, k++ )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( constraint->body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
rhs[k] = -rhs[k];
|
|
|
|
lo[k] = constraint->lo[j];
|
|
|
|
hi[k] = constraint->hi[j];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( constraint->boxIndex[j] >= 0 )
|
|
|
|
{
|
|
|
|
if( constraint->boxConstraint->fl.isPrimary )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "cannot reference primary constraints for the box index" );
|
|
|
|
}
|
|
|
|
boxIndex[k] = constraint->boxConstraint->firstIndex + constraint->boxIndex[j];
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
boxIndex[k] = -1;
|
|
|
|
}
|
|
|
|
jmk[k][k] += constraint->e[j] * invStep;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_lcp.Start();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate lagrange multipliers for auxiliary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !lcp->Solve( jmk, lm, rhs, lo, hi, boxIndex ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return; // bad monkey!
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_lcp.Stop();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate auxiliary constraint forces
|
2012-11-28 15:47:07 +00:00
|
|
|
for( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = auxiliaryConstraints[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( j = 0; j < constraint->J1.GetNumRows(); j++, k++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint->lm[j] = u = lm[k];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
j1 = constraint->J1[j];
|
|
|
|
ptr = constraint->body1->auxForce.ToFloatPtr();
|
2012-11-28 15:47:07 +00:00
|
|
|
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 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
j2 = constraint->J2[j];
|
|
|
|
ptr = constraint->body2->auxForce.ToFloatPtr();
|
2012-11-28 15:47:07 +00:00
|
|
|
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;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// recalculate primary constraint forces in response to auxiliary constraint forces
|
|
|
|
PrimaryForces( timeStep );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// clear pointers pointing to stack space so tools don't get confused
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
body->response = NULL;
|
|
|
|
body->responseIndex = NULL;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::VerifyContactConstraints
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::VerifyContactConstraints()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
#if 0
|
|
|
|
int i;
|
|
|
|
float impulseNumerator, impulseDenominator;
|
|
|
|
idVec3 r, velocity, normalVelocity, normal, impulse;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
for( i = 0; i < contactConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = contactConstraints[i]->body1;
|
2012-11-28 15:47:07 +00:00
|
|
|
const contactInfo_t& contact = contactConstraints[i]->GetContact();
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
r = contact.point - body->GetCenterOfMass();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate velocity at contact point
|
|
|
|
velocity = body->GetLinearVelocity() + body->GetAngularVelocity().Cross( r );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// velocity along normal vector
|
|
|
|
normalVelocity = ( velocity * contact.normal ) * contact.normal;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if moving towards the surface at the contact point
|
2012-11-28 15:47:07 +00:00
|
|
|
if( normalVelocity * contact.normal < 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate impulse
|
|
|
|
normal = -normalVelocity;
|
|
|
|
impulseNumerator = normal.Normalize();
|
|
|
|
impulseDenominator = body->GetInverseMass() + ( ( body->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal );
|
2012-11-28 15:47:07 +00:00
|
|
|
impulse = ( impulseNumerator / impulseDenominator ) * normal * 1.0001f;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// apply impulse
|
|
|
|
body->SetLinearVelocity( body->GetLinearVelocity() + impulse );
|
|
|
|
body->SetAngularVelocity( body->GetAngularVelocity() + r.Cross( impulse ) );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#else
|
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 normal;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < contactConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = contactConstraints[i]->body1;
|
|
|
|
normal = contactConstraints[i]->GetContact().normal;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( normal * body->next->spatialVelocity.SubVec3( 0 ) <= 0.0f )
|
|
|
|
{
|
|
|
|
body->next->spatialVelocity.SubVec3( 0 ) -= 1.0001f * ( normal * body->next->spatialVelocity.SubVec3( 0 ) ) * normal;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
body = contactConstraints[i]->body2;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
normal = -normal;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( normal * body->next->spatialVelocity.SubVec3( 0 ) <= 0.0f )
|
|
|
|
{
|
|
|
|
body->next->spatialVelocity.SubVec3( 0 ) -= 1.0001f * ( normal * body->next->spatialVelocity.SubVec3( 0 ) ) * normal;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Evolve
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::Evolve( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
float angle;
|
|
|
|
idVec3 vec;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec6 force;
|
|
|
|
idRotation rotation;
|
|
|
|
float vSqr, maxLinearVelocity, maxAngularVelocity;
|
|
|
|
|
|
|
|
maxLinearVelocity = af_maxLinearVelocity.GetFloat() / timeStep;
|
|
|
|
maxAngularVelocity = af_maxAngularVelocity.GetFloat() / timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate the spatial velocity for the next physics state
|
|
|
|
body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
body->next->spatialVelocity = body->current->spatialVelocity + timeStep * body->acceleration.SubVec6( 0 );
|
|
|
|
|
|
|
|
if( maxLinearVelocity > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// cap the linear velocity
|
2012-11-28 15:47:07 +00:00
|
|
|
vSqr = body->next->spatialVelocity.SubVec3( 0 ).LengthSqr();
|
|
|
|
if( vSqr > Square( maxLinearVelocity ) )
|
|
|
|
{
|
|
|
|
body->next->spatialVelocity.SubVec3( 0 ) *= idMath::InvSqrt( vSqr ) * maxLinearVelocity;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( maxAngularVelocity > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// cap the angular velocity
|
2012-11-28 15:47:07 +00:00
|
|
|
vSqr = body->next->spatialVelocity.SubVec3( 1 ).LengthSqr();
|
|
|
|
if( vSqr > Square( maxAngularVelocity ) )
|
|
|
|
{
|
|
|
|
body->next->spatialVelocity.SubVec3( 1 ) *= idMath::InvSqrt( vSqr ) * maxAngularVelocity;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// make absolutely sure all contact constraints are satisfied
|
|
|
|
VerifyContactConstraints();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate the position of the bodies for the next physics state
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// translate world origin
|
|
|
|
body->next->worldOrigin = body->current->worldOrigin + timeStep * body->next->spatialVelocity.SubVec3( 0 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// convert angular velocity to a rotation matrix
|
|
|
|
vec = body->next->spatialVelocity.SubVec3( 1 );
|
2012-11-28 15:47:07 +00:00
|
|
|
angle = -timeStep * ( float ) RAD2DEG( vec.Normalize() );
|
2012-11-26 18:58:24 +00:00
|
|
|
rotation = idRotation( vec3_origin, vec, angle );
|
|
|
|
rotation.Normalize180();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// rotate world axis
|
|
|
|
body->next->worldAxis = body->current->worldAxis * rotation.ToMat3();
|
|
|
|
body->next->worldAxis.OrthoNormalizeSelf();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// linear and angular friction
|
2012-11-28 15:47:07 +00:00
|
|
|
body->next->spatialVelocity.SubVec3( 0 ) -= body->linearFriction * body->next->spatialVelocity.SubVec3( 0 );
|
|
|
|
body->next->spatialVelocity.SubVec3( 1 ) -= body->angularFriction * body->next->spatialVelocity.SubVec3( 1 );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody* body, trace_t& collision )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 r, velocity, impulse;
|
|
|
|
idMat3 inverseWorldInertiaTensor;
|
|
|
|
float impulseNumerator, impulseDenominator;
|
|
|
|
impactInfo_t info;
|
2012-11-28 15:47:07 +00:00
|
|
|
idEntity* ent;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
ent = gameLocal.entities[collision.c.entityNum];
|
2012-11-28 15:47:07 +00:00
|
|
|
if( ent == self )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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
|
2012-11-28 15:47:07 +00:00
|
|
|
r = collision.c.point - ( body->current->worldOrigin + body->centerOfMass * body->current->worldAxis );
|
2012-11-26 18:58:24 +00:00
|
|
|
// the velocity at the collision point
|
2012-11-28 15:47:07 +00:00
|
|
|
velocity = body->current->spatialVelocity.SubVec3( 0 ) + body->current->spatialVelocity.SubVec3( 1 ).Cross( r );
|
2012-11-26 18:58:24 +00:00
|
|
|
// subtract velocity of other entity
|
|
|
|
velocity -= info.velocity;
|
|
|
|
// never stick
|
2012-11-28 15:47:07 +00:00
|
|
|
if( velocity * collision.c.normal > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( info.invMass )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
impulse = ( impulseNumerator / impulseDenominator ) * collision.c.normal;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// apply impact to other entity
|
|
|
|
ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// callback to self to let the entity know about the impact
|
|
|
|
return self->Collide( collision, velocity );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ApplyCollisions
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::ApplyCollisions( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < collisions.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( CollisionImpulse( timeStep, collisions[i].body, collisions[i].trace ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetupCollisionForBody
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idEntity* idPhysics_AF::SetupCollisionForBody( idAFBody* body ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* b;
|
|
|
|
idEntity* passEntity;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
passEntity = NULL;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( !selfCollision || !body->fl.selfCollision || af_skipSelfCollision.GetBool() )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// disable all bodies
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->clipModel->Disable();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// don't collide with world collision model if attached to the world
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < body->constraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !body->constraints[i]->fl.noCollision )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
// if this constraint attaches the body to the world
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->constraints[i]->body2 == NULL )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// don't collide with the world collision model
|
|
|
|
passEntity = gameLocal.world;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// enable all bodies that have self collision
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( bodies[i]->fl.selfCollision )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->clipModel->Enable();
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->clipModel->Disable();
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// don't let the body collide with itself
|
|
|
|
body->clipModel->Disable();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// disable any bodies attached with constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < body->constraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !body->constraints[i]->fl.noCollision )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
// if this constraint attaches the body to the world
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->constraints[i]->body2 == NULL )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// don't collide with the world collision model
|
|
|
|
passEntity = gameLocal.world;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
if( body->constraints[i]->body1 == body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
b = body->constraints[i]->body2;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else if( body->constraints[i]->body2 == body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
b = body->constraints[i]->body1;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
// don't collide with this body
|
|
|
|
b->clipModel->Disable();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::CheckForCollisions( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// #define TEST_COLLISION_DETECTION
|
|
|
|
int i, index;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3 axis;
|
|
|
|
idRotation rotation;
|
|
|
|
trace_t collision;
|
2012-11-28 15:47:07 +00:00
|
|
|
idEntity* passEntity;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// clear list with collisions
|
|
|
|
collisions.SetNum( 0 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( !enableCollision )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->clipMask != 0 )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
passEntity = SetupCollisionForBody( body );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef TEST_COLLISION_DETECTION
|
|
|
|
bool startsolid = false;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( gameLocal.clip.Contents( body->current->worldOrigin, body->clipModel,
|
|
|
|
body->current->worldAxis, body->clipMask, passEntity ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
startsolid = true;
|
|
|
|
}
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
TransposeMultiply( body->current->worldAxis, body->next->worldAxis, axis );
|
|
|
|
rotation = axis.ToRotation();
|
|
|
|
rotation.SetOrigin( body->current->worldOrigin );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if there was a collision
|
2012-11-28 15:47:07 +00:00
|
|
|
if( gameLocal.clip.Motion( collision, body->current->worldOrigin, body->next->worldOrigin, rotation,
|
|
|
|
body->clipModel, body->current->worldAxis, body->clipMask, passEntity ) )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// set the next state to the state at the moment of impact
|
|
|
|
body->next->worldOrigin = collision.endpos;
|
|
|
|
body->next->worldAxis = collision.endAxis;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add collision to the list
|
|
|
|
index = collisions.Num();
|
|
|
|
collisions.SetNum( index + 1 );
|
|
|
|
collisions[index].trace = collision;
|
|
|
|
collisions[index].body = body;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef TEST_COLLISION_DETECTION
|
2012-11-28 15:47:07 +00:00
|
|
|
if( gameLocal.clip.Contents( body->next->worldOrigin, body->clipModel,
|
|
|
|
body->next->worldAxis, body->clipMask, passEntity ) )
|
|
|
|
{
|
|
|
|
if( !startsolid )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int bah = 1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->next->worldOrigin, body->next->worldAxis );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::EvaluateContacts
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::EvaluateContacts()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, j, k, numContacts, numBodyContacts;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
contactInfo_t contactInfo[10];
|
2012-11-28 15:47:07 +00:00
|
|
|
idEntity* passEntity;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVecX dir( 6, VECX_ALLOCA( 6 ) );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// evaluate bodies
|
|
|
|
EvaluateBodies( current.lastTimeStep );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// remove all existing contacts
|
|
|
|
ClearContacts();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
contactBodies.SetNum( 0 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( !enableCollision )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// find all the contacts
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->clipMask == 0 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
passEntity = SetupCollisionForBody( body );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->InverseWorldSpatialInertiaMultiply( dir, body->current->externalForce.ToFloatPtr() );
|
2012-11-28 15:47:07 +00:00
|
|
|
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 );
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#if 1
|
|
|
|
// merge nearby contacts between the same bodies
|
|
|
|
// and assure there are at most three planar contacts between any pair of bodies
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = 0; j < numContacts; j++ )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
numBodyContacts = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
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 ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
break;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( idMath::Fabs( contacts[k].normal * contactInfo[j].normal ) > 0.9f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
numBodyContacts++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( k >= contacts.Num() && numBodyContacts < 3 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contacts.Append( contactInfo[j] );
|
|
|
|
contactBodies.Append( i );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#else
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( j = 0; j < numContacts; j++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contacts.Append( contactInfo[j] );
|
|
|
|
contactBodies.Append( i );
|
|
|
|
}
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
AddContactEntitiesForContacts();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return ( contacts.Num() != 0 );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetupContactConstraints
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetupContactConstraints()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// make sure enough contact constraints are allocated
|
|
|
|
contactConstraints.AssureSizeAlloc( contacts.Num(), idListNewElement<idAFConstraint_Contact> );
|
|
|
|
contactConstraints.SetNum( contacts.Num() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// setup contact constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < contacts.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// add contact constraint
|
|
|
|
contactConstraints[i]->physics = this;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( contacts[i].entityNum == self->entityNumber )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contactConstraints[i]->Setup( bodies[contactBodies[i]], bodies[ contacts[i].id ], contacts[i] );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contactConstraints[i]->Setup( bodies[contactBodies[i]], NULL, contacts[i] );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ApplyContactForces
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ApplyContactForces()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
#if 0
|
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idEntity* ent;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 force;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < contactConstraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( contactConstraints[i]->body2 != NULL )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
const contactInfo_t& contact = contactConstraints[i]->GetContact();
|
2012-11-26 18:58:24 +00:00
|
|
|
ent = gameLocal.entities[contact.entityNum];
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !ent )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
force.Zero();
|
|
|
|
ent->AddForce( self, contact.id, contact.point, force );
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ClearExternalForce
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ClearExternalForce()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// clear external force
|
|
|
|
body->current->externalForce.Zero();
|
|
|
|
body->next->externalForce.Zero();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::AddGravity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::AddGravity()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
// add gravitational force
|
|
|
|
body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SwapStates
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SwapStates()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
AFBodyPState_t* swap;
|
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// swap the current and next state for next simulation step
|
|
|
|
swap = body->current;
|
|
|
|
body->current = body->next;
|
|
|
|
body->next = swap;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::UpdateClipModels
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::UpdateClipModels()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->current->worldOrigin, body->current->worldAxis );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetSuspendSpeed
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetSuspendSpeed( const idVec2& velocity, const idVec2& acceleration )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->suspendVelocity = velocity;
|
|
|
|
this->suspendAcceleration = acceleration;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetSuspendTime
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetSuspendTime( const float minTime, const float maxTime )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->minMoveTime = minTime;
|
|
|
|
this->maxMoveTime = maxTime;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetSuspendTolerance
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetSuspendTolerance( const float noMoveTime, const float noMoveTranslation, const float noMoveRotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
this->noMoveTime = noMoveTime;
|
|
|
|
this->noMoveTranslation = noMoveTranslation;
|
|
|
|
this->noMoveRotation = noMoveRotation;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetTimeScaleRamp
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetTimeScaleRamp( const float start, const float end )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
timeScaleRampStart = start;
|
|
|
|
timeScaleRampEnd = end;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetJointFrictionDent
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetJointFrictionDent( const float dent, const float start, const float end )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
jointFrictionDent = dent;
|
|
|
|
jointFrictionDentStart = start;
|
|
|
|
jointFrictionDentEnd = end;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetJointFrictionScale
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
float idPhysics_AF::GetJointFrictionScale() const
|
|
|
|
{
|
|
|
|
if( jointFrictionDentScale > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return jointFrictionDentScale;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else if( jointFrictionScale > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return jointFrictionScale;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else if( af_jointFrictionScale.GetFloat() > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return af_jointFrictionScale.GetFloat();
|
|
|
|
}
|
|
|
|
return 1.0f;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetContactFrictionDent
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetContactFrictionDent( const float dent, const float start, const float end )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contactFrictionDent = dent;
|
|
|
|
contactFrictionDentStart = start;
|
|
|
|
contactFrictionDentEnd = end;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetContactFrictionScale
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
float idPhysics_AF::GetContactFrictionScale() const
|
|
|
|
{
|
|
|
|
if( contactFrictionDentScale > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return contactFrictionDentScale;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else if( contactFrictionScale > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return contactFrictionScale;
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else if( af_contactFrictionScale.GetFloat() > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return af_contactFrictionScale.GetFloat();
|
|
|
|
}
|
|
|
|
return 1.0f;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::TestIfAtRest
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::TestIfAtRest( float timeStep )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
float translationSqr, maxTranslationSqr, rotation, maxRotation;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
if( current.atRest >= 0 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
current.activateTime += timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the simulation should never be suspended before a certaint amount of time passed
|
2012-11-28 15:47:07 +00:00
|
|
|
if( minMoveTime > 0.0f && current.activateTime < minMoveTime )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the simulation should always be suspended after a certain amount time passed
|
2012-11-28 15:47:07 +00:00
|
|
|
if( maxMoveTime > 0.0f && current.activateTime > maxMoveTime )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// test if all bodies hardly moved over a period of time
|
2012-11-28 15:47:07 +00:00
|
|
|
if( current.noMoveTime == 0.0f )
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
body->atRestOrigin = body->current->worldOrigin;
|
|
|
|
body->atRestAxis = body->current->worldAxis;
|
|
|
|
}
|
|
|
|
current.noMoveTime += timeStep;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( current.noMoveTime > noMoveTime )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
current.noMoveTime = 0.0f;
|
|
|
|
maxTranslationSqr = 0.0f;
|
|
|
|
maxRotation = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
translationSqr = ( body->current->worldOrigin - body->atRestOrigin ).LengthSqr();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( translationSqr > maxTranslationSqr )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
maxTranslationSqr = translationSqr;
|
|
|
|
}
|
|
|
|
rotation = ( body->atRestAxis.Transpose() * body->current->worldAxis ).ToRotation().GetAngle();
|
2012-11-28 15:47:07 +00:00
|
|
|
if( rotation > maxRotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
maxRotation = rotation;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( maxTranslationSqr < Square( noMoveTranslation ) && maxRotation < noMoveRotation )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// hardly moved over a period of time so the articulated figure may come to rest
|
|
|
|
return true;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
current.noMoveTime += timeStep;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// test if the velocity or acceleration of any body is still too large to come to rest
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->current->spatialVelocity.SubVec3( 0 ).LengthSqr() > Square( suspendVelocity[0] ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->current->spatialVelocity.SubVec3( 1 ).LengthSqr() > Square( suspendVelocity[1] ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->acceleration.SubVec3( 0 ).LengthSqr() > Square( suspendAcceleration[0] ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->acceleration.SubVec3( 1 ).LengthSqr() > Square( suspendAcceleration[1] ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// all bodies have a velocity and acceleration small enough to come to rest
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Rest
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::Rest()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
current.atRest = gameLocal.time;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->current->spatialVelocity.Zero();
|
|
|
|
bodies[i]->current->externalForce.Zero();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
self->BecomeInactive( TH_PHYSICS );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Activate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::Activate()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the articulated figure was at rest
|
2012-11-28 15:47:07 +00:00
|
|
|
if( current.atRest >= 0 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// 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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::PutToRest()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
Rest();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::EnableImpact
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::EnableImpact()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
noImpact = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::DisableImpact
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::DisableImpact()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
noImpact = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::AddPushVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::AddPushVelocity( const idVec6& pushVelocity )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( pushVelocity != vec6_origin )
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->current->spatialVelocity += pushVelocity;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetClipModel
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetClipModel( idClipModel* model, float density, int id, bool freeOld )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetClipModel
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idClipModel* idPhysics_AF::GetClipModel( int id ) const
|
|
|
|
{
|
|
|
|
if( id >= 0 && id < bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->GetClipModel();
|
|
|
|
}
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetNumClipModels
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetNumClipModels() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies.Num();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetMass
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetMass( float mass, int id )
|
|
|
|
{
|
|
|
|
if( id >= 0 && id < bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
forceTotalMass = mass;
|
|
|
|
}
|
|
|
|
SetChanged();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetMass
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
float idPhysics_AF::GetMass( int id ) const
|
|
|
|
{
|
|
|
|
if( id >= 0 && id < bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->mass;
|
|
|
|
}
|
|
|
|
return totalMass;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetContents
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetContents( int contents, int id )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( id >= 0 && id < bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[id]->GetClipModel()->SetContents( contents );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->GetClipModel()->SetContents( contents );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetContents
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetContents( int id ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, contents;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( id >= 0 && id < bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->GetClipModel()->GetContents();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contents = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contents |= bodies[i]->GetClipModel()->GetContents();
|
|
|
|
}
|
|
|
|
return contents;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetBounds
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idBounds& idPhysics_AF::GetBounds( int id ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
static idBounds relBounds;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( id >= 0 && id < bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->GetClipModel()->GetBounds();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( !bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
relBounds.Zero();
|
|
|
|
return relBounds;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
relBounds = bodies[0]->GetClipModel()->GetBounds();
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 1; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idBounds& idPhysics_AF::GetAbsBounds( int id ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
static idBounds absBounds;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( id >= 0 && id < bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->GetClipModel()->GetAbsBounds();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else if( !bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
absBounds.Zero();
|
|
|
|
return absBounds;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
absBounds = bodies[0]->GetClipModel()->GetAbsBounds();
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 1; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
absBounds += bodies[i]->GetClipModel()->GetAbsBounds();
|
|
|
|
}
|
|
|
|
return absBounds;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Evaluate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
float timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( timeScaleRampStart < MS2SEC( endTimeMSec ) && timeScaleRampEnd > MS2SEC( endTimeMSec ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
timeStep = MS2SEC( timeStepMSec ) * ( MS2SEC( endTimeMSec ) - timeScaleRampStart ) / ( timeScaleRampEnd - timeScaleRampStart );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else if( af_timeScale.GetFloat() != 1.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
timeStep = MS2SEC( timeStepMSec ) * af_timeScale.GetFloat();
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
timeStep = MS2SEC( timeStepMSec ) * timeScale;
|
|
|
|
}
|
|
|
|
current.lastTimeStep = timeStep;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the articulated figure changed
|
2012-11-28 15:47:07 +00:00
|
|
|
if( changedAF || ( linearTime != af_useLinearTime.GetBool() ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
BuildTrees();
|
|
|
|
changedAF = false;
|
|
|
|
linearTime = af_useLinearTime.GetBool();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// get the new master position
|
2012-11-28 15:47:07 +00:00
|
|
|
if( masterBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 masterOrigin;
|
|
|
|
idMat3 masterAxis;
|
|
|
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( current.atRest >= 0 && ( masterBody->current->worldOrigin != masterOrigin || masterBody->current->worldAxis != masterAxis ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
Activate();
|
|
|
|
}
|
|
|
|
masterBody->current->worldOrigin = masterOrigin;
|
|
|
|
masterBody->current->worldAxis = masterAxis;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// if the simulation is suspended because the figure is at rest
|
2012-11-28 15:47:07 +00:00
|
|
|
if( current.atRest >= 0 || timeStep <= 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
DebugDraw();
|
|
|
|
return false;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// move the af velocity into the frame of a pusher
|
|
|
|
AddPushVelocity( -current.pushVelocity );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_total.Start();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_collision.Start();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// evaluate contacts
|
|
|
|
EvaluateContacts();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// setup contact constraints
|
|
|
|
SetupContactConstraints();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_collision.Stop();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// evaluate constraint equations
|
|
|
|
EvaluateConstraints( timeStep );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// apply friction
|
|
|
|
ApplyFriction( timeStep, endTimeMSec );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add frame constraints
|
|
|
|
AddFrameConstraints();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
int i, numPrimary = 0, numAuxiliary = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < primaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
numPrimary += primaryConstraints[i]->J1.GetNumRows();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
numAuxiliary += auxiliaryConstraints[i]->J1.GetNumRows();
|
|
|
|
}
|
|
|
|
timer_pc.Start();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// factor matrices for primary constraints
|
|
|
|
PrimaryFactor();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate forces on bodies after applying primary constraints
|
|
|
|
PrimaryForces( timeStep );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_pc.Stop();
|
|
|
|
timer_ac.Start();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// calculate and apply auxiliary constraint forces
|
|
|
|
AuxiliaryForces( timeStep );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_ac.Stop();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// evolve current state to next state
|
|
|
|
Evolve( timeStep );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// debug graphics
|
|
|
|
DebugDraw();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// clear external forces on all bodies
|
|
|
|
ClearExternalForce();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// apply contact force to other entities
|
|
|
|
ApplyContactForces();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// remove all frame constraints
|
|
|
|
RemoveFrameConstraints();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_collision.Start();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// check for collisions between current and next state
|
|
|
|
CheckForCollisions( timeStep );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_collision.Stop();
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// swap the current and next state
|
|
|
|
SwapStates();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// make sure all clip models are disabled in case they were enabled for self collision
|
2012-11-28 15:47:07 +00:00
|
|
|
if( selfCollision && !af_skipSelfCollision.GetBool() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
DisableClip();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// apply collision impulses
|
2012-11-28 15:47:07 +00:00
|
|
|
if( ApplyCollisions( timeStep ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
current.atRest = gameLocal.time;
|
|
|
|
comeToRest = true;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// test if the simulation can be suspended because the whole figure is at rest
|
2012-11-28 15:47:07 +00:00
|
|
|
if( comeToRest && TestIfAtRest( timeStep ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
Rest();
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
ActivateContactEntities();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add gravitational force
|
|
|
|
AddGravity();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// move the af velocity back into the world frame
|
|
|
|
AddPushVelocity( current.pushVelocity );
|
|
|
|
current.pushVelocity.Zero();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( IsOutsideWorld() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "articulated figure moved outside world bounds for entity '%s' type '%s' at (%s)",
|
2012-11-28 15:47:07 +00:00
|
|
|
self->name.c_str(), self->GetType()->classname, bodies[0]->current->worldOrigin.ToString( 0 ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
Rest();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
timer_total.Stop();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showTimings.GetInteger() == 1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Printf( "%12s: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
|
2012-11-28 15:47:07 +00:00
|
|
|
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 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
numArticulatedFigures++;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( endTimeMSec > lastTimerReset )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Printf( "af %d: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n",
|
2012-11-28 15:47:07 +00:00
|
|
|
numArticulatedFigures,
|
|
|
|
timer_total.Milliseconds(),
|
|
|
|
numPrimary, timer_pc.Milliseconds(),
|
|
|
|
numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(),
|
|
|
|
timer_lcp.Milliseconds(), timer_collision.Milliseconds() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( endTimeMSec > lastTimerReset )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
lastTimerReset = endTimeMSec;
|
|
|
|
numArticulatedFigures = 0;
|
|
|
|
timer_total.Clear();
|
|
|
|
timer_pc.Clear();
|
|
|
|
timer_ac.Clear();
|
|
|
|
timer_collision.Clear();
|
|
|
|
timer_lcp.Clear();
|
|
|
|
}
|
|
|
|
#endif
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::UpdateTime
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::UpdateTime( int endTimeMSec )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetTime
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetTime() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return gameLocal.time;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
DrawTraceModelSilhouette
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void DrawTraceModelSilhouette( const idVec3& projectionOrigin, const idClipModel* clipModel )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, numSilEdges;
|
|
|
|
int silEdges[MAX_TRACEMODEL_EDGES];
|
|
|
|
idVec3 v1, v2;
|
2012-11-28 15:47:07 +00:00
|
|
|
const idTraceModel* trm = clipModel->GetTraceModel();
|
|
|
|
const idVec3& origin = clipModel->GetOrigin();
|
|
|
|
const idMat3& axis = clipModel->GetAxis();
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
numSilEdges = trm->GetProjectionSilhouetteEdges( ( projectionOrigin - origin ) * axis.Transpose(), silEdges );
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < numSilEdges; i++ )
|
|
|
|
{
|
|
|
|
v1 = trm->verts[ trm->edges[ abs( silEdges[i] ) ].v[ INT32_SIGNBITSET( silEdges[i] ) ] ];
|
|
|
|
v2 = trm->verts[ trm->edges[ abs( silEdges[i] ) ].v[ INT32_SIGNBITNOTSET( silEdges[i] ) ] ];
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DebugArrow( colorRed, origin + v1 * axis, origin + v2 * axis, 1 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::DebugDraw
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::DebugDraw()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body, *highlightBody = NULL, *constrainedBody1 = NULL, *constrainedBody2 = NULL;
|
|
|
|
idAFConstraint* constraint;
|
2012-11-26 18:58:24 +00:00
|
|
|
idVec3 center;
|
|
|
|
idMat3 axis;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_highlightConstraint.GetString()[0] )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = GetConstraint( af_highlightConstraint.GetString() );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint->GetCenter( center );
|
|
|
|
axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
|
2012-11-28 15:47:07 +00:00
|
|
|
gameRenderWorld->DebugCone( colorYellow, center, ( axis[2] - axis[1] ) * 4.0f, 0.0f, 1.0f, 0 );
|
|
|
|
|
|
|
|
if( af_showConstrainedBodies.GetBool() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
cvarSystem->SetCVarString( "cm_drawColor", colorCyan.ToString( 0 ) );
|
|
|
|
constrainedBody1 = constraint->body1;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constrainedBody1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
collisionModelManager->DrawModel( constrainedBody1->clipModel->Handle(), constrainedBody1->clipModel->GetOrigin(),
|
2012-11-28 15:47:07 +00:00
|
|
|
constrainedBody1->clipModel->GetAxis(), vec3_origin, 0.0f );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
cvarSystem->SetCVarString( "cm_drawColor", colorBlue.ToString( 0 ) );
|
|
|
|
constrainedBody2 = constraint->body2;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constrainedBody2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
collisionModelManager->DrawModel( constrainedBody2->clipModel->Handle(), constrainedBody2->clipModel->GetOrigin(),
|
2012-11-28 15:47:07 +00:00
|
|
|
constrainedBody2->clipModel->GetAxis(), vec3_origin, 0.0f );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_highlightBody.GetString()[0] )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
highlightBody = GetBody( af_highlightBody.GetString() );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( highlightBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
cvarSystem->SetCVarString( "cm_drawColor", colorYellow.ToString( 0 ) );
|
|
|
|
collisionModelManager->DrawModel( highlightBody->clipModel->Handle(), highlightBody->clipModel->GetOrigin(),
|
2012-11-28 15:47:07 +00:00
|
|
|
highlightBody->clipModel->GetAxis(), vec3_origin, 0.0f );
|
2012-11-26 18:58:24 +00:00
|
|
|
cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showBodies.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body == constrainedBody1 || body == constrainedBody2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body == highlightBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
collisionModelManager->DrawModel( body->clipModel->Handle(), body->clipModel->GetOrigin(),
|
2012-11-28 15:47:07 +00:00
|
|
|
body->clipModel->GetAxis(), vec3_origin, 0.0f );
|
2012-11-26 18:58:24 +00:00
|
|
|
//DrawTraceModelSilhouette( gameLocal.GetLocalPlayer()->GetEyePosition(), body->clipModel );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showBodyNames.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
gameRenderWorld->DrawText( body->GetName().c_str(), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showMass.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
gameRenderWorld->DrawText( va( "\n%1.2f", 1.0f / body->GetInverseMass() ), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showTotalMass.GetBool() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3();
|
|
|
|
gameRenderWorld->DrawText( va( "\n%1.2f", totalMass ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showInertia.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
idMat3& I = body->inertiaTensor;
|
2012-11-26 18:58:24 +00:00
|
|
|
gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )",
|
2012-11-28 15:47:07 +00:00
|
|
|
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 );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showVelocity.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
DrawVelocity( bodies[i]->clipModel->GetId(), 0.1f, 4.0f );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showConstraints.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < primaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = primaryConstraints[i];
|
|
|
|
constraint->DebugDraw();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !af_showPrimaryOnly.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = auxiliaryConstraints[i];
|
|
|
|
constraint->DebugDraw();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showConstraintNames.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < primaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = primaryConstraints[i];
|
|
|
|
constraint->GetCenter( center );
|
|
|
|
gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !af_showPrimaryOnly.GetBool() )
|
|
|
|
{
|
|
|
|
for( i = 0; i < auxiliaryConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraint = auxiliaryConstraints[i];
|
|
|
|
constraint->GetCenter( center );
|
|
|
|
gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_showTrees.GetBool() || ( af_showActive.GetBool() && current.atRest < 0 ) )
|
|
|
|
{
|
|
|
|
for( i = 0; i < trees.Num(); i++ )
|
|
|
|
{
|
|
|
|
trees[i]->DebugDraw( idStr::ColorForIndex( i + 3 ) );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::idPhysics_AF
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idPhysics_AF::idPhysics_AF()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
trees.Clear();
|
|
|
|
bodies.Clear();
|
|
|
|
constraints.Clear();
|
|
|
|
primaryConstraints.Clear();
|
|
|
|
auxiliaryConstraints.Clear();
|
|
|
|
frameConstraints.Clear();
|
|
|
|
contacts.Clear();
|
|
|
|
collisions.Clear();
|
|
|
|
changedAF = true;
|
|
|
|
masterBody = NULL;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
lcp = idLCP::AllocSymmetric();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
memset( ¤t, 0, sizeof( current ) );
|
|
|
|
current.atRest = -1;
|
|
|
|
current.lastTimeStep = 0.0f;
|
|
|
|
saved = current;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
linearFriction = 0.005f;
|
|
|
|
angularFriction = 0.005f;
|
|
|
|
contactFriction = 0.8f;
|
|
|
|
bouncyness = 0.4f;
|
|
|
|
totalMass = 0.0f;
|
|
|
|
forceTotalMass = -1.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
timeScale = 1.0f;
|
|
|
|
timeScaleRampStart = 0.0f;
|
|
|
|
timeScaleRampEnd = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
jointFrictionScale = 0.0f;
|
|
|
|
jointFrictionDent = 0.0f;
|
|
|
|
jointFrictionDentStart = 0.0f;
|
|
|
|
jointFrictionDentEnd = 0.0f;
|
|
|
|
jointFrictionDentScale = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
contactFrictionScale = 0.0f;
|
|
|
|
contactFrictionDent = 0.0f;
|
|
|
|
contactFrictionDentStart = 0.0f;
|
|
|
|
contactFrictionDentEnd = 0.0f;
|
|
|
|
contactFrictionDentScale = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
enableCollision = true;
|
|
|
|
selfCollision = true;
|
|
|
|
comeToRest = true;
|
|
|
|
linearTime = true;
|
|
|
|
noImpact = false;
|
|
|
|
worldConstraintsLocked = false;
|
|
|
|
forcePushable = false;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
#ifdef AF_TIMINGS
|
|
|
|
lastTimerReset = 0;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::~idPhysics_AF
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idPhysics_AF::~idPhysics_AF()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
trees.DeleteContents( true );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete bodies[i];
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete constraints[i];
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
contactConstraints.SetNum( contactConstraints.NumAllocated() );
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < contactConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete contactConstraints[i];
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
delete lcp;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( masterBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete masterBody;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF_SavePState
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF_SavePState( idSaveGame* saveFile, const AFPState_t& state )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteInt( state.atRest );
|
|
|
|
saveFile->WriteFloat( state.noMoveTime );
|
|
|
|
saveFile->WriteFloat( state.activateTime );
|
|
|
|
saveFile->WriteFloat( state.lastTimeStep );
|
|
|
|
saveFile->WriteVec6( state.pushVelocity );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF_RestorePState
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF_RestorePState( idRestoreGame* saveFile, AFPState_t& state )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadInt( state.atRest );
|
|
|
|
saveFile->ReadFloat( state.noMoveTime );
|
|
|
|
saveFile->ReadFloat( state.activateTime );
|
|
|
|
saveFile->ReadFloat( state.lastTimeStep );
|
|
|
|
saveFile->ReadVec6( state.pushVelocity );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Save
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::Save( idSaveGame* saveFile ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// the articulated figure structure is handled by the owner
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
idPhysics_AF_SavePState( saveFile, current );
|
|
|
|
idPhysics_AF_SavePState( saveFile, saved );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteInt( bodies.Num() );
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->Save( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( masterBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( true );
|
|
|
|
masterBody->Save( saveFile );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( false );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteInt( constraints.Num() );
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraints[i]->Save( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( changedAF );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteFloat( linearFriction );
|
|
|
|
saveFile->WriteFloat( angularFriction );
|
|
|
|
saveFile->WriteFloat( contactFriction );
|
|
|
|
saveFile->WriteFloat( bouncyness );
|
|
|
|
saveFile->WriteFloat( totalMass );
|
|
|
|
saveFile->WriteFloat( forceTotalMass );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteVec2( suspendVelocity );
|
|
|
|
saveFile->WriteVec2( suspendAcceleration );
|
|
|
|
saveFile->WriteFloat( noMoveTime );
|
|
|
|
saveFile->WriteFloat( noMoveTranslation );
|
|
|
|
saveFile->WriteFloat( noMoveRotation );
|
|
|
|
saveFile->WriteFloat( minMoveTime );
|
|
|
|
saveFile->WriteFloat( maxMoveTime );
|
|
|
|
saveFile->WriteFloat( impulseThreshold );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteFloat( timeScale );
|
|
|
|
saveFile->WriteFloat( timeScaleRampStart );
|
|
|
|
saveFile->WriteFloat( timeScaleRampEnd );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteFloat( jointFrictionScale );
|
|
|
|
saveFile->WriteFloat( jointFrictionDent );
|
|
|
|
saveFile->WriteFloat( jointFrictionDentStart );
|
|
|
|
saveFile->WriteFloat( jointFrictionDentEnd );
|
|
|
|
saveFile->WriteFloat( jointFrictionDentScale );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteFloat( contactFrictionScale );
|
|
|
|
saveFile->WriteFloat( contactFrictionDent );
|
|
|
|
saveFile->WriteFloat( contactFrictionDentStart );
|
|
|
|
saveFile->WriteFloat( contactFrictionDentEnd );
|
|
|
|
saveFile->WriteFloat( contactFrictionDentScale );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->WriteBool( enableCollision );
|
|
|
|
saveFile->WriteBool( selfCollision );
|
|
|
|
saveFile->WriteBool( comeToRest );
|
|
|
|
saveFile->WriteBool( linearTime );
|
|
|
|
saveFile->WriteBool( noImpact );
|
|
|
|
saveFile->WriteBool( worldConstraintsLocked );
|
|
|
|
saveFile->WriteBool( forcePushable );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Restore
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::Restore( idRestoreGame* saveFile )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, num;
|
|
|
|
bool hasMaster;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// the articulated figure structure should have already been restored
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
idPhysics_AF_RestorePState( saveFile, current );
|
|
|
|
idPhysics_AF_RestorePState( saveFile, saved );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadInt( num );
|
|
|
|
assert( num == bodies.Num() );
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->Restore( saveFile );
|
|
|
|
}
|
|
|
|
saveFile->ReadBool( hasMaster );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( hasMaster )
|
|
|
|
{
|
|
|
|
masterBody = new( TAG_PHYSICS_AF ) idAFBody();
|
2012-11-26 18:58:24 +00:00
|
|
|
masterBody->Restore( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadInt( num );
|
|
|
|
assert( num == constraints.Num() );
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraints[i]->Restore( saveFile );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadBool( changedAF );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadFloat( linearFriction );
|
|
|
|
saveFile->ReadFloat( angularFriction );
|
|
|
|
saveFile->ReadFloat( contactFriction );
|
|
|
|
saveFile->ReadFloat( bouncyness );
|
|
|
|
saveFile->ReadFloat( totalMass );
|
|
|
|
saveFile->ReadFloat( forceTotalMass );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadVec2( suspendVelocity );
|
|
|
|
saveFile->ReadVec2( suspendAcceleration );
|
|
|
|
saveFile->ReadFloat( noMoveTime );
|
|
|
|
saveFile->ReadFloat( noMoveTranslation );
|
|
|
|
saveFile->ReadFloat( noMoveRotation );
|
|
|
|
saveFile->ReadFloat( minMoveTime );
|
|
|
|
saveFile->ReadFloat( maxMoveTime );
|
|
|
|
saveFile->ReadFloat( impulseThreshold );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadFloat( timeScale );
|
|
|
|
saveFile->ReadFloat( timeScaleRampStart );
|
|
|
|
saveFile->ReadFloat( timeScaleRampEnd );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadFloat( jointFrictionScale );
|
|
|
|
saveFile->ReadFloat( jointFrictionDent );
|
|
|
|
saveFile->ReadFloat( jointFrictionDentStart );
|
|
|
|
saveFile->ReadFloat( jointFrictionDentEnd );
|
|
|
|
saveFile->ReadFloat( jointFrictionDentScale );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadFloat( contactFrictionScale );
|
|
|
|
saveFile->ReadFloat( contactFrictionDent );
|
|
|
|
saveFile->ReadFloat( contactFrictionDentStart );
|
|
|
|
saveFile->ReadFloat( contactFrictionDentEnd );
|
|
|
|
saveFile->ReadFloat( contactFrictionDentScale );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saveFile->ReadBool( enableCollision );
|
|
|
|
saveFile->ReadBool( selfCollision );
|
|
|
|
saveFile->ReadBool( comeToRest );
|
|
|
|
saveFile->ReadBool( linearTime );
|
|
|
|
saveFile->ReadBool( noImpact );
|
|
|
|
saveFile->ReadBool( worldConstraintsLocked );
|
|
|
|
saveFile->ReadBool( forcePushable );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
changedAF = true;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
UpdateClipModels();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::IsClosedLoop
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::IsClosedLoop( const idAFBody* body1, const idAFBody* body2 ) const
|
|
|
|
{
|
|
|
|
const idAFBody* b1, *b2;
|
|
|
|
|
|
|
|
for( b1 = body1; b1->parent; b1 = b1->parent )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
for( b2 = body2; b2->parent; b2 = b2->parent )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
return ( b1 == b2 );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::BuildTrees
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::BuildTrees()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
float scale;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* b;
|
|
|
|
idAFConstraint* c;
|
|
|
|
idAFTree* tree;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
primaryConstraints.Clear();
|
|
|
|
auxiliaryConstraints.Clear();
|
|
|
|
trees.DeleteContents( true );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
totalMass = 0.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
b = bodies[i];
|
|
|
|
b->parent = NULL;
|
|
|
|
b->primaryConstraint = NULL;
|
|
|
|
b->constraints.SetNum( 0 );
|
|
|
|
b->children.Clear();
|
|
|
|
b->tree = NULL;
|
|
|
|
totalMass += b->mass;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( forceTotalMass > 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
scale = forceTotalMass / totalMass;
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
b = bodies[i];
|
|
|
|
b->mass *= scale;
|
|
|
|
b->invMass = 1.0f / b->mass;
|
|
|
|
b->inertiaTensor *= scale;
|
|
|
|
b->inverseInertiaTensor = b->inertiaTensor.Inverse();
|
|
|
|
}
|
|
|
|
totalMass = forceTotalMass;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( af_useLinearTime.GetBool() )
|
|
|
|
{
|
|
|
|
|
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c = constraints[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c->body1->constraints.Append( c );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( c->body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c->body2->constraints.Append( c );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// only bilateral constraints between two non-world bodies that do not
|
|
|
|
// create loops can be used as primary constraints
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !c->body1->primaryConstraint && c->fl.allowPrimary && c->body2 != NULL && !IsClosedLoop( c->body1, c->body2 ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c->fl.isPrimary = false;
|
|
|
|
auxiliaryConstraints.Append( c );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// create trees for all parent bodies
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !bodies[i]->parent )
|
|
|
|
{
|
|
|
|
tree = new( TAG_PHYSICS_AF ) idAFTree();
|
2012-11-26 18:58:24 +00:00
|
|
|
tree->sortedBodies.Clear();
|
|
|
|
tree->sortedBodies.Append( bodies[i] );
|
|
|
|
bodies[i]->tree = tree;
|
|
|
|
trees.Append( tree );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// add each child body to the appropriate tree
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( bodies[i]->parent )
|
|
|
|
{
|
|
|
|
for( b = bodies[i]->parent; !b->tree; b = b->parent )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
b->tree->sortedBodies.Append( bodies[i] );
|
|
|
|
bodies[i]->tree = b->tree;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( trees.Num() > 1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "Articulated figure has multiple seperate tree structures for entity '%s' type '%s'.",
|
2012-11-28 15:47:07 +00:00
|
|
|
self->name.c_str(), self->GetType()->classname );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// sort bodies in each tree to make sure parents come first
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < trees.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
trees[i]->SortBodies();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// create a tree for each body
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
tree = new( TAG_PHYSICS_AF ) idAFTree();
|
2012-11-26 18:58:24 +00:00
|
|
|
tree->sortedBodies.Clear();
|
|
|
|
tree->sortedBodies.Append( bodies[i] );
|
|
|
|
bodies[i]->tree = tree;
|
|
|
|
trees.Append( tree );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c = constraints[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
c->body1->constraints.Append( c );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( c->body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
c->body2->constraints.Append( c );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::AddBody( idAFBody* body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int id = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->clipModel == NULL )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddBody: body '%s' has no clip model.", body->name.c_str() );
|
|
|
|
return 0;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( bodies.Find( body ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddBody: body '%s' added twice.", body->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( GetBody( body->name ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
id = bodies.Num();
|
|
|
|
body->clipModel->SetId( id );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->linearFriction < 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->linearFriction = linearFriction;
|
|
|
|
body->angularFriction = angularFriction;
|
|
|
|
body->contactFriction = contactFriction;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( body->bouncyness < 0.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->bouncyness = bouncyness;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !body->fl.clipMaskSet )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body->clipMask = clipMask;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies.Append( body );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
changedAF = true;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return id;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::AddConstraint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::AddConstraint( idAFConstraint* constraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constraints.Find( constraint ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddConstraint: constraint '%s' added twice.", constraint->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( GetConstraint( constraint->name ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddConstraint: a constraint with the name '%s' already exists.", constraint->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !constraint->body1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddConstraint: body1 == NULL on constraint '%s'.", constraint->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !bodies.Find( constraint->body1 ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddConstraint: body1 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constraint->body2 && !bodies.Find( constraint->body2 ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddConstraint: body2 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( constraint->body1 == constraint->body2 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "idPhysics_AF::AddConstraint: body1 and body2 of constraint '%s' are the same.", constraint->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
constraints.Append( constraint );
|
|
|
|
constraint->physics = this;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
changedAF = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::AddFrameConstraint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::AddFrameConstraint( idAFConstraint* constraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
frameConstraints.Append( constraint );
|
|
|
|
constraint->physics = this;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ForceBodyId
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ForceBodyId( idAFBody* body, int newId )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int id;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
id = bodies.FindIndex( body );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( id == -1 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "ForceBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( id != newId )
|
|
|
|
{
|
|
|
|
idAFBody* b = bodies[newId];
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[newId] = bodies[id];
|
|
|
|
bodies[id] = b;
|
|
|
|
changedAF = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetBodyId
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetBodyId( idAFBody* body ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int id;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
id = bodies.FindIndex( body );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( id == -1 && body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "GetBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() );
|
|
|
|
}
|
|
|
|
return id;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetBodyId
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetBodyId( const char* bodyName ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !bodies[i]->name.Icmp( bodyName ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetConstraintId( idAFConstraint* constraint ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int id;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
id = constraints.FindIndex( constraint );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( id == -1 && constraint )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "GetConstraintId: constraint '%s' is not part of the articulated figure.\n", constraint->name.c_str() );
|
|
|
|
}
|
|
|
|
return id;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetConstraintId
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetConstraintId( const char* constraintName ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( constraints[i]->name.Icmp( constraintName ) == 0 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetNumBodies() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies.Num();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetNumConstraints
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetNumConstraints() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return constraints.Num();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetBody
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* idPhysics_AF::GetBody( const char* bodyName ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !bodies[i]->name.Icmp( bodyName ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[i];
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetBody
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* idPhysics_AF::GetBody( const int id ) const
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "GetBody: no body with id %d exists\n", id );
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
return bodies[id];
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetConstraint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint* idPhysics_AF::GetConstraint( const char* constraintName ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( constraints[i]->name.Icmp( constraintName ) == 0 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return constraints[i];
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetConstraint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFConstraint* idPhysics_AF::GetConstraint( const int id ) const
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= constraints.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "GetConstraint: no constraint with id %d exists\n", id );
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
return constraints[id];
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::DeleteBody
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::DeleteBody( const char* bodyName )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// find the body with the given name
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !bodies[i]->name.Icmp( bodyName ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( i >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "DeleteBody: no body found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
|
2012-11-28 15:47:07 +00:00
|
|
|
bodyName, self->name.c_str(), self->GetType()->classname );
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
DeleteBody( i );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::DeleteBody
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::DeleteBody( const int id )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int j;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( id < 0 || id > bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "DeleteBody: no body with id %d.", id );
|
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// remove any constraints attached to this body
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = 0; j < constraints.Num(); j++ )
|
|
|
|
{
|
|
|
|
if( constraints[j]->body1 == bodies[id] || constraints[j]->body2 == bodies[id] )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
delete constraints[j];
|
|
|
|
constraints.RemoveIndex( j );
|
|
|
|
j--;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// remove the body
|
|
|
|
delete bodies[id];
|
|
|
|
bodies.RemoveIndex( id );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// set new body ids
|
2012-11-28 15:47:07 +00:00
|
|
|
for( j = 0; j < bodies.Num(); j++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[j]->clipModel->SetId( j );
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
changedAF = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::DeleteConstraint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::DeleteConstraint( const char* constraintName )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// find the constraint with the given name
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( !constraints[i]->name.Icmp( constraintName ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( i >= constraints.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Warning( "DeleteConstraint: no constriant found in the articulated figure with the name '%s' for entity '%s' type '%s'.",
|
2012-11-28 15:47:07 +00:00
|
|
|
constraintName, self->name.c_str(), self->GetType()->classname );
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
DeleteConstraint( i );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::DeleteConstraint
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::DeleteConstraint( const int id )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
|
2012-11-28 15:47:07 +00:00
|
|
|
if( id < 0 || id >= constraints.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.Error( "DeleteConstraint: no constraint with id %d.", id );
|
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// remove the constraint
|
|
|
|
delete constraints[id];
|
|
|
|
constraints.RemoveIndex( id );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
changedAF = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetBodyContactConstraints
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetBodyContactConstraints( const int id, idAFConstraint_Contact* contacts[], int maxContacts ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, numContacts;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
idAFConstraint_Contact* contact;
|
|
|
|
|
|
|
|
if( id < 0 || id >= bodies.Num() || maxContacts <= 0 )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return 0;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
numContacts = 0;
|
|
|
|
body = bodies[id];
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < contactConstraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contact = contactConstraints[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
if( contact->body1 == body || contact->body2 == body )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contacts[numContacts++] = contact;
|
2012-11-28 15:47:07 +00:00
|
|
|
if( numContacts >= maxContacts )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return numContacts;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return numContacts;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetDefaultFriction
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetDefaultFriction( float linear, float angular, float contact )
|
|
|
|
{
|
|
|
|
if( linear < 0.0f || linear > 1.0f ||
|
2012-11-26 18:58:24 +00:00
|
|
|
angular < 0.0f || angular > 1.0f ||
|
2012-11-28 15:47:07 +00:00
|
|
|
contact < 0.0f || contact > 1.0f )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
linearFriction = linear;
|
|
|
|
angularFriction = angular;
|
|
|
|
contactFriction = contact;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetImpactInfo
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::GetImpactInfo( const int id, const idVec3& point, impactInfo_t* info ) const
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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;
|
2012-11-28 15:47:07 +00:00
|
|
|
info->velocity = bodies[id]->current->spatialVelocity.SubVec3( 0 ) + bodies[id]->current->spatialVelocity.SubVec3( 1 ).Cross( info->position );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ApplyImpulse
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ApplyImpulse( const int id, const idVec3& point, const idVec3& impulse )
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( noImpact || impulse.LengthSqr() < Square( impulseThreshold ) )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
const float maxImpulse = 100000.0f;
|
|
|
|
const float maxRotation = 100000.0f;
|
|
|
|
idMat3 invWorldInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis;
|
2012-11-28 15:47:07 +00:00
|
|
|
bodies[id]->current->spatialVelocity.SubVec3( 0 ) += bodies[id]->invMass * impulse.Truncate( maxImpulse );
|
|
|
|
bodies[id]->current->spatialVelocity.SubVec3( 1 ) += invWorldInertiaTensor * ( point - bodies[id]->current->worldOrigin ).Cross( impulse ).Truncate( maxRotation );
|
2012-11-26 18:58:24 +00:00
|
|
|
Activate();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::AddForce
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::AddForce( const int id, const idVec3& point, const idVec3& force )
|
|
|
|
{
|
|
|
|
if( noImpact )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
bodies[id]->current->externalForce.SubVec3( 0 ) += force;
|
2012-11-28 15:47:07 +00:00
|
|
|
bodies[id]->current->externalForce.SubVec3( 1 ) += ( point - bodies[id]->current->worldOrigin ).Cross( force );
|
2012-11-26 18:58:24 +00:00
|
|
|
Activate();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::IsAtRest
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::IsAtRest() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return current.atRest >= 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetRestStartTime
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::GetRestStartTime() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return current.atRest;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::IsPushable
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
bool idPhysics_AF::IsPushable() const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return ( !noImpact && ( masterBody == NULL || forcePushable ) );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SaveState
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SaveState()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
saved = current;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
memcpy( &bodies[i]->saved, bodies[i]->current, sizeof( AFBodyPState_t ) );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::RestoreState
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::RestoreState()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
current = saved;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
*( bodies[i]->current ) = bodies[i]->saved;
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
EvaluateContacts();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetOrigin
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetOrigin( const idVec3& newOrigin, int id )
|
|
|
|
{
|
|
|
|
if( masterBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
Translate( masterBody->current->worldOrigin + masterBody->current->worldAxis * newOrigin - bodies[0]->current->worldOrigin );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
Translate( newOrigin - bodies[0]->current->worldOrigin );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetAxis
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetAxis( const idMat3& newAxis, int id )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
idMat3 axis;
|
|
|
|
idRotation rotation;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( masterBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = bodies[0]->current->worldAxis.Transpose() * ( newAxis * masterBody->current->worldAxis );
|
2012-11-28 15:47:07 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
axis = bodies[0]->current->worldAxis.Transpose() * newAxis;
|
|
|
|
}
|
|
|
|
rotation = axis.ToRotation();
|
|
|
|
rotation.SetOrigin( bodies[0]->current->worldOrigin );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
Rotate( rotation );
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Translate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::Translate( const idVec3& translation, int id )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
if( !worldConstraintsLocked )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// translate constraints attached to the world
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraints[i]->Translate( translation );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// translate all the bodies
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
|
|
|
body->current->worldOrigin += translation;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
Activate();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
UpdateClipModels();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::Rotate
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::Rotate( const idRotation& rotation, int id )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
|
|
|
if( !worldConstraintsLocked )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// rotate constraints attached to the world
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraints[i]->Rotate( rotation );
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// rotate all the bodies
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
body->current->worldOrigin *= rotation;
|
|
|
|
body->current->worldAxis *= rotation.ToMat3();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
Activate();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
UpdateClipModels();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetOrigin
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idVec3& idPhysics_AF::GetOrigin( int id ) const
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return vec3_origin;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->current->worldOrigin;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetAxis
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idMat3& idPhysics_AF::GetAxis( int id ) const
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return mat3_identity;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->current->worldAxis;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetLinearVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetLinearVelocity( const idVec3& newLinearVelocity, int id )
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
bodies[id]->current->spatialVelocity.SubVec3( 0 ) = newLinearVelocity;
|
|
|
|
Activate();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetAngularVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetAngularVelocity( const idVec3& newAngularVelocity, int id )
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
bodies[id]->current->spatialVelocity.SubVec3( 1 ) = newAngularVelocity;
|
|
|
|
Activate();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetLinearVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idVec3& idPhysics_AF::GetLinearVelocity( int id ) const
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return vec3_origin;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->current->spatialVelocity.SubVec3( 0 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetAngularVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idVec3& idPhysics_AF::GetAngularVelocity( int id ) const
|
|
|
|
{
|
|
|
|
if( id < 0 || id >= bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return vec3_origin;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
return bodies[id]->current->spatialVelocity.SubVec3( 1 );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ClipTranslation
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ClipTranslation( trace_t& results, const idVec3& translation, const idClipModel* model ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
trace_t bodyResults;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
results.fraction = 1.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->clipModel->IsTraceModel() )
|
|
|
|
{
|
|
|
|
if( model )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.clip.TranslationModel( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
|
2012-11-28 15:47:07 +00:00
|
|
|
body->clipModel, body->current->worldAxis, body->clipMask,
|
|
|
|
model->Handle(), model->GetOrigin(), model->GetAxis() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.clip.Translation( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation,
|
2012-11-28 15:47:07 +00:00
|
|
|
body->clipModel, body->current->worldAxis, body->clipMask, self );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( bodyResults.fraction < results.fraction )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
results = bodyResults;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
results.endpos = bodies[0]->current->worldOrigin + results.fraction * translation;
|
|
|
|
results.endAxis = bodies[0]->current->worldAxis;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ClipRotation
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ClipRotation( trace_t& results, const idRotation& rotation, const idClipModel* model ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
trace_t bodyResults;
|
|
|
|
idRotation partialRotation;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
results.fraction = 1.0f;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->clipModel->IsTraceModel() )
|
|
|
|
{
|
|
|
|
if( model )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.clip.RotationModel( bodyResults, body->current->worldOrigin, rotation,
|
2012-11-28 15:47:07 +00:00
|
|
|
body->clipModel, body->current->worldAxis, body->clipMask,
|
|
|
|
model->Handle(), model->GetOrigin(), model->GetAxis() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
gameLocal.clip.Rotation( bodyResults, body->current->worldOrigin, rotation,
|
2012-11-28 15:47:07 +00:00
|
|
|
body->clipModel, body->current->worldAxis, body->clipMask, self );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
if( bodyResults.fraction < results.fraction )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
results = bodyResults;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
partialRotation = rotation * results.fraction;
|
|
|
|
results.endpos = bodies[0]->current->worldOrigin * partialRotation;
|
|
|
|
results.endAxis = bodies[0]->current->worldAxis * partialRotation.ToMat3();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ClipContents
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
int idPhysics_AF::ClipContents( const idClipModel* model ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, contents;
|
2012-11-28 15:47:07 +00:00
|
|
|
idAFBody* body;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
contents = 0;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[i];
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( body->clipModel->IsTraceModel() )
|
|
|
|
{
|
|
|
|
if( model )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contents |= gameLocal.clip.ContentsModel( body->current->worldOrigin,
|
2012-11-28 15:47:07 +00:00
|
|
|
body->clipModel, body->current->worldAxis, -1,
|
|
|
|
model->Handle(), model->GetOrigin(), model->GetAxis() );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
contents |= gameLocal.clip.Contents( body->current->worldOrigin,
|
2012-11-28 15:47:07 +00:00
|
|
|
body->clipModel, body->current->worldAxis, -1, NULL );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
return contents;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::DisableClip
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::DisableClip()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->clipModel->Disable();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::EnableClip
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::EnableClip()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->clipModel->Enable();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::UnlinkClip
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::UnlinkClip()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
bodies[i]->clipModel->Unlink();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::LinkClip
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::LinkClip()
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
UpdateClipModels();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetPushed
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetPushed( int deltaTime )
|
|
|
|
{
|
|
|
|
idAFBody* body;
|
2012-11-26 18:58:24 +00:00
|
|
|
idRotation rotation;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( bodies.Num() )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
body = bodies[0];
|
|
|
|
rotation = ( body->saved.worldAxis.Transpose() * body->current->worldAxis ).ToRotation();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
// velocity with which the af is pushed
|
2012-11-28 15:47:07 +00:00
|
|
|
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 );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetPushedLinearVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idVec3& idPhysics_AF::GetPushedLinearVelocity( const int id ) const
|
|
|
|
{
|
|
|
|
return current.pushVelocity.SubVec3( 0 );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::GetPushedAngularVelocity
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
const idVec3& idPhysics_AF::GetPushedAngularVelocity( const int id ) const
|
|
|
|
{
|
|
|
|
return current.pushVelocity.SubVec3( 1 );
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::SetMaster
|
|
|
|
|
|
|
|
the binding is orientated based on the constraints being used
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::SetMaster( idEntity* master, const bool orientated )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
idVec3 masterOrigin;
|
|
|
|
idMat3 masterAxis;
|
|
|
|
idRotation rotation;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
if( master )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
self->GetMasterPosition( masterOrigin, masterAxis );
|
2012-11-28 15:47:07 +00:00
|
|
|
if( !masterBody )
|
|
|
|
{
|
|
|
|
masterBody = new( TAG_PHYSICS_AF ) idAFBody();
|
2012-11-26 18:58:24 +00:00
|
|
|
// translate and rotate all the constraints with body2 == NULL from world space to master space
|
|
|
|
rotation = masterAxis.Transpose().ToRotation();
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( constraints[i]->GetBody2() == NULL )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
constraints[i]->Translate( -masterOrigin );
|
|
|
|
constraints[i]->Rotate( rotation );
|
|
|
|
}
|
|
|
|
}
|
|
|
|
Activate();
|
|
|
|
}
|
|
|
|
masterBody->current->worldOrigin = masterOrigin;
|
|
|
|
masterBody->current->worldAxis = masterAxis;
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
if( masterBody )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
// translate and rotate all the constraints with body2 == NULL from master space to world space
|
|
|
|
rotation = masterBody->current->worldAxis.ToRotation();
|
2012-11-28 15:47:07 +00:00
|
|
|
for( i = 0; i < constraints.Num(); i++ )
|
|
|
|
{
|
|
|
|
if( constraints[i]->GetBody2() == NULL )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
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
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::WriteToSnapshot( idBitMsg& msg ) const
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i;
|
|
|
|
idCQuat quat;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
msg.WriteLong( 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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
msg.WriteByte( bodies.Num() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
AFBodyPState_t* state = bodies[i]->current;
|
2012-11-26 18:58:24 +00:00
|
|
|
quat = state->worldAxis.ToCQuat();
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
/* 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 );
|
|
|
|
*/
|
2012-11-26 18:58:24 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
================
|
|
|
|
idPhysics_AF::ReadFromSnapshot
|
|
|
|
================
|
|
|
|
*/
|
2012-11-28 15:47:07 +00:00
|
|
|
void idPhysics_AF::ReadFromSnapshot( const idBitMsg& msg )
|
|
|
|
{
|
2012-11-26 18:58:24 +00:00
|
|
|
int i, num;
|
|
|
|
idCQuat quat;
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
current.atRest = msg.ReadLong();
|
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
num = msg.ReadByte();
|
|
|
|
assert( num == bodies.Num() );
|
2012-11-28 15:47:07 +00:00
|
|
|
|
|
|
|
for( i = 0; i < bodies.Num(); i++ )
|
|
|
|
{
|
|
|
|
AFBodyPState_t* state = bodies[i]->current;
|
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
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 );
|
2012-11-28 15:47:07 +00:00
|
|
|
/* 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 );
|
|
|
|
*/
|
2012-11-26 18:58:24 +00:00
|
|
|
state->worldAxis = quat.ToMat3();
|
|
|
|
}
|
2012-11-28 15:47:07 +00:00
|
|
|
|
2012-11-26 18:58:24 +00:00
|
|
|
UpdateClipModels();
|
|
|
|
}
|