diff --git a/CMakeLists.txt b/CMakeLists.txt index 0eea2aa..d8719ec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/cmake/docs.cmake b/cmake/docs.cmake index 6fa44db..e4d31a0 100644 --- a/cmake/docs.cmake +++ b/cmake/docs.cmake @@ -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) diff --git a/include/scmp/Math.h b/include/scmp/Math.h index 014964a..599a333 100644 --- a/include/scmp/Math.h +++ b/include/scmp/Math.h @@ -71,7 +71,7 @@ WithinTolerance(T a, T b, T epsilon) } -} // namespace math +} // namespace scmp #endif //SCCCL_MATH_H diff --git a/include/scmp/filter/Madgwick.h b/include/scmp/filter/Madgwick.h new file mode 100644 index 0000000..aba8327 --- /dev/null +++ b/include/scmp/filter/Madgwick.h @@ -0,0 +1,158 @@ +/// +/// \file Madwick.cc +/// \author K. Isom +/// \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 +/// +/// 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 +#include + + +/// 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 +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 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 sf) : + deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {}; + + + /// Return the current Orientation as measured by the filter. + /// + /// \return The current sensor frame. + scmp::geom::Quaternion + 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 + AngularRate(const scmp::geom::Vector &gyro) const + { + return (this->sensorFrame * 0.5) * scmp::geom::Quaternion(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 &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 &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 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 + Euler() + { + return this->sensorFrame.euler(); + } + +private: + T deltaT; + scmp::geom::Quaternion previousSensorFrame; + scmp::geom::Quaternion sensorFrame; +}; + + +/// Madgwickd is a shorthand alias for a Madgwick. +typedef Madgwick Madgwickd; + +/// Madgwickf is a shorthand alias for a Madgwick. +typedef Madgwick Madgwickf; + + +} // namespace filter +} // namespace scmp + + +#endif // SCMP_FILTER_MADGWICK_H diff --git a/include/scmp/geom/Orientation.h b/include/scmp/geom/Orientation.h index adfd0bd..cfce891 100644 --- a/include/scmp/geom/Orientation.h +++ b/include/scmp/geom/Orientation.h @@ -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 diff --git a/include/scmp/geom/Quaternion.h b/include/scmp/geom/Quaternion.h index 7499cd9..77252e1 100644 --- a/include/scmp/geom/Quaternion.h +++ b/include/scmp/geom/Quaternion.h @@ -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 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); diff --git a/include/scmp/geom/Vector.h b/include/scmp/geom/Vector.h index 79395bf..69df2e9 100644 --- a/include/scmp/geom/Vector.h +++ b/include/scmp/geom/Vector.h @@ -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 // @@ -415,7 +415,7 @@ typedef Vector Vector4d; } // namespace geom -} // namespace math +} // namespace scmp diff --git a/include/scsl/Arena.h b/include/scsl/Arena.h index f57858d..df1c42f 100644 --- a/include/scsl/Arena.h +++ b/include/scsl/Arena.h @@ -37,7 +37,7 @@ #include #include -#include "Exceptions.h" +#include "sctest/Exceptions.h" #if defined(__WIN64__) || defined(__WIN32__) || defined(WIN32) diff --git a/include/sctest/Assert.h b/include/sctest/Assert.h index 68a3393..8127853 100644 --- a/include/sctest/Assert.h +++ b/include/sctest/Assert.h @@ -27,23 +27,23 @@ #include -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 diff --git a/include/sctest/Checks.h b/include/sctest/Checks.h new file mode 100755 index 0000000..d87d323 --- /dev/null +++ b/include/sctest/Checks.h @@ -0,0 +1,53 @@ +/// +/// \file Checks.h +/// \author K. Isom +/// \date 2017-06-05 +/// \brief Provides a number of utility macros for testing. +/// +/// Copyright 2017 K. Isom +/// +/// 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 + + +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 diff --git a/include/sctest/debug.h b/include/sctest/Debug.h similarity index 100% rename from include/sctest/debug.h rename to include/sctest/Debug.h diff --git a/include/scsl/Exceptions.h b/include/sctest/Exceptions.h similarity index 96% rename from include/scsl/Exceptions.h rename to include/sctest/Exceptions.h index c4bab16..6fe2561 100644 --- a/include/scsl/Exceptions.h +++ b/include/sctest/Exceptions.h @@ -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 -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 diff --git a/include/sctest/Report.h b/include/sctest/Report.h index a053490..91e6d2f 100755 --- a/include/sctest/Report.h +++ b/include/sctest/Report.h @@ -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 diff --git a/include/sctest/SimpleSuite.h b/include/sctest/SimpleSuite.h index e37c96d..7e7399c 100755 --- a/include/sctest/SimpleSuite.h +++ b/include/sctest/SimpleSuite.h @@ -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. diff --git a/include/sctest/checks.h b/include/sctest/checks.h deleted file mode 100755 index 827603d..0000000 --- a/include/sctest/checks.h +++ /dev/null @@ -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 -// -// 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 - - -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 diff --git a/src/scmp/Math.cc b/src/scmp/Math.cc index 9d04ac3..3f325fb 100644 --- a/src/scmp/Math.cc +++ b/src/scmp/Math.cc @@ -144,5 +144,5 @@ DefaultEpsilon(float &epsilon) } -} // namespace math +} // namespace scmp diff --git a/src/scmp/Orientation.cc b/src/scmp/Orientation.cc index 26f3523..4afe9b4 100644 --- a/src/scmp/Orientation.cc +++ b/src/scmp/Orientation.cc @@ -37,4 +37,4 @@ Heading3d(Vector3d vec) } // namespace geom -} // namespace math +} // namespace scmp diff --git a/src/scmp/Quaternion.cc b/src/scmp/Quaternion.cc index e9fe7aa..d99ed51 100644 --- a/src/scmp/Quaternion.cc +++ b/src/scmp/Quaternion.cc @@ -88,4 +88,4 @@ Quaternion_SelfTest() } // namespace geom -} // namespace math +} // namespace scmp diff --git a/src/test/Assert.cc b/src/test/Assert.cc index f7980c6..501b690 100644 --- a/src/test/Assert.cc +++ b/src/test/Assert.cc @@ -20,7 +20,7 @@ /// PERFORMANCE OF THIS SOFTWARE. /// -#include +#include "sctest/Exceptions.h" #include #include @@ -28,10 +28,10 @@ #include -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 diff --git a/src/sl/Exceptions.cc b/src/test/Exceptions.cc similarity index 94% rename from src/sl/Exceptions.cc rename to src/test/Exceptions.cc index 637fd6b..dd347c9 100644 --- a/src/sl/Exceptions.cc +++ b/src/test/Exceptions.cc @@ -20,10 +20,10 @@ /// PERFORMANCE OF THIS SOFTWARE. /// -#include +#include -namespace scsl { +namespace sctest { AssertionFailed::AssertionFailed(std::string message) : msg(message) {} @@ -36,4 +36,4 @@ AssertionFailed::what() const throw() } -} \ No newline at end of file +} // namespace sctest \ No newline at end of file diff --git a/test/coord2d.cc b/test/coord2d.cc new file mode 100755 index 0000000..d7df339 --- /dev/null +++ b/test/coord2d.cc @@ -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 +// +// 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 +#include + +#include +#include +#include +#include + +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 vertices { + Polar2D(2, 0), + Polar2D(1.41421, 2.35619), + Polar2D(1.41421, -2.35619) + }; + + // expected coordinates with no rotation + std::vector 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 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 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; + } +} diff --git a/test/dictionary.cc b/test/dictionary.cc index 4ba4c6b..983ad16 100644 --- a/test/dictionary.cc +++ b/test/dictionary.cc @@ -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"; diff --git a/test/flag.cc b/test/flag.cc index 446e568..fc71857 100644 --- a/test/flag.cc +++ b/test/flag.cc @@ -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); diff --git a/test/madgwick.cc b/test/madgwick.cc new file mode 100644 index 0000000..9d10be1 --- /dev/null +++ b/test/madgwick.cc @@ -0,0 +1,63 @@ +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +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; + } +} diff --git a/test/orientation.cc b/test/orientation.cc new file mode 100644 index 0000000..6cab0d2 --- /dev/null +++ b/test/orientation.cc @@ -0,0 +1,98 @@ +#include +#include + +#include +#include +#include + +using namespace std; +using namespace scmp; +using namespace sctest; + + +static bool +UnitConversions_RadiansToDegreesF() +{ + for (int i = 0; i < 360; i++) { + auto deg = static_cast(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(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; + } +} diff --git a/test/quaternion.cc b/test/quaternion.cc new file mode 100644 index 0000000..aece52f --- /dev/null +++ b/test/quaternion.cc @@ -0,0 +1,477 @@ +#include +#include + +#include + +#include +#include + + +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; + } +} diff --git a/test/stringutil.cc b/test/stringutil.cc index 47b0237..eedd151 100644 --- a/test/stringutil.cc +++ b/test/stringutil.cc @@ -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 +// +// 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 +#include + +#include +#include +#include + + +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; + } +}