1/**
2 * @file testSimWall2D2D
3 * @author Alex Cunningham
4 */
5
6#include <CppUnitLite/TestHarness.h>
7#include <gtsam/base/TestableAssertions.h>
8
9#include <gtsam_unstable/geometry/SimWall2D.h>
10
11using namespace gtsam;
12
13const double tol = 1e-5;
14
15/* ************************************************************************* */
16TEST(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/* ************************************************************************* */
26TEST(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/* ************************************************************************* */
35TEST(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(wall: w2, pt);
41 EXPECT(assert_equal(Point2(4.0, 2.0), pt, tol));
42}
43
44/* ************************************************************************* */
45TEST(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/* ************************************************************************* */
53TEST(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);
58 Rot2 expected = Rot2::fromDegrees(theta: 45);
59 EXPECT(assert_equal(expected, actual1, tol));
60 EXPECT(assert_equal(expected, actual2, tol));
61}
62
63/* ************************************************************************* */
64int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
65/* ************************************************************************* */
66