/*
* 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);
}