DART  6.10.1
HeightmapShape-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 <algorithm>
36 #include <cmath>
37 #include <limits>
38 #include "dart/common/Console.hpp"
40 
41 namespace dart {
42 namespace dynamics {
43 
44 //==============================================================================
45 template <typename S>
46 HeightmapShape<S>::HeightmapShape() : Shape(HEIGHTMAP), mScale(1, 1, 1)
47 {
48  static_assert(
49  std::is_same<S, float>::value || std::is_same<S, double>::value,
50  "Height field needs to be double or float");
51 }
52 
53 //==============================================================================
54 template <typename S>
55 const std::string& HeightmapShape<S>::getType() const
56 {
57  return getStaticType();
58 }
59 
60 //==============================================================================
61 template <typename S>
63 {
64  static const std::string type
65  = "HeightmapShape (" + std::string(typeid(S).name()) + ")";
66  return type;
67 }
68 
69 //==============================================================================
70 template <typename S>
72 {
73  assert(scale[0] > 0.0);
74  assert(scale[1] > 0.0);
75  assert(scale[2] > 0.0);
76  mScale = scale;
77  mIsBoundingBoxDirty = true;
78  mIsVolumeDirty = true;
79 
80  incrementVersion();
81 }
82 
83 //==============================================================================
84 template <typename S>
85 auto HeightmapShape<S>::getScale() const -> const Vector3&
86 {
87  return mScale;
88 }
89 
90 //==============================================================================
91 template <typename S>
93  const std::size_t& width,
94  const std::size_t& depth,
95  const std::vector<S>& heights)
96 {
97  assert(heights.size() == width * depth);
98  if ((width * depth) != heights.size())
99  {
100  dterr << "[HeightmapShape] Size of height field needs to be width*depth="
101  << width * depth << "\n";
102  return;
103  }
104  if (heights.empty())
105  {
106  dtwarn << "Empty height field makes no sense.\n";
107  return;
108  }
109 
110  // make heightmap data local copy
111  const Eigen::Map<const HeightField> data(heights.data(), depth, width);
112 
113  setHeightField(data);
114 }
115 
116 //==============================================================================
117 template <typename S>
119 {
120  mHeights = heights;
121 
122  mMinHeight = heights.minCoeff();
123  mMaxHeight = heights.maxCoeff();
124 
125  mIsBoundingBoxDirty = true;
126  mIsVolumeDirty = true;
127 
128  incrementVersion();
129 }
130 
131 //==============================================================================
132 template <typename S>
134 {
135  return mHeights;
136 }
137 
138 //==============================================================================
139 template <typename S>
141 {
142  return mHeights;
143 }
144 
145 //==============================================================================
146 template <typename S>
148 {
149  mHeights = mHeights.colwise().reverse().eval();
150 }
151 
152 //==============================================================================
153 template <typename S>
155 {
156  return mMaxHeight;
157 }
158 
159 //==============================================================================
160 template <typename S>
162 {
163  return mMinHeight;
164 }
165 
166 //==============================================================================
167 template <typename S>
168 std::size_t HeightmapShape<S>::getWidth() const
169 {
170  return mHeights.cols();
171 }
172 
173 //==============================================================================
174 template <typename S>
175 std::size_t HeightmapShape<S>::getDepth() const
176 {
177  return mHeights.rows();
178 }
179 
180 //==============================================================================
181 template <typename S>
182 void HeightmapShape<S>::notifyColorUpdated(const Eigen::Vector4d& /*color*/)
183 {
184  incrementVersion();
185 }
186 
187 //==============================================================================
188 template <typename S>
189 Eigen::Matrix3d HeightmapShape<S>::computeInertia(double mass) const
190 {
191  if (mIsBoundingBoxDirty)
192  {
193  updateBoundingBox();
194  }
195  return BoxShape::computeInertia(getBoundingBox().computeFullExtents(), mass);
196 }
197 
198 //==============================================================================
199 template <typename S>
201  Eigen::Vector3d& min, Eigen::Vector3d& max) const
202 {
203  const double dimX = getWidth() * mScale.x();
204  const double dimY = getDepth() * mScale.y();
205  const double dimZ = (mMaxHeight - mMinHeight) * mScale.z();
206  min = Eigen::Vector3d(-dimX * 0.5, -dimY * 0.5, mMinHeight * mScale.z());
207  max = min + Eigen::Vector3d(dimX, dimY, dimZ);
208 }
209 
210 //==============================================================================
211 template <typename S>
213 {
214  Eigen::Vector3d min;
215  Eigen::Vector3d max;
216  computeBoundingBox(min, max);
217  mBoundingBox.setMin(min);
218  mBoundingBox.setMax(max);
219  mIsBoundingBoxDirty = false;
220 }
221 
222 //==============================================================================
223 template <typename S>
225 {
226  updateBoundingBox();
227  const Eigen::Vector3d size = mBoundingBox.getMax() - mBoundingBox.getMin();
228  mVolume = size.x() * size.y() * size.z();
229  mIsVolumeDirty = false;
230 }
231 
232 } // namespace dynamics
233 } // namespace dart
#define dterr
Output an error message.
Definition: Console.hpp:49
#define dtwarn
Output a warning message.
Definition: Console.hpp:46
std::string type
Definition: SdfParser.cpp:82
std::string * name
Definition: SkelParser.cpp:1697
Shape for a height map.
Definition: HeightmapShape.hpp:47
S S
Definition: HeightmapShape.hpp:49
HeightmapShape()
Constructor.
Definition: HeightmapShape-impl.hpp:46
const std::string & getType() const override
Returns a string representing the shape type.
Definition: HeightmapShape-impl.hpp:55
Eigen::Matrix< S, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > HeightField
Definition: HeightmapShape.hpp:53
Eigen::Matrix< S, 3, 1 > Vector3
Definition: HeightmapShape.hpp:51
Definition: Shape.hpp:54
Definition: BulletCollisionDetector.cpp:65