RoboidControl-cpp/LinearAlgebra/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.ToFloat(), 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.ToFloat(), 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.ToFloat(), -45.0F, 1.0e-05) << "s.angle -1 1";
}

TEST(Polar, FromSpherical) {
  Spherical s;
  Polar p;

  s = Spherical(1, 0, 0);
  p = Polar::FromSpherical(s);

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

  s = Spherical(1, 45, 0);
  p = Polar::FromSpherical(s);

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

  s = Spherical(1, -45, 0);
  p = Polar::FromSpherical(s);

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

  s = Spherical(0, 0, 0);
  p = Polar::FromSpherical(s);

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

  s = Spherical(-1, 0, 0);
  p = Polar::FromSpherical(s);

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

  s = Spherical(0, 0, 90);
  p = Polar::FromSpherical(s);

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

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

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

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

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

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

TEST(Polar, Subtraction) {
  Polar v1 = Polar(4, 45);
  Polar v2 = Polar(1, -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, 45);
  Polar v2 = Polar(1, -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, -45);
  r = v1 + v2;
  EXPECT_FLOAT_EQ(r.distance, sqrtf(2)) << "Addition(0 0 0)";
  EXPECT_FLOAT_EQ(r.angle.ToFloat(), 0) << "Addition(0 0 0)";
}

TEST(Polar, Scale_Multiply) {
  Polar v1 = Polar(4, 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.ToFloat(), v1.angle.ToFloat())
      << "ScaleMult(4 45, 2)";
}

TEST(Polar, Scale_Divide) {
  Polar v1 = Polar(4, 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.ToFloat(), v1.angle.ToFloat()) << "ScaleDiv(4 45, 2)";
}

TEST(Polar, Distance) {
  Polar v1 = Polar(4, 45);
  Polar v2 = Polar(1, -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, 45);
  Polar r = Polar::zero;

  r = Polar::Rotate(v, 45);
  EXPECT_FLOAT_EQ(r.distance, v.distance) << "Rotate(4 45, 45)";
  EXPECT_FLOAT_EQ(r.angle.ToFloat(), 90.0f) << "Rotate(4 45, 45)";
}

#endif