RoboidControl-cpp/test/Polar_test.cc

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

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

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

TEST(Polar, FromVector2) {
  Vector2 v = Vector2(0, 1);
  Polar p = Polar::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 = Polar::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 = Polar::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) {
  Spherical s;
  Polar p;

  s = Spherical(1, Angle::Degrees(0), Angle::Degrees(0));
  p = Polar::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 = Spherical(1, Angle::Degrees(45), Angle::Degrees(0));
  p = Polar::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 = Spherical(1, Angle::Degrees(-45), Angle::Degrees(0));
  p = Polar::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 = Spherical(0, Angle::Degrees(0), Angle::Degrees(0));
  p = Polar::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 = Spherical(-1, Angle::Degrees(0), Angle::Degrees(0));
  p = Polar::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 = Spherical(0, Angle::Degrees(0), Angle::Degrees(90));
  p = Polar::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) {
  Polar v = Polar(2, Angle::Degrees(45));
  Polar r = Polar::zero;

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

  v = Polar(2, Angle::Degrees(-45));
  r = -v;
  EXPECT_TRUE(r == Polar(2, Angle::Degrees(135))) << "Negate(2 -45)";

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

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

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

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

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

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

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

  v2 = Polar::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 = Polar(1, Angle::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) {
  Polar v1 = Polar(4, Angle::Degrees(45));
  Polar r = Polar::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) {
  Polar v1 = Polar(4, Angle::Degrees(45));
  Polar r = Polar::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) {
  Polar v1 = Polar(4, Angle::Degrees(45));
  Polar v2 = Polar(1, Angle::Degrees(-90));
  float d = 0;

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

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

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

  r = Polar::Rotate(v, Angle::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)";
}

#endif