Go to the documentation of this file.
16 std::cout <<
"SimWall2D " <<
s <<
":" << std::endl;
29 const bool debug =
false;
37 double len =
A.length();
38 if (
debug) cout <<
"len: " <<
len << endl;
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;
53 if (Ba.y() == 0.0 && Ba.x() > 0.0 && Ba.x() <
len) {
55 if (
debug) cout <<
"Ba on the line" << endl;
57 }
else if (Bb.y() == 0.0 && Bb.x() > 0.0 && Bb.x() <
len) {
59 if (
debug) cout <<
"Bb on the line" << endl;
65 if (
debug) cout <<
"vertical line" << endl;
66 if (Ba.x() <
len && Ba.x() > 0.0) {
68 if (
debug) cout <<
" within range" << endl;
71 if (
debug) cout <<
" not within range" << endl;
77 Point2 low(0,0), high(0,0);
78 if (Ba.y() > Bb.y()) {
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(low.y())/slope : high.x() -
std::abs(high.y())/slope;
92 if (
debug) cout <<
"xintercept " << xint << endl;
93 if (xint > 0.0 && xint <
len) {
97 if (
debug) cout <<
"xintercept out of range" << endl;
105 return a_ + vec * 0.5 * vec.norm();
111 return Point2(vec.y(), -vec.x());
120 Point2 t_goal(-t_init.x(), t_init.y());
126 const std::vector<SimWall2D> walls,
Sampler& angle_drift,
136 bool collision =
false;
Point2 intersection(1
e+10, 1
e+10);
140 if (wall.intersects(traj,cur_intersection)) {
143 intersection = cur_intersection;
155 norm = norm / norm.norm();
158 if (
distance2(cur_pose.
t(), intersection + norm) >
distance2(cur_pose.
t(), intersection - norm))
162 const double inside_bias = 0.05;
163 pose =
Pose2(closest_wall.
reflection(cur_pose.
t(), intersection), intersection + inside_bias * norm);
169 return make_pair(
pose, collision);
bool equals(const SimWall2D &other, double tol=1e-9) const
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
const Point2 & t() const
translation
static Rot2 relativeBearing(const Point2 &d, OptionalJacobian< 1, 2 > H={})
const Vector3 bias(1, 2, 3)
static const Point3 pt(1.0, 2.0, 3.0)
bool intersects(const SimWall2D &wall, Point2 *pt=nullptr) const
Implementation of walls for use with simulators.
Rot2 reflection(const Point2 &init, const Point2 &intersection) const
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
static constexpr bool debug
static Vector1 Logmap(const Rot2 &r, ChartJacobian H={})
Log map at identity - return the canonical coordinates of this rotation.
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1={}, OptionalJacobian< 2, 2 > H2={}) const
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)
Vector sample() const
sample from distribution
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
size_t len(handle h)
Get the length of a Python object.
void print(const std::string &s="") const
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:19