From 42ba0c9a15e193ae9be26420b5e290b6de0bfde6 Mon Sep 17 00:00:00 2001 From: JulioJerez Date: Sat, 28 Sep 2024 05:37:53 -0700 Subject: [PATCH] adding clean memory to double hinge (not tested yet) --- .../dNewton/dJoints/ndJointDoubleHinge.cpp | 26 +++++++++++++++++++ .../sdk/dNewton/dJoints/ndJointDoubleHinge.h | 1 + 2 files changed, 27 insertions(+) diff --git a/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.cpp b/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.cpp index d8905bb634..65215eee0a 100644 --- a/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.cpp +++ b/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.cpp @@ -254,6 +254,32 @@ ndFloat32 ndJointDoubleHinge::PenetrationOmega(ndFloat32 penetration) const return omega; } +void ndJointDoubleHinge::ClearMemory() +{ + ndMatrix matrix0; + ndMatrix matrix1; + CalculateGlobalMatrix(matrix0, matrix1); + + ndJointBilateralConstraint::ClearMemory(); + + // save the current joint Omega + const ndVector omega0(m_body0->GetOmega()); + const ndVector omega1(m_body1->GetOmega()); + + const ndVector frontDir((matrix0.m_front - matrix1.m_up.Scale(matrix0.m_front.DotProduct(matrix1.m_up).GetScalar())).Normalize()); + + // calculate joint parameters, angles and omega + const ndFloat32 deltaAngle0 = CalculateAngle(matrix0.m_up, matrix1.m_up, frontDir); + m_axis0.m_angle = deltaAngle0; + m_axis0.m_offsetAngle = deltaAngle0; + m_axis0.m_omega = frontDir.DotProduct(omega0 - omega1).GetScalar(); + + const ndFloat32 deltaAngle1 = CalculateAngle(frontDir, matrix1.m_front, matrix1.m_up); + m_axis1.m_angle += deltaAngle1; + m_axis1.m_offsetAngle = deltaAngle1; + m_axis1.m_omega = matrix1.m_up.DotProduct(omega0 - omega1).GetScalar(); +} + void ndJointDoubleHinge::SubmitLimits(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1) { if ((m_axis0.m_minLimit > (ndFloat32(-1.0f) * ndDegreeToRad)) && (m_axis0.m_maxLimit < (ndFloat32(1.0f) * ndDegreeToRad))) diff --git a/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.h b/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.h index d905155bcc..546c93e7a4 100644 --- a/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.h +++ b/newton-4.00/sdk/dNewton/dJoints/ndJointDoubleHinge.h @@ -60,6 +60,7 @@ class ndJointDoubleHinge: public ndJointBilateralConstraint D_NEWTON_API void GetSpringDamper1(ndFloat32& regularizer, ndFloat32& spring, ndFloat32& damper) const; protected: + D_NEWTON_API void ClearMemory(); D_NEWTON_API void JacobianDerivative(ndConstraintDescritor& desc); D_NEWTON_API ndFloat32 PenetrationOmega(ndFloat32 penetartion) const; D_NEWTON_API void DebugJoint(ndConstraintDebugCallback& debugCallback) const;