Continuing refactor work.
This commit is contained in:
227
test/coord2d.cc
Executable file
227
test/coord2d.cc
Executable file
@@ -0,0 +1,227 @@
|
||||
//
|
||||
// Project: scccl
|
||||
// File: test/math/geom2d_test.cpp
|
||||
// Author: Kyle Isom
|
||||
// Date: 2017-06-05
|
||||
//
|
||||
// geom2d_test runs a set of unit tests on the 2D parts of the
|
||||
// math::geom namespace.
|
||||
//
|
||||
// Copyright 2017 Kyle Isom <kyle@imap.cc>
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
#include <scmp/Math.h>
|
||||
#include <scmp/geom/Coord2D.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
#include <sctest/Checks.h>
|
||||
|
||||
using namespace scmp::geom;
|
||||
using namespace sctest;
|
||||
|
||||
|
||||
#define CHECK_ROTATE(theta, expected) if (!scmp::WithinTolerance(scmp::RotateRadians((double)theta, 0), (double)expected, (double)0.0001)) { \
|
||||
std::cerr << "Expected " << theta << " to wrap to " << expected << std::endl; \
|
||||
std::cerr << " have " << scmp::RotateRadians(theta, 0) << std::endl; \
|
||||
return false; \
|
||||
}
|
||||
|
||||
static bool
|
||||
geom_validate_angular_rotation(void)
|
||||
{
|
||||
CHECK_ROTATE(0, 0);
|
||||
CHECK_ROTATE(M_PI/4, M_PI/4);
|
||||
CHECK_ROTATE(M_PI/2, M_PI/2);
|
||||
CHECK_ROTATE(3 * M_PI / 4, 3 * M_PI / 4);
|
||||
CHECK_ROTATE(M_PI, M_PI);
|
||||
CHECK_ROTATE(5 * M_PI / 4, -3 * M_PI / 4);
|
||||
CHECK_ROTATE(3 * M_PI / 2, -(M_PI / 2));
|
||||
CHECK_ROTATE(7 * M_PI / 4, -(M_PI / 4));
|
||||
CHECK_ROTATE(4 * M_PI, 0)
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
geom_conversion_identities(void)
|
||||
{
|
||||
Point2D points[4] = {
|
||||
Point2D(1, 0),
|
||||
Point2D(0, 1),
|
||||
Point2D(-1, 0),
|
||||
Point2D(0, -1)
|
||||
};
|
||||
|
||||
Polar2D polars[4] = {
|
||||
Polar2D(1, 0),
|
||||
Polar2D(1, scmp::DegreesToRadiansD(90)),
|
||||
Polar2D(1, scmp::DegreesToRadiansD(180)),
|
||||
Polar2D(1, scmp::DegreesToRadiansD(-90)),
|
||||
};
|
||||
|
||||
for (auto i = 0; i < 4; i++) {
|
||||
Polar2D pol(points[i]);
|
||||
if (pol != polars[i]) {
|
||||
std::cerr << "! measured value outside tolerance (" << i << ")" << std::endl;
|
||||
std::cerr << " " << points[i] << " → " << pol << " ← " << polars[i] << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D pt(pol);
|
||||
SCTEST_CHECK(pt == points[i]);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
geom_verify_basic_properties(void)
|
||||
{
|
||||
Point2D p1(1, 1);
|
||||
Point2D p2(2, 2);
|
||||
Point2D p3(3, 3);
|
||||
|
||||
SCTEST_CHECK((p1 + p2) == p3);
|
||||
SCTEST_CHECK((p3 - p2) == p1);
|
||||
|
||||
// commutative
|
||||
SCTEST_CHECK((p1 + p2) == (p2 + p1));
|
||||
SCTEST_CHECK((p1 + p3) == (p3 + p1));
|
||||
SCTEST_CHECK((p2 + p3) == (p3 + p2));
|
||||
|
||||
// associative
|
||||
SCTEST_CHECK(((p1 + p2) + p3) == (p1 + (p2 + p3)));
|
||||
|
||||
// transitive
|
||||
Point2D p4(1, 1);
|
||||
Point2D p5(1, 1);
|
||||
SCTEST_CHECK(p1 == p4);
|
||||
SCTEST_CHECK(p4 == p5);
|
||||
SCTEST_CHECK(p1 == p5);
|
||||
|
||||
// scaling
|
||||
Point2D p6(2, 3);
|
||||
Point2D p7(8, 12);
|
||||
SCTEST_CHECK((p6 * 4) == p7);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
geom_compare_point2d(void)
|
||||
{
|
||||
Point2D p1(1, 1);
|
||||
Point2D p2(1, 1);
|
||||
Point2D p3(0, 1);
|
||||
|
||||
SCTEST_CHECK(p1 == p2);
|
||||
SCTEST_CHECK_FALSE(p2 == p3);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
geom_rotate_point2d(void)
|
||||
{
|
||||
Point2D vertices[4] = {
|
||||
Point2D(1, 0), // θ = 0
|
||||
Point2D(0, 1), // θ = π/2
|
||||
Point2D(-1, 0), // θ = π
|
||||
Point2D(0, -1) // θ = 3π/2
|
||||
};
|
||||
|
||||
Point2D vertex;
|
||||
vertices[0].Rotate(vertex, 1.5708);
|
||||
|
||||
if (vertex != vertices[1]) {
|
||||
std::cerr << "expected: " << vertices[1] << std::endl;
|
||||
std::cerr << " have: " << vertex << std::endl;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
geom_rotate_points_about_origin(void)
|
||||
{
|
||||
Point2D origin(3, 3);
|
||||
double theta = 0;
|
||||
|
||||
std::vector<Polar2D> vertices {
|
||||
Polar2D(2, 0),
|
||||
Polar2D(1.41421, 2.35619),
|
||||
Polar2D(1.41421, -2.35619)
|
||||
};
|
||||
|
||||
// expected coordinates with no rotation
|
||||
std::vector<Point2D> rotated0 {
|
||||
Point2D(5, 3),
|
||||
Point2D(2, 4),
|
||||
Point2D(2, 2)
|
||||
};
|
||||
|
||||
auto rotated = origin.Rotate(vertices, theta);
|
||||
for (auto i = 0; i < 3; i++) {
|
||||
SCTEST_CHECK(rotated.at(i) == rotated0.at(i));
|
||||
}
|
||||
|
||||
// expected after 90° rotation
|
||||
theta = scmp::DegreesToRadiansD(90);
|
||||
std::vector<Point2D> rotated90 {
|
||||
Point2D(3, 5),
|
||||
Point2D(2, 2),
|
||||
Point2D(4, 2)
|
||||
};
|
||||
|
||||
rotated = origin.Rotate(vertices, theta);
|
||||
for (auto i = 0; i < 3; i++) {
|
||||
SCTEST_CHECK(rotated.at(i) == rotated90.at(i));
|
||||
}
|
||||
|
||||
// expected after 180° rotation
|
||||
theta = scmp::DegreesToRadiansD(180);
|
||||
std::vector<Point2D> rotated180 {
|
||||
Point2D(1, 3),
|
||||
Point2D(4, 2),
|
||||
Point2D(4, 4)
|
||||
};
|
||||
|
||||
rotated = origin.Rotate(vertices, theta);
|
||||
for (auto i = 0; i < 3; i++) {
|
||||
SCTEST_CHECK(rotated.at(i) == rotated180.at(i));
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
int
|
||||
main(void)
|
||||
{
|
||||
SimpleSuite ts;
|
||||
ts.AddTest("geom_validate_angular_rotation", geom_validate_angular_rotation);
|
||||
ts.AddTest("geom_conversion_identities", geom_conversion_identities);
|
||||
ts.AddTest("geom_verify_basic_properties", geom_verify_basic_properties);
|
||||
ts.AddTest("geom_compare_point2d", geom_compare_point2d);
|
||||
ts.AddTest("geom_rotate_point2d", geom_rotate_point2d);
|
||||
ts.AddTest("geom_rotate_points_about_origin", geom_rotate_points_about_origin);
|
||||
|
||||
if (ts.Run()) {
|
||||
std::cout << "OK" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
auto r = ts.GetReport();
|
||||
std::cerr << r.Failing << "/" << r.Total << " tests failed." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -7,6 +7,7 @@
|
||||
|
||||
|
||||
using namespace scsl;
|
||||
using namespace sctest;
|
||||
|
||||
|
||||
constexpr char TEST_KVSTR1[] = "foo";
|
||||
@@ -60,40 +61,40 @@ main(int argc, const char *argv[])
|
||||
TLV::SetRecord(expect, DICTIONARY_TAG_VAL, TEST_KVSTRLEN3, TEST_KVSTR3);
|
||||
|
||||
Dictionary dict(arena);
|
||||
TestAssert(!dict.Contains(TEST_KVSTR2, TEST_KVSTRLEN2));
|
||||
Assert(!dict.Contains(TEST_KVSTR2, TEST_KVSTRLEN2));
|
||||
|
||||
TestAssert(testSetKV(dict, TEST_KVSTR1, TEST_KVSTRLEN1, TEST_KVSTR3,
|
||||
TEST_KVSTRLEN3));
|
||||
Assert(testSetKV(dict, TEST_KVSTR1, TEST_KVSTRLEN1, TEST_KVSTR3,
|
||||
TEST_KVSTRLEN3));
|
||||
std::cout << dict;
|
||||
TestAssert(testSetKV(dict, TEST_KVSTR2, TEST_KVSTRLEN2, TEST_KVSTR3,
|
||||
TEST_KVSTRLEN3));
|
||||
Assert(testSetKV(dict, TEST_KVSTR2, TEST_KVSTRLEN2, TEST_KVSTR3,
|
||||
TEST_KVSTRLEN3));
|
||||
std::cout << dict;
|
||||
TestAssert(dict.Contains(TEST_KVSTR2, TEST_KVSTRLEN2));
|
||||
TestAssert(testSetKV(dict, TEST_KVSTR4, TEST_KVSTRLEN4, TEST_KVSTR5,
|
||||
TEST_KVSTRLEN5));
|
||||
Assert(dict.Contains(TEST_KVSTR2, TEST_KVSTRLEN2));
|
||||
Assert(testSetKV(dict, TEST_KVSTR4, TEST_KVSTRLEN4, TEST_KVSTR5,
|
||||
TEST_KVSTRLEN5));
|
||||
std::cout << dict;
|
||||
TestAssert(dict.Lookup(TEST_KVSTR2, TEST_KVSTRLEN2, value));
|
||||
Assert(dict.Lookup(TEST_KVSTR2, TEST_KVSTRLEN2, value));
|
||||
|
||||
TestAssert(cmpRecord(value, expect));
|
||||
Assert(cmpRecord(value, expect));
|
||||
|
||||
std::cout << "test overwriting key" << "\n";
|
||||
TestAssert(testSetKV(dict, TEST_KVSTR2, TEST_KVSTRLEN2, TEST_KVSTR6,
|
||||
TEST_KVSTRLEN6));
|
||||
Assert(testSetKV(dict, TEST_KVSTR2, TEST_KVSTRLEN2, TEST_KVSTR6,
|
||||
TEST_KVSTRLEN6));
|
||||
std::cout << dict;
|
||||
TLV::SetRecord(expect, DICTIONARY_TAG_VAL, TEST_KVSTRLEN6, TEST_KVSTR6);
|
||||
std::cout << "\tlookup" << "\n";
|
||||
TestAssert(dict.Lookup(TEST_KVSTR2, TEST_KVSTRLEN2, value));
|
||||
Assert(dict.Lookup(TEST_KVSTR2, TEST_KVSTRLEN2, value));
|
||||
std::cout << "\tcompare records" << "\n";
|
||||
TestAssert(cmpRecord(value, expect));
|
||||
Assert(cmpRecord(value, expect));
|
||||
|
||||
std::cout << "\tadd new key to dictionary" << "\n";
|
||||
TestAssert(testSetKV(dict, TEST_KVSTR3, TEST_KVSTRLEN3, TEST_KVSTR5,
|
||||
TEST_KVSTRLEN5));
|
||||
Assert(testSetKV(dict, TEST_KVSTR3, TEST_KVSTRLEN3, TEST_KVSTR5,
|
||||
TEST_KVSTRLEN5));
|
||||
std::cout << dict;
|
||||
|
||||
TLV::SetRecord(expect, DICTIONARY_TAG_VAL, TEST_KVSTRLEN5, TEST_KVSTR5);
|
||||
TestAssert(dict.Lookup(TEST_KVSTR4, TEST_KVSTRLEN4, value));
|
||||
TestAssert(cmpRecord(value, expect));
|
||||
Assert(dict.Lookup(TEST_KVSTR4, TEST_KVSTRLEN4, value));
|
||||
Assert(cmpRecord(value, expect));
|
||||
|
||||
std::cout << "OK" << "\n";
|
||||
|
||||
|
||||
@@ -26,7 +26,7 @@ main(int argc, char *argv[])
|
||||
flags->Register("-u", (unsigned int)42, "test unsigned integer with a long description line. This should trigger multiline text-wrapping.");
|
||||
flags->Register("-i", -42, "test integer");
|
||||
flags->Register("-size", FlagType::SizeT, "test size_t");
|
||||
TestAssert(flags->Size() == 5, "flags weren't registered");
|
||||
sctest::Assert(flags->Size() == 5, "flags weren't registered");
|
||||
|
||||
auto status = flags->Parse(argc, argv);
|
||||
|
||||
|
||||
63
test/madgwick.cc
Normal file
63
test/madgwick.cc
Normal file
@@ -0,0 +1,63 @@
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
#include <scmp/geom/Vector.h>
|
||||
#include <scmp/geom/Quaternion.h>
|
||||
#include <scmp/Math.h>
|
||||
#include <scmp/filter/Madgwick.h>
|
||||
|
||||
#include <sctest/Assert.h>
|
||||
#include <sctest/Checks.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace scmp;
|
||||
|
||||
|
||||
bool
|
||||
SimpleAngularOrientation()
|
||||
{
|
||||
filter::Madgwickd mf;
|
||||
geom::Vector3d gyro{0.174533, 0.0, 0.0}; // 10° X rotation.
|
||||
geom::Quaterniond frame20Deg{0.984808, 0.173648, 0, 0}; // 20° final Orientation.
|
||||
double delta = 0.00917; // assume 109 updates per second, as per the paper.
|
||||
double twentyDegrees = scmp::DegreesToRadiansD(20.0);
|
||||
|
||||
// The paper specifies a minimum of 109 IMU readings to stabilize; for
|
||||
// two seconds, that means 218 updates.
|
||||
for (int i = 0; i < 218; i++) {
|
||||
mf.UpdateAngularOrientation(gyro, delta);
|
||||
}
|
||||
|
||||
SCTEST_CHECK_EQ(mf.Orientation(), frame20Deg);
|
||||
|
||||
auto euler = mf.Euler();
|
||||
SCTEST_CHECK_DEQ_EPS(euler[0], twentyDegrees, 0.01);
|
||||
SCTEST_CHECK_DEQ_EPS(euler[1], 0.0, 0.01);
|
||||
SCTEST_CHECK_DEQ_EPS(euler[2], 0.0, 0.01);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
main(int argc, char **argv)
|
||||
{
|
||||
sctest::SimpleSuite suite;
|
||||
|
||||
suite.AddTest("SimpleAngularOrientation", SimpleAngularOrientation);
|
||||
auto result = suite.Run();
|
||||
|
||||
if (suite.IsReportReady()) {
|
||||
auto report = suite.GetReport();
|
||||
std::cout << report.Failing << " / " << report.Total;
|
||||
std::cout << " tests failed.\n";
|
||||
}
|
||||
|
||||
if (result) {
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
98
test/orientation.cc
Normal file
98
test/orientation.cc
Normal file
@@ -0,0 +1,98 @@
|
||||
#include <sctest/Checks.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
|
||||
#include <scmp/Math.h>
|
||||
#include <scmp/geom/Vector.h>
|
||||
#include <scmp/geom/Orientation.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace scmp;
|
||||
using namespace sctest;
|
||||
|
||||
|
||||
static bool
|
||||
UnitConversions_RadiansToDegreesF()
|
||||
{
|
||||
for (int i = 0; i < 360; i++) {
|
||||
auto deg = static_cast<float>(i);
|
||||
SCTEST_CHECK_FEQ(scmp::RadiansToDegreesF(scmp::DegreesToRadiansF(deg)), deg);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
UnitConversions_RadiansToDegreesD()
|
||||
{
|
||||
for (int i = 0; i < 360; i++) {
|
||||
auto deg = static_cast<double>(i);
|
||||
SCTEST_CHECK_DEQ(scmp::RadiansToDegreesD(scmp::DegreesToRadiansD(deg)), deg);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Orientation2f_Heading()
|
||||
{
|
||||
geom::Vector2f a {2.0, 2.0};
|
||||
|
||||
SCTEST_CHECK_FEQ(geom::Heading2f(a), scmp::DegreesToRadiansF(45));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Orientation3f_Heading()
|
||||
{
|
||||
geom::Vector3f a {2.0, 2.0, 2.0};
|
||||
|
||||
SCTEST_CHECK_FEQ(geom::Heading3f(a), scmp::DegreesToRadiansF(45));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Orientation2d_Heading()
|
||||
{
|
||||
geom::Vector2d a {2.0, 2.0};
|
||||
|
||||
return scmp::WithinTolerance(geom::Heading2d(a), scmp::DegreesToRadiansD(45), 0.000001);
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Orientation3d_Heading()
|
||||
{
|
||||
geom::Vector3d a {2.0, 2.0, 2.0};
|
||||
|
||||
return scmp::WithinTolerance(geom::Heading3d(a), scmp::DegreesToRadiansD(45), 0.000001);
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
main(void)
|
||||
{
|
||||
SimpleSuite ts;
|
||||
|
||||
ts.AddTest("UnitConversions_RadiansToDegreesF", UnitConversions_RadiansToDegreesF);
|
||||
ts.AddTest("UnitConversions_RadiansToDegreesD", UnitConversions_RadiansToDegreesD);
|
||||
ts.AddTest("Orientation2f_Heading", Orientation2f_Heading);
|
||||
ts.AddTest("Orientation3f_Heading", Orientation3f_Heading);
|
||||
ts.AddTest("Orientation2d_Heading", Orientation2d_Heading);
|
||||
ts.AddTest("Orientation3d_Heading", Orientation3d_Heading);
|
||||
|
||||
if (ts.Run()) {
|
||||
std::cout << "OK" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
auto r = ts.GetReport();
|
||||
std::cerr << r.Failing << "/" << r.Total << " tests failed." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
477
test/quaternion.cc
Normal file
477
test/quaternion.cc
Normal file
@@ -0,0 +1,477 @@
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
#include <scmp/geom/Quaternion.h>
|
||||
|
||||
#include <sctest/Checks.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
|
||||
|
||||
using namespace std;
|
||||
using namespace scmp;
|
||||
using namespace sctest;
|
||||
|
||||
|
||||
static bool
|
||||
Quaternion_SelfTest()
|
||||
{
|
||||
geom::Quaternion_SelfTest();
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Addition()
|
||||
{
|
||||
geom::Quaterniond p(geom::Vector4d {3.0, 1.0, -2.0, 1.0});
|
||||
geom::Quaterniond q(geom::Vector4d {2.0, -1.0, 2.0, 3.0});
|
||||
geom::Quaterniond expected(geom::Vector4d{5.0, 0.0, 0.0, 4.0});
|
||||
|
||||
SCTEST_CHECK_EQ(p + q, expected);
|
||||
SCTEST_CHECK_EQ(expected - q, p);
|
||||
SCTEST_CHECK_NE(expected - q, q); // exercise !=
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Conjugate()
|
||||
{
|
||||
geom::Quaterniond p {2.0, 3.0, 4.0, 5.0};
|
||||
geom::Quaterniond q {2.0, -3.0, -4.0, -5.0};
|
||||
|
||||
SCTEST_CHECK_EQ(p.conjugate(), q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
Quaterniond_Euler()
|
||||
{
|
||||
geom::Quaterniond p = geom::quaterniond(geom::Vector3d{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0);
|
||||
geom::Quaterniond q = geom::quaterniond_from_euler(p.euler());
|
||||
|
||||
SCTEST_CHECK_EQ(p, q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Identity()
|
||||
{
|
||||
geom::Quaterniond p {3.0, 1.0, -2.0, 1.0};
|
||||
geom::Quaterniond q;
|
||||
|
||||
SCTEST_CHECK(q.isIdentity());
|
||||
SCTEST_CHECK_EQ(p * q, p);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Inverse()
|
||||
{
|
||||
geom::Quaterniond p {2.0, 3.0, 4.0, 5.0};
|
||||
geom::Quaterniond q {0.03704, -0.05556, -0.07407, -0.09259};
|
||||
|
||||
SCTEST_CHECK_EQ(p.inverse(), q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Norm()
|
||||
{
|
||||
geom::Quaterniond p {5.563199889674063, 0.9899139811480784, 9.387110042325054, 6.161341707794767};
|
||||
double norm = 12.57016663729933;
|
||||
|
||||
SCTEST_CHECK_DEQ(p.norm(), norm);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Product()
|
||||
{
|
||||
geom::Quaterniond p {3.0, 1.0, -2.0, 1.0};
|
||||
geom::Quaterniond q {2.0, -1.0, 2.0, 3.0};
|
||||
geom::Quaterniond expected {8.0, -9.0, -2.0, 11.0};
|
||||
|
||||
SCTEST_CHECK_EQ(p * q, expected);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Rotate()
|
||||
{
|
||||
// This test aims to rotate a vector v using a quaternion.
|
||||
// c.f. https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion
|
||||
// If we assume a standard IMU frame of reference following the
|
||||
// right hand rule:
|
||||
// + The x axis points toward magnetic north
|
||||
// + The y axis points toward magnentic west
|
||||
// + The z axis points toward the sky
|
||||
// Given a vector pointing due north, rotating by 90º about
|
||||
// the y-axis should leave us pointing toward the sky.
|
||||
|
||||
geom::Vector3d v {1.0, 0.0, 0.0}; // a vector pointed north
|
||||
geom::Vector3d yAxis {0.0, 1.0, 0.0}; // a vector representing the y axis.
|
||||
double angle = M_PI / 2; // 90º rotation
|
||||
|
||||
// A quaternion representing a 90º rotation about the y axis.
|
||||
geom::Quaterniond p = geom::quaterniond(yAxis, angle);
|
||||
geom::Vector3d vr {0.0, 0.0, 1.0}; // expected rotated vector.
|
||||
|
||||
// A rotation quaternion should be a unit quaternion.
|
||||
SCTEST_CHECK(p.isUnitQuaternion());
|
||||
SCTEST_CHECK_EQ(p.rotate(v), vr);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_ShortestSLERP()
|
||||
{
|
||||
// Our starting point is an Orientation that is yawed 45° - our
|
||||
// Orientation is pointed π/4 radians in the X axis.
|
||||
geom::Quaterniond p {0.92388, 0.382683, 0, 0};
|
||||
// Our ending point is an Orientation that is yawed -45° - or
|
||||
// pointed -π/4 radians in the X axis.
|
||||
geom::Quaterniond q {0.92388, -0.382683, 0, 0};
|
||||
// The halfway point should be oriented midway about the X axis. It turns
|
||||
// out this is an identity quaternion.
|
||||
geom::Quaterniond r;
|
||||
|
||||
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, 0.0), p);
|
||||
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, 1.0), q);
|
||||
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, 0.5), r);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_ShortestSLERP2()
|
||||
{
|
||||
// Start with an Orientation pointing forward, all Euler angles
|
||||
// set to 0.
|
||||
geom::Quaterniond start {1.0, 0.0, 0.0, 0.0};
|
||||
// The goal is to end up face up, or 90º pitch (still facing forward).
|
||||
geom::Quaterniond end {0.707107, 0, -0.707107, 0};
|
||||
// Halfway to the endpoint should be a 45º pitch.
|
||||
geom::Quaterniond halfway {0.92388, 0, -0.382683, 0};
|
||||
// 2/3 of the way should be 60º pitch.
|
||||
geom::Quaterniond twoThirds {0.866025, 0, -0.5, 0};
|
||||
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, 0.0), start);
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, 1.0), end);
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, 0.5), halfway);
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, 2.0/3.0), twoThirds);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_Unit()
|
||||
{
|
||||
geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
|
||||
|
||||
SCTEST_CHECK(q.isUnitQuaternion());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaterniond_UtilityCreator()
|
||||
{
|
||||
geom::Vector3d v {1.0, 1.0, 1.0};
|
||||
double w = M_PI;
|
||||
geom::Quaterniond p = geom::quaterniond(v, w);
|
||||
geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
|
||||
|
||||
SCTEST_CHECK_EQ(p, q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Addition()
|
||||
{
|
||||
geom::Quaternionf p {3.0, 1.0, -2.0, 1.0};
|
||||
geom::Quaternionf q {2.0, -1.0, 2.0, 3.0};
|
||||
geom::Quaternionf expected {5.0, 0.0, 0.0, 4.0};
|
||||
|
||||
SCTEST_CHECK_EQ(p + q, expected);
|
||||
SCTEST_CHECK_EQ(expected - q, p);
|
||||
SCTEST_CHECK_NE(expected - q, q); // exercise !=
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Conjugate()
|
||||
{
|
||||
geom::Quaternionf p {2.0, 3.0, 4.0, 5.0};
|
||||
geom::Quaternionf q {2.0, -3.0, -4.0, -5.0};
|
||||
|
||||
SCTEST_CHECK_EQ(p.conjugate(), q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Euler()
|
||||
{
|
||||
geom::Quaternionf p = geom::quaternionf(geom::Vector3f{5.037992718099102, 6.212303632611285, 1.7056797335843106}, M_PI/4.0);
|
||||
geom::Quaternionf q = geom::quaternionf_from_euler(p.euler());
|
||||
|
||||
SCTEST_CHECK_EQ(p, q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Identity()
|
||||
{
|
||||
geom::Quaternionf p {1.0, 3.0, 1.0, -2.0};
|
||||
geom::Quaternionf q;
|
||||
|
||||
SCTEST_CHECK_EQ(p * q, p);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Inverse()
|
||||
{
|
||||
geom::Quaternionf p {2.0, 3.0, 4.0, 5.0};
|
||||
geom::Quaternionf q {0.03704, -0.05556, -0.07407, -0.09259};
|
||||
|
||||
SCTEST_CHECK_EQ(p.inverse(), q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Norm()
|
||||
{
|
||||
geom::Quaternionf p {0.9899139811480784, 9.387110042325054, 6.161341707794767, 5.563199889674063};
|
||||
float norm = 12.57016663729933;
|
||||
|
||||
SCTEST_CHECK_FEQ(p.norm(), norm);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Product()
|
||||
{
|
||||
geom::Quaternionf p {3.0, 1.0, -2.0, 1.0};
|
||||
geom::Quaternionf q {2.0, -1.0, 2.0, 3.0};
|
||||
geom::Quaternionf expected {8.0, -9.0, -2.0, 11.0};
|
||||
|
||||
SCTEST_CHECK_EQ(p * q, expected);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Rotate()
|
||||
{
|
||||
geom::Vector3f v {1.0, 0.0, 0.0};
|
||||
geom::Vector3f yAxis {0.0, 1.0, 0.0};
|
||||
float angle = M_PI / 2;
|
||||
|
||||
geom::Quaternionf p = geom::quaternionf(yAxis, angle);
|
||||
geom::Vector3f vr {0.0, 0.0, 1.0};
|
||||
|
||||
SCTEST_CHECK(p.isUnitQuaternion());
|
||||
SCTEST_CHECK_EQ(p.rotate(v), vr);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_ShortestSLERP()
|
||||
{
|
||||
// Our starting point is an Orientation that is yawed 45° - our
|
||||
// Orientation is pointed π/4 radians in the X axis.
|
||||
geom::Quaternionf p {0.92388, 0.382683, 0, 0};
|
||||
// Our ending point is an Orientation that is yawed -45° - or
|
||||
// pointed -π/4 radians in the X axis.
|
||||
geom::Quaternionf q {0.92388, -0.382683, 0, 0};
|
||||
// The halfway point should be oriented midway about the X axis. It turns
|
||||
// out this is an identity quaternion.
|
||||
geom::Quaternionf r;
|
||||
|
||||
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, (float)0.0), p);
|
||||
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, (float)1.0), q);
|
||||
SCTEST_CHECK_EQ(geom::ShortestSLERP(p, q, (float)0.5), r);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_ShortestSLERP2()
|
||||
{
|
||||
// Start with an Orientation pointing forward, all Euler angles
|
||||
// set to 0.
|
||||
geom::Quaternionf start {1.0, 0.0, 0.0, 0.0};
|
||||
// The goal is to end up face up, or 90º pitch (still facing forward).
|
||||
geom::Quaternionf end {0.707107, 0, -0.707107, 0};
|
||||
// Halfway to the endpoint should be a 45º pitch.
|
||||
geom::Quaternionf halfway {0.92388, 0, -0.382683, 0};
|
||||
// 2/3 of the way should be 60º pitch.
|
||||
geom::Quaternionf twoThirds {0.866025, 0, -0.5, 0};
|
||||
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, (float)0.0), start);
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, (float)1.0), end);
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, (float)0.5), halfway);
|
||||
SCTEST_CHECK_EQ(ShortestSLERP(start, end, (float)(2.0/3.0)), twoThirds);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_Unit()
|
||||
{
|
||||
geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
|
||||
|
||||
SCTEST_CHECK(q.isUnitQuaternion());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Quaternionf_UtilityCreator()
|
||||
{
|
||||
geom::Vector3f v {1.0, 1.0, 1.0};
|
||||
float w = M_PI;
|
||||
geom::Quaternionf p = geom::quaternionf(v, w);
|
||||
geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
|
||||
|
||||
SCTEST_CHECK_EQ(p, q);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
QuaternionMiscellaneous_SanityChecks()
|
||||
{
|
||||
geom::Vector4d q {4.0, 1.0, 2.0, 3.0};
|
||||
geom::Vector3d v {1.0, 2.0, 3.0};
|
||||
double w = 4.0;
|
||||
geom::Quaterniond p(q);
|
||||
geom::Quaterniond u = p.unitQuaternion();
|
||||
|
||||
SCTEST_CHECK_EQ(p.axis(), v);
|
||||
SCTEST_CHECK_DEQ(p.angle(), w);
|
||||
SCTEST_CHECK(u.isUnitQuaternion());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
QuaternionMiscellaneous_OutputStream()
|
||||
{
|
||||
geom::Quaternionf p {4.0, 1.0, 2.0, 3.0};
|
||||
geom::Quaterniond q {4.0, 1.0, 2.0, 3.0};
|
||||
stringstream ss;
|
||||
|
||||
ss << p;
|
||||
SCTEST_CHECK_EQ(ss.str(), "4 + <1, 2, 3>");
|
||||
ss.str("");
|
||||
|
||||
ss << q;
|
||||
SCTEST_CHECK_EQ(ss.str(), "4 + <1, 2, 3>");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
QuaternionMiscellanous_InitializerConstructor()
|
||||
{
|
||||
geom::Quaternionf p {1.0, 1.0, 1.0, 1.0};
|
||||
geom::Quaternionf q(geom::Vector4f {1.0, 1.0, 1.0, 1.0});
|
||||
|
||||
SCTEST_CHECK_EQ(p, q);
|
||||
SCTEST_CHECK_FEQ(p.norm(), (float)2.0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
main(void)
|
||||
{
|
||||
SimpleSuite ts;
|
||||
|
||||
ts.AddTest("Quaternion_SelfTest", Quaternion_SelfTest);
|
||||
ts.AddTest("QuaternionMiscellanous_InitializerConstructor",
|
||||
QuaternionMiscellanous_InitializerConstructor);
|
||||
ts.AddTest("QuaternionMiscellaneous_SanityChecks",
|
||||
QuaternionMiscellaneous_SanityChecks);
|
||||
ts.AddTest("QuaternionMiscellaneous_OutputStream",
|
||||
QuaternionMiscellaneous_OutputStream);
|
||||
|
||||
ts.AddTest("Quaterniond_Addition", Quaterniond_Addition);
|
||||
ts.AddTest("Quaterniond_Conjugate", Quaterniond_Conjugate);
|
||||
ts.AddTest("Quaterniond_Euler", Quaterniond_Euler);
|
||||
ts.AddTest("Quaterniond_Identity", Quaterniond_Identity);
|
||||
ts.AddTest("Quaterniond_Inverse", Quaterniond_Inverse);
|
||||
ts.AddTest("Quaterniond_Norm", Quaterniond_Norm);
|
||||
ts.AddTest("Quaterniond_Product", Quaterniond_Product);
|
||||
ts.AddTest("Quaterniond_Rotate", Quaterniond_Rotate);
|
||||
ts.AddTest("Quaterniond_ShortestSLERP", Quaterniond_ShortestSLERP);
|
||||
ts.AddTest("Quaterniond_ShortestSLERP2", Quaterniond_ShortestSLERP2);
|
||||
ts.AddTest("Quaterniond_Unit", Quaterniond_Unit);
|
||||
ts.AddTest("Quaterniond_UtilityCreator", Quaterniond_UtilityCreator);
|
||||
|
||||
ts.AddTest("Quaternionf_Addition", Quaternionf_Addition);
|
||||
ts.AddTest("Quaternionf_Conjugate", Quaternionf_Conjugate);
|
||||
ts.AddTest("Quaternionf_Euler", Quaternionf_Euler);
|
||||
ts.AddTest("Quaternionf_Identity", Quaternionf_Identity);
|
||||
ts.AddTest("Quaternionf_Inverse", Quaternionf_Inverse);
|
||||
ts.AddTest("Quaternionf_Norm", Quaternionf_Norm);
|
||||
ts.AddTest("Quaternionf_Product", Quaternionf_Product);
|
||||
ts.AddTest("Quaternionf_Rotate", Quaternionf_Rotate);
|
||||
ts.AddTest("Quaternionf_ShortestSLERP", Quaternionf_ShortestSLERP);
|
||||
ts.AddTest("Quaternionf_ShortestSLERP2", Quaternionf_ShortestSLERP2);
|
||||
ts.AddTest("Quaternionf_Unit", Quaternionf_Unit);
|
||||
ts.AddTest("Quaternionf_UtilityCreator", Quaternionf_UtilityCreator);
|
||||
|
||||
if (ts.Run()) {
|
||||
std::cout << "OK" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
auto r = ts.GetReport();
|
||||
std::cerr << r.Failing << "/" << r.Total << " tests failed." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
@@ -39,30 +39,30 @@ TestTrimming(std::string line, std::string lExpected, std::string rExpected, std
|
||||
|
||||
result = U::S::TrimLeadingWhitespaceDup(line);
|
||||
message = "TrimLeadingDup(\"" + line + "\"): '" + result + "'";
|
||||
TestAssert(result == lExpected, message);
|
||||
sctest::Assert(result == lExpected, message);
|
||||
|
||||
result = U::S::TrimTrailingWhitespaceDup(line);
|
||||
message = "TrimTrailingDup(\"" + line + "\"): '" + result + "'";
|
||||
TestAssert(result == rExpected, message);
|
||||
sctest::Assert(result == rExpected, message);
|
||||
|
||||
result = U::S::TrimWhitespaceDup(line);
|
||||
message = "TrimDup(\"" + line + "\"): '" + result + "'";
|
||||
TestAssert(result == expected, message);
|
||||
sctest::Assert(result == expected, message);
|
||||
|
||||
result = line;
|
||||
U::S::TrimLeadingWhitespace(result);
|
||||
message = "TrimLeadingDup(\"" + line + "\"): '" + result + "'";
|
||||
TestAssert(result == lExpected, message);
|
||||
sctest::Assert(result == lExpected, message);
|
||||
|
||||
result = line;
|
||||
U::S::TrimTrailingWhitespace(result);
|
||||
message = "TrimTrailingDup(\"" + line + "\"): '" + result + "'";
|
||||
TestAssert(result == rExpected, message);
|
||||
sctest::Assert(result == rExpected, message);
|
||||
|
||||
result = line;
|
||||
U::S::TrimWhitespace(result);
|
||||
message = "TrimDup(\"" + line + "\"): '" + result + "'";
|
||||
TestAssert(result == expected, message);
|
||||
sctest::Assert(result == expected, message);
|
||||
}
|
||||
|
||||
|
||||
@@ -96,7 +96,7 @@ TestSplit(std::string line, std::string delim, size_t maxCount, std::vector<std:
|
||||
std::cout << "\texpect: " << vec2string(expected) << "\n";
|
||||
auto result = U::S::SplitN(line, delim, maxCount);
|
||||
std::cout << "\tresult: " << U::S::VectorToString(result) << "\n";
|
||||
TestAssert(result == expected, U::S::VectorToString(result));
|
||||
sctest::Assert(result == expected, U::S::VectorToString(result));
|
||||
std::cout << "OK!\n";
|
||||
}
|
||||
|
||||
@@ -119,12 +119,12 @@ TestWrapping()
|
||||
};
|
||||
|
||||
auto wrapped = U::S::WrapText(testLine, 16);
|
||||
TestAssert(wrapped.size() == expected.size(),
|
||||
U::S::VectorToString(wrapped) + " != " + U::S::VectorToString(expected));
|
||||
sctest::Assert(wrapped.size() == expected.size(),
|
||||
U::S::VectorToString(wrapped) + " != " + U::S::VectorToString(expected));
|
||||
|
||||
for (size_t i = 0; i < wrapped.size(); i++) {
|
||||
TestAssert(wrapped[i] == expected[i],
|
||||
"\"" + wrapped[i] + "\" != \"" + expected[i] + "\"");
|
||||
sctest::Assert(wrapped[i] == expected[i],
|
||||
"\"" + wrapped[i] + "\" != \"" + expected[i] + "\"");
|
||||
}
|
||||
|
||||
U::S::WriteTabIndented(std::cout, wrapped, 4, true);
|
||||
|
||||
498
test/vector.cc
Normal file
498
test/vector.cc
Normal file
@@ -0,0 +1,498 @@
|
||||
//
|
||||
// Project: scccl
|
||||
// File: test/math/geom2d_test.cpp
|
||||
// Author: Kyle Isom
|
||||
// Date: 2020-02-19
|
||||
//
|
||||
// vector runs a set of unit tests on the vector parts of the
|
||||
// math::geom namespace.
|
||||
//
|
||||
// Copyright 2020 Kyle Isom <kyle@imap.cc>
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
||||
#include <scmp/geom/Vector.h>
|
||||
#include <sctest/SimpleSuite.h>
|
||||
#include <sctest/Checks.h>
|
||||
|
||||
|
||||
using namespace scmp;
|
||||
using namespace sctest;
|
||||
using namespace std;
|
||||
|
||||
|
||||
static bool
|
||||
Vector3Miscellaneous_ExtractionOperator3d()
|
||||
{
|
||||
geom::Vector3d vec {1.0, 2.0, 3.0};
|
||||
stringstream vecBuffer;
|
||||
|
||||
vecBuffer << vec;
|
||||
SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
Vector3Miscellaneous_ExtractionOperator3f()
|
||||
{
|
||||
geom::Vector3f vec {1.0, 2.0, 3.0};
|
||||
stringstream vecBuffer;
|
||||
|
||||
vecBuffer << vec;
|
||||
SCTEST_CHECK_EQ(vecBuffer.str(), "<1, 2, 3>");
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3Miscellaneous_SetEpsilon() {
|
||||
geom::Vector3f a {1.0, 1.0, 1.0};
|
||||
geom::Vector3f b;
|
||||
|
||||
a.setEpsilon(1.1);
|
||||
SCTEST_CHECK_EQ(a, b);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_Magnitude()
|
||||
{
|
||||
geom::Vector3f v3f {1.0, -2.0, 3.0};
|
||||
const float expected = 3.74165738677394;
|
||||
|
||||
SCTEST_CHECK_FEQ(v3f.magnitude(), expected);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_Equality()
|
||||
{
|
||||
geom::Vector3f a {1.0, 2.0, 3.0};
|
||||
geom::Vector3f b {1.0, 2.0, 3.0};
|
||||
geom::Vector3f c {1.0, 2.0, 1.0};
|
||||
|
||||
SCTEST_CHECK_EQ(a, b);
|
||||
SCTEST_CHECK_EQ(b, a);
|
||||
SCTEST_CHECK_NE(a, c);
|
||||
SCTEST_CHECK_NE(b, c);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_Addition()
|
||||
{
|
||||
geom::Vector3f a {1.0, 2.0, 3.0};
|
||||
geom::Vector3f b {4.0, 5.0, 6.0};
|
||||
geom::Vector3f expected {5.0, 7.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(a+b, expected);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_Subtraction()
|
||||
{
|
||||
geom::Vector3f a {1.0, 2.0, 3.0};
|
||||
geom::Vector3f b {4.0, 5.0, 6.0};
|
||||
geom::Vector3f c {5.0, 7.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(c-b, a);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_ScalarMultiplication()
|
||||
{
|
||||
geom::Vector3f a {1.0, 2.0, 3.0};
|
||||
geom::Vector3f expected {3.0, 6.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(a * 3.0, expected);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_ScalarDivision()
|
||||
{
|
||||
geom::Vector3f a {1.0, 2.0, 3.0};
|
||||
geom::Vector3f b {3.0, 6.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(b / 3.0, a);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_DotProduct()
|
||||
{
|
||||
geom::Vector3f a {1.0, 2.0, 3.0};
|
||||
geom::Vector3f b {4.0, 5.0, 6.0};
|
||||
|
||||
SCTEST_CHECK_FEQ(a * b, (float)32.0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_UnitVector()
|
||||
{
|
||||
// Test values randomly generated and calculated with numpy.
|
||||
geom::Vector3f vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652};
|
||||
geom::Vector3f unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804};
|
||||
geom::Vector3f unit2;
|
||||
|
||||
SCTEST_CHECK_EQ(vec3.unitVector(), unit);
|
||||
SCTEST_CHECK_FALSE(vec3.isUnitVector());
|
||||
SCTEST_CHECK(unit.isUnitVector());
|
||||
SCTEST_CHECK(unit2.isUnitVector());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_Angle()
|
||||
{
|
||||
geom::Vector3f a {0.3977933061361172, 8.053980094436525, 8.1287759943773};
|
||||
geom::Vector3f b {9.817895298608196, 4.034166890407462, 4.37628316513266};
|
||||
geom::Vector3f c {7.35, 0.221, 5.188};
|
||||
geom::Vector3f d {2.751, 8.259, 3.985};
|
||||
|
||||
SCTEST_CHECK_FEQ(a.angle(b), (float)0.9914540426033251);
|
||||
if (!scmp::WithinTolerance(c.angle(d), (float)1.052, (float)0.001)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_ParallelOrthogonalVectors()
|
||||
{
|
||||
geom::Vector3f a {-2.029, 9.97, 4.172};
|
||||
geom::Vector3f b {-9.231, -6.639, -7.245};
|
||||
geom::Vector3f c {-2.328, -7.284, -1.214};
|
||||
geom::Vector3f d {-1.821, 1.072, -2.94};
|
||||
geom::Vector3f e {-2.0, 1.0, 3.0};
|
||||
geom::Vector3f f {-6.0, 3.0, 9.0};
|
||||
geom::Vector3f zeroVector {0.0, 0.0, 0.0};
|
||||
|
||||
SCTEST_CHECK_FALSE(a.isParallel(b));
|
||||
SCTEST_CHECK_FALSE(a.isOrthogonal(b));
|
||||
|
||||
SCTEST_CHECK_FALSE(c.isParallel(d));
|
||||
SCTEST_CHECK(c.isOrthogonal(d));
|
||||
|
||||
SCTEST_CHECK(e.isParallel(f));
|
||||
SCTEST_CHECK_FALSE(e.isOrthogonal(f));
|
||||
|
||||
SCTEST_CHECK(zeroVector.isZero());
|
||||
SCTEST_CHECK(c.isParallel(zeroVector));
|
||||
SCTEST_CHECK(c.isOrthogonal(zeroVector));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_Projections()
|
||||
{
|
||||
geom::Vector3f a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
|
||||
geom::Vector3f b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
|
||||
geom::Vector3f c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
|
||||
geom::Vector3f d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
|
||||
|
||||
SCTEST_CHECK_EQ(a.projectParallel(b), c);
|
||||
SCTEST_CHECK_EQ(a.projectOrthogonal(b), d);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3FloatTests_CrossProduct()
|
||||
{
|
||||
geom::Vector3f a {8.462, 7.893, -8.187};
|
||||
geom::Vector3f b {6.984, -5.975, 4.778};
|
||||
geom::Vector3f c {-11.2046, -97.6094, -105.685};
|
||||
|
||||
c.setEpsilon(0.001);
|
||||
SCTEST_CHECK_EQ(c, a.cross(b));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_Magnitude()
|
||||
{
|
||||
geom::Vector3d v3d{1.0, -2.0, 3.0};
|
||||
const double expected = 3.74165738677394;
|
||||
|
||||
SCTEST_CHECK_DEQ(v3d.magnitude(), expected);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_Equality()
|
||||
{
|
||||
geom::Vector3d a {1.0, 2.0, 3.0};
|
||||
geom::Vector3d b {1.0, 2.0, 3.0};
|
||||
geom::Vector3d c {1.0, 2.0, 1.0};
|
||||
|
||||
SCTEST_CHECK_EQ(a, b);
|
||||
SCTEST_CHECK_EQ(b, a);
|
||||
SCTEST_CHECK_NE(a, c);
|
||||
SCTEST_CHECK_NE(b, c);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_Addition()
|
||||
{
|
||||
geom::Vector3d a {1.0, 2.0, 3.0};
|
||||
geom::Vector3d b {4.0, 5.0, 6.0};
|
||||
geom::Vector3d expected {5.0, 7.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(a+b, expected);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_Subtraction()
|
||||
{
|
||||
geom::Vector3d a {1.0, 2.0, 3.0};
|
||||
geom::Vector3d b {4.0, 5.0, 6.0};
|
||||
geom::Vector3d c {5.0, 7.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(c-b, a);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_ScalarMultiplication()
|
||||
{
|
||||
geom::Vector3d a {1.0, 2.0, 3.0};
|
||||
geom::Vector3d expected {3.0, 6.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(a * 3.0, expected);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_ScalarDivision()
|
||||
{
|
||||
geom::Vector3d a {1.0, 2.0, 3.0};
|
||||
geom::Vector3d b {3.0, 6.0, 9.0};
|
||||
|
||||
SCTEST_CHECK_EQ(b / 3.0, a);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_DotProduct()
|
||||
{
|
||||
geom::Vector3d a {1.0, 2.0, 3.0};
|
||||
geom::Vector3d b {4.0, 5.0, 6.0};
|
||||
|
||||
SCTEST_CHECK_DEQ(a * b, 32.0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_UnitVector()
|
||||
{
|
||||
// Test values randomly generated and calculated with numpy.
|
||||
geom::Vector3d vec3 {5.320264018493507, 5.6541812891273935, 1.9233435162644652};
|
||||
geom::Vector3d unit {0.6651669556972103, 0.7069150218815566, 0.24046636539587804};
|
||||
geom::Vector3d unit2;
|
||||
|
||||
SCTEST_CHECK_EQ(vec3.unitVector(), unit);
|
||||
SCTEST_CHECK_FALSE(vec3.isUnitVector());
|
||||
SCTEST_CHECK(unit.isUnitVector());
|
||||
SCTEST_CHECK(unit2.isUnitVector());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_Angle()
|
||||
{
|
||||
geom::Vector3d a {0.3977933061361172, 8.053980094436525, 8.1287759943773};
|
||||
geom::Vector3d b {9.817895298608196, 4.034166890407462, 4.37628316513266};
|
||||
geom::Vector3d c {7.35, 0.221, 5.188};
|
||||
geom::Vector3d d {2.751, 8.259, 3.985};
|
||||
|
||||
SCTEST_CHECK_DEQ(a.angle(b), 0.9914540426033251);
|
||||
if (!scmp::WithinTolerance(c.angle(d), (double)1.052, (double)0.001)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_ParallelOrthogonalVectors()
|
||||
{
|
||||
geom::Vector3d a {-2.029, 9.97, 4.172};
|
||||
geom::Vector3d b {-9.231, -6.639, -7.245};
|
||||
geom::Vector3d c {-2.328, -7.284, -1.214};
|
||||
geom::Vector3d d {-1.821, 1.072, -2.94};
|
||||
geom::Vector3d e {-2.0, 1.0, 3.0};
|
||||
geom::Vector3d f {-6.0, 3.0, 9.0};
|
||||
geom::Vector3d zeroVector {0.0, 0.0, 0.0};
|
||||
|
||||
SCTEST_CHECK_FALSE(a.isParallel(b));
|
||||
SCTEST_CHECK_FALSE(a.isOrthogonal(b));
|
||||
|
||||
SCTEST_CHECK_FALSE(c.isParallel(d));
|
||||
SCTEST_CHECK(c.isOrthogonal(d));
|
||||
|
||||
SCTEST_CHECK(e.isParallel(f));
|
||||
SCTEST_CHECK_FALSE(e.isOrthogonal(f));
|
||||
|
||||
SCTEST_CHECK(zeroVector.isZero());
|
||||
SCTEST_CHECK(c.isParallel(zeroVector));
|
||||
SCTEST_CHECK(c.isOrthogonal(zeroVector));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_Projections()
|
||||
{
|
||||
geom::Vector3d a {4.866769214609107, 6.2356222686140566, 9.140878417029711};
|
||||
geom::Vector3d b {6.135533104801077, 8.757851406697895, 0.6738031370548048};
|
||||
geom::Vector3d c {4.843812341655318, 6.9140509888133055, 0.5319465962229454};
|
||||
geom::Vector3d d {0.02295687295378901, -0.6784287201992489, 8.608931820806765};
|
||||
|
||||
SCTEST_CHECK_EQ(a.projectParallel(b), c);
|
||||
SCTEST_CHECK_EQ(a.projectOrthogonal(b), d);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool
|
||||
Vector3DoubleTests_CrossProduct()
|
||||
{
|
||||
geom::Vector3d a {8.462, 7.893, -8.187};
|
||||
geom::Vector3d b {6.984, -5.975, 4.778};
|
||||
geom::Vector3d c {-11.2046, -97.6094, -105.685};
|
||||
|
||||
c.setEpsilon(0.001); // double trouble
|
||||
SCTEST_CHECK_EQ(c, a.cross(b));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
main(void)
|
||||
{
|
||||
SimpleSuite ts;
|
||||
ts.AddTest("Vector3Miscellaneous_ExtractionOperator3d",
|
||||
Vector3Miscellaneous_ExtractionOperator3d);
|
||||
ts.AddTest("Vector3Miscellaneous_ExtractionOperator3f",
|
||||
Vector3Miscellaneous_ExtractionOperator3f);
|
||||
ts.AddTest("Vector3Miscellaneous_SetEpsilon",
|
||||
Vector3Miscellaneous_SetEpsilon);
|
||||
ts.AddTest("Vector3FloatTests_Magnitude",
|
||||
Vector3FloatTests_Magnitude);
|
||||
ts.AddTest("Vector3FloatTests_Equality",
|
||||
Vector3FloatTests_Equality);
|
||||
ts.AddTest("Vector3FloatTests_Addition",
|
||||
Vector3FloatTests_Addition);
|
||||
ts.AddTest("Vector3FloatTests_Subtraction",
|
||||
Vector3FloatTests_Subtraction);
|
||||
ts.AddTest("Vector3FloatTests_ScalarMultiplication",
|
||||
Vector3FloatTests_ScalarMultiplication);
|
||||
ts.AddTest("Vector3FloatTests_ScalarDivision",
|
||||
Vector3FloatTests_ScalarDivision);
|
||||
ts.AddTest("Vector3FloatTests_DotProduct",
|
||||
Vector3FloatTests_DotProduct);
|
||||
ts.AddTest("Vector3FloatTests_UnitVector",
|
||||
Vector3FloatTests_UnitVector);
|
||||
ts.AddTest("Vector3FloatTests_Angle",
|
||||
Vector3FloatTests_Angle);
|
||||
ts.AddTest("Vector3FloatTests_ParallelOrthogonalVectors",
|
||||
Vector3FloatTests_ParallelOrthogonalVectors);
|
||||
ts.AddTest("Vector3FloatTests_Projections",
|
||||
Vector3FloatTests_Projections);
|
||||
ts.AddTest("Vector3FloatTests_CrossProduct",
|
||||
Vector3FloatTests_CrossProduct);
|
||||
ts.AddTest("Vector3DoubleTests_Magnitude",
|
||||
Vector3DoubleTests_Magnitude);
|
||||
ts.AddTest("Vector3DoubleTests_Equality",
|
||||
Vector3DoubleTests_Equality);
|
||||
ts.AddTest("Vector3DoubleTests_Addition",
|
||||
Vector3DoubleTests_Addition);
|
||||
ts.AddTest("Vector3DoubleTests_Subtraction",
|
||||
Vector3DoubleTests_Subtraction);
|
||||
ts.AddTest("Vector3DoubleTests_ScalarMultiplication",
|
||||
Vector3DoubleTests_ScalarMultiplication);
|
||||
ts.AddTest("Vector3DoubleTests_ScalarDivision",
|
||||
Vector3DoubleTests_ScalarDivision);
|
||||
ts.AddTest("Vector3DoubleTests_DotProduct",
|
||||
Vector3DoubleTests_DotProduct);
|
||||
ts.AddTest("Vector3DoubleTests_UnitVector",
|
||||
Vector3DoubleTests_UnitVector);
|
||||
ts.AddTest("Vector3DoubleTests_Angle",
|
||||
Vector3DoubleTests_Angle);
|
||||
ts.AddTest("Vector3DoubleTests_ParallelOrthogonalVectors",
|
||||
Vector3DoubleTests_ParallelOrthogonalVectors);
|
||||
ts.AddTest("Vector3DoubleTests_Projections",
|
||||
Vector3DoubleTests_Projections);
|
||||
ts.AddTest("Vector3DoubleTests_CrossProduct",
|
||||
Vector3DoubleTests_CrossProduct);
|
||||
|
||||
if (ts.Run()) {
|
||||
std::cout << "OK" << std::endl;
|
||||
return 0;
|
||||
}
|
||||
else {
|
||||
auto r = ts.GetReport();
|
||||
std::cerr << r.Failing << "/" << r.Total << " tests failed." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user