Anonymous avatar Anonymous committed 7cb9b1a

fix some bugs.
delete obsolete code in pgShapeObject.c

Comments (0)

Files changed (7)

 	int i = 0;
 	s_world = PG_WorldNew();
 	s_world->fStepTime = 0.03;
-	PG_Set_Vector2(s_world->vecGravity, 0, -200.f);
+	PG_Set_Vector2(s_world->vecGravity, 0, -600.f);
 
-	for(i = 0; i < 3; ++i)
+	for(i = 0; i < 12; ++i)
 	{
 		body = PG_BodyNew();
-		PG_Set_Vector2(body->vecPosition, -100+100*i, 200);
-		PG_Set_Vector2(body->vecLinearVelocity, (100 - 100*i)/2, 0.f);
-		body->fRotation = M_PI/2;
-		body->fAngleVelocity = 3.f;
-		body->fRestitution = 0.0f;
-		body->fMass = 600.f;
-		PG_Bind_RectShape(body, 40, 30, 0);
+		//PG_Set_Vector2(body->vecPosition, -100+100*i, 200);
+		//PG_Set_Vector2(body->vecLinearVelocity, (100 - 100*i)/2, 0.f);
+		PG_Set_Vector2(body->vecPosition, 0, 400 - 40*i);
+		
+		body->fRotation = 0.f;
+		body->fAngleVelocity = 0.f;
+		body->fRestitution = 1.f;
+		body->fMass = 10;
+		PG_Bind_RectShape(body, 30, 30, 0);
 		PG_AddBodyToWorld(s_world, body);
 	}
 
 	PG_Set_Vector2(body1->vecPosition,0, -100);
 	body1->bStatic = 1;
 	body1->fRestitution = 1.f;//for test
-	body1->fMass = 1e24;
+	body1->fMass = 1e32;
 	body1->fRotation = 0.f;
 	PG_Bind_RectShape(body1, 1000, 20, 0);
 	PG_AddBodyToWorld(s_world, body1);

include/pgAABBBox.h

 pgAABBBox PG_GenAABB(double left, double right, double bottom, double top);
 void PG_AABBExpandTo(pgAABBBox* box, pgVector2* p);
 void PG_AABBClear(pgAABBBox* box);
-int PG_IsOverlap(pgAABBBox* boxA, pgAABBBox* boxB);
+int PG_IsOverlap(pgAABBBox* boxA, pgAABBBox* boxB, double eps);
 int PG_IsIn(pgVector2* p, pgAABBBox* box, double eps);
 
 #endif //_PYGAME_MATH_AABBBOX_

include/pgCollision.h

 	double depth;
 	double weight;
 	double resist;
+	double kFactor, tFactor;
 	pgVector2** ppAccMoment;
 	pgVector2** ppSplitAccMoment;
 }pgContact;
 	box->top = MAX(box->top, p->imag);
 }
 
-int PG_IsOverlap(pgAABBBox* boxA, pgAABBBox* boxB)
+int PG_IsOverlap(pgAABBBox* boxA, pgAABBBox* boxB, double eps)
 {
 	double from_x, from_y, to_x, to_y;
 	from_x = MAX(boxA->left, boxB->left);
 	from_y = MAX(boxA->bottom, boxB->bottom);
 	to_x = MIN(boxA->right, boxB->right);
 	to_y = MIN(boxA->top, boxB->top);
-	return from_x <= to_x && from_y <= to_y;
+	return from_x - eps <= to_x + eps && from_y - eps <= to_y + eps;
 }
 
 PG_IsIn(pgVector2* p, pgAABBBox* box, double eps)

src/pgCollision.c

 
 void PG_ApplyContact(PyObject* contactObject, double step)
 {
-#define MAX_C_DEP 0.02
-#define BIAS_FACTOR 0.15
+#define MAX_C_DEP 0.01
+#define BIAS_FACTOR 0.25
 
 	pgVector2 neg_dV, refV, incidV;
 	pgVector2 refR, incidR;
 	pgContact *contact;
 	pgBodyObject *refBody, *incidBody;
-	double k, tmp1, tmp2;
 	double moment_len;
 	pgVector2 moment;
 	pgVector2* p;
 
 	contact->resist = sqrtf(refBody->fRestitution*incidBody->fRestitution);
 
-	//calculate the normal impulse
-	//k
 	refR = c_diff(contact->pos, refBody->vecPosition);
 	incidR = c_diff(contact->pos, incidBody->vecPosition);
-
-	tmp1 = c_dot(c_fcross(c_cross(refR, contact->normal), refR), contact->normal)
-		 /refBody->shape->rInertia;
-	tmp2 = c_dot(c_fcross(c_cross(incidR, contact->normal), incidR), contact->normal)
-		/incidBody->shape->rInertia;
-
-	k = 1/refBody->fMass + 1/incidBody->fMass + tmp1 + tmp2;
-	
 	//dV = v2 + w2xr2 - (v1 + w1xr1)
 	incidV = c_sum(incidBody->vecLinearVelocity, c_fcross(incidBody->fAngleVelocity,
 		           incidR));
 	neg_dV = c_diff(refV, incidV);
 	
 	moment_len = c_dot(c_mul_complex_with_real(neg_dV, (1 + contact->resist)), 
-		               contact->normal)/k;
+		contact->normal)/contact->kFactor;
 	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);
 	bneg_dV = c_diff(brefV, bincidV); 
 	//bias_moment
 	bm_len = c_dot(c_mul_complex_with_real(bneg_dV, (1)),
-		contact->normal)/k;
-	bm_len = MAX(0, bm_len + vbias/k);
+		contact->normal)/contact->kFactor;
+	bm_len = MAX(0, bm_len + vbias/contact->kFactor);
 	bm = c_mul_complex_with_real(contact->normal, bm_len);
 	p = *(contact->ppSplitAccMoment);
 	p->real += bm.real/contact->weight;
 
 void PG_UpdateP(pgJointObject* joint, double step)
 {
-
-	pgVector2 v;
-	double w;
-
-	//TODO: concern dt
-	//if(!joint->body1->bStatic)
-	//{
-	//	//v = joint->body1->vecLinearVelocity;
-	//	//w = joint->body1->fAngleVelocity;
-
-	//	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(v, step));
-	//	joint->body1->fRotation += w*step;
-	//}
-
-	//if(!joint->body2->bStatic)
-	//{
-	//	//v = joint->body1->vecLinearVelocity;
-	//	//w = joint->body1->fAngleVelocity;
-
-	//	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(v, step));
-	//	joint->body2->fRotation += w*step;
-	//}
+	//isolated function
 }
 
 void PG_ContactDestroy(pgJointObject* contact)

src/pgShapeObject.c

 	pgContact* contact;
 	int i;
 	double minDep;
+	pgVector2 refR, incidR;
+	double tmp1, tmp2;
 
 
 	overlap = _SAT_Select(selfBody, incidBody,
 		break;
 	}
 
+
 	pAcc = PyObject_Malloc(sizeof(pgVector2));
 	pAcc->real = pAcc->imag = 0;
 	pSplitAcc = PyObject_Malloc(sizeof(pgVector2));
 
 		contact->weight = csize;
 		contact->depth = minDep;
+
+		//precompute kFactor
+		refR = c_diff(contact->pos, selfBody->vecPosition);
+		incidR = c_diff(contact->pos, incidBody->vecPosition);
+		tmp1 = c_dot(c_fcross(c_cross(refR, contact->normal), refR), contact->normal)
+			 /selfBody->shape->rInertia;
+		tmp2 = c_dot(c_fcross(c_cross(incidR, contact->normal), incidR), contact->normal)
+			/incidBody->shape->rInertia;
+
+		contact->kFactor = 1/selfBody->fMass + 1/incidBody->fMass + tmp1 + tmp2;
+
+
 		PyList_Append(contactList, (PyObject*)contact);
 	}
 

src/pgWorldObject.c

 		{
 			incBody = (pgBodyObject*)(PyList_GetItem((PyObject*)(world->bodyList), j));
 			if(refBody->bStatic && incBody->bStatic) continue;
-			if(PG_IsOverlap(&(refBody->shape->box), &(incBody->shape->box)))
+			if(PG_IsOverlap(&(refBody->shape->box), &(incBody->shape->box), 1e-8))
 			{
 				PG_AppendContact(refBody, incBody, (PyObject*)world->contactList);
 			}
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.