Commits

Anonymous committed 889a0a9

fixed some bugs.

  • Participants
  • Parent commits 81dfa73
  • Branches physics

Comments (0)

Files changed (7)

 void do_render()
 {
 	glColor3f(1.f, 1.f, 1.f);
-	PG_Update(s_world, 0.02);
+	PG_Update(s_world, 0.002);
 	PGT_RenderWorld(s_world);
 	//glprintf(0, 0, "Velocity of body: (%.2f, %.2f)", body->vecLinearVelocity.real, 
 	//	body->vecLinearVelocity.imag);
 	int i = 0;
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
-	PG_Set_Vector2(s_world->vecGravity, 0, -600.f); //Orz重力居然要-10000.f才像话
+	PG_Set_Vector2(s_world->vecGravity, 0, -200.f);
 
-	for(i = 0; i < 7; ++i)
+	for(i = 0; i < 3; ++i)
 	{
 		body = PG_BodyNew();
 		PG_Set_Vector2(body->vecPosition, -100+100*i, 200);
-		PG_Set_Vector2(body->vecLinearVelocity, (20 - 10*i)/2, 0.f);
-		body->fRotation = 0;
+		PG_Set_Vector2(body->vecLinearVelocity, (100 - 100*i)/2, 0.f);
+		body->fRotation = M_PI/2;
 		body->fAngleVelocity = 3.f;
 		body->fRestitution = 0.0f;
-		body->fMass = 30.f;
-		PG_Bind_RectShape(body, 40, 25, 0);
+		body->fMass = 600.f;
+		PG_Bind_RectShape(body, 40, 30, 0);
 		PG_AddBodyToWorld(s_world, body);
 	}
 
 	body1->bStatic = 1;
 	body1->fRestitution = 1.f;//for test
 	body1->fMass = 1e24;
+	body1->fRotation = 0.f;
 	PG_Bind_RectShape(body1, 1000, 20, 0);
 	PG_AddBodyToWorld(s_world, body1);
 

include/pgAABBBox.h

 void PG_AABBExpandTo(pgAABBBox* box, pgVector2* p);
 void PG_AABBClear(pgAABBBox* box);
 int PG_IsOverlap(pgAABBBox* boxA, pgAABBBox* boxB);
-int PG_IsIn(pgVector2* p, pgAABBBox* box);
+int PG_IsIn(pgVector2* p, pgAABBBox* box, double eps);
 
 #endif //_PYGAME_MATH_AABBBOX_
 
 	return from_x <= to_x && from_y <= to_y;
 }
 
-PG_IsIn(pgVector2* p, pgAABBBox* box)
+PG_IsIn(pgVector2* p, pgAABBBox* box, double eps)
 {
-	return box->left <= p->real && p->real <= box->right
-		&& box->bottom <= p->imag && p->imag <= box->top;
-}
+	return box->left - eps < p->real && p->real < box->right + eps
+		&& box->bottom - eps < p->imag && p->imag < box->top + eps;
+}

src/pgBodyObject.c

 
 void PG_FreeUpdateBodyVel(pgWorldObject* world,pgBodyObject* body, double dt)
 {
-	pgVector2 totalVelAdd;
 	pgVector2 totalF;
-	double k;
 	if(body->bStatic) return;
 
-	totalF = c_sum(body->vecForce, world->vecGravity);
-	k = dt / body->fMass;
-	totalVelAdd = c_mul_complex_with_real(totalF, k);
-	body->vecLinearVelocity = c_sum(body->vecLinearVelocity, totalVelAdd);
+	totalF = c_sum(body->vecForce, c_mul_complex_with_real(world->vecGravity,
+		body->fMass));
+	body->vecLinearVelocity = c_sum(body->vecLinearVelocity, 
+		c_mul_complex_with_real(totalF, dt/body->fMass));
 	
 }
 

src/pgCollision.c

 void PG_ApplyContact(PyObject* contactObject, double step)
 {
 #define MAX_C_DEP 0.02
-#define BIAS_FACTOR 0.05
+#define BIAS_FACTOR 0.15
 
 	pgVector2 neg_dV, refV, incidV;
 	pgVector2 refR, incidR;

src/pgShapeObject.c

 	return (pgShapeObject*)p;
 }
 
-//static int _pgRectShape_init(pgRectShape* joint,PyObject *args, PyObject *kwds)
-//{
-//	if(pgJointType.tp_init((PyObject*)joint, args, kwds) < 0)
-//	{
-//		return -1;
-//	}
-//
-//	return 0;
-//}
-
-//-------------box's collision test------------------
-//TEST: these functions have been partly tested.
-
-//we use a simple SAT to select the contactNormal:
-//Supposing the relative velocity between selfBody and incidBody in
-//two frame is "small", the face(actually is an edge in 2D) with minimum 
-//average penetrating depth is considered to be contact face, then we
-//get the contact normal.
-//note: this method is not available in CCD(continue collision detection)
-//since the velocity is not small.
-//static double _SAT_GetContactNormal1(pgAABBBox* clipBox, PyObject* contactList,
-//								  int from, int to)
-//{
-//	int i;
-//	int id;
-//	double deps[4], min_dep;
-//	pgContact* p;
-//	pgVector2 normal;
-//		
-//	memset(deps, 0, sizeof(deps));
-//	for(i = from; i <= to; ++i)
-//	{
-//		p = (pgContact*)PyList_GetItem(contactList, i);
-//		deps[0] += p->pos.real - clipBox->left; //left
-//		deps[1] += p->pos.imag - clipBox->bottom; //bottom
-//		deps[2] += clipBox->right - p->pos.real; //right
-//		deps[3] += clipBox->top - p->pos.imag; //top
-//	}
-//	
-//	//find min penetrating face
-//	id = 0;
-//	min_dep = deps[0];
-//	for(i = 1; i < 4; ++i)
-//	{
-//		if(min_dep > deps[i])
-//		{
-//			min_dep = deps[i];
-//			id = i;
-//		}
-//	}
-//	PG_Set_Vector2(normal, 0, 0);
-//	//generate contactNormal
-//	switch(id)
-//	{
-//	case 0://left
-//		PG_Set_Vector2(normal, -1, 0);
-//		break;
-//	case 1://bottom
-//		PG_Set_Vector2(normal, 0, -1);
-//		break;
-//	case 2://right
-//		PG_Set_Vector2(normal, 1, 0);
-//		break;
-//	case 3://top
-//		PG_Set_Vector2(normal, 0, 1);
-//		break;
-//	}
-//
-//    for(i = from; i <= to; ++i)
-//    {
-//        p = (pgContact*)PyList_GetItem(contactList, i);
-//        p->normal = normal;
-//    }
-//
-//	return min_dep;
-//}
-//
-//#define _swap(a, b, t) {(t) = (a); (a) = (b); (b) = (t);}
-//
-////TODO: now just detect Box-Box collision, later add Box-Circle
-//int PG_RectShapeCollision1(pgBodyObject* selfBody, pgBodyObject* incidBody, PyObject* contactList)
-//{
-//	int i, i1, k;
-//	int from, to, _from[2], _to[2];
-//	int apart;
-//	pgVector2 ip[4];
-//	int has_ip[4]; //use it to prevent from duplication
-//	pgVector2 pf, pt;
-//	pgRectShape *self, *incid, *tmp;
-//	pgBodyObject * tmpBody;
-//	pgAABBBox clipBox;
-//	pgContact* contact;
-//	pgVector2* pAcc;
-//	PyObject* list[2];
-//	double dist[2];
-//
-//	list[0] = PyList_New(0);
-//	list[1] = PyList_New(0);
-//
-//	self = (pgRectShape*)selfBody->shape;
-//	incid = (pgRectShape*)incidBody->shape;
-//
-//	apart = 1;	
-//	for(k = 0; k < 2; ++k)
-//	{
-//		//transform incidBody's coordinate according to selfBody's coordinate
-//		for(i = 0; i < 4; ++i)
-//			ip[i] = PG_GetRelativePos(selfBody, incidBody, &(incid->point[i]));
-//		//clip incidBody by selfBody
-//		clipBox = PG_GenAABB(self->bottomLeft.real, self->topRight.real,
-//			self->bottomLeft.imag, self->topRight.imag);
-//		memset(has_ip, 0, sizeof(has_ip));
-//		_from[k] = PyList_Size(list[k]);
-//		
-//		for(i = 0; i < 4; ++i)
-//		{
-//			i1 = (i+1)%4;
-//			if(PG_LiangBarskey(&clipBox, &ip[i], &ip[i1], &pf, &pt))
-//			{
-//				apart = 0;
-//				if(pf.real == ip[i].real && pf.imag == ip[i].imag)
-//				{
-//					has_ip[i] = 1;
-//				}
-//				else
-//				{
-//					contact = (pgContact*)PG_ContactNew(selfBody, incidBody);
-//					contact->pos = pf;
-//					PyList_Append(list[k], (PyObject*)contact);
-//				}
-//				
-//				if(pt.real == ip[i1].real && pt.imag == ip[i1].imag)
-//				{	
-//					has_ip[i1] = 1;
-//				}
-//				else
-//				{
-//					contact = (pgContact*)PG_ContactNew(selfBody, incidBody);
-//					contact->pos = pt;
-//					PyList_Append(list[k], (PyObject*)contact);
-//				}
-//
-//			}
-//		}
-//
-//		if(apart)
-//			goto END;
-//
-//		for(i = 0; i < 4; ++i)
-//		{
-//			if(has_ip[i])
-//			{
-//				contact = (pgContact*)PG_ContactNew(selfBody, incidBody);
-//				contact->pos = ip[i];
-//				PyList_Append(list[k], (PyObject*)contact);
-//			}
-//		}
-//		//now all the contact points are added to list
-//		_to[k] = PyList_Size(list[k]) - 1;
-//		dist[k] = _SAT_GetContactNormal(&clipBox, list[k], _from[k], _to[k]);
-//
-//		//now swap refBody and incBody, and do it again
-//		_swap(selfBody, incidBody, tmpBody);
-//		_swap(self, incid, tmp);
-//	}
-//
-//	from = PyList_Size(contactList);
-//	for(i = 0; i < 2; ++i)
-//	{
-//		i1 = (i+1)%2;
-//		if(dist[i] <= dist[i1])
-//		{
-//			for(k = _from[i]; k <= _to[i]; ++k)
-//			{
-//				contact = (pgContact*)PyList_GetItem(list[i], k);
-//				PyList_Append(contactList, (PyObject*)contact);
-//				Py_XINCREF((PyObject*)contact);
-//			}
-//			break;
-//		}
-//		_swap(selfBody, incidBody, tmpBody);
-//	}
-//	to = PyList_Size(contactList) - 1;
-//
-//	pAcc = PyObject_Malloc(sizeof(pgVector2));
-//	pAcc->real = pAcc->imag = 0;
-//	for(i = from; i <= to; ++i)
-//	{
-//		contact = (pgContact*)PyList_GetItem(contactList, i);
-//
-//		c_rotate(&(contact->pos), selfBody->fRotation);
-//		contact->pos = c_sum(contact->pos, selfBody->vecPosition);
-//		c_rotate(&(contact->normal), selfBody->fRotation);
-//
-//		contact->ppAccMoment = PyObject_Malloc(sizeof(pgVector2*));
-//		*(contact->ppAccMoment) = pAcc;
-//
-//		contact->weight = (to - from)+1;
-//	}
-//
-//
-//END:
-//	Py_XDECREF(list[0]);
-//	Py_XDECREF(list[1]);
-//	return 1;
-//}
-
-
-////all the points are in refbox's locate coordinate system
-//static double _SAT_GetContactNormal(pgAABBBox* clipBox, pgVector2* clist, 
-//									int csize, pgVector2* normal)
-//{	
-//	int i;
-//	int id;
-//	double deps[4], min_dep;
-//	pgVector2* p;
-//		
-//	memset(deps, 0, sizeof(deps));
-//	for(i = 0; i < csize; ++i)
-//	{
-//		p = &clist[i];
-//		deps[0] += fabs(p->real - clipBox->left); //left
-//		deps[1] += fabs(p->imag - clipBox->bottom); //bottom
-//		deps[2] += fabs(clipBox->right - p->real); //right
-//		deps[3] += fabs(clipBox->top - p->imag); //top
-//	}
-//	
-//	//find min penetrating face
-//	id = 0;
-//	min_dep = deps[0];
-//	for(i = 1; i < 4; ++i)
-//	{
-//		if(min_dep > deps[i])
-//		{
-//			min_dep = deps[i];
-//			id = i;
-//		}
-//	}
-//	PG_Set_Vector2(*normal, 0, 0);
-//	//generate contactNormal
-//	switch(id)
-//	{
-//	case 0://left
-//		PG_Set_Vector2(*normal, -1, 0);
-//		break;
-//	case 1://bottom
-//		PG_Set_Vector2(*normal, 0, -1);
-//		break;
-//	case 2://right
-//		PG_Set_Vector2(*normal, 1, 0);
-//		break;
-//	case 3://top
-//		PG_Set_Vector2(*normal, 0, 1);
-//		break;
-//	default:
-//		assert(0);
-//		break;
-//	}
-//
-//	return min_dep;
-//}
-//
-//
-//static int _PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody,
-//								  pgVector2* clist, int* csize, pgVector2 *normal, double* depth)
-//{
-//	pgRectShape * self, * incid;
-//	pgVector2 gp[4], ans_p1, ans_p2;
-//	int has_ip[4];
-//	pgAABBBox clipBox;
-//	int i, i1;
-//	int axis;
-//	int valid_p1, valid_p2;
-//	pgVector2 cpoints[2][10], n[2];
-//	int csizes[2];
-//	double dep[2];
-//	int id;
-//	int apart[2];
-//
-//	self = (pgRectShape*)selfBody->shape;
-//	incid = (pgRectShape*)incidBody->shape;
-//
-//	clipBox = PG_GenAABB(self->bottomLeft.real, self->topRight.real,
-//		self->bottomLeft.imag, self->topRight.imag);
-//
-//	for(i = 0; i < 4; ++i)
-//		gp[i] = PG_GetRelativePos(selfBody, incidBody, &(incid->point[i]));
-//	
-//	for(axis = CA_X; axis <= CA_Y; ++axis)
-//	{
-//		memset(has_ip, 0, sizeof(has_ip));
-//		apart[axis] = 1;
-//		csizes[axis] = 0;
-//		for(i = 0; i < 4; ++i)
-//		{
-//			i1 = (i+1)%4;
-//			if(PG_PartlyLB(&clipBox, &gp[i], &gp[i1], axis, 
-//				&ans_p1, &ans_p2, &valid_p1, &valid_p2))
-//			{
-//				apart[axis] = 0;
-//				if(valid_p1)
-//				{
-//					if(c_equal(&ans_p1, &gp[i]))
-//						has_ip[i] = 1;
-//					else
-//						cpoints[axis][csizes[axis]++] = ans_p1;
-//				}
-//				if(valid_p2)
-//				{
-//					if(c_equal(&ans_p2, &gp[i1]))
-//						has_ip[i1] = 1;
-//					else
-//						cpoints[axis][csizes[axis]++] = ans_p2;
-//				}
-//			}
-//		}
-//
-//		for(i = 0; i < 4; ++i)
-//			if(has_ip[i])
-//				cpoints[axis][csizes[axis]++] = gp[i];
-//		dep[axis] = _SAT_GetContactNormal(&clipBox, cpoints[axis], csizes[axis], &n[axis]);
-//	}
-//
-//	if(apart[CA_X] && apart[CA_Y]) return 0;
-//	//assert(csizes[0] > 0 && csizes[1] > 0);
-//	
-//	id = dep[CA_X] < dep[CA_Y] ? CA_X : CA_Y;
-//	if(csizes[id] == 0)
-//		id = (id + 1)%2;
-//	*csize = csizes[id];
-//	for(i = 0; i < csizes[id]; ++i)
-//		clist[i] = cpoints[id][i];
-//	*normal = n[id];
-//	*depth = dep[id];
-//
-//	return 1;
-//}
-//
-//int PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody, 
-//						  PyObject* contactList)
-//{
-//	pgVector2 clist1[10], clist2[10], *clist, n1, n2, *n;
-//	int csize1, csize2, *csize;
-//	double dep1, dep2;
-//	int is_collided;
-//	pgBodyObject *self, *incid;
-//	int i;
-//	pgContact* contact;
-//	pgVector2* pAcc;
-//
-//	is_collided = _PG_RectShapeCollision(selfBody, incidBody, clist1, 
-//				  &csize1, &n1, &dep1);
-//	if(!is_collided) return 0;
-//	is_collided = _PG_RectShapeCollision(incidBody, selfBody, clist2, 
-//				  &csize2, &n2, &dep2);
-//	assert(is_collided);
-//
-//	if(dep1 < dep2)
-//	{
-//		clist = clist1;
-//		csize = &csize1;
-//		n = &n1;
-//		self = selfBody;
-//		incid = incidBody;
-//	}
-//	else
-//	{
-//		clist = clist2;
-//		csize = &csize2;
-//		n = &n2;
-//		self = incidBody;
-//		incid = selfBody;
-//	}
-//	assert(*csize > 0);
-//
-//
-//	pAcc = PyObject_Malloc(sizeof(pgVector2));
-//	pAcc->real = pAcc->imag = 0;
-//	for(i = 0; i < *csize; ++i)
-//	{
-//		contact = (pgContact*)PG_ContactNew(self, incid);
-//		contact->pos = clist[i];
-//		contact->normal = *n;
-//		c_rotate(&(contact->pos), self->fRotation);
-//		contact->pos = c_sum(contact->pos, self->vecPosition);
-//		c_rotate(&(contact->normal), self->fRotation);
-//
-//		contact->ppAccMoment = PyObject_Malloc(sizeof(pgVector2*));
-//		*(contact->ppAccMoment) = pAcc;
-//		contact->weight = *csize;
-//		PyList_Append(contactList, (PyObject*)contact);
-//	}
-//
-//	return 1;
-//}
-
-
 static int _Get_Depth(pgBodyObject* refBody, pgBodyObject* incBody,
 					   int* faceId, double* min_dep, pgVector2* gp_in_ref, 
 					   pgAABBBox* clipBox)
 {
+#define _EPS_DEPTH 1e-8
+
 	int i, apart;
 	pgRectShape *ref, *inc;
 	double deps[4];
 
 	apart = 1;
 	for(i = 0; i < 4; ++i)
-		if(PG_IsIn(&gp_in_ref[i], clipBox))
+		if(PG_IsIn(&gp_in_ref[i], clipBox, _EPS_DEPTH))
 		{
 			apart = 0;
 			deps[CF_LEFT] += fabs(gp_in_ref[i].real - clipBox->left);
 	return 1;
 
 }
-
-
-//PyTypeObject pgRectShapeType =
-//{
-//	PyObject_HEAD_INIT(NULL)
-//	0,
-//	"physics.RectShape",            /* tp_name */
-//	sizeof(pgRectShape),      /* tp_basicsize */
-//	0,                          /* tp_itemsize */
-//	(destructor) 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 */
-//	(initproc)_pgRectShape_init,  /* tp_init */
-//	0,                          /* tp_alloc */
-//	0,							/* 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 */
-//};
-

src/pgWorldObject.c

 #include "pgShapeObject.h"
 #include <structmember.h>
 
-#define MAX_SOLVE_INTERAT 10
+#define MAX_ITERATION 5
 
 extern PyTypeObject pgWorldType;
 
 void _PG_FreeBodySimulation(pgWorldObject* world,double stepTime)
 {
+	Py_ssize_t i;
 	Py_ssize_t size = PyList_Size((PyObject*)(world->bodyList));
-	Py_ssize_t i;
 	for (i = 0; i < size; ++i)
 	{
 		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));		
 
 void _PG_BodyCollisionDetection(pgWorldObject* world, double step)
 {
-	Py_ssize_t i, j, cnt, size, body_size;
+	Py_ssize_t i, j, body_cnt, contact_cnt;
 	pgBodyObject* refBody, *incBody;
 	pgJointObject* contact;
 	
-	size = PyList_Size((PyObject*)(world->bodyList));
+	body_cnt = 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;
+	contact_cnt = PyList_Size((PyObject*)(world->contactList));
+	if(PyList_SetSlice((PyObject*)(world->contactList), 0, contact_cnt, NULL) < 0) return;
 	assert(PyList_Size((PyObject*)(world->contactList))==0);
 	
-	//for all pair of objects, do collision test
-	//clear bias
-	body_size = PyList_Size((PyObject*)(world->bodyList));
-	for(i = 0; i < body_size; ++i)
-	{
-		refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
-		PG_Set_Vector2(refBody->cBiasLV, 0.f, 0.f);
-		refBody->cBiasW = 0.f;
-	}
-	
+	//for all pair of objects, do collision test	
 	//update AABB
-	for(i = 0; i < size; ++i)
+	for(i = 0; i < body_cnt; ++i)
 	{
 		refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
 		refBody->shape->UpdateAABB(refBody);
 	}
 	
 	//collision test
-	for(i = 0; i < size-1; ++i)
+	for(i = 0; i < body_cnt-1; ++i)
 	{
 		refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
-		for(j = i+1; j < size; ++j)
+		for(j = i+1; j < body_cnt; ++j)
 		{
 			incBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), j));
 			if(refBody->bStatic && incBody->bStatic) continue;
 			}
 		}
 	}
-	//collision reaction
-	cnt = PyList_Size((PyObject*)(world->contactList));
-	for(i = 0; i < cnt; ++i)
+
+	contact_cnt = PyList_Size((PyObject*)(world->contactList));
+	for(j = 0; j < MAX_ITERATION; ++j)
 	{
-		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
-		PG_ApplyContact((PyObject*)contact, step);
-	}
-	
-	//update V
-	for(j = 0; j < 1; ++j)
-		for(i = 0; i < cnt; ++i)
+		//clear bias
+		for(i = 0; i < body_cnt; ++i)
+		{
+			refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
+			PG_Set_Vector2(refBody->cBiasLV, 0.f, 0.f);
+			refBody->cBiasW = 0.f;
+		}
+		//clear impulse
+		for(i = 0; i < contact_cnt; ++i)
+		{
+			contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
+			PG_Set_Vector2(**(((pgContact*)contact)->ppAccMoment), 0, 0);
+			PG_Set_Vector2(**(((pgContact*)contact)->ppSplitAccMoment), 0, 0);
+		}
+
+		//collision reaction
+		contact_cnt = PyList_Size((PyObject*)(world->contactList));
+		for(i = 0; i < contact_cnt; ++i)
+		{
+			contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
+			PG_ApplyContact((PyObject*)contact, step);
+		}
+		//update V	
+		for(i = 0; i < contact_cnt; ++i)
 		{
 			contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
 			contact->SolveConstraintVelocity(contact, step);
 		}
+	}
 }
 
 void _PG_JointSolve(pgWorldObject* world,double stepTime)
 void PG_Update(pgWorldObject* world,double stepTime)
 {
 	int i;
+
 	_PG_FreeBodySimulation(world, stepTime);
-
 	_PG_BodyCollisionDetection(world, stepTime);
-	for (i = 0;i < MAX_SOLVE_INTERAT;i++)
-	{
+	for(i = 0; i < MAX_ITERATION; ++i)
 		_PG_JointSolve(world, stepTime);
-	}
-	
 	_PG_BodyPositionUpdate(world, stepTime);
 }