/* * This takes what is supposed to be a rotation matrix, * and make sure it is correct. * Note: this operates on rows, not columns, because for rotations * both ways give equivalent results. */ void dOrthogonalizeR(dMatrix3 m) { dReal n0 = dCalcVectorLengthSquare3(m); if (n0 != 1) dSafeNormalize3(m); // project row[0] on row[1], should be zero dReal proj = dCalcVectorDot3(m, m+4); if (proj != 0) { // Gram-Schmidt step on row[1] m[4] -= proj * m[0]; m[5] -= proj * m[1]; m[6] -= proj * m[2]; } dReal n1 = dCalcVectorLengthSquare3(m+4); if (n1 != 1) dSafeNormalize3(m+4); /* just overwrite row[2], this makes sure the matrix is not a reflection */ dCalcVectorCross3(m+8, m, m+4); m[3] = m[4+3] = m[8+3] = 0; }
int _dSafeNormalize3 (dVector3 a) { dIASSERT(a); dReal l = dCalcVectorLengthSquare3(a); if (l > dEpsilon) { dScaleVector3r4(a, dRecipSqrt(l)); return 1; } a[0] = 1; // if all a's are zero, this is where we'll end up. a[1] = 0; // return a default unit length vector. a[2] = 0; return 0; }
void CCharEntity::OnStepBefore() { if (IsCollisionEnabled() && !IsRagdollActive()) { CEnvInfo EnvInfo; vector3 Pos = GetTransform().pos_component(); Pos.y += Height; float DistanceToGround; if (EnvQueryMgr->GetEnvInfoAt(Pos, EnvInfo, Height + 0.1f, GetUniqueID())) { DistanceToGround = Pos.y - Height - EnvInfo.WorldHeight; GroundMtl = EnvInfo.Material; } else { DistanceToGround = FLT_MAX; GroundMtl = InvalidMaterial; } CRigidBody* pMasterBody = Composite->GetMasterBody(); bool BodyIsEnabled = IsEnabled(); vector3 AngularVel = pMasterBody->GetAngularVelocity(); vector3 LinearVel = pMasterBody->GetLinearVelocity(); vector3 DesiredLVelChange = DesiredLinearVel - LinearVel; if (DistanceToGround <= 0.f) { // No torques now, angular velocity changes by impulse immediately to desired value bool Actuated = DesiredAngularVel != AngularVel.y; if (Actuated) { if (!BodyIsEnabled) SetEnabled(true); pMasterBody->SetAngularVelocity(vector3(0.f, DesiredAngularVel, 0.f)); } if (!DesiredLVelChange.isequal(vector3::Zero, 0.0001f)) { if (!Actuated) { Actuated = true; SetEnabled(true); } // Vertical movement for non-flying actors is impulse (jump). // Since speed if already clamped to actor caps, we save vertical desired velocity as is. // Spring force pushes us from below the ground. //!!!!!!!!!!!!!!! //!!!calc correct impulse for the spring! //!!!!!!!!!!!!!!! float VerticalDesVelChange = DesiredLVelChange.y - (50.0f * DistanceToGround); float Mass = pMasterBody->GetMass(); //!!!remove calcs for Y, it is zero (optimization)! dVector3 ODEForce; dWorldImpulseToForce(Level->GetODEWorldID(), dReal(Level->GetStepSize()), DesiredLVelChange.x * Mass, 0.f, DesiredLVelChange.z * Mass, ODEForce); float SqForceMagnitude = (float)dCalcVectorLengthSquare3(ODEForce); if (SqForceMagnitude > 0.f) { float MaxForceMagnitude = Mass * MaxHorizAccel; float SqMaxForceMagnitude = MaxForceMagnitude * MaxForceMagnitude; if (SqForceMagnitude > SqMaxForceMagnitude) dScaleVector3(ODEForce, MaxForceMagnitude / n_sqrt(SqForceMagnitude)); dBodyAddForce(pMasterBody->GetODEBodyID(), ODEForce[0], ODEForce[1], ODEForce[2]); } if (VerticalDesVelChange != 0.f) { dWorldImpulseToForce(Level->GetODEWorldID(), dReal(Level->GetStepSize()), 0.f, VerticalDesVelChange * Mass, 0.f, ODEForce); dBodyAddForce(pMasterBody->GetODEBodyID(), ODEForce[0], ODEForce[1], ODEForce[2]); } } if (BodyIsEnabled && !Actuated && DistanceToGround > -0.002f) { const float FreezeThreshold = 0.00001f; //???use TINY? bool AVelIsAlmostZero = n_fabs(AngularVel.y) < FreezeThreshold; bool LVelIsAlmostZero = n_fabs(LinearVel.x) * (float)Level->GetStepSize() < FreezeThreshold && n_fabs(LinearVel.z) * (float)Level->GetStepSize() < FreezeThreshold; if (AVelIsAlmostZero) pMasterBody->SetAngularVelocity(vector3::Zero); if (LVelIsAlmostZero) pMasterBody->SetLinearVelocity(vector3::Zero); if (AVelIsAlmostZero && LVelIsAlmostZero) SetEnabled(false); } } //???!!!else (when falling) apply damping?! } // NOTE: do NOT call the parent class, we don't need any damping }
// Ripped from Opcode 1.1. static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, dReal& u, dReal& v){ // now onto the bulk of the collision... dVector3 Diff; dSubtractVectors3r4(Diff, Origin, Center); dReal A00 = dCalcVectorLengthSquare3(Edge0); dReal A01 = dCalcVectorDot3(Edge0, Edge1); dReal A11 = dCalcVectorLengthSquare3(Edge1); dReal B0 = dCalcVectorDot3(Diff, Edge0); dReal B1 = dCalcVectorDot3(Diff, Edge1); dReal C = dCalcVectorLengthSquare3(Diff); dReal Det = dFabs(A00 * A11 - A01 * A01); u = A01 * B1 - A11 * B0; v = A01 * B0 - A00 * B1; dReal DistSq; if (u + v <= Det) { if(u < REAL(0.0)) { if(v < REAL(0.0)) { // region 4 if(B0 < REAL(0.0)) { v = REAL(0.0); if (-B0 >= A00) { u = REAL(1.0); DistSq = A00 + REAL(2.0) * B0 + C; } else { u = -B0 / A00; DistSq = B0 * u + C; } } else { u = REAL(0.0); if(B1 >= REAL(0.0)) { v = REAL(0.0); DistSq = C; } else if(-B1 >= A11) { v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else { v = -B1 / A11; DistSq = B1 * v + C; } } } else { // region 3 u = REAL(0.0); if(B1 >= REAL(0.0)) { v = REAL(0.0); DistSq = C; } else if(-B1 >= A11) { v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else { v = -B1 / A11; DistSq = B1 * v + C; } } } else if(v < REAL(0.0)) { // region 5 v = REAL(0.0); if (B0 >= REAL(0.0)) { u = REAL(0.0); DistSq = C; } else if (-B0 >= A00) { u = REAL(1.0); DistSq = A00 + REAL(2.0) * B0 + C; } else { u = -B0 / A00; DistSq = B0 * u + C; } } else { // region 0 // minimum at interior point if (Det == REAL(0.0)) { u = REAL(0.0); v = REAL(0.0); DistSq = FLT_MAX; } else { dReal InvDet = REAL(1.0) / Det; u *= InvDet; v *= InvDet; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } } else { dReal Tmp0, Tmp1, Numer, Denom; if(u < REAL(0.0)) { // region 2 Tmp0 = A01 + B0; Tmp1 = A11 + B1; if (Tmp1 > Tmp0) { Numer = Tmp1 - Tmp0; Denom = A00 - REAL(2.0) * A01 + A11; if (Numer >= Denom) { u = REAL(1.0); v = REAL(0.0); DistSq = A00 + REAL(2.0) * B0 + C; } else { u = Numer / Denom; v = REAL(1.0) - u; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } else { u = REAL(0.0); if(Tmp1 <= REAL(0.0)) { v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else if(B1 >= REAL(0.0)) { v = REAL(0.0); DistSq = C; } else { v = -B1 / A11; DistSq = B1 * v + C; } } } else if(v < REAL(0.0)) { // region 6 Tmp0 = A01 + B1; Tmp1 = A00 + B0; if (Tmp1 > Tmp0) { Numer = Tmp1 - Tmp0; Denom = A00 - REAL(2.0) * A01 + A11; if (Numer >= Denom) { v = REAL(1.0); u = REAL(0.0); DistSq = A11 + REAL(2.0) * B1 + C; } else { v = Numer / Denom; u = REAL(1.0) - v; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } else { v = REAL(0.0); if (Tmp1 <= REAL(0.0)){ u = REAL(1.0); DistSq = A00 + REAL(2.0) * B0 + C; } else if(B0 >= REAL(0.0)) { u = REAL(0.0); DistSq = C; } else { u = -B0 / A00; DistSq = B0 * u + C; } } } else { // region 1 Numer = A11 + B1 - A01 - B0; if (Numer <= REAL(0.0)) { u = REAL(0.0); v = REAL(1.0); DistSq = A11 + REAL(2.0) * B1 + C; } else { Denom = A00 - REAL(2.0) * A01 + A11; if (Numer >= Denom) { u = REAL(1.0); v = REAL(0.0); DistSq = A00 + REAL(2.0) * B0 + C; } else { u = Numer / Denom; v = REAL(1.0) - u; DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C; } } } } Dist = dSqrt(dFabs(DistSq)); if (Dist <= Radius) { Dist = Radius - Dist; return true; } else return false; }
static inline dReal _length2OfVector3(dVector3 v) { return dCalcVectorLengthSquare3(v); }