SimWall2D.h

Go to the documentation of this file.

#pragma once

#include <gtsam_unstable/dllexport.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/Sampler.h>

namespace gtsam {

  class GTSAM_UNSTABLE_EXPORT SimWall2D {
  protected:
    Point2 a_, b_;

  public:
    SimWall2D() : a_(1.0, 0.0), b_(1.0, 1.0) {}

    SimWall2D(const Point2& a, const Point2& b)
      : a_(a), b_(b) {}

    SimWall2D(double ax, double ay, double bx, double by)
      : a_(ax, ay), b_(bx, by) {}

    void print(const std::string& s="") const;
    bool equals(const SimWall2D& other, double tol=1e-9) const;

    Point2 a() const { return a_; }
    Point2 b() const { return b_; }

    SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }

    double length() const { return distance2(a_, b_); }
    Point2 midpoint() const;

    bool intersects(const SimWall2D& wall, Point2* pt = nullptr) const;
    
    bool intersects(const SimWall2D& wall, Point2& pt) const {
      return intersects(wall, &pt);
    }

    Point2 norm() const;

    Rot2 reflection(const Point2& init, const Point2& intersection) const;

  };

  typedef std::vector<SimWall2D> SimWall2DVector;

  template<> struct traits<SimWall2D> : public Testable<SimWall2D> {};

  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 = Rot2());

} // \namespace gtsam