Merge pull request #273 from jcarpent/topic/hfield

Fix computation of aabb_center
coal-library · Mar 16, 2022 · 4512387 · 4512387
2 parents 5a96f2c + c23ca3a
commit 4512387
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 4 deletions.
diff --git a/cmake b/cmake
diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h
@@ -273,12 +273,13 @@ class HPP_FCL_DLLAPI HeightField
   /// @brief Compute the AABB for the HeightField, used for broad-phase collision
   void computeLocalAABB()
   {
-    Vec3f A(x_grid[0],y_grid[0],min_height);
-    Vec3f B(x_grid[x_grid.size()-1],y_grid[y_grid.size()-1],max_height);
-    AABB aabb_(A,B);
+    const Vec3f A(x_grid[0],y_grid[0],min_height);
+    const Vec3f B(x_grid[x_grid.size()-1],y_grid[y_grid.size()-1],max_height);
+    const AABB aabb_(A,B);
 
     aabb_radius = (A-B).norm()/2.;
     aabb_local = aabb_;
+    aabb_center = aabb_.center();
   }
 
   /// \brief Comparison operators