Add geometry functions & various modifications.

1 files changed, 7 insertions, 2 deletions

diff --git a/problem.hpp b/problem.hpp
index 535affa..10adaf8 100644
--- a/problem.hpp
+++ b/problem.hpp
@@ -16,9 +16,14 @@ struct hilare_a {	// System A
 	// position actuelle
 	double x, y, theta, phi;
 
+	vec pos() const { return vec(x, y); }
+	vec dir() const { return vec::from_polar(1, theta); }
+
 	vec pos_trolley() const {
-		//TODO
-		return vec(0, 0);
+		return pos() + vec::from_polar(l, theta + phi + M_PI);
+	}
+	vec dir_trolley() const {
+		return vec::from_polar(1, theta + phi);
 	}
 };