aboutsummaryrefslogtreecommitdiffstats
path: root/problem.hpp
diff options
context:
space:
mode:
authorAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 23:44:41 +0100
committerAlex Auvolat <alex.auvolat@ens.fr>2015-02-01 23:44:41 +0100
commit97366c03e6a9fc9cc98455fd6dd84cdd0d8197f3 (patch)
treef77879e5d72574f88057f1b6b22ec0a2bca4e21b /problem.hpp
parent499d8b00d8017a32ae31deafc88416c452b48c8c (diff)
downloadRobotique-Projet-97366c03e6a9fc9cc98455fd6dd84cdd0d8197f3.tar.gz
Robotique-Projet-97366c03e6a9fc9cc98455fd6dd84cdd0d8197f3.zip
Some of it works...
Diffstat (limited to 'problem.hpp')
-rw-r--r--problem.hpp25
1 files changed, 11 insertions, 14 deletions
diff --git a/problem.hpp b/problem.hpp
index 5d9269a..2fe70b1 100644
--- a/problem.hpp
+++ b/problem.hpp
@@ -26,28 +26,25 @@ struct hilare_a { // System A
vec pos() const { return vec(x, y); }
vec dir() const { return vec::from_polar(1, theta); }
- vec dir_trolley() const {
- return vec::from_polar(1, theta + phi + M_PI);
- }
vec pos_trolley() const {
- return pos() + param->l * dir_trolley();
+ return pos() - vec::from_polar(param->l, theta + phi);
}
vec canon_curve_center() const {
- double delta = cos(theta) * sin(theta + phi) - sin(theta) * cos(theta + phi);
- assert(delta != 0);
-
vec a = pos();
vec b = pos_trolley();
- vec eth = vec::from_polar(1, theta);
- vec ethph = vec::from_polar(1, theta + phi);
- double xc = (vec::dot(a, eth) * sin(theta + phi) - vec::dot(b, ethph) * sin(theta)) / delta;
- double yc = (cos(theta) * vec::dot(b, ethph) - cos(theta + phi) * vec::dot(a, eth));
- return vec(xc, yc);
- }
- bool intersects(const obstacle &o) const; // intersects an obstacle ?
+ double u = b.x - a.x;
+ double v = b.y - a.y;
+
+ double dd = sin(theta) * (a.x - b.x) + cos(theta) * (b.y - a.y);
+ assert(dd != 0);
+ double lambda = (u * u + v * v) / dd;
+ return a + lambda * vec(- sin(theta), cos(theta));
+ }
+
+ bool intersects(const obstacle &o) const; // intersects an obstacle ?
};
struct problem {