Commits

minz  committed 7010fd4

add position correction

  • Participants
  • Parent commits 3a89a5f
  • Branches physics

Comments (0)

Files changed (5)

 void do_render()
 {
 	glColor3f(1.f, 1.f, 1.f);
-	PG_Update(s_world, 0.005);
+	PG_Update(s_world, 0.004);
 	PGT_RenderWorld(s_world);
 	//glprintf(0, 0, "Velocity of body: (%.2f, %.2f)", body->vecLinearVelocity.real, 
 	//	body->vecLinearVelocity.imag);
 
 	body = PG_BodyNew();
 	PG_Set_Vector2(body->vecPosition, -50, 0);
-	PG_Set_Vector2(body->vecLinearVelocity, 16.f, -400.f);
+	PG_Set_Vector2(body->vecLinearVelocity, 20.f, -100.f);
 	body->fRotation = M_PI/4;
-	body->fAngleVelocity = 5.f;
+	body->fAngleVelocity = -2.f;
 	body->fRestitution = 1.f;
-	PG_Bind_RectShape(body, 30, 30, 0);
+	PG_Bind_RectShape(body, 40, 15, 0);
 	PG_AddBodyToWorld(s_world, body);
 	
 	body1 = PG_BodyNew();
 	{
 		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_Set_Vector2(body[i]->vecPosition,0,(-i*60 + 100))
+		PG_Set_Vector2(body[i]->vecLinearVelocity,50,0)
 		PG_AddBodyToWorld(s_world,body[i]);
 	}
 

File include/pgCollision.h

 pgJointObject* PG_ContactNew(pgBodyObject* refBody, pgBodyObject* incidBody);
 
 void PG_AppendContact(pgBodyObject* refBody, pgBodyObject* incidBody, PyObject* contactList);
-void PG_ApplyContact(PyObject* contactObject);
+void PG_ApplyContact(PyObject* contactObject, double step);
 
 #endif

File src/pgCollision.c

 	refBody->shape->Collision(refBody, incidBody, contactList);
 }
 
-void PG_ApplyContact(PyObject* contactObject)
+void PG_ApplyContact(PyObject* contactObject, double step)
 {
+#define MAX_C_DEP 0.08
+#define BIAS_FACTOR 0.02
+
 	pgVector2 neg_dV, refV, incidV;
 	pgVector2 refR, incidR;
 	pgContact *contact;
 	double moment_len;
 	pgVector2 moment;
 	pgVector2* p;
+	double vbias;
 
 	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
 	
 	moment_len = c_dot(neg_dV, contact->normal)/k;
 	moment_len *= contact->resist;
-	if(moment_len < 0)
-		moment_len = 0;
+	//climp
+	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);

File src/pgShapeObject.c

 
 static int _SAT_Select(pgBodyObject* body1, pgBodyObject* body2,
 					   pgBodyObject** refBody, pgBodyObject** incBody,
-					   int* face_id, pgVector2* gp_in_ref, pgAABBBox* clipBox)
+					   int* face_id, pgVector2* gp_in_ref, pgAABBBox* clipBox,
+					   double* minDep)
 {
 	double min_dep[2];
 	int id[2];
 		for(i = 0; i < 4; ++i)
 			gp_in_ref[i] = gp[0][i];
 		*clipBox = cb[0];
+		*minDep = min_dep[0];
 	}
 	else
 	{
 		for(i = 0; i < 4; ++i)
 			gp_in_ref[i] = gp[1][i];
 		*clipBox = cb[1];
+		*minDep = min_dep[1];
 	}
 
 	return 1;
 int PG_RectShapeCollision(pgBodyObject* selfBody, pgBodyObject* incidBody, 
 						  PyObject* contactList)
 {
-#define MAX_CONTACTS 10
+#define MAX_CONTACTS 8
 
 	pgBodyObject* ref = NULL, *inc = NULL;
 	int face_id;
 	pgVector2* pAcc;
 	pgContact* contact;
 	int i;
+	double minDep;
 
 
 	overlap = _SAT_Select(selfBody, incidBody,
 						  &ref, &inc,
-						  &face_id, gp, &clipBox);
+						  &face_id, gp, &clipBox, &minDep);
 
 	if(!overlap) return 0;
 
 		contact->ppAccMoment = PyObject_Malloc(sizeof(pgVector2*));
 		*(contact->ppAccMoment) = pAcc;
 		contact->weight = csize;
+		contact->depth = minDep;
 		PyList_Append(contactList, (PyObject*)contact);
 	}
 

File src/pgWorldObject.c

 	for(i = 0; i < cnt; ++i)
 	{
 		contact = (pgJointObject*)(PyList_GetItem((PyObject*)(world->contactList), i));
-		PG_ApplyContact((PyObject*)contact);
+		PG_ApplyContact((PyObject*)contact, step);
 	}
 	//update V
 	for(i = 0; i < cnt; ++i)