testGeometryUtilities.cc

Go to the documentation of this file.

#include <cmath>
#include <utl/CoordinateSystem.h>
#include <utl/GeometryUtilities.h>
#include <utl/Math.h>
#include <utl/MathConstants.h>
#include <utl/Test.h>

#include <cppunit/extensions/HelperMacros.h>

#include <iostream>

using namespace std;
using namespace utl;


#define TEST(x...) CPPUNIT_ASSERT(Test x)


class testGeometryUtilities : public CppUnit::TestFixture {

  CPPUNIT_TEST_SUITE(testGeometryUtilities);
  CPPUNIT_TEST(testNormalizeAngleMinusPiPi);
  CPPUNIT_TEST(testPlaneCtor);
  CPPUNIT_TEST(testLineCtor);
  CPPUNIT_TEST(testLineLineDistance);
  CPPUNIT_TEST(testLinePointDistance);
  CPPUNIT_TEST(testPlanePointDistance);
  CPPUNIT_TEST(testPlanePlaneIntersection1);
  CPPUNIT_TEST(testPlanePlaneIntersection2);
  CPPUNIT_TEST(testPlanePlaneIntersection3);
  CPPUNIT_TEST(testLinePlaneIntersection1);
  CPPUNIT_TEST(testPerpendicularProjection);
  CPPUNIT_TEST_SUITE_END();

private:
  void
  CheckIntersection(const Plane& plane1, const Plane& plane2,
                    const Vector& resDirection, const Point& resAnchor)
    const
  {
    const Line line = Intersection(plane1, plane2);
    const Vector& dir = line.GetDirection();
    const Point& anch = line.GetAnchor();
    TEST(<CloseTo>(Distance(plane1, anch), 0.));
    TEST(<CloseTo>(Distance(plane2, anch), 0.));
    TEST(<CloseTo>(Distance(line, anch), 0.));
    TEST(<CloseTo>(dir.GetCoordinates(fCS), resDirection.GetCoordinates(fCS)));
    TEST(<CloseTo>(anch.GetCoordinates(fCS), resAnchor.GetCoordinates(fCS)));
  }

  void
  CheckIntersection(const Plane& plane, const Line& line,
                    const Point& resPoint)
    const
  {
    const Point point = Intersection(plane, line);
    TEST(<CloseTo>(Distance(plane, point), 0.));
    TEST(<CloseTo>(Distance(line, point), 0.));
    TEST(<CloseTo>(point.GetCoordinates(fCS), resPoint.GetCoordinates(fCS)));
  }

  void
  CheckProjection(const Plane& plane, const Point& point,
                  const Point& resPoint)
    const
  {
    TEST(<CloseTo>(Distance(plane, resPoint), 0.));
    const Point proj = PerpendicularProjection(plane, point);
    TEST(<CloseTo>(Distance(plane, proj), 0.));
  }

  CoordinateSystemPtr fCS;

public:
  void setUp() { fCS = CoordinateSystem::GetRootCoordinateSystem(); }

  void tearDown() { }

  void
  testNormalizeAngleMinusPiPi()
  {
    TEST(<Equal>(NormalizeAngleMinusPiPi(2.), 2.));
    TEST(<Equal>(NormalizeAngleMinusPiPi(-2.), -2.));
    TEST(<CloseTo>(NormalizeAngleMinusPiPi(0.1 + kPi), 0.1 - kPi));
    TEST(<CloseTo>(NormalizeAngleMinusPiPi(0.1 + 2*kPi), 0.1));
    TEST(<CloseTo>(NormalizeAngleMinusPiPi(0.1 + 3*kPi), 0.1 - kPi));
    TEST(<CloseTo>(NormalizeAngleMinusPiPi(-0.1 - kPi), -0.1 + kPi));
    TEST(<CloseTo>(NormalizeAngleMinusPiPi(-0.1 - 2*kPi), -0.1));
    TEST(<CloseTo>(NormalizeAngleMinusPiPi(-0.1 - 3*kPi), -0.1 + kPi));
  }

  void
  testPlaneCtor()
  {
    const Plane plane(Point(1,2,3, fCS), Vector(4,5,6, fCS));
    TEST(<Equal>(plane.GetAnchor().GetCoordinates(fCS), Triple(1,2,3)));
    TEST(<CloseTo>(plane.GetNormal().GetMag(), 1.));
  }

  void
  testLineCtor()
  {
    const Line line(Point(1,2,3, fCS), Vector(4,5,6, fCS));
    TEST(<Equal>(line.GetAnchor().GetCoordinates(fCS), Triple(1,2,3)));
    TEST(<CloseTo>(line.GetDirection().GetMag(), 1.));
  }

  void
  testLineLineDistance()
  {
    const Line line1(Point(1,0,0, fCS), Vector(1,0,0, fCS));
    const Line line2(Point(0,1,0, fCS), Vector(0,0,1, fCS));
    const Line line3(Point(0,1,1, fCS), Vector(0,0,1, fCS));
    TEST(<CloseTo>(Distance(line1, line2), 1.));
    TEST(<CloseTo>(Distance(line1, line3), 1.));
  }

  void
  testLinePointDistance()
  {
    const Line line(Point(0,0,0, fCS), Vector(1,0,0, fCS));
    TEST(<CloseTo>(Distance(line, Point(3,0,1, fCS)), 1.));
    TEST(<CloseTo>(Distance(line, Point(3,0,1, fCS)+line.GetDirection()), 1.));
    TEST(<CloseTo>(Distance(line, Point(-1,0,0, fCS)), 0.));
    TEST(<CloseTo>(Distance(line, Point(1,1,1, fCS)), sqrt(2.)));
  }

  void
  testPlanePointDistance()
  {
    const Plane plane(Point(1,1,1, fCS), Vector(1,1,1, fCS));
    TEST(<CloseTo>(Distance(plane, Point(0,0,0, fCS)), -sqrt(3.)));
    TEST(<CloseTo>(Distance(plane, Point(2,2,2, fCS)),  sqrt(3.)));
    TEST(<CloseTo>(Distance(plane, Point(2,2,2, fCS)+Vector(-1,0,1, fCS)), sqrt(3.)));
  }

  void
  testPlanePlaneIntersection1()
  {
    // x=0 and y=0 planes
    CheckIntersection(Plane(Point(0,0,0, fCS), Vector(1,0,0, fCS)),
                      Plane(Point(0,0,0, fCS), Vector(0,1,0, fCS)),
                      Vector(0,0,1, fCS),
                      Point(0,0,0, fCS));
    CheckIntersection(Plane(Point(1,0,0, fCS), Vector(1,0,0, fCS)),
                      Plane(Point(0,0,0, fCS), Vector(0,1,0, fCS)),
                      Vector(0,0,1, fCS),
                      Point(1,0,0, fCS));
    CheckIntersection(Plane(Point(0,0,0, fCS), Vector(1,0,0, fCS)),
                      Plane(Point(0,1,0, fCS), Vector(0,1,0, fCS)),
                      Vector(0,0,1, fCS),
                      Point(0,1,0, fCS));
    CheckIntersection(Plane(Point(1,0,0, fCS), Vector(1,0,0, fCS)),
                      Plane(Point(0,1,0, fCS), Vector(0,1,0, fCS)),
                      Vector(0,0,1, fCS),
                      Point(1,1,0, fCS));
  }

  void
  testPlanePlaneIntersection2()
  {
    CheckIntersection(Plane(Point(0,2,2, fCS), Vector(1,0,0, fCS)),
                      Plane(Point(0,0,0, fCS), Vector(0,1,0, fCS)),
                      Vector(0,0,1, fCS),
                      Point(0,0,1, fCS));
  }

  void
  testPlanePlaneIntersection3()
  {
    CheckIntersection(Plane(Point(1,1,1, fCS), Vector(0,0,1, fCS)),
                      Plane(Point(2,2,0, fCS), Vector(1,1,1, fCS)),
                      Vector(-sqrt(2.)/2,sqrt(2.)/2,0, fCS),
                      Point(1.5,1.5,1, fCS));
  }

  void
  testLinePlaneIntersection1()
  {
    CheckIntersection(Plane(Point(0,0,0, fCS), Vector(1,0,0, fCS)),
                      Line(Point(1,1,1, fCS), Vector(-1,-1,1, fCS)),
                      Point(0,0,2, fCS));
  }

  void
  testPerpendicularProjection()
  {
    CheckProjection(Plane(Point(2,2,0, fCS), Vector(0,0,1, fCS)),
                    Point(1,1,1, fCS), Point(1,1,0, fCS));
  }

};


CPPUNIT_TEST_SUITE_REGISTRATION(testGeometryUtilities);