sc/scsl
sc
/
scsl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
CMakeLists.txt
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)

2
cmake/docs.cmake
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)

2
include/scmp/Math.h
View File

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

158
include/scmp/filter/Madgwick.h Normal file
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

12
include/scmp/geom/Orientation.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

6
include/scmp/geom/Quaternion.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);

4
include/scmp/geom/Vector.h
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

2
include/scsl/Arena.h
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)

16
include/sctest/Assert.h
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

0
include/sctest/debug.h → include/sctest/Debug.h
View File

7
include/scsl/Exceptions.h → include/sctest/Exceptions.h
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

4
include/sctest/Report.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>

4
include/sctest/SimpleSuite.h
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.

50
include/sctest/checks.h
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

2
src/scmp/Math.cc
View File

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

2
src/scmp/Orientation.cc
View File

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

2
src/scmp/Quaternion.cc
View File

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

10
src/test/Assert.cc
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

6
src/sl/Exceptions.cc → src/test/Exceptions.cc
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;
}
}

37
test/dictionary.cc
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,
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";

2
test/flag.cc
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;
}
}

22
test/stringutil.cc
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,12 +119,12 @@ TestWrapping()
};
auto wrapped = U::S::WrapText(testLine, 16);
TestAssert(wrapped.size() == expected.size(),
U::S::VectorToString(wrapped) + " != " + U::S::VectorToString(expected));
sctest::Assert(wrapped.size() == expected.size(),
U::S::VectorToString(wrapped) + " != " + U::S::VectorToString(expected));
for (size_t i = 0; i < wrapped.size(); i++) {
TestAssert(wrapped[i] == expected[i],
"\"" + wrapped[i] + "\" != \"" + expected[i] + "\"");
sctest::Assert(wrapped[i] == expected[i],
"\"" + wrapped[i] + "\" != \"" + expected[i] + "\"");
}
U::S::WriteTabIndented(std::cout, wrapped, 4, true);

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