Commits

Anonymous committed 34361b4

add split impulse so that collsion is more stable.

Comments (0)

Files changed (6)

 #include <GL/glut.h>
+#include <math.h>
 
 #define HEIGHT 600
 #define WIDTH 600
 void do_render()
 {
 	glColor3f(1.f, 1.f, 1.f);
-	PG_Update(s_world, 0.004);
+	PG_Update(s_world, 0.005);
 	PGT_RenderWorld(s_world);
 	//glprintf(0, 0, "Velocity of body: (%.2f, %.2f)", body->vecLinearVelocity.real, 
 	//	body->vecLinearVelocity.imag);
 //test collision
 void TestBasic2Init()
 {
+	int i = 0;
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
 
-	body = PG_BodyNew();
-	PG_Set_Vector2(body->vecPosition, -50, 0);
-	PG_Set_Vector2(body->vecLinearVelocity, 20.f, -100.f);
-	body->fRotation = M_PI/4;
-	body->fAngleVelocity = -2.f;
-	body->fRestitution = 1.f;
-	PG_Bind_RectShape(body, 40, 15, 0);
-	PG_AddBodyToWorld(s_world, body);
-	
+	for(i = 0; i < 3; ++i)
+	{
+		body = PG_BodyNew();
+		PG_Set_Vector2(body->vecPosition, -100+100*i, 200);
+		PG_Set_Vector2(body->vecLinearVelocity, (100 - 100)/2, 0.f);
+		body->fRotation = .8f;
+		body->fAngleVelocity = 30.f;
+		body->fRestitution = 0.f;
+		PG_Bind_RectShape(body, 50, 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 = 1e24;
-	PG_Bind_RectShape(body1, 500, 20, 0);
+	PG_Bind_RectShape(body1, 10000, 20, 0);
 	PG_AddBodyToWorld(s_world, body1);
 
 }
 
 void InitWorld()
 {
-	TestBasic4Init();
+	TestBasic2Init();
 }
 
 int main (int argc, char** argv)

include/pgBodyObject.h

 
 	pgShapeObject* shape;
 
+	pgVector2 cBiasLV;
+	double cBiasW;
+
 };
 
 pgBodyObject* PG_BodyNew();

include/pgCollision.h

 	double weight;
 	double resist;
 	pgVector2** ppAccMoment;
+	pgVector2** ppSplitAccMoment;
 }pgContact;
 
 typedef enum _pgCollisionType

src/pgCollision.c

 
 void PG_ApplyContact(PyObject* contactObject, double step)
 {
-#define MAX_C_DEP 0.08
-#define BIAS_FACTOR 0.02
+#define MAX_C_DEP 0.01
+#define BIAS_FACTOR 0.1
 
 	pgVector2 neg_dV, refV, incidV;
 	pgVector2 refR, incidR;
 	double moment_len;
 	pgVector2 moment;
 	pgVector2* p;
+
 	double vbias;
+	pgVector2 brefV, bincidV, bneg_dV;
+	double bm_len;
+	pgVector2 bm;
 
 	contact = (pgContact*)contactObject;
 	refBody = contact->joint.body1;
 	incidBody = contact->joint.body2;
 
-	vbias = BIAS_FACTOR*MAX(0, contact->depth - MAX_C_DEP)/step;
 	contact->resist = sqrtf(refBody->fRestitution*incidBody->fRestitution);
 
 	//calculate the normal impulse
 	contact->dv = c_diff(incidV, refV);
 	neg_dV = c_diff(refV, incidV);
 	
-	moment_len = c_dot(neg_dV, contact->normal)/k;
-	moment_len *= contact->resist;
-	//climp
-	moment_len = MAX(0, moment_len + vbias/k);
+	moment_len = c_dot(c_mul_complex_with_real(neg_dV, (1 + contact->resist)), 
+		               contact->normal)/k;
+	moment_len = MAX(0, moment_len);
+	//moment_len = MAX(0, moment_len + vbias/k);
 	
 	//finally we get the momentum(oh...)
 	moment = c_mul_complex_with_real(contact->normal, moment_len);
 	p = *(contact->ppAccMoment);
-	//TODO: test weight, temp codes
+	//TODO: test weight
 	p->real += moment.real/contact->weight;
 	p->imag += moment.imag/contact->weight; 
+
+	//split impulse
+	vbias = BIAS_FACTOR*MAX(0, contact->depth - MAX_C_DEP)/step;
+	//biasdv
+	bincidV = c_sum(incidBody->cBiasLV, c_fcross(incidBody->cBiasW, incidR));
+	brefV = c_sum(refBody->cBiasLV, c_fcross(refBody->cBiasW, refR));
+	bneg_dV = c_diff(brefV, bincidV); 
+	//bias_moment
+	bm_len = c_dot(c_mul_complex_with_real(bneg_dV, (1+contact->resist)),
+		contact->normal)/k;
+	bm_len = MAX(0, bm_len + vbias/k);
+	bm = c_mul_complex_with_real(contact->normal, bm_len);
+	p = *(contact->ppSplitAccMoment);
+	p->real += bm.real/contact->weight;
+	p->imag += bm.imag/contact->weight;
+
 }
 
 void PG_UpdateV(pgJointObject* joint, double step)
 {
 	pgContact *contact;
 	pgBodyObject *refBody, *incidBody;
-	pgVector2 moment;
+	pgVector2 moment, bm;
 	pgVector2 refR, incidR;
 
 	contact = (pgContact*)joint;
 	refBody = joint->body1;
 	incidBody = joint->body2;
 	moment = **(contact->ppAccMoment);
+	bm = **(contact->ppSplitAccMoment);
 
 	refR = c_diff(contact->pos, refBody->vecPosition);
 	incidR = c_diff(contact->pos, incidBody->vecPosition);
 		refBody->vecLinearVelocity = c_diff(refBody->vecLinearVelocity, 
 			c_div_complex_with_real(moment, refBody->fMass));
 		refBody->fAngleVelocity -= c_cross(refR, moment)/refBody->shape->rInertia;
+
+		refBody->cBiasLV = c_diff(refBody->cBiasLV,
+			c_div_complex_with_real(bm, refBody->fMass));
+		refBody->cBiasW -= c_cross(refR, bm)/refBody->shape->rInertia;
 	}
 
 	if(!incidBody->bStatic)
 		incidBody->vecLinearVelocity = c_sum(incidBody->vecLinearVelocity, 
 			c_div_complex_with_real(moment, incidBody->fMass));
 		incidBody->fAngleVelocity += c_cross(incidR, moment)/incidBody->shape->rInertia;
+
+		incidBody->cBiasLV = c_sum(incidBody->cBiasLV,
+			c_div_complex_with_real(bm, incidBody->fMass));
+		incidBody->cBiasW += c_cross(incidR, bm)/incidBody->shape->rInertia;
 	}
 }
 
 void PG_UpdateP(pgJointObject* joint, double step)
 {
+
+	pgVector2 v;
+	double w;
+
 	//TODO: concern dt
 	if(!joint->body1->bStatic)
 	{
+		v = c_sum(joint->body1->vecLinearVelocity, joint->body1->cBiasLV);
+		w = joint->body1->fAngleVelocity + joint->body1->cBiasW;
+
 		joint->body1->vecPosition = c_sum(joint->body1->vecPosition, 
-			c_mul_complex_with_real(joint->body1->vecLinearVelocity, step));
-		joint->body1->fRotation += joint->body1->fAngleVelocity*step;
+			c_mul_complex_with_real(v, step));
+		joint->body1->fRotation += w*step;
 	}
 
 	if(!joint->body2->bStatic)
 	{
+		v = c_sum(joint->body2->vecLinearVelocity, joint->body2->cBiasLV);
+		w = joint->body2->fAngleVelocity + joint->body2->cBiasW;
+
 		joint->body2->vecPosition = c_sum(joint->body2->vecPosition, 
-			c_mul_complex_with_real(joint->body2->vecLinearVelocity, step));
-		joint->body2->fRotation += joint->body2->fAngleVelocity*step;
+			c_mul_complex_with_real(v, step));
+		joint->body2->fRotation += w*step;
 	}
 }
 
 		PyObject_Free(p);
 		p = NULL;
 	}
+
+	p = ((pgContact*)contact)->ppSplitAccMoment;
+	if(p)
+	{
+		if(*p)
+		{
+			PyObject_Free(*p);
+			*p = NULL;
+		}
+		PyObject_Free(p);
+		p = NULL;
+	}
 }
 
 PyObject* _PG_ContactNew(PyTypeObject *type, PyObject *args, PyObject *kwds)
 	contact->joint.Destroy = PG_ContactDestroy;
 
 	contact->ppAccMoment = NULL;
+	contact->ppSplitAccMoment = NULL;
 
 	return (pgJointObject*)contact;
 }

src/pgShapeObject.c

 	pgVector2 normal;
 	pgAABBBox clipBox;
 	int overlap;
-	pgVector2* pAcc;
+	pgVector2* pAcc, * pSplitAcc;
 	pgContact* contact;
 	int i;
 	double minDep;
 
 	pAcc = PyObject_Malloc(sizeof(pgVector2));
 	pAcc->real = pAcc->imag = 0;
+	pSplitAcc = PyObject_Malloc(sizeof(pgVector2));
+	pSplitAcc->real = pSplitAcc->imag = 0;
 	for(i = 0; i < csize; ++i)
 	{
 		contact = (pgContact*)PG_ContactNew(ref, inc);
 
 		contact->ppAccMoment = PyObject_Malloc(sizeof(pgVector2*));
 		*(contact->ppAccMoment) = pAcc;
+		contact->ppSplitAccMoment = PyObject_Malloc(sizeof(pgVector2*));
+		*(contact->ppSplitAccMoment) = pSplitAcc;
+
 		contact->weight = csize;
 		contact->depth = minDep;
 		PyList_Append(contactList, (PyObject*)contact);

src/pgWorldObject.c

 
 void _PG_BodyCollisionDetection(pgWorldObject* world, double step)
 {
-	Py_ssize_t i, j, cnt, size;
+	Py_ssize_t i, j, cnt, size, body_size;
 	pgBodyObject* refBody, *incBody;
 	pgJointObject* contact;
 	
 	cnt = PyList_Size((PyObject*)(world->contactList));
 	if(PyList_SetSlice((PyObject*)(world->contactList), 0, cnt, NULL) < 0) return;
 	assert(PyList_Size((PyObject*)(world->contactList))==0);
-	//Py_XDECREF((PyObject*)world->contactList);
-	//world->contactList = (PyListObject*)PyList_New(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;
+	}
 	
 	//update AABB
 	for(i = 0; i < size; ++i)
 		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
 		PG_ApplyContact((PyObject*)contact, step);
 	}
+	
 	//update V
-	for(i = 0; i < cnt; ++i)
-	{
-		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
-		contact->SolveConstraintVelocity(contact, step);
-	}
+	for(j = 0; j < 1; ++j)
+		for(i = 0; i < cnt; ++i)
+		{
+			contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
+			contact->SolveConstraintVelocity(contact, step);
+		}
 	//update P
 	for(i = 0; i < cnt; ++i)
 	{
 	int i;
 	pgBodyObject* refBody;
 	_PG_FreeBodySimulation(world, stepTime);
+
 	_PG_BodyCollisionDetection(world, stepTime);
 	for (i = 0;i < MAX_SOLVE_INTERAT;i++)
 	{