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