From fe4c553acda476693e8121dce4071e54cfa938d2 Mon Sep 17 00:00:00 2001 From: JulioJerez Date: Sat, 12 Oct 2024 06:52:36 -0700 Subject: [PATCH] remove old path to assuming only principal axis inertia. 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. --- .../sdk/dCollision/ndBodyKinematic.cpp | 107 +----------------- newton-4.00/sdk/dCollision/ndBodyKinematic.h | 7 -- newton-4.00/sdk/dNewton/ndBodyDynamic.cpp | 22 ---- 3 files changed, 1 insertion(+), 135 deletions(-) diff --git a/newton-4.00/sdk/dCollision/ndBodyKinematic.cpp b/newton-4.00/sdk/dCollision/ndBodyKinematic.cpp index 81adffe84..9d7560b77 100644 --- a/newton-4.00/sdk/dCollision/ndBodyKinematic.cpp +++ b/newton-4.00/sdk/dCollision/ndBodyKinematic.cpp @@ -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) @@ -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) @@ -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); @@ -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); } @@ -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; @@ -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; @@ -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 + @@ -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 @@ -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 + @@ -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 @@ -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 @@ -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); @@ -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; } diff --git a/newton-4.00/sdk/dCollision/ndBodyKinematic.h b/newton-4.00/sdk/dCollision/ndBodyKinematic.h index 207db6bcb..dd22d028f 100644 --- a/newton-4.00/sdk/dCollision/ndBodyKinematic.h +++ b/newton-4.00/sdk/dCollision/ndBodyKinematic.h @@ -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)) @@ -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; @@ -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() diff --git a/newton-4.00/sdk/dNewton/ndBodyDynamic.cpp b/newton-4.00/sdk/dNewton/ndBodyDynamic.cpp index 35664613b..60435ab56 100644 --- a/newton-4.00/sdk/dNewton/ndBodyDynamic.cpp +++ b/newton-4.00/sdk/dNewton/ndBodyDynamic.cpp @@ -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); } @@ -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); @@ -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 @@ -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 {