Skip to content

Commit

Permalink
remove old path to assuming only principal axis inertia.
Browse files Browse the repository at this point in the history 
only full inertia matrix is support and the use must set to have symmetry if he or she want the special case of axis alignment .
In general this is teh case since must all pr8imital has axis of symmetry, bu the use can set explicitly if he desire.
  • Loading branch information
@JulioJerez
JulioJerez committed Oct 12, 2024
1 parent fcac326 commit fe4c553
Show file tree
Hide file tree
Showing 3 changed files with 1 addition and 135 deletions.
107 changes: 1 addition & 106 deletions newton-4.00/sdk/dCollision/ndBodyKinematic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,9 +92,7 @@ void ndBodyKinematic::ndContactMap::DetachContact(ndContact* const contact)

ndBodyKinematic::ndBodyKinematic()
:ndBody()
#ifdef D_USE_FULL_INERTIA
,m_inertiaPrincipalAxis(ndGetIdentityMatrix())
#endif
,m_invWorldInertiaMatrix(ndGetZeroMatrix())
,m_shapeInstance(new ndShapeNull)
,m_mass(ndVector::m_zero)
Expand Down Expand Up @@ -129,9 +127,7 @@ ndBodyKinematic::ndBodyKinematic()

ndBodyKinematic::ndBodyKinematic(const ndBodyKinematic& src)
:ndBody(src)
#ifdef D_USE_FULL_INERTIA
,m_inertiaPrincipalAxis(src.m_inertiaPrincipalAxis)
#endif
,m_invWorldInertiaMatrix(src.m_invWorldInertiaMatrix)
,m_shapeInstance(src.m_shapeInstance)
,m_mass(ndVector::m_zero)
Expand Down Expand Up @@ -276,7 +272,6 @@ void ndBodyKinematic::SetMassMatrix(ndFloat32 mass, const ndShapeInstance& shape
// although the engine fully supports asymmetric inertia, I will ignore cross inertia for now
SetCentreOfMass(origin);

#ifdef D_USE_FULL_INERTIA
if (!fullInertia)
{
ndMatrix matrix (inertia);
Expand All @@ -286,16 +281,6 @@ void ndBodyKinematic::SetMassMatrix(ndFloat32 mass, const ndShapeInstance& shape
inertia[1][1] = eigenValues[1];
inertia[2][2] = eigenValues[2];
}
#else
fullInertia = false;
ndMatrix matrix(inertia);
ndVector eigenValues(matrix.EigenVectors());
inertia = ndGetIdentityMatrix();
inertia[0][0] = eigenValues[0];
inertia[1][1] = eigenValues[1];
inertia[2][2] = eigenValues[2];

#endif
SetMassMatrix(mass, inertia);
}

Expand All @@ -304,9 +289,7 @@ void ndBodyKinematic::SetMassMatrix(ndFloat32 mass, const ndMatrix& inertia)
mass = ndAbs(mass);
ndShape* const shape = m_shapeInstance.GetShape();

#ifdef D_USE_FULL_INERTIA
m_inertiaPrincipalAxis = ndGetIdentityMatrix();
//if ((mass < D_MINIMUM_MASS) || shape->GetAsShapeNull() || shape->GetAsShapeStaticMesh())
if ((mass < D_MINIMUM_MASS) || shape->GetAsShapeStaticMesh())
{
mass = D_INFINITE_MASS * 2.0f;
Expand Down Expand Up @@ -352,47 +335,6 @@ void ndBodyKinematic::SetMassMatrix(ndFloat32 mass, const ndMatrix& inertia)
m_invMass.m_w = ndFloat32(1.0f) / mass;
}

#else

if ((mass < D_MINIMUM_MASS) || shape->GetAsShapeNull() || shape->GetAsShapeStaticMesh())
{
mass = D_INFINITE_MASS * 2.0f;
}

if (mass >= D_INFINITE_MASS)
{
m_mass.m_x = D_INFINITE_MASS;
m_mass.m_y = D_INFINITE_MASS;
m_mass.m_z = D_INFINITE_MASS;
m_mass.m_w = D_INFINITE_MASS;
m_invMass = ndVector::m_zero;
}
else
{
ndFloat32 Ixx = ndAbs(inertia[0][0]);
ndFloat32 Iyy = ndAbs(inertia[1][1]);
ndFloat32 Izz = ndAbs(inertia[2][2]);

ndFloat32 Ixx1 = ndClamp(Ixx, ndFloat32(0.0001f) * mass, ndFloat32(10000.0f) * mass);
ndFloat32 Iyy1 = ndClamp(Iyy, ndFloat32(0.0001f) * mass, ndFloat32(10000.0f) * mass);
ndFloat32 Izz1 = ndClamp(Izz, ndFloat32(0.0001f) * mass, ndFloat32(10000.0f) * mass);

ndAssert(Ixx1 > ndFloat32(0.0f));
ndAssert(Iyy1 > ndFloat32(0.0f));
ndAssert(Izz1 > ndFloat32(0.0f));

m_mass.m_x = Ixx1;
m_mass.m_y = Iyy1;
m_mass.m_z = Izz1;
m_mass.m_w = mass;

m_invMass.m_x = ndFloat32(1.0f) / Ixx1;
m_invMass.m_y = ndFloat32(1.0f) / Iyy1;
m_invMass.m_z = ndFloat32(1.0f) / Izz1;
m_invMass.m_w = ndFloat32(1.0f) / mass;
}
#endif

//#ifdef _DEBUG
#if 0
dgBodyMasterList& me = *m_world;
Expand Down Expand Up @@ -494,7 +436,6 @@ ndMatrix ndBodyKinematic::CalculateInvInertiaMatrix() const
const ndVector invIyy(m_invMass[1]);
const ndVector invIzz(m_invMass[2]);

#ifdef D_USE_FULL_INERTIA
const ndMatrix matrix(m_inertiaPrincipalAxis * m_matrix);
return ndMatrix(
matrix.m_front.Scale(matrix.m_front[0]) * invIxx +
Expand All @@ -509,22 +450,6 @@ ndMatrix ndBodyKinematic::CalculateInvInertiaMatrix() const
matrix.m_up.Scale(matrix.m_up[2]) * invIyy +
matrix.m_right.Scale(matrix.m_right[2]) * invIzz,
ndVector::m_wOne);

#else
return ndMatrix(
m_matrix.m_front.Scale(m_matrix.m_front[0]) * invIxx +
m_matrix.m_up.Scale(m_matrix.m_up[0]) * invIyy +
m_matrix.m_right.Scale(m_matrix.m_right[0]) * invIzz,

m_matrix.m_front.Scale(m_matrix.m_front[1]) * invIxx +
m_matrix.m_up.Scale(m_matrix.m_up[1]) * invIyy +
m_matrix.m_right.Scale(m_matrix.m_right[1]) * invIzz,

m_matrix.m_front.Scale(m_matrix.m_front[2]) * invIxx +
m_matrix.m_up.Scale(m_matrix.m_up[2]) * invIyy +
m_matrix.m_right.Scale(m_matrix.m_right[2]) * invIzz,
ndVector::m_wOne);
#endif
}

ndMatrix ndBodyKinematic::CalculateInertiaMatrix() const
Expand All @@ -533,7 +458,6 @@ ndMatrix ndBodyKinematic::CalculateInertiaMatrix() const
const ndVector Iyy(m_mass.m_y);
const ndVector Izz(m_mass.m_z);

#ifdef D_USE_FULL_INERTIA
const ndMatrix matrix(m_inertiaPrincipalAxis * m_matrix);
return ndMatrix(
matrix.m_front.Scale(matrix.m_front[0]) * Ixx +
Expand All @@ -548,22 +472,6 @@ ndMatrix ndBodyKinematic::CalculateInertiaMatrix() const
matrix.m_up.Scale(matrix.m_up[2]) * Iyy +
matrix.m_right.Scale(matrix.m_right[2]) * Izz,
ndVector::m_wOne);

#else
return ndMatrix(
m_matrix.m_front.Scale(m_matrix.m_front[0]) * Ixx +
m_matrix.m_up.Scale(m_matrix.m_up[0]) * Iyy +
m_matrix.m_right.Scale(m_matrix.m_right[0]) * Izz,

m_matrix.m_front.Scale(m_matrix.m_front[1]) * Ixx +
m_matrix.m_up.Scale(m_matrix.m_up[1]) * Iyy +
m_matrix.m_right.Scale(m_matrix.m_right[1]) * Izz,

m_matrix.m_front.Scale(m_matrix.m_front[2]) * Ixx +
m_matrix.m_up.Scale(m_matrix.m_up[2]) * Iyy +
m_matrix.m_right.Scale(m_matrix.m_right[2]) * Izz,
ndVector::m_wOne);
#endif
}

ndVector ndBodyKinematic::CalculateLinearMomentum() const
Expand All @@ -573,15 +481,9 @@ ndVector ndBodyKinematic::CalculateLinearMomentum() const

ndVector ndBodyKinematic::CalculateAngularMomentum() const
{
#ifdef D_USE_FULL_INERTIA
const ndVector localOmega(m_inertiaPrincipalAxis.UnrotateVector (m_matrix.UnrotateVector(m_omega)));
const ndVector localAngularMomentum(m_mass * localOmega);
return m_matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(localAngularMomentum));
#else
const ndVector localOmega(m_matrix.UnrotateVector(m_omega));
const ndVector localAngularMomentum(m_mass * localOmega);
return m_matrix.RotateVector(localAngularMomentum);
#endif
}

ndJacobian ndBodyKinematic::CalculateNetForce() const
Expand Down Expand Up @@ -717,11 +619,7 @@ void ndBodyKinematic::IntegrateExternalForce(ndFloat32 timestep)
// Iy * ay + (Ix - Iz) * dwz * ax + (Ix - Iz) * dwx * az = Ty - (Ix - Iz) * wz * wx
// Iz * az + (Iy - Ix) * dwx * ay + (Iy - Ix) * dwy * ax = Tz - (Iy - Ix) * wx * wy

#ifdef D_USE_FULL_INERTIA
const ndMatrix matrix(m_inertiaPrincipalAxis * m_matrix);
#else
const ndMatrix& matrix = m_matrix;
#endif

ndVector localOmega(matrix.UnrotateVector(m_omega));
const ndVector localAngularMomentum(m_mass * localOmega);
Expand Down Expand Up @@ -822,9 +720,6 @@ void ndBodyKinematic::InitSurrogateBody(ndBodyKinematic* const surrogate) const
surrogate->m_gyroTorque = m_gyroTorque;
surrogate->m_gyroRotation = m_gyroRotation;
surrogate->m_invWorldInertiaMatrix = m_invWorldInertiaMatrix;

#ifdef D_USE_FULL_INERTIA
surrogate->m_inertiaPrincipalAxis = m_inertiaPrincipalAxis;
#endif
surrogate->m_inertiaPrincipalAxis = m_inertiaPrincipalAxis;
}

7 changes: 0 additions & 7 deletions newton-4.00/sdk/dCollision/ndBodyKinematic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@ class ndModel;
class ndSkeletonContainer;
class ndJointBilateralConstraint;

#define D_USE_FULL_INERTIA
#define D_FREEZZING_VELOCITY_DRAG ndFloat32 (0.9f)
#define D_SOLVER_MAX_ACCEL_ERROR (D_FREEZE_MAG * ndFloat32 (0.5f))

Expand Down Expand Up @@ -195,9 +194,7 @@ class ndBodyKinematic : public ndBody

virtual void SetAcceleration(const ndVector& accel, const ndVector& alpha);

#ifdef D_USE_FULL_INERTIA
ndMatrix m_inertiaPrincipalAxis;
#endif
ndMatrix m_invWorldInertiaMatrix;
ndShapeInstance m_shapeInstance;
ndVector m_mass;
Expand Down Expand Up @@ -311,11 +308,7 @@ inline void ndBodyKinematic::SetMassMatrix(ndFloat32 Ixx, ndFloat32 Iyy, ndFloat

inline ndMatrix ndBodyKinematic::GetPrincipalAxis() const
{
#ifdef D_USE_FULL_INERTIA
return m_inertiaPrincipalAxis;
#else
return ndGetIdentityMatrix();
#endif
}

inline ndBodyKinematic* ndBodyKinematic::GetAsBodyKinematic()
Expand Down
22 changes: 0 additions & 22 deletions newton-4.00/sdk/dNewton/ndBodyDynamic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -254,13 +254,8 @@ void ndBodyDynamic::AddDampingAcceleration(ndFloat32 timestep)
}

const ndVector omegaDamp(m_cachedDampCoef & ndVector::m_triplexMask);
#ifdef D_USE_FULL_INERTIA
const ndVector omega(omegaDamp * m_inertiaPrincipalAxis.UnrotateVector(m_matrix.UnrotateVector(m_omega)));
m_omega = m_matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(omega));
#else
const ndVector omega(m_matrix.UnrotateVector(m_omega) * omegaDamp);
m_omega = m_matrix.RotateVector(omega);
#endif
m_veloc = m_veloc.Scale(m_cachedDampCoef.m_w);
}

Expand All @@ -286,13 +281,8 @@ ndJacobian ndBodyDynamic::IntegrateForceAndToque(const ndVector& force, const nd
ndJacobian velocStep;

const ndMatrix matrix(ndCalculateMatrix(m_gyroRotation, ndVector::m_wOne));
#ifdef D_USE_FULL_INERTIA
const ndVector localOmega(m_inertiaPrincipalAxis.UnrotateVector(matrix.UnrotateVector(m_omega)));
const ndVector localTorque(m_inertiaPrincipalAxis.UnrotateVector(matrix.UnrotateVector(torque)));
#else
const ndVector localOmega(matrix.UnrotateVector(m_omega));
const ndVector localTorque(matrix.UnrotateVector(torque));
#endif

// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
const ndVector dw(localOmega * timestep);
Expand All @@ -306,11 +296,7 @@ ndJacobian ndBodyDynamic::IntegrateForceAndToque(const ndVector& force, const nd
// calculate gradient at a full time step
const ndVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));

#ifdef D_USE_FULL_INERTIA
velocStep.m_angular = matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(gradientStep));
#else
velocStep.m_angular = matrix.RotateVector(gradientStep);
#endif
velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;

#ifdef _DEBUG
Expand Down Expand Up @@ -347,18 +333,10 @@ void ndBodyDynamic::IntegrateGyroSubstep(const ndVector& timestep)
// calculate new Gyro torque and Gyro acceleration
const ndMatrix matrix(ndCalculateMatrix(m_gyroRotation, ndVector::m_wOne));

#ifdef D_USE_FULL_INERTIA
const ndVector localOmega(m_inertiaPrincipalAxis.UnrotateVector(matrix.UnrotateVector(m_omega)));
const ndVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(localGyroTorque));
m_gyroAlpha = matrix.RotateVector(m_inertiaPrincipalAxis.RotateVector(localGyroTorque * m_invMass));
//ndAssert(m_gyroTorque.DotProduct(m_gyroTorque).GetScalar() < ndFloat32 (10000.0f));
#else
const ndVector localOmega(matrix.UnrotateVector(m_omega));
const ndVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
#endif
}
else
{
Expand Down

0 comments on commit fe4c553

Please sign in to comment.