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; }