Commits

Anonymous committed 6d187e2

free anchors of joints algorithm are added, it's not finished, split impulse method will be added for position correction

  • Participants
  • Parent commits b7c7926
  • Branches physics

Comments (0)

Files changed (8)

 
 void TestBasic4Init()
 {
-	#define BODY_NUM 3
+#define  BODY_NUM  3
 
 	int i;
 	pgBodyObject* body[BODY_NUM + 1];
 	pgJointObject* joint[BODY_NUM];
 	pgVector2 a1,a2;
-	PG_Set_Vector2(a1,0,0);
+	PG_Set_Vector2(a1,5,0);
 	PG_Set_Vector2(a2,0,100);
 
 	s_world = PG_WorldNew();
 		PG_AddBodyToWorld(s_world,body[i]);
 	}
 
-	body1 = PG_BodyNew();
+	/*body1 = PG_BodyNew();
 	PG_Set_Vector2(body1->vecPosition,50, 0);
 	body1->bStatic = 1;
 	PG_Bind_RectShape(body1, 20, 300, 0);
-	PG_AddBodyToWorld(s_world, body1);
+	PG_AddBodyToWorld(s_world, body1);*/
 
 	PG_Set_Vector2(body[BODY_NUM]->vecLinearVelocity,100,0)
 
 		joint[i] = PG_DistanceJointNew(body[i],body[i+1],0,50,a1,a2);
 		PG_AddJointToWorld(s_world,joint[i]);
 	}
+#undef BODY_NUM
 
+}
 
+void TestBasic5Init()
+{
+#define  BODY_NUM  1
+
+	int i;
+	pgBodyObject* body[BODY_NUM + 1];
+	pgJointObject* joint[BODY_NUM];
+	pgVector2 a1,a2;
+	PG_Set_Vector2(a1,0,0);
+	PG_Set_Vector2(a2,0,0);
+
+	s_world = PG_WorldNew();
+	s_world->fStepTime = 0.03;
+
+	body[0] = NULL;
+	for (i = 1;i < BODY_NUM + 1;i++)
+	{
+		body[i] = PG_BodyNew();
+		PG_Bind_RectShape(body[i], 20, 20, 0);
+		PG_Set_Vector2(body[i]->vecPosition,0,(-i*50))
+			PG_Set_Vector2(body[i]->vecLinearVelocity,50,0)
+			PG_AddBodyToWorld(s_world,body[i]);
+	}
+
+
+	PG_Set_Vector2(body[BODY_NUM]->vecLinearVelocity,30,0)
+
+		i = 0;
+	joint[i] = PG_DistanceJointNew(body[i+1],body[i],0,50,a1,a2);
+	PG_AddJointToWorld(s_world,joint[i]);
+	for (i = 1;i < BODY_NUM;i++)
+	{
+		joint[i] = PG_DistanceJointNew(body[i],body[i+1],0,50,a1,a2);
+		PG_AddJointToWorld(s_world,joint[i]);
+	}
+
+	#undef BODY_NUM
 }
 
 //===============================================
 
 void InitWorld()
 {
-	TestBasic2Init();
+	TestBasic4Init();
 }
 
 int main (int argc, char** argv)

Test/pgPhysicsRenderer.c

 #include "pgAABBBox.h"
 #include <GL/glut.h>
 
-int RENDER_AABB = 1;
+int RENDER_AABB = 0;
 
 void PGT_RenderWorld(pgWorldObject* world)
 {
 	glBegin(GL_LINES);
 	if (joint->body1 && (!joint->body2))
 	{
-		glVertex2d(joint->body1->vecPosition.real,joint->body1->vecPosition.imag);
+		pgVector2 pos = PG_GetGlobalPos(joint->body1,&pj->anchor1);
+		glVertex2d(pos.real,pos.imag);
+		//glVertex2d(joint->body1->vecPosition.real,joint->body1->vecPosition.imag);
 		glVertex2d(pj->anchor2.real,pj->anchor2.imag);
 	}
 	else if(joint->body1 && joint->body2)

include/pgBodyObject.h

 //bind rect shape with body
 void PG_Bind_RectShape(pgBodyObject* body, double width, double height, double seta);
 
+//get velocity with a local point of a body,assume center is local (0,0)
+pgVector2 PG_GetLocalPointVelocity(pgBodyObject* body,pgVector2 localPoint);
+
 #endif //_PYGAME_PHYSICS_BODY_
 

include/pgVector2.h

 pgVector2 c_crossf(pgVector2 a, double b);
 void c_rotate(pgVector2* a, double seta);
 int c_equal(pgVector2* a, pgVector2* b);
+pgVector2 c_project(pgVector2 l,pgVector2 p);
 
 #endif
 

src/pgBodyObject.c

 	return p_in_A;
 }
 
+pgVector2 PG_GetLocalPointVelocity(pgBodyObject* body,pgVector2 localPoint)
+{
+	pgVector2 vel = c_fcross(body->fAngleVelocity,localPoint);
+	return c_sum(vel,body->vecLinearVelocity);
+}
+
 //============================================================
 //getter and setter functions
 

src/pgJointObject.c

 #include "pgJointObject.h"
+#include "pgShapeObject.h"
 #include <structmember.h>
 
 extern PyTypeObject pgDistanceJointType;
 	PyObject* body1, *body2;
 	int bCollide;
 	static char *kwlist[] = {"body1", "body2", "isCollideConnect", NULL};
-	if(!PyArg_ParseTupleAndKeywords(args,kwds,"|OOi",kwlist,&body1,&body2,&bCollide))
+	static char *kwlist2[] = {"body1", "isCollideConnect", NULL};
+	if(!PyArg_ParseTupleAndKeywords(args,kwds,"OOi",kwlist,&body1,&body2,&bCollide))
 	{
-		return -1;
+		if(!PyArg_ParseTupleAndKeywords(args,kwds,"Oi",kwlist2,&body1,&bCollide))
+			return -1;
+		else
+		{
+			body2 = NULL;
+		}
 	}
 	PG_InitJointBase(joint,(pgBodyObject*)body1,(pgBodyObject*)body2,bCollide);
 	return 0;
 
 static PyObject* _pgJoint_getBody1(pgJointObject* joint,void* closure)
 {
-	return (PyObject*)joint->body1;
+	if (joint->body1)
+	{
+		return (PyObject*)joint->body1;
+	}
+	else
+	{
+		PyErr_SetString(PyExc_ValueError,"body1 is NULL!");
+		return NULL;
+	}
+	
 }
 
 static int _pgJoint_setBody2(pgJointObject* joint,PyObject* value,void* closure)
 
 static PyObject* _pgJoint_getBody2(pgJointObject* joint,void* closure)
 {
-	return (PyObject*)joint->body2;
+	if (joint->body2)
+	{
+		return (PyObject*)joint->body2;
+	}
+	else
+	{
+		PyErr_SetString(PyExc_ValueError,"body2 is NULL!");
+		return NULL;
+	}
 }
 
 static PyGetSetDef _pgJointBase_getseters[] = {
 	}
 }
 
+void _PG_DistanceJoint_ComputeOneDynamic(pgBodyObject* body,pgVector2* staticAnchor,pgVector2* localAnchor,double stepTime)
+{
+	/*double a,b,c,d,e,f,k;
+	pgVector2 localP = PG_GetGlobalPos(body,localAnchor);
+	pgVector2 L = c_diff(localP,*staticAnchor);
+	pgVector2 vP = PG_GetLocalPointVelocity(body,*localAnchor);
+	pgVector2 vPL = c_project(L,vP);
+	pgVector2 dvBody;
+	double dAngleV;
+	vPL = c_neg(vPL);
+
+	localP = c_diff(localP,body->vecPosition);
+	k = body->shape->rInertia / body->fMass;
+
+
+
+	a = (1 - localP.real * localP.imag / k);
+	b = (localP.imag * localP.imag / k);
+	c = (localP.real * localP.real /k);
+	d = a;
+	e = vPL.real;
+	f = vPL.imag;
+
+	dvBody.imag = (e*c - a*f) / (b*c - a*d);
+	dvBody.real = (e*d - f*b) / (a*d - b*c);
+
+	body->vecLinearVelocity = c_sum(body->vecLinearVelocity,dvBody);
+	dAngleV = c_cross(localP,dvBody);
+	dAngleV /= k;
+	body->fRotation += dAngleV;*/
+
+	double a,b,k,temp,lengthP; //for solve equation
+	pgVector2 localP = PG_GetGlobalPos(body,localAnchor);
+	pgVector2 L = c_diff(localP,*staticAnchor);
+	pgVector2 vP = PG_GetLocalPointVelocity(body,*localAnchor);
+	pgVector2 vPL = c_project(L,vP);
+	pgVector2 dvBody;
+	double dAngleV;
+	vPL = c_neg(vPL);
+
+	localP = c_diff(localP,body->vecPosition);
+	k = body->shape->rInertia / body->fMass;
+
+	c_normalize(&L);
+	temp = c_cross(localP,L);
+	a = 1 + c_dot(c_fcross(temp,localP),L) / k;
+	/*lengthP = c_get_length(localP);
+	if (lengthP < 1e-5)
+	{
+		a = 1;
+	}
+	else
+	{
+		a = (1 + temp * temp / (k * lengthP));
+	}*/
+	
+	b = c_dot(vPL,L);
+
+	temp = b /a;
+	dvBody = c_mul_complex_with_real(L,temp);
+
+	body->vecLinearVelocity = c_sum(body->vecLinearVelocity,dvBody);
+	dAngleV = c_cross(localP,dvBody);
+	dAngleV /= k;
+	body->fAngleVelocity += dAngleV;
+
+	return;
+}
+
+void _PG_DistanceJoint_ComputeTwoDynamic(pgDistanceJointObject* joint,double stepTime)
+{
+	pgBodyObject *body1 = joint->joint.body1,*body2 = joint->joint.body2;
+	double a1,a2,b1,b2,k1,k2,temp,temp1,temp2,lengthP1,lengthP2; //for solve equation
+	pgVector2 localP1 = PG_GetGlobalPos(body1,&joint->anchor1);
+	pgVector2 localP2 = PG_GetGlobalPos(body2,&joint->anchor2);
+	pgVector2 L = c_diff(localP1,localP2);
+	pgVector2 vP1 = PG_GetLocalPointVelocity(body1,joint->anchor1);
+	pgVector2 vP2 = PG_GetLocalPointVelocity(body2,joint->anchor2);
+	pgVector2 vPL1 = c_project(L,vP1);
+	pgVector2 vPL2 = c_project(L,vP2);
+	pgVector2 dvBody1,dvBody2;
+	double dAngleV1,dAngleV2;
+	k1 = body1->shape->rInertia / body1->fMass;
+	k2 = body2->shape->rInertia / body2->fMass;
+
+	localP1 = c_diff(localP1,body1->vecPosition);
+	localP2 = c_diff(localP2,body2->vecPosition);
+
+	c_normalize(&L);
+	temp = c_cross(localP1,L);
+	a1 = 1 + c_dot(c_fcross(temp,localP1),L) / k1;
+	/*lengthP1 = c_get_length(localP1);
+	if (lengthP1 < 1e-5)
+	{
+		a1 = 1;
+	}
+	else
+	{
+		a1 = (1 + temp * temp / (k1 * lengthP1));
+	}*/
+
+	a1 /= body1->fMass;
+
+	temp = c_cross(localP2,L);
+	a2 = 1 + c_dot(c_fcross(temp,localP2),L) / k2;
+	/*lengthP2 = c_get_length(localP2);
+	if (lengthP2 < 1e-5)
+	{
+		a2 = 1;
+	}
+	else
+	{
+		a2 = (1 + temp * temp / (k2 * lengthP2));
+	}*/
+
+	a2 /= body2->fMass;
+
+	b1 = c_dot(vPL1,L);
+	b2 = c_dot(vPL2,L);
+
+	temp = (b2 - b1) /(a1 + a2);
+	temp1 = temp / body1->fMass;
+	temp2 = -temp / body2->fMass;
+	dvBody1 = c_mul_complex_with_real(L,temp1);
+	dvBody2 = c_mul_complex_with_real(L,temp2);
+
+	body1->vecLinearVelocity = c_sum(body1->vecLinearVelocity,dvBody1);
+	dAngleV1 = c_cross(localP1,dvBody1);
+	dAngleV1 /= k1;
+	body1->fAngleVelocity += dAngleV1;
+
+	body2->vecLinearVelocity = c_sum(body2->vecLinearVelocity,dvBody2);
+	dAngleV2 = c_cross(localP2,dvBody2);
+	dAngleV2 /= k2;
+	body2->fAngleVelocity += dAngleV2;
+
+}
+
 void PG_SolveDistanceJointVelocity(pgJointObject* joint,double stepTime)
 {
-	pgVector2 vecL;
+	/*pgVector2 vecL;
 	double lamda,cosTheta1V,cosTheta2V,mk;
 	pgVector2 impuseAdd,v1Add,v2Add;
 	pgDistanceJointObject* pJoint = (pgDistanceJointObject*)joint;
 		impuseAdd = c_mul_complex_with_real(vecL, lamda);
 		v1Add = c_div_complex_with_real(impuseAdd, joint->body1->fMass);
 		v2Add = c_div_complex_with_real(impuseAdd, joint->body2->fMass);
-		/*joint->body1->vecLinearVelocity = c_sum(joint->body1->vecLinearVelocity,v1Add);
-		joint->body2->vecLinearVelocity = c_diff(joint->body2->vecLinearVelocity,v2Add);*/
 		joint->body1->vecLinearVelocity = c_diff(joint->body1->vecLinearVelocity, v1Add);
 		joint->body2->vecLinearVelocity = c_sum(joint->body2->vecLinearVelocity, v2Add);
 		return;
+	}*/
+
+	pgDistanceJointObject* pJoint = (pgDistanceJointObject*)joint;
+	pgBodyObject* body1 = joint->body1;
+	pgBodyObject* body2 = joint->body2;
+	if (body1 && (!body2))
+	{
+		if (body1->bStatic)
+		{
+			return;
+		}
+		else
+		{
+			_PG_DistanceJoint_ComputeOneDynamic(body1,&pJoint->anchor2,&pJoint->anchor1,stepTime);
+
+			/*double a,b,c,d,e,f,k;
+			pgVector2 localP = PG_GetGlobalPos(body1,&pJoint->anchor1);
+			pgVector2 L = c_diff(localP,pJoint->anchor2);
+			pgVector2 vP = PG_GetLocalPointVelocity(body1,pJoint->anchor1);
+			pgVector2 vPL = c_project(L,vP);
+			pgVector2 dvBody;
+			double dAngleV;
+			vPL = c_neg(vPL);
+
+			localP = c_diff(localP,body1->vecPosition);
+			k = body1->shape->rInertia / body1->fMass;
+
+
+			
+			a = (1 - localP.real * localP.imag / k);
+			b = (localP.imag * localP.imag / k);
+			c = (localP.real * localP.real /k);
+			d = a;
+			e = vPL.real;
+			f = vPL.imag;
+			
+			dvBody.imag = (e*c - a*f) / (b*c - a*d);
+			dvBody.real = (e*d - f*b) / (a*d - b*c);
+
+			body1->vecLinearVelocity = c_sum(body1->vecLinearVelocity,dvBody);
+			dAngleV = c_cross(localP,dvBody);
+			dAngleV /= k;
+			body1->fRotation += dAngleV;*/
+
+
+			return;
+		}
+	}
+
+	if(body1 && body2)
+	{
+		if (body1->bStatic || body2->bStatic)
+		{
+			return;
+		}
+		_PG_DistanceJoint_ComputeTwoDynamic(pJoint,stepTime);
+				
 	}
 }
 
 //just for C test usage, not for python???
 pgJointObject* PG_DistanceJointNew(pgBodyObject* b1,pgBodyObject* b2,int bCollideConnect,double dist,pgVector2 a1,pgVector2 a2)
 {
-	pgDistanceJointObject* pjoint = (pgDistanceJointObject*)PyObject_MALLOC(sizeof(pgDistanceJointObject));	
-	//pgDistanceJointObject* pjoint = (pgDistanceJointObject*)PyObject_MALLOC(sizeof(pgDistanceJointObject));
+	//pgDistanceJointObject* pjoint = (pgDistanceJointObject*)PyObject_MALLOC(sizeof(pgDistanceJointObject));	
+	pgDistanceJointObject* pjoint = (pgDistanceJointObject*)PyObject_MALLOC(sizeof(pgDistanceJointObject));
 	PG_InitJointBase(&(pjoint->joint), b1, b2, bCollideConnect);
 	pjoint->distance = dist;
 	pjoint->anchor1 = a1;
 	return (pgJointObject*)pjoint;
 }
 
+static PyObject* _pgDistanceJoint_getDistance(pgDistanceJointObject* joint,void* closure)
+{
+	return PyFloat_FromDouble(joint->distance);
+}
+
+static int _pgDistanceJoint_setDistance(pgDistanceJointObject* joint,PyObject* value,void* closure)
+{
+	if(PyFloat_Check(value))
+	{
+		joint->distance = PyFloat_AsDouble(value);
+		return 0;
+	}
+	else
+	{
+		PyErr_SetString(PyExc_TypeError, "value must be float number");
+		return -1;
+	}
+}
+
+static PyObject* _pgDistanceJoint_getAnchor1(pgDistanceJointObject* joint,void* closure)
+{
+	return PyComplex_FromCComplex(joint->anchor1);
+}
+
+static int _pgDistanceJoint_setAnchor1(pgDistanceJointObject* joint,PyObject* value,void* closure)
+{
+	if (PyComplex_Check(value))
+	{
+		joint->anchor1 = PyComplex_AsCComplex(value);
+		return 0;
+	}
+	else
+	{
+		PyErr_SetString(PyExc_TypeError, "value must be complex number");
+		return -1;
+	}
+}
+
+static PyObject* _pgDistanceJoint_getAnchor2(pgDistanceJointObject* joint,void* closure)
+{
+	return PyComplex_FromCComplex(joint->anchor2);
+}
+
+static int _pgDistanceJoint_setAnchor2(pgDistanceJointObject* joint,PyObject* value,void* closure)
+{
+	if (PyComplex_Check(value))
+	{
+		joint->anchor2 = PyComplex_AsCComplex(value);
+		return 0;
+	}
+	else
+	{
+		PyErr_SetString(PyExc_TypeError, "value must be complex number");
+		return -1;
+	}
+}
+
 static PyMemberDef _pgDistanceJoint_members[] = 
 {
-	{"distance",T_DOUBLE,offsetof(pgDistanceJointObject,distance),0,""},
+	{	NULL }
+}; 
+
+static PyGetSetDef _pgDistanceJoint_getseters[] = {
+	{
+		"distance",(getter)_pgDistanceJoint_getDistance,(setter)_pgDistanceJoint_setDistance,"",NULL,
+	},
+	{
+		"anchor1",(getter)_pgDistanceJoint_getAnchor1,(setter)_pgDistanceJoint_setAnchor1,"",NULL,
+	},
+	{
+		"anchor2",(getter)_pgDistanceJoint_getAnchor2,(setter)_pgDistanceJoint_setAnchor2,"",NULL,
+	},
 	{
 		NULL
 	}
-}; 
+};
+
 
 
 
 	0,                          /* tp_iternext */
 	0,							/* tp_methods */
 	_pgDistanceJoint_members,	/* tp_members */
-	0,							/* tp_getset */
+	_pgDistanceJoint_getseters,	/* tp_getset */
 	0,							/* tp_base */
 	0,                          /* tp_dict */
 	0,                          /* tp_descr_get */
 }
 
 
+pgVector2 c_project(pgVector2 l,pgVector2 p)
+{
+	double lp;
+	c_normalize(&l);
+	lp = c_dot(l,p);
+	return c_mul_complex_with_real(l,lp);
+}
+
+

src/pgWorldObject.c

 	{
 		pgJointObject* joint = (pgJointObject*)(PyList_GetItem((PyObject*)(world->jointList),i));
 		//what happened here?
-		if (joint->SolveConstraintPosition)
-		{
-			joint->SolveConstraintPosition(joint,stepTime);
-		}
 		if (joint->SolveConstraintVelocity)
 		{
 			joint->SolveConstraintVelocity(joint,stepTime);
 		}
+		/*if (joint->SolveConstraintPosition)
+		{
+			joint->SolveConstraintPosition(joint,stepTime);
+		}*/
 	}
 }
 
 	}
 }
 
+static PyObject* _world_add_joint(pgWorldObject* world,PyObject* args)
+{
+	pgJointObject* joint;
+	if (!PyArg_ParseTuple(args,"O",&joint))
+	{
+		PyErr_SetString(PyExc_ValueError,"arg is not joint type");
+		return NULL;
+	}
+	if(PG_AddJointToWorld(world,joint))
+	{
+		Py_RETURN_FALSE;
+	}
+	else
+	{
+		Py_RETURN_TRUE;
+	}
+}
+
 
 static PyObject* _pgWorld_getGravity(pgWorldObject* world,void* closure)
 {
 	return (PyObject*)world->bodyList;
 }
 
+static PyObject* _pgWorld_getJointList(pgWorldObject* world,void* closure)
+{
+	Py_INCREF ((PyObject*)world->jointList);
+	return (PyObject*)world->jointList;
+}
+
+
 
 /**
 * Here we allow the Python object to do stuff like
 		"body_list",(getter)_pgWorld_getBodyList,NULL,NULL
 	},
 	{
+		"joint_list",(getter)_pgWorld_getJointList,NULL,NULL
+	},
+	{
 		NULL
 	}
 };
 	//{ "test_args", (PyCFunction) _world_test_args, METH_VARARGS, "" },
 	{ "update", (PyCFunction) _world_update, METH_VARARGS, "" },
 	{"add_body",(PyCFunction) _world_add_body, METH_VARARGS, ""},
+	{"add_joint",(PyCFunction) _world_add_joint, METH_VARARGS, ""},
 	{ NULL, NULL, 0, NULL } /* The NULL sentinel is important! */
 };