1/**
2 * @file SimWall2D.h
3 * @brief Implementation of walls for use with simulators
4 * @author Alex Cunningham
5 */
6
7#pragma once
8
9#include <gtsam_unstable/dllexport.h>
10#include <gtsam/geometry/Pose2.h>
11#include <gtsam/linear/Sampler.h>
12
13namespace gtsam {
14
15 /**
16 * General Wall class for walls defined around unordered endpoints
17 * Primarily to handle ray intersections
18 */
19 class GTSAM_UNSTABLE_EXPORT SimWall2D {
20 protected:
21 Point2 a_, b_;
22
23 public:
24 /** default constructor makes canonical wall */
25 SimWall2D() : a_(1.0, 0.0), b_(1.0, 1.0) {}
26
27 /** constructors using endpoints */
28 SimWall2D(const Point2& a, const Point2& b)
29 : a_(a), b_(b) {}
30
31 SimWall2D(double ax, double ay, double bx, double by)
32 : a_(ax, ay), b_(bx, by) {}
33
34 /** required by testable */
35 void print(const std::string& s="") const;
36 bool equals(const SimWall2D& other, double tol=1e-9) const;
37
38 /** access */
39 Point2 a() const { return a_; }
40 Point2 b() const { return b_; }
41
42 /** scales a wall to produce a new wall */
43 SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
44
45 /** geometry */
46 double length() const { return distance2(p1: a_, q: b_); }
47 Point2 midpoint() const;
48
49 /**
50 * intersection check between two segments
51 * returns true if they intersect, with the intersection
52 * point in the optional second argument
53 */
54 bool intersects(const SimWall2D& wall, Point2* pt = nullptr) const;
55
56 /**
57 * An overload of intersects that takes an l-value reference to a Point2
58 * instead of a pointer.
59 */
60 bool intersects(const SimWall2D& wall, Point2& pt) const {
61 return intersects(wall, pt: &pt);
62 }
63
64 /**
65 * norm is a 2D point representing the norm of the wall
66 */
67 Point2 norm() const;
68
69 /**
70 * reflection around a point of impact with a wall from a starting (init) point
71 * at a given impact point (intersection), returning the angle away from the impact
72 */
73 Rot2 reflection(const Point2& init, const Point2& intersection) const;
74
75 };
76
77 typedef std::vector<SimWall2D> SimWall2DVector;
78
79 /// traits
80 template<> struct traits<SimWall2D> : public Testable<SimWall2D> {};
81
82 /**
83 * Calculates the next pose in a trajectory constrained by walls, with noise on
84 * angular drift and reflection noise
85 * @param cur_pose is the pose of the robot
86 * @param step_size is the size of the forward step the robot tries to take
87 * @param walls is a set of walls to use for bouncing
88 * @param angle_drift is a sampler for angle drift (dim=1)
89 * @param reflect_noise is a sampler for scatter after hitting a wall (dim=3)
90 * @return the next pose for the robot
91 * NOTE: samplers cannot be const
92 */
93 std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
94 const std::vector<SimWall2D> walls, Sampler& angle_drift,
95 Sampler& reflect_noise, const Rot2& bias = Rot2());
96
97} // \namespace gtsam
98