From a3d932b09e2ebd7b3d51bdb0baa42feebac1c8de Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Fri, 7 Aug 2020 17:50:33 -0700
Subject: [PATCH 1/2] Test for dart bug with asymmetric heightmaps

The heightmap_valley.png used in the heightmap tests for ODE
and DART is symmetric, which obscures the fact that DART
is not treating asymmetric heightmaps properly.
This adds a test using worlds/heightmap.world and
heightmap_bowl.png to illustrate the bug.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 test/integration/heightmap.cc | 78 +++++++++++++++++++++++++++++++++++
 worlds/heightmap.world        |  5 +++
 2 files changed, 83 insertions(+)

diff --git a/test/integration/heightmap.cc b/test/integration/heightmap.cc
index ef8f1db5dd..4e574af08f 100644
--- a/test/integration/heightmap.cc
+++ b/test/integration/heightmap.cc
@@ -69,6 +69,9 @@ class HeightmapTest : public ServerFixture,
   //  is the collision detector to use in DART. Can be fcl, dart, bullet or ode.
   public: void TerrainCollision(const std::string &_physicsEngine,
                                 const std::string &_dartCollision = "");
+  // \brief Test dropping boxes on asymmetric terrain
+  // \param[in] _physics the physics engine to test
+  public: void TerrainCollisionAsymmetric(const std::string &_physics);
 
   /// \brief Test loading a heightmap that has no visuals
   public: void NoVisual();
@@ -884,6 +887,75 @@ void HeightmapTest::TerrainCollision(const std::string &_physicsEngine,
   EXPECT_GE(spherePose.Pos().Z(), (minHeight + radius*0.99));
 }
 
+/////////////////////////////////////////////////
+void HeightmapTest::TerrainCollisionAsymmetric(const std::string &_physics)
+{
+  if (_physics == "bullet")
+  {
+    gzerr << "Skipping test for bullet. See issue #2506" << std::endl;
+    return;
+  }
+
+  if (_physics == "simbody")
+  {
+    // SimbodyHeightmapShape unimplemented.
+    gzerr << "Aborting test for " << _physics << std::endl;
+    return;
+  }
+
+  Load("worlds/heightmap.world", true, _physics);
+
+  physics::WorldPtr world = physics::get_world("default");
+  ASSERT_NE(world, nullptr);
+
+  if (_physics == "dart")
+  {
+#ifdef HAVE_DART
+    physics::PhysicsEnginePtr engine = world->Physics();
+    ASSERT_NE(engine, nullptr);
+    physics::DARTPhysicsPtr dartEngine
+      = boost::dynamic_pointer_cast<physics::DARTPhysics>(engine);
+    ASSERT_NE(dartEngine, nullptr);
+    std::string cd = dartEngine->CollisionDetectorInUse();
+    ASSERT_FALSE(cd.empty());
+    if (cd != "bullet")
+    {
+      // the test only works if DART uses bullet as a collision detector at the
+      // moment.
+      gzerr << "Aborting test for dart, see issue #909 and pull request #2956"
+            << std::endl;
+      return;
+    }
+#else
+    gzerr << "Have no DART installed, skipping test for DART." << std::endl;
+    return;
+#endif
+  }
+
+  // each box has an initial z position of 10. meters
+  physics::ModelPtr box1 = GetModel("box1");
+  physics::ModelPtr box2 = GetModel("box2");
+  physics::ModelPtr box3 = GetModel("box3");
+  physics::ModelPtr box4 = GetModel("box4");
+  ASSERT_NE(box1, nullptr);
+  ASSERT_NE(box2, nullptr);
+  ASSERT_NE(box3, nullptr);
+  ASSERT_NE(box4, nullptr);
+  EXPECT_GE(box1->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box2->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box3->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box4->WorldPose().Pos().Z(), 9.9);
+
+  // step the world and verify that only box2 falls
+  world->Step(1000);
+
+  EXPECT_GE(box1->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box3->WorldPose().Pos().Z(), 9.9);
+  EXPECT_GE(box4->WorldPose().Pos().Z(), 9.9);
+
+  EXPECT_LT(box2->WorldPose().Pos().Z(), 5.5);
+}
+
 /////////////////////////////////////////////////
 TEST_F(HeightmapTest, NotSquareImage)
 {
@@ -1055,6 +1127,12 @@ TEST_F(HeightmapTest, TerrainCollisionDartBullet)
   TerrainCollision("dart", "bullet");
 }
 
+/////////////////////////////////////////////////
+TEST_P(HeightmapTest, TerrainCollisionAsymmetric)
+{
+  TerrainCollisionAsymmetric(GetParam());
+}
+
 /////////////////////////////////////////////////
 //
 // Disabled: segfaults ocassionally
diff --git a/worlds/heightmap.world b/worlds/heightmap.world
index 3cddc00edf..b102a80296 100644
--- a/worlds/heightmap.world
+++ b/worlds/heightmap.world
@@ -1,6 +1,11 @@
 <?xml version="1.0" ?>
 <sdf version="1.5">
   <world name="default">
+    <physics type="ode">
+      <dart>
+        <collision_detector>bullet</collision_detector>
+      </dart>
+    </physics>
     <!-- A global light source -->
     <include>
       <uri>model://sun</uri>

From cf44d5275bf551b20380f3143d6372271433ed4a Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Fri, 7 Aug 2020 17:54:41 -0700
Subject: [PATCH 2/2] DARTHeightmapShape: set flipY = false

This variable was uninitialized and was causing problems
with asymmetric heightmaps.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 gazebo/physics/dart/DARTHeightmapShape.cc | 1 +
 1 file changed, 1 insertion(+)

diff --git a/gazebo/physics/dart/DARTHeightmapShape.cc b/gazebo/physics/dart/DARTHeightmapShape.cc
index 59fbe55f50..b86c9ba07d 100644
--- a/gazebo/physics/dart/DARTHeightmapShape.cc
+++ b/gazebo/physics/dart/DARTHeightmapShape.cc
@@ -30,6 +30,7 @@ DARTHeightmapShape::DARTHeightmapShape(DARTCollisionPtr _parent)
   : HeightmapShape(_parent),
     dataPtr(new DARTHeightmapShapePrivate<HeightmapShape::HeightType>())
 {
+  this->flipY = false;
 }
 
 //////////////////////////////////////////////////