Commits

Anonymous committed 2102fd0

update collision algorithms(better than before)

Comments (0)

Files changed (11)

 void do_render()
 {
 	glColor3f(1.f, 1.f, 1.f);
-	PG_Update(s_world, 0.005);
+	PG_Update(s_world, 0.003);
 	PGT_RenderWorld(s_world);
-	glprintf(0, 0, "Velocity of body1:(%.2f, %.2f)", body->vecLinearVelocity.real, 
-		body->vecLinearVelocity.imag);
+	//glprintf(0, 0, "Velocity of body1:(%.2f, %.2f)", body->vecLinearVelocity.real, 
+	//	body->vecLinearVelocity.imag);
 }
 
 
 	s_world->fStepTime = 0.03;
 
 	body = PG_BodyNew();
-	PG_Set_Vector2(body->vecPosition,0,0);
-	PG_Set_Vector2(body->vecLinearVelocity, 0, -80.f);
-	body->fRotation = M_PI/4;
+	PG_Set_Vector2(body->vecPosition, 0, 0);
+	PG_Set_Vector2(body->vecLinearVelocity, 0, 0.f);
+	body->fRotation = 0;
 	body->fAngleVelocity = 0.f;
+	body->fRestitution = 1.f;
 	PG_Bind_RectShape(body, 20, 20, 0);
 	PG_AddBodyToWorld(s_world, body);
 	
 	body1 = PG_BodyNew();
 	PG_Set_Vector2(body1->vecPosition,0, -100);
 	body1->bStatic = 1;
-	PG_Bind_RectShape(body1, 300, 20, 0);
+	body1->fRestitution = 1.f;//for test
+	body1->fMass = 1e24;
+	PG_Bind_RectShape(body1, 600, 20, 0);
 	PG_AddBodyToWorld(s_world, body1);
 
 }

Test/pgPhysicsRenderer.c

 		gp[i] = PG_GetGlobalPos(body, &(rect->point[i]));
 
 	glColor3f(1.f, 1.f, 0.f);
-	glLineWidth(2.f);
+	glLineWidth(1.f);
 	glBegin(GL_LINE_LOOP);
 	glVertex2d(gp[0].real, gp[0].imag);
 	glVertex2d(gp[1].real, gp[1].imag);
 {
 	pgDistanceJointObject* pj = (pgDistanceJointObject*)joint;
 	glColor3f(1.f, 0.f, 0.f);
-	glLineWidth(2.f);
+	glLineWidth(1.f);
 	glBegin(GL_LINES);
 	if (joint->body1 && (!joint->body2))
 	{
 	};
 } pgAABBBox;
 
-typedef enum _pgBoxDirect
-{
-	BD_NONE = -1,
-	BD_LEFT,
-	BD_BOTTOM,
-	BD_RIGHT,
-	BD_TOP
-}pgBoxDirect;
-
 pgAABBBox PG_GenAABB(double left, double right, double bottom, double top);
 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);
 
 #endif //_PYGAME_MATH_AABBBOX_
 

include/pgCollision.h

 
 	pgVector2 pos;
 	pgVector2 normal;
+	pgVector2 dv;
 	double depth;
 	double weight;
+	double resist;
 	pgVector2** ppAccMoment;
 }pgContact;
 
 	MOVING_TOWARD
 }pgCollisionType;
 
+typedef enum _pgCollisionAxis
+{
+	CA_X = 0,
+	CA_Y = 1
+}pgCollisionAxis;
+
+typedef enum _pgCollisionFace
+{
+	CF_LEFT,
+	CF_BOTTOM,
+	CF_RIGHT,
+	CF_TOP
+}pgCollisionFace;
+
 int PG_LiangBarskey(pgAABBBox* box, pgVector2* p1, pgVector2* p2, 
 					 pgVector2* ans_p1, pgVector2* ans_p2);
 
+int PG_PartlyLB(pgAABBBox* box, pgVector2* p1, pgVector2* p2, 
+				pgCollisionAxis axis, pgVector2* ans_p1, pgVector2* ans_p2,
+				int* valid_p1, int* valid_p2);
+
 pgJointObject* PG_ContactNew(pgBodyObject* refBody, pgBodyObject* incidBody);
 
 void PG_AppendContact(pgBodyObject* refBody, pgBodyObject* incidBody, PyObject* contactList);
 #include <Python.h>
 #include <math.h>
 
-#define ZERO_EPSILON 1e-7
+#define ZERO_EPSILON 1e-6
+#define RELATIVE_ZERO 1e-6
 
 #ifndef M_PI
 #define M_PI 3.1415926535897932384626433832795
 
 int is_zero(double num);
 int is_equal(double a, double b);
+int less_equal(double a, double b);
+int more_equal(double a, double b);
 
 #define MAX(x, y) ( ((x) > (y)) ? (x) : (y) )
 #define MIN(x, y) ( ((x) < (y)) ? (x) : (y) )
 
+
 double c_get_length_square(pgVector2 c);
 double c_get_length(pgVector2 c);
 Py_complex c_mul_complex_with_real(pgVector2 c,double d);
 void	c_normalize(pgVector2* pVec);
 double c_dot(pgVector2 a,pgVector2 b);
 double c_cross(pgVector2 a, pgVector2 b);
+pgVector2 c_fcross(double a, pgVector2 b);
+pgVector2 c_crossf(pgVector2 a, double b);
 void c_rotate(pgVector2* a, double seta);
+int c_equal(pgVector2* a, pgVector2* b);
 
 #endif
 
 	to_y = MIN(boxA->top, boxB->top);
 	return from_x <= to_x && from_y <= to_y;
 }
+
+PG_IsIn(pgVector2* p, pgAABBBox* box)
+{
+	return box->left <= p->real && p->real <= box->right
+		&& box->bottom <= p->imag && p->imag <= box->top;
+}
 
 pgVector2 PG_AngleToLinear(pgVector2* r, double w)
 {
-	pgVector2 v;
-	double r_len, v_len;
+	//pgVector2 v;
+	//double r_len, v_len;
 
-	r_len = c_get_length(*r);
-	if(is_zero(r_len))
-	{
-		v.imag = v.real = 0;
-	}
-	else
-	{
-		r->real /= r_len;
-		r->imag /= r_len;
-		v_len = fabs(r_len*w);
-		r->real *= v_len;
-		r->imag *= v_len;
-		if(w > 0) //counter-clock wise
-		{	
-			v.real = -r->imag;
-			v.imag = r->real;
-		}
-		else //clock wise
-		{
-			v.real = r->imag;
-			v.imag = -r->real;
-		}
-	}
+	//r_len = c_get_length(*r);
+	//if(is_zero(r_len))
+	//{
+	//	v.imag = v.real = 0;
+	//}
+	//else
+	//{
+	//	r->real /= r_len;
+	//	r->imag /= r_len;
+	//	v_len = fabs(r_len*w);
+	//	r->real *= v_len;
+	//	r->imag *= v_len;
+	//	if(w > 0) //counter-clock wise
+	//	{	
+	//		v.real = -r->imag;
+	//		v.imag = r->real;
+	//	}
+	//	else //clock wise
+	//	{
+	//		v.real = r->imag;
+	//		v.imag = -r->real;
+	//	}
+	//}
 
-	return v;
+	//return v;
+
+	pgVector2 ans;
+	ans = c_fcross(w, *r);
+	return ans;
 }
 
 pgVector2 PG_AngleToLinear1(pgBodyObject* body, pgVector2* global_p)
 	return 1;
 }
 
+int PG_PartlyLB(pgAABBBox* box, pgVector2* p1, pgVector2* p2, 
+				pgCollisionAxis axis, pgVector2* ans_p1, pgVector2* ans_p2, 
+				int* valid_p1, int* valid_p2)
+{
+	pgVector2 dp;
+	double u1, u2;
+	
+	u1 = 0.f;
+	u2 = 1.f;
+	dp = c_diff(*p2, *p1);
+
+	switch(axis)
+	{
+	case CA_X:
+		if(!_LiangBarskey_Internal(-dp.imag, p1->imag - box->bottom, &u1, &u2)) return 0;
+		if(!_LiangBarskey_Internal(dp.imag, box->top - p1->imag, &u1, &u2)) return 0;
+		break;
+	case CA_Y:
+		if(!_LiangBarskey_Internal(-dp.real, p1->real - box->left, &u1, &u2)) return 0;
+		if(!_LiangBarskey_Internal(dp.real, box->right - p1->real, &u1, &u2)) return 0;
+		break;
+	default:
+		assert(0);
+		break;
+	}
+
+	if(u1 > u2) return 0;
+
+	if(u1 == 0.f)
+		*ans_p1 = *p1;
+	else
+		*ans_p1 = c_sum(*p1, c_mul_complex_with_real(dp, u1)); //ans_p1 = p1 + u1*dp
+	if(u2 == 1.f)
+		*ans_p2 = *p2;
+	else
+		*ans_p2 = c_sum(*p1, c_mul_complex_with_real(dp, u2)); //ans_p2 = p2 + u2*dp;
+
+	switch(axis)
+	{
+	case CA_X:
+		*valid_p1 = less_equal(box->left, ans_p1->real) && 
+					less_equal(ans_p1->real, box->right);
+		*valid_p2 = less_equal(box->left, ans_p2->real) && 
+					less_equal(ans_p2->real, box->right);
+		break;
+	case CA_Y:
+		*valid_p1 = less_equal(box->bottom, ans_p1->imag) && 
+					less_equal(ans_p1->imag, box->top);
+		*valid_p2 = less_equal(box->bottom, ans_p2->imag) && 
+					less_equal(ans_p2->imag, box->top);
+		break;
+	default:
+		assert(0);
+		break;
+	}
+
+	return *valid_p1 || *valid_p2;
+}
+
+
 void PG_AppendContact(pgBodyObject* refBody, pgBodyObject* incidBody, PyObject* contactList)
 {
 	refBody->shape->Collision(refBody, incidBody, contactList);
 	refBody = contact->joint.body1;
 	incidBody = contact->joint.body2;
 
+	contact->resist = sqrtf(refBody->fRestitution*incidBody->fRestitution);
+
 	//calculate the normal impulse
 	//k
 	refR = c_diff(contact->pos, refBody->vecPosition);
 	incidR = c_diff(contact->pos, incidBody->vecPosition);
 	
-	tmp1 = refR.real*contact->normal.imag - refR.imag*contact->normal.real;
-	tmp2 = incidR.real*contact->normal.imag - incidR.imag*contact->normal.real;
+	//tmp1 = refR.real*contact->normal.imag - refR.imag*contact->normal.real;
+	//tmp2 = incidR.real*contact->normal.imag - incidR.imag*contact->normal.real;
 
-	k = 1/refBody->fMass + 1/incidBody->fMass + tmp1*tmp1/refBody->shape->rInertia
-		+ tmp2*tmp2/incidBody->shape->rInertia;
+	//k = 1/refBody->fMass + 1/incidBody->fMass + tmp1*tmp1/refBody->shape->rInertia
+	//	+ tmp2*tmp2/incidBody->shape->rInertia;
+
+	tmp1 = c_dot(c_fcross(c_cross(refR, contact->normal), refR), contact->normal)
+		 /refBody->shape->rInertia;
+	tmp2 = c_dot(c_fcross(c_cross(incidR, contact->normal), incidR), contact->normal)
+		/incidBody->shape->rInertia;
+
+	k = 1/refBody->fMass + 1/incidBody->fMass + tmp1 + tmp2;
 	
 	//dV = v2 + w2xr2 - (v1 + w1xr1)
 	incidV = c_sum(incidBody->vecLinearVelocity, PG_AngleToLinear1(incidBody, &(contact->pos)));
 	refV = c_sum(refBody->vecLinearVelocity, PG_AngleToLinear1(refBody, &(contact->pos)));
+	contact->dv = c_diff(incidV, refV);
 	neg_dV = c_diff(refV, incidV);
 	
 	moment_len = c_dot(neg_dV, contact->normal)/k;
-	moment_len *= refBody->fRestitution;
+	moment_len *= contact->resist;
 	if(moment_len < 0)
 		moment_len = 0;
 	//finally we get the momentum(oh...)
 	moment = c_mul_complex_with_real(contact->normal, moment_len);
 	p = *(contact->ppAccMoment);
 	//TODO: test weight, temp codes
-	p->real += moment.real / contact->weight;
-	p->imag += moment.imag / contact->weight; 
+	p->real += moment.real/contact->weight;
+	p->imag += moment.imag/contact->weight; 
 }
 
 void PG_UpdateV(pgJointObject* joint, double step)
 	refR = c_diff(contact->pos, refBody->vecPosition);
 	incidR = c_diff(contact->pos, incidBody->vecPosition);
 
+	if(contact->dv.imag > 0 && contact->dv.real > 0) return;
+
 	if(!refBody->bStatic)
 	{
 		refBody->vecLinearVelocity = c_diff(refBody->vecLinearVelocity, 
 #include <string.h>
 #include <math.h>
 #include <assert.h>
+#include <float.h>
 
 //functions of pgShapeObject
 
 //get the contact normal.
 //note: this method is not available in CCD(continue collision detection)
 //since the velocity is not small.
-static double _SAT_GetContactNormal(pgAABBBox* clipBox, PyObject* contactList,
-								  int from, int to)
+//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)
 {
+	int i, apart;
+	pgRectShape *ref, *inc;
+	double deps[4];
+
+	ref = (pgRectShape*)refBody->shape;
+	inc = (pgRectShape*)incBody->shape;
+	memset(gp_in_ref, 0, sizeof(gp_in_ref));
+	memset(deps, 0, sizeof(deps));
+	for(i = 0; i < 4; ++i)
+		gp_in_ref[i] = PG_GetRelativePos(refBody, incBody, &(inc->point[i]));
+
+	*clipBox = PG_GenAABB(ref->bottomLeft.real, ref->topRight.real,
+		ref->bottomLeft.imag, ref->topRight.imag);
+
+	apart = 1;
+	for(i = 0; i < 4; ++i)
+		if(PG_IsIn(&gp_in_ref[i], clipBox))
+		{
+			apart = 0;
+			deps[CF_LEFT] += fabs(gp_in_ref[i].real - clipBox->left);
+			deps[CF_RIGHT] += fabs(clipBox->right - gp_in_ref[i].real);
+			deps[CF_BOTTOM] += fabs(gp_in_ref[i].imag - clipBox->bottom);
+			deps[CF_TOP] += fabs(clipBox->top - gp_in_ref[i].imag);
+		}
+
+	if(apart) return 0;
+
+	*min_dep = deps[0];
+	*faceId = 0;
+
+	for(i = 1; i < 4; ++i)
+		if(deps[i] < *min_dep)
+		{
+			*min_dep = deps[i];
+			*faceId = i;
+		}
+
+	return 1;
+}
+
+static int _SAT_Select(pgBodyObject* body1, pgBodyObject* body2,
+					   pgBodyObject** refBody, pgBodyObject** incBody,
+					   int* face_id, pgVector2* gp_in_ref, pgAABBBox* clipBox)
+{
+	double min_dep[2];
+	int id[2];
+	pgVector2 gp[2][4];
+	pgAABBBox cb[2];
+	int is_in[2];
 	int i;
-	int id;
-	double deps[4], min_dep;
-	pgContact* p;
-	pgVector2 normal;
-		
-	memset(deps, 0, sizeof(deps));
-	for(i = from; i <= to; ++i)
+	
+	min_dep[0] = min_dep[1] = DBL_MAX;
+	is_in[0] = _Get_Depth(body1, body2, &id[0], &min_dep[0], gp[0], &cb[0]);
+	is_in[1] = _Get_Depth(body2, body1, &id[1], &min_dep[1], gp[1], &cb[1]);
+
+	if(!is_in[0] && !is_in[1]) return 0;
+
+	if(min_dep[0] < min_dep[1])
 	{
-		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
+		*refBody = body1;
+		*incBody = body2;
+		*face_id = id[0];
+		for(i = 0; i < 4; ++i)
+			gp_in_ref[i] = gp[0][i];
+		*clipBox = cb[0];
 	}
-	
-	//find min penetrating face
-	id = 0;
-	min_dep = deps[0];
-	for(i = 1; i < 4; ++i)
+	else
 	{
-		if(min_dep > deps[i])
+		*refBody = body2;
+		*incBody = body1;
+		*face_id = id[1];
+		for(i = 0; i < 4; ++i)
+			gp_in_ref[i] = gp[1][i];
+		*clipBox = cb[1];
+	}
+
+	return 1;
+}
+
+int _Build_Contacts(pgVector2* gp, pgAABBBox* clipBox, int axis,
+					 pgVector2 contacts[], int* size)
+{
+	int i, i1;
+	int apart = 1;
+	int has_ip[4];
+	pgVector2 pf, pt;
+	int valid_pf, valid_pt;
+
+	*size = 0;
+	memset(has_ip, 0, sizeof(has_ip));
+	for(i = 0; i < 4; ++i)
+	{
+		i1 = (i+1)%4;
+		if(PG_PartlyLB(clipBox, &gp[i], &gp[i1], axis, 
+			&pf, &pt, &valid_pf, &valid_pt))
 		{
-			min_dep = deps[i];
-			id = i;
+			apart = 0;
+			if(valid_pf)
+			{
+				if(c_equal(&pf, &gp[i]))
+					has_ip[i] = 1;
+				else
+					contacts[(*size)++] = pf;
+			}
+			if(valid_pt)
+			{
+				if(c_equal(&pt, &gp[i1]))
+					has_ip[i1] = 1;
+				else
+					contacts[(*size)++] = pt;
+			}
 		}
 	}
-	PG_Set_Vector2(normal, 0, 0);
-	//generate contactNormal
-	switch(id)
+	for(i = 0; i < 4; ++i)
+		if(has_ip[i])
+			contacts[(*size)++] = gp[i];
+
+	return !apart;
+}
+
+int PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody, 
+						  PyObject* contactList)
+{
+#define MAX_CONTACTS 10
+
+	pgBodyObject* ref = NULL, *inc = NULL;
+	int face_id;
+	pgVector2 gp[4];
+	pgVector2 contacts[MAX_CONTACTS];
+	int csize;
+	pgVector2 normal;
+	pgAABBBox clipBox;
+	int overlap;
+	pgVector2* pAcc;
+	pgContact* contact;
+	int i;
+
+
+	overlap = _SAT_Select(selfBody, incidBody,
+						  &ref, &inc,
+						  &face_id, gp, &clipBox);
+
+	if(!overlap) return 0;
+
+	switch(face_id)
 	{
-	case 0://left
+	case CF_LEFT:
 		PG_Set_Vector2(normal, -1, 0);
+		assert(_Build_Contacts(gp, &clipBox, CA_X, contacts, &csize)); 
 		break;
-	case 1://bottom
+	case CF_BOTTOM:
 		PG_Set_Vector2(normal, 0, -1);
+		assert(_Build_Contacts(gp, &clipBox, CA_Y, contacts, &csize));
 		break;
-	case 2://right
+	case CF_RIGHT:
 		PG_Set_Vector2(normal, 1, 0);
+		assert(_Build_Contacts(gp, &clipBox, CA_X, contacts, &csize));
 		break;
-	case 3://top
+	case CF_TOP:
 		PG_Set_Vector2(normal, 0, 1);
+		assert(_Build_Contacts(gp, &clipBox, CA_Y, contacts, &csize));
+		break;
+	default:
+		assert(0);
 		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_RectShapeCollision(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);
-	//here has some bugs
-	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)
+	for(i = 0; i < csize; ++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 = (pgContact*)PG_ContactNew(ref, inc);
+		contact->pos = contacts[i];
+		contact->normal = normal;
+		c_rotate(&(contact->pos), ref->fRotation);
+		contact->pos = c_sum(contact->pos, ref->vecPosition);
+		c_rotate(&(contact->normal), ref->fRotation);
 
 		contact->ppAccMoment = PyObject_Malloc(sizeof(pgVector2*));
 		*(contact->ppAccMoment) = pAcc;
-
-		contact->weight = (to - from)+1;
+		contact->weight = csize;
+		PyList_Append(contactList, (PyObject*)contact);
 	}
 
+	return 1;
 
-END:
-	Py_XDECREF(list[0]);
-	Py_XDECREF(list[1]);
-	return 1;
 }
 
 int is_zero(double num)
 {
-	return fabs(num) < ZERO_EPSILON;
+	return fabs(num) <= ZERO_EPSILON;
 }
 
 int is_equal(double a, double b)
 {
-	return is_zero(a - b);
+	double rErr;
+	if(is_zero(a - b)) return 1;
+
+	if(fabs(b) > fabs(a))
+		rErr = fabs((a - b) / b);
+	else
+		rErr = fabs((a - b) / a);
+
+	return rErr <= RELATIVE_ZERO;
+}
+
+int less_equal(double a, double b)
+{
+	return a < b || is_equal(a, b);
+}
+
+int more_equal(double a, double b)
+{
+	return a > b || is_equal(a, b);
 }
 
 double c_get_length_square(Py_complex c)
 	return a.real*b.imag - a.imag*b.real;
 }
 
+pgVector2 c_fcross(double a, pgVector2 b)
+{
+	pgVector2 ans;
+	ans.real = -a*b.imag;
+	ans.imag = a*b.real;
+	return ans;
+}
+
+pgVector2 c_crossf(pgVector2 a, double b)
+{
+	pgVector2 ans;
+	ans.real = a.imag*b;
+	ans.imag = -a.real*b;
+	return ans;
+}
+
 void c_rotate(pgVector2* a, double seta)
 {
 	double x = a->real;
 	a->imag = x*sin(seta) + y*cos(seta);
 }
 
+int c_equal(pgVector2* a, pgVector2* b)
+{
+	return is_equal(a->real, b->real) && is_equal(a->imag, b->imag);
+}
+
+
 		for(j = i+1; j < size; ++j)
 		{
 			incBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), j));
+			if(refBody->bStatic && incBody->bStatic) continue;
 			if(PG_IsOverlap(&(refBody->shape->box), &(incBody->shape->box)))
 			{
 				PG_AppendContact(refBody, incBody, (PyObject*)world->contactList);