RoboidControl-cpp/test/Vector3_test.cc

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

#include "Vector3.h"

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

TEST(Vector3, FromSpherical) {
  Vector3 v = Vector3(0, 0, 1);
  SphericalOf<float> s = SphericalOf<float>::FromVector3(v);
  Vector3 r = Vector3(s);

  EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 0 1";
  EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 0 0 1";
  EXPECT_FLOAT_EQ(r.Forward(), 1.0F) << "toVector3.z 0 0 1";

  v = Vector3(0, 1, 0);
  s = SphericalOf<float>::FromVector3(v);
  r = Vector3(s);

  EXPECT_FLOAT_EQ(r.Right(), 0.0F) << "toVector3.x 0 1 0";
  EXPECT_FLOAT_EQ(r.Up(), 1.0F) << "toVector3.y 0 1 0";
  EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 0 1 0";

  v = Vector3(1, 0, 0);
  s = SphericalOf<float>::FromVector3(v);
  r = Vector3(s);

  EXPECT_FLOAT_EQ(r.Right(), 1.0F) << "toVector3.x 1 0 0";
  EXPECT_NEAR(r.Up(), 0.0F, 1.0e-06) << "toVector3.y 1 0 0";
  EXPECT_NEAR(r.Forward(), 0.0F, 1.0e-06) << "toVector3.z 1 0 0";
}

TEST(Vector3, Magnitude) {
  Vector3 v = Vector3(1, 2, 3);
  float m = 0;

  m = v.magnitude();
  EXPECT_FLOAT_EQ(m, 3.741657F) << "v.magnitude 1 2 3";

  m = Vector3::Magnitude(v);
  EXPECT_FLOAT_EQ(m, 3.741657F) << "Vector3::Magnitude 1 2 3";

  v = Vector3(-1, -2, -3);
  m = v.magnitude();
  EXPECT_FLOAT_EQ(m, 3.741657F) << "v.magnitude -1 -2 -3";

  v = Vector3(0, 0, 0);
  m = v.magnitude();
  EXPECT_FLOAT_EQ(m, 0) << "v.magnitude 0 0 0 ";

  if (std::numeric_limits<float>::is_iec559) {
    v = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    m = v.magnitude();
    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
        << "v.magnitude INFINITY INFINITY INFINITY ";

    v = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    m = v.magnitude();
    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
        << "v.magnitude -INFINITY -INFINITY -INFINITY ";
  }
}

TEST(Vector3, SqrMagnitude) {
  Vector3 v = Vector3(1, 2, 3);
  float m = 0;

  m = v.sqrMagnitude();
  EXPECT_FLOAT_EQ(m, 14) << "v.sqrMagnitude 1 2 3";

  m = Vector3::SqrMagnitude(v);
  EXPECT_FLOAT_EQ(m, 14) << "Vector3::SqrMagnitude 1 2 3";

  v = Vector3(-1, -2, -3);
  m = v.sqrMagnitude();
  EXPECT_FLOAT_EQ(m, 14) << "v.sqrMagnitude -1 -2 -3";

  v = Vector3(0, 0, 0);
  m = v.sqrMagnitude();
  EXPECT_FLOAT_EQ(m, 0) << "v.sqrMagnitude 0 0 0 ";

  if (std::numeric_limits<float>::is_iec559) {
    v = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    m = v.sqrMagnitude();
    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
        << "v.sqrMagnitude INFINITY INFINITY INFINITY ";

    v = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    m = v.sqrMagnitude();
    EXPECT_FLOAT_EQ(m, FLOAT_INFINITY)
        << "v.sqrMagnitude -INFINITY -INFINITY -INFINITY ";
  }
}

TEST(Vector3, Normalize) {
  bool r = false;

  Vector3 v1 = Vector3(0, 2, 0);
  Vector3 v = Vector3::zero;

  v = v1.normalized();
  EXPECT_TRUE(v == Vector3(0, 1, 0)) << "v.normalized 0 2 0";

  v = Vector3::Normalize(v1);
  EXPECT_TRUE(v == Vector3(0, 1, 0)) << "Vector3::Normalize 0 2 0";

  v1 = Vector3(0, -2, 0);
  v = v1.normalized();
  EXPECT_TRUE(v == Vector3(0, -1, 0)) << "v.normalized 0 -2 0";

  v1 = Vector3(0, 0, 0);
  v = v1.normalized();
  EXPECT_TRUE(v == Vector3(0, 0, 0)) << "v.normalized 0 0 0";

  if (std::numeric_limits<float>::is_iec559) {
    v1 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = v1.normalized();
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "v.normalized INFINITY INFINITY INFINITY";

    v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = v1.normalized();
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "v.normalized -INFINITY -INFINITY -INFINITY";
  }
}

TEST(Vector3, Negate) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v = Vector3::zero;

  v = -v1;
  EXPECT_TRUE(v == Vector3(-4, -5, -6)) << "- 4 5 6";

  v1 = Vector3(-4, -5, -6);
  v = -v1;
  EXPECT_TRUE(v == Vector3(4, 5, 6)) << "- -4 -5 -6";

  v1 = Vector3(0, 0, 0);
  v = -v1;
  EXPECT_TRUE(v == Vector3(0, 0, 0)) << "- 0 0 0";

  if (std::numeric_limits<float>::is_iec559) {
    v1 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = -v1;
    EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
        << "- INFINITY INFINITY INFINITY";

    v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = -v1;
    EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
        << "- -INFINITY -INFINITY -INFINITY";
  }
}

TEST(Vector3, Subtract) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 v = Vector3::zero;

  v = v1 - v2;
  EXPECT_TRUE(v == Vector3(3, 3, 3)) << "4 5 6 - 1 2 3";

  v2 = Vector3(-1, -2, -3);
  v = v1 - v2;
  EXPECT_TRUE(v == Vector3(5, 7, 9)) << "4 5 6 - -1 -2 -3";

  v2 = Vector3(4, 5, 6);
  v = v1 - v2;
  EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 - 4 5 6";

  v2 = Vector3(0, 0, 0);
  v = v1 - v2;
  EXPECT_TRUE(v == Vector3(4, 5, 6)) << "4 5 6 - 0 0 0";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = v1 - v2;
    EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
        << "4 5 6 - INFINITY INFINITY INFINITY";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = v1 - v2;
    EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
        << "4 5 6 - -INFINITY -INFINITY -INFINITY";
  }
}

TEST(Vector3, Addition) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 v = Vector3::zero;

  v = v1 + v2;
  EXPECT_TRUE(v == Vector3(5, 7, 9)) << "4 5 6 + 1 2 3";

  v2 = Vector3(-1, -2, -3);
  v = v1 + v2;
  EXPECT_TRUE(v == Vector3(3, 3, 3)) << "4 5 6 + -1 -2 -3";

  v2 = Vector3(0, 0, 0);
  v = v1 + v2;
  EXPECT_TRUE(v == Vector3(4, 5, 6)) << "4 5 6 + 0 0 0";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = v1 + v2;
    EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
        << "4 5 6 + INFINITY INFINITY INFINITY";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = v1 + v2;
    EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
        << "4 5 6 + -INFINITY -INFINITY -INFINITY";
  }
}

TEST(Vector3, Scale) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 v = Vector3::zero;

  v = Vector3::Scale(v1, v2);
  EXPECT_TRUE(v == Vector3(4, 10, 18)) << "Scale 4 5 6 , 1 2 3";

  v2 = Vector3(-1, -2, -3);
  v = Vector3::Scale(v1, v2);
  EXPECT_TRUE(v == Vector3(-4, -10, -18)) << "Scale 4 5 6 , -1 -2 -3";

  v2 = Vector3(0, 0, 0);
  v = Vector3::Scale(v1, v2);
  EXPECT_TRUE(v == Vector3(0, 0, 0)) << "Scale 4 5 6 , 0 0 0";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = Vector3::Scale(v1, v2);
    EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
        << "4 5 6 + INFINITY INFINITY INFINITY";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = Vector3::Scale(v1, v2);
    EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
        << "4 5 6 + -INFINITY -INFINITY -INFINITY";
  }
}

TEST(Vector3, Multiply) {
  Vector3 v1 = Vector3(4, 5, 6);
  float f = 3;
  Vector3 v = Vector3::zero;

  v = v1 * f;
  EXPECT_TRUE(v == Vector3(12, 15, 18)) << "4 5 6 * 3";

  f = -3;
  v = v1 * f;
  EXPECT_TRUE(v == Vector3(-12, -15, -18)) << "4 5 6 * -3";

  f = 0;
  v = v1 * f;
  EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 * 0";

  if (std::numeric_limits<float>::is_iec559) {
    f = FLOAT_INFINITY;
    v = v1 * f;
    EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
        << "4 5 6 * INFINITY";

    f = -FLOAT_INFINITY;
    v = v1 * f;
    EXPECT_TRUE(v == Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY))
        << "4 5 6 * -INFINITY";
  }
}

TEST(Vector3, Divide) {
  Vector3 v1 = Vector3(4, 5, 6);
  float f = 2;
  Vector3 v = Vector3::zero;

  v = v1 / f;
  EXPECT_TRUE(v == Vector3(2, 2.5F, 3)) << "4 5 6 / 3";

  f = -2;
  v = v1 / f;
  EXPECT_TRUE(v == Vector3(-2, -2.5F, -3)) << "4 5 6 / -3";

  if (std::numeric_limits<float>::is_iec559) {
    f = 0;
    v = v1 / f;
    EXPECT_TRUE(v == Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY))
        << "4 5 6 / 0";

    f = FLOAT_INFINITY;
    v = v1 / f;
    EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 / INFINITY";

    f = -FLOAT_INFINITY;
    v = v1 / f;
    EXPECT_TRUE(v == Vector3(0, 0, 0)) << "4 5 6 / -INFINITY";
  }
}

TEST(Vector3, Dot) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  float f = 0;

  f = Vector3::Dot(v1, v2);
  EXPECT_FLOAT_EQ(f, 32) << "Dot(4 5 6, 1 2 3)";

  v2 = Vector3(-1, -2, -3);
  f = Vector3::Dot(v1, v2);
  EXPECT_FLOAT_EQ(f, -32) << "Dot(4 5 6, -1 -2 -3)";

  v2 = Vector3(0, 0, 0);
  f = Vector3::Dot(v1, v2);
  EXPECT_FLOAT_EQ(f, 0) << "Dot(4 5 6, 0 0 0)";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    f = Vector3::Dot(v1, v2);
    EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
        << "Dot(4 5 6, INFINITY INFINITY INFINITY)";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    f = Vector3::Dot(v1, v2);
    EXPECT_FLOAT_EQ(f, -FLOAT_INFINITY)
        << "Dot(4 5 6, -INFINITY -INFINITY -INFINITY)";
  }
}

TEST(Vector3, Equality) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  bool r = false;

  r = v1 == v2;
  EXPECT_FALSE(r) << "4 5 6 == 1 2 3";

  v2 = Vector3(4, 5, 6);
  r = v1 == v2;
  EXPECT_TRUE(r) << "4 5 6 == 1 2 3";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    r = v1 == v2;
    EXPECT_FALSE(r) << "4 5 6 == INFINITY INFINITY INFINITY";

    v1 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    r = v1 == v2;
    EXPECT_FALSE(r)
        << "-INFINITY -INFINITY -INFINITY == INFINITY INFINITY INFINITY";
  }
}

TEST(Vector3, Distance) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  float f = 0;

  f = Vector3::Distance(v1, v2);
  EXPECT_FLOAT_EQ(f, 5.19615221F) << "Distance(4 5 6, 1 2 3)";

  v2 = Vector3(-1, -2, -3);
  f = Vector3::Distance(v1, v2);
  EXPECT_FLOAT_EQ(f, 12.4498997F) << "Distance(4 5 6, -1 -2 -3)";

  v2 = Vector3(0, 0, 0);
  f = Vector3::Distance(v1, v2);
  EXPECT_FLOAT_EQ(f, 8.77496433F) << "Distance(4 5 6, 0 0 0)";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    f = Vector3::Distance(v1, v2);
    EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
        << "Distance(4 5 6, INFINITY INFINITY INFINITY)";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    f = Vector3::Distance(v1, v2);
    EXPECT_FLOAT_EQ(f, FLOAT_INFINITY)
        << "Distance(4 5 6, -INFINITY -INFINITY -INFINITY)";
  }
}

TEST(Vector3, Cross) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 v = Vector3::zero;
  bool r = false;

  v = Vector3::Cross(v1, v2);
  r = v == Vector3(3, -6, 3);
  EXPECT_TRUE(r) << "Cross(4 5 6, 1 2 3)";

  v2 = Vector3(-1, -2, -3);
  v = Vector3::Cross(v1, v2);
  r = v == Vector3(-3, 6, -3);
  EXPECT_TRUE(r) << "Cross(4 5 6, -1 -2 -3)";

  v2 = Vector3(0, 0, 0);
  v = Vector3::Cross(v1, v2);
  r = v == Vector3(0, 0, 0);
  EXPECT_TRUE(r) << "Cross(4 5 6, 0 0 0)";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = Vector3::Cross(v1, v2);
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "Cross(4 5 6, INFINITY INFINITY INFINITY)";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = Vector3::Cross(v1, v2);
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "Cross(4 5 6, -INFINITY -INFINITY -INFINITY)";
  }
}

TEST(Vector3, Project) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 v = Vector3::zero;
  bool r = false;

  v = Vector3::Project(v1, v2);
  r = v == Vector3(2.28571439F, 4.57142878F, 6.85714293F);
  EXPECT_TRUE(r) << "Project(4 5 6, 1 2 3)";

  v2 = Vector3(-1, -2, -3);
  v = Vector3::Project(v1, v2);
  r = v == Vector3(2.28571439F, 4.57142878F, 6.85714293F);
  EXPECT_TRUE(r) << "Project(4 5 6, -1 -2 -3)";

  v2 = Vector3(0, 0, 0);
  v = Vector3::Project(v1, v2);
  r = v == Vector3(0, 0, 0);
  EXPECT_TRUE(r) << "Project(4 5 6, 0 0 0)";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = Vector3::Project(v1, v2);
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "Project(4 5 6, INFINITY INFINITY INFINITY)";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = Vector3::Project(v1, v2);
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "Project(4 5 6, -INFINITY -INFINITY -INFINITY)";
  }
}

TEST(Vector3, ProjectOnPlane) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 v = Vector3::zero;
  bool r = false;

  v = Vector3::ProjectOnPlane(v1, v2);
  r = v == Vector3(1.71428561F, 0.428571224F, -0.857142925F);
  EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, 1 2 3)";

  v2 = Vector3(-1, -2, -3);
  v = Vector3::ProjectOnPlane(v1, v2);
  r = v == Vector3(1.71428561F, 0.428571224F, -0.857142925F);
  EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, -1 -2 -3)";

  v2 = Vector3(0, 0, 0);
  v = Vector3::ProjectOnPlane(v1, v2);
  r = v == Vector3(4, 5, 6);
  EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, 0 0 0)";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    v = Vector3::ProjectOnPlane(v1, v2);
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, INFINITY INFINITY INFINITY)";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    v = Vector3::ProjectOnPlane(v1, v2);
    r = isnan(v.Right()) && isnan(v.Up()) && isnan(v.Forward());
    EXPECT_TRUE(r) << "ProjectOnPlane(4 5 6, -INFINITY -INFINITY -INFINITY)";
  }
}

TEST(Vector3, Angle) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  AngleOf<float> f = AngleOf<float>::Degrees(0);
  bool r = false;

  f = Vector3::Angle(v1, v2);
  EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F) << "Angle(4 5 6, 1 2 3)";

  v2 = Vector3(-1, -2, -3);
  f = Vector3::Angle(v1, v2);
  EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F) << "Angle(4 5 6, -1 -2 -3)";

  v2 = Vector3(0, 0, 0);
  f = Vector3::Angle(v1, v2);
  EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "Angle(4 5 6, 0 0 0)";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    f = Vector3::Angle(v1, v2);
    r = isnan(f.InDegrees());
    EXPECT_TRUE(r) << "Angle(4 5 6, INFINITY INFINITY INFINITY)";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    f = Vector3::Angle(v1, v2);
    r = isnan(f.InDegrees());
    EXPECT_TRUE(r) << "Angle(4 5 6, -INFINITY -INFINITY -INFINITY)";
  }
}

TEST(Vector3, SignedAngle) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 v3 = Vector3(7, 8, -9);
  AngleOf<float> f = AngleOf<float>::Degrees(0);
  bool r = false;

  f = Vector3::SignedAngle(v1, v2, v3);
  EXPECT_FLOAT_EQ(f.InDegrees(), -12.9331388F)
      << "SignedAngle(4 5 6, 1 2 3, 7 8 -9)";

  v2 = Vector3(-1, -2, -3);
  f = Vector3::SignedAngle(v1, v2, v3);
  EXPECT_FLOAT_EQ(f.InDegrees(), 167.066864F)
      << "SignedAngle(4 5 6, -1 -2 -3, 7 8 -9)";

  v2 = Vector3(0, 0, 0);
  f = Vector3::SignedAngle(v1, v2, v3);
  EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 0 0 0, 7 8 -9 )";

  v2 = Vector3(1, 2, 3);

  v3 = Vector3(-7, -8, 9);
  f = Vector3::SignedAngle(v1, v2, v3);
  EXPECT_FLOAT_EQ(f.InDegrees(), 12.9331388F)
      << "SignedAngle(4 5 6, 1 2 3, -7 -8 9)";

  v3 = Vector3(0, 0, 0);
  f = Vector3::SignedAngle(v1, v2, v3);
  EXPECT_FLOAT_EQ(f.InDegrees(), 0) << "SignedAngle(4 5 6, 1 2 3, 0 0 0)";

  if (std::numeric_limits<float>::is_iec559) {
    v2 = Vector3(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
    f = Vector3::SignedAngle(v1, v2, v3);
    r = isnan(f.InDegrees());
    EXPECT_TRUE(r) << "SignedAngle(4 5 6, INFINITY INFINITY INFINITY)";

    v2 = Vector3(-FLOAT_INFINITY, -FLOAT_INFINITY, -FLOAT_INFINITY);
    f = Vector3::SignedAngle(v1, v2, v3);
    r = isnan(f.InDegrees());
    EXPECT_TRUE(r) << "SignedAngle(4 5 6, -INFINITY -INFINITY -INFINITY)";
  }
}

TEST(Vector3, Lerp) {
  Vector3 v1 = Vector3(4, 5, 6);
  Vector3 v2 = Vector3(1, 2, 3);
  Vector3 r = Vector3(0, 0, 0);

  r = Vector3::Lerp(v1, v2, 0);
  EXPECT_FLOAT_EQ(Vector3::Distance(r, v1), 0);

  r = Vector3::Lerp(v1, v2, 1);
  EXPECT_FLOAT_EQ(Vector3::Distance(r, v2), 0);

  r = Vector3::Lerp(v1, v2, 0.5f);
  EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(2.5f, 3.5f, 4.5f)), 0);

  r = Vector3::Lerp(v1, v2, -1);
  EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(7.0f, 8.0f, 9.0f)), 0);

  r = Vector3::Lerp(v1, v2, 2);
  EXPECT_FLOAT_EQ(Vector3::Distance(r, Vector3(-2.0, -1.0f, 0.0f)), 0);
}

#endif