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
33 #include <cmath>
34
35 #include <dart/dart.hpp>
36 #include <dart/external/imgui/imgui.h>
37 #include <dart/gui/osg/osg.hpp>
38 #include <dart/utils/urdf/urdf.hpp>
39 #include <dart/utils/utils.hpp>
40
41 using namespace dart;
42 using namespace dart::common;
43 using namespace dart::dynamics;
44 using namespace dart::math;
45
46 template <typename S>
createHeightmapShape(std::size_t xResolution=20u,std::size_t yResolution=20u,S xSize=S (2),S ySize=S (2),S zMin=S (0.0),S zMax=S (0.1))47 dynamics::ShapePtr createHeightmapShape(
48 std::size_t xResolution = 20u,
49 std::size_t yResolution = 20u,
50 S xSize = S(2),
51 S ySize = S(2),
52 S zMin = S(0.0),
53 S zMax = S(0.1))
54 {
55 using Vector3 = Eigen::Matrix<S, 3, 1>;
56
57 typename HeightmapShape<S>::HeightField data(yResolution, xResolution);
58 for (auto i = 0u; i < yResolution; ++i)
59 {
60 for (auto j = 0u; j < xResolution; ++j)
61 {
62 data(i, j) = math::Random::uniform(zMin, zMax);
63 }
64 }
65 auto scale = Vector3(xSize / xResolution, ySize / yResolution, 1);
66
67 auto terrainShape = std::make_shared<HeightmapShape<S>>();
68 terrainShape->setScale(scale);
69 terrainShape->setHeightField(data);
70
71 return terrainShape;
72 }
73
74 template <typename S>
createHeightmapFrame(std::size_t xResolution=20u,std::size_t yResolution=20u,S xSize=S (2),S ySize=S (2),S zMin=S (0.0),S zMax=S (0.1))75 dynamics::SimpleFramePtr createHeightmapFrame(
76 std::size_t xResolution = 20u,
77 std::size_t yResolution = 20u,
78 S xSize = S(2),
79 S ySize = S(2),
80 S zMin = S(0.0),
81 S zMax = S(0.1))
82 {
83 auto terrainFrame = SimpleFrame::createShared(Frame::World());
84 auto tf = terrainFrame->getRelativeTransform();
85 tf.translation()[0] = -static_cast<double>(xSize) / 2.0;
86 tf.translation()[1] = +static_cast<double>(ySize) / 2.0;
87 terrainFrame->setRelativeTransform(tf);
88
89 terrainFrame->createVisualAspect();
90
91 // TODO(JS): Remove?
92 auto terrainShape = createHeightmapShape(
93 xResolution, yResolution, xSize, ySize, zMin, zMax);
94 terrainFrame->setShape(terrainShape);
95
96 return terrainFrame;
97 }
98
99 class HeightmapWorld : public gui::osg::WorldNode
100 {
101 public:
HeightmapWorld(simulation::WorldPtr world)102 explicit HeightmapWorld(simulation::WorldPtr world)
103 : gui::osg::WorldNode(std::move(world))
104 {
105 // Do nothing
106 }
107
108 // Triggered at the beginning of each simulation step
customPreStep()109 void customPreStep() override
110 {
111 // Do nothing
112 }
113
114 protected:
115 };
116
117 template <typename S>
118 class HeightmapWidget : public dart::gui::osg::ImGuiWidget
119 {
120 public:
HeightmapWidget(dart::gui::osg::ImGuiViewer * viewer,HeightmapWorld * node,dynamics::SimpleFramePtr terrain,gui::osg::GridVisual * grid,std::size_t xResolution=20u,std::size_t yResolution=20u,S xSize=S (2),S ySize=S (2),S zMin=S (0.0),S zMax=S (0.1))121 HeightmapWidget(
122 dart::gui::osg::ImGuiViewer* viewer,
123 HeightmapWorld* node,
124 dynamics::SimpleFramePtr terrain,
125 gui::osg::GridVisual* grid,
126 std::size_t xResolution = 20u,
127 std::size_t yResolution = 20u,
128 S xSize = S(2),
129 S ySize = S(2),
130 S zMin = S(0.0),
131 S zMax = S(0.1))
132 : mViewer(viewer), mNode(node), mTerrain(std::move(terrain)), mGrid(grid)
133 {
134 mXResolution = xResolution;
135 mYResolution = yResolution;
136 mXSize = xSize;
137 mYSize = ySize;
138 mZMin = zMin;
139 mZMax = zMax;
140
141 updateHeightmapShape();
142 }
143
updateHeightmapShape()144 void updateHeightmapShape()
145 {
146 mTerrain->setShape(createHeightmapShape(
147 mXResolution, mYResolution, mXSize, mYSize, mZMin, mZMax));
148
149 auto tf = mTerrain->getRelativeTransform();
150 tf.translation()[0] = -static_cast<double>(mXSize) / 2.0;
151 tf.translation()[1] = +static_cast<double>(mYSize) / 2.0;
152 mTerrain->setRelativeTransform(tf);
153 }
154
render()155 void render() override
156 {
157 ImGui::SetNextWindowPos(ImVec2(10, 20));
158 ImGui::SetNextWindowSize(ImVec2(360, 600));
159 ImGui::SetNextWindowBgAlpha(0.5f);
160 if (!ImGui::Begin(
161 "Point Cloud & Voxel Grid Demo",
162 nullptr,
163 ImGuiWindowFlags_MenuBar | ImGuiWindowFlags_HorizontalScrollbar))
164 {
165 // Early out if the window is collapsed, as an optimization.
166 ImGui::End();
167 return;
168 }
169
170 // Menu
171 if (ImGui::BeginMenuBar())
172 {
173 if (ImGui::BeginMenu("Menu"))
174 {
175 if (ImGui::MenuItem("Exit"))
176 mViewer->setDone(true);
177 ImGui::EndMenu();
178 }
179 if (ImGui::BeginMenu("Help"))
180 {
181 if (ImGui::MenuItem("About DART"))
182 mViewer->showAbout();
183 ImGui::EndMenu();
184 }
185 ImGui::EndMenuBar();
186 }
187
188 ImGui::Text("Heightmap rendering example");
189 ImGui::Spacing();
190 ImGui::TextWrapped("TODO.");
191
192 if (ImGui::CollapsingHeader("Help"))
193 {
194 ImGui::PushTextWrapPos(ImGui::GetCursorPos().x + 320);
195 ImGui::Text("User Guid:\n");
196 ImGui::Text("%s", mViewer->getInstructions().c_str());
197 ImGui::PopTextWrapPos();
198 }
199
200 if (ImGui::CollapsingHeader("Simulation", ImGuiTreeNodeFlags_DefaultOpen))
201 {
202 int e = mViewer->isSimulating() ? 0 : 1;
203 if (mViewer->isAllowingSimulation())
204 {
205 if (ImGui::RadioButton("Play", &e, 0) && !mViewer->isSimulating())
206 mViewer->simulate(true);
207 ImGui::SameLine();
208 if (ImGui::RadioButton("Pause", &e, 1) && mViewer->isSimulating())
209 mViewer->simulate(false);
210 }
211 }
212
213 if (mTerrain)
214 {
215 if (ImGui::CollapsingHeader("Heightmap", ImGuiTreeNodeFlags_DefaultOpen))
216 {
217 ImGui::Text("Terrain");
218
219 auto aspect = mTerrain->getVisualAspect();
220 bool display = !aspect->isHidden();
221 if (ImGui::Checkbox("Show##Terrain", &display))
222 {
223 if (display)
224 aspect->show();
225 else
226 aspect->hide();
227 }
228
229 auto shape = std::dynamic_pointer_cast<dynamics::HeightmapShapef>(
230 mTerrain->getShape());
231 assert(shape);
232
233 int xResolution = static_cast<int>(mXResolution);
234 if (ImGui::InputInt("X Resolution", &xResolution, 5, 10))
235 {
236 if (xResolution < 5)
237 xResolution = 5;
238
239 if (static_cast<int>(mXResolution) != xResolution)
240 {
241 mXResolution = xResolution;
242 updateHeightmapShape();
243 }
244 }
245
246 int yResolution = static_cast<int>(mYResolution);
247 if (ImGui::InputInt("Y Resolution", &yResolution, 5, 10))
248 {
249 if (yResolution < 5)
250 yResolution = 5;
251
252 if (static_cast<int>(mYResolution) != yResolution)
253 {
254 mYResolution = yResolution;
255 updateHeightmapShape();
256 }
257 }
258
259 ImGui::Separator();
260
261 if (ImGui::InputFloat("X Size", &mXSize, 0.1, 0.2))
262 {
263 if (mXSize < 0.1)
264 mXSize = 0.1;
265
266 updateHeightmapShape();
267 }
268
269 if (ImGui::InputFloat("Y Size", &mYSize, 0.1, 0.2))
270 {
271 if (mYSize < 0.1)
272 mYSize = 0.1;
273
274 updateHeightmapShape();
275 }
276
277 ImGui::Separator();
278
279 if (ImGui::InputFloat("Z Min", &mZMin, 0.05, 0.1))
280 updateHeightmapShape();
281
282 if (ImGui::InputFloat("Z Max", &mZMax, 0.05, 0.1))
283 updateHeightmapShape();
284
285 ImGui::Separator();
286
287 auto visualAspect = mTerrain->getVisualAspect();
288
289 float color[4];
290 auto visualColor = visualAspect->getRGBA();
291 color[0] = static_cast<float>(visualColor[0]);
292 color[1] = static_cast<float>(visualColor[1]);
293 color[2] = static_cast<float>(visualColor[2]);
294 color[3] = static_cast<float>(visualColor[3]);
295 if (ImGui::ColorEdit4("Color##Heightmap", color))
296 {
297 visualColor[0] = static_cast<double>(color[0]);
298 visualColor[1] = static_cast<double>(color[1]);
299 visualColor[2] = static_cast<double>(color[2]);
300 visualColor[3] = static_cast<double>(color[3]);
301 visualAspect->setColor(visualColor);
302 }
303 }
304 }
305
306 if (mGrid)
307 {
308 if (ImGui::CollapsingHeader("Grid", ImGuiTreeNodeFlags_None))
309 {
310 ImGui::Text("Grid");
311
312 bool display = mGrid->isDisplayed();
313 if (ImGui::Checkbox("Show##Grid", &display))
314 mGrid->display(display);
315
316 if (display)
317 {
318 int e = static_cast<int>(mGrid->getPlaneType());
319 if (mViewer->isAllowingSimulation())
320 {
321 if (ImGui::RadioButton("XY-Plane", &e, 0))
322 mGrid->setPlaneType(gui::osg::GridVisual::PlaneType::XY);
323 ImGui::SameLine();
324 if (ImGui::RadioButton("YZ-Plane", &e, 1))
325 mGrid->setPlaneType(gui::osg::GridVisual::PlaneType::YZ);
326 ImGui::SameLine();
327 if (ImGui::RadioButton("ZX-Plane", &e, 2))
328 mGrid->setPlaneType(gui::osg::GridVisual::PlaneType::ZX);
329 }
330
331 static Eigen::Vector3f offset;
332 ImGui::Columns(3);
333 offset = mGrid->getOffset().cast<float>();
334 if (ImGui::InputFloat("X", &offset[0], 0.1f, 0.5f, "%.1f"))
335 mGrid->setOffset(offset.cast<double>());
336 ImGui::NextColumn();
337 if (ImGui::InputFloat("Y", &offset[1], 0.1f, 0.5f, "%.1f"))
338 mGrid->setOffset(offset.cast<double>());
339 ImGui::NextColumn();
340 if (ImGui::InputFloat("Z", &offset[2], 0.1f, 0.5f, "%.1f"))
341 mGrid->setOffset(offset.cast<double>());
342 ImGui::Columns(1);
343
344 static int cellCount;
345 cellCount = static_cast<int>(mGrid->getNumCells());
346 if (ImGui::InputInt("Line Count", &cellCount, 1, 5))
347 {
348 if (cellCount < 0)
349 cellCount = 0;
350 mGrid->setNumCells(static_cast<std::size_t>(cellCount));
351 }
352
353 static float cellStepSize;
354 cellStepSize = static_cast<float>(mGrid->getMinorLineStepSize());
355 if (ImGui::InputFloat("Line Step Size", &cellStepSize, 0.001f, 0.1f))
356 {
357 mGrid->setMinorLineStepSize(static_cast<double>(cellStepSize));
358 }
359
360 static int minorLinesPerMajorLine;
361 minorLinesPerMajorLine
362 = static_cast<int>(mGrid->getNumMinorLinesPerMajorLine());
363 if (ImGui::InputInt(
364 "Minor Lines per Major Line", &minorLinesPerMajorLine, 1, 5))
365 {
366 if (minorLinesPerMajorLine < 0)
367 minorLinesPerMajorLine = 0;
368 mGrid->setNumMinorLinesPerMajorLine(
369 static_cast<std::size_t>(minorLinesPerMajorLine));
370 }
371
372 static float axisLineWidth;
373 axisLineWidth = mGrid->getAxisLineWidth();
374 if (ImGui::InputFloat(
375 "Axis Line Width", &axisLineWidth, 1.f, 2.f, "%.0f"))
376 {
377 mGrid->setAxisLineWidth(axisLineWidth);
378 }
379
380 static float majorLineWidth;
381 majorLineWidth = mGrid->getMajorLineWidth();
382 if (ImGui::InputFloat(
383 "Major Line Width", &majorLineWidth, 1.f, 2.f, "%.0f"))
384 {
385 mGrid->setMajorLineWidth(majorLineWidth);
386 }
387
388 static float majorColor[3];
389 auto internalmajorColor = mGrid->getMajorLineColor();
390 majorColor[0] = static_cast<float>(internalmajorColor.x());
391 majorColor[1] = static_cast<float>(internalmajorColor.y());
392 majorColor[2] = static_cast<float>(internalmajorColor.z());
393 if (ImGui::ColorEdit3("Major Line Color", majorColor))
394 {
395 internalmajorColor[0] = static_cast<double>(majorColor[0]);
396 internalmajorColor[1] = static_cast<double>(majorColor[1]);
397 internalmajorColor[2] = static_cast<double>(majorColor[2]);
398 mGrid->setMajorLineColor(internalmajorColor);
399 }
400
401 static float minorLineWidth;
402 minorLineWidth = mGrid->getMinorLineWidth();
403 if (ImGui::InputFloat(
404 "Minor Line Width", &minorLineWidth, 1.f, 2.f, "%.0f"))
405 {
406 mGrid->setMinorLineWidth(minorLineWidth);
407 }
408
409 float minorColor[3];
410 auto internalMinorColor = mGrid->getMinorLineColor();
411 minorColor[0] = static_cast<float>(internalMinorColor.x());
412 minorColor[1] = static_cast<float>(internalMinorColor.y());
413 minorColor[2] = static_cast<float>(internalMinorColor.z());
414 if (ImGui::ColorEdit3("Minor Line Color", minorColor))
415 {
416 internalMinorColor[0] = static_cast<double>(minorColor[0]);
417 internalMinorColor[1] = static_cast<double>(minorColor[1]);
418 internalMinorColor[2] = static_cast<double>(minorColor[2]);
419 mGrid->setMinorLineColor(internalMinorColor);
420 }
421 }
422 }
423 }
424
425 ImGui::End();
426 }
427
428 protected:
429 dart::gui::osg::ImGuiViewer* mViewer;
430 HeightmapWorld* mNode;
431 dynamics::SimpleFramePtr mTerrain;
432 ::osg::ref_ptr<gui::osg::GridVisual> mGrid;
433 std::size_t mXResolution;
434 std::size_t mYResolution;
435 float mXSize;
436 float mYSize;
437 float mZMin;
438 float mZMax;
439 };
440
main()441 int main()
442 {
443 auto world = dart::simulation::World::create();
444 world->setGravity(Eigen::Vector3d::Zero());
445
446 auto terrain = createHeightmapFrame<float>(100u, 100u, 2.f, 2.f, 0.f, 0.1f);
447 world->addSimpleFrame(terrain);
448
449 assert(world->getNumSimpleFrames() == 1u);
450
451 // Create an instance of our customized WorldNode
452 ::osg::ref_ptr<HeightmapWorld> node = new HeightmapWorld(world);
453 node->setNumStepsPerCycle(16);
454
455 // Create the Viewer instance
456 dart::gui::osg::ImGuiViewer viewer;
457 viewer.addWorldNode(node);
458 viewer.simulate(true);
459
460 // Create grid
461 ::osg::ref_ptr<gui::osg::GridVisual> grid = new gui::osg::GridVisual();
462
463 // Add control widget for atlas
464 viewer.getImGuiHandler()->addWidget(std::make_shared<HeightmapWidget<float>>(
465 &viewer, node.get(), terrain, grid));
466
467 viewer.addAttachment(grid);
468
469 // Print out instructions
470 std::cout << viewer.getInstructions() << std::endl;
471
472 // Set up the window to be 1280x720 pixels
473 viewer.setUpViewInWindow(0, 0, 1280, 720);
474
475 viewer.getCameraManipulator()->setHomePosition(
476 ::osg::Vec3(2.57f, 3.14f, 1.64f),
477 ::osg::Vec3(0.00f, 0.00f, 0.30f),
478 ::osg::Vec3(-0.24f, -0.25f, 0.94f));
479 // We need to re-dirty the CameraManipulator by passing it into the viewer
480 // again, so that the viewer knows to update its HomePosition setting
481 viewer.setCameraManipulator(viewer.getCameraManipulator());
482
483 // Begin the application loop
484 viewer.run();
485 }
486