Commits

Anonymous committed adf4936

better collsion test

  • Participants
  • Parent commits f6ce9e1
  • Branches physics

Comments (0)

Files changed (8)

 	glColor3f(1.f, 1.f, 1.f);
 	PG_Update(s_world, 0.005);
 	PGT_RenderWorld(s_world);
-	glprintf(0, 0, "vec of body:(%.2f, %.2f)", body->vecLinearVelocity.real, 
+	glprintf(0, 0, "Velocity of body1:(%.2f, %.2f)", body->vecLinearVelocity.real, 
 		body->vecLinearVelocity.imag);
-	glprintf(0, 16, "force of body: (%.2f, %.2f)", body->vecForce.real,
-		body->vecForce.imag);
 }
 
 
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
 	body = PG_BodyNew();
+	PG_Bind_RectShape(body, 20, 20, 0);
 	PG_Set_Vector2(body->vecPosition,0,0)
 	PG_Set_Vector2(body->vecLinearVelocity,40,0)
 	PG_AddBodyToWorld(s_world,body);
 	PG_AddJointToWorld(s_world,joint);
 }
 
+//test collision
 void TestBasic2Init()
 {
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
 	body = PG_BodyNew();
-	PG_Set_Vector2(body->vecPosition,0,0)
-	PG_Set_Vector2(body->vecLinearVelocity,0,30)
+	PG_Set_Vector2(body->vecPosition,0,0);
+	PG_Set_Vector2(body->vecLinearVelocity,0,30);
+	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, 20, 20, 0);
 	PG_AddBodyToWorld(s_world, body1);
 }
 
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
 	body1 = PG_BodyNew();
+	PG_Bind_RectShape(body1, 20, 20, 0);
 	PG_Set_Vector2(body1->vecPosition,0,0)
 	PG_Set_Vector2(body1->vecLinearVelocity,10,0)
 	PG_AddBodyToWorld(s_world,body1);
 
 	body2 = PG_BodyNew();
+	PG_Bind_RectShape(body2, 20, 20, 0);
 	PG_Set_Vector2(body2->vecPosition,0,100)
 	PG_Set_Vector2(body2->vecLinearVelocity,0,0)
 	PG_AddBodyToWorld(s_world,body2);
 	for (i = 1;i < BODY_NUM + 1;i++)
 	{
 		body[i] = PG_BodyNew();
+		PG_Bind_RectShape(body[i], 20, 20, 0);
 		PG_Set_Vector2(body[i]->vecPosition,0,(-i*50 + 100))
 		PG_Set_Vector2(body[i]->vecLinearVelocity,0,0)
 		PG_AddBodyToWorld(s_world,body[i]);

File Test/pgPhysicsRenderer.c

 
 void PGT_RenderAABB(pgBodyObject* body)
 {
-	pgVector2 p[4], gp[4];
-	int i;
+	pgVector2 p[4];
 	pgAABBBox* box;
 
 	box = &(body->shape->box);

File include/pgBodyObject.h

 pgVector2 PG_GetRelativePos(pgBodyObject* bodyA, pgBodyObject* bodyB, pgVector2* p_in_B);
 
 
+//bind rect shape with body
+void PG_Bind_RectShape(pgBodyObject* body, int width, int height, double seta);
 
 #endif //_PYGAME_PHYSICS_BODY_
 

File include/pgCollision.h

 	pgVector2 pos;
 	pgVector2 normal;
 	double depth;
+	double weight;
+	pgVector2** ppAccMoment;
 }pgContact;
 
 typedef enum _pgCollisionType
 					 pgVector2* ans_p1, pgVector2* ans_p2);
 
 pgJointObject* PG_ContactNew(pgBodyObject* refBody, pgBodyObject* incidBody);
+
 void PG_AppendContact(pgBodyObject* refBody, pgBodyObject* incidBody, PyObject* contactList);
+void PG_ApplyContact(PyObject* contactObject);
 
 #endif

File src/pgBodyObject.c

 
 extern PyTypeObject pgBodyType;
 
-
+void PG_Bind_RectShape(pgBodyObject* body, int width, int height, double seta)
+{
+	body->shape = PG_RectShapeNew(body, width, height, seta);
+}
 
 void PG_FreeUpdateBodyVel(pgWorldObject* world,pgBodyObject* body, double dt)
 {
 	pgVector2 totalVelAdd;
+	pgVector2 totalF;
 	double k;
 	if(body->bStatic) return;
 
-	totalVelAdd = c_sum(body->vecForce,world->vecGravity);
+	totalF = c_sum(body->vecForce, world->vecGravity);
 	k = dt / body->fMass;
-	totalVelAdd = c_mul_complex_with_real(totalVelAdd,k);
-	body->vecLinearVelocity = c_sum(body->vecLinearVelocity,totalVelAdd);
+	totalVelAdd = c_mul_complex_with_real(totalF, k);
+	body->vecLinearVelocity = c_sum(body->vecLinearVelocity, totalVelAdd);
 }
 
 void PG_FreeUpdateBodyPos(pgWorldObject* world,pgBodyObject* body,double dt)
 	PG_Set_Vector2(body->vecImpulse,0.0,0.0);
 	PG_Set_Vector2(body->vecLinearVelocity,0.0,0.0);
 	PG_Set_Vector2(body->vecPosition,0.0,0.0);
-
-	//TODO: here just for testing, would be replaced by generic function
-	body->shape = PG_RectShapeNew(body, 20, 20, 0);
 }
 
 PyObject* _PG_BodyNew(PyTypeObject *type, PyObject *args, PyObject *kwds)

File src/pgCollision.c

 	refBody->shape->Collision(refBody, incidBody, contactList);
 }
 
-void PG_UpdateV(pgJointObject* joint, double step)
+void PG_ApplyContact(PyObject* contactObject)
 {
 	pgVector2 neg_dV, refV, incidV;
 	pgVector2 refR, incidR;
 	double k, tmp1, tmp2;
 	double moment_len;
 	pgVector2 moment;
+	pgVector2* p;
 
-	contact = (pgContact*)joint;
-	refBody = joint->body1;
-	incidBody = joint->body2;
+	contact = (pgContact*)contactObject;
+	refBody = contact->joint.body1;
+	incidBody = contact->joint.body2;
 
 	//calculate the normal impulse
 	//k
 		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 / 2;
+	p->imag += moment.imag / 2; 
+}
 
-	//update the v and w
+void PG_UpdateV(pgJointObject* joint, double step)
+{
+	pgContact *contact;
+	pgBodyObject *refBody, *incidBody;
+	pgVector2 moment;
+	pgVector2 refR, incidR;
+
+	contact = (pgContact*)joint;
+	refBody = joint->body1;
+	incidBody = joint->body2;
+	moment = **(contact->ppAccMoment);
+
+	refR = c_diff(contact->pos, refBody->vecPosition);
+	incidR = c_diff(contact->pos, incidBody->vecPosition);
+
 	if(!refBody->bStatic)
 	{
 		refBody->vecLinearVelocity = c_diff(refBody->vecLinearVelocity, 
-			c_div_complex_with_real(moment, refBody->fMass/10));
-		//refBody->fAngleVelocity -= c_cross(refR, moment)/refBody->shape->rInertia;
+			c_div_complex_with_real(moment, refBody->fMass));
+		refBody->fAngleVelocity -= c_cross(refR, moment)/refBody->shape->rInertia;
 	}
 
 	if(!incidBody->bStatic)
 	{
 		incidBody->vecLinearVelocity = c_sum(incidBody->vecLinearVelocity, 
 			c_div_complex_with_real(moment, incidBody->fMass));
-		//incidBody->fAngleVelocity -= c_cross(refR, moment)/incidBody->shape->rInertia;
+		incidBody->fAngleVelocity += c_cross(refR, moment)/incidBody->shape->rInertia;
 	}
 }
 
 	}
 }
 
+PG_ContactDestroy(pgJointObject* contact)
+{
+	pgVector2 **p = ((pgContact*)contact)->ppAccMoment;
+	if(p)
+	{
+		if(*p)
+		{
+			PyObject_Free(*p);
+			*p = NULL;
+		}
+		PyObject_Free(p);
+		p = NULL;
+	}
+}
+
 PyObject* _PG_ContactNew(PyTypeObject *type, PyObject *args, PyObject *kwds)
 {
 	pgContact* op;
 	contact->joint.body2 = incidBody;
 	contact->joint.SolveConstraintPosition = PG_UpdateP;
 	contact->joint.SolveConstraintVelocity = PG_UpdateV;
-	contact->joint.Destroy = NULL;
+	contact->joint.Destroy = PG_ContactDestroy;
+
+	contact->ppAccMoment = NULL;
 
 	return (pgJointObject*)contact;
 }

File src/pgShapeObject.c

 	pgRectShape *self, *incid;
 	pgAABBBox clipBox;
 	pgContact* contact;
+	pgVector2* pAcc;
 
 	self = (pgRectShape*)selfBody->shape;
 	incid = (pgRectShape*)incidBody->shape;
 	}
 	//now all the contact points are added to list
 	to = PyList_Size(contactList) - 1;
-
-	
 	_SAT_GetContactNormal(&clipBox, contactList, from, to);
 
-	//transform from selfBody's locate coordinate to global coordinate
+	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;
 	}
 
-
 	return 1;
 }

File src/pgWorldObject.c

 {
 	Py_ssize_t size = PyList_Size((PyObject*)(world->bodyList));
 	Py_ssize_t i;
-	for (i = 0;i < size;i++)
+	for (i = 0; i < size; ++i)
 	{
-		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList),i));		
-		PG_FreeUpdateBodyVel(world,body,stepTime);
+		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));		
+		PG_FreeUpdateBodyVel(world, body, stepTime);
 	}
 }
 
 	//clear contactList first
 	cnt = PyList_Size((PyObject*)(world->contactList));
 	if(PyList_SetSlice((PyObject*)(world->contactList), 0, cnt, NULL) < 0) return;
-	cnt = PyList_Size((PyObject*)(world->contactList));//test
-	assert(cnt==0);
+	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
+
 	//update AABB
 	for(i = 0; i < size; ++i)
 	{
 		refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
 		refBody->shape->UpdateAABB(refBody);
 	}
+	//collision test
 	for(i = 0; i < size-1; ++i)
 	{
 		refBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), i));
 			}
 		}
 	}
-	//now collision reaction
+	//collision reaction
 	cnt = PyList_Size((PyObject*)(world->contactList));
 	for(i = 0; i < cnt; ++i)
 	{
 		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
+		PG_ApplyContact(contact);
+	}
+	//update V
+	for(i = 0; i < cnt; ++i)
+	{
+		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
 		contact->SolveConstraintVelocity(contact, step);
+	}
+	//update T
+	for(i = 0; i < cnt; ++i)
+	{
+		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
 		contact->SolveConstraintPosition(contact, step);
 	}
 }
 {
 	Py_ssize_t size = PyList_Size((PyObject*)(world->jointList));
 	Py_ssize_t i;
-	for (i = 0;i < size;i++)
+	for (i = 0; i < size; ++i)
 	{
 		pgJointObject* joint = (pgJointObject*)(PyList_GetItem((PyObject*)(world->jointList),i));
 		if (joint->SolveConstraintPosition)
 {
 	Py_ssize_t size = PyList_Size((PyObject*)(world->bodyList));
 	Py_ssize_t i;
-	for (i = 0;i < size;i++)
+	for (i = 0; i < size; ++i)
 	{
 		pgBodyObject* body = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList),i));
 		PG_FreeUpdateBodyPos(world,body,stepTime);
 	/* In case we have arguments in the python code, parse them later
 	* on.
 	*/
-	if(PyType_Ready(type)==-1) return NULL;
+	if(PyType_Ready(type) == -1) return NULL;
 	return (PyObject*) _PG_WorldNewInternal(type);
 }
 
 	* release any other memory hold by it.
 	*/
 	Py_XDECREF(world->bodyList);
-	//TODO: i don't know how to release jointObject here,
-	//		since it's sub class's constructor function is diff from each other
-	//Py_XDECREF(world->jointList);
+	Py_XDECREF(world->jointList);
+	Py_XDECREF(world->contactList);
 
 	world->ob_type->tp_free((PyObject*)world);
 }