Commits

Anonymous committed 79bd29a

Comments (0)

Files changed (5)

include/pgCollision.h

 #define _PYGAME_PHYSICS_COLLISION_
 
 #include "pgBodyObject.h"
+#include "pgJointObject.h"
 #include "pgAABBBox.h"
 
 typedef struct _pgContact
 {
+	//assert body2 is the incident rigid body
+	//and body1 is the reference rigid body
+	pgJointObject joint;
+
 	pgVector2 pos;
 	pgVector2 normal;
 	double depth;
-	pgBodyObject* incBody; //incident rigid body
-	pgBodyObject* refBody; //reference rigid body
 }pgContact;
 
 typedef enum _pgCollisionType

include/pgShapeObject.h

 
 	//virtual functions
 	void (*Destroy)(pgShapeObject* shape);
-	int (*Collision)(pgBodyObject* selfBody, pgBodyObject* incidBody, PyListObject* contactPoints, 
-		               pgVector2* contactNormal);
+	int (*Collision)(pgBodyObject* selfBody, pgBodyObject* incidBody, PyObject* contactList);
 	void (*UpdateAABB)(pgShapeObject* shape);
 };
 

include/pgVector2.h

 
 
 typedef Py_complex	pgVector2;
-#define PG_Set_Vector2(vec, x, y) {vec.real = x; vec.imag = y;}
+#define PG_Set_Vector2(vec, x, y) {(vec).real = x; (vec).imag = y;}
 
 int is_zero(double num);
 int is_equal(double a, double b);

src/pgCollision.c

 		*ans_p1 = *p1;
 	else
 		*ans_p1 = c_sum(*p1, c_mul_complex_with_real(dp, u1)); //ans_p1 = p1 + u1*dp
-	if(u2 == 0.f)
+	if(u2 == 1.f)
 		*ans_p2 = *p2;
 	else
-		*ans_p2 = c_sum(*p2, c_mul_complex_with_real(dp, u2)); //ans_p2 = p2 + u2*dp;
+		*ans_p2 = c_sum(*p1, c_mul_complex_with_real(dp, u2)); //ans_p2 = p2 + u2*dp;
 
 	return 1;
 }

src/pgShapeObject.c

 	PyObject_Free((pgRectShape*)rectShape);
 }
 
-int PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody, 
-						  PyListObject* contactPoints, pgVector2* contactNormal);
+
+int PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody, PyObject* contactList);
 
 
 pgShapeObject*	PG_RectShapeNew(pgBodyObject* body, double width, double height, double seta)
 
 //we use a simple SAT to select the contactNormal:
 //Supposing the relative velocity between selfBody and incidBody in
-//two frame is small, the
-static void _SAT_GetContactNormal(pgAABBBox* clipBox, PyListObject* contactPoints,
-								  pgVector2* contactNormal)
+//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)
+static void _SAT_GetContactNormal(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;
+		}
+	}
+
+	//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;
+    }
 
 }
 
 
 //TODO: now just detect Box-Box collision, later add Box-Circle
-int PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody, 
-						  PyListObject* contactPoints, pgVector2* contactNormal)
+int PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody, PyObject* contactList)
 {
 	int i, i1;
+	int from, to;
 	int apart;
 	pgVector2 ip[4];
 	int has_ip[4]; //use it to prevent from duplication
 	pgVector2 pf, pt;
 	pgRectShape *self, *incid;
 	pgAABBBox clipBox;
+	pgContact* contact;
 
 	self = (pgRectShape*)selfBody->shape;
 	incid = (pgRectShape*)incidBody->shape;
 		self->bottomLeft.imag, self->topRight.imag);
 	apart = 1;
 	memset(has_ip, 0, sizeof(has_ip));
-	//watch out! we create contactPoints here
-	contactPoints = (PyListObject*)PyList_New(0);
 
+	from = PyList_Size(contactList);
 	for(i = 0; i < 4; ++i)
 	{
 		i1 = (i+1)%4;
 		{
 			apart = 0;
 			if(pf.real == ip[i].real && pf.imag == ip[i].imag)
+			{
 				has_ip[i] = 1;
+			}
 			else
-				PyList_Append((PyObject*)contactPoints, (PyObject*)PyComplex_FromCComplex(pf));
+			{
+				contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+				contact->pos = pf;
+				PyList_Append(contactList, (PyObject*)contact);
+			}
+			
 			if(pt.real == ip[i1].real && pt.imag == ip[i1].imag)
+			{	
 				has_ip[i1] = 1;
+			}
 			else
-				PyList_Append((PyObject*)contactPoints, (PyObject*)PyComplex_FromCComplex(pt));
+			{
+				contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+				contact->pos = pt;
+				PyList_Append(contactList, (PyObject*)contact);
+			}
+
 		}
 	}
 
 	for(i = 0; i < 4; ++i)
 	{
 		if(has_ip[i])
-			PyList_Append((PyObject*)contactPoints, (PyObject*)PyComplex_FromCComplex(ip[i]));
+		{
+			contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+			contact->pos = ip[i];
+			PyList_Append(contactList, (PyObject*)contact);
+		}
 	}
 	//now all the contact points are added to list
-	//note at the moment they are in selfBody's locate coordinate system
+	to = PyList_Size(contactList);
+
 	
+	_SAT_GetContactNormal(&clipBox, contactList, from, to);
+
+	//transform vectors from selfBody's locate coordinate
+    //to global coordinate
+	for(i = from; i <= to; ++i)
+	{
+		contact = (pgContact*)PyList_GetItem(contactList, i);
+
+		c_rotate(&(contact->pos), selfBody->fRotation);
+		c_sum(contact->pos, selfBody->vecPosition);
+		
+		c_rotate(&(contact->normal), selfBody->fRotation);
+	}
 
 
 	return 1;
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.