Commits

Anonymous committed 3d6eab5

Limited body restitution to a range of [0,1].
Limited body friction to [0,inf).
Limited world step time to [0, inf).
Added python inheritance support to shapes for AABB collisions.

  • Participants
  • Parent commits 390bd04
  • Branches physics

Comments (0)

Files changed (11)

     body1.shape = physics.RectShape(80,33,0)
     body1.position = 100, 100
     body1.velocity = 2,0
-    body1.restitution = 3.0
+    body1.restitution = 1.0
     w.add_body(body1)
     body2 = physics.Body()
     body2.shape = physics.RectShape (20,20,0)
     body2.position = 200, 100
     body2.velocity = -2, 0
-    body1.restitution = 3.0
+    body1.restitution = 1.0
     w.add_body(body2)
     return w
 
     body = physics.Body()
     body.shape = physics.RectShape (20, 20, 0)
     body.position = 200, 100
-    body.restitution = 3.0
+    body.restitution = 1.0
     body.static = True
     w.add_body(body)
     body1 = physics.Body()
     body1.shape = physics.RectShape (20,20,0)
     body1.position = 200, 200
-    body1.restitution = 300.0
+    body1.restitution = 1.0
     w.add_body(body1)
     body2 = physics.Body()
     body2.shape = physics.RectShape (20,20,0)
     body2.position = 300, 200
-    body1.restitution = 3.0
+    body1.restitution = 1.0
     w.add_body(body2)
     
     joint1 = physics.DistanceJoint(body1,body,1)
-    #joint1.distance = 10
     joint1.anchor1 = 0, 0
     joint1.anchor2 = 0, 0
     w.add_joint(joint1)
     joint2 = physics.DistanceJoint(body1,body2,1)
-    #joint2.distance = 10
     joint2.anchor1 = 0, 0
     joint2.anchor2 = 0, 0
     w.add_joint(joint2)
 //test collision
 void TestBasic2Init()
 {
-	int i = 0;
-	s_world = PG_WorldNew();
-	s_world->fStepTime = 0.03;
-	PG_Set_Vector2(s_world->vecGravity, 0, -20.f);
-
-	for(i = 0; i < 100; ++i)
-	{
-		body = PG_BodyNew();
-		PG_Set_Vector2(body->vecPosition, 0, 4200 - 40*i);
-		
-		body->fRotation = i;
-		body->fAngleVelocity = 0.f;
-		body->fRestitution = 0.0f;
-		body->fMass = 30;
-		PG_Bind_RectShape(body, 30, 30, 0);
-		PG_AddBodyToWorld(s_world, body);
-	}
-
-	body1 = PG_BodyNew();
-	PG_Set_Vector2(body1->vecPosition,0, -100);
-	body1->bStatic = 1;
-	body1->fRestitution = 1.f;//for test
-	body1->fMass = 1e32;
-	body1->fRotation = 0.f;
-	PG_Bind_RectShape(body1, 1000, 20, 0);
-	PG_AddBodyToWorld(s_world, body1);
-
+    PyObject *body, *body1;
+    int i = 0;
+    s_world = PyWorld_New();
+    ((PyWorldObject*)s_world)->fStepTime = 0.03;
+    PyVector2_Set(((PyWorldObject*)s_world)->vecGravity, 0, -1.f);
+    
+    for(i = 0; i < 100; ++i)
+    {
+        body = PyBody_New();
+        PyVector2_Set(((PyBodyObject*)body)->vecPosition, 0, 4200 - 40*i);
+	
+        ((PyBodyObject*)body)->fRotation = i;
+        ((PyBodyObject*)body)->fAngleVelocity = 0.f;
+        ((PyBodyObject*)body)->fRestitution = 0.0f;
+        ((PyBodyObject*)body)->fMass = 30;
+        PyBody_SetShape (body, PyRectShape_New (30, 30, 0));
+        PyWorld_AddBody(s_world, body);
+    }
+    
+    body1 = PyBody_New();
+    PyVector2_Set(((PyBodyObject*)body1)->vecPosition,0, -100);
+    ((PyBodyObject*)body1)->bStatic = 1;
+    ((PyBodyObject*)body1)->fRestitution = 1.0f;//for test
+    ((PyBodyObject*)body1)->fMass = 1e32;
+    ((PyBodyObject*)body1)->fRotation = 0.f;
+    PyBody_SetShape (body1, PyRectShape_New (1000, 20, 0));
+    PyWorld_AddBody(s_world, body1);
 }
 
 void TestBasic22Init()
 {
-	int i = 0;
-	s_world = PG_WorldNew();
-	PG_Set_Vector2(s_world->vecGravity, 0.f, -0.f);
-	
-	body = PG_BodyNew();
-	PG_Set_Vector2(body->vecPosition, -100.f, 200.f);
-	PG_Set_Vector2(body->vecLinearVelocity, 10.f, 0.f);
-	body->fMass = 1e10;
-	PG_Bind_RectShape(body, 30, 30, 0);
-	PG_AddBodyToWorld(s_world, body);
-
-	body1 = PG_BodyNew();
-	PG_Set_Vector2(body1->vecPosition, 100.f, 200.f);
-	PG_Set_Vector2(body1->vecLinearVelocity, -10.f, 0.f);
-	body1->fMass = 1e10;
-	PG_Bind_RectShape(body1, 30, 30, 0);
-	PG_AddBodyToWorld(s_world, body1);
+    PyObject *body, *body1;
+    int i = 0;
+    s_world = PyWorld_New();
+    PyVector2_Set(((PyWorldObject*)s_world)->vecGravity, 0.f, -0.f);
+    
+    body = PyBody_New();
+    PyVector2_Set(((PyBodyObject*)body)->vecPosition, -100.f, 200.f);
+    PyVector2_Set(((PyBodyObject*)body)->vecLinearVelocity, 10.f, 0.f);
+    ((PyBodyObject*)body)->fMass = 1e10;
+    PyBody_SetShape (body, PyRectShape_New(30, 30, 0));
+    PyWorld_AddBody(s_world, body);
+    
+    body1 = PyBody_New();
+    PyVector2_Set(((PyBodyObject*)body1)->vecPosition, 100.f, 200.f);
+    PyVector2_Set(((PyBodyObject*)body1)->vecLinearVelocity, -10.f, 0.f);
+    ((PyBodyObject*)body1)->fMass = 1e10;
+    PyBody_SetShape (body1, PyRectShape_New(30, 30, 0));
+    PyWorld_AddBody(s_world, body1);
 }
 
 void TestBasic3Init()
     ((PyWorldObject*)s_world)->fStepTime = 0.03;
     PyVector2_Set(((PyWorldObject*)s_world)->vecGravity,0,10);
 
-	for (i = 0;i < BODY_NUM;i++)
-	{
+    for (i = 0;i < BODY_NUM;i++)
+    {
         body[i] = PyBody_New();
         if (i != BODY_NUM - 1)
         {
         }
         //PyVector2_Set(((PyBodyObject*)body[i])->vecLinearVelocity,50,0)
         PyWorld_AddBody(s_world,body[i]);
-	}
+    }
     PyVector2_Set(((PyBodyObject*)body[0])->vecPosition,200,0);
-	PyVector2_Set(((PyBodyObject*)body[1])->vecPosition,200,100);
-	PyVector2_Set(((PyBodyObject*)body[2])->vecPosition,300,200);
-	PyVector2_Set(((PyBodyObject*)body[3])->vecPosition,300,200);
-	((PyBodyObject*)body[0])->bStatic = 1;
+    PyVector2_Set(((PyBodyObject*)body[1])->vecPosition,200,100);
+    PyVector2_Set(((PyBodyObject*)body[2])->vecPosition,300,200);
+    PyVector2_Set(((PyBodyObject*)body[3])->vecPosition,300,200);
+    ((PyBodyObject*)body[0])->bStatic = 1;
     ((PyBodyObject*)body[3])->bStatic = 1;
 
     //PyVector2_Set(((PyBodyObject*)body[BODY_NUM])->vecLinearVelocity,20,60);
     ((PyWorldObject*)s_world)->fStepTime = 0.03;
     PyVector2_Set(((PyWorldObject*)s_world)->vecGravity,0,0);
 
-	body[0] = PyBody_New();
+    body[0] = PyBody_New();
     PyBody_SetShape (body[0], PyRectShape_New (20, 20, 0));
-    PyVector2_Set(((PyBodyObject*)body[0])->vecPosition,-50,0)
-	PyWorld_AddBody(s_world,body[0]);
+    PyVector2_Set(((PyBodyObject*)body[0])->vecPosition,-50,0);
+    PyWorld_AddBody(s_world,body[0]);
 
     body[1] = PyBody_New();
     PyBody_SetShape (body[1], PyRectShape_New (20, 20, 0));
     PyVector2_Set(((PyBodyObject*)body[1])->vecPosition,50,0);
-	PyWorld_AddBody(s_world,body[1]);
+    PyWorld_AddBody(s_world,body[1]);
 
     PyVector2_Set(((PyBodyObject*)body[0])->vecLinearVelocity,0,0);
-	PyVector2_Set(((PyBodyObject*)body[1])->vecLinearVelocity,0,0);
+    PyVector2_Set(((PyBodyObject*)body[1])->vecLinearVelocity,0,0);
 
-	joint = PyDistanceJoint_New(body[0],body[1],0);
+    joint = PyDistanceJoint_New(body[0],body[1],0);
     PyDistanceJoint_SetAnchors (joint,a1, a2);
     PyWorld_AddJoint(s_world,joint);
 }
     ((PyWorldObject*)s_world)->fStepTime = 0.03;
     PyVector2_Set(((PyWorldObject*)s_world)->vecGravity,0,-.1);
 
-	body[0] = PyBody_New();
+    body[0] = PyBody_New();
     //((PyBodyObject*)body[0])->bStatic = 1;
     PyBody_SetShape (body[0], PyRectShape_New (20, 20, 0));
-    PyVector2_Set(((PyBodyObject*)body[0])->vecPosition,10,200)
-	PyWorld_AddBody(s_world,body[0]);
+    PyVector2_Set(((PyBodyObject*)body[0])->vecPosition,10,200);
+    PyWorld_AddBody(s_world,body[0]);
 
     body[1] = PyBody_New();
     ((PyBodyObject*)body[1])->bStatic = 1;
     PyBody_SetShape (body[1], PyRectShape_New (20, 20, 0));
     PyVector2_Set(((PyBodyObject*)body[1])->vecPosition,0,100);
-	PyWorld_AddBody(s_world,body[1]);
+    PyWorld_AddBody(s_world,body[1]);
 
     body[2] = PyBody_New();
     PyBody_SetShape (body[2], PyRectShape_New (20, 20, 0));
     PyVector2_Set(((PyBodyObject*)body[2])->vecPosition,0,0);
-	PyWorld_AddBody(s_world,body[2]);
+    PyWorld_AddBody(s_world,body[2]);
 
     PyVector2_Set(((PyBodyObject*)body[0])->vecLinearVelocity,0,0);
-	PyVector2_Set(((PyBodyObject*)body[1])->vecLinearVelocity,0,0);
+    PyVector2_Set(((PyBodyObject*)body[1])->vecLinearVelocity,0,0);
     PyVector2_Set(((PyBodyObject*)body[2])->vecLinearVelocity,0,0);
 
-	joint = PyDistanceJoint_New(body[0],body[1],0);
+    joint = PyDistanceJoint_New(body[0],body[1],0);
     PyDistanceJoint_SetAnchors (joint,a1, a2);
     PyWorld_AddJoint(s_world,joint);
     
-	joint = PyDistanceJoint_New(body[1],body[2],0);
+    joint = PyDistanceJoint_New(body[1],body[2],0);
     PyDistanceJoint_SetAnchors (joint,a1, a2);
     PyWorld_AddJoint(s_world,joint);
 }
 
 void InitWorld()
 {
-	TestBasic2Init();
+    TestBasic2Init();
 }
 
 int main (int argc, char** argv)
     glutInitWindowSize(WIDTH, HEIGHT);
     glutCreateWindow("test physics");
     
-	Py_Initialize();
+    Py_Initialize();
     if (import_physics () == -1)
-	{
-		// Needed for the glut threading.
-		Py_Finalize();
+    {
+        // Needed for the glut threading.
+        Py_Finalize();
         return 0;
-	}
-	InitWorld();
+    }
+    InitWorld();
     InitGL();
     glutDisplayFunc(display);
     glutKeyboardFunc(keyboard);

include/pgCollision.h

  *
  * @param
  * @param
- * @param
- */
-void Collision_DetectCollision(PyBodyObject* refBody, PyBodyObject* incidBody,
-    PyObject* contactList);
-
-/**
- * TODO
- *
- * @param
- * @param
  */
 void Collision_ApplyContact(PyObject* contactObject, double step);
 

include/pgHelpFunctions.h

 DoubleFromObj (PyObject* obj, double* val);
 
 /**
+ * Tries to retrieve the int value from a python object.
+ * Taken from the pygame codebase.
+ *
+ * @param obj The PyObject to get the int from.
+ * @param val Pointer to the int to store the value in.
+ * @return 0 on failure, 1 on success
+ */
+int
+IntFromObj (PyObject* obj, int* val);
+
+/**
  * Returns a two-value tuple containing floats representing the passed
  * PyVector2.
  *

include/pgShapeObject.h

 #ifndef _PHYSICS_SHAPE_H_
 #define _PHYSICS_SHAPE_H_
 
+#include "pgphysics.h"
+
+int PyShapeObject_UpdateAABB (PyShapeObject *shape, PyBodyObject *refbody);
+
+int PyShapeObject_Collision (PyShapeObject *shape, PyBodyObject *refbody,
+    PyBodyObject *incbody, PyObject *contactlist);
+
 /**
  * Python C API export hook
  *

src/pgBodyObject.c

     body->fTorque = 0.0;
     body->shape = NULL;
     body->bStatic = 0;
-	body->fLinearVelDamping = 0.0;
-	body->fAngleVelDamping = 0.06;
+    body->fLinearVelDamping = 0.0;
+    body->fAngleVelDamping = 0.06;
     PyVector2_Set(body->vecForce,0.0,0.0);
     PyVector2_Set(body->vecImpulse,0.0,0.0);
     PyVector2_Set(body->vecLinearVelocity,0.0,0.0);
             Py_DECREF (tmp);
             if (PyErr_Occurred ())
                 return -1;
+            if (rest < 0 || rest > 1)
+            {
+                PyErr_SetString(PyExc_ValueError,
+                    "restitution must be in the range [0,1]");
+                return -1;
+            }
             body->fRestitution = rest;
             return 0;
         }
             Py_DECREF (tmp);
             if (PyErr_Occurred ())
                 return -1;
+            if (friction < 0)
+            {
+                PyErr_SetString(PyExc_ValueError,
+                    "friction must not be negative");
+                return -1;
+            }
             body->fFriction = friction;
             return 0;
         }
  */
 static PyObject* _Body_getBStatic (PyBodyObject* body,void* closure)
 {
-    return PyInt_FromLong (body->bStatic);
+    if (body->bStatic)
+        Py_RETURN_TRUE;
+    Py_RETURN_FALSE;
 }
 
 /**
     double dt)
 {
     PyVector2 totalF;
-	double k1,k2;
+    double k1,k2;
+    
     if (body->bStatic)
         return;
 
         PyVector2_MultiplyWithReal(gravity, body->fMass));
     body->vecLinearVelocity = c_sum(body->vecLinearVelocity, 
         PyVector2_MultiplyWithReal(totalF, dt/body->fMass));
-	k1 = PG_Clamp(1-dt*body->fLinearVelDamping,0,1);
-	k2 = PG_Clamp(1-dt*body->fAngleVelocity,0,1);
-	body->vecLinearVelocity = PyVector2_MultiplyWithReal(body->vecLinearVelocity,k1);
-	body->fAngleVelocity *= k2;
+    k1 = PG_Clamp(1-dt*body->fLinearVelDamping,0.,1.);
+    k2 = PG_Clamp(1-dt*body->fAngleVelocity,0.,1.);
+    body->vecLinearVelocity = PyVector2_MultiplyWithReal(body->vecLinearVelocity,k1);
+    body->fAngleVelocity *= k2;
 }
 
 void PyBodyObject_FreeUpdatePos(PyBodyObject* body,double dt)

src/pgCollision.c

     return *valid_p1 || *valid_p2;
 }
 
-
-void Collision_DetectCollision(PyBodyObject* refBody, PyBodyObject* incidBody,
-    PyObject* contactList)
-{
-    ((PyShapeObject*)refBody->shape)->Collision(refBody, incidBody, contactList);
-}
-
 void Collision_ApplyContact(PyObject* contactObject, double step)
 {
 #define MAX_C_DEP 0.01

src/pgHelpFunctions.c

     return 0;
 }
 
+int
+IntFromObj (PyObject* obj, int* val)
+{
+    PyObject* intobj;
+    long tmp;
+    
+    if (PyNumber_Check (obj))
+    {
+        if (!(intobj = PyNumber_Int (obj)))
+            return 0;
+        tmp = PyInt_AsLong (intobj);
+        Py_DECREF (intobj);
+        if (tmp == -1 && PyErr_Occurred ())
+            return 0;
+        *val = tmp;
+        return 1;
+    }
+    return 0;
+}
+
 PyObject* FromPhysicsVector2ToPoint(PyVector2 v2)
 {
 	PyObject* tuple = PyTuple_New(2);

src/pgShapeObject.c

 #include "pgCollision.h"
 #include "pgBodyObject.h"
 #include "pgShapeObject.h"
+#include "pgHelpFunctions.h"
 
 static PyObject* _ShapeNew(PyTypeObject *type, PyObject *args, PyObject *kwds);
 static void _ShapeObjectDestroy(PyShapeObject* shape);
+static PyObject* _Shape_collision(PyShapeObject* shape, PyObject *args);
+static PyObject* _Shape_updateAABB(PyShapeObject* shape, PyObject *args);
 
 static void _RectShapeUpdateAABB(PyBodyObject* body);
 static int _RectShape_init(PyRectShapeObject* shape,PyObject *args, PyObject *kwds);
 
 typedef struct _Candidate_
 {
-	PyVector2 normal;
-	PyVector2 contacts[MAX_CONTACTS];
-	double kFactors[MAX_CONTACTS];
-	int contact_size;
-	double min_depth;
+    PyVector2 normal;
+    PyVector2 contacts[MAX_CONTACTS];
+    double kFactors[MAX_CONTACTS];
+    int contact_size;
+    double min_depth;
 }_Candidate;
 
 
 static int _ClipTest(AABBBox* box, PyVector2* points, _Candidate* candi)
 {
-	int  i, i1;
-	int apart;
-	PyVector2 pf, pt;
-	int has_ip[4];
+    int  i, i1;
+    int apart;
+    PyVector2 pf, pt;
+    int has_ip[4];
+    
+    memset(has_ip, 0, sizeof(has_ip));
+    apart = 1;
+    candi->contact_size = 0;
+    for(i = 0; i < 4; ++i)
+    {
+        i1 = (i + 1)%4;
+        if(Collision_LiangBarskey(box, &points[i], &points[i1], &pf, &pt))
+        {
+            apart = 0;
+            if(PyVector2_Equal(&pf, &points[i]))
+                has_ip[i] = 1;
+            else
+                candi->contacts[candi->contact_size++] = pf;
+            
+            if(PyVector2_Equal(&pt, &points[i1]))
+                has_ip[i1] = 1;
+            else
+                candi->contacts[candi->contact_size++] = pt;
+        }
+    }
 
-	memset(has_ip, 0, sizeof(has_ip));
-	apart = 1;
-	candi->contact_size = 0;
-	for(i = 0; i < 4; ++i)
-	{
-		i1 = (i + 1)%4;
-		if(Collision_LiangBarskey(box, &points[i], &points[i1], &pf, &pt))
-		{
-			apart = 0;
-			if(PyVector2_Equal(&pf, &points[i]))
-				has_ip[i] = 1;
-			else
-				candi->contacts[candi->contact_size++] = pf;
+    if(apart) return 0;
 
-			if(PyVector2_Equal(&pt, &points[i1]))
-				has_ip[i1] = 1;
-			else
-				candi->contacts[candi->contact_size++] = pt;
-		}
-	}
-
-	if(apart) return 0;
-
-	for(i = 0; i < 4; ++i)
-		if(has_ip[i])
-			candi->contacts[candi->contact_size++] = points[i];
-
-	return 1;
-
+    for(i = 0; i < 4; ++i)
+        if(has_ip[i])
+            candi->contacts[candi->contact_size++] = points[i];
+    
+    return 1;
 }
 
-static void _SATFindCollisionProperty(PyBodyObject* selfBody, PyBodyObject* incBody, 
-							 AABBBox* selfBox, AABBBox* incBox, _Candidate *candi,
-							 PyBodyObject** ans_ref, PyBodyObject** ans_inc)
+static void _SATFindCollisionProperty(PyBodyObject* selfBody,
+    PyBodyObject* incBody, AABBBox* selfBox, AABBBox* incBox, _Candidate *candi,
+    PyBodyObject** ans_ref, PyBodyObject** ans_inc)
 {
-	int i, k;
-	double deps[4];
-	double min_dep[2];
-	int face_id[2];
-	PyVector2 conts[2][MAX_CONTACTS];
-	AABBBox* box[2];
-	PyBodyObject* self[2], * inc[2];
-	PyVector2 refR, incidR;
-	int size;
-	double tmp1, tmp2;
-		
-	for(i = 0; i < candi->contact_size; ++i)
-	{
-		conts[0][i] = candi->contacts[i];
-		conts[1][i] = PyBodyObject_GetRelativePos(incBody, selfBody, &conts[0][i]);
-	}
-	box[0] = selfBox;
-	box[1] = incBox;
-	self[0] = inc[1] = selfBody;
-	inc[0] = self[1] = incBody;
-
-	for(k = 0; k <= 1; ++k)
-	{
-		memset(deps, 0, sizeof(deps));
-		for(i = 0; i < candi->contact_size; ++i)
-		{
-			deps[CF_LEFT] += fabs(conts[k][i].real - box[k]->left);
-			deps[CF_RIGHT] += fabs(box[k]->right - conts[k][i].real);
-			deps[CF_BOTTOM] += fabs(conts[k][i].imag - box[k]->bottom);
-			deps[CF_TOP] += fabs(box[k]->top - conts[k][i].imag);
-		}
-
-		min_dep[k] = DBL_MAX;
-		for(i = CF_LEFT; i <= CF_TOP; ++i)
-			if(min_dep[k] > deps[i])
-			{
-				face_id[k] = i;
-				min_dep[k] = deps[i];
-			}
-	}
-
-	//now select min depth one
-	k = min_dep[0] < min_dep[1] ? 0 : 1;
-
-	candi->min_depth = min_dep[k];
-	size = candi->contact_size;
-	candi->contact_size = 0;
-	switch(face_id[k])
-	{
-	case CF_LEFT:
-		PyVector2_Set(candi->normal, -1, 0);
-		for(i = 0; i < size; ++i)
-			if(!PyMath_IsNearEqual(conts[k][i].real, box[k]->left))
-				candi->contacts[candi->contact_size++] = conts[k][i];
-		break;
-	case CF_RIGHT:
-		PyVector2_Set(candi->normal, 1, 0);
-		for(i = 0; i < size; ++i)
-			if(!PyMath_IsNearEqual(conts[k][i].real, box[k]->right))
-				candi->contacts[candi->contact_size++] = conts[k][i];
-		break;
-	case CF_BOTTOM:
-		PyVector2_Set(candi->normal, 0, -1);
-		for(i = 0; i < size; ++i)
-			if(!PyMath_IsNearEqual(conts[k][i].imag, box[k]->bottom))
-				candi->contacts[candi->contact_size++] = conts[k][i];
-		break;
-	case CF_TOP:
-		PyVector2_Set(candi->normal, 0, 1);
-		for(i = 0; i < size; ++i)
-			if(!PyMath_IsNearEqual(conts[k][i].imag, box[k]->top))
-				candi->contacts[candi->contact_size++] = conts[k][i];		
-		break;
-	default:
-		assert(0);
-	}
-	
-	//translate to global coordinate
-	PyVector2_Rotate(&(candi->normal), self[k]->fRotation);
-	for(i = 0; i < candi->contact_size; ++i)
-	{
-		PyVector2_Rotate(&(candi->contacts[i]), self[k]->fRotation);
-		candi->contacts[i] = c_sum(candi->contacts[i], self[k]->vecPosition);
-		//precompute kFactor
-		refR = c_diff(candi->contacts[i], self[k]->vecPosition);
-		incidR = c_diff(candi->contacts[i], inc[k]->vecPosition);
-		tmp1 = PyVector2_Dot(PyVector2_fCross(PyVector2_Cross(refR, candi->normal), refR), candi->normal)
-			 /((PyShapeObject*)self[k]->shape)->rInertia;
-		tmp2 = PyVector2_Dot(PyVector2_fCross(PyVector2_Cross(incidR, candi->normal), incidR), candi->normal)
-			 /((PyShapeObject*)inc[k]->shape)->rInertia;
-
-		candi->kFactors[i] = 1/self[k]->fMass + 1/inc[k]->fMass + tmp1 + tmp2;
-	}
-
-	*ans_ref = self[k];
-	*ans_inc = inc[k];
-
+    int i, k;
+    double deps[4];
+    double min_dep[2];
+    int face_id[2];
+    PyVector2 conts[2][MAX_CONTACTS];
+    AABBBox* box[2];
+    PyBodyObject* self[2], * inc[2];
+    PyVector2 refR, incidR;
+    int size;
+    double tmp1, tmp2;
+    
+    for(i = 0; i < candi->contact_size; ++i)
+    {
+        conts[0][i] = candi->contacts[i];
+        conts[1][i] = PyBodyObject_GetRelativePos(incBody, selfBody, &conts[0][i]);
+    }
+    box[0] = selfBox;
+    box[1] = incBox;
+    self[0] = inc[1] = selfBody;
+    inc[0] = self[1] = incBody;
+    
+    for(k = 0; k <= 1; ++k)
+    {
+        memset(deps, 0, sizeof(deps));
+        for(i = 0; i < candi->contact_size; ++i)
+        {
+            deps[CF_LEFT] += fabs(conts[k][i].real - box[k]->left);
+            deps[CF_RIGHT] += fabs(box[k]->right - conts[k][i].real);
+            deps[CF_BOTTOM] += fabs(conts[k][i].imag - box[k]->bottom);
+            deps[CF_TOP] += fabs(box[k]->top - conts[k][i].imag);
+        }
+        
+        min_dep[k] = DBL_MAX;
+        for(i = CF_LEFT; i <= CF_TOP; ++i)
+            if(min_dep[k] > deps[i])
+            {
+                face_id[k] = i;
+                min_dep[k] = deps[i];
+            }
+    }
+    
+    //now select min depth one
+    k = min_dep[0] < min_dep[1] ? 0 : 1;
+    
+    candi->min_depth = min_dep[k];
+    size = candi->contact_size;
+    candi->contact_size = 0;
+    switch(face_id[k])
+    {
+    case CF_LEFT:
+        PyVector2_Set(candi->normal, -1, 0);
+        for(i = 0; i < size; ++i)
+            if(!PyMath_IsNearEqual(conts[k][i].real, box[k]->left))
+                candi->contacts[candi->contact_size++] = conts[k][i];
+        break;
+    case CF_RIGHT:
+        PyVector2_Set(candi->normal, 1, 0);
+        for(i = 0; i < size; ++i)
+            if(!PyMath_IsNearEqual(conts[k][i].real, box[k]->right))
+                candi->contacts[candi->contact_size++] = conts[k][i];
+        break;
+    case CF_BOTTOM:
+        PyVector2_Set(candi->normal, 0, -1);
+        for(i = 0; i < size; ++i)
+            if(!PyMath_IsNearEqual(conts[k][i].imag, box[k]->bottom))
+                candi->contacts[candi->contact_size++] = conts[k][i];
+        break;
+    case CF_TOP:
+        PyVector2_Set(candi->normal, 0, 1);
+        for(i = 0; i < size; ++i)
+            if(!PyMath_IsNearEqual(conts[k][i].imag, box[k]->top))
+                candi->contacts[candi->contact_size++] = conts[k][i];		
+        break;
+    default:
+        assert(0);
+    }
+    
+    //translate to global coordinate
+    PyVector2_Rotate(&(candi->normal), self[k]->fRotation);
+    for(i = 0; i < candi->contact_size; ++i)
+    {
+        PyVector2_Rotate(&(candi->contacts[i]), self[k]->fRotation);
+        candi->contacts[i] = c_sum(candi->contacts[i], self[k]->vecPosition);
+        //precompute kFactor
+        refR = c_diff(candi->contacts[i], self[k]->vecPosition);
+        incidR = c_diff(candi->contacts[i], inc[k]->vecPosition);
+        tmp1 = PyVector2_Dot(PyVector2_fCross(PyVector2_Cross(refR, candi->normal), refR), candi->normal)
+            /((PyShapeObject*)self[k]->shape)->rInertia;
+        tmp2 = PyVector2_Dot(PyVector2_fCross(PyVector2_Cross(incidR, candi->normal), incidR), candi->normal)
+            /((PyShapeObject*)inc[k]->shape)->rInertia;
+        
+        candi->kFactors[i] = 1/self[k]->fMass + 1/inc[k]->fMass + tmp1 + tmp2;
+    }
+    
+    *ans_ref = self[k];
+    *ans_inc = inc[k];
 }
 
 static int _RectShapeCollision(PyBodyObject* selfBody, PyBodyObject* incidBody, 
-							   PyObject* contactList)
+    PyObject* contactList)
 {
-	
-	PyVector2 p_in_self[4], p_in_inc[4];
-	AABBBox box_self, box_inc;
-	int i;
-	PyRectShapeObject * self, * inc;
-	_Candidate candi;
-	PyContact* contact;
-	PyVector2 * pAcc, * pSplitAcc;
-	PyBodyObject* ans_ref, * ans_inc;
+    PyVector2 p_in_self[4], p_in_inc[4];
+    AABBBox box_self, box_inc;
+    int i;
+    PyRectShapeObject * self, * inc;
+    _Candidate candi;
+    PyContact* contact;
+    PyVector2 * pAcc, * pSplitAcc;
+    PyBodyObject* ans_ref, * ans_inc;
 
-	
-	self = (PyRectShapeObject*)selfBody->shape;
-	inc = (PyRectShapeObject*)incidBody->shape;
+    candi.normal.real = candi.normal.imag = 0;
 
-	p_in_self[0] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->bottomleft));
-	p_in_self[1] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->bottomright));
-	p_in_self[2] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->topright));
-	p_in_self[3] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->topleft));
-	
-	p_in_inc[0] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->bottomleft));
-	p_in_inc[1] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->bottomright));
-	p_in_inc[2] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->topright));
-	p_in_inc[3] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->topleft));
+    self = (PyRectShapeObject*)selfBody->shape;
+    inc = (PyRectShapeObject*)incidBody->shape;
 
+    p_in_self[0] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->bottomleft));
+    p_in_self[1] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->bottomright));
+    p_in_self[2] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->topright));
+    p_in_self[3] = PyBodyObject_GetRelativePos(selfBody, incidBody, &(inc->topleft));
+    
+    p_in_inc[0] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->bottomleft));
+    p_in_inc[1] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->bottomright));
+    p_in_inc[2] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->topright));
+    p_in_inc[3] = PyBodyObject_GetRelativePos(incidBody, selfBody, &(self->topleft));
+    
+    
+    box_self = AABB_Gen(self->bottomleft.real, self->topright.real,
+        self->bottomleft.imag, self->topright.imag);
+    box_inc = AABB_Gen(inc->bottomleft.real, inc->topright.real,
+        inc->bottomleft.imag, inc->topright.imag);
+    
+    if(!_ClipTest(&box_self, p_in_self, &candi)) return 0;
+    
+    if(AABB_IsIn(&p_in_inc[0], &box_inc, 0.f))
+        candi.contacts[candi.contact_size++] = self->bottomleft;
+    if(AABB_IsIn(&p_in_inc[1], &box_inc, 0.f))
+        candi.contacts[candi.contact_size++] = self->bottomright;
+    if(AABB_IsIn(&p_in_inc[2], &box_inc, 0.f))
+        candi.contacts[candi.contact_size++] = self->topright;
+    if(AABB_IsIn(&p_in_inc[3], &box_inc, 0.f))
+        candi.contacts[candi.contact_size++] = self->topleft;
+    
+    _SATFindCollisionProperty(selfBody, incidBody, &box_self, &box_inc, &candi, &ans_ref, &ans_inc);
+    
+    
+    pAcc = PyObject_Malloc(sizeof(PyVector2));
+    pAcc->real = pAcc->imag = 0;
+    pSplitAcc = PyObject_Malloc(sizeof(PyVector2));
+    pSplitAcc->real = pSplitAcc->imag = 0;
+    for(i = 0; i < candi.contact_size; ++i)
+    {
+        contact = (PyContact*)PyContact_New(ans_ref, ans_inc);
+        contact->pos = candi.contacts[i];
+        contact->normal = candi.normal;
 
-	box_self = AABB_Gen(self->bottomleft.real, self->topright.real,
-		self->bottomleft.imag, self->topright.imag);
-	box_inc = AABB_Gen(inc->bottomleft.real, inc->topright.real,
-		inc->bottomleft.imag, inc->topright.imag);
+        contact->ppAccMoment = PyObject_Malloc(sizeof(PyVector2*));
+        *(contact->ppAccMoment) = pAcc;
+        contact->ppSplitAccMoment = PyObject_Malloc(sizeof(PyVector2*));
+        *(contact->ppSplitAccMoment) = pSplitAcc;
 
-	if(!_ClipTest(&box_self, p_in_self, &candi)) return 0;
+        contact->weight = candi.contact_size;
+        contact->depth = candi.min_depth;
+        contact->kFactor = candi.kFactors[i];
 
-	if(AABB_IsIn(&p_in_inc[0], &box_inc, 0.f))
-		candi.contacts[candi.contact_size++] = self->bottomleft;
-	if(AABB_IsIn(&p_in_inc[1], &box_inc, 0.f))
-		candi.contacts[candi.contact_size++] = self->bottomright;
-	if(AABB_IsIn(&p_in_inc[2], &box_inc, 0.f))
-		candi.contacts[candi.contact_size++] = self->topright;
-	if(AABB_IsIn(&p_in_inc[3], &box_inc, 0.f))
-		candi.contacts[candi.contact_size++] = self->topleft;
+        PyList_Append(contactList, (PyObject*)contact);
+    }
 
-	_SATFindCollisionProperty(selfBody, incidBody, &box_self, &box_inc, &candi, &ans_ref, &ans_inc);
-
-	
-	pAcc = PyObject_Malloc(sizeof(PyVector2));
-	pAcc->real = pAcc->imag = 0;
-	pSplitAcc = PyObject_Malloc(sizeof(PyVector2));
-	pSplitAcc->real = pSplitAcc->imag = 0;
-	for(i = 0; i < candi.contact_size; ++i)
-	{
-		contact = (PyContact*)PyContact_New(ans_ref, ans_inc);
-		contact->pos = candi.contacts[i];
-		contact->normal = candi.normal;
-
-		contact->ppAccMoment = PyObject_Malloc(sizeof(PyVector2*));
-		*(contact->ppAccMoment) = pAcc;
-		contact->ppSplitAccMoment = PyObject_Malloc(sizeof(PyVector2*));
-		*(contact->ppSplitAccMoment) = pSplitAcc;
-
-		contact->weight = candi.contact_size;
-		contact->depth = candi.min_depth;
-		contact->kFactor = candi.kFactors[i];
-
-		PyList_Append(contactList, (PyObject*)contact);
-	}
-
-	return 1;
+    return 1;
 }
 
 
 #undef MAX_CONTACTS
 
 
-
-
-
 /* C API */
 static PyObject *PyShape_New (void);
 static PyObject *PyRectShape_New (double width, double height, double seta);
 
+static PyMethodDef _Shape_methods[] = {
+    { "_collision",(PyCFunction)_Shape_collision,METH_VARARGS,"" },
+    { "_update_aabb",(PyCFunction)_Shape_updateAABB,METH_VARARGS,"" },
+    { NULL, NULL, 0, NULL }   /* Sentinel */
+};
+
 
 PyTypeObject PyShape_Type =
 {
     0,                          /* tp_weaklistoffset */
     0,                          /* tp_iter */
     0,                          /* tp_iternext */
-    0,		  	                /* tp_methods */
+    _Shape_methods,             /* tp_methods */
     0,                          /* tp_members */
     0,                          /* tp_getset */
     0,                          /* tp_base */
     shape->ob_type->tp_free((PyObject*)shape);
 }
 
+static PyObject* _Shape_collision(PyShapeObject* shape, PyObject *args)
+{
+    PyErr_SetString (PyExc_NotImplementedError, "method not implemented");
+    return NULL;
+}
+
+static PyObject* _Shape_updateAABB(PyShapeObject* shape, PyObject *args)
+{
+    PyErr_SetString (PyExc_NotImplementedError, "method not implemented");
+    return NULL;
+}
+
 PyTypeObject PyRectShape_Type =
 {
     PyObject_HEAD_INIT(NULL)
     0,                          /* tp_weaklistoffset */
     0,                          /* tp_iter */
     0,                          /* tp_iternext */
-    0,				            /* tp_methods */
+    0,				/* tp_methods */
     0,	                        /* tp_members */
-    0,				            /* tp_getset */
-    0,				            /* tp_base */
+    0,				/* tp_getset */
+    0,				/* tp_base */
     0,                          /* tp_dict */
     0,                          /* tp_descr_get */
     0,                          /* tp_descr_set */
     0,                          /* tp_dictoffset */
-    (initproc)_RectShape_init,   /* tp_init */
+    (initproc)_RectShape_init,  /* tp_init */
     0,                          /* tp_alloc */
     _RectShapeNew,              /* tp_new */
     0,                          /* tp_free */
     return (PyObject*)shape;
 }
 
+int PyShapeObject_UpdateAABB (PyShapeObject *shape, PyBodyObject *refbody)
+{
+    PyObject *result;
+    int retval;
+
+    /* C implementations should fill that */
+    if (shape->UpdateAABB)
+    {
+        shape->UpdateAABB (refbody);
+        return 1;
+    }
+    /* No internal collision implementation, try the python one. */
+    result = PyObject_CallMethod ((PyObject*)shape, "_update_aabb", "O",
+        (PyObject*)refbody);
+    if (!result)
+        return 0;
+    if (!IntFromObj (result, &retval))
+        return 0;
+    return retval;
+}
+
+int PyShapeObject_Collision (PyShapeObject *shape, PyBodyObject *refbody,
+    PyBodyObject *incbody, PyObject *contactlist)
+{
+    PyObject *result;
+    int retval;
+
+    /* C implementations should fill that */
+    if (shape->Collision)
+        return shape->Collision (refbody, incbody, contactlist);
+
+    /* No internal collision implementation, try the python one. */
+    result = PyObject_CallMethod ((PyObject*)shape, "_collision", "OOO",
+        (PyObject*)refbody, (PyObject*)incbody, contactlist);
+    if (!result)
+        return -1;
+    if (!IntFromObj (result, &retval))
+        return -1;
+    return retval;
+}
+
 /* C API */
 static PyObject *PyShape_New (void)
 {

src/pgWorldObject.c

 #include "pgDeclare.h"
 #include "pgphysics.h"
 #include "pgVector2.h"
+#include "pgAABBBox.h"
 #include "pgBodyObject.h"
 #include "pgWorldObject.h"
-#include "pgAABBBox.h"
+#include "pgShapeObject.h"
 #include "pgCollision.h"
 #include "pgHelpFunctions.h"
 
     {
         refBody = (PyBodyObject*)(PyList_GetItem(world->bodyList, i));
         refShape = (PyShapeObject*)refBody->shape;
-        refShape->UpdateAABB(refBody);
+        PyShapeObject_UpdateAABB (refShape, refBody);
     }
     
     //collision test
             incShape = (PyShapeObject*)incBody->shape;
             if(AABB_IsOverlap(&(refShape->box), &(incShape->box), 1e-8))
             {
-                Collision_DetectCollision(refBody, incBody, world->contactList);
+                PyShapeObject_Collision (refShape, refBody, incBody,
+                    world->contactList);
             }
         }
     }
     
     if (!PyArg_ParseTuple(args,"|d", &dt))
         dt = 0.1;
+    if (dt < 0)
+    {
+        PyErr_SetString (PyExc_ValueError,
+            "step time must not be smaller than 0");
+        return NULL;
+    }
     _Update (world,dt);
     Py_RETURN_NONE;
 }