Show current configuration in configuration space view

Author: rickertm

demos/rlPlanDemo/ConfigurationModel.cpp

@@ -151,7 +151,7 @@ ConfigurationModel::setData(const QModelIndex& index, const QVariant& value, int
 			(*MainWindow::instance()->q)(index.row()) = value.value<rl::math::Real>();
 		}
 
-		MainWindow::instance()->viewer->drawConfiguration(*MainWindow::instance()->q);
+		MainWindow::instance()->thread->drawConfiguration(*MainWindow::instance()->q);
 
 		emit dataChanged(index, index);
 

demos/rlPlanDemo/ConfigurationSpaceScene.cpp

@@ -44,6 +44,7 @@ ConfigurationSpaceScene::ConfigurationSpaceScene(QObject* parent) :
 	range(),
 	steps(),
 	collisions(nullptr),
+	configuration(nullptr),
 	edges(nullptr),
 	path(nullptr),
 	scene(nullptr),
@@ -63,6 +64,11 @@ ConfigurationSpaceScene::ConfigurationSpaceScene(QObject* parent) :
 	this->collisions = this->createItemGroup(QList<QGraphicsItem*>());
 	this->collisions->setZValue(1);
 
+	this->configuration = this->addEllipse(-3, -3, 6, 6, QPen(Qt::NoPen), QBrush((QColor(247, 127, 7))));
+	this->configuration->setFlag(QGraphicsItem::ItemIgnoresTransformations);
+	this->configuration->setVisible(false);
+	this->configuration->setZValue(4);
+	
 	this->edges = this->createItemGroup(QList<QGraphicsItem*>());
 	this->edges->setZValue(2);
 
@@ -111,6 +117,10 @@ ConfigurationSpaceScene::clear()
 void
 ConfigurationSpaceScene::drawConfiguration(const rl::math::Vector& q)
 {
+	this->configuration->setPos(
+		q(this->axis[0]),
+		-q(this->axis[1])
+	);
 }
 
 void
@@ -240,6 +250,11 @@ ConfigurationSpaceScene::init()
 	this->steps[0] = static_cast<int>(std::ceil(this->range[0] / this->delta[0]));
 	this->steps[1] = static_cast<int>(std::ceil(this->range[1] / this->delta[1]));
 
+	this->configuration->setPos(
+		(*MainWindow::instance()->q)(this->axis[0]),
+		-(*MainWindow::instance()->q)(this->axis[1])
+	);
+	
 	this->scene->setRect(
 		this->minimum[0],
 		-this->maximum[1],
@@ -286,6 +301,7 @@ ConfigurationSpaceScene::mousePressEvent(QGraphicsSceneMouseEvent* mouseEvent)
 			}
 
 			MainWindow::instance()->configurationModel->invalidate();
+			this->drawConfiguration(*MainWindow::instance()->q);
 			MainWindow::instance()->viewer->drawConfiguration(*MainWindow::instance()->q);
 		}
 	}
@@ -349,6 +365,12 @@ ConfigurationSpaceScene::toggleCollisions(const bool& doOn)
 	this->collisions->setVisible(doOn);
 }
 
+void
+ConfigurationSpaceScene::toggleConfiguration(const bool& doOn)
+{
+	this->configuration->setVisible(doOn);
+}
+
 void
 ConfigurationSpaceScene::toggleConfigurationEdges(const bool& doOn)
 {

demos/rlPlanDemo/ConfigurationSpaceScene.h

@@ -111,6 +111,8 @@ public slots:
 
 	void toggleCollisions(const bool& doOn);
 
+	void toggleConfiguration(const bool& doOn);
+	
 	void toggleConfigurationEdges(const bool& doOn);
 
 	void togglePathEdges(const bool& doOn);
@@ -123,6 +125,8 @@ public slots:
 private:
 	QGraphicsItemGroup* collisions;
 
+	QGraphicsEllipseItem* configuration;
+	
 	QGraphicsItemGroup* edges;
 
 	QGraphicsPathItem* path;

demos/rlPlanDemo/MainWindow.cpp

@@ -523,7 +523,7 @@ MainWindow::getGoalConfiguration()
 {
 	*this->q = *this->goal;
 	this->configurationModel->invalidate();
-	this->viewer->drawConfiguration(*this->q);
+	this->thread->drawConfiguration(*this->q);
 	this->statusBar()->showMessage("Showing goal configuration.", 1000);
 }
 
@@ -532,7 +532,7 @@ MainWindow::getRandomConfiguration()
 {
 	*this->q = this->sampler2->generate();
 	this->configurationModel->invalidate();
-	this->viewer->drawConfiguration(*this->q);
+	this->thread->drawConfiguration(*this->q);
 	this->statusBar()->showMessage("Showing random configuration.", 1000);
 }
 
@@ -541,7 +541,7 @@ MainWindow::getRandomFreeConfiguration()
 {
 	*this->q = this->sampler2->generateCollisionFree();
 	this->configurationModel->invalidate();
-	this->viewer->drawConfiguration(*this->q);
+	this->thread->drawConfiguration(*this->q);
 	this->statusBar()->showMessage("Showing random collision-free configuration.", 1000);
 }
 
@@ -550,7 +550,7 @@ MainWindow::getStartConfiguration()
 {
 	*this->q = *this->start;
 	this->configurationModel->invalidate();
-	this->viewer->drawConfiguration(*this->q);
+	this->thread->drawConfiguration(*this->q);
 	this->statusBar()->showMessage("Showing start configuration.", 1000);
 }
 
@@ -2075,7 +2075,7 @@ MainWindow::load(const QString& filename)
 		path.eval("number((/rl/plan|/rlplan)//viewer/camera/scale)").getValue<rl::math::Real>(1)
 	);
 
-	this->viewer->drawConfiguration(*this->start);
+	this->thread->drawConfiguration(*this->start);
 
 	this->configurationModel->invalidate();
 	this->configurationSpaceModel->invalidate();
@@ -2521,6 +2521,8 @@ MainWindow::toggleConfigurationSpaceActive(const bool& doOn)
 	{
 		this->disconnect(this->thread, this->configurationSpaceScene);
 	}
+	
+	this->configurationSpaceScene->toggleConfiguration(doOn);
 }
 
 void