Commits

Anonymous committed 097ee23

Distance Joint algorithm for Both Dynamic Bodys (Test case is added: TestBasic3Init)

  • Participants
  • Parent commits 49fbe0c
  • Branches physics

Comments (0)

Files changed (6)

 		return;
 	}
 	dt = watch_stop();
+	watch_start();
 	/*s_updateTime += dt;
 	if(s_updateTime >= s_world->fStepTime)
 	{
 		glutSwapBuffers();
 	}*/
 	PG_Update(s_world,dt);
-	watch_start();
+	
 	glClear(GL_COLOR_BUFFER_BIT);
 	glLoadIdentity();
 	do_render();
 	s_world->fStepTime = 0.03;
 	body = PG_BodyNew();
 	PG_Set_Vector2(body->vecPosition,0,0)
-	PG_Set_Vector2(body->vecLinearVelocity,10,0)
+	PG_Set_Vector2(body->vecLinearVelocity,0,30)
 	PG_AddBodyToWorld(s_world,body);
 }
 
+void TestBasic3Init()
+{
+	pgBodyObject* body1,*body2;
+	pgJointObject* joint;
+	pgVector2 a1,a2;
+	PG_Set_Vector2(a1,0,0);
+	PG_Set_Vector2(a2,0,0);
+
+	s_world = PG_WorldNew();
+	s_world->fStepTime = 0.03;
+	body1 = PG_BodyNew();
+	PG_Set_Vector2(body1->vecPosition,0,0)
+	PG_Set_Vector2(body1->vecLinearVelocity,10,0)
+	PG_AddBodyToWorld(s_world,body1);
+
+	body2 = PG_BodyNew();
+	PG_Set_Vector2(body2->vecPosition,0,100)
+	PG_Set_Vector2(body2->vecLinearVelocity,0,0)
+	PG_AddBodyToWorld(s_world,body2);
+
+
+	joint = PG_DistanceJointNew(body1,body2,0,100,a1,a2);
+	PG_AddJointToWorld(s_world,joint);
+}
+
 //===============================================
 
 void InitWorld()
 {
-	TestBasic1Init();
+	TestBasic3Init();
 }
 
 int main (int argc, char** argv)

File Test/pgPhysicsRenderer.c

 	glColor3f(1.f, 0.f, 0.f);
 	glLineWidth(2.f);
 	glBegin(GL_LINES);
-	glVertex2d(joint->body1->vecPosition.real,joint->body1->vecPosition.imag);
-	glVertex2d(pj->anchor2.real,pj->anchor2.imag);
+	if (joint->body1 && (!joint->body2))
+	{
+		glVertex2d(joint->body1->vecPosition.real,joint->body1->vecPosition.imag);
+		glVertex2d(pj->anchor2.real,pj->anchor2.imag);
+	}
+	else if(joint->body1 && joint->body2)
+	{
+		glVertex2d(joint->body1->vecPosition.real,joint->body1->vecPosition.imag);
+		glVertex2d(joint->body2->vecPosition.real,joint->body2->vecPosition.imag);
+	}
+	
 	glEnd();
 	glLineWidth(1.f);
 }

File include/pgVector2.h

 double c_get_length_square(pgVector2 c);
 double c_get_length(pgVector2 c);
 Py_complex c_mul_complex_with_real(pgVector2 c,double d);
+Py_complex c_div_complex_with_real(pgVector2 c,double d);
 void	c_normalize(pgVector2* pVec);
 double c_dot(pgVector2 a,pgVector2 b);
 double c_cross(pgVector2 a, pgVector2 b);

File src/pgBodyObject.c

 {
 	pgVector2 totalPosAdd;
 
+	//totalVelAdd = c_div_complex_with_real(body->vecImpulse,body->fMass);
+	//body->vecLinearVelocity = c_sum(body->vecLinearVelocity,totalVelAdd);
+
 	totalPosAdd = c_mul_complex_with_real(body->vecLinearVelocity,dt);
 	body->vecPosition = c_sum(body->vecPosition,totalPosAdd);
 }

File src/pgJointObject.c

 void PG_SolveDistanceJoint(pgJointObject* joint,double stepTime)
 {
 	pgVector2 vecL;
-	double lamda;
+	double lamda,cosTheta1V,cosTheta2V,mk;
+	Py_complex impuseAdd,v1Add,v2Add;
 	pgDistanceJoint* pJoint = (pgDistanceJoint*)joint;
 	if (joint->body1 && (!joint->body2))
 	{
 		lamda = -c_dot(vecL,joint->body1->vecLinearVelocity);
 		vecL = c_mul_complex_with_real(vecL,lamda);
 		joint->body1->vecLinearVelocity = c_sum(joint->body1->vecLinearVelocity,vecL);
+		return;
+	}
+
+	if(joint->body1 && joint->body2)
+	{
+		vecL = c_diff(joint->body1->vecPosition,joint->body2->vecPosition);
+		c_normalize(&vecL);
+		cosTheta1V = c_dot(vecL,joint->body1->vecLinearVelocity);
+		cosTheta2V = c_dot(vecL,joint->body2->vecLinearVelocity);
+		lamda = cosTheta1V - cosTheta2V;
+		mk = joint->body1->fMass * joint->body2->fMass / (joint->body1->fMass + joint->body2->fMass);
+		lamda *= mk;
+		impuseAdd = c_mul_complex_with_real(vecL,lamda);
+		v1Add = c_div_complex_with_real(impuseAdd,joint->body1->fMass);
+		v2Add = c_div_complex_with_real(impuseAdd,joint->body2->fMass);
+		/*joint->body1->vecLinearVelocity = c_sum(joint->body1->vecLinearVelocity,v1Add);
+		joint->body2->vecLinearVelocity = c_diff(joint->body2->vecLinearVelocity,v2Add);*/
+		joint->body1->vecLinearVelocity = c_diff(joint->body1->vecLinearVelocity,v1Add);
+		joint->body2->vecLinearVelocity = c_sum(joint->body2->vecLinearVelocity,v2Add);
+		return;
 	}
 }
 

File src/pgVector2.c

 	return r;
 }
 
+Py_complex c_div_complex_with_real(pgVector2 c,double d)
+{
+	Py_complex r;
+	r.real = c.real / d;
+	r.imag = c.imag / d;
+	return r;
+}
+
 void c_normalize(pgVector2* pVec)
 {
 	double l = c_get_length(*pVec);