Merge remote-tracking branch 'origin/GamePhysics' into Camera

Code/GamePhysics/Implementation/PhysicsAPI_Impl.cpp

@@ -24,6 +24,11 @@ API_Impl::API_Impl()
     this->dispatcher = NULL;
 	this->solver = NULL;
 	this->dynamicsWorld = NULL;
+
+	this->timeStep = 1.0f/120.0f;
+
+	this->gravityPoint = Float3(0.0f, 0.0f, 0.0f);
+	this->gravity = 10.0f;
 }
 
 API_Impl::~API_Impl() 
@@ -39,13 +44,23 @@ API_Impl::~API_Impl()
     delete this->broadphase;
 	this->broadphase = NULL;
 
-	for(int i = 0; i < this->customBodies.size(); i++)
+	for(unsigned int i = 0; i < this->customBodies.size(); i++)
 	{
 		delete this->customBodies[i];
 		this->customBodies[i] = NULL;
 	}
 }
 
+void API_Impl::SetGravityPoint(::Oyster::Math::Float3 gravityPoint)
+{
+	this->gravityPoint = gravityPoint;
+}
+
+void API_Impl::SetGravity(float gravity)
+{
+	this->gravity = gravity;
+}
+
 // Bullet physics
 ICustomBody* API_Impl::AddCollisionSphere(float radius, ::Oyster::Math::Float4 rotation, ::Oyster::Math::Float3 position, float mass, float restitution, float staticFriction, float dynamicFriction)
 {
@@ -164,27 +179,38 @@ ICustomBody* API_Impl::AddCollisionCylinder(::Oyster::Math::Float3 halfSize, ::O
 	return body;
 }
 
+void API_Impl::SetTimeStep(float timeStep)
+{
+	this->timeStep = timeStep;
+}
+
 void API_Impl::UpdateWorld()
 {
-	this->dynamicsWorld->stepSimulation(1.0f/60.0f, 1.0f, 1.0f/60.0f);
+	for(unsigned int i = 0; i < this->customBodies.size(); i++ )
+	{
+		this->customBodies[i]->SetGravity(-(this->customBodies[i]->GetState().centerPos - this->gravityPoint).GetNormalized()*this->gravity);	
+	}
+
+	this->dynamicsWorld->stepSimulation(this->timeStep, 1, this->timeStep);
 
 	ICustomBody::State state;
 
 	for(unsigned int i = 0; i < this->customBodies.size(); i++ )
 	{
+		SimpleRigidBody* simpleBody = dynamic_cast<SimpleRigidBody*>(this->customBodies[i]);
 		btTransform trans;
-		dynamic_cast<SimpleRigidBody*>(this->customBodies[i])->GetMotionState()->getWorldTransform(trans);
+		simpleBody->GetMotionState()->getWorldTransform(trans);
 		this->customBodies[i]->SetPosition(Float3(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z()));
 		this->customBodies[i]->SetRotation(Quaternion(Float3(trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()), trans.getRotation().w()));
 		
-		if(dynamic_cast<SimpleRigidBody*>(this->customBodies[i])->GetRigidBody()->getActivationState() == ACTIVE_TAG)
+		if(simpleBody->GetRigidBody()->getActivationState() == ACTIVE_TAG)
 		{
-			dynamic_cast<SimpleRigidBody*>(this->customBodies[i])->CallSubscription_Move();
+			simpleBody->CallSubscription_Move();
 		}
 	}
 
 	int numManifolds = this->dynamicsWorld->getDispatcher()->getNumManifolds();
-	for (int i=0;i<numManifolds;i++)
+	for (int i = 0; i < numManifolds; i++)
 	{
 		btPersistentManifold* contactManifold =  this->dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
 		const btCollisionObject* obA = contactManifold->getBody0();
@@ -195,18 +221,6 @@ void API_Impl::UpdateWorld()
 
 		dynamic_cast<SimpleRigidBody*>(bodyA)->CallSubscription_AfterCollisionResponse(bodyA, bodyB, 0.0f);
 		dynamic_cast<SimpleRigidBody*>(bodyB)->CallSubscription_AfterCollisionResponse(bodyB, bodyA, 0.0f);
-
-		int numContacts = contactManifold->getNumContacts();
-		for (int j=0;j<numContacts;j++)
-		{
-			btManifoldPoint& pt = contactManifold->getContactPoint(j);
-			if (pt.getDistance()<0.f)
-			{   
-				const btVector3& ptA = pt.getPositionWorldOnA();
-				const btVector3& ptB = pt.getPositionWorldOnB();
-				const btVector3& normalOnB = pt.m_normalWorldOnB;
-			}
-		}
 	}
 
 }
@@ -257,9 +271,7 @@ namespace Oyster
 			}
 
 			void EventAction_AfterCollisionResponse( const ::Oyster::Physics::ICustomBody *proto, const ::Oyster::Physics::ICustomBody *deuter, ::Oyster::Math::Float kineticEnergyLoss )
-			{ /* Do nothing except returning business as usual. */
-				
-			}
+			{ /* Do nothing except returning business as usual. */ }
 
 			void EventAction_Move( const ::Oyster::Physics::ICustomBody *object )
 			{ /* Do nothing. */ }

Code/GamePhysics/Implementation/PhysicsAPI_Impl.h

@@ -21,11 +21,16 @@ namespace Oyster
 			void MoveToLimbo( const ICustomBody* objRef );
 			void ReleaseFromLimbo( const ICustomBody* objRef );
 
+			void SetGravityPoint(::Oyster::Math::Float3 gravityPoint);
+			void SetGravity(float gravity);
+
 			// Bullet physics
 			ICustomBody* AddCollisionSphere(float radius, ::Oyster::Math::Float4 rotation, ::Oyster::Math::Float3 position, float mass, float restitution, float staticFriction, float dynamicFriction);
 			ICustomBody* AddCollisionBox(::Oyster::Math::Float3 halfSize, ::Oyster::Math::Float4 rotation, ::Oyster::Math::Float3 position, float mass, float restitution, float staticFriction, float dynamicFriction);
 			ICustomBody* AddCollisionCylinder(::Oyster::Math::Float3 halfSize, ::Oyster::Math::Float4 rotation, ::Oyster::Math::Float3 position, float mass, float restitution, float staticFriction, float dynamicFriction);
 
+			void SetTimeStep(float timeStep);
+
 			void UpdateWorld();
 
 			void ApplyEffect( const Oyster::Collision3D::ICollideable& collideable, void* args, void(hitAction)(ICustomBody*, void*) );
@@ -37,6 +42,11 @@ namespace Oyster
 			btSequentialImpulseConstraintSolver* solver;
 			btDiscreteDynamicsWorld* dynamicsWorld;
 			std::vector<ICustomBody*> customBodies;
+
+			float timeStep;
+
+			::Oyster::Math::Float3 gravityPoint;
+			float gravity;
 		};
 
 		namespace Default

Code/GamePhysics/Implementation/SimpleRigidBody.cpp

@@ -134,10 +134,80 @@ void SimpleRigidBody::SetRotation(Float3 eulerAngles)
 	this->state.quaternion = Quaternion(Float3(trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()), trans.getRotation().w());
 }
 
+void SimpleRigidBody::SetAngularFactor(Float factor)
+{
+	this->rigidBody->setAngularFactor(factor);
+}
+
+void SimpleRigidBody::SetGravity(Float3 gravity)
+{
+	this->rigidBody->setGravity(btVector3(gravity.x, gravity.y, gravity.z));
+	this->gravity = gravity;
+}
+
+void SimpleRigidBody::SetUpAndRight(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 right)
+{
+	btTransform trans;
+	btMatrix3x3 rotation;
+	btVector3 upVector(up.x, up.y, up.z);
+	btVector3 rightVector(right.x, right.y, right.z);
+	rotation[1] = upVector.normalized();
+	rotation[0] = rightVector.normalized();
+	rotation[2] = rightVector.cross(upVector).normalized();
+
+	trans = this->rigidBody->getWorldTransform();
+	trans.setBasis(rotation);
+	this->rigidBody->setWorldTransform(trans);
+
+	btQuaternion quaternion;
+	quaternion = trans.getRotation();
+	this->state.quaternion = Quaternion(Float3(quaternion.x(), quaternion.y(), quaternion.z()), quaternion.w());
+}
+
+void SimpleRigidBody::SetUpAndForward(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 forward)
+{
+	btTransform trans;
+	btMatrix3x3 rotation;
+	btVector3 upVector(up.x, up.y, up.z);
+	btVector3 forwardVector(forward.x, forward.y, forward.z);
+	rotation[1] = upVector.normalized();
+	rotation[2] = forwardVector.normalized();
+	rotation[0] = forwardVector.cross(upVector).normalized();
+	trans = this->rigidBody->getWorldTransform();
+	trans.setBasis(rotation);
+	this->rigidBody->setWorldTransform(trans);
+
+	btQuaternion quaternion;
+	quaternion = trans.getRotation();
+	this->state.quaternion = Quaternion(Float3(quaternion.x(), quaternion.y(), quaternion.z()), quaternion.w());
+}
+
 Float4x4 SimpleRigidBody::GetRotation() const
 {
 	return this->state.GetRotation();
 }
+Float4 SimpleRigidBody::GetRotationAsAngularAxis()
+{
+	Float4 axis = Float4::null;
+	Float s = sqrtf(1 - this->state.quaternion.real*this->state.quaternion.real);
+
+	axis.w = 2*acos(this->state.quaternion.real*this->state.quaternion.real);
+
+	if(1 - this->state.quaternion.real > 0.001f)
+	{
+		axis.x = this->state.quaternion.imaginary.x/s;
+		axis.y = this->state.quaternion.imaginary.y/s;
+		axis.z = this->state.quaternion.imaginary.z/s;
+	}
+	else
+	{
+		axis.x = this->state.quaternion.imaginary.x;
+		axis.y = this->state.quaternion.imaginary.y;
+		axis.z = this->state.quaternion.imaginary.z;
+	}
+
+	return axis;
+}
 
 Float4x4 SimpleRigidBody::GetOrientation() const
 {

Code/GamePhysics/Implementation/SimpleRigidBody.h

@@ -26,12 +26,19 @@ namespace Oyster
 			void SetSubscription(EventAction_Move function);
 
 			void SetLinearVelocity(Math::Float3 velocity);
-			void SetPosition(::Oyster::Math::Float3 position);
+			void SetPosition(Math::Float3 position);
 			void SetRotation(Math::Float4 quaternion);
-			void SetRotation(::Oyster::Math::Quaternion quaternion);
+			void SetRotation(Math::Quaternion quaternion);
 			void SetRotation(Math::Float3 eulerAngles);
+			void SetAngularFactor(Math::Float factor);
+
+			void SetGravity(Math::Float3 gravity);
+
+			void SetUpAndRight(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 right);
+			void SetUpAndForward(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 forward);
 
 			Math::Float4x4 GetRotation() const;
+			Math::Float4 GetRotationAsAngularAxis();
 			Math::Float4x4 GetOrientation() const;
 			Math::Float4x4 GetView() const;
 			Math::Float4x4 GetView( const ::Oyster::Math::Float3 &offset ) const;
@@ -47,6 +54,7 @@ namespace Oyster
 			void* GetCustomTag() const;
 
 			private:
+
 			btCollisionShape* collisionShape;
 			btDefaultMotionState* motionState;
 			btRigidBody* rigidBody;
@@ -57,6 +65,8 @@ namespace Oyster
 			EventAction_Move onMovement;
 
 			void *customTag;
+
+			::Oyster::Math::Float3 gravity;
 		};
 	} 
 }

Code/GamePhysics/PhysicsAPI.h

@@ -78,12 +78,16 @@ namespace Oyster
 			 ********************************************************/
 			virtual void ReleaseFromLimbo( const ICustomBody* objRef ) = 0;
 
+			virtual void SetGravityPoint(::Oyster::Math::Float3 gravityPoint) = 0; 
+			virtual void SetGravity(float gravity) = 0; 
 
 			// Bullet physics
 			virtual ICustomBody* AddCollisionSphere(float radius, ::Oyster::Math::Float4 rotation, ::Oyster::Math::Float3 position, float mass, float restitution, float staticFriction, float dynamicFriction) = 0;
 			virtual ICustomBody* AddCollisionBox(::Oyster::Math::Float3 halfSize, ::Oyster::Math::Float4 rotation, ::Oyster::Math::Float3 position, float mass, float restitution, float staticFriction, float dynamicFriction) = 0;
 			virtual ICustomBody* AddCollisionCylinder(::Oyster::Math::Float3 halfSize, ::Oyster::Math::Float4 rotation, ::Oyster::Math::Float3 position, float mass, float restitution, float staticFriction, float dynamicFriction) = 0;
 
+			virtual void SetTimeStep(float timeStep) = 0;
+
 			virtual void UpdateWorld() = 0;
 
 
@@ -135,11 +139,18 @@ namespace Oyster
 			virtual void SetRotation(::Oyster::Math::Float4 quaternion) = 0;
 			virtual void SetRotation(::Oyster::Math::Quaternion quaternion) = 0;
 			virtual void SetRotation(::Oyster::Math::Float3 eulerAngles) = 0;
+			virtual void SetAngularFactor(::Oyster::Math::Float factor) = 0;
 
-			::Oyster::Math::Float4x4 GetRotation() const;
-			::Oyster::Math::Float4x4 GetOrientation() const;
-			::Oyster::Math::Float4x4 GetView() const;
-			::Oyster::Math::Float4x4 GetView( const ::Oyster::Math::Float3 &offset ) const;
+			virtual void SetGravity(::Oyster::Math::Float3 gravity) = 0;
+
+			virtual void SetUpAndRight(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 right) = 0;
+			virtual void SetUpAndForward(::Oyster::Math::Float3 up, ::Oyster::Math::Float3 forward) = 0;
+
+			virtual ::Oyster::Math::Float4x4 GetRotation() const = 0;
+			virtual ::Oyster::Math::Float4 GetRotationAsAngularAxis() = 0;
+			virtual ::Oyster::Math::Float4x4 GetOrientation() const = 0;
+			virtual ::Oyster::Math::Float4x4 GetView() const = 0;
+			virtual ::Oyster::Math::Float4x4 GetView(const ::Oyster::Math::Float3 &offset) const = 0;
 
 			/********************************************************
 			 * @return the void pointer set by SetCustomTag.

Code/OysterMath/OysterMath.cpp

@@ -96,6 +96,52 @@ namespace Oyster { namespace Math3D
 	//	return ::LinearAlgebra3D::ExtractAngularAxis( orientationMatrix );
 	//}
 
+	Float4 QuaternionToAngularAxis(Float4 quaternion)
+	{
+		Float4 axis = Float4::null;
+		Float s = sqrtf(1 - quaternion.w*quaternion.w);
+
+		axis.w = 2*acos(quaternion.w*quaternion.w);
+
+		if(1 - quaternion.w > 0.001f)
+		{
+			axis.x = quaternion.x/s;
+			axis.y = quaternion.y/s;
+			axis.z = quaternion.z/s;
+		}
+		else
+		{
+			axis.x = quaternion.x;
+			axis.y = quaternion.y;
+			axis.z = quaternion.z;
+		}
+
+		return axis;
+	}
+
+	Float4 QuaternionToAngularAxis(Quaternion quaternion)
+	{
+		Float4 axis = Float4::null;
+		Float s = sqrtf(1 - quaternion.real*quaternion.real);
+
+		axis.w = 2*acos(quaternion.real*quaternion.real);
+
+		if(1 - quaternion.real > 0.001f)
+		{
+			axis.x = quaternion.imaginary.x/s;
+			axis.y = quaternion.imaginary.y/s;
+			axis.z = quaternion.imaginary.z/s;
+		}
+		else
+		{
+			axis.x = quaternion.imaginary.x;
+			axis.y = quaternion.imaginary.y;
+			axis.z = quaternion.imaginary.z;
+		}
+
+		return axis;
+	}
+
 	Float4x4 & TranslationMatrix( const Float3 &position, Float4x4 &targetMem )
 	{
 		return ::LinearAlgebra3D::TranslationMatrix( position, targetMem );

Code/OysterMath/OysterMath.h

@@ -149,6 +149,12 @@ namespace Oyster { namespace Math3D //! Oyster's native math library specialized
 	////! Extracts the angularAxis from orientationMatrix
 	//Float4 ExtractAngularAxis( const Float4x4 &orientationMatrix );
 
+	//! Converts a quaternion as Float4 to angular axis as Float4
+	Float4 QuaternionToAngularAxis(Float4 quaternion);
+
+	//! Converts a quaternion to angular axis as Float4
+	Float4 QuaternionToAngularAxis(Quaternion quaternion);
+
 	//! Sets and returns targetMem to a translationMatrix with position as translation. 
 	Float4x4 & TranslationMatrix( const Float3 &position, Float4x4 &targetMem = Float4x4() );