From 13e08945df31ef5786359e8fa9f6d846ac793584 Mon Sep 17 00:00:00 2001
From: Jasper <Jaspermanousek@hotmail.com>
Date: Fri, 28 Nov 2014 13:52:03 +0100
Subject: [PATCH] Added a constraint for distances, as such a constraint did
 not exist in bullet.Why, IDK.

---
 physics.cc | 32 ++++++++++++++++++++++++++++++++
 physics.hh | 49 +++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 81 insertions(+)

diff --git a/physics.cc b/physics.cc
index 03d353c..c9cc895 100644
--- a/physics.cc
+++ b/physics.cc
@@ -1,5 +1,6 @@
 #include "physics.hh"
 
+
 Physics::Physics() {
 }
 
@@ -158,6 +159,37 @@ void Physics::addSphere(float rad, Entity entity, float mass, unsigned indice)
 
 }
 
+void Physics::addCamera(float rad, float distance)
+{
+    btSphereShape* sphere = new btSphereShape(rad);
+
+	btVector3 inertia(0,0,0);
+	btDefaultMotionState* motion = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)));
+	
+	btRigidBody::btRigidBodyConstructionInfo info(1/(playerBall->getInvMass()/100),motion,sphere,inertia);
+	
+	cameraBody = new btRigidBody(info);
+	
+    cameraBody->setDamping(0.2f,0.4f);
+
+	world->addRigidBody(cameraBody);
+		
+    cameraBody->setSleepingThresholds(0,0);
+    
+    
+    btVector3 pivotInA(5,0,0);
+    btVector3 pivotInB(-5, 0, 0);
+    btDistanceConstraint* pdc = new btDistanceConstraint(*cameraBody,*playerBall,pivotInA,pivotInB, distance);
+    world->addConstraint(pdc);
+}
+
+glm::vec3 Physics::getCameraPosition()
+{
+	btVector3 origin = cameraBody->getCenterOfMassPosition();
+	glm::vec3 save(origin.getX(),origin.getY(),origin.getZ());
+	return save;
+}
+
 glm::vec3 Physics::getPos(int i)
 {
 	btVector3 origin = bodies[i]->getCenterOfMassPosition();
diff --git a/physics.hh b/physics.hh
index a9cca30..8ba675d 100644
--- a/physics.hh
+++ b/physics.hh
@@ -18,6 +18,7 @@
 
 #include "extern/bullet/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h"
 #include "extern/bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"//YAY!
+#include "extern/bullet/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
 
 #include "extern/bullet/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
 #include "extern/bullet/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h"
@@ -46,6 +47,8 @@ class Physics {
 	glm::vec3 getPos(int i);
 	glm::mat4 getRotation(int i);
 	void addStaticGroundPlane();
+	void addCamera(float rad,float distance); //Do NOT impliment before Player has been created;
+	glm::vec3 getCameraPosition();
 	void addTerrain(int width, int length, float** heightData);
 	void addPlayer(float rad, Entity entity, float mass, 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, unsigned indice); //The Indice should be set to physicObjects.size()
@@ -54,6 +57,7 @@ class Physics {
     private:
 	btRigidBody* playerBall;    //allows for quicker access to the ball
 	btRigidBody* terrainBody;   //duh
+	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;
 
@@ -64,4 +68,49 @@ class Physics {
 	btBroadphaseInterface* broadphase; //defines how objects are culled from collision detection.
 	btConstraintSolver* solver; //solver for forces and impulses.
 };
+
+
+class btDistanceConstraint : public btPoint2PointConstraint
+{
+protected:
+   btScalar   m_distance;
+public:
+   btDistanceConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btScalar dist)
+      : btPoint2PointConstraint(rbA, rbB, pivotInA, pivotInB)
+   {
+      m_distance = dist;
+   }
+   virtual void getInfo1 (btConstraintInfo1* info)
+   {
+      info->m_numConstraintRows = 1;
+      info->nub = 5;
+   }
+   virtual void getInfo2 (btConstraintInfo2* info)
+   {
+      btVector3 relA = m_rbA.getCenterOfMassTransform().getBasis() * getPivotInA();
+      btVector3 relB = m_rbB.getCenterOfMassTransform().getBasis() * getPivotInB();
+      btVector3 posA = m_rbA.getCenterOfMassTransform().getOrigin() + relA;
+      btVector3 posB = m_rbB.getCenterOfMassTransform().getOrigin() + relB;
+      btVector3 del = posB - posA;
+      btScalar currDist = btSqrt(del.dot(del));
+      btVector3 ortho = del / currDist;
+      info->m_J1linearAxis[0] = ortho[0];
+      info->m_J1linearAxis[1] = ortho[1];
+      info->m_J1linearAxis[2] = ortho[2];
+      btVector3 p, q;
+      p = relA.cross(ortho);
+      q = relB.cross(ortho);
+      info->m_J1angularAxis[0] = p[0];
+      info->m_J1angularAxis[1] = p[1];
+      info->m_J1angularAxis[2] = p[2];
+      info->m_J2angularAxis[0] = -q[0];
+      info->m_J2angularAxis[1] = -q[1];
+      info->m_J2angularAxis[2] = -q[2];
+      btScalar rhs = (currDist - m_distance) * info->fps * info->erp;
+      info->m_constraintError[0] = rhs;
+      info->cfm[0] = btScalar(0.f);
+      info->m_lowerLimit[0] = -SIMD_INFINITY;
+      info->m_upperLimit[0] = SIMD_INFINITY;
+   }
+};
 #endif