Fixing gravity

8 files changed, 39 insertions, 31 deletions

diff --git a/include/YAGE/Math/matrix.hpp b/include/YAGE/Math/matrix.hpp
index 238f7d09..c7c40f00 100644
--- a/include/YAGE/Math/matrix.hpp
+++ b/include/YAGE/Math/matrix.hpp
@@ -269,15 +269,9 @@ template<int M, int N, class T>
 bool operator==(const Matrix<M, N, T> &lhs, const Matrix<M, N, T> &rhs)
 {
 	for(int i=0; i<M; ++i)
-	{
 		for(int j=0; j<N; ++j)
-		{
 			if(lhs[i][j]!=rhs[i][j])
-			{
-				return false;
-			}
-		}
-	}
+					return false;
 	return true;
 }
 
@@ -303,18 +297,30 @@ public:
 		return this->data_[col];
 	}
 
-	
+	virtual std::string toString() const
+	{
+		std::stringstream ss;
+		ss<<"[";
+		for(std::size_t i=0; i<this->data_.size()-1; ++i)
+		{
+			ss<<this->data_[i]<<" ";
+		}
+		ss<<this->data_[this->data_.size()-1]<<"]";
+		return ss.str();
+	}
 };
 
 template<class Type=double> class Vector2 : public Vector<2, Type>
 {
 public:
 	Vector2<Type>() : Vector<2, Type>() {}
+	
 	Vector2<Type>(Type x, Type y)
 	{
 		this->data_[0]=x;
 		this->data_[1]=y;
 	}
+	
 	Vector2<Type>(const Matrix<2, 1, Type> &other) : Vector<2, Type>(other) {} 
 
 	Type& x()
diff --git a/include/YAGE/Physics/body.hpp b/include/YAGE/Physics/body.hpp
index f09cd5d5..5a0e879e 100644
--- a/include/YAGE/Physics/body.hpp
+++ b/include/YAGE/Physics/body.hpp
@@ -19,7 +19,7 @@ protected:
     double mass_=1;
 	
 	// current velocity of the object
-	Vector2d velocity_=Vector2d(0.f, 0.f);
+	Vector2d velocity_=Vector2d(0, 0);
 
 	// boolean that defines if gravity can act on the object
 	bool gravity_=true;
@@ -37,13 +37,13 @@ public:
 	virtual void applyForce(const Vector2d &force)=0;
 	virtual void update()=0;
 
-	float xPosition() const;
-	float yPosition() const;
+	double xPosition() const;
+	double yPosition() const;
 protected:
 	// protected constructor to initialize member variables
-	Body(const Vector2d &position=Vector2d(0.f, 0.f),
-	     double mass=1.0,
-	     const Vector2d &velocity=Vector2d(0.f, 0.f),
+	Body(const Vector2d &position=Vector2d(0, 0),
+	     double mass=1,
+	     const Vector2d &velocity=Vector2d(0, 0),
 	     bool gravity=false);
 };
 
diff --git a/include/YAGE/Physics/particlebody.hpp b/include/YAGE/Physics/particlebody.hpp
index 09ad8c7b..62e6fc43 100644
--- a/include/YAGE/Physics/particlebody.hpp
+++ b/include/YAGE/Physics/particlebody.hpp
@@ -11,9 +11,9 @@ namespace yage
 class ParticleBody : public Body
 {
 public:
-	ParticleBody(const Vector2d &position=Vector2d(0.f, 0.f),
-	             double mass=1.0,
-	             const Vector2d &velocity=Vector2d(0.f, 0.f),
+	ParticleBody(const Vector2d &position=Vector2d(0, 0),
+	             double mass=1,
+	             const Vector2d &velocity=Vector2d(0, 0),
 	             bool gravity=true);
 
 	// apply a force to the rigid body
diff --git a/include/YAGE/Physics/rigidbody.hpp b/include/YAGE/Physics/rigidbody.hpp
index 48380dac..05d8d0ad 100644
--- a/include/YAGE/Physics/rigidbody.hpp
+++ b/include/YAGE/Physics/rigidbody.hpp
@@ -11,9 +11,9 @@ namespace yage
 class RigidBody : public ParticleBody
 {
 public:
-	RigidBody(const glm::vec2 &position=glm::vec2(0.f, 0.f),
-	          double mass=1.0,
- 	          const glm::vec2 &velocity=glm::vec2(0.f, 0.f),
+	RigidBody(const Vector2d &position=Vector2d(0, 0),
+	          double mass=1,
+ 	          const Vector2d &velocity=Vector2d(0, 0),
 	          bool gravity=true);
 };
 
diff --git a/src/body.cpp b/src/body.cpp
index 23360988..e942d617 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -8,17 +8,17 @@ const double Body::GRAVITY=-9.81;
 Body::~Body()
 {}
 
-float Body::xPosition() const
+double Body::xPosition() const
 {
-	return position_.x;
+	return position_[0];
 }
 
-float Body::yPosition() const
+double Body::yPosition() const
 {
-	return position_.y;
+	return position_[1];
 }
 
-Body::Body(const glm::vec2 &position, double mass, const glm::vec2 &velocity, bool gravity) :
+Body::Body(const Vector2d &position, double mass, const Vector2d &velocity, bool gravity) :
 	position_(position), mass_(mass), velocity_(velocity), gravity_(gravity)
 {}
 
diff --git a/src/particlebody.cpp b/src/particlebody.cpp
index f882f279..7435737c 100644
--- a/src/particlebody.cpp
+++ b/src/particlebody.cpp
@@ -1,5 +1,6 @@
 #include "Physics/particlebody.hpp"
 
+#include <iostream>
 #include <cmath>
 
 namespace yage
@@ -26,16 +27,15 @@ void ParticleBody::update()
 	Vector2d last_acceleration=acceleration_;
 
 	// update the position of the body
-	//position_.x+=velocity_.x*time_step+(0.5*last_acceleration.x*std::pow(time_step, 2));
-	//position_.y+=velocity_.y*time_step+(0.5*last_acceleration.y*std::pow(time_step, 2));
-
 	position_+=velocity_*time_step+(0.5*last_acceleration*std::pow(time_step, 2));
 
 	// update the acceleration
 	if(gravity_)
 		acceleration_=Vector2d(force_.x()/mass_, (GRAVITY+force_.y())/mass_);
 	else
-		acceleration_=Vector2d(force_.x()/mass_, force_.y()/mass_);	
+		acceleration_=Vector2d(force_.x()/mass_, force_.y()/mass_);
+
+	std::cout<<acceleration_<<"\n";
 
 	Vector2d avg_acceleration=(acceleration_+last_acceleration)/2.0;
 
diff --git a/src/rigidbody.cpp b/src/rigidbody.cpp
index f92500b8..62032b74 100644
--- a/src/rigidbody.cpp
+++ b/src/rigidbody.cpp
@@ -3,9 +3,9 @@
 namespace yage
 {
 
-RigidBody::RigidBody(const glm::vec2 &position,
+RigidBody::RigidBody(const Vector2d &position,
                      double mass,
-                     const glm::vec2 &velocity,
+                     const Vector2d &velocity,
                      bool gravity) :
 	ParticleBody(position, mass, velocity, gravity)
 {}
diff --git a/test/rigidbodytest.cpp b/test/rigidbodytest.cpp
index c5683697..11efd20d 100644
--- a/test/rigidbodytest.cpp
+++ b/test/rigidbodytest.cpp
@@ -13,6 +13,8 @@ int main(int, char**)
 
 	double ideal_position=0.5*-9.81*3*3;
 
+	std::cout<<"Ideal Position: "<<ideal_position<<"\n";
+
 	if(body.yPosition()<ideal_position*0.95 && body.yPosition()>ideal_position*1.05)
 		return 0;
 	return 1;