Commits

Anonymous committed 2dd3b71

Add damping method which can make simulation look stable

Comments (0)

Files changed (5)

include/pgHelpFunctions.h

 PyObject*
 FromPhysicsVector2ToPoint (PyVector2 v2);
 
+/**
+* Return a clamp value between low and high value
+*
+* @param 
+* @return 
+*/
+double
+PG_Clamp(double x,double low,double high);
+
 #endif /* _PHYSICS_HELPFUNCTIONS_H_ */

include/pgphysics.h

     
     double    fRestitution;
     double    fFriction;
+	double	  fLinearVelDamping;
+	double	  fAngleVelDamping;
     
     PyObject* shape;
     

src/pgBodyObject.c

     body->fTorque = 0.0;
     body->shape = NULL;
     body->bStatic = 0;
+	body->fLinearVelDamping = 0.0;
+	body->fAngleVelDamping = 0.06;
     PyVector2_Set(body->vecForce,0.0,0.0);
     PyVector2_Set(body->vecImpulse,0.0,0.0);
     PyVector2_Set(body->vecLinearVelocity,0.0,0.0);
     double dt)
 {
     PyVector2 totalF;
+	double k1,k2;
     if (body->bStatic)
         return;
 
         PyVector2_MultiplyWithReal(gravity, body->fMass));
     body->vecLinearVelocity = c_sum(body->vecLinearVelocity, 
         PyVector2_MultiplyWithReal(totalF, dt/body->fMass));
+	k1 = PG_Clamp(1-dt*body->fLinearVelDamping,0,1);
+	k2 = PG_Clamp(1-dt*body->fAngleVelocity,0,1);
+	body->vecLinearVelocity = PyVector2_MultiplyWithReal(body->vecLinearVelocity,k1);
+	body->fAngleVelocity *= k2;
 }
 
 void PyBodyObject_FreeUpdatePos(PyBodyObject* body,double dt)

src/pgHelpFunctions.c

 */
 
 #include "pgVector2.h"
+#include <assert.h>
 #include "pgHelpFunctions.h"
 
+
 int
 DoubleFromObj (PyObject* obj, double* val)
 {
 	PyTuple_SetItem(tuple,1,ynum);
 	return tuple;
 }
+
+
+double PG_Clamp(double x,double low,double high)
+{
+	double t;
+	assert(low<high);
+	t = x<high?x:high;
+	return t>low?t:low;
+}

src/pgJointObject.c

 
     localP = c_diff(localP,body->vecPosition);
     k = ((PyShapeObject*)body->shape)->rInertia / body->fMass;
-    k*=2;
     k += PyVector2_GetLengthSquare(localP);
     bb = (distance - PyVector2_GetLength(L));
     PyVector2_Normalize(&L);
     PyVector2 localP1 = PyBodyObject_GetGlobalPos(body1,&joint->anchor1);
     PyVector2 localP2 = PyBodyObject_GetGlobalPos(body2,&joint->anchor2);
     PyVector2 L = c_diff(localP1,localP2);
+	PyVector2 eL = L;
     PyVector2 vP1 = PyBodyObject_GetLocalPointVelocity(body1,joint->anchor1);
     PyVector2 vP2 = PyBodyObject_GetLocalPointVelocity(body2,joint->anchor2);
-    PyVector2 vPL1 = PyVector2_Project(L,vP1);
-    PyVector2 vPL2 = PyVector2_Project(L,vP2);
+    PyVector2 vPL1,vPL2;
     PyVector2 dvBody1,dvBody2;
     double dAngleV1,dAngleV2;
+	PyVector2_Normalize(&eL)
+	vPL1 = PyVector2_Project(eL,vP1);
+	vPL2 = PyVector2_Project(eL,vP2);
     k1 = ((PyShapeObject*)body1->shape)->rInertia / body1->fMass;
     k2 = ((PyShapeObject*)body2->shape)->rInertia / body2->fMass;
     
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.