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 "dart/gui/osg/GridVisual.hpp"
34
35 #include "dart/dynamics/BodyNode.hpp"
36 #include "dart/dynamics/SimpleFrame.hpp"
37 #include "dart/dynamics/Skeleton.hpp"
38 #include "dart/dynamics/SphereShape.hpp"
39 #include "dart/math/Helpers.hpp"
40
41 namespace dart {
42 namespace gui {
43 namespace osg {
44
45 //==============================================================================
GridVisual()46 GridVisual::GridVisual()
47 {
48 initialize();
49 }
50
51 //==============================================================================
setNumCells(std::size_t cells)52 void GridVisual::setNumCells(std::size_t cells)
53 {
54 if (mNumCells == cells)
55 return;
56
57 mNumCells = cells;
58 mNeedUpdate = true;
59 }
60
61 //==============================================================================
getNumCells() const62 std::size_t GridVisual::getNumCells() const
63 {
64 return mNumCells;
65 }
66
67 //==============================================================================
setMinorLineStepSize(double size)68 void GridVisual::setMinorLineStepSize(double size)
69 {
70 if (std::abs(mMinorLineStepSize - size) < 1e-6)
71 return;
72
73 if (size < 0.0)
74 size = 0.0;
75
76 mMinorLineStepSize = size;
77 mNeedUpdate = true;
78 }
79
80 //==============================================================================
getMinorLineStepSize() const81 double GridVisual::getMinorLineStepSize() const
82 {
83 return mMinorLineStepSize;
84 }
85
86 //==============================================================================
setNumMinorLinesPerMajorLine(std::size_t size)87 void GridVisual::setNumMinorLinesPerMajorLine(std::size_t size)
88 {
89 if (mNumMinorLinesPerMajorLine == size)
90 return;
91
92 mNumMinorLinesPerMajorLine = size;
93 mNeedUpdate = true;
94 }
95
96 //==============================================================================
getNumMinorLinesPerMajorLine() const97 std::size_t GridVisual::getNumMinorLinesPerMajorLine() const
98 {
99 return mNumMinorLinesPerMajorLine;
100 }
101
102 //==============================================================================
setPlaneType(GridVisual::PlaneType type)103 void GridVisual::setPlaneType(GridVisual::PlaneType type)
104 {
105 if (mPlaneType == type)
106 return;
107
108 mPlaneType = type;
109 mNeedUpdate = true;
110 }
111
112 //==============================================================================
getPlaneType() const113 GridVisual::PlaneType GridVisual::getPlaneType() const
114 {
115 return mPlaneType;
116 }
117
118 //==============================================================================
setOffset(const Eigen::Vector3d & offset)119 void GridVisual::setOffset(const Eigen::Vector3d& offset)
120 {
121 if (mOffset.isApprox(offset))
122 return;
123
124 mOffset = offset;
125 mNeedUpdate = true;
126 }
127
128 //==============================================================================
getOffset() const129 const Eigen::Vector3d& GridVisual::getOffset() const
130 {
131 return mOffset;
132 }
133
134 //==============================================================================
display(bool display)135 void GridVisual::display(bool display)
136 {
137 if (mDisplayGrid == display)
138 return;
139
140 mDisplayGrid = display;
141
142 if (mDisplayGrid)
143 addChild(mGeode);
144 else
145 removeChild(mGeode);
146 }
147
148 //==============================================================================
isDisplayed() const149 bool GridVisual::isDisplayed() const
150 {
151 return mDisplayGrid;
152 }
153
154 //==============================================================================
setMajorLineColor(const Eigen::Vector4d & color)155 void GridVisual::setMajorLineColor(const Eigen::Vector4d& color)
156 {
157 assert(mMajorLineColor->size() == 1);
158 mMajorLineColor->at(0) = ::osg::Vec4(
159 static_cast<float>(color[0]),
160 static_cast<float>(color[1]),
161 static_cast<float>(color[2]),
162 static_cast<float>(color[3]));
163 mMajorLineGeom->setColorArray(mMajorLineColor, ::osg::Array::BIND_OVERALL);
164 }
165
166 //==============================================================================
getMajorLineColor() const167 Eigen::Vector4d GridVisual::getMajorLineColor() const
168 {
169 const ::osg::Vec4& c = mMajorLineColor->at(0);
170 return Eigen::Vector4f(c[0], c[1], c[2], c[3]).cast<double>();
171 }
172
173 //==============================================================================
setMinorLineColor(const Eigen::Vector4d & color)174 void GridVisual::setMinorLineColor(const Eigen::Vector4d& color)
175 {
176 assert(mMinorLineColor->size() == 1);
177 mMinorLineColor->at(0) = ::osg::Vec4(
178 static_cast<float>(color[0]),
179 static_cast<float>(color[1]),
180 static_cast<float>(color[2]),
181 static_cast<float>(color[3]));
182 mMinorLineGeom->setColorArray(mMinorLineColor, ::osg::Array::BIND_OVERALL);
183 }
184
185 //==============================================================================
getMinorLineColor() const186 Eigen::Vector4d GridVisual::getMinorLineColor() const
187 {
188 const ::osg::Vec4& c = mMinorLineColor->at(0);
189 return Eigen::Vector4f(c[0], c[1], c[2], c[3]).cast<double>();
190 }
191
192 //==============================================================================
setAxisLineWidth(float width)193 void GridVisual::setAxisLineWidth(float width)
194 {
195 if (width < 1)
196 width = 1;
197
198 mAxisLineWidth->setWidth(width);
199 mAxisLineGeom->getOrCreateStateSet()->setAttributeAndModes(mAxisLineWidth);
200 }
201
202 //==============================================================================
getAxisLineWidth() const203 float GridVisual::getAxisLineWidth() const
204 {
205 return mAxisLineWidth->getWidth();
206 }
207
208 //==============================================================================
setMajorLineWidth(float width)209 void GridVisual::setMajorLineWidth(float width)
210 {
211 if (width < 1)
212 width = 1;
213
214 mMajorLineWidth->setWidth(width);
215 mMinorLineGeom->getOrCreateStateSet()->setAttributeAndModes(mMinorLineWidth);
216 }
217
218 //==============================================================================
getMajorLineWidth() const219 float GridVisual::getMajorLineWidth() const
220 {
221 return mMajorLineWidth->getWidth();
222 }
223
224 //==============================================================================
setMinorLineWidth(float width)225 void GridVisual::setMinorLineWidth(float width)
226 {
227 if (width < 1)
228 width = 1;
229
230 mMinorLineWidth->setWidth(width);
231 mMajorLineGeom->getOrCreateStateSet()->setAttributeAndModes(mMajorLineWidth);
232 }
233
234 //==============================================================================
getMinorLineWidth() const235 float GridVisual::getMinorLineWidth() const
236 {
237 return mMinorLineWidth->getWidth();
238 }
239
240 //==============================================================================
toVec3(const Eigen::Vector3d & point)241 ::osg::Vec3 toVec3(const Eigen::Vector3d& point)
242 {
243 return ::osg::Vec3(
244 static_cast<float>(point.x()),
245 static_cast<float>(point.y()),
246 static_cast<float>(point.z()));
247 }
248
249 //==============================================================================
setVertices(::osg::Vec3Array * axisLineVertices,::osg::Vec3Array * majorLineVertices,::osg::Vec3Array * minorLineVertices,std::size_t numCells,std::size_t numMinorLinesPerMajorLine,float stepSize,GridVisual::PlaneType planeType,const Eigen::Vector3d & offset)250 void setVertices(
251 ::osg::Vec3Array* axisLineVertices,
252 ::osg::Vec3Array* majorLineVertices,
253 ::osg::Vec3Array* minorLineVertices,
254 std::size_t numCells,
255 std::size_t numMinorLinesPerMajorLine,
256 float stepSize,
257 GridVisual::PlaneType planeType,
258 const Eigen::Vector3d& offset)
259 {
260 assert(axisLineVertices);
261 assert(majorLineVertices);
262 assert(minorLineVertices);
263
264 int axis1 = 0;
265 int axis2 = 1;
266
267 switch (planeType)
268 {
269 case GridVisual::PlaneType::XY:
270 {
271 axis1 = 0;
272 axis2 = 1;
273 break;
274 }
275 case GridVisual::PlaneType::YZ:
276 {
277 axis1 = 1;
278 axis2 = 2;
279 break;
280 }
281 case GridVisual::PlaneType::ZX:
282 {
283 axis1 = 2;
284 axis2 = 0;
285 break;
286 }
287 }
288
289 const std::size_t numAxisLineVertices = 5;
290 const std::size_t numMajorLineVertices
291 = numMinorLinesPerMajorLine > 0
292 ? 8 * (numCells / numMinorLinesPerMajorLine)
293 : 0;
294 const std::size_t numMinorLineVertices = 8 * numCells - numMajorLineVertices;
295
296 axisLineVertices->clear();
297 axisLineVertices->reserve(numAxisLineVertices);
298 majorLineVertices->clear();
299 majorLineVertices->reserve(numMajorLineVertices);
300 minorLineVertices->clear();
301 minorLineVertices->reserve(numMinorLineVertices);
302
303 const auto posInAxis1 = numCells * stepSize;
304 const auto axis1Negative = -posInAxis1;
305 const auto axis1Positive = +posInAxis1;
306
307 ::osg::Vec3 vec3;
308 const ::osg::Vec3 osgOffset = toVec3(offset);
309
310 //----------------
311 // Axis Vertices
312 //----------------
313
314 // Origin
315 axisLineVertices->push_back(vec3 + osgOffset);
316
317 // Axis1 positive
318 vec3[axis1] = posInAxis1;
319 axisLineVertices->push_back(vec3 + osgOffset);
320
321 // Axis1 negative
322 vec3[axis1] = -posInAxis1;
323 axisLineVertices->push_back(vec3 + osgOffset);
324 vec3[axis1] = 0;
325
326 // Axis2 positive
327 vec3[axis2] = posInAxis1;
328 axisLineVertices->push_back(vec3 + osgOffset);
329
330 // Axis2 negative
331 vec3[axis2] = -posInAxis1;
332 axisLineVertices->push_back(vec3 + osgOffset);
333 vec3[axis2] = 0;
334
335 //-------------------------------
336 // Major and minor line vertices
337 //-------------------------------
338
339 for (auto i = 1u; i < numCells + 1; ++i)
340 {
341 const float posInAxis2 = stepSize * i;
342
343 const auto axis2Negative = -posInAxis2;
344 const auto axis2Positive = +posInAxis2;
345
346 if (numMinorLinesPerMajorLine > 0 && i % numMinorLinesPerMajorLine == 0)
347 {
348 vec3[axis1] = axis1Negative;
349 vec3[axis2] = axis2Positive;
350 majorLineVertices->push_back(vec3 + osgOffset);
351
352 vec3[axis1] = axis1Positive;
353 vec3[axis2] = axis2Positive;
354 majorLineVertices->push_back(vec3 + osgOffset);
355
356 vec3[axis1] = axis1Negative;
357 vec3[axis2] = axis2Negative;
358 majorLineVertices->push_back(vec3 + osgOffset);
359
360 vec3[axis1] = axis1Positive;
361 vec3[axis2] = axis2Negative;
362 majorLineVertices->push_back(vec3 + osgOffset);
363
364 vec3[axis2] = axis1Negative;
365 vec3[axis1] = axis2Positive;
366 majorLineVertices->push_back(vec3 + osgOffset);
367
368 vec3[axis2] = axis1Positive;
369 vec3[axis1] = axis2Positive;
370 majorLineVertices->push_back(vec3 + osgOffset);
371
372 vec3[axis2] = axis1Negative;
373 vec3[axis1] = axis2Negative;
374 majorLineVertices->push_back(vec3 + osgOffset);
375
376 vec3[axis2] = axis1Positive;
377 vec3[axis1] = axis2Negative;
378 majorLineVertices->push_back(vec3 + osgOffset);
379 }
380 else
381 {
382 vec3[axis1] = axis1Negative;
383 vec3[axis2] = axis2Positive;
384 minorLineVertices->push_back(vec3 + osgOffset);
385
386 vec3[axis1] = axis1Positive;
387 vec3[axis2] = axis2Positive;
388 minorLineVertices->push_back(vec3 + osgOffset);
389
390 vec3[axis1] = axis1Negative;
391 vec3[axis2] = axis2Negative;
392 minorLineVertices->push_back(vec3 + osgOffset);
393
394 vec3[axis1] = axis1Positive;
395 vec3[axis2] = axis2Negative;
396 minorLineVertices->push_back(vec3 + osgOffset);
397
398 vec3[axis2] = axis1Negative;
399 vec3[axis1] = axis2Positive;
400 minorLineVertices->push_back(vec3 + osgOffset);
401
402 vec3[axis2] = axis1Positive;
403 vec3[axis1] = axis2Positive;
404 minorLineVertices->push_back(vec3 + osgOffset);
405
406 vec3[axis2] = axis1Negative;
407 vec3[axis1] = axis2Negative;
408 minorLineVertices->push_back(vec3 + osgOffset);
409
410 vec3[axis2] = axis1Positive;
411 vec3[axis1] = axis2Negative;
412 minorLineVertices->push_back(vec3 + osgOffset);
413 }
414 }
415 }
416
417 //==============================================================================
refresh()418 void GridVisual::refresh()
419 {
420 if (!mNeedUpdate)
421 return;
422
423 if (mDisplayGrid)
424 {
425 setVertices(
426 mAxisLineVertices,
427 mMajorLineVertices,
428 mMinorLineVertices,
429 mNumCells,
430 mNumMinorLinesPerMajorLine,
431 static_cast<float>(mMinorLineStepSize),
432 mPlaneType,
433 mOffset);
434
435 mMajorLineFaces->clear();
436 mMajorLineFaces->reserve(mMajorLineVertices->size());
437 for (auto i = 0u; i < mMajorLineVertices->size(); ++i)
438 mMajorLineFaces->push_back(i);
439
440 mMinorLineFaces->clear();
441 mMinorLineFaces->reserve(mMinorLineVertices->size());
442 for (auto i = 0u; i < mMinorLineVertices->size(); ++i)
443 mMinorLineFaces->push_back(i);
444
445 mMinorLineGeom->setVertexArray(mMinorLineVertices);
446 mMinorLineGeom->getOrCreateStateSet()->setAttributeAndModes(
447 mMinorLineWidth);
448 mMinorLineGeom->setPrimitiveSet(0, mMinorLineFaces);
449
450 mMajorLineGeom->setVertexArray(mMajorLineVertices);
451 mMajorLineGeom->getOrCreateStateSet()->setAttributeAndModes(
452 mMajorLineWidth);
453 mMajorLineGeom->setPrimitiveSet(0, mMajorLineFaces);
454
455 static const ::osg::Vec4 xAxisLineColor(0.9f, 0.1f, 0.1f, 1.0f);
456 static const ::osg::Vec4 yAxisLineColor(0.1f, 0.9f, 0.1f, 1.0f);
457 static const ::osg::Vec4 zAxisLineColor(0.1f, 0.1f, 0.9f, 1.0f);
458
459 switch (mPlaneType)
460 {
461 case GridVisual::PlaneType::XY:
462 {
463 mAxisLineColor->at(0) = xAxisLineColor;
464 mAxisLineColor->at(2) = yAxisLineColor;
465 break;
466 }
467 case GridVisual::PlaneType::YZ:
468 {
469 mAxisLineColor->at(0) = yAxisLineColor;
470 mAxisLineColor->at(2) = zAxisLineColor;
471 break;
472 }
473 case GridVisual::PlaneType::ZX:
474 {
475 mAxisLineColor->at(0) = zAxisLineColor;
476 mAxisLineColor->at(2) = xAxisLineColor;
477 break;
478 }
479 }
480
481 mAxisLineGeom->setColorArray(mAxisLineColor);
482 }
483
484 mNeedUpdate = false;
485 }
486
487 //==============================================================================
initialize()488 void GridVisual::initialize()
489 {
490 mNeedUpdate = true;
491
492 mDisplayGrid = true;
493
494 mPlaneType = PlaneType::XY;
495 mNumCells = 20;
496 mMinorLineStepSize = 0.1;
497 mNumMinorLinesPerMajorLine = 5;
498
499 mOffset = Eigen::Vector3d::Zero();
500
501 mGeode = new ::osg::Geode;
502 mGeode->getOrCreateStateSet()->setMode(
503 GL_LIGHTING, ::osg::StateAttribute::OFF);
504 addChild(mGeode);
505
506 mMinorLineGeom = new ::osg::Geometry;
507 mMajorLineGeom = new ::osg::Geometry;
508 mAxisLineGeom = new ::osg::Geometry;
509 mGeode->addDrawable(mMinorLineGeom);
510 mGeode->addDrawable(mMajorLineGeom);
511 mGeode->addDrawable(mAxisLineGeom);
512
513 mMinorLineVertices = new ::osg::Vec3Array;
514 mMinorLineGeom->setVertexArray(mMinorLineVertices);
515 mMinorLineGeom->setDataVariance(::osg::Object::STATIC);
516
517 mMajorLineVertices = new ::osg::Vec3Array;
518 mMajorLineGeom->setVertexArray(mMajorLineVertices);
519 mMajorLineGeom->setDataVariance(::osg::Object::STATIC);
520
521 mAxisLineVertices = new ::osg::Vec3Array;
522 mAxisLineGeom->setVertexArray(mAxisLineVertices);
523 mAxisLineGeom->setDataVariance(::osg::Object::STATIC);
524 mAxisLineGeom->getOrCreateStateSet()->setMode(
525 GL_BLEND, ::osg::StateAttribute::ON);
526 mAxisLineGeom->getOrCreateStateSet()->setRenderingHint(
527 ::osg::StateSet::TRANSPARENT_BIN);
528
529 // Set grid color
530 static const ::osg::Vec4 majorLineColor(0.4f, 0.4f, 0.4f, 1.0f);
531 static const ::osg::Vec4 minorLineColor(0.5f, 0.5f, 0.5f, 1.0f);
532
533 mAxisLineColor = new ::osg::Vec4Array;
534 mAxisLineColor->resize(4);
535 mAxisLineColor->at(0) = majorLineColor; // will be set on the fly
536 mAxisLineColor->at(1) = majorLineColor;
537 mAxisLineColor->at(2) = majorLineColor; // will be set on the fly
538 mAxisLineColor->at(3) = majorLineColor;
539 mAxisLineGeom->setColorArray(mAxisLineColor);
540 mAxisLineGeom->setColorBinding(::osg::Geometry::BIND_PER_PRIMITIVE_SET);
541
542 mMajorLineColor = new ::osg::Vec4Array;
543 mMajorLineColor->resize(1);
544 mMajorLineColor->at(0) = majorLineColor;
545 mMajorLineGeom->setColorArray(mMajorLineColor);
546 mMajorLineGeom->setColorBinding(::osg::Geometry::BIND_OVERALL);
547 mMajorLineGeom->getOrCreateStateSet()->setMode(
548 GL_BLEND, ::osg::StateAttribute::ON);
549 mMajorLineGeom->getOrCreateStateSet()->setRenderingHint(
550 ::osg::StateSet::TRANSPARENT_BIN);
551
552 mMinorLineColor = new ::osg::Vec4Array;
553 mMinorLineColor->resize(1);
554 mMinorLineColor->at(0) = minorLineColor;
555 mMinorLineGeom->setColorArray(mMinorLineColor);
556 mMinorLineGeom->setColorBinding(::osg::Geometry::BIND_OVERALL);
557 mMinorLineGeom->getOrCreateStateSet()->setMode(
558 GL_BLEND, ::osg::StateAttribute::ON);
559 mMinorLineGeom->getOrCreateStateSet()->setRenderingHint(
560 ::osg::StateSet::TRANSPARENT_BIN);
561
562 mMinorLineFaces = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
563 mMinorLineGeom->addPrimitiveSet(mMinorLineFaces);
564
565 mMajorLineFaces = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
566 mMajorLineGeom->addPrimitiveSet(mMajorLineFaces);
567
568 mAxis1PositiveFaces
569 = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
570 mAxis1NegativeFaces
571 = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
572 mAxis2PositiveFaces
573 = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
574 mAxis2NegativeFaces
575 = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
576 mAxis1PositiveFaces->resize(2);
577 mAxis1NegativeFaces->resize(2);
578 mAxis2PositiveFaces->resize(2);
579 mAxis2NegativeFaces->resize(2);
580 mAxis1PositiveFaces->at(0) = 0;
581 mAxis1PositiveFaces->at(1) = 1;
582 mAxis1NegativeFaces->at(0) = 0;
583 mAxis1NegativeFaces->at(1) = 2;
584 mAxis2PositiveFaces->at(0) = 0;
585 mAxis2PositiveFaces->at(1) = 3;
586 mAxis2NegativeFaces->at(0) = 0;
587 mAxis2NegativeFaces->at(1) = 4;
588 mAxisLineGeom->addPrimitiveSet(mAxis1PositiveFaces);
589 mAxisLineGeom->addPrimitiveSet(mAxis1NegativeFaces);
590 mAxisLineGeom->addPrimitiveSet(mAxis2PositiveFaces);
591 mAxisLineGeom->addPrimitiveSet(mAxis2NegativeFaces);
592
593 mAxisLineWidth = new ::osg::LineWidth(2);
594 mMajorLineWidth = new ::osg::LineWidth(2);
595 mMinorLineWidth = new ::osg::LineWidth(1);
596
597 mAxisLineGeom->getOrCreateStateSet()->setAttributeAndModes(mAxisLineWidth);
598 mMinorLineGeom->getOrCreateStateSet()->setAttributeAndModes(mMinorLineWidth);
599 mMajorLineGeom->getOrCreateStateSet()->setAttributeAndModes(mMajorLineWidth);
600 }
601
602 } // namespace osg
603 } // namespace gui
604 } // namespace dart
605