Commits

Anonymous committed f6ce9e1

workable but not perfect collision reactions.

Comments (0)

Files changed (10)

 
 //ĹÜąí
 static double s_time;
-static int s_first = 1;
 
 void watch_start()
 {
 
 /*-------------------------------˛âĘÔşŻĘý----------------------------*/
 
+pgBodyObject* body, * body1;
+
 //äÖČžşŻĘý
 void do_render()
 {
-	//glprintf(0, 0, "Your RP value is: %d", 0);
 	glColor3f(1.f, 1.f, 1.f);
-	/*glBegin(GL_LINES);
-	glVertex2f(-10, 0);
-	glVertex2f(10, 0);
-	glEnd();*/
+	PG_Update(s_world, 0.005);
 	PGT_RenderWorld(s_world);
-
+	glprintf(0, 0, "vec of body:(%.2f, %.2f)", body->vecLinearVelocity.real, 
+		body->vecLinearVelocity.imag);
+	glprintf(0, 16, "force of body: (%.2f, %.2f)", body->vecForce.real,
+		body->vecForce.imag);
 }
 
 
 
 void display(void)
 {
-	double dt;
-	//watch_start();
-
-	
-
-	if(s_first)
-	{
-		watch_start();
-		s_first = 0;
-		return;
-	}
-	dt = watch_stop();
-	watch_start();
-	/*s_updateTime += dt;
-	if(s_updateTime >= s_world->fStepTime)
-	{
-		PG_Update(s_world,s_updateTime);
-		s_updateTime = 0.0;
-
-		glClear(GL_COLOR_BUFFER_BIT);
-		glLoadIdentity();
-		do_render();
-		glutSwapBuffers();
-	}*/
-	PG_Update(s_world,dt);
-	
 	glClear(GL_COLOR_BUFFER_BIT);
 	glLoadIdentity();
 	do_render();
 	glutSwapBuffers();
-
-	
-	
 }
 
 //Őâ¸öşŻĘýŇťżŞĘźžÍťáąťľ÷ÓĂŁŹšĘgluPerspectiveşŻĘýĂťąŘŇŞÔÚinitGLťňŐßdisplayşŻĘýŔďľ÷ÓĂ
 
 void TestBasic2Init()
 {
-	pgBodyObject* body;
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
 	body = PG_BodyNew();
 	PG_Set_Vector2(body->vecPosition,0,0)
 	PG_Set_Vector2(body->vecLinearVelocity,0,30)
-	PG_AddBodyToWorld(s_world,body);
+	PG_AddBodyToWorld(s_world, body);
+	body1 = PG_BodyNew();
+	PG_Set_Vector2(body1->vecPosition,0, -100);
+	body1->bStatic = 1;
+	PG_AddBodyToWorld(s_world, body1);
 }
 
 void TestBasic3Init()
 
 void InitWorld()
 {
-	TestBasic4Init();
+	TestBasic2Init();
 }
 
 int main (int argc, char** argv)

Test/pgPhysicsRenderer.c

 	PG_Set_Vector2(p[1], box->right, box->bottom);
 	PG_Set_Vector2(p[2], box->right, box->top);
 	PG_Set_Vector2(p[3], box->left, box->top);
-	for(i = 0; i < 4; ++i)
-		gp[i] = PG_GetGlobalPos(body, &p[i]);
 
 	glColor3f(0.f, 1.f, 1.f);
 	glEnable(GL_LINE_STIPPLE);
 	glLineWidth(1.f);
 	glLineStipple(1, 0xF0F0);
 	glBegin(GL_LINE_LOOP);
-	glVertex2d(gp[0].real, gp[0].imag);
-	glVertex2d(gp[1].real, gp[1].imag);
-	glVertex2d(gp[2].real, gp[2].imag);
-	glVertex2d(gp[3].real, gp[3].imag);
-	glVertex2d(gp[0].real, gp[0].imag);
+	glVertex2d(p[0].real, p[0].imag);
+	glVertex2d(p[1].real, p[1].imag);
+	glVertex2d(p[2].real, p[2].imag);
+	glVertex2d(p[3].real, p[3].imag);
+	glVertex2d(p[0].real, p[0].imag);
 	glEnd();
 	glDisable(GL_LINE_STIPPLE);
 }

include/pgBodyObject.h

 	double		fMass;
 	pgVector2	vecLinearVelocity;
 	double		fAngleVelocity;
-	//int			bStatic;
+	int			bStatic;
 
 	pgVector2	vecPosition;
 	double		fRotation;

include/pgJointObject.h

 
 pgJointObject* PG_DistanceJointNew(pgBodyObject* b1,pgBodyObject* b2,int bCollideConnect,double distance,pgVector2 a1,pgVector2 a2);
 
+extern PyTypeObject pgJointType;
+
 #endif //_PYGAME_PHYSICS_JOINT_
 

include/pgShapeObject.h

 	//virtual functions
 	void (*Destroy)(pgShapeObject* shape);
 	int (*Collision)(pgBodyObject* selfBody, pgBodyObject* incidBody, PyObject* contactList);
-	void (*UpdateAABB)(pgShapeObject* shape);
+	void (*UpdateAABB)(pgBodyObject* body);
 };
 
 
 {
 	pgVector2 totalVelAdd;
 	double k;
+	if(body->bStatic) return;
+
 	totalVelAdd = c_sum(body->vecForce,world->vecGravity);
 	k = dt / body->fMass;
 	totalVelAdd = c_mul_complex_with_real(totalVelAdd,k);
 	//totalVelAdd = c_div_complex_with_real(body->vecImpulse,body->fMass);
 	//body->vecLinearVelocity = c_sum(body->vecLinearVelocity,totalVelAdd);
 
+	if(body->bStatic) return;
+
 	totalPosAdd = c_mul_complex_with_real(body->vecLinearVelocity,dt);
 	body->vecPosition = c_sum(body->vecPosition,totalPosAdd);
-	body->shape->UpdateAABB(body->shape);
 }
 
 void PG_BodyInit(pgBodyObject* body)
 #include "pgBodyObject.h"
 #include <assert.h>
 
+extern PyTypeObject pgContactType;
+
 // We borrow this graph from Box2DLite
 // Box vertex and edge numbering:
 //
 	moment = c_mul_complex_with_real(contact->normal, moment_len);
 
 	//update the v and w
-	refBody->vecLinearVelocity = c_diff(refBody->vecLinearVelocity, 
-		c_div_complex_with_real(moment, refBody->fMass));
-	refBody->fAngleVelocity -= c_cross(refR, moment)/refBody->shape->rInertia;
+	if(!refBody->bStatic)
+	{
+		refBody->vecLinearVelocity = c_diff(refBody->vecLinearVelocity, 
+			c_div_complex_with_real(moment, refBody->fMass/10));
+		//refBody->fAngleVelocity -= c_cross(refR, moment)/refBody->shape->rInertia;
+	}
 
-	incidBody->vecLinearVelocity = c_sum(incidBody->vecLinearVelocity, 
-		c_div_complex_with_real(moment, incidBody->fMass));
-	incidBody->fAngleVelocity -= c_cross(refR, moment)/incidBody->shape->rInertia;
+	if(!incidBody->bStatic)
+	{
+		incidBody->vecLinearVelocity = c_sum(incidBody->vecLinearVelocity, 
+			c_div_complex_with_real(moment, incidBody->fMass));
+		//incidBody->fAngleVelocity -= c_cross(refR, moment)/incidBody->shape->rInertia;
+	}
 }
 
 void PG_UpdateP(pgJointObject* joint, double step)
 {
 	//TODO: concern dt
-	joint->body1->vecPosition = c_sum(joint->body1->vecPosition, joint->body1->vecLinearVelocity);
-	joint->body1->fRotation += joint->body1->fAngleVelocity;
+	if(!joint->body1->bStatic)
+	{
+		joint->body1->vecPosition = c_sum(joint->body1->vecPosition, 
+			c_mul_complex_with_real(joint->body1->vecLinearVelocity, step));
+		joint->body1->fRotation += joint->body1->fAngleVelocity*step;
+	}
 
-	joint->body2->vecPosition = c_sum(joint->body2->vecPosition, joint->body2->vecLinearVelocity);
-	joint->body2->fRotation += joint->body2->fAngleVelocity;
+	if(!joint->body2->bStatic)
+	{
+		joint->body2->vecPosition = c_sum(joint->body2->vecPosition, 
+			c_mul_complex_with_real(joint->body2->vecLinearVelocity, step));
+		joint->body2->fRotation += joint->body2->fAngleVelocity*step;
+	}
 }
 
-
+PyObject* _PG_ContactNew(PyTypeObject *type, PyObject *args, PyObject *kwds)
+{
+	pgContact* op;
+	type->tp_base = &pgJointType;
+	if(PyType_Ready(type) < 0) return NULL;
+	op = (pgContact*)type->tp_alloc(type, 0);
+	return (PyObject*)op;
+}
 
 pgJointObject* PG_ContactNew(pgBodyObject* refBody, pgBodyObject* incidBody)
 {
 	pgContact* contact;
 	//TODO: this function would be replaced.
-	contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+	contact = (pgContact*)_PG_ContactNew(&pgContactType, NULL, NULL);
 	contact->joint.body1 = refBody;
 	contact->joint.body2 = incidBody;
 	contact->joint.SolveConstraintPosition = PG_UpdateP;
 	contact->joint.SolveConstraintVelocity = PG_UpdateV;
+	contact->joint.Destroy = NULL;
 
 	return (pgJointObject*)contact;
 }
 
+PyTypeObject pgContactType =
+{
+	PyObject_HEAD_INIT(NULL)
+	0,
+	"physics.Contact",			/* tp_name */
+	sizeof(pgContact),			/* tp_basicsize */
+	0,                          /* tp_itemsize */
+	0,							/* tp_dealloc */
+	0,                          /* tp_print */
+	0,                          /* tp_getattr */
+	0,                          /* tp_setattr */
+	0,                          /* tp_compare */
+	0,                          /* tp_repr */
+	0,                          /* tp_as_number */
+	0,                          /* tp_as_sequence */
+	0,                          /* tp_as_mapping */
+	0,                          /* tp_hash */
+	0,                          /* tp_call */
+	0,                          /* tp_str */
+	0,                          /* tp_getattro */
+	0,                          /* tp_setattro */
+	0,                          /* tp_as_buffer */
+	Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, /*tp_flags*/
+	"",                         /* tp_doc */
+	0,                          /* tp_traverse */
+	0,                          /* tp_clear */
+	0,                          /* tp_richcompare */
+	0,                          /* tp_weaklistoffset */
+	0,                          /* tp_iter */
+	0,                          /* tp_iternext */
+	0,							/* tp_methods */
+	0,							/* tp_members */
+	0,							/* tp_getset */
+	0,							/* tp_base */
+	0,                          /* tp_dict */
+	0,                          /* tp_descr_get */
+	0,                          /* tp_descr_set */
+	0,                          /* tp_dictoffset */
+	0,							/* tp_init */
+	0,							/* tp_alloc */
+	_PG_ContactNew,				/* tp_new */
+	0,                          /* tp_free */
+	0,                          /* tp_is_gc */
+	0,                          /* tp_bases */
+	0,                          /* tp_mro */
+	0,                          /* tp_cache */
+	0,                          /* tp_subclasses */
+	0,                          /* tp_weaklist */
+	0                           /* tp_del */
+};
 #include "pgJointObject.h"
 #include <structmember.h>
 
-extern PyTypeObject pgJointType;
 extern PyTypeObject pgDistanceJointType;
 
 void PG_InitJointBase(pgJointObject* joint,pgBodyObject* b1,pgBodyObject* b2,int bCollideConnect)
 	return op;
 }
 
+//TODO: this function would get err when inherited level > 2
 void PG_JointDestroy(pgJointObject* joint)
 {
 	if (joint->Destroy)
 	{
 		joint->Destroy(joint);
 	}
-	
+
 	joint->ob_type->tp_free((PyObject*)joint);
 }
 
 
 static int _pgJointBase_init(pgJointObject* joint,PyObject *args, PyObject *kwds)
 {
-	PyObject* body1,*body2;
+	PyObject* body1, *body2;
 	int bCollide;
 	static char *kwlist[] = {"body1", "body2", "isCollideConnect", NULL};
 	if(!PyArg_ParseTupleAndKeywords(args,kwds,"|OOi",kwlist,&body1,&body2,&bCollide))
 	0,                          /* tp_weaklistoffset */
 	0,                          /* tp_iter */
 	0,                          /* tp_iternext */
-	0,           /* tp_methods */
-	0,           /* tp_members */
-	_pgJointBase_getseters,         /* tp_getset */
+	0,							/* tp_methods */
+	0,							/* tp_members */
+	_pgJointBase_getseters,     /* tp_getset */
 	0,                          /* tp_base */
 	0,                          /* tp_dict */
 	0,                          /* tp_descr_get */
 	0,                          /* tp_descr_set */
 	0,                          /* tp_dictoffset */
-	(initproc)_pgJointBase_init,                          /* tp_init */
+	(initproc)_pgJointBase_init, /* tp_init */
 	0,                          /* tp_alloc */
-	_PG_JointBaseNew,               /* tp_new */
+	_PG_JointBaseNew,           /* tp_new */
 	0,                          /* tp_free */
 	0,                          /* tp_is_gc */
 	0,                          /* tp_bases */
 
 static int _pgDistanceJoint_init(pgDistanceJointObject* joint,PyObject *args, PyObject *kwds)
 {
-	if(pgJointType.tp_init((PyObject*)joint,args,kwds) < 0)
+	if(pgJointType.tp_init((PyObject*)joint, args, kwds) < 0)
 	{
 		return -1;
 	}
 	}
 }
 
-//just for C test usage, not for python
+//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));	
 	0,							/* tp_methods */
 	_pgDistanceJoint_members,	/* tp_members */
 	0,							/* tp_getset */
-	0,               /* tp_base */
+	0,							/* tp_base */
 	0,                          /* tp_dict */
 	0,                          /* tp_descr_get */
 	0,                          /* tp_descr_set */
 
 //functions of pgRectShape
 
-void PG_RectShapeUpdateAABB(pgShapeObject* rectShape)
+void PG_RectShapeUpdateAABB(pgBodyObject* body)
 {
 	int i;
-	pgRectShape *p = (pgRectShape*)rectShape;
-	PG_AABBClear(&(p->shape.box));
-	for(i = 0; i < 4; ++i)
-		PG_AABBExpandTo(&(p->shape.box), &(p->point[i]));
+	pgVector2 gp[4];
+
+	if(body->shape->type == ST_RECT)
+	{
+		pgRectShape *p = (pgRectShape*)body->shape;
+		
+		PG_AABBClear(&(p->shape.box));
+		for(i = 0; i < 4; ++i)
+			gp[i] = PG_GetGlobalPos(body, &(p->point[i]));
+		for(i = 0; i < 4; ++i)
+			PG_AABBExpandTo(&(p->shape.box), &gp[i]);
+	}
 }
 
 void PG_RectShapeDestroy(pgShapeObject* rectShape)
 		}
 	}
 	//now all the contact points are added to list
-	to = PyList_Size(contactList);
+	to = PyList_Size(contactList) - 1;
 
 	
 	_SAT_GetContactNormal(&clipBox, contactList, from, to);
 		contact = (pgContact*)PyList_GetItem(contactList, i);
 
 		c_rotate(&(contact->pos), selfBody->fRotation);
-		c_sum(contact->pos, selfBody->vecPosition);
+		contact->pos = c_sum(contact->pos, selfBody->vecPosition);
 		
 		c_rotate(&(contact->normal), selfBody->fRotation);
 	}
 #include "pgShapeObject.h"
 #include <structmember.h>
 
-#define MAX_SOLVE_INTERAT 20
+#define MAX_SOLVE_INTERAT 10
 
 extern PyTypeObject pgWorldType;
 
 	Py_ssize_t i;
 	for (i = 0;i < size;i++)
 	{
-		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList),i));
-		
+		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList),i));		
 		PG_FreeUpdateBodyVel(world,body,stepTime);
 	}
 }
 
-void _PG_BodyCollisionDetection(pgWorldObject* world)
+void _PG_BodyCollisionDetection(pgWorldObject* world, double step)
 {
-	Py_ssize_t i, j;
+	Py_ssize_t i, j, cnt, size;
 	pgBodyObject* refBody, *incBody;
-	Py_ssize_t size = PyList_Size((PyObject*)(world->bodyList));
-	//TODO: clear contactList first
+	pgJointObject* contact;
+	
+	size = PyList_Size((PyObject*)(world->bodyList));
+	//clear contactList first
+	cnt = PyList_Size((PyObject*)(world->contactList));
+	if(PyList_SetSlice((PyObject*)(world->contactList), 0, cnt, NULL) < 0) return;
+	cnt = PyList_Size((PyObject*)(world->contactList));//test
+	assert(cnt==0);
+	//Py_XDECREF((PyObject*)world->contactList);
+	//world->contactList = (PyListObject*)PyList_New(0);
+	//for all pair of objects, do collision test
+	//update AABB
+	for(i = 0; i < size; ++i)
+	{
+		refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
+		refBody->shape->UpdateAABB(refBody);
+	}
 	for(i = 0; i < size-1; ++i)
 	{
 		refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
 		for(j = i+1; j < size; ++j)
 		{
 			incBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), j));
-			if(PG_IsOverlap(&(refBody->shape->box), &(refBody->shape->box)))
+			if(PG_IsOverlap(&(refBody->shape->box), &(incBody->shape->box)))
 			{
 				PG_AppendContact(refBody, incBody, (PyObject*)world->contactList);
 			}
 		}
 	}
+	//now collision reaction
+	cnt = PyList_Size((PyObject*)(world->contactList));
+	for(i = 0; i < cnt; ++i)
+	{
+		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
+		contact->SolveConstraintVelocity(contact, step);
+		contact->SolveConstraintPosition(contact, step);
+	}
 }
 
 void _PG_JointSolve(pgWorldObject* world,double stepTime)
 	int i;
 
 	_PG_FreeBodySimulation(world, stepTime);
-	//_PG_BodyCollisionDetection(world);
+	_PG_BodyCollisionDetection(world, stepTime);
 	for (i = 0;i < MAX_SOLVE_INTERAT;i++)
 	{
-		_PG_JointSolve(world,stepTime);
+		_PG_JointSolve(world, stepTime);
 	}
 	
 	_PG_BodyPositionUpdate(world, stepTime);
 
 }
 
-
-
-
-
-
 void PG_WorldInit(pgWorldObject* world)
 {
 	world->bodyList = (PyListObject*)PyList_New(0);
 	world->jointList = (PyListObject*)PyList_New(0);
+	world->contactList = (PyListObject*)PyList_New(0);
 	world->fDamping = 0.0;
 	world->fStepTime = 0.1;
 	PG_Set_Vector2(world->vecGravity,0.0,-50);