Kyle Isom
c47c91f418
Brings the C++ library in line with the Rust wrmath crate in astro-rs, which extends this library with additional geometry and estimation primitives. All changes generated with AI assistance (Claude Fable 5). New headers: - wrmath/geom/matrix.h: Matrix<T,M,N> with rank, det, inv, transpose, and matrix-vector/matrix-matrix multiplication - wrmath/geom/coord2d.h: Polar<T> 2D polar coordinates with navigation convention (clockwise-positive heading) - wrmath/geom/coord3d.h: Spherical<T> 3D spherical coordinates with yaw/pitch, slerp, great-circle path interpolation, and quaternion direction - wrmath/estim/imu.h: IMU<T> for 6-DoF and 9-DoF (MARG) sensor fusion Extensions to existing headers: - math.h: Epsilon3/6/Max constants; AbsTolerance (NaN/inf-safe), AbsError, RotateRadians, Circumference - vector.h: zero(), withEpsilon(), asArray(), fromArray/Eps(), map(), isNaN(), angle2() (signed), euclidist(), projectLower/Tail<M>(), x()/y()/z() accessors; fixed isParallel() to use unit-vector equality (matches Rust fix for macOS/arm64 acos domain issue) - quaternion.h: lerp(), slerp() methods; jacobian() returning Matrix<T,3,4>; direction() - filter/madgwick.h: beta gain field, setDeltaT(), setGain(), direction(), updateFrame2(), updateAngularOrientation2() - orientation.h/.cc: RBearing3d/f, ABearing3d/f, CompassHeading3d/f Infrastructure: - C++ standard bumped to C++17 (required for std::optional) - CMake include path fixed so source-relative includes work - Umbrella headers (geom.h, filter.h) updated Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
66 lines
1.2 KiB
C++
66 lines
1.2 KiB
C++
#include <gtest/gtest.h>
|
|
#include "wrmath/math.h"
|
|
#include "wrmath/geom/vector.h"
|
|
#include "wrmath/geom/orientation.h"
|
|
|
|
using namespace std;
|
|
using namespace wr;
|
|
|
|
|
|
TEST(UnitConversions, RadiansToDegreesF)
|
|
{
|
|
for (int i = 0; i < 360; i++) {
|
|
auto deg = static_cast<float>(i);
|
|
EXPECT_FLOAT_EQ(math::RadiansToDegreesF(math::DegreesToRadiansF(deg)), deg);
|
|
}
|
|
}
|
|
|
|
|
|
TEST(UnitConversions, RadiansToDegreesD)
|
|
{
|
|
for (int i = 0; i < 360; i++) {
|
|
auto deg = static_cast<double>(i);
|
|
EXPECT_FLOAT_EQ(math::RadiansToDegreesD(math::DegreesToRadiansD(deg)), deg);
|
|
}
|
|
}
|
|
|
|
|
|
TEST(Orientation2f, Heading)
|
|
{
|
|
geom::Vector2f a {2.0, 2.0};
|
|
|
|
EXPECT_FLOAT_EQ(geom::Heading2f(a), math::DegreesToRadiansF(45));
|
|
}
|
|
|
|
|
|
TEST(Orientation3f, Heading)
|
|
{
|
|
geom::Vector3f a {2.0, 2.0, 2.0};
|
|
|
|
EXPECT_FLOAT_EQ(geom::Heading3f(a), math::DegreesToRadiansF(45));
|
|
}
|
|
|
|
|
|
TEST(Orientation2d, Heading)
|
|
{
|
|
geom::Vector2d a {2.0, 2.0};
|
|
|
|
EXPECT_NEAR(geom::Heading2d(a), math::DegreesToRadiansF(45), 0.000001);
|
|
}
|
|
|
|
|
|
TEST(Orientation3d, Heading)
|
|
{
|
|
geom::Vector3d a {2.0, 2.0, 2.0};
|
|
|
|
EXPECT_NEAR(geom::Heading3d(a), math::DegreesToRadiansF(45), 0.000001);
|
|
}
|
|
|
|
|
|
int
|
|
main(int argc, char **argv)
|
|
{
|
|
::testing::InitGoogleTest(&argc, argv);
|
|
return RUN_ALL_TESTS();
|
|
}
|