Add raycasting

core/Rigidbody.cpp

@@ -45,6 +45,7 @@ Rigidbody *Rigidbody::Instance() {
   rigidbody->collisionShape = collisionShape;
   rigidbody->btBody = new btRigidBody(rigidbody->mass, new RigidbodyMotionState(rigidbody), rigidbody->collisionShape);
   rigidbody->btBody->setAngularFactor(character ? 0.0f : 1.0f);
+  rigidbody->btBody->setUserPointer(rigidbody);
 
   World *world = World::GetWorld();
   world->AddRigidbody(rigidbody);
@@ -119,8 +120,8 @@ void RigidbodyMotionState::getWorldTransform(btTransform &worldTrans) const {
 void RigidbodyMotionState::setWorldTransform(const btTransform &worldTrans) {
   Transform transform = rigidbody->GetTransform();
   transform.position = Vector3(worldTrans.getOrigin().getX(),
-					  worldTrans.getOrigin().getY(),
-					  worldTrans.getOrigin().getZ());
+			       worldTrans.getOrigin().getY(),
+			       worldTrans.getOrigin().getZ());
   
   worldTrans.getRotation().getEulerZYX(transform.rotation.z,
 				       transform.rotation.y,

core/World.cpp

@@ -37,6 +37,26 @@ void World::Step(float delta) {
   btWorld->stepSimulation(delta);
 }
 
+RaycastResult World::Raycast(const Vector3 &from, const Vector3 &to) {
+  RaycastResult rcr;
+  btVector3 btFrom = btVector3(from.x, from.y, from.z);
+  btVector3 btTo = btVector3(to.x, to.y, to.z);
+  btCollisionWorld::ClosestRayResultCallback rayCallback(btFrom, btTo);
+  rayCallback.m_collisionFilterGroup = -1; // Everything
+  btWorld->rayTest(btFrom, btTo, rayCallback);
+  rcr.hit = rayCallback.hasHit();
+  if (rcr.hit) {
+    rcr.position = Vector3(rayCallback.m_hitPointWorld.getX(),
+			   rayCallback.m_hitPointWorld.getY(),
+			   rayCallback.m_hitPointWorld.getZ());
+    rcr.normal = Vector3(rayCallback.m_hitNormalWorld.getX(),
+			 rayCallback.m_hitNormalWorld.getY(),
+			 rayCallback.m_hitNormalWorld.getZ());
+    rcr.object = (Rigidbody*) rayCallback.m_collisionObject->getUserPointer();
+  }
+  return rcr;
+}
+
 World* World::world { new World() };
 
 World::World() {

core/World.h

@@ -27,6 +27,13 @@
 
 #include "Rigidbody.h"
 
+struct RaycastResult {
+  bool hit;
+  Vector3 position;
+  Vector3 normal;
+  Rigidbody *object;
+};
+
 /**
    World is the object that performs the rigidbody physics simulation. Presently there is only one world.
 */
@@ -46,8 +53,11 @@ public:
      @param delta the time that has passed since the last physics update
   */
   void Step(float delta);
+
+  RaycastResult Raycast(const Vector3 &from, const Vector3 &to);
 private:
   static World* world;
+  // Some hocus pocus
   btDiscreteDynamicsWorld *btWorld;
   btDefaultCollisionConfiguration *collisionConfiguration;
   btCollisionDispatcher *dispatcher;
@@ -55,6 +65,7 @@ private:
   btSequentialImpulseConstraintSolver *solver;
   World();
   ~World();
+  
 };
 
 #endif /* WORLD_H */