From 7dae30718b6b0b9d7085b4a02dc4b7549e80c20b Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 8 Feb 2016 22:27:11 +0100
Subject: [PATCH] Working on the testbed application (raycast and heightfield
 scenes)

---
 testbed/common/HeightField.cpp                |  2 +-
 .../scenes/heightfield/HeightFieldScene.cpp   |  3 +--
 testbed/scenes/raycast/RaycastScene.cpp       | 20 +++++++++++++++++++
 testbed/scenes/raycast/RaycastScene.h         |  4 +++-
 4 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/testbed/common/HeightField.cpp b/testbed/common/HeightField.cpp
index d628dce2..d32f85bc 100644
--- a/testbed/common/HeightField.cpp
+++ b/testbed/common/HeightField.cpp
@@ -208,7 +208,7 @@ void HeightField::generateGraphicsMesh() {
         for (int j=0; j<HEIGHTFIELD_LENGTH; j++) {
 
             float height = mHeightData[i][j] - (mMaxHeight - mMinHeight) * 0.5f;
-            openglframework::Vector3 vertex(-HEIGHTFIELD_WIDTH * 0.5f + i, height, -HEIGHTFIELD_LENGTH * 0.5 + j);
+            openglframework::Vector3 vertex(-(HEIGHTFIELD_WIDTH - 1) * 0.5f + i, height, -(HEIGHTFIELD_LENGTH - 1) * 0.5 + j);
 
             mVertices.push_back(vertex);
 
diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp
index d2132c86..01d7613f 100644
--- a/testbed/scenes/heightfield/HeightFieldScene.cpp
+++ b/testbed/scenes/heightfield/HeightFieldScene.cpp
@@ -52,7 +52,6 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
     // ---------- Create the cube ----------- //
 
     // Position
-    rp3d::decimal radius = 2.0;
     openglframework::Vector3 spherePos(15, 10, 0);
 
     // Create a sphere and a corresponding rigid in the dynamics world
@@ -165,7 +164,7 @@ void HeightFieldScene::reset() {
     rp3d::Transform transform(rp3d::Vector3(0, 0, 0), initOrientation);
     mHeightField->resetTransform(transform);
 
-    rp3d::Vector3 spherePos(2, 15, 0);
+    rp3d::Vector3 spherePos(-3.6, 15, 1.7);
     rp3d::Transform sphereTransform(spherePos, initOrientation);
     mBox->resetTransform(sphereTransform);
 }
diff --git a/testbed/scenes/raycast/RaycastScene.cpp b/testbed/scenes/raycast/RaycastScene.cpp
index 2f5846ed..d6bd0af4 100644
--- a/testbed/scenes/raycast/RaycastScene.cpp
+++ b/testbed/scenes/raycast/RaycastScene.cpp
@@ -132,6 +132,16 @@ RaycastScene::RaycastScene(const std::string& name)
     mConcaveMesh->setColor(mGreyColorDemo);
     mConcaveMesh->setSleepingColor(mRedColorDemo);
 
+    // ---------- Heightfield ---------- //
+    openglframework::Vector3 position9(0, 0, 0);
+
+    // Create a convex mesh and a corresponding collision body in the dynamics world
+    mHeightField = new HeightField(position9, mCollisionWorld);
+
+    // Set the color
+    mHeightField->setColor(mGreyColorDemo);
+    mHeightField->setSleepingColor(mRedColorDemo);
+
     // Create the lines that will be used for raycasting
     createLines();
 
@@ -184,6 +194,7 @@ void RaycastScene::changeBody() {
     mConvexMesh->getCollisionBody()->setIsActive(false);
     mDumbbell->getCollisionBody()->setIsActive(false);
     mConcaveMesh->getCollisionBody()->setIsActive(false);
+    mHeightField->getCollisionBody()->setIsActive(false);
 
     switch(mCurrentBodyIndex) {
         case 0: mSphere->getCollisionBody()->setIsActive(true);
@@ -202,6 +213,8 @@ void RaycastScene::changeBody() {
                 break;
         case 7: mConcaveMesh->getCollisionBody()->setIsActive(true);
                 break;
+        case 8: mHeightField->getCollisionBody()->setIsActive(true);
+                break;
 
     }
 }
@@ -259,6 +272,12 @@ RaycastScene::~RaycastScene() {
     // Destroy the convex mesh
     delete mConcaveMesh;
 
+    // Destroy the corresponding rigid body from the dynamics world
+    mCollisionWorld->destroyCollisionBody(mHeightField->getCollisionBody());
+
+    // Destroy the convex mesh
+    delete mHeightField;
+
     mRaycastManager.resetPoints();
 
     // Destroy the static data for the visual contact points
@@ -365,6 +384,7 @@ void RaycastScene::renderSinglePass(openglframework::Shader& shader,
     if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(shader, worldToCameraMatrix);
     if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(shader, worldToCameraMatrix);
     if (mConcaveMesh->getCollisionBody()->isActive()) mConcaveMesh->render(shader, worldToCameraMatrix);
+    if (mHeightField->getCollisionBody()->isActive()) mHeightField->render(shader, worldToCameraMatrix);
 
     shader.unbind();
 }
diff --git a/testbed/scenes/raycast/RaycastScene.h b/testbed/scenes/raycast/RaycastScene.h
index fa62ab5a..7fc7617f 100644
--- a/testbed/scenes/raycast/RaycastScene.h
+++ b/testbed/scenes/raycast/RaycastScene.h
@@ -40,6 +40,7 @@
 #include "Line.h"
 #include "ConvexMesh.h"
 #include "ConcaveMesh.h"
+#include "HeightField.h"
 #include "Dumbbell.h"
 #include "VisualContactPoint.h"
 #include "../common/Viewer.h"
@@ -59,7 +60,7 @@ const float CAPSULE_HEIGHT = 5.0f;
 const float DUMBBELL_HEIGHT = 5.0f;
 const int NB_RAYS = 100;
 const float RAY_LENGTH = 30.0f;
-const int NB_BODIES = 8;
+const int NB_BODIES = 9;
 
 // Raycast manager
 class RaycastManager : public rp3d::RaycastCallback {
@@ -147,6 +148,7 @@ class RaycastScene : public SceneDemo {
         ConvexMesh* mConvexMesh;
         Dumbbell* mDumbbell;
         ConcaveMesh* mConcaveMesh;
+        HeightField* mHeightField;
 
         /// Collision world used for the physics simulation
         rp3d::CollisionWorld* mCollisionWorld;