From 917008167e46a5c3129a07552278827969313137 Mon Sep 17 00:00:00 2001
From: Jasper <Jaspermanousek@hotmail.com>
Date: Fri, 16 Jan 2015 16:17:26 +0100
Subject: [PATCH] Added position Constraint structure, an add/remove function
 for them, and added their handeling to the update function

---
 physics.cc | 58 ++++++++++++++++++++++++++++++++++++++++++------------
 physics.hh |  5 +++++
 2 files changed, 50 insertions(+), 13 deletions(-)

diff --git a/physics.cc b/physics.cc
index 6970a57..d196ea6 100644
--- a/physics.cc
+++ b/physics.cc
@@ -28,10 +28,39 @@ void Physics::init(lua_State* L)
 }
 
 void Physics::takeUpdateStep(float timeDiff)
-{    
+{            
 	world->stepSimulation(timeDiff);
+	for(unsigned i = 0; i < allPositionConstraints.size();i++)
+	{
+	    if(allPositionConstraints[i].position != allPositionConstraints[i].body->getCenterOfMassPosition())
+	    {
+	        btVector3 dir = allPositionConstraints[i].body->getCenterOfMassPosition() - allPositionConstraints[i].position;
+	        allPositionConstraints[i].body->applyCentralForce(dir*allPositionConstraints[i].strength);
+	    }	
+	}
+	
 }
 
+void Physics::removePositionConstraint(int bodyIndice)
+{
+	for(unsigned i = 0; i < allPositionConstraints.size();i++)
+	{
+	    if(allPositionConstraints[i].body == bodies[i] )
+	    {
+	        allPositionConstraints.erase(allPositionConstraints.begin()+i);
+	    }
+	}
+}
+
+void Physics::addPositionConstraint(int bodyIndice, float strength, glm::vec3 position)
+{
+    positionConstraint cons;
+    cons.body = bodies[bodyIndice];
+    cons.strength = strength;
+    cons.position = btVector3(position.x,position.y,position.z);
+    allPositionConstraints.push_back(cons);
+}   
+
 //TERRAIN SUBSET
 void Physics::addTerrainTriangles(int width, int length, float** heightData)
 {//not working correctly something with offset wrong?
@@ -44,7 +73,8 @@ void Physics::addTerrainTriangles(int width, int length, float** heightData)
             btVector3 v1(i+1,heightData[j][i+1],j);
             btVector3 v2(i,heightData[j+1][i],j+1);   
             
-            trimesh->addTriangle(v0,v1,v2);                             
+            trimesh->addTriangle(v0,v1,v2);     
+                                    
         }    
     }
     for(int i = 1; i < width;i++)
@@ -100,17 +130,6 @@ void Physics::addTerrain(int width, int length, float** heightData)
 	world->addRigidBody(terrainBody, COL_TERRAIN, COL_TERRAIN | COL_OBJECTS);
 }
 
-void Physics::addStaticGroundPlane()
-{
-	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 1, 0), 0);
-	btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 0)));
-	btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0, groundMotionState, groundShape, btVector3(0, 0, 0));
-        staticGroundBody = new btRigidBody(groundRigidBodyCI);
-
-        world->addRigidBody(staticGroundBody);
-}
-
-
 //players and objects
 void Physics::addPlayer(float friction, float rad, Entity entity, float mass, float dampningL, float dampningA, unsigned indice)
 {
@@ -129,6 +148,7 @@ void Physics::addPlayer(float friction, float rad, Entity entity, float mass, fl
 	btRigidBody::btRigidBodyConstructionInfo info(mass,motion,sphere,inertia);
 
     info.m_friction = friction;
+    info.m_restitution = 0.0f;
 
 	playerBall = new btRigidBody(info);
     
@@ -389,6 +409,18 @@ void Physics::rollRight(glm::vec3 camPos,float strength)
     playerBall->applyTorque(-pos);
 }
 
+//not used right now
+
+void Physics::addStaticGroundPlane()
+{
+	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0, 1, 0), 0);
+	btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 0)));
+	btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0, groundMotionState, groundShape, btVector3(0, 0, 0));
+        staticGroundBody = new btRigidBody(groundRigidBodyCI);
+
+        world->addRigidBody(staticGroundBody);
+}
+
 /*
 void kill()
 {
diff --git a/physics.hh b/physics.hh
index 55a42ae..c3eedcd 100644
--- a/physics.hh
+++ b/physics.hh
@@ -69,6 +69,10 @@ class Physics {
 	void addPlayer(float friction, float rad, Entity entity, float mass, float dampningL, float dampningA, unsigned indice); //use these AFTER physicObjects.push_back(object)! if mass == 0 then the object is unmoveable
 	void addSphere(float rad, Entity entity, float mass, float dampningL, float dampningA, unsigned indice); //The Indice should be set to physicObjects.size()
 	void addBox(float width, float height, float length, Entity entity, float mass, float dampningL, float dampningA, unsigned indice); //this is used to ensuer that the system is synchronized
+	void addPositionConstraint(int bodyIndice, float strength, glm::vec3 position);
+	void removePositionConstraint(int bodyIndice);
+
+    struct positionConstraint{btRigidBody* body; float strength; btVector3 position;};
 
     private:
 	btRigidBody* playerBall;    //allows for easier access to the ball
@@ -76,6 +80,7 @@ class Physics {
 	btRigidBody* cameraBody;
 	std::vector<btRigidBody*> bodies; //list of all bodies. bodies are also in world, but save again to ease cleaning up process.
         btRigidBody* staticGroundBody;
+    std::vector<positionConstraint> allPositionConstraints;
     
 	btDynamicsWorld* world; //contains physical attributes of the world.
 	btDispatcher* dispatcher; //