Exemplo n.º 1
0
/*
* 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;
}
Exemplo n.º 2
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;
}
Exemplo n.º 3
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);
}