SimWall2D.h
Go to the documentation of this file.
1 
7 #pragma once
8 
9 #include <gtsam_unstable/dllexport.h>
10 #include <gtsam/geometry/Pose2.h>
11 #include <gtsam/linear/Sampler.h>
12 
13 namespace gtsam {
14 
19  class GTSAM_UNSTABLE_EXPORT SimWall2D {
20  protected:
21  Point2 a_, b_;
22 
23  public:
25  SimWall2D() : a_(1.0, 0.0), b_(1.0, 1.0) {}
26 
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 
35  void print(const std::string& s="") const;
36  bool equals(const SimWall2D& other, double tol=1e-9) const;
37 
39  Point2 a() const { return a_; }
40  Point2 b() const { return b_; }
41 
43  SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
44 
46  double length() const { return distance2(a_, b_); }
47  Point2 midpoint() const;
48 
54  bool intersects(const SimWall2D& wall, Point2* pt = nullptr) const;
55 
60  bool intersects(const SimWall2D& wall, Point2& pt) const {
61  return intersects(wall, &pt);
62  }
63 
67  Point2 norm() const;
68 
73  Rot2 reflection(const Point2& init, const Point2& intersection) const;
74 
75  };
76 
77  typedef std::vector<SimWall2D> SimWall2DVector;
78 
80  template<> struct traits<SimWall2D> : public Testable<SimWall2D> {};
81 
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
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
SimWall2D scale(double s) const
Definition: SimWall2D.h:43
double length() const
Definition: SimWall2D.h:46
Vector2 Point2
Definition: Point2.h:32
std::pair< Pose2, bool > moveWithBounce(const Pose2 &cur_pose, double step_size, const std::vector< SimWall2D > walls, Sampler &angle_drift, Sampler &reflect_noise, const Rot2 &bias)
Definition: SimWall2D.cpp:125
std::vector< SimWall2D > SimWall2DVector
Definition: SimWall2D.h:77
static const Point3 pt(1.0, 2.0, 3.0)
Point2 a() const
Definition: SimWall2D.h:39
SimWall2D(double ax, double ay, double bx, double by)
Definition: SimWall2D.h:31
SimWall2D(const Point2 &a, const Point2 &b)
Definition: SimWall2D.h:28
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
sampling from a NoiseModel
const G & b
Definition: Group.h:86
bool intersects(const SimWall2D &wall, Point2 &pt) const
Definition: SimWall2D.h:60
traits
Definition: chartTesting.h:28
Point2 b() const
Definition: SimWall2D.h:40
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
const G double tol
Definition: Group.h:86
2D Pose


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:49