Commits

Anonymous committed e1bdd84

Add joint body position correction algorithm, make simulation more stable

Comments (0)

Files changed (3)

include/pgJointObject.h

 	pgBodyObject*	body1;
 	pgBodyObject*	body2;
 	int		isCollideConnect;
-	void	(*SolveConstraint)(pgJointObject* joint,double stepTime);
+	void	(*SolveConstraintPosition)(pgJointObject* joint,double stepTime);
+	void	(*SolveConstraintVelocity)(pgJointObject* joint,double stepTime);
 	void	(*Destroy)(pgJointObject* joint);
 } pgJointObject;
 

src/pgJointObject.c

 	joint->body1 = b1;
 	joint->body2 = b2;
 	joint->isCollideConnect = bCollideConnect;
-	joint->SolveConstraint = NULL;
+	joint->SolveConstraintVelocity = NULL;
+	joint->SolveConstraintPosition = NULL;
 }
 
-void PG_SolveDistanceJoint(pgJointObject* joint,double stepTime)
+void PG_SolveDistanceJointPosition(pgJointObject* joint,double stepTime)
+{
+	pgVector2 vecL,vecP;
+	pgDistanceJoint* pJoint = (pgDistanceJoint*)joint;
+	
+	if (joint->body1 && (!joint->body2))
+	{
+		vecL = c_diff(joint->body1->vecPosition,pJoint->anchor2);
+		c_normalize(&vecL);
+		vecL = c_mul_complex_with_real(vecL,pJoint->distance);
+		joint->body1->vecPosition = c_sum(pJoint->anchor2,vecL);
+		return;
+	} 
+
+	if(joint->body1 && joint->body2)
+	{
+		vecL = c_diff(joint->body1->vecPosition,joint->body2->vecPosition);
+		vecP = c_sum(joint->body1->vecPosition,joint->body2->vecPosition);
+		c_normalize(&vecL);
+		vecL = c_mul_complex_with_real(vecL,pJoint->distance * 0.5);
+		joint->body1->vecPosition = c_sum(joint->body1->vecPosition,vecL);
+		joint->body2->vecPosition = c_diff(joint->body2->vecPosition,vecL);
+		return;
+	}
+}
+
+void PG_SolveDistanceJointVelocity(pgJointObject* joint,double stepTime)
 {
 	pgVector2 vecL;
 	double lamda,cosTheta1V,cosTheta2V,mk;
-	Py_complex impuseAdd,v1Add,v2Add;
+	pgVector2 impuseAdd,v1Add,v2Add;
 	pgDistanceJoint* pJoint = (pgDistanceJoint*)joint;
 	if (joint->body1 && (!joint->body2))
 	{
 	pjoint->distance = dist;
 	pjoint->anchor1 = a1;
 	pjoint->anchor2 = a2;
-	pjoint->joint.SolveConstraint = PG_SolveDistanceJoint;
+	pjoint->joint.SolveConstraintVelocity = PG_SolveDistanceJointVelocity;
 	return (pgJointObject*)pjoint;
 }

src/pgWorldObject.c

 	for (i = 0;i < size;i++)
 	{
 		pgJointObject* joint = (pgJointObject*)(PyList_GetItem((PyObject*)(world->jointList),i));
-		if (joint->SolveConstraint)
+		if (joint->SolveConstraintPosition)
 		{
-			joint->SolveConstraint(joint,stepTime);
+			joint->SolveConstraintPosition(joint,stepTime);
+		}
+		if (joint->SolveConstraintVelocity)
+		{
+			joint->SolveConstraintVelocity(joint,stepTime);
 		}
 	}
 }