Anonymous avatar Anonymous committed 11a720f

add collision reaction code(not be tested yet)

Comments (0)

Files changed (10)

include/pgAABBBox.h

 		{
 			double left, bottom, right, top;
 		};
+
 		struct
 		{
 			double from[2], to[2];

include/pgBodyObject.h

 	double		fMass;
 	pgVector2	vecLinearVelocity;
 	double		fAngleVelocity;
-	int			bStatic;
+	//int			bStatic;
 
 	pgVector2	vecPosition;
 	double		fRotation;
 
 //return the global velocity of a point p (on the rigid body)
 //(notice: here p is defined in the global coordinate)
-pgVector2 PG_GetVelocity(pgBodyObject* body, pgVector2* global_p);
-
-pgVector2 PG_GetVelocity1(pgVector2 r, double w);
+pgVector2 PG_AngleToLinear1(pgBodyObject* body, pgVector2* global_p);
+pgVector2 PG_AngleToLinear(pgVector2* r, double w);
 
 //translate vector from coordinate B to coordinate A
 pgVector2 PG_GetRelativePos(pgBodyObject* bodyA, pgBodyObject* bodyB, pgVector2* p_in_B);

include/pgCollision.h

 int PG_LiangBarskey(pgAABBBox* box, pgVector2* p1, pgVector2* p2, 
 					 pgVector2* ans_p1, pgVector2* ans_p2);
 
+pgJointObject* PG_ContactNew(pgBodyObject* refBody, pgBodyObject* incidBody);
+void PG_AppendContact(pgBodyObject* refBody, pgBodyObject* incidBody, PyObject* contactList);
+
 #endif

include/pgShapeObject.h

 
 	pgAABBBox box;
 	ShapeType type;
+	double rInertia; //Rotor inertia  
 
 	//virtual functions
 	void (*Destroy)(pgShapeObject* shape);

include/pgWorldObject.h

 
 	PyListObject*	bodyList;
 	PyListObject*	jointList;
+	PyListObject*	contactList;
 
 	Py_complex		vecGravity;
 	double			fDamping;

src/pgBodyObject.c

 	return p_in_A;
 }
 
-pgVector2 PG_GetVelocity1(pgVector2 r, double w)
+pgVector2 PG_AngleToLinear(pgVector2* r, double w)
 {
 	pgVector2 v;
 	double r_len, v_len;
 
-	r_len = c_get_length(r);
+	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;
+		r->real /= r_len;
+		r->imag /= r_len;
 		v_len = fabs(r_len*w);
-		r.real *= v_len;
-		r.imag *= v_len;
+		r->real *= v_len;
+		r->imag *= v_len;
 		if(w > 0) //counter-clock wise
 		{	
-			v.real = -r.imag;
-			v.imag = r.real;
+			v.real = -r->imag;
+			v.imag = r->real;
 		}
-		else
+		else //clock wise
 		{
-			v.real = r.imag;
-			v.imag = -r.real;
+			v.real = r->imag;
+			v.imag = -r->real;
 		}
 	}
 
 	return v;
 }
 
-pgVector2 PG_GetVelocity(pgBodyObject* body, pgVector2* global_p)
+pgVector2 PG_AngleToLinear1(pgBodyObject* body, pgVector2* global_p)
 {	
 	//get rotate radius vector r
 	pgVector2 r = c_diff(*global_p, body->vecPosition);
-	return PG_GetVelocity1(r, body->fRotation);
+	return PG_AngleToLinear(&r, body->fRotation);
 }
 
 //============================================================

src/pgCollision.c

 #include "pgCollision.h"
 #include "pgAABBBox.h"
+#include "pgShapeObject.h"
+#include "pgBodyObject.h"
 #include <assert.h>
 
 // We borrow this graph from Box2DLite
 // Apply Liang-Barskey clip on a AABB box
 // (p1, p2) is the input line segment to be clipped (note: it's a 2d vector)
 // (ans_p1, ans_p2) is the output line segment
-// TODO: tone Liang-Barskey clip
-
-
-typedef struct _LB_Rec
-{
-	double val;
-	pgBoxDirect d;
-}LB_Rec;
-
-#define LB_Rec_MAX(x, y) ( ( ( (x).val ) > ( (y).val ) ) ? (x) : (y) )
-#define LB_Rec_MIN(x, y) ( ( ( (x).val ) < ( (y).val ) ) ? (x) : (y) )
-
+//TEST: Liang-Barskey clip has been tested.
 int _LiangBarskey_Internal(double p, double q, double* u1, double* u2)
 {
 	double val;
 	return 1;
 }
 
-pgContact* pg_BuildContact(pgBodyObject* bodyA, pgBodyObject* bodyB)
+void PG_AppendContact(pgBodyObject* refBody, pgBodyObject* incidBody, PyObject* contactList)
 {
-	//clipping
-	//find contact points
-	//find the (proper) normal
-
-	return NULL;
+	refBody->shape->Collision(refBody, incidBody, contactList);
 }
 
+void PG_UpdateV(pgJointObject* joint, double step)
+{
+	pgVector2 neg_dV, refV, incidV;
+	pgVector2 refR, incidR;
+	pgContact *contact;
+	pgBodyObject *refBody, *incidBody;
+	double k, tmp1, tmp2;
+	double moment_len;
+	pgVector2 moment;
 
+	contact = (pgContact*)joint;
+	refBody = joint->body1;
+	incidBody = joint->body2;
 
+	//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;
+
+	k = 1/refBody->fMass + 1/incidBody->fMass + tmp1*tmp1/refBody->shape->rInertia
+		+ tmp2*tmp2/incidBody->shape->rInertia;
+	
+	//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)));
+	neg_dV = c_diff(refV, incidV);
+	
+	moment_len = c_dot(neg_dV, contact->normal)/k;
+	moment_len *= refBody->fRestitution;
+	if(moment_len < 0)
+		moment_len = 0;
+	//finally we get the momentum(oh...)
+	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;
+
+	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;
+
+	joint->body2->vecPosition = c_sum(joint->body2->vecPosition, joint->body2->vecLinearVelocity);
+	joint->body2->fRotation += joint->body2->fAngleVelocity;
+}
+
+
+
+pgJointObject* PG_ContactNew(pgBodyObject* refBody, pgBodyObject* incidBody)
+{
+	pgContact* contact;
+	//TODO: this function would be changed
+	contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+	contact->joint.body1 = refBody;
+	contact->joint.body2 = incidBody;
+	contact->joint.SolveConstraintPosition = PG_UpdateP;
+	contact->joint.SolveConstraintVelocity = PG_UPdateV;
+
+	return (pgJointObject*)contact;
+}

src/pgJointObject.c

 	"physics.distance_joint",            /* tp_name */
 	sizeof(pgDistanceJointObject),      /* tp_basicsize */
 	0,                          /* tp_itemsize */
-	(destructor) 0,/* tp_dealloc */
+	(destructor) 0,				/* tp_dealloc */
 	0,                          /* tp_print */
 	0,                          /* tp_getattr */
 	0,                          /* tp_setattr */
 	0,                          /* tp_weaklistoffset */
 	0,                          /* tp_iter */
 	0,                          /* tp_iternext */
-	0,           /* tp_methods */
-	0,           /* tp_members */
-	0,         /* tp_getset */
+	0,							/* tp_methods */
+	0,							/* tp_members */
+	0,							/* tp_getset */
 	0,                          /* tp_base */
 	0,                          /* tp_dict */
 	0,                          /* tp_descr_get */
 	0,                          /* tp_dictoffset */
 	0,                          /* tp_init */
 	0,                          /* tp_alloc */
-	0,               /* tp_new */
+	0,							/* tp_new */
 	0,                          /* tp_free */
 	0,                          /* tp_is_gc */
 	0,                          /* tp_bases */

src/pgShapeObject.c

 	p->shape.Destroy = PG_RectShapeDestroy;
 	p->shape.UpdateAABB = PG_RectShapeUpdateAABB;
 	p->shape.type = ST_RECT;
+	p->shape.rInertia = body->fMass*(width*width + height*height)/12; // I = M(a^2 + b^2)/12
 
 	PG_Set_Vector2(p->bottomLeft, -width/2, -height/2);
 	PG_Set_Vector2(p->bottomRight, width/2, -height/2);
 }
 
 //-------------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 
+//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
+//get the contact normal.
 //note: this method is not available in CCD(continue collision detection)
+//since the velocity is not small.
 static void _SAT_GetContactNormal(pgAABBBox* clipBox, PyObject* contactList,
 								  int from, int to)
 {
 			}
 			else
 			{
-				contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+				contact = (pgContact*)PG_ContactNew(selfBody, incidBody);
 				contact->pos = pf;
 				PyList_Append(contactList, (PyObject*)contact);
 			}
 			}
 			else
 			{
-				contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+				contact = (pgContact*)PG_ContactNew(selfBody, incidBody);
 				contact->pos = pt;
 				PyList_Append(contactList, (PyObject*)contact);
 			}
 	{
 		if(has_ip[i])
 		{
-			contact = (pgContact*)PyObject_MALLOC(sizeof(pgContact));
+			contact = (pgContact*)PG_ContactNew(selfBody, incidBody);
 			contact->pos = ip[i];
 			PyList_Append(contactList, (PyObject*)contact);
 		}
 	
 	_SAT_GetContactNormal(&clipBox, contactList, from, to);
 
-	//transform vectors from selfBody's locate coordinate
-    //to global coordinate
+	//transform from selfBody's locate coordinate to global coordinate
 	for(i = from; i <= to; ++i)
 	{
 		contact = (pgContact*)PyList_GetItem(contactList, i);

src/pgWorldObject.c

 #include "pgWorldObject.h"
 #include "pgBodyObject.h"
 #include "pgJointObject.h"
+#include "pgCollision.h"
+#include "pgShapeObject.h"
 #include <structmember.h>
 
 #define MAX_SOLVE_INTERAT 20
 
 void _PG_BodyCollisionDetection(pgWorldObject* world)
 {
-
+	int i, j;
+	pgBodyObject* refBody, *incBody;
+	int size = PyList_Size((PyObject*)(world->bodyList));
+	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)))
+			{
+				PG_AppendContact(refBody, incBody, (PyObject*)world->contactList);
+			}
+		}
+	}
 }
 
 void _PG_JointSolve(pgWorldObject* world,double stepTime)
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.