added joint funtionality (wip)

MADEAPPS · Sep 2, 2024 · a68311e · a68311e
1 parent 3cb015e
commit a68311e
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 15 deletions.
diff --git a/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.cpp b/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.cpp
@@ -165,6 +165,22 @@ void ndJointBilateralConstraint::CalculateLocalMatrix(const ndMatrix& globalMatr
 	localMatrix1 = globalMatrix * m_body1->GetMatrix().OrthoInverse();
 }
 
+void ndJointBilateralConstraint::CalculateGlobalMatrix(ndMatrix& matrix0, ndMatrix& matrix1) const
+{
+	matrix0 = m_localMatrix0 * m_body0->GetMatrix();
+	matrix1 = m_localMatrix1 * m_body1->GetMatrix();
+}
+
+ndMatrix ndJointBilateralConstraint::CalculateGlobalMatrix0() const
+{
+	return m_localMatrix0* m_body0->GetMatrix();
+}
+
+ndMatrix ndJointBilateralConstraint::CalculateGlobalMatrix1() const
+{
+	return m_localMatrix1 * m_body1->GetMatrix();
+}
+
 ndFloat32 ndJointBilateralConstraint::CalculateSpringDamperAcceleration(ndFloat32 dt, ndFloat32 ks, ndFloat32 x, ndFloat32 kd, ndFloat32 v) const
 {
 	//at = - (ks * x + kd * v);

diff --git a/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.h b/newton-4.00/sdk/dCollision/ndJointBilateralConstraint.h
@@ -92,7 +92,6 @@ class ndJointBilateralConstraint : public ndConstraint
 
 	D_COLLISION_API ndFloat32 CalculateAngle(const ndVector& planeDir, const ndVector& cosDir, const ndVector& sinDir) const;
 	D_COLLISION_API virtual void JointAccelerations(ndJointAccelerationDecriptor* const desc);
-	D_COLLISION_API void CalculateLocalMatrix(const ndMatrix& pinsAndPivotFrame, ndMatrix& localMatrix0, ndMatrix& localMatrix1) const;
 	D_COLLISION_API void AddAngularRowJacobian(ndConstraintDescritor& desc, const ndVector& dir, ndFloat32 relAngle);
 	D_COLLISION_API void AddLinearRowJacobian(ndConstraintDescritor& desc, const ndVector& pivot0, const ndVector& pivot1, const ndVector& dir);
 
@@ -102,10 +101,12 @@ class ndJointBilateralConstraint : public ndConstraint
 
 	D_COLLISION_API virtual ndInt32 GetKinematicState(ndKinematicState* const state) const;
 
-	virtual bool IsHolonomic(ndFloat32 timestep) const;
-
 	const ndMatrix& GetLocalMatrix0() const;
 	const ndMatrix& GetLocalMatrix1() const;
+	D_COLLISION_API virtual ndMatrix CalculateGlobalMatrix0() const;
+	D_COLLISION_API virtual ndMatrix CalculateGlobalMatrix1() const;
+	D_COLLISION_API virtual void CalculateGlobalMatrix(ndMatrix& matrix0, ndMatrix& matrix1) const;
+	D_COLLISION_API void CalculateLocalMatrix(const ndMatrix& pinsAndPivotFrame, ndMatrix& localMatrix0, ndMatrix& localMatrix1) const;
 
 	void SetLocalMatrix0(const ndMatrix& matrix);
 	void SetLocalMatrix1(const ndMatrix& matrix);
@@ -115,10 +116,9 @@ class ndJointBilateralConstraint : public ndConstraint
 	bool IsBilateral() const;
 	bool IsCollidable() const;
 	bool GetSkeletonFlag() const;
-
 	void SetCollidable(bool state);
 	void SetSkeletonFlag(bool flag);
-	void CalculateGlobalMatrix(ndMatrix& matrix0, ndMatrix& matrix1) const;
+	virtual bool IsHolonomic(ndFloat32 timestep) const;
 
 	void SetHighFriction(ndConstraintDescritor& desc, ndFloat32 friction);
 	void SetLowerFriction(ndConstraintDescritor& desc, ndFloat32 friction);
@@ -185,12 +185,6 @@ inline void ndJointBilateralConstraint::SetSolverModel(ndJointBilateralSolverMod
 //	return m_maxDof;
 //}
 
-inline void ndJointBilateralConstraint::CalculateGlobalMatrix(ndMatrix& matrix0, ndMatrix& matrix1) const
-{
-	matrix0 = m_localMatrix0 * m_body0->GetMatrix();
-	matrix1 = m_localMatrix1 * m_body1->GetMatrix();
-}
-
 inline const ndMatrix& ndJointBilateralConstraint::GetLocalMatrix0() const
 {
 	return m_localMatrix0;

diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.cpp b/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.cpp
@@ -110,11 +110,16 @@ ndMatrix ndIk6DofEffector::GetEffectorMatrix() const
 {
 	ndMatrix matrix0;
 	ndMatrix matrix1;
-	CalculateGlobalMatrix(matrix0, matrix1);
+	ndJointBilateralConstraint::CalculateGlobalMatrix(matrix0, matrix1);
 	const ndMatrix matrix(matrix0 * matrix1.OrthoInverse());
 	return matrix;
 }
 
+ndMatrix ndIk6DofEffector::CalculateGlobalMatrix1() const
+{
+	return m_targetFrame * ndJointBilateralConstraint::CalculateGlobalMatrix1();
+}
+
 ndFloat32 ndIk6DofEffector::GetMaxForce() const
 {
 	return m_linearMaxForce;
@@ -165,9 +170,12 @@ void ndIk6DofEffector::GetAngularSpringDamper(ndFloat32& regularizer, ndFloat32&
 
 void ndIk6DofEffector::DebugJoint(ndConstraintDebugCallback& debugCallback) const
 {
-	const ndMatrix matrix0(m_localMatrix0 * m_body0->GetMatrix());
-	const ndMatrix matrix1(m_localMatrix1 * m_body1->GetMatrix());
-	const ndMatrix targetFrame(m_targetFrame * matrix1);
+	//const ndMatrix matrix0(m_localMatrix0 * m_body0->GetMatrix());
+	//const ndMatrix matrix1(m_localMatrix1 * m_body1->GetMatrix());
+	//const ndMatrix targetFrame(m_targetFrame * matrix1);
+	const ndMatrix matrix0(CalculateGlobalMatrix0());
+	const ndMatrix matrix1(ndJointBilateralConstraint::CalculateGlobalMatrix1());
+	const ndMatrix targetFrame(CalculateGlobalMatrix1());
 
 	debugCallback.DrawFrame(matrix0);
 	debugCallback.DrawFrame(matrix1);

diff --git a/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.h b/newton-4.00/sdk/dNewton/dIkSolver/ndIk6DofEffector.h
@@ -58,6 +58,7 @@ class ndIk6DofEffector: public ndJointBilateralConstraint
 	D_NEWTON_API void SetMaxTorque(ndFloat32 torque);
 	D_NEWTON_API void DebugJoint(ndConstraintDebugCallback& debugCallback) const;
 
+	D_NEWTON_API ndMatrix CalculateGlobalMatrix1() const;
 	D_NEWTON_API virtual bool IsHolonomic(ndFloat32 timestep) const;
 
 	protected: