better model reset when clearing memory.

MADEAPPS · Sep 6, 2024 · 38dfda2 · 38dfda2
1 parent e18e1b8
commit 38dfda2
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 16 deletions.
diff --git a/newton-4.00/sdk/dNewton/dJoints/ndJointHinge.cpp b/newton-4.00/sdk/dNewton/dJoints/ndJointHinge.cpp
@@ -195,14 +195,6 @@ void ndJointHinge::SubmitSpringDamper(ndConstraintDescritor& desc, const ndMatri
 	SetMassSpringDamperAcceleration(desc, m_springDamperRegularizer, m_springK, m_damperC);
 }
 
-void ndJointHinge::ClearMemory()
-{
-	ndJointBilateralConstraint::ClearMemory();
-	m_angle = ndFloat32(0.0f);
-	m_omega = ndFloat32(0.0f);
-	m_targetAngle = ndFloat32(0.0f);
-}
-
 void ndJointHinge::ApplyBaseRows(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)
 {
 	AddLinearRowJacobian(desc, matrix0.m_posit, matrix1.m_posit, matrix1[0]);
@@ -233,6 +225,25 @@ ndFloat32 ndJointHinge::PenetrationOmega(ndFloat32 penetration) const
 	return omega;
 }
 
+//#pragma optimize( "", off )
+void ndJointHinge::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());
+
+	// the joint angle can be determined by getting the angle between any two non parallel vectors
+	m_angle = CalculateAngle(matrix0.m_up, matrix1.m_up, matrix1.m_front);
+	m_omega = matrix1.m_front.DotProduct(omega0 - omega1).GetScalar();
+	m_targetAngle = m_angle;
+}
+
 ndInt32 ndJointHinge::GetKinematicState(ndKinematicState* const state) const
 {
 	state->m_posit = m_angle;

diff --git a/newton-4.00/sdk/dNewton/dJoints/ndJointSlider.cpp b/newton-4.00/sdk/dNewton/dJoints/ndJointSlider.cpp
@@ -159,6 +159,28 @@ ndInt32 ndJointSlider::GetKinematicState(ndKinematicState* const state) const
 	return 1;
 }
 
+//#pragma optimize( "", off )
+void ndJointSlider::ClearMemory()
+{
+	ndMatrix matrix0;
+	ndMatrix matrix1;
+	CalculateGlobalMatrix(matrix0, matrix1);
+
+	ndJointBilateralConstraint::ClearMemory();
+
+	const ndVector& p0 = matrix0.m_posit;
+	const ndVector& p1 = matrix1.m_posit;
+	const ndVector veloc0(m_body0->GetVelocityAtPoint(matrix0.m_posit));
+	const ndVector veloc1(m_body1->GetVelocityAtPoint(matrix1.m_posit));
+
+	const ndVector prel(p0 - p1);
+	const ndVector vrel(veloc0 - veloc1);
+
+	m_speed = vrel.DotProduct(matrix1.m_front).GetScalar();
+	m_posit = prel.DotProduct(matrix1.m_front).GetScalar();
+	m_positOffset = m_posit;
+}
+
 void ndJointSlider::SubmitLimits(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)
 {
 	if (m_limitState)
@@ -193,14 +215,6 @@ void ndJointSlider::SubmitLimits(ndConstraintDescritor& desc, const ndMatrix& ma
 	}
 }
 
-void ndJointSlider::ClearMemory()
-{
-	ndJointBilateralConstraint::ClearMemory();
-	m_posit = ndFloat32(0.0f);
-	m_speed = ndFloat32(0.0f);
-	m_positOffset = ndFloat32(0.0f);
-}
-
 void ndJointSlider::SubmitSpringDamper(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)
 {
 	// add spring damper row