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