From 3c71159ed0a89f1b16e037b3dd25d846b935d536 Mon Sep 17 00:00:00 2001
From: Jules Waldhart <jules.waldhart@laas.fr>
Date: Wed, 20 Apr 2016 14:50:17 +0200
Subject: [PATCH] fix RobotState and remove useless functions

Removed functions are +,-,*, to be updated if needed
---
 src/API/ConfigSpace/RobotState.cpp                 | 75 +++++++++++-----------
 src/API/ConfigSpace/RobotState.hpp                 | 18 ++++--
 .../CostFunctions/HRICS_CF_TimeToCollision.cpp     | 10 +--
 3 files changed, 56 insertions(+), 47 deletions(-)

diff --git a/src/API/ConfigSpace/RobotState.cpp b/src/API/ConfigSpace/RobotState.cpp
index cba74f8..0b99845 100644
--- a/src/API/ConfigSpace/RobotState.cpp
+++ b/src/API/ConfigSpace/RobotState.cpp
@@ -102,7 +102,10 @@ Robot *RobotState::getRobot() {
 }
 
 configPt RobotState::getConfigStruct() {
-	return &state[0][0];
+    if(state.size() && state[0].size())
+        return &state[0][0];
+    else
+        return NULL;
 }
 
 vector<vector<double> > RobotState::getState() const {
@@ -456,48 +459,48 @@ bool RobotState::operator==(RobotState &Conf) { return this->equal (Conf); }
 
 bool RobotState::operator!=(RobotState &Conf) { return !(this->equal (Conf)); }
 
-statePtr_t RobotState::operator-(RobotState &Conf) { return this->sub (Conf); }
+//statePtr_t RobotState::operator-(RobotState &Conf) { return this->sub (Conf); }
 
-statePtr_t RobotState::operator+(RobotState &Conf) { return this->add (Conf); }
+//statePtr_t RobotState::operator+(RobotState &Conf) { return this->add (Conf); }
 
-RobotState &RobotState::operator*(const double coeff) { return this->mult (coeff); }
+//RobotState &RobotState::operator*(const double coeff) { return this->mult (coeff); }
 
 double &RobotState::at(const int &i) { return getConfigStruct ()[i]; }
 
-statePtr_t RobotState::add(RobotState &C) {
-	shared_ptr<RobotState> ptrQ (new RobotState (_Robot));
-
-	p3d_addConfig (_Robot->getRobotStruct (), getConfigStruct (),
-				   C.getConfigStruct (), ptrQ->getConfigStruct ());
-
-	return ptrQ;
-}
-
-statePtr_t RobotState::sub(RobotState &C) {
-	statePtr_t ptrQ (new RobotState (_Robot));
-
-	p3d_subConfig (_Robot->getRobotStruct (), getConfigStruct (),
-				   C.getConfigStruct (), ptrQ->getConfigStruct ());
-
-	return ptrQ;
-}
-
-
-RobotState &RobotState::mult(double coeff) {
-	RobotState *q = new RobotState (*this);
+//statePtr_t RobotState::add(RobotState &C) {
+//	shared_ptr<RobotState> ptrQ (new RobotState (_Robot));
+//
+//	p3d_addConfig (_Robot->getRobotStruct (), getConfigStruct (),
+//				   C.getConfigStruct (), ptrQ->getConfigStruct ());
+//
+//	return ptrQ;
+//}
+//
+//statePtr_t RobotState::sub(RobotState &C) {
+//	statePtr_t ptrQ (new RobotState (_Robot));
+//
+//	p3d_subConfig (_Robot->getRobotStruct (), getConfigStruct (),
+//				   C.getConfigStruct (), ptrQ->getConfigStruct ());
+//
+//	return ptrQ;
+//}
 
-	int njnt = _Robot->getRobotStruct ()->njoints;
-	int k = 0;
-	for (int i = 0; i <= njnt; i++) {
-		p3d_jnt *jntPt = _Robot->getRobotStruct ()->joints[i];
-		for (int j = 0; j < jntPt->dof_equiv_nbr; j++) {
-			(*q)[k] *= coeff;
-			k++;
-		}
-	}
 
-	return (*q);
-}
+// RobotState &RobotState::mult(double coeff) {
+// 	RobotState *q = new RobotState (*this);
+//
+// 	int njnt = _Robot->getRobotStruct ()->njoints;
+// 	int k = 0;
+// 	for (int i = 0; i <= njnt; i++) {
+// 		p3d_jnt *jntPt = _Robot->getRobotStruct ()->joints[i];
+// 		for (int j = 0; j < jntPt->dof_equiv_nbr; j++) {
+// 			(*q)[k] *= coeff;
+// 			k++;
+// 		}
+// 	}
+//
+// 	return (*q);
+// }
 
 Eigen::VectorXd RobotState::getEigenVector() {
 	unsigned int nbDof = 0;
diff --git a/src/API/ConfigSpace/RobotState.hpp b/src/API/ConfigSpace/RobotState.hpp
index f30b3b5..ce47891 100644
--- a/src/API/ConfigSpace/RobotState.hpp
+++ b/src/API/ConfigSpace/RobotState.hpp
@@ -79,19 +79,25 @@ public:
 
 	bool operator!=(RobotState &Conf);
 
-	statePtr_t operator-(RobotState &Conf);
+    //needs to be updated to take into account full state
+    //statePtr_t operator-(RobotState &Conf);
 
-	statePtr_t operator+(RobotState &Conf);
+    //needs to be updated to take into account full state
+    //statePtr_t operator+(RobotState &Conf);
 
-	RobotState &operator*(const double coeff);
+    //needs to be updated to take into account full state
+    //RobotState &operator*(const double coeff);
 
 	RobotState &operator=(const RobotState &other);
 
-	statePtr_t sub(RobotState &C);
+    //needs to be updated to take into account full state
+    //statePtr_t sub(RobotState &C);
 
-	statePtr_t add(RobotState &C);
+    //needs to be updated to take into account full state
+    //statePtr_t add(RobotState &C);
 
-	RobotState &mult(double coeff);
+    //needs to be updated to take into account full state
+    //RobotState &mult(double coeff);
 
 	bool equal(RobotState &Conf, bool print = false);
 
diff --git a/src/HRI_costspace/CostFunctions/HRICS_CF_TimeToCollision.cpp b/src/HRI_costspace/CostFunctions/HRICS_CF_TimeToCollision.cpp
index 588539a..975dffa 100644
--- a/src/HRI_costspace/CostFunctions/HRICS_CF_TimeToCollision.cpp
+++ b/src/HRI_costspace/CostFunctions/HRICS_CF_TimeToCollision.cpp
@@ -65,12 +65,12 @@ statePtr_t  TimeToCollision::projectConfiguration(statePtr_t cprev, statePtr_t c
     //cprev was dt seconds before ccurrent. Returned configuration is the projected one at time t seconds after ccurrent.
 
 
-    Robot* rob = cprev->getRobot();
-    statePtr_t cnext(new RobotState(rob)),cdiff(new RobotState(rob));
-    cdiff = *cprev - *ccurrent;
+    //Robot* rob = cprev->getRobot();
+    //statePtr_t cnext(new RobotState(rob)),cdiff(new RobotState(rob));
+    //cdiff = *cprev - *ccurrent;
 
-    cnext = *ccurrent + (*cdiff * (t/dt));
-    return cnext;
+    //cnext = *ccurrent + (*cdiff * (t/dt));
+    //return cnext;
 }
 
 int TimeToCollision::test(CostFunctionCommon* common){
-- 
1.9.1

