DART  6.10.1
OdeHeightmap-impl.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
34 
35 #include <type_traits>
37 
38 namespace dart {
39 namespace collision {
40 namespace detail {
41 
42 #define HF_THICKNESS 0.05
43 
44 //==============================================================================
45 // creates the ODE height field. Only enabled if the height data type is float.
46 template <class S>
48  const dHeightfieldDataID odeHeightfieldId,
49  const S* heights,
50  const std::size_t& width,
51  const std::size_t& height,
52  const Eigen::Matrix<S, 3, 1>& scale,
53  typename std::enable_if<std::is_same<float, S>::value>::type* = 0)
54 {
55  assert(width >= 2);
56  assert(height >= 2);
57  if ((width < 2) || (height < 2))
58  {
59  dtwarn << "Cannot create height field of dimensions " << width << "x"
60  << height << ", needs to be at least 2" << std::endl;
61  return;
62  }
63  dGeomHeightfieldDataBuildSingle(
64  odeHeightfieldId,
65  heights,
66  0,
67  (width - 1) * scale.x(), // width (in meters)
68  (height - 1) * scale.y(), // height (in meters)
69  width, // width (sampling size)
70  height, // height (sampling size)
71  scale.z(), // vertical scaling
72  0.0, // vertical offset
73  HF_THICKNESS, // vertical thickness for closing the mesh
74  0); // wrap mode
75 }
76 
77 //==============================================================================
78 // creates the ODE height field. Only enabled if the height data type is double.
79 template <class S>
81  const dHeightfieldDataID odeHeightfieldId,
82  const S* heights,
83  const std::size_t& width,
84  const std::size_t& height,
85  const Eigen::Matrix<S, 3, 1>& scale,
86  typename std::enable_if<std::is_same<double, S>::value>::type* = 0)
87 {
88  assert(width >= 2);
89  assert(height >= 2);
90  if ((width < 2) || (height < 2))
91  {
92  dtwarn << "Cannot create height field of dimensions " << width << "x"
93  << height << ", needs to be at least 2" << std::endl;
94  return;
95  }
96 
97  dGeomHeightfieldDataBuildDouble(
98  odeHeightfieldId,
99  heights,
100  0,
101  (width - 1) * scale.x(), // width (in meters)
102  (height - 1) * scale.y(), // height (in meters)
103  width, // width (sampling size)
104  height, // height (sampling size)
105  scale.z(), // vertical scaling
106  0.0, // vertical offset
107  HF_THICKNESS, // vertical thickness for closing the mesh
108  0); // wrap mode
109 }
110 
111 //==============================================================================
112 template <typename S>
114  const OdeCollisionObject* parent,
115  const dynamics::HeightmapShape<S>* heightMap)
116  : OdeGeom(parent)
117 {
118  assert(heightMap);
119 
120  // get the heightmap parameters
121  const auto& scale = heightMap->getScale();
122  const auto& heights = heightMap->getHeightField();
123 
124  // Create the ODE heightfield
125  mOdeHeightfieldId = dGeomHeightfieldDataCreate();
126 
127  // specify height field details
130  heights.data(),
131  heightMap->getWidth(),
132  heightMap->getDepth(),
133  scale);
134 
135  // Restrict the bounds of the AABB to improve efficiency
136  dGeomHeightfieldDataSetBounds(
137  mOdeHeightfieldId, heightMap->getMinHeight(), heightMap->getMaxHeight());
138 
139  // create the height field
140  mGeomId = dCreateHeightfield(0, mOdeHeightfieldId, 1);
141 
142  // rotate it so that z axis is up.
143  // remember the transform as a permanent relative transform by assigning
144  // it to the geometry.
145  dQuaternion q;
146  q[0] = sqrt(0.5);
147  q[1] = sqrt(0.5);
148  q[2] = 0;
149  q[3] = 0;
150  dGeomSetQuaternion(mGeomId, q);
151 
152  // TODO Take this out as soon as testing is finished, getting the
153  // AABB is only needed for the debug print.
154  dReal aabb[6];
155  dGeomGetAABB(mGeomId, aabb);
156  dtdbg << "ODE Heightfield AABB: min = {" << aabb[0] << ", " << aabb[2] << ", "
157  << aabb[4] << "} max = {" << aabb[1] << ", " << aabb[3] << ", "
158  << aabb[5] << "}" << std::endl;
159 }
160 
161 //==============================================================================
162 template <typename S>
164 {
165  dGeomHeightfieldDataDestroy(mOdeHeightfieldId);
166  dGeomDestroy(mGeomId);
167 }
168 
169 } // namespace detail
170 } // namespace collision
171 } // namespace dart
#define dtwarn
Output a warning message.
Definition: Console.hpp:46
#define dtdbg
Output a debug message.
Definition: Console.hpp:43
#define HF_THICKNESS
Definition: OdeHeightmap-impl.hpp:42
std::string type
Definition: SdfParser.cpp:82
Definition: OdeCollisionObject.hpp:49
Definition: OdeGeom.hpp:46
dGeomID mGeomId
ODE geom ID associated with this object.
Definition: OdeGeom.hpp:76
OdeHeightmap(const OdeCollisionObject *parent, const dynamics::HeightmapShape< S > *hs)
Constructor.
Definition: OdeHeightmap-impl.hpp:113
~OdeHeightmap() override
Destructor.
Definition: OdeHeightmap-impl.hpp:163
dHeightfieldDataID mOdeHeightfieldId
Definition: OdeHeightmap.hpp:57
const HeightField & getHeightField() const
Returns the height field.
Definition: HeightmapShape-impl.hpp:133
std::size_t getWidth() const
Returns the width dimension of the height field.
Definition: HeightmapShape-impl.hpp:168
const Vector3 & getScale() const
Returns scale of this heightmap.
Definition: HeightmapShape-impl.hpp:85
std::size_t getDepth() const
Returns the height dimension of the height field.
Definition: HeightmapShape-impl.hpp:175
S getMinHeight() const
Returns the minimum height set by setHeightField()
Definition: HeightmapShape-impl.hpp:161
S getMaxHeight() const
Returns the maximum height set by setHeightField()
Definition: HeightmapShape-impl.hpp:154
void setOdeHeightfieldDetails(const dHeightfieldDataID odeHeightfieldId, const S *heights, const std::size_t &width, const std::size_t &height, const Eigen::Matrix< S, 3, 1 > &scale, typename std::enable_if< std::is_same< float, S >::value >::type *=0)
Definition: OdeHeightmap-impl.hpp:47
double dQuaternion[4]
Definition: DARTCollide.cpp:65
Definition: BulletCollisionDetector.cpp:65