void idPhysics_RigidBody::SetClipModel(idClipModel *model, const float density, int id, bool freeOld)
{
int minIndex;
idMat3 inertiaScale;
assert(self);
assert(model); // we need a clip model
assert(model->IsTraceModel()); // and it should be a trace model
assert(density > 0.0f); // density should be valid
if (clipModel && clipModel != model && freeOld) {
delete clipModel;
}
clipModel = model;
clipModel->Link(gameLocal.clip, self, 0, current.i.position, current.i.orientation);
// get mass properties from the trace model
clipModel->GetMassProperties(density, mass, centerOfMass, inertiaTensor);
// check whether or not the clip model has valid mass properties
if (mass <= 0.0f || FLOAT_IS_NAN(mass)) {
gameLocal.Warning("idPhysics_RigidBody::SetClipModel: invalid mass for entity '%s' type '%s'",
self->name.c_str(), self->GetType()->classname);
mass = 1.0f;
centerOfMass.Zero();
inertiaTensor.Identity();
}
// check whether or not the inertia tensor is balanced
minIndex = Min3Index(inertiaTensor[0][0], inertiaTensor[1][1], inertiaTensor[2][2]);
inertiaScale.Identity();
inertiaScale[0][0] = inertiaTensor[0][0] / inertiaTensor[minIndex][minIndex];
inertiaScale[1][1] = inertiaTensor[1][1] / inertiaTensor[minIndex][minIndex];
inertiaScale[2][2] = inertiaTensor[2][2] / inertiaTensor[minIndex][minIndex];
if (inertiaScale[0][0] > MAX_INERTIA_SCALE || inertiaScale[1][1] > MAX_INERTIA_SCALE || inertiaScale[2][2] > MAX_INERTIA_SCALE) {
gameLocal.DWarning("idPhysics_RigidBody::SetClipModel: unbalanced inertia tensor for entity '%s' type '%s'",
self->name.c_str(), self->GetType()->classname);
float min = inertiaTensor[minIndex][minIndex] * MAX_INERTIA_SCALE;
inertiaScale[(minIndex+1)%3][(minIndex+1)%3] = min / inertiaTensor[(minIndex+1)%3][(minIndex+1)%3];
inertiaScale[(minIndex+2)%3][(minIndex+2)%3] = min / inertiaTensor[(minIndex+2)%3][(minIndex+2)%3];
inertiaTensor *= inertiaScale;
}
inverseMass = 1.0f / mass;
inverseInertiaTensor = inertiaTensor.Inverse() * (1.0f / 6.0f);
current.i.linearMomentum.Zero();
current.i.angularMomentum.Zero();
}
/*
================
idForce_Spring::Evaluate
================
*/
void idForce_Spring::Evaluate( int time ) {
float length;
idMat3 axis;
idVec3 pos1, pos2, velocity1, velocity2, force, dampingForce;
impactInfo_t info;
pos1 = p1;
pos2 = p2;
velocity1 = velocity2 = vec3_origin;
if ( physics1.IsValid() ) { // HUMANHEAD mdl: Added IsValid()
axis = physics1->GetPhysics()->GetAxis( id1 ); // HUMANHEAD: Added GetPhysics()
pos1 = physics1->GetPhysics()->GetOrigin( id1 ); // HUMANHEAD: Added GetPhysics()
pos1 += p1 * axis;
if ( damping > 0.0f ) {
physics1->GetPhysics()->GetImpactInfo( id1, pos1, &info ); // HUMANHEAD: Added GetPhysics()
velocity1 = info.velocity;
}
}
if ( physics2.IsValid() ) { // HUMANHEAD mdl: Added IsValid()
axis = physics2->GetPhysics()->GetAxis( id2 ); // HUMANHEAD: Added GetPhysics()
pos2 = physics2->GetPhysics()->GetOrigin( id2 ); // HUMANHEAD: Added GetPhysics()
pos2 += p2 * axis;
if ( damping > 0.0f ) {
physics2->GetPhysics()->GetImpactInfo( id2, pos2, &info ); // HUMANHEAD: Added GetPhysics()
velocity2 = info.velocity;
}
}
force = pos2 - pos1;
#ifdef _HH_FLOAT_PROTECTION //HUMANHEAD rww
if (FLOAT_IS_NAN(velocity1.x) || FLOAT_IS_NAN(velocity1.y) || FLOAT_IS_NAN(velocity1.z) ||
FLOAT_IS_NAN(velocity2.x) || FLOAT_IS_NAN(velocity2.y) || FLOAT_IS_NAN(velocity2.z)) {
gameLocal.DWarning( "idForce_Spring::Evaluate: NaN velocity." );
return;
}
#endif //HUMANHEAD END
if (force == vec3_origin) { //HUMANHEAD rww
//gameLocal.Warning( "idForce_Spring::Evaluate: force equal to zero." );
dampingForce = vec3_origin;
}
else { //HUMANHEAD END
dampingForce = ( damping * ( ((velocity2 - velocity1) * force) / (force * force) ) ) * force;
}
length = force.Normalize();
// if the spring is stretched
if ( length > restLength ) {
if ( Kstretch > 0.0f ) {
force = ( Square( length - restLength ) * Kstretch ) * force - dampingForce;
if ( physics1.IsValid() ) { // HUMANHEAD mdl: Added IsValid()
physics1->GetPhysics()->AddForce( id1, pos1, force ); // HUMANHEAD: Added GetPhysics()
}
if ( physics2.IsValid() ) { // HUMANHEAD mdl: Added IsValid()
physics2->GetPhysics()->AddForce( id2, pos2, -force ); // HUMANHEAD: Added GetPhysics()
}
}
}
else {
if ( Kcompress > 0.0f ) {
force = ( Square( length - restLength ) * Kcompress ) * force - dampingForce;
if ( physics1.IsValid() ) { // HUMANHEAD mdl: Added IsValid()
physics1->GetPhysics()->AddForce( id1, pos1, -force ); // HUMANHEAD: Added GetPhysics()
}
if ( physics2.IsValid() ) { // HUMANHEAD mdl: Added IsValid()
physics2->GetPhysics()->AddForce( id2, pos2, force ); // HUMANHEAD: Added GetPhysics()
}
}
}
}
/*
============
Matrix3::InverseFastSelf
============
*/
bool Matrix3::InverseFastSelf( void ) {
#if 1
// 18+3+9 = 30 multiplications
// 1 division
Matrix3 inverse;
DOUBLE det, invDet;
inverse[0][0] = mColumns[1][1] * mColumns[2][2] - mColumns[1][2] * mColumns[2][1];
inverse[1][0] = mColumns[1][2] * mColumns[2][0] - mColumns[1][0] * mColumns[2][2];
inverse[2][0] = mColumns[1][0] * mColumns[2][1] - mColumns[1][1] * mColumns[2][0];
det = mColumns[0][0] * inverse[0][0] + mColumns[0][1] * inverse[1][0] + mColumns[0][2] * inverse[2][0];
if ( fabs( det ) < MATRIX_INVERSE_EPSILON ) {
return false;
}
invDet = 1.0f / det;
inverse[0][1] = mColumns[0][2] * mColumns[2][1] - mColumns[0][1] * mColumns[2][2];
inverse[0][2] = mColumns[0][1] * mColumns[1][2] - mColumns[0][2] * mColumns[1][1];
inverse[1][1] = mColumns[0][0] * mColumns[2][2] - mColumns[0][2] * mColumns[2][0];
inverse[1][2] = mColumns[0][2] * mColumns[1][0] - mColumns[0][0] * mColumns[1][2];
inverse[2][1] = mColumns[0][1] * mColumns[2][0] - mColumns[0][0] * mColumns[2][1];
inverse[2][2] = mColumns[0][0] * mColumns[1][1] - mColumns[0][1] * mColumns[1][0];
mColumns[0][0] = inverse[0][0] * invDet;
mColumns[0][1] = inverse[0][1] * invDet;
mColumns[0][2] = inverse[0][2] * invDet;
mColumns[1][0] = inverse[1][0] * invDet;
mColumns[1][1] = inverse[1][1] * invDet;
mColumns[1][2] = inverse[1][2] * invDet;
mColumns[2][0] = inverse[2][0] * invDet;
mColumns[2][1] = inverse[2][1] * invDet;
mColumns[2][2] = inverse[2][2] * invDet;
return true;
#elif 0
// 3*10 = 30 multiplications
// 3 divisions
FLOAT *mColumns = reinterpret_cast<FLOAT *>(this);
FLOAT s;
DOUBLE d, di;
di = mColumns[0];
s = di;
mColumns[0] = d = 1.0f / di;
mColumns[1] *= d;
mColumns[2] *= d;
d = -d;
mColumns[3] *= d;
mColumns[6] *= d;
d = mColumns[3] * di;
mColumns[4] += mColumns[1] * d;
mColumns[5] += mColumns[2] * d;
d = mColumns[6] * di;
mColumns[7] += mColumns[1] * d;
mColumns[8] += mColumns[2] * d;
di = mColumns[4];
s *= di;
mColumns[4] = d = 1.0f / di;
mColumns[3] *= d;
mColumns[5] *= d;
d = -d;
mColumns[1] *= d;
mColumns[7] *= d;
d = mColumns[1] * di;
mColumns[0] += mColumns[3] * d;
mColumns[2] += mColumns[5] * d;
d = mColumns[7] * di;
mColumns[6] += mColumns[3] * d;
mColumns[8] += mColumns[5] * d;
di = mColumns[8];
s *= di;
mColumns[8] = d = 1.0f / di;
mColumns[6] *= d;
mColumns[7] *= d;
d = -d;
mColumns[2] *= d;
mColumns[5] *= d;
d = mColumns[2] * di;
mColumns[0] += mColumns[6] * d;
mColumns[1] += mColumns[7] * d;
d = mColumns[5] * di;
mColumns[3] += mColumns[6] * d;
mColumns[4] += mColumns[7] * d;
return ( s != 0.0f && !FLOAT_IS_NAN( s ) );
#else
// 4*2+4*4 = 24 multiplications
// 2*1 = 2 divisions
Matrix2 r0;
FLOAT r1[2], r2[2], r3;
FLOAT det, invDet;
FLOAT *mColumns = reinterpret_cast<FLOAT *>(this);
// r0 = m0.Inverse(); // 2x2
det = mColumns[0*3+0] * mColumns[1*3+1] - mColumns[0*3+1] * mColumns[1*3+0];
if ( fabs( det ) < MATRIX_INVERSE_EPSILON ) {
return false;
}
invDet = 1.0f / det;
r0[0][0] = mColumns[1*3+1] * invDet;
r0[0][1] = - mColumns[0*3+1] * invDet;
r0[1][0] = - mColumns[1*3+0] * invDet;
r0[1][1] = mColumns[0*3+0] * invDet;
// r1 = r0 * m1; // 2x1 = 2x2 * 2x1
r1[0] = r0[0][0] * mColumns[0*3+2] + r0[0][1] * mColumns[1*3+2];
r1[1] = r0[1][0] * mColumns[0*3+2] + r0[1][1] * mColumns[1*3+2];
// r2 = m2 * r1; // 1x1 = 1x2 * 2x1
r2[0] = mColumns[2*3+0] * r1[0] + mColumns[2*3+1] * r1[1];
// r3 = r2 - m3; // 1x1 = 1x1 - 1x1
r3 = r2[0] - mColumns[2*3+2];
// r3.InverseSelf();
if ( fabs( r3 ) < MATRIX_INVERSE_EPSILON ) {
return false;
}
r3 = 1.0f / r3;
// r2 = m2 * r0; // 1x2 = 1x2 * 2x2
r2[0] = mColumns[2*3+0] * r0[0][0] + mColumns[2*3+1] * r0[1][0];
r2[1] = mColumns[2*3+0] * r0[0][1] + mColumns[2*3+1] * r0[1][1];
// m2 = r3 * r2; // 1x2 = 1x1 * 1x2
mColumns[2*3+0] = r3 * r2[0];
mColumns[2*3+1] = r3 * r2[1];
// m0 = r0 - r1 * m2; // 2x2 - 2x1 * 1x2
mColumns[0*3+0] = r0[0][0] - r1[0] * mColumns[2*3+0];
mColumns[0*3+1] = r0[0][1] - r1[0] * mColumns[2*3+1];
mColumns[1*3+0] = r0[1][0] - r1[1] * mColumns[2*3+0];
mColumns[1*3+1] = r0[1][1] - r1[1] * mColumns[2*3+1];
// m1 = r1 * r3; // 2x1 = 2x1 * 1x1
mColumns[0*3+2] = r1[0] * r3;
mColumns[1*3+2] = r1[1] * r3;
// m3 = -r3;
mColumns[2*3+2] = -r3;
return true;
#endif
}
bool sdTransportPositionManager::EjectPlayer( sdVehiclePosition& position, bool force ) {
idPlayer* player = position.GetPlayer();
if ( !player ) {
return true;
}
//
// Find a position to eject to
//
bool foundOrg = false;
idVec3 selectedOrg = player->GetPhysics()->GetOrigin();
idMat3 selectedAxes = player->GetPhysics()->GetAxis();
if ( transport->UnbindOnEject() ) {
if( !gameLocal.isClient ) {
player->DisableClip( false );
sdTeleporter* teleportEnt = transport->GetTeleportEntity();
if ( teleportEnt != NULL ) {
teleportEnt->GetTeleportEndPoint( transport, selectedOrg, selectedAxes );
selectedOrg.z += 64.f;
foundOrg = true;
} else {
// prioritize exit joints by the nearest
idStaticList< sdExitJointDistanceInfo, MAX_EXIT_JOINTS > sortedExitJoints;
sortedExitJoints.SetNum( exitJoints.Num() );
idVec3 traceFromPoint;
transport->GetWorldOrigin( position.GetAttachJoint(), traceFromPoint );
for( int i = 0; i < exitJoints.Num(); i++ ) {
sortedExitJoints[ i ].joint = exitJoints[ i ];
transport->GetWorldOriginAxis( exitJoints[ i ], sortedExitJoints[ i ].origin, sortedExitJoints[ i ].axis );
sortedExitJoints[ i ].distanceSqr = ( traceFromPoint - sortedExitJoints[ i ].origin ).LengthSqr();
}
sortedExitJoints.Sort( sdExitJointDistanceInfo::SortByDistance );
// choose a point to do the cast-to-exit-point from - if we just use the origin it could
// potentially be in all sorts of wacky positions depending how the vehicle is built
// this enures the the point casted from is inside the vehicle
traceFromPoint = transport->GetPhysics()->GetAxis().TransposeMultiply( traceFromPoint - transport->GetPhysics()->GetOrigin() );
const idBounds& transportBounds = transport->GetPhysics()->GetBounds();
traceFromPoint.z = ( transportBounds[ 0 ].z + transportBounds[ 1 ].z ) * 0.5f;
traceFromPoint = traceFromPoint * transport->GetPhysics()->GetAxis() + transport->GetPhysics()->GetOrigin();
// default position to get out is inside the vehicle
selectedOrg = traceFromPoint;
selectedAxes;
const idClipModel* playerClip = player->GetPlayerPhysics().GetNormalClipModel();
for ( int i = 0; i < sortedExitJoints.Num(); i++ ) {
idVec3 org = sortedExitJoints[ i ].origin;
idMat3 axes = sortedExitJoints[ i ].axis;
if ( gameRenderWorld->PointInArea( org ) == -1 ) {
// outside the map, so no go
continue;
}
// check that the point is clear
int contents = gameLocal.clip.Contents( CLIP_DEBUG_PARMS org, playerClip, mat3_identity, MASK_PLAYERSOLID, NULL );
if( !contents ) {
// check that theres nothing in between the vehicle and the exit point
trace_t trace;
if( !gameLocal.clip.TracePoint( CLIP_DEBUG_PARMS trace, traceFromPoint, org, MASK_PLAYERSOLID, transport ) ) {
selectedOrg = org;
selectedAxes = axes;
foundOrg = true;
break;
}
}
}
if( !foundOrg ) {
// Search all 8 positions around every exit joint, should find at least one.
for ( int i = 0; i < sortedExitJoints.Num(); i++ ) {
idVec3 orgBase = sortedExitJoints[ i ].origin;
idMat3 axes = sortedExitJoints[ i ].axis;
const int size = playerClip->GetBounds().GetSize().x;
const int spacing = 8;
for ( int j = -1; j < 2 && !foundOrg; j++ ) {
for ( int k = -1; k < 2 && !foundOrg; k++ ) {
if ( j == 0 && k == 0 ) {
continue;
}
idVec3 org = orgBase + idVec3( j * size + j * spacing, k * size + k * spacing, 0.0f );
if ( gameRenderWorld->PointInArea( org ) == -1 ) {
// outside the map, so no go
continue;
}
// check that the point is clear
int contents = gameLocal.clip.Contents( CLIP_DEBUG_PARMS org, playerClip, mat3_identity, MASK_PLAYERSOLID, NULL );
if( !contents ) {
// check that theres nothing in between the vehicle and the exit point
trace_t trace;
if( !gameLocal.clip.TracePoint( CLIP_DEBUG_PARMS trace, traceFromPoint, org, MASK_PLAYERSOLID, transport ) ) {
selectedOrg = org;
selectedAxes = axes;
foundOrg = true;
}
}
}
}
}
}
}
}
}
if ( !gameLocal.isClient ) {
if ( !foundOrg ) {
if ( !force ) {
return false;
} else {
gameLocal.Warning( "sdTransportPositionManager::EjectPlayer No Valid Eject Position Found" );
}
}
}
//
// Actually eject
//
player->SetSuppressPredictionReset( true );
RemovePlayer( position );
if ( transport->UnbindOnEject() ) {
// copy the velocity over
idVec3 v = transport->GetPhysics()->GetLinearVelocity();
for ( int i = 0; i < 3; i++ ) {
if ( FLOAT_IS_NAN( v[ i ] ) ) {
v[ i ] = 0.f;
}
}
v.FixDenormals();
player->GetPhysics()->SetLinearVelocity( v );
// set the position
if ( foundOrg ) {
idAngles temp;
temp = selectedAxes.ToAngles();
temp.roll = 0.0f;
if ( temp.pitch < -10.0f ) {
temp.pitch = -10.0f;
}
player->SetViewAngles( temp );
player->SetOrigin( selectedOrg );
}
player->EnableClip();
}
player->SetProxyEntity( NULL, 0 );
// this forces the reset message to be re-sent
player->SetSuppressPredictionReset( false );
player->ResetPredictionErrorDecay();
return true;
}
