Continuing refactor work.

This commit is contained in:
Kyle Isom 2023-10-19 00:37:56 -07:00
parent 8d02d078e7
commit 36fe049485
28 changed files with 1658 additions and 122 deletions

View File

@ -39,7 +39,7 @@ set(HEADER_FILES
include/scsl/Buffer.h
include/scsl/Commander.h
include/scsl/Dictionary.h
include/scsl/Exceptions.h
include/sctest/Exceptions.h
include/scsl/Flag.h
include/scsl/StringUtil.h
include/scsl/TLV.h
@ -53,6 +53,7 @@ set(HEADER_FILES
include/sctest/Assert.h
include/sctest/Report.h
include/scmp/filter/Madgwick.h
)
include_directories(include)
@ -62,7 +63,7 @@ set(SOURCE_FILES
src/sl/Buffer.cc
src/sl/Commander.cc
src/sl/Dictionary.cc
src/sl/Exceptions.cc
src/test/Exceptions.cc
src/sl/Flag.cc
src/sl/StringUtil.cc
src/sl/TLV.cc
@ -102,12 +103,21 @@ macro(generate_test name)
add_test(test_${name} test_${name})
endmacro()
# core standard library
generate_test(buffer)
generate_test(tlv)
generate_test(dictionary)
generate_test(flag)
generate_test(stringutil)
# math and physics
generate_test(coord2d)
generate_test(madgwick)
generate_test(orientation)
generate_test(quaternion)
generate_test(vector)
# test tooling
generate_test(simple_suite_example)
include(CMakePackageConfigHelpers)

View File

@ -8,7 +8,7 @@ set(DOXYGEN_EXTRACT_ALL YES)
message(STATUS "Doxygen found, building docs.")
doxygen_add_docs(scsl_docs
${HEADER_FILES} ${SOURCE_FILES}
${HEADER_FILES}
USE_STAMP_FILE)
add_dependencies(scsl scsl_docs)
install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/html DESTINATION share/doc/scsl)

View File

@ -71,7 +71,7 @@ WithinTolerance(T a, T b, T epsilon)
}
} // namespace math
} // namespace scmp
#endif //SCCCL_MATH_H

View File

@ -0,0 +1,158 @@
///
/// \file Madwick.cc
/// \author K. Isom <kyle@imap.cc>
/// \date 2019-08-06
/// \brief Implementation of a Madgwick filter.
///
/// See https://courses.cs.washington.edu/courses/cse466/14au/labs/l4/madgwick_internal_report.pdf.
///
/// Copyright 2019 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
/// \file madgwick.h
/// \brief Implementation of a Madgwick filter.
///
/// See
#ifndef SCMP_FILTER_MADGWICK_H
#define SCMP_FILTER_MADGWICK_H
#include <scmp/geom/Vector.h>
#include <scmp/geom/Quaternion.h>
/// scmp contains the wntrmute robotics code.
namespace scmp {
/// filter contains filtering algorithms.
namespace filter {
/// @brief Madgwick implements an efficient Orientation filter for IMUs.
///
/// Madgwick is a novel Orientation filter applicable to IMUs
/// consisting of tri-axis gyroscopes and accelerometers, and MARG
/// sensor arrays that also include tri-axis magnetometers. The MARG
/// implementation incorporates magnetic distortionand gyroscope bias
/// drift compensation.
///
/// It is described in the paper [An efficient Orientation filter for inertial and inertial/magnetic sensor arrays](http://x-io.co.uk/res/doc/madgwick_internal_report.pdf).
///
/// \tparam T A floating point type.
template <typename T>
class Madgwick {
public:
/// The Madgwick filter is initialised with an identity quaternion.
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {};
/// The Madgwick filter is initialised with a sensor frame.
///
/// \param sf A sensor frame; if zero, the sensor frame will be
/// initialised as an identity quaternion.
Madgwick(scmp::geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame()
{
if (!sf.isZero()) {
sensorFrame = scmp::geom::quaternion(sf, 0.0);
}
}
/// Initialise the filter with a sensor frame quaternion.
///
/// \param sf A quaternion representing the current Orientation.
Madgwick(scmp::geom::Quaternion<T> sf) :
deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {};
/// Return the current Orientation as measured by the filter.
///
/// \return The current sensor frame.
scmp::geom::Quaternion<T>
Orientation() const
{
return this->sensorFrame;
}
/// Return the rate of change of the Orientation of the earth frame
/// with respect to the sensor frame.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
/// \return A quaternion representing the rate of angular change.
scmp::geom::Quaternion<T>
AngularRate(const scmp::geom::Vector<T, 3> &gyro) const
{
return (this->sensorFrame * 0.5) * scmp::geom::Quaternion<T>(gyro, 0.0);
}
/// Update the sensor frame to a new frame.
///
/// \param sf The new sensor frame replacing the previous one.
/// \param delta The time delta since the last update.
void
UpdateFrame(const scmp::geom::Quaternion<T> &sf, T delta)
{
this->previousSensorFrame = this->sensorFrame;
this->sensorFrame = sf;
this->deltaT = delta;
}
/// Update the sensor frame with a gyroscope reading.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as w_x, w_y, w_z.
/// \param delta The time step between readings. It must not be zero.
void
UpdateAngularOrientation(const scmp::geom::Vector<T, 3> &gyro, T delta)
{
// Ensure the delta isn't zero within a 100 μs tolerance.
assert(!scmp::WithinTolerance(delta, 0.0, 0.0001));
scmp::geom::Quaternion<T> q = this->AngularRate(gyro) * delta;
this->UpdateFrame(this->sensorFrame + q, delta);
}
/// Retrieve a vector of the Euler angles in ZYX Orientation.
///
/// \return A vector of Euler angles as <ψ, θ, ϕ>.
scmp::geom::Vector<T, 3>
Euler()
{
return this->sensorFrame.euler();
}
private:
T deltaT;
scmp::geom::Quaternion<T> previousSensorFrame;
scmp::geom::Quaternion<T> sensorFrame;
};
/// Madgwickd is a shorthand alias for a Madgwick<double>.
typedef Madgwick<double> Madgwickd;
/// Madgwickf is a shorthand alias for a Madgwick<float>.
typedef Madgwick<float> Madgwickf;
} // namespace filter
} // namespace scmp
#endif // SCMP_FILTER_MADGWICK_H

View File

@ -1,5 +1,5 @@
/**
* orientation.h concerns itself with computing the orientation of some
* orientation.h concerns itself with computing the Orientation of some
* vector with respect to a reference plane that is assumed to be the
* of the Earth.
*/
@ -61,28 +61,28 @@ static const Vector3f Basis3f[] = {
/// Heading2f returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
float Heading2f(Vector2f vec);
/// Heading2d returns a compass heading for a Vector2d.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
double Heading2d(Vector2d vec);
/// Heading3f returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
float Heading3f(Vector3f vec);
/// Heading3d returns a compass heading for a Vector2f.
/// @param vec A vector orientation.
/// @param vec A vector Orientation.
/// @return The compass heading of the vector in radians.
double Heading3d(Vector3d vec);
} // namespace geom
} // namespace math
} // namespace scmp
#endif // __WRMATH_ORIENTATION_H

View File

@ -19,7 +19,7 @@ namespace scmp {
namespace geom {
/// @brief Quaternions provide a representation of orientation and rotations
/// @brief Quaternions provide a representation of Orientation and rotations
/// in three dimensions.
///
/// Quaternions encode rotations in three-dimensional space. While technically
@ -441,7 +441,7 @@ quaternion(Vector<T, 3> axis, T angle)
/// return a quaternion.
///
/// @param euler A vector Euler angle in ZYX sequence.
/// @return A Quaternion representation of the orientation represented
/// @return A Quaternion representation of the Orientation represented
/// by the Euler angles.
/// @relatesalso Quaternion
Quaternionf quaternionf_from_euler(Vector3f euler);
@ -451,7 +451,7 @@ Quaternionf quaternionf_from_euler(Vector3f euler);
/// return a quaternion.
///
/// @param euler A vector Euler angle in ZYX sequence.
/// @return A Quaternion representation of the orientation represented
/// @return A Quaternion representation of the Orientation represented
/// by the Euler angles.
/// @relatesalso Quaternion
Quaterniond quaterniond_from_euler(Vector3d euler);

View File

@ -6,7 +6,7 @@
// Namespace: math::vectors.
//
// vectors.h defines the Vector2D class and associated functions in the
// namespace math::vectors.
// namespace scmp::vectors.
//
// Copyright 2017 Kyle Isom <kyle@imap.cc>
//
@ -415,7 +415,7 @@ typedef Vector<double, 4> Vector4d;
} // namespace geom
} // namespace math
} // namespace scmp

View File

@ -37,7 +37,7 @@
#include <iostream>
#include <sys/stat.h>
#include "Exceptions.h"
#include "sctest/Exceptions.h"
#if defined(__WIN64__) || defined(__WIN32__) || defined(WIN32)

View File

@ -27,23 +27,23 @@
#include <string>
namespace scsl {
namespace sctest {
/// TestAssert is a variant on the assert macro. This variant is intended to be
/// Assert is a variant on the assert macro. This variant is intended to be
/// a drop-in replacement for the cassert macro: even in release mode, the tests
/// should still run.
///
/// If NDEBUG is set, TestAssert will throw an exception if condition is false.
/// If NDEBUG is set, Assert will throw an exception if condition is false.
/// Otherwise, it calls assert after printing the message.
///
/// \param condition If true, TestAssert throws an exception.
void TestAssert(bool condition);
/// \param condition If true, Assert throws an exception.
void Assert(bool condition);
/// TestAssert is a variant on the assert macro.
/// Assert is a variant on the assert macro.
///
/// If NDEBUG is set, TestAssert will throw an exception if condition is false.
/// If NDEBUG is set, Assert will throw an exception if condition is false.
/// Otherwise, it calls assert after printing the message.
///
/// In addition to NDEBUG, SCSL_NOEXCEPT will suppress assertions.
@ -52,7 +52,7 @@ void TestAssert(bool condition);
///
/// \param condition The condition to assert.
/// \param message The message that should be displayed if condition is false.
void TestAssert(bool condition, std::string message);
void Assert(bool condition, std::string message);
} // namespace scsl

53
include/sctest/Checks.h Executable file
View File

@ -0,0 +1,53 @@
///
/// \file Checks.h
/// \author K. Isom <kyle@imap.cc>
/// \date 2017-06-05
/// \brief Provides a number of utility macros for testing.
///
/// Copyright 2017 K. Isom <kyle@imap.cc>
///
/// Permission to use, copy, modify, and/or distribute this software for
/// any purpose with or without fee is hereby granted, provided that
/// the above copyright notice and this permission notice appear in all /// copies.
///
/// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
/// WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
/// WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE
/// AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL
/// DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA
/// OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
#ifndef SCTEST_CHECKS_H
#define SCTEST_CHECKS_H
#include <scmp/Math.h>
namespace sctest {
// The following checks are designed as shortcuts that return false on
// if some condition isn't met.
#define SCTEST_CHECK(x) if (!(x)) { return false; }
#define SCTEST_CHECK_FALSE(x) if ((x)) { return false; }
#define SCTEST_CHECK_EQ(x, y) if ((x) != (y)) { return false; }
#define SCTEST_CHECK_NE(x, y) if ((x) == (y)) { return false; }
#define SCTEST_CHECK_ZERO(x) if ((x) != 0) { return false; }
#define SCTEST_CHECK_GTZ(x) if ((x) > 0) { return false; }
#define SCTEST_CHECK_GEZ(x) if ((x) >= 0) { return false; }
#define SCTEST_CHECK_LEZ(x) if ((x) <= 0) { return false; }
#define SCTEST_CHECK_LTZ(x) if ((x) < 0) { return false; }
#define SCTEST_CHECK_FEQ(x, y) { float eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_DEQ(x, y) { double eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_FEQ_EPS(x, y, eps) { if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_DEQ_EPS(x, y, eps) { if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
} // namespace sctest
#endif

View File

@ -19,6 +19,7 @@
/// TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
/// PERFORMANCE OF THIS SOFTWARE.
///
#ifndef SCSL_EXCEPTIONS_H
#define SCSL_EXCEPTIONS_H
@ -27,7 +28,7 @@
#include <string>
namespace scsl {
namespace sctest {
/// NotImplemented is an exception reserved for unsupported platforms.
@ -63,7 +64,7 @@ private:
};
} // namespace scsl
} // namespace sctest
#endif //SCSL_EXCEPTIONS_H
#endif // SCSL_EXCEPTIONS_H

View File

@ -21,8 +21,8 @@
// 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.
#ifndef __SCTEST_REPORT_H
#define __SCTEST_REPORT_H
#ifndef SCTEST_REPORT_H
#define SCTEST_REPORT_H
#include <chrono>

View File

@ -20,8 +20,8 @@
// 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.
#ifndef __SCTEST_SIMPLESUITE_H
#define __SCTEST_SIMPLESUITE_H
#ifndef SCTEST_SIMPLESUITE_H
#define SCTEST_SIMPLESUITE_H
// SimpleSuite.h
// This header file defines the interface for a simple suite of tests.

View File

@ -1,50 +0,0 @@
//
// Project: scccl
// File: include/test/checks.h
// Author: Kyle Isom
// Date: 2017-06-05
// Namespace: test.
//
// checks.h defines a number of macros (which are global in scope) for
// use in test functions that return bools.
//
// 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.
#ifndef __SCTEST_CHECKS_H
#define __SCTEST_CHECKS_H
#include <scmp/Math.h>
namespace sctest {
// The following checks are designed as shortcuts that just return false on certain
// conditions.
#define SCTEST_CHECK(x) if (!(x)) { return false; }
#define SCTEST_CHECK_FALSE(x) if ((x)) { return false; }
#define SCTEST_CHECK_EQ(x, y) if ((x) != (y)) { return false; }
#define SCTEST_CHECK_NE(x, y) if ((x) == (y)) { return false; }
#define SCTEST_CHECK_ZERO(x) if ((x) != 0) { return false; }
#define SCTEST_CHECK_GTZ(x) if ((x) > 0) { return false; }
#define SCTEST_CHECK_GEZ(x) if ((x) >= 0) { return false; }
#define SCTEST_CHECK_LEZ(x) if ((x) <= 0) { return false; }
#define SCTEST_CHECK_LTZ(x) if ((x) < 0) { return false; }
#define SCTEST_CHECK_FEQ(x, y) { float eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
#define SCTEST_CHECK_DEQ(x, y) { double eps; scmp::DefaultEpsilon(eps); if (!scmp::WithinTolerance((x), (y), eps)) { return false; }}
} // namespace test
#endif

View File

@ -144,5 +144,5 @@ DefaultEpsilon(float &epsilon)
}
} // namespace math
} // namespace scmp

View File

@ -37,4 +37,4 @@ Heading3d(Vector3d vec)
} // namespace geom
} // namespace math
} // namespace scmp

View File

@ -88,4 +88,4 @@ Quaternion_SelfTest()
} // namespace geom
} // namespace math
} // namespace scmp

View File

@ -20,7 +20,7 @@
/// PERFORMANCE OF THIS SOFTWARE.
///
#include <scsl/Exceptions.h>
#include "sctest/Exceptions.h"
#include <sctest/Assert.h>
#include <cassert>
@ -28,10 +28,10 @@
#include <sstream>
namespace scsl {
namespace sctest {
void
TestAssert(bool condition, std::string message)
Assert(bool condition, std::string message)
{
#if defined(NDEBUG) || defined(SCSL_NOEXCEPT)
if (!condition) {
@ -47,7 +47,7 @@ TestAssert(bool condition, std::string message)
void
TestAssert(bool condition)
Assert(bool condition)
{
#if defined(NDEBUG)
if (condition) {
@ -67,4 +67,4 @@ TestAssert(bool condition)
}
} // namespace scsl
} // namespace sctest

View File

@ -20,10 +20,10 @@
/// PERFORMANCE OF THIS SOFTWARE.
///
#include <scsl/Exceptions.h>
#include <sctest/Exceptions.h>
namespace scsl {
namespace sctest {
AssertionFailed::AssertionFailed(std::string message) : msg(message) {}
@ -36,4 +36,4 @@ AssertionFailed::what() const throw()
}
}
} // namespace sctest

227
test/coord2d.cc Executable file
View 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;
}
}

View File

@ -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,
Assert(testSetKV(dict, TEST_KVSTR1, TEST_KVSTRLEN1, TEST_KVSTR3,
TEST_KVSTRLEN3));
std::cout << dict;
TestAssert(testSetKV(dict, TEST_KVSTR2, TEST_KVSTRLEN2, TEST_KVSTR3,
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,
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,
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,
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";

View File

@ -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
View 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
View 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
View 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;
}
}

View File

@ -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,11 +119,11 @@ TestWrapping()
};
auto wrapped = U::S::WrapText(testLine, 16);
TestAssert(wrapped.size() == expected.size(),
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],
sctest::Assert(wrapped[i] == expected[i],
"\"" + wrapped[i] + "\" != \"" + expected[i] + "\"");
}

498
test/vector.cc Normal file
View 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;
}
}