RoboidControl-cpp/test/Polar_test.cc

#if GTEST
#include <gtest/gtest.h>
#include <limits>
#include <math.h>

#include "Polar.h"
#include "Spherical.h"

#define FLOAT_INFINITY std::numeric_limits<float>::infinity()

TEST(Polar, FromVector2) {
  Vector2 v = Vector2(0, 1);
  PolarSingle p = PolarSingle::FromVector2(v);

  EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 0 1";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "s.angle 0 0 1";

  v = Vector2(1, 0);
  p = PolarSingle::FromVector2(v);

  EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance 1 0";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), 90.0F) << "s.angle 1 0";

  v = Vector2(-1, 1);
  p = PolarSingle::FromVector2(v);

  EXPECT_FLOAT_EQ(p.distance, sqrt(2.0F)) << "p.distance -1 1";
  EXPECT_NEAR(p.angle.InDegrees(), -45.0F, 1.0e-05) << "s.angle -1 1";
}

TEST(Polar, FromSpherical) {
  SphericalSingle s;
  PolarSingle p;

  s = SphericalSingle(1, DirectionSingle::forward);
  p = PolarSingle::FromSpherical(s);

  EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 0 0)";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(1 0 0)";

  s = SphericalSingle(1, AngleSingle::Degrees(45), AngleSingle::Degrees(0));
  p = PolarSingle::FromSpherical(s);

  EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 45 0)";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), 45.0F)
      << "p.angle FromSpherical(1 45 0)";

  s = SphericalSingle(1, AngleSingle::Degrees(-45), AngleSingle::Degrees(0));
  p = PolarSingle::FromSpherical(s);

  EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(1 -45 0)";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), -45.0F)
      << "p.angle FromSpherical(1 -45 0)";

  s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(0));
  p = PolarSingle::FromSpherical(s);

  EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 0)";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 0)";

  s = SphericalSingle(-1, AngleSingle::Degrees(0), AngleSingle::Degrees(0));
  p = PolarSingle::FromSpherical(s);

  EXPECT_FLOAT_EQ(p.distance, 1.0F) << "p.distance FromSpherical(-1 0 0)";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), -180.0F)
      << "p.angle FromSpherical(-1 0 0)";

  s = SphericalSingle(0, AngleSingle::Degrees(0), AngleSingle::Degrees(90));
  p = PolarSingle::FromSpherical(s);

  EXPECT_FLOAT_EQ(p.distance, 0.0F) << "p.distance FromSpherical(0 0 90)";
  EXPECT_FLOAT_EQ(p.angle.InDegrees(), 0.0F) << "p.angle FromSpherical(0 0 90)";
}

TEST(Polar, Negation) {
  PolarSingle v = PolarSingle(2, AngleSingle::Degrees(45));
  PolarSingle r = PolarSingle::zero;

  r = -v;
  EXPECT_FLOAT_EQ(r.distance, 2);
  EXPECT_FLOAT_EQ(r.angle.InDegrees(), -135);
  EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(-135)))
      << "Negate(2 45)";

  v = PolarSingle::Deg(2, -45);
  r = -v;
  EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(135)))
      << "Negate(2 -45)";

  v = PolarSingle::Degrees(2, 0);
  r = -v;
  EXPECT_TRUE(r == PolarSingle(2, AngleSingle::Degrees(180))) << "Negate(2 0)";

  v = PolarSingle(0, AngleSingle::Degrees(0));
  r = -v;
  EXPECT_FLOAT_EQ(r.distance, 0.0f);
  EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0.0f);
  EXPECT_TRUE(r == PolarSingle(0, AngleSingle::Degrees(0))) << "Negate(0 0)";
}

TEST(Polar, Subtraction) {
  PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
  PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
  PolarSingle r = PolarSingle::zero;

  r = v1 - v2;
  // don't know what to expect yet

  v2 = PolarSingle::zero;
  r = v1 - v2;
  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Subtraction(0 0)";
}

TEST(Polar, Addition) {
  PolarSingle v1 = PolarSingle(1, AngleSingle::Degrees(45));
  PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
  PolarSingle r = PolarSingle::zero;

  r = v1 - v2;
  // don't know what to expect yet

  v2 = PolarSingle::zero;
  r = v1 + v2;
  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";

  r = v1;
  r += v2;
  EXPECT_FLOAT_EQ(r.distance, v1.distance) << "Addition(0 0)";

  v2 = PolarSingle(1, AngleSingle::Degrees(-45));
  r = v1 + v2;
  EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(0 0 0)";
  EXPECT_FLOAT_EQ(r.angle.InDegrees(), 0) << "Addition(0 0 0)";
}

TEST(Polar, Scale_Multiply) {
  PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
  PolarSingle r = PolarSingle::zero;

  r = v1 * 2.0f;
  EXPECT_FLOAT_EQ(r.distance, v1.distance * 2) << "ScaleMult(4 45, 2)";
  EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees())
      << "ScaleMult(4 45, 2)";
}

TEST(Polar, Scale_Divide) {
  PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
  PolarSingle r = PolarSingle::zero;

  r = v1 / 2.0f;
  EXPECT_FLOAT_EQ(r.distance, v1.distance / 2) << "ScaleDiv(4 45, 2)";
  EXPECT_FLOAT_EQ(r.angle.InDegrees(), v1.angle.InDegrees())
      << "ScaleDiv(4 45, 2)";
}

TEST(Polar, Distance) {
  PolarSingle v1 = PolarSingle(4, AngleSingle::Degrees(45));
  PolarSingle v2 = PolarSingle(1, AngleSingle::Degrees(-90));
  float d = 0;

  d = PolarSingle::Distance(v1, v2);
  // don't know what to expect yet

  v2 = PolarSingle::zero;
  d = PolarSingle::Distance(v1, v2);
  EXPECT_FLOAT_EQ(d, v1.distance) << "Distance(4 45, zero)";
}

TEST(Polar, Rotate) {
  PolarSingle v = PolarSingle(4, AngleSingle::Degrees(45));
  PolarSingle r = PolarSingle::zero;

  r = PolarSingle::Rotate(v, AngleSingle::Degrees(45));
  EXPECT_FLOAT_EQ(r.distance, v.distance) << "Rotate(4 45, 45)";
  EXPECT_FLOAT_EQ(r.angle.InDegrees(), 90.0f) << "Rotate(4 45, 45)";
}

// Performance Test
TEST(PolarOfTest, PerformanceTest) {
  const int numIterations = 1000000; // Number of instances to test
  std::vector<PolarOf<float>> polarObjects;

  // Measure time for creating a large number of PolarOf objects
  auto start = std::chrono::high_resolution_clock::now();

  for (int i = 0; i < numIterations; ++i) {
    float distance =
        static_cast<float>(rand() % 100); // Random distance from 0 to 100
    AngleOf<float> angle = AngleOf<float>::Degrees(
        static_cast<float>(rand() % 360)); // Random angle from 0 to 360 degrees
    PolarOf<float> p = PolarOf<float>(distance, angle);
    polarObjects.emplace_back(p); // Create and store the object
  }

  auto end = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> duration = end - start;
  std::cout << "Time to construct " << numIterations
            << " PolarOf objects: " << duration.count() << " seconds."
            << std::endl;

  // Test completion with a message
  ASSERT_GE(duration.count(), 0); // Ensure duration is non-negative

  // Assert that the duration is less than or equal to 1 second
  ASSERT_LE(duration.count(), 1.0)
      << "Performance test failed: Construction took longer than 1 second.";
}

// Edge Case 1: Testing with distance = 0 and angle = 45
TEST(PolarOfTest, TestDistanceZero) {
  PolarOf<float> p1(0.0f, AngleOf<float>::Degrees(45.0f));
  EXPECT_EQ(p1.distance, 0.0f);          // Ensure distance is 0
  EXPECT_EQ(p1.angle.InDegrees(), 0.0f); // Ensure angle is 0 when distance is 0
}

// Edge Case 2: Testing with negative distance, angle should be adjusted
TEST(PolarOfTest, TestNegativeDistance) {
  PolarOf<float> p2(-10.0f, AngleOf<float>::Degrees(90.0f));
  EXPECT_EQ(p2.distance, 10.0f); // Ensure distance is positive
  EXPECT_NEAR(p2.angle.InDegrees(), -90.0f,
              0.0001f); // Ensure angle is normalized to 270 degrees (180 + 90)
}

// Edge Case 3: Testing with positive distance and angle = 180
TEST(PolarOfTest, TestPositiveDistance) {
  PolarOf<float> p3(100.0f, AngleOf<float>::Degrees(180.0f));
  EXPECT_EQ(p3.distance, 100.0f); // Ensure distance is correct
  EXPECT_NEAR(p3.angle.InDegrees(), -180.0f,
              0.0001f); // Ensure angle is correct
}

#endif