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>
120 lines
2.3 KiB
C++
120 lines
2.3 KiB
C++
#include <optional>
|
|
#include <cmath>
|
|
#include "wrmath/geom/vector.h"
|
|
#include "wrmath/geom/orientation.h"
|
|
|
|
|
|
namespace wr {
|
|
namespace geom {
|
|
|
|
|
|
float
|
|
Heading2f(Vector2f vec)
|
|
{
|
|
return vec.angle(Basis2f[Basis_x]);
|
|
}
|
|
|
|
|
|
float
|
|
Heading3f(Vector3f vec)
|
|
{
|
|
Vector2f vec2f {vec[0], vec[1]};
|
|
return Heading2f(vec2f);
|
|
}
|
|
|
|
|
|
double
|
|
Heading2d(Vector2d vec)
|
|
{
|
|
return vec.angle(Basis2d[Basis_x]);
|
|
}
|
|
|
|
|
|
double
|
|
Heading3d(Vector3d vec)
|
|
{
|
|
Vector2d vec2d {vec[0], vec[1]};
|
|
return Heading2d(vec2d);
|
|
}
|
|
|
|
|
|
std::optional<double>
|
|
RBearing3d(Vector3d origin, Vector3d point)
|
|
{
|
|
if (origin.isZero() || point.isZero())
|
|
return std::nullopt;
|
|
|
|
Vector3d u_origin = origin.unitVector();
|
|
Vector3d u_point = point.unitVector();
|
|
|
|
double theta_h = std::atan2(u_origin[1], u_origin[0]);
|
|
double theta_d = std::atan2(u_point[1], u_point[0]);
|
|
double diff = theta_d - theta_h;
|
|
if (diff < 0.0) diff += 2.0 * M_PI;
|
|
return diff;
|
|
}
|
|
|
|
std::optional<float>
|
|
RBearing3f(Vector3f origin, Vector3f point)
|
|
{
|
|
if (origin.isZero() || point.isZero())
|
|
return std::nullopt;
|
|
|
|
Vector3f u_origin = origin.unitVector();
|
|
Vector3f u_point = point.unitVector();
|
|
|
|
float theta_h = std::atan2(u_origin[1], u_origin[0]);
|
|
float theta_d = std::atan2(u_point[1], u_point[0]);
|
|
float diff = theta_d - theta_h;
|
|
if (diff < 0.0f) diff += 2.0f * (float)M_PI;
|
|
return diff;
|
|
}
|
|
|
|
std::optional<double>
|
|
ABearing3d(Vector3d point)
|
|
{
|
|
if (point.isZero()) return std::nullopt;
|
|
Vector3d unit = point.unitVector();
|
|
double theta = std::atan2(unit[0], unit[1]);
|
|
if (theta < 0.0) theta += 2.0 * M_PI;
|
|
return theta;
|
|
}
|
|
|
|
std::optional<float>
|
|
ABearing3f(Vector3f point)
|
|
{
|
|
if (point.isZero()) return std::nullopt;
|
|
Vector3f unit = point.unitVector();
|
|
float theta = std::atan2(unit[0], unit[1]);
|
|
if (theta < 0.0f) theta += 2.0f * (float)M_PI;
|
|
return theta;
|
|
}
|
|
|
|
std::optional<double>
|
|
CompassHeading3d(Vector3d v)
|
|
{
|
|
Vector2d v2 {v[0], v[1]};
|
|
if (v2.isZero()) return std::nullopt;
|
|
Vector2d unit = v2.unitVector();
|
|
Vector2d y_basis {0.0, 1.0};
|
|
double theta = unit.angle2(y_basis);
|
|
if (theta < 0.0) theta += 2.0 * M_PI;
|
|
return theta;
|
|
}
|
|
|
|
std::optional<float>
|
|
CompassHeading3f(Vector3f v)
|
|
{
|
|
Vector2f v2 {v[0], v[1]};
|
|
if (v2.isZero()) return std::nullopt;
|
|
Vector2f unit = v2.unitVector();
|
|
Vector2f y_basis {0.0f, 1.0f};
|
|
float theta = unit.angle2(y_basis);
|
|
if (theta < 0.0f) theta += 2.0f * (float)M_PI;
|
|
return theta;
|
|
}
|
|
|
|
|
|
} // namespace geom
|
|
} // namespace wr
|