testSimWall2D.cpp
Go to the documentation of this file.
1 
8 
10 
11 using namespace gtsam;
12 
13 const double tol = 1e-5;
14 
15 /* ************************************************************************* */
16 TEST(testSimWall2D2D, construction ) {
17  Point2 a(1.0, 0.0), b(1.0, 2.0);
18  SimWall2D wall1(a, b), wall2(a.x(), a.y(), b.x(), b.y());
19  EXPECT(assert_equal(a, wall1.a(), tol));
20  EXPECT(assert_equal(a, wall2.a(), tol));
21  EXPECT(assert_equal(b, wall1.b(), tol));
22  EXPECT(assert_equal(b, wall2.b(), tol));
23 }
24 
25 /* ************************************************************************* */
26 TEST(testSimWall2D2D, equals ) {
27  Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3(0,0);
28  SimWall2D w1(p1, p2), w2(p1, p3);
29  EXPECT(assert_equal(w1, w1));
30  EXPECT(assert_inequal(w1, w2));
31  EXPECT(assert_inequal(w2, w1));
32 }
33 
34 /* ************************************************************************* */
35 TEST(testSimWall2D2D, intersection1 ) {
36  SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0);
37  Point2 pt(0,0);
38  EXPECT(w1.intersects(w2));
39  EXPECT(w2.intersects(w1));
40  w1.intersects(w2, pt);
41  EXPECT(assert_equal(Point2(4.0, 2.0), pt, tol));
42 }
43 
44 /* ************************************************************************* */
45 TEST(testSimWall2D2D, intersection2 ) {
46  SimWall2D traj(7.07107, 7.07107, 0, 0);
47  SimWall2D wall(1.5, 3, 1.5, 1);
48  EXPECT(wall.intersects(traj));
49  EXPECT(traj.intersects(wall));
50 }
51 
52 /* ************************************************************************* */
53 TEST(testSimWall2D2D, reflection1 ) {
54  SimWall2D wall1(1.0, 1.0, 7.0, 1.0), wall2(7.0, 1.0, 1.0, 1.0);
55  Point2 init(2.0, 3.0), intersection(4.0, 1.0);
56  Rot2 actual1 = wall1.reflection(init, intersection);
57  Rot2 actual2 = wall2.reflection(init, intersection);
59  EXPECT(assert_equal(expected, actual1, tol));
60  EXPECT(assert_equal(expected, actual2, tol));
61 }
62 
63 /* ************************************************************************* */
64 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
65 /* ************************************************************************* */
Provides additional testing facilities for common data structures.
static Point3 p3
static int runAllTests(TestResult &result)
Vector3f p1
Matrix expected
Definition: testMatrix.cpp:971
Vector2 Point2
Definition: Point2.h:32
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
static const Point3 pt(1.0, 2.0, 3.0)
int main()
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition: Rot2.h:66
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:60
bool intersects(const SimWall2D &wall, Point2 *pt=nullptr) const
Definition: SimWall2D.cpp:28
#define EXPECT(condition)
Definition: Test.h:150
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const G & b
Definition: Group.h:86
traits
Definition: chartTesting.h:28
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
Implementation of walls for use with simulators.
static Point3 p2
const G double tol
Definition: Group.h:86
TEST(SmartFactorBase, Pinhole)
Rot2 reflection(const Point2 &init, const Point2 &intersection) const
Definition: SimWall2D.cpp:115


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:31