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();
}
/*
============
idBox::AddPoint
============
*/
bool idBox::AddPoint( const idVec3& v )
{
idMat3 axis2;
idBounds bounds1, bounds2;
if( extents[0] < 0.0f )
{
extents.Zero();
center = v;
axis.Identity();
return true;
}
bounds1[0][0] = bounds1[1][0] = center * axis[0];
bounds1[0][1] = bounds1[1][1] = center * axis[1];
bounds1[0][2] = bounds1[1][2] = center * axis[2];
bounds1[0] -= extents;
bounds1[1] += extents;
if( !bounds1.AddPoint( idVec3( v * axis[0], v * axis[1], v * axis[2] ) ) )
{
// point is contained in the box
return false;
}
axis2[0] = v - center;
axis2[0].Normalize();
axis2[1] = axis[ Min3Index( axis2[0] * axis[0], axis2[0] * axis[1], axis2[0] * axis[2] ) ];
axis2[1] = axis2[1] - ( axis2[1] * axis2[0] ) * axis2[0];
axis2[1].Normalize();
axis2[2].Cross( axis2[0], axis2[1] );
AxisProjection( axis2, bounds2 );
bounds2.AddPoint( idVec3( v * axis2[0], v * axis2[1], v * axis2[2] ) );
// create new box based on the smallest bounds
if( bounds1.GetVolume() < bounds2.GetVolume() )
{
center = ( bounds1[0] + bounds1[1] ) * 0.5f;
extents = bounds1[1] - center;
center *= axis;
}
else
{
center = ( bounds2[0] + bounds2[1] ) * 0.5f;
extents = bounds2[1] - center;
center *= axis2;
axis = axis2;
}
return true;
}
/*
============
idBox::AddBox
============
*/
bool idBox::AddBox( const idBox &a ) {
int i, besti;
float v, bestv;
idVec3 dir;
idMat3 ax[4];
idBounds bounds[4], b;
if ( a.extents[0] < 0.0f ) {
return false;
}
if ( extents[0] < 0.0f ) {
center = a.center;
extents = a.extents;
axis = a.axis;
return true;
}
// test axis of this box
ax[0] = axis;
bounds[0][0][0] = bounds[0][1][0] = center * ax[0][0];
bounds[0][0][1] = bounds[0][1][1] = center * ax[0][1];
bounds[0][0][2] = bounds[0][1][2] = center * ax[0][2];
bounds[0][0] -= extents;
bounds[0][1] += extents;
a.AxisProjection( ax[0], b );
if ( !bounds[0].AddBounds( b ) ) {
// the other box is contained in this box
return false;
}
// test axis of other box
ax[1] = a.axis;
bounds[1][0][0] = bounds[1][1][0] = a.center * ax[1][0];
bounds[1][0][1] = bounds[1][1][1] = a.center * ax[1][1];
bounds[1][0][2] = bounds[1][1][2] = a.center * ax[1][2];
bounds[1][0] -= a.extents;
bounds[1][1] += a.extents;
AxisProjection( ax[1], b );
if ( !bounds[1].AddBounds( b ) ) {
// this box is contained in the other box
center = a.center;
extents = a.extents;
axis = a.axis;
return true;
}
// test axes aligned with the vector between the box centers and one of the box axis
dir = a.center - center;
dir.Normalize();
for ( i = 2; i < 4; i++ ) {
ax[i][0] = dir;
ax[i][1] = ax[i-2][ Min3Index( dir * ax[i-2][0], dir * ax[i-2][1], dir * ax[i-2][2] ) ];
ax[i][1] = ax[i][1] - ( ax[i][1] * dir ) * dir;
ax[i][1].Normalize();
ax[i][2].Cross( dir, ax[i][1] );
AxisProjection( ax[i], bounds[i] );
a.AxisProjection( ax[i], b );
bounds[i].AddBounds( b );
}
// get the bounds with the smallest volume
bestv = idMath::INFINITY;
besti = 0;
for ( i = 0; i < 4; i++ ) {
v = bounds[i].GetVolume();
if ( v < bestv ) {
bestv = v;
besti = i;
}
}
// create a box from the smallest bounds axis pair
center = ( bounds[besti][0] + bounds[besti][1] ) * 0.5f;
extents = bounds[besti][1] - center;
center *= ax[besti];
axis = ax[besti];
return false;
}
