Skip to content

Commit 86895cb

Browse files
rickertmrickertm
committed
Add visualization option for path vertices
1 parent 2c44075 commit 86895cb

File tree

4 files changed

+161
-52
lines changed

4 files changed

+161
-52
lines changed

‎demos/rlPlanDemo/MainWindow.cpp‎

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,8 @@ MainWindow::MainWindow(QWidget* parent, Qt::WindowFlags f) :
180180
toggleConfigurationSpaceSceneAction(new QAction(this)),
181181
toggleConfigurationVerticesAction(new QAction(this)),
182182
toggleLinesAction(new QAction(this)),
183-
togglePathAction(new QAction(this)),
183+
togglePathEdgesAction(new QAction(this)),
184+
togglePathVerticesAction(new QAction(this)),
184185
togglePlannerAction(new QAction(this)),
185186
togglePointsAction(new QAction(this)),
186187
toggleSpheresAction(new QAction(this)),
@@ -701,12 +702,19 @@ MainWindow::init()
701702

702703
viewMenu->addSeparator();
703704

704-
this->togglePathAction->setCheckable(true);
705-
this->togglePathAction->setChecked(true);
706-
this->togglePathAction->setText("Path");
707-
QObject::connect(this->togglePathAction, SIGNAL(toggled(bool)), this->viewer, SLOT(togglePath(bool)));
708-
this->addAction(this->togglePathAction);
709-
viewMenu->addAction(this->togglePathAction);
705+
this->togglePathEdgesAction->setCheckable(true);
706+
this->togglePathEdgesAction->setChecked(true);
707+
this->togglePathEdgesAction->setText("Path Edges");
708+
QObject::connect(this->togglePathEdgesAction, SIGNAL(toggled(bool)), this->viewer, SLOT(togglePathEdges(bool)));
709+
this->addAction(this->togglePathEdgesAction);
710+
viewMenu->addAction(this->togglePathEdgesAction);
711+
712+
this->togglePathVerticesAction->setCheckable(true);
713+
this->togglePathVerticesAction->setChecked(false);
714+
this->togglePathVerticesAction->setText("Path Vertices");
715+
QObject::connect(this->togglePathVerticesAction, SIGNAL(toggled(bool)), this->viewer, SLOT(togglePathVertices(bool)));
716+
this->addAction(this->togglePathVerticesAction);
717+
viewMenu->addAction(this->togglePathVerticesAction);
710718

711719
this->toggleAnimationAction->setCheckable(true);
712720
this->toggleAnimationAction->setChecked(true);

‎demos/rlPlanDemo/MainWindow.h‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -267,7 +267,9 @@ public slots:
267267

268268
QAction* toggleLinesAction;
269269

270-
QAction* togglePathAction;
270+
QAction* togglePathEdgesAction;
271+
272+
QAction* togglePathVerticesAction;
271273

272274
QAction* togglePlannerAction;
273275

‎demos/rlPlanDemo/Viewer.cpp‎

Lines changed: 114 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -80,12 +80,20 @@ Viewer::Viewer(QWidget* parent, Qt::WindowFlags f) :
8080
linesMaterial(new SoVRMLMaterial()),
8181
linesShape(new SoVRMLShape()),
8282
path(new SoVRMLSwitch()),
83-
pathAppearance(new SoVRMLAppearance()),
84-
pathCoordinate(new SoVRMLCoordinate()),
85-
pathDrawStyle(new SoDrawStyle()),
86-
pathIndexedLineSet(new SoVRMLIndexedLineSet()),
87-
pathMaterial(new SoVRMLMaterial()),
88-
pathShape(new SoVRMLShape()),
83+
pathEdges(new SoVRMLSwitch()),
84+
pathEdgesAppearance(new SoVRMLAppearance()),
85+
pathEdgesCoordinate(new SoVRMLCoordinate()),
86+
pathEdgesDrawStyle(new SoDrawStyle()),
87+
pathEdgesIndexedLineSet(new SoVRMLIndexedLineSet()),
88+
pathEdgesMaterial(new SoVRMLMaterial()),
89+
pathEdgesShape(new SoVRMLShape()),
90+
pathVertices(new SoVRMLSwitch()),
91+
pathVerticesAppearance(new SoVRMLAppearance()),
92+
pathVerticesCoordinate(new SoVRMLCoordinate()),
93+
pathVerticesDrawStyle(new SoDrawStyle()),
94+
pathVerticesPointSet(new SoVRMLPointSet()),
95+
pathVerticesMaterial(new SoVRMLMaterial()),
96+
pathVerticesShape(new SoVRMLShape()),
8997
path3(new SoVRMLSwitch()),
9098
path3Appearance(new SoVRMLAppearance()),
9199
path3Coordinate(new SoVRMLCoordinate()),
@@ -270,20 +278,47 @@ Viewer::Viewer(QWidget* parent, Qt::WindowFlags f) :
270278
this->path->setName("path");
271279
this->path->whichChoice = SO_SWITCH_ALL;
272280

273-
this->pathDrawStyle->lineWidth = 3.0f;
274-
this->pathDrawStyle->pointSize = 0.0f;
275-
this->path->addChild(this->pathDrawStyle);
281+
this->root->addChild(this->path);
276282

277-
this->pathMaterial->diffuseColor.setValue(55.0f / 255.0f, 176.0f / 255.0f, 55.0f / 255.0f);
278-
this->pathAppearance->material = this->pathMaterial;
279-
this->pathShape->appearance = this->pathAppearance;
283+
// path edges
280284

281-
this->pathIndexedLineSet->coord = this->pathCoordinate;
282-
this->pathShape->geometry = this->pathIndexedLineSet;
285+
this->pathEdges->setName("pathEdges");
286+
this->pathEdges->whichChoice = SO_SWITCH_ALL;
283287

284-
this->path->addChild(this->pathShape);
288+
this->pathEdgesDrawStyle->lineWidth = 3.0f;
289+
this->pathEdgesDrawStyle->pointSize = 0.0f;
290+
this->pathEdges->addChild(this->pathEdgesDrawStyle);
285291

286-
this->root->addChild(this->path);
292+
this->pathEdgesMaterial->diffuseColor.setValue(55.0f / 255.0f, 176.0f / 255.0f, 55.0f / 255.0f);
293+
this->pathEdgesAppearance->material = this->pathEdgesMaterial;
294+
this->pathEdgesShape->appearance = this->pathEdgesAppearance;
295+
296+
this->pathEdgesIndexedLineSet->coord = this->pathEdgesCoordinate;
297+
this->pathEdgesShape->geometry = this->pathEdgesIndexedLineSet;
298+
299+
this->pathEdges->addChild(this->pathEdgesShape);
300+
301+
this->path->addChild(this->pathEdges);
302+
303+
// path vertices
304+
305+
this->pathVertices->setName("pathVertices");
306+
this->pathVertices->whichChoice = SO_SWITCH_NONE;
307+
308+
this->pathVerticesDrawStyle->lineWidth = 0.0f;
309+
this->pathVerticesDrawStyle->pointSize = 8.0f;
310+
this->pathVertices->addChild(this->pathVerticesDrawStyle);
311+
312+
this->pathVerticesMaterial->emissiveColor.setValue(55.0f / 255.0f, 176.0f / 255.0f, 55.0f / 255.0f);
313+
this->pathVerticesAppearance->material = this->pathVerticesMaterial;
314+
this->pathVerticesShape->appearance = this->pathVerticesAppearance;
315+
316+
this->pathVerticesPointSet->coord = this->pathVerticesCoordinate;
317+
this->pathVerticesShape->geometry = this->pathVerticesPointSet;
318+
319+
this->pathVertices->addChild(this->pathVerticesShape);
320+
321+
this->path->addChild(this->pathVertices);
287322

288323
// path3
289324

@@ -505,8 +540,10 @@ Viewer::drawConfigurationPath(const rl::plan::VectorList& path)
505540
{
506541
this->path->enableNotify(false);
507542

508-
this->pathCoordinate->point.setNum(0);
509-
this->pathIndexedLineSet->coordIndex.setNum(0);
543+
this->pathEdgesCoordinate->point.setNum(0);
544+
this->pathEdgesIndexedLineSet->coordIndex.setNum(0);
545+
546+
this->pathVerticesCoordinate->point.setNum(0);
510547

511548
rl::math::Vector inter(this->model->getDofPosition());
512549

@@ -520,16 +557,23 @@ Viewer::drawConfigurationPath(const rl::plan::VectorList& path)
520557
this->model->setPosition(*i);
521558
this->model->updateFrames();
522559

523-
this->pathCoordinate->point.set1Value(
524-
this->pathCoordinate->point.getNum(),
560+
this->pathEdgesCoordinate->point.set1Value(
561+
this->pathEdgesCoordinate->point.getNum(),
525562
this->model->forwardPosition(l)(0, 3),
526563
this->model->forwardPosition(l)(1, 3),
527564
this->model->forwardPosition(l)(2, 3)
528565
);
529566

530-
this->pathIndexedLineSet->coordIndex.set1Value(
531-
this->pathIndexedLineSet->coordIndex.getNum(),
532-
this->pathCoordinate->point.getNum() - 1
567+
this->pathEdgesIndexedLineSet->coordIndex.set1Value(
568+
this->pathEdgesIndexedLineSet->coordIndex.getNum(),
569+
this->pathEdgesCoordinate->point.getNum() - 1
570+
);
571+
572+
this->pathVerticesCoordinate->point.set1Value(
573+
this->pathVerticesCoordinate->point.getNum(),
574+
this->model->forwardPosition(l)(0, 3),
575+
this->model->forwardPosition(l)(1, 3),
576+
this->model->forwardPosition(l)(2, 3)
533577
);
534578
}
535579

@@ -544,22 +588,29 @@ Viewer::drawConfigurationPath(const rl::plan::VectorList& path)
544588
this->model->setPosition(inter);
545589
this->model->updateFrames(false);
546590

547-
this->pathCoordinate->point.set1Value(
548-
this->pathCoordinate->point.getNum(),
591+
this->pathEdgesCoordinate->point.set1Value(
592+
this->pathEdgesCoordinate->point.getNum(),
549593
this->model->forwardPosition(l)(0, 3),
550594
this->model->forwardPosition(l)(1, 3),
551595
this->model->forwardPosition(l)(2, 3)
552596
);
553597

554-
this->pathIndexedLineSet->coordIndex.set1Value(
555-
this->pathIndexedLineSet->coordIndex.getNum(),
556-
this->pathCoordinate->point.getNum() - 1
598+
this->pathEdgesIndexedLineSet->coordIndex.set1Value(
599+
this->pathEdgesIndexedLineSet->coordIndex.getNum(),
600+
this->pathEdgesCoordinate->point.getNum() - 1
557601
);
558602
}
603+
604+
this->pathVerticesCoordinate->point.set1Value(
605+
this->pathVerticesCoordinate->point.getNum(),
606+
this->model->forwardPosition(l)(0, 3),
607+
this->model->forwardPosition(l)(1, 3),
608+
this->model->forwardPosition(l)(2, 3)
609+
);
559610
}
560611

561-
this->pathIndexedLineSet->coordIndex.set1Value(
562-
this->pathIndexedLineSet->coordIndex.getNum(),
612+
this->pathEdgesIndexedLineSet->coordIndex.set1Value(
613+
this->pathEdgesIndexedLineSet->coordIndex.getNum(),
563614
SO_END_FACE_INDEX
564615
);
565616
}
@@ -855,13 +906,11 @@ Viewer::reset()
855906
{
856907
this->resetEdges();
857908
this->resetLines();
909+
this->resetPath();
910+
this->resetPath3();
858911
this->resetPoints();
859912
this->resetSpheres();
860913
this->resetVertices();
861-
this->pathCoordinate->point.setNum(0);
862-
this->pathIndexedLineSet->coordIndex.setNum(0);
863-
this->path3Coordinate->point.setNum(0);
864-
this->path3IndexedLineSet->coordIndex.setNum(0);
865914
this->sweptGroup->removeAllChildren();
866915
this->workTransform->setMatrix(SbMatrix::identity());
867916
}
@@ -884,6 +933,21 @@ Viewer::resetLines()
884933
this->linesIndexedLineSet->coordIndex.setNum(0);
885934
}
886935

936+
void
937+
Viewer::resetPath()
938+
{
939+
this->pathEdgesCoordinate->point.setNum(0);
940+
this->pathEdgesIndexedLineSet->coordIndex.setNum(0);
941+
this->pathVerticesCoordinate->point.setNum(0);
942+
}
943+
944+
void
945+
Viewer::resetPath3()
946+
{
947+
this->path3Coordinate->point.setNum(0);
948+
this->path3IndexedLineSet->coordIndex.setNum(0);
949+
}
950+
887951
void
888952
Viewer::resetPoints()
889953
{
@@ -1015,15 +1079,28 @@ Viewer::toggleLines(const bool& doOn)
10151079
}
10161080

10171081
void
1018-
Viewer::togglePath(const bool& doOn)
1082+
Viewer::togglePathEdges(const bool& doOn)
1083+
{
1084+
if (doOn)
1085+
{
1086+
this->pathEdges->whichChoice = SO_SWITCH_ALL;
1087+
}
1088+
else
1089+
{
1090+
this->pathEdges->whichChoice = SO_SWITCH_NONE;
1091+
}
1092+
}
1093+
1094+
void
1095+
Viewer::togglePathVertices(const bool& doOn)
10191096
{
10201097
if (doOn)
10211098
{
1022-
this->path->whichChoice = SO_SWITCH_ALL;
1099+
this->pathVertices->whichChoice = SO_SWITCH_ALL;
10231100
}
10241101
else
10251102
{
1026-
this->path->whichChoice = SO_SWITCH_NONE;
1103+
this->pathVertices->whichChoice = SO_SWITCH_NONE;
10271104
}
10281105
}
10291106

‎demos/rlPlanDemo/Viewer.h‎

Lines changed: 29 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -98,6 +98,10 @@ public slots:
9898

9999
void resetLines();
100100

101+
void resetPath();
102+
103+
void resetPath3();
104+
101105
void resetPoints();
102106

103107
void resetSpheres();
@@ -116,7 +120,9 @@ public slots:
116120

117121
void toggleLines(const bool& doOn);
118122

119-
void togglePath(const bool& doOn);
123+
void togglePathEdges(const bool& doOn);
124+
125+
void togglePathVertices(const bool& doOn);
120126

121127
void togglePoints(const bool& doOn);
122128

@@ -195,17 +201,33 @@ public slots:
195201

196202
SoVRMLSwitch* path;
197203

198-
SoVRMLAppearance* pathAppearance;
204+
SoVRMLSwitch* pathEdges;
205+
206+
SoVRMLAppearance* pathEdgesAppearance;
207+
208+
SoVRMLCoordinate* pathEdgesCoordinate;
209+
210+
SoDrawStyle* pathEdgesDrawStyle;
211+
212+
SoVRMLIndexedLineSet* pathEdgesIndexedLineSet;
213+
214+
SoVRMLMaterial* pathEdgesMaterial;
215+
216+
SoVRMLShape* pathEdgesShape;
217+
218+
SoVRMLSwitch* pathVertices;
219+
220+
SoVRMLAppearance* pathVerticesAppearance;
199221

200-
SoVRMLCoordinate* pathCoordinate;
222+
SoVRMLCoordinate* pathVerticesCoordinate;
201223

202-
SoDrawStyle* pathDrawStyle;
224+
SoDrawStyle* pathVerticesDrawStyle;
203225

204-
SoVRMLIndexedLineSet* pathIndexedLineSet;
226+
SoVRMLPointSet* pathVerticesPointSet;
205227

206-
SoVRMLMaterial* pathMaterial;
228+
SoVRMLMaterial* pathVerticesMaterial;
207229

208-
SoVRMLShape* pathShape;
230+
SoVRMLShape* pathVerticesShape;
209231

210232
SoVRMLSwitch* path3;
211233

0 commit comments

Comments
 (0)