1/**
2 * @file SimWall2D.cpp
3 * @author Alex Cunningham
4 */
5
6
7#include <gtsam/geometry/Pose2.h>
8#include <gtsam_unstable/geometry/SimWall2D.h>
9
10namespace gtsam {
11
12using namespace std;
13
14/* ************************************************************************* */
15void SimWall2D::print(const std::string& s) const {
16 std::cout << "SimWall2D " << s << ":" << std::endl;
17 traits<Point2>::Print(m: a_, str: " a");
18 traits<Point2>::Print(m: b_, str: " b");
19}
20
21/* ************************************************************************* */
22bool SimWall2D::equals(const SimWall2D& other, double tol) const {
23 return traits<Point2>::Equals(v1: a_, v2: other.a_, tol) &&
24 traits<Point2>::Equals(v1: b_, v2: other.b_, tol);
25}
26
27/* ************************************************************************* */
28bool SimWall2D::intersects(const SimWall2D& B, Point2* pt) const {
29 const bool debug = false;
30
31 const SimWall2D& A = *this;
32
33 // translate so A.a is at the origin, then rotate so that A.b is along x axis
34 Pose2 transform(Rot2::relativeBearing(d: A.b() - A.a()), A.a());
35
36 // normalized points, Aa at origin, Ab at (length, 0.0)
37 double len = A.length();
38 if (debug) cout << "len: " << len << endl;
39 Point2 Ba = transform.transformTo(point: B.a()),
40 Bb = transform.transformTo(point: B.b());
41 if (debug) traits<Point2>::Print(m: Ba, str: "Ba");
42 if (debug) traits<Point2>::Print(m: Bb, str: "Bb");
43
44 // check sides of line
45 if (Ba.y() * Bb.y() > 0.0 ||
46 (Ba.x() * Bb.x() > 0.0 && Ba.x() < 0.0) ||
47 (Ba.x() > len && Bb.x() > len)) {
48 if (debug) cout << "Failed first check" << endl;
49 return false;
50 }
51
52 // check conditions for exactly on the same line
53 if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() < len) {
54 if (pt) *pt = transform.transformFrom(point: Ba);
55 if (debug) cout << "Ba on the line" << endl;
56 return true;
57 } else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() < len) {
58 if (pt) *pt = transform.transformFrom(point: Bb);
59 if (debug) cout << "Bb on the line" << endl;
60 return true;
61 }
62
63 // handle vertical case to avoid calculating slope
64 if (std::abs(x: Ba.x() - Bb.x()) < 1e-5) {
65 if (debug) cout << "vertical line" << endl;
66 if (Ba.x() < len && Ba.x() > 0.0) {
67 if (pt) *pt = transform.transformFrom(point: Point2(Ba.x(), 0.0));
68 if (debug) cout << " within range" << endl;
69 return true;
70 } else {
71 if (debug) cout << " not within range" << endl;
72 return false;
73 }
74 }
75
76 // find lower point by y
77 Point2 low(0,0), high(0,0);
78 if (Ba.y() > Bb.y()) {
79 high = Ba;
80 low = Bb;
81 } else {
82 high = Bb;
83 low = Ba;
84 }
85 if (debug) traits<Point2>::Print(m: high, str: "high");
86 if (debug) traits<Point2>::Print(m: low, str: "low");
87
88 // find x-intercept
89 double slope = (high.y() - low.y())/(high.x() - low.x());
90 if (debug) cout << "slope " << slope << endl;
91 double xint = (low.x() < high.x()) ? low.x() + std::abs(x: low.y())/slope : high.x() - std::abs(x: high.y())/slope;
92 if (debug) cout << "xintercept " << xint << endl;
93 if (xint > 0.0 && xint < len) {
94 if (pt) *pt = transform.transformFrom(point: Point2(xint, 0.0));
95 return true;
96 } else {
97 if (debug) cout << "xintercept out of range" << endl;
98 return false;
99 }
100}
101
102/* ************************************************************************* */
103Point2 SimWall2D::midpoint() const {
104 Point2 vec = b_ - a_;
105 return a_ + vec * 0.5 * vec.norm();
106}
107
108/* ************************************************************************* */
109Point2 SimWall2D::norm() const {
110 Point2 vec = b_ - a_;
111 return Point2(vec.y(), -vec.x());
112}
113
114/* ************************************************************************* */
115Rot2 SimWall2D::reflection(const Point2& init, const Point2& intersection) const {
116 // translate to put the intersection at the origin and the wall along the x axis
117 Rot2 wallAngle = Rot2::relativeBearing(d: b_ - a_);
118 Pose2 transform(wallAngle, intersection);
119 Point2 t_init = transform.transformTo(point: init);
120 Point2 t_goal(-t_init.x(), t_init.y());
121 return Rot2::relativeBearing(d: wallAngle.rotate(p: t_goal));
122}
123
124/* ***************************************************************** */
125std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
126 const std::vector<SimWall2D> walls, Sampler& angle_drift,
127 Sampler& reflect_noise, const Rot2& bias) {
128
129 // calculate angle to change by
130 Rot2 dtheta = Rot2::fromAngle(theta: angle_drift.sample()(0) + bias.theta());
131 Pose2 test_pose = cur_pose.retract(v: (Vector(3) << step_size, 0.0, Rot2::Logmap(r: dtheta)(0)).finished());
132
133 // create a segment to use for intersection checking
134 // find the closest intersection
135 SimWall2D traj(test_pose.t(), cur_pose.t());
136 bool collision = false; Point2 intersection(1e+10, 1e+10);
137 SimWall2D closest_wall;
138 for(const SimWall2D& wall: walls) {
139 Point2 cur_intersection;
140 if (wall.intersects(wall: traj,pt&: cur_intersection)) {
141 collision = true;
142 if (distance2(p1: cur_pose.t(), q: cur_intersection) < distance2(p1: cur_pose.t(), q: intersection)) {
143 intersection = cur_intersection;
144 closest_wall = wall;
145 }
146 }
147 }
148
149 // reflect off of wall with some noise
150 Pose2 pose(test_pose);
151 if (collision) {
152
153 // make sure norm is on the robot's side
154 Point2 norm = closest_wall.norm();
155 norm = norm / norm.norm();
156
157 // Simple check to make sure norm is on side closest robot
158 if (distance2(p1: cur_pose.t(), q: intersection + norm) > distance2(p1: cur_pose.t(), q: intersection - norm))
159 norm = - norm;
160
161 // using the reflection
162 const double inside_bias = 0.05;
163 pose = Pose2(closest_wall.reflection(init: cur_pose.t(), intersection), intersection + inside_bias * norm);
164
165 // perturb the rotation for better exploration
166 pose = pose.retract(v: reflect_noise.sample());
167 }
168
169 return make_pair(x&: pose, y&: collision);
170}
171/* ***************************************************************** */
172
173} // \namespace gtsam
174