Extend wrmath to match Rust library in astro-rs
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>
This commit is contained in:
@@ -9,7 +9,7 @@ cmake_policy(SET CMP0048 NEW)
|
||||
## CONFIG
|
||||
|
||||
project(wrmath VERSION 0.0.1 LANGUAGES CXX)
|
||||
set(CMAKE_CXX_STANDARD 11)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH})
|
||||
|
||||
@@ -29,7 +29,7 @@ message(STATUS, "Code coverage enabled.")
|
||||
endif()
|
||||
|
||||
|
||||
include_directories(wrmath)
|
||||
include_directories(${PROJECT_SOURCE_DIR})
|
||||
|
||||
file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS wrmath/**.h)
|
||||
file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES wrmath/*.cc)
|
||||
|
||||
@@ -1,10 +1,10 @@
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
#include <gtest/gtest.h>
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <wrmath/geom/quaternion.h>
|
||||
#include <wrmath/math.h>
|
||||
#include <wrmath/filter/madgwick.h>
|
||||
#include "wrmath/geom/vector.h"
|
||||
#include "wrmath/geom/quaternion.h"
|
||||
#include "wrmath/math.h"
|
||||
#include "wrmath/filter/madgwick.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wr;
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <wrmath/math.h>
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <wrmath/geom/orientation.h>
|
||||
#include "wrmath/math.h"
|
||||
#include "wrmath/geom/vector.h"
|
||||
#include "wrmath/geom/orientation.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wr;
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
#include <gtest/gtest.h>
|
||||
#include <wrmath/geom/quaternion.h>
|
||||
#include "wrmath/geom/quaternion.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wr;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#include <sstream>
|
||||
#include <gtest/gtest.h>
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include "wrmath/geom/vector.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace wr;
|
||||
|
||||
129
wrmath/estim/imu.h
Normal file
129
wrmath/estim/imu.h
Normal file
@@ -0,0 +1,129 @@
|
||||
/// imu.h - IMU sensor packaging for 6-DoF and 9-DoF (MARG) IMUs.
|
||||
///
|
||||
/// Terrestrial frame of reference:
|
||||
/// x-axis: north
|
||||
/// y-axis: west
|
||||
/// z-axis: up
|
||||
#ifndef __WRMATH_ESTIM_IMU_H
|
||||
#define __WRMATH_ESTIM_IMU_H
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <optional>
|
||||
#include "wrmath/math.h"
|
||||
#include "wrmath/geom/vector.h"
|
||||
#include "wrmath/geom/matrix.h"
|
||||
#include "wrmath/geom/quaternion.h"
|
||||
#include "wrmath/geom/orientation.h"
|
||||
#include "wrmath/geom/coord3d.h"
|
||||
|
||||
namespace wr {
|
||||
namespace estim {
|
||||
|
||||
/// @brief IMU packages sensor readings from inertial measurement units.
|
||||
///
|
||||
/// Models both 6-DoF (gyroscope + accelerometer) and 9-DoF MARG
|
||||
/// (gyroscope + accelerometer + magnetometer) IMUs.
|
||||
template <typename T>
|
||||
class IMU {
|
||||
public:
|
||||
geom::Vector<T, 3> gyro;
|
||||
geom::Vector<T, 3> acc;
|
||||
std::optional<geom::Vector<T, 3>> mag;
|
||||
T epsilon;
|
||||
|
||||
/// Construct a zero-valued IMU.
|
||||
explicit IMU(T eps)
|
||||
: gyro(geom::Vector<T, 3>::zero())
|
||||
, acc(geom::Vector<T, 3>::zero())
|
||||
, mag(std::nullopt)
|
||||
, epsilon(eps)
|
||||
{}
|
||||
|
||||
/// Construct from a 3x3 matrix: rows are gyro, acc, mag.
|
||||
/// If the mag row is all-NaN, it is treated as absent.
|
||||
static IMU
|
||||
fromMat(const geom::Matrix<T, 3, 3>& mat)
|
||||
{
|
||||
IMU imu(mat.epsilon);
|
||||
imu.gyro = mat.row(0);
|
||||
imu.acc = mat.row(1);
|
||||
geom::Vector<T, 3> mag_row = mat.row(2);
|
||||
if (!mag_row.isNaN()) {
|
||||
imu.mag = mag_row;
|
||||
}
|
||||
return imu;
|
||||
}
|
||||
|
||||
void
|
||||
updateAcceleration(const geom::Vector<T, 3>& frame)
|
||||
{
|
||||
acc = frame.withEpsilon(epsilon);
|
||||
}
|
||||
|
||||
void
|
||||
updateMagnetometer(const std::optional<geom::Vector<T, 3>>& frame)
|
||||
{
|
||||
if (frame.has_value()) {
|
||||
mag = frame->withEpsilon(epsilon);
|
||||
} else {
|
||||
mag = std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
updateRotation(const geom::Vector<T, 3>& frame)
|
||||
{
|
||||
gyro = frame.withEpsilon(epsilon);
|
||||
}
|
||||
|
||||
/// Return the compass heading from the magnetometer, if available.
|
||||
std::optional<T>
|
||||
heading() const
|
||||
{
|
||||
if (!mag.has_value()) return std::nullopt;
|
||||
return (T)geom::Heading3d(geom::Vector3d{(*mag)[0], (*mag)[1], (*mag)[2]});
|
||||
}
|
||||
|
||||
/// Return the spherical orientation from the magnetometer.
|
||||
geom::Spherical<T>
|
||||
orientation() const
|
||||
{
|
||||
if (!mag.has_value()) {
|
||||
return geom::Spherical<T>((T)0.0, (T)0.0, (T)0.0);
|
||||
}
|
||||
return geom::Spherical<T>::fromPoint(*mag);
|
||||
}
|
||||
|
||||
/// Return the degrees of freedom: 6 for gyro+acc, 9 with mag.
|
||||
size_t
|
||||
dof() const
|
||||
{
|
||||
size_t d = gyro.asArray().size() + acc.asArray().size();
|
||||
if (mag.has_value()) d += mag->asArray().size();
|
||||
return d;
|
||||
}
|
||||
|
||||
/// Return the IMU data as a 3x3 matrix (rows: gyro, acc, mag).
|
||||
/// The mag row is all-NaN if no magnetometer.
|
||||
geom::Matrix<T, 3, 3>
|
||||
mat() const
|
||||
{
|
||||
geom::Matrix<T, 3, 3> result;
|
||||
result.setEpsilon(epsilon);
|
||||
for (size_t j = 0; j < 3; j++) {
|
||||
result.m[0][j] = gyro[j];
|
||||
result.m[1][j] = acc[j];
|
||||
result.m[2][j] = mag.has_value() ? (*mag)[j] : std::numeric_limits<T>::quiet_NaN();
|
||||
}
|
||||
return result;
|
||||
}
|
||||
};
|
||||
|
||||
typedef IMU<float> IMUf;
|
||||
typedef IMU<double> IMUd;
|
||||
|
||||
} // namespace estim
|
||||
} // namespace wr
|
||||
|
||||
#endif // __WRMATH_ESTIM_IMU_H
|
||||
@@ -1,8 +1,7 @@
|
||||
#ifndef __WRMATH_FILTER_H
|
||||
#define __WRMATH_FILTER_H
|
||||
|
||||
|
||||
#include <wrmath/filter/madgwick.h>
|
||||
#include <wrmath/estim/imu.h>
|
||||
|
||||
|
||||
#endif __WRMATH_FILTER_H
|
||||
#endif // __WRMATH_FILTER_H
|
||||
|
||||
@@ -6,8 +6,9 @@
|
||||
#define __WRMATH_FILTER_MADGWICK_H
|
||||
|
||||
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <wrmath/geom/quaternion.h>
|
||||
#include "wrmath/geom/vector.h"
|
||||
#include "wrmath/geom/quaternion.h"
|
||||
#include "wrmath/estim/imu.h"
|
||||
|
||||
|
||||
/// wr contains the wntrmute robotics code.
|
||||
@@ -31,14 +32,14 @@ template <typename T>
|
||||
class Madgwick {
|
||||
public:
|
||||
/// The Madgwick filter is initialised with an identity quaternion.
|
||||
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {};
|
||||
Madgwick() : deltaT(0.0), beta(0.05), 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(geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame()
|
||||
Madgwick(geom::Vector<T, 3> sf) : deltaT(0.0), beta(0.05), previousSensorFrame()
|
||||
{
|
||||
if (!sf.isZero()) {
|
||||
sensorFrame = geom::quaternion(sf, 0.0);
|
||||
@@ -50,7 +51,7 @@ public:
|
||||
///
|
||||
/// \param sf A quaternion representing the current orientation.
|
||||
Madgwick(geom::Quaternion<T> sf) :
|
||||
deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {};
|
||||
deltaT(0.0), beta(0.05), previousSensorFrame(), sensorFrame(sf) {};
|
||||
|
||||
|
||||
/// Return the current orientation as measured by the filter.
|
||||
@@ -112,8 +113,47 @@ public:
|
||||
return this->sensorFrame.euler();
|
||||
}
|
||||
|
||||
/// Set the stored time delta (for use with update variants that don't take delta).
|
||||
void
|
||||
setDeltaT(T delta)
|
||||
{
|
||||
this->deltaT = delta;
|
||||
}
|
||||
|
||||
/// Return a new filter with an adjusted gain (beta).
|
||||
Madgwick
|
||||
setGain(T gain) const
|
||||
{
|
||||
Madgwick result = *this;
|
||||
result.beta = gain;
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Return the direction vector of the current orientation.
|
||||
geom::Vector<T, 3>
|
||||
direction() const
|
||||
{
|
||||
return this->sensorFrame.direction();
|
||||
}
|
||||
|
||||
/// Update using stored deltaT (must have been set via setDeltaT or a prior update).
|
||||
void
|
||||
updateFrame2(const geom::Quaternion<T> &sf)
|
||||
{
|
||||
assert(!wr::math::WithinTolerance(this->deltaT, (T)0.0, (T)0.001));
|
||||
updateFrame(sf, this->deltaT);
|
||||
}
|
||||
|
||||
/// Update angular orientation using stored deltaT.
|
||||
void
|
||||
updateAngularOrientation2(const geom::Vector<T, 3> &gyro)
|
||||
{
|
||||
updateAngularOrientation(gyro, this->deltaT);
|
||||
}
|
||||
|
||||
private:
|
||||
T deltaT;
|
||||
T beta;
|
||||
geom::Quaternion<T> previousSensorFrame;
|
||||
geom::Quaternion<T> sensorFrame;
|
||||
};
|
||||
|
||||
@@ -2,6 +2,10 @@
|
||||
#define __WRMATH_GEOM_H
|
||||
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <wrmath/geom/matrix.h>
|
||||
#include <wrmath/geom/quaternion.h>
|
||||
#include <wrmath/geom/orientation.h>
|
||||
#include <wrmath/geom/coord2d.h>
|
||||
#include <wrmath/geom/coord3d.h>
|
||||
|
||||
#endif // __WRMATH_GEOM_H
|
||||
|
||||
155
wrmath/geom/coord2d.h
Normal file
155
wrmath/geom/coord2d.h
Normal file
@@ -0,0 +1,155 @@
|
||||
/// coord2d.h - Polar coordinate system for 2D navigation.
|
||||
///
|
||||
/// Convention: clockwise-positive rotation, +x axis is north.
|
||||
#ifndef __WRMATH_GEOM_COORD2D_H
|
||||
#define __WRMATH_GEOM_COORD2D_H
|
||||
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
#include <ostream>
|
||||
#include "wrmath/math.h"
|
||||
#include "wrmath/geom/vector.h"
|
||||
|
||||
namespace wr {
|
||||
namespace geom {
|
||||
|
||||
/// CLOCKWISE_POSITIVE controls the rotation convention.
|
||||
/// When true (default), positive angles are clockwise from +x (navigation convention).
|
||||
static constexpr bool POLAR_CLOCKWISE_POSITIVE = true;
|
||||
|
||||
/// @brief Polar represents a 2D coordinate in polar form (r, theta).
|
||||
///
|
||||
/// The coordinate system uses clockwise-positive rotation, where
|
||||
/// theta=0 points along the +x axis (north). This matches standard
|
||||
/// navigation conventions where compass headings increase clockwise.
|
||||
///
|
||||
/// Conversion formulas (with navigation convention):
|
||||
/// x = r * cos(-theta)
|
||||
/// y = r * sin(-theta)
|
||||
template <typename T>
|
||||
class Polar {
|
||||
public:
|
||||
T r; ///< Radial distance from origin.
|
||||
T theta; ///< Azimuth angle from +x axis.
|
||||
T epsilon;
|
||||
|
||||
/// Default constructor creates the origin.
|
||||
Polar() : r((T)0.0), theta((T)0.0)
|
||||
{
|
||||
wr::math::DefaultEpsilon(this->epsilon);
|
||||
}
|
||||
|
||||
/// Construct from radius and angle.
|
||||
Polar(T r, T theta) : r(r), theta(theta)
|
||||
{
|
||||
wr::math::DefaultEpsilon(this->epsilon);
|
||||
}
|
||||
|
||||
/// Construct with custom epsilon.
|
||||
Polar(T r, T theta, T eps) : r(r), theta(theta), epsilon(eps) {}
|
||||
|
||||
void setEpsilon(T eps) { this->epsilon = eps; }
|
||||
|
||||
/// Return a copy with a different epsilon.
|
||||
Polar withEpsilon(T eps) const
|
||||
{
|
||||
return Polar(r, theta, eps);
|
||||
}
|
||||
|
||||
/// Convert to a Cartesian point (Vector2).
|
||||
Vector<T, 2>
|
||||
toPoint() const
|
||||
{
|
||||
T theta_internal = POLAR_CLOCKWISE_POSITIVE ? -theta : theta;
|
||||
return Vector<T, 2>{r * std::cos(theta_internal), r * std::sin(theta_internal)};
|
||||
}
|
||||
|
||||
/// Convert a Cartesian point to polar coordinates.
|
||||
static Polar
|
||||
fromPoint(const Vector<T, 2>& point)
|
||||
{
|
||||
T x = point[0], y = point[1];
|
||||
T theta_internal = std::atan2(y, x);
|
||||
T theta_nav = POLAR_CLOCKWISE_POSITIVE ? -theta_internal : theta_internal;
|
||||
T r = std::sqrt(x*x + y*y);
|
||||
Polar p(r, theta_nav);
|
||||
p.epsilon = (T)wr::math::Epsilon6;
|
||||
return p;
|
||||
}
|
||||
|
||||
/// Convert x,y to polar coordinates.
|
||||
static Polar
|
||||
fromXY(T x, T y)
|
||||
{
|
||||
T theta_internal = std::atan2(y, x);
|
||||
T theta_nav = POLAR_CLOCKWISE_POSITIVE ? -theta_internal : theta_internal;
|
||||
Polar p(std::sqrt(x*x + y*y), theta_nav);
|
||||
p.epsilon = (T)wr::math::Epsilon6;
|
||||
return p;
|
||||
}
|
||||
|
||||
/// Returns the current heading (azimuth angle theta).
|
||||
T heading() const { return this->theta; }
|
||||
|
||||
/// Rotate heading by delta_theta (positive = clockwise).
|
||||
Polar
|
||||
rotate(T delta_theta) const
|
||||
{
|
||||
return Polar(r, wr::math::RotateRadians(theta, delta_theta), epsilon);
|
||||
}
|
||||
|
||||
/// Rotate this polar coordinate around a given Cartesian point.
|
||||
Vector<T, 2>
|
||||
rotateAround(const Vector<T, 2>& point, T delta_theta) const
|
||||
{
|
||||
return rotate(delta_theta).toPoint() + point;
|
||||
}
|
||||
|
||||
/// Compute the relative bearing from this position to a target point.
|
||||
/// Returns a Polar where r=distance and theta=relative bearing.
|
||||
Polar
|
||||
bearingTo(const Vector<T, 2>& point) const
|
||||
{
|
||||
if (point.isZero()) {
|
||||
return Polar((T)0.0, (T)0.0, this->epsilon);
|
||||
}
|
||||
Polar absolute = fromPoint(point);
|
||||
return Polar(absolute.r, wr::math::RotateRadians(absolute.theta, -this->theta), this->epsilon);
|
||||
}
|
||||
|
||||
bool
|
||||
operator==(const Polar<T>& other) const
|
||||
{
|
||||
return wr::math::AbsTolerance(r, other.r, this->epsilon) &&
|
||||
wr::math::AbsTolerance(theta, other.theta, this->epsilon);
|
||||
}
|
||||
|
||||
bool
|
||||
operator!=(const Polar<T>& other) const
|
||||
{
|
||||
return !(*this == other);
|
||||
}
|
||||
|
||||
friend std::ostream&
|
||||
operator<<(std::ostream& outs, const Polar<T>& p)
|
||||
{
|
||||
outs << "(" << p.r << ", " << (p.theta * 180.0 / M_PI) << "\xc2\xb0)";
|
||||
return outs;
|
||||
}
|
||||
};
|
||||
|
||||
typedef Polar<float> Polarf;
|
||||
typedef Polar<double> Polard;
|
||||
|
||||
/// Rotate a Cartesian point around the origin by angle theta.
|
||||
template <typename T>
|
||||
Vector<T, 2>
|
||||
RotatePoint2D(const Vector<T, 2>& point, T theta)
|
||||
{
|
||||
return Polar<T>::fromPoint(point).rotate(theta).toPoint();
|
||||
}
|
||||
|
||||
} // namespace geom
|
||||
} // namespace wr
|
||||
|
||||
#endif // __WRMATH_GEOM_COORD2D_H
|
||||
224
wrmath/geom/coord3d.h
Normal file
224
wrmath/geom/coord3d.h
Normal file
@@ -0,0 +1,224 @@
|
||||
/// coord3d.h - Spherical coordinate system for 3D navigation.
|
||||
///
|
||||
/// Convention: clockwise-positive yaw (azimuth), pitch positive upward.
|
||||
#ifndef __WRMATH_GEOM_COORD3D_H
|
||||
#define __WRMATH_GEOM_COORD3D_H
|
||||
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
#include <ostream>
|
||||
#include "wrmath/math.h"
|
||||
#include "wrmath/geom/vector.h"
|
||||
#include "wrmath/geom/quaternion.h"
|
||||
|
||||
namespace wr {
|
||||
namespace geom {
|
||||
|
||||
static constexpr bool SPHERICAL_CLOCKWISE_POSITIVE = true;
|
||||
|
||||
/// @brief Spherical represents a 3D coordinate in spherical form (r, theta, phi).
|
||||
///
|
||||
/// - r: radial distance from origin
|
||||
/// - theta: azimuth angle from +x axis (north), clockwise-positive
|
||||
/// - phi: elevation angle from xy-plane, positive upward
|
||||
template <typename T>
|
||||
class Spherical {
|
||||
public:
|
||||
T r; ///< Radial distance.
|
||||
T theta; ///< Azimuth angle (yaw), clockwise from +x.
|
||||
T phi; ///< Elevation angle (pitch), positive = nose up.
|
||||
T epsilon;
|
||||
|
||||
Spherical() : r((T)0.0), theta((T)0.0), phi((T)0.0)
|
||||
{
|
||||
wr::math::DefaultEpsilon(this->epsilon);
|
||||
}
|
||||
|
||||
Spherical(T r, T theta, T phi) : r(r), theta(theta), phi(phi)
|
||||
{
|
||||
wr::math::DefaultEpsilon(this->epsilon);
|
||||
}
|
||||
|
||||
Spherical(T r, T theta, T phi, T eps)
|
||||
: r(r), theta(theta), phi(phi), epsilon(eps) {}
|
||||
|
||||
void setEpsilon(T eps) { this->epsilon = eps; }
|
||||
|
||||
Spherical withEpsilon(T eps) const
|
||||
{
|
||||
return Spherical(r, theta, phi, eps);
|
||||
}
|
||||
|
||||
/// Convert to a Cartesian point.
|
||||
Vector<T, 3>
|
||||
toPoint() const
|
||||
{
|
||||
T theta_internal = SPHERICAL_CLOCKWISE_POSITIVE ? -theta : theta;
|
||||
T xy = r * std::cos(phi);
|
||||
return Vector<T, 3>{
|
||||
xy * std::cos(theta_internal),
|
||||
xy * std::sin(theta_internal),
|
||||
r * std::sin(phi)
|
||||
};
|
||||
}
|
||||
|
||||
/// Compute a Spherical from a Cartesian point.
|
||||
static Spherical
|
||||
fromPoint(const Vector<T, 3>& point)
|
||||
{
|
||||
T r_val = std::sqrt(point[0]*point[0] + point[1]*point[1] + point[2]*point[2]);
|
||||
T theta_internal = std::atan2(point[1], point[0]);
|
||||
T theta_nav = SPHERICAL_CLOCKWISE_POSITIVE ? -theta_internal : theta_internal;
|
||||
T phi_val = (r_val < (T)wr::math::Epsilon6) ? (T)0.0 : std::asin(point[2] / r_val);
|
||||
Spherical s(r_val, theta_nav, phi_val);
|
||||
s.epsilon = (T)wr::math::Epsilon6;
|
||||
return s;
|
||||
}
|
||||
|
||||
/// Compute a Spherical from a point with custom epsilon.
|
||||
static Spherical
|
||||
fromPointEps(const Vector<T, 3>& point, T eps)
|
||||
{
|
||||
Spherical s = fromPoint(point);
|
||||
s.epsilon = eps;
|
||||
return s;
|
||||
}
|
||||
|
||||
/// Returns the current heading (azimuth angle theta).
|
||||
T heading() const { return this->theta; }
|
||||
|
||||
/// Yaw: rotate heading by delta_theta (positive = clockwise).
|
||||
Spherical
|
||||
yaw(T delta_theta) const
|
||||
{
|
||||
return Spherical(r, wr::math::RotateRadians(theta, delta_theta), phi, epsilon);
|
||||
}
|
||||
|
||||
/// Pitch: adjust elevation by delta_phi, clamped to [-pi/2, pi/2].
|
||||
Spherical
|
||||
pitch(T delta_phi) const
|
||||
{
|
||||
const T half_pi = static_cast<T>(M_PI) / (T)2.0;
|
||||
T new_phi = phi + delta_phi;
|
||||
if (new_phi > half_pi) new_phi = half_pi;
|
||||
if (new_phi < -half_pi) new_phi = -half_pi;
|
||||
return Spherical(r, theta, new_phi, epsilon);
|
||||
}
|
||||
|
||||
/// Apply an arbitrary rotation via a quaternion.
|
||||
Spherical
|
||||
rotate(const Quaternion<T>& q) const
|
||||
{
|
||||
Vector<T, 3> point = toPoint();
|
||||
Vector<T, 3> rotated = q.rotate(point);
|
||||
return fromPointEps(rotated, this->epsilon);
|
||||
}
|
||||
|
||||
/// Create a yaw (z-axis) rotation quaternion.
|
||||
static Quaternion<T>
|
||||
yawQuaternion(T delta_theta)
|
||||
{
|
||||
return quaternion(Vector<T, 3>{(T)0.0, (T)0.0, (T)1.0}, delta_theta);
|
||||
}
|
||||
|
||||
/// Create a pitch (y-axis) rotation quaternion.
|
||||
static Quaternion<T>
|
||||
pitchQuaternion(T delta_phi)
|
||||
{
|
||||
return quaternion(Vector<T, 3>{(T)0.0, (T)1.0, (T)0.0}, delta_phi);
|
||||
}
|
||||
|
||||
/// Compute the orientation as a quaternion (combined yaw + pitch).
|
||||
Quaternion<T>
|
||||
direction() const
|
||||
{
|
||||
Quaternion<T> q_theta = quaternion(Vector<T, 3>{(T)0.0, (T)0.0, (T)1.0}, theta);
|
||||
Quaternion<T> q_phi = quaternion(Vector<T, 3>{(T)0.0, (T)1.0, (T)0.0}, -phi);
|
||||
return q_theta * q_phi;
|
||||
}
|
||||
|
||||
/// Spherical linear interpolation between two orientations.
|
||||
Spherical
|
||||
slerp(const Spherical<T>& other, T t) const
|
||||
{
|
||||
if (t < (T)0.0) t = (T)0.0;
|
||||
if (t > (T)1.0) t = (T)1.0;
|
||||
Quaternion<T> q1 = this->direction();
|
||||
Quaternion<T> q2 = other.direction();
|
||||
Quaternion<T> interp = q1.slerp(q2, t);
|
||||
T r_interp = r + t * (other.r - r);
|
||||
Vector<T, 3> ref {(T)1.0, (T)0.0, (T)0.0};
|
||||
Vector<T, 3> dir = interp.rotate(ref);
|
||||
Spherical result = fromPoint(dir);
|
||||
result.r = r_interp;
|
||||
result.epsilon = std::min(epsilon, other.epsilon);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Compute an interpolated position along a great-circle path.
|
||||
Spherical
|
||||
path(const Spherical<T>& other, T t) const
|
||||
{
|
||||
if (t < (T)0.0) t = (T)0.0;
|
||||
if (t > (T)1.0) t = (T)1.0;
|
||||
Vector<T, 3> u_self = toPoint().unitVector();
|
||||
Vector<T, 3> u_other = other.toPoint().unitVector();
|
||||
Vector<T, 3> axis = u_self.cross(u_other).unitVector();
|
||||
T angle = std::acos(u_self * u_other);
|
||||
Quaternion<T> q = quaternion(axis, angle * t);
|
||||
Vector<T, 3> dir = q.rotate(u_self);
|
||||
T r_interp = r + t * (other.r - r);
|
||||
Spherical result = fromPoint(dir);
|
||||
result.r = r_interp;
|
||||
result.epsilon = std::min(epsilon, other.epsilon);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Compute the relative bearing from this position to a target point.
|
||||
Spherical
|
||||
bearingTo(const Vector<T, 3>& point) const
|
||||
{
|
||||
if (point.isZero()) {
|
||||
return Spherical((T)0.0, (T)0.0, (T)0.0, this->epsilon);
|
||||
}
|
||||
return fromPointEps(direction().conjugate().rotate(point), this->epsilon);
|
||||
}
|
||||
|
||||
/// True if radius is near zero.
|
||||
bool isZero() const
|
||||
{
|
||||
return wr::math::AbsTolerance(r, (T)0.0, this->epsilon);
|
||||
}
|
||||
|
||||
bool
|
||||
operator==(const Spherical<T>& other) const
|
||||
{
|
||||
T eps = std::min(this->epsilon, other.epsilon);
|
||||
return wr::math::AbsTolerance(r, other.r, eps) &&
|
||||
wr::math::AbsTolerance(theta, other.theta, eps) &&
|
||||
wr::math::AbsTolerance(phi, other.phi, eps);
|
||||
}
|
||||
|
||||
bool
|
||||
operator!=(const Spherical<T>& other) const
|
||||
{
|
||||
return !(*this == other);
|
||||
}
|
||||
|
||||
friend std::ostream&
|
||||
operator<<(std::ostream& outs, const Spherical<T>& s)
|
||||
{
|
||||
outs << "(" << s.r
|
||||
<< ", " << (s.theta * 180.0 / M_PI) << "\xc2\xb0"
|
||||
<< ", " << (s.phi * 180.0 / M_PI) << "\xc2\xb0)";
|
||||
return outs;
|
||||
}
|
||||
};
|
||||
|
||||
typedef Spherical<float> Sphericalf;
|
||||
typedef Spherical<double> Sphericald;
|
||||
|
||||
} // namespace geom
|
||||
} // namespace wr
|
||||
|
||||
#endif // __WRMATH_GEOM_COORD3D_H
|
||||
370
wrmath/geom/matrix.h
Normal file
370
wrmath/geom/matrix.h
Normal file
@@ -0,0 +1,370 @@
|
||||
/// matrix.h provides an implementation of matrices.
|
||||
#ifndef __WRMATH_GEOM_MATRIX_H
|
||||
#define __WRMATH_GEOM_MATRIX_H
|
||||
|
||||
#include <array>
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <optional>
|
||||
#include <ostream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
#include "wrmath/math.h"
|
||||
#include "wrmath/geom/vector.h"
|
||||
|
||||
namespace wr {
|
||||
namespace geom {
|
||||
|
||||
/// @brief Matrix provides a fixed-size MxN matrix.
|
||||
///
|
||||
/// Matrix is designed to interoperate with wr::geom::Vector.
|
||||
/// Row and column access return vectors; matrix-vector multiplication
|
||||
/// returns a vector.
|
||||
template <typename T, size_t M, size_t N>
|
||||
class Matrix {
|
||||
public:
|
||||
T m[M][N];
|
||||
T epsilon;
|
||||
|
||||
/// Default constructor creates a zero matrix.
|
||||
Matrix()
|
||||
{
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < N; j++)
|
||||
m[i][j] = (T)0.0;
|
||||
wr::math::DefaultEpsilon(this->epsilon);
|
||||
}
|
||||
|
||||
void setEpsilon(T eps) { this->epsilon = eps; }
|
||||
|
||||
/// Return element at (i, j).
|
||||
T at(size_t i, size_t j) const { return m[i][j]; }
|
||||
|
||||
/// Return row i as a vector.
|
||||
Vector<T, N>
|
||||
row(size_t i) const
|
||||
{
|
||||
std::array<T, N> arr;
|
||||
for (size_t j = 0; j < N; j++) arr[j] = m[i][j];
|
||||
return Vector<T, N>::fromArrayEps(arr, this->epsilon);
|
||||
}
|
||||
|
||||
/// Return column j as a vector.
|
||||
Vector<T, M>
|
||||
col(size_t j) const
|
||||
{
|
||||
std::array<T, M> arr;
|
||||
for (size_t i = 0; i < M; i++) arr[i] = m[i][j];
|
||||
return Vector<T, M>::fromArrayEps(arr, this->epsilon);
|
||||
}
|
||||
|
||||
/// Return the transpose of this matrix.
|
||||
Matrix<T, N, M>
|
||||
transpose() const
|
||||
{
|
||||
Matrix<T, N, M> result;
|
||||
result.setEpsilon(this->epsilon);
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < N; j++)
|
||||
result.m[j][i] = m[i][j];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Compute the rank of the matrix via Gaussian elimination.
|
||||
size_t
|
||||
rank() const
|
||||
{
|
||||
T tmp[M][N];
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < N; j++)
|
||||
tmp[i][j] = m[i][j];
|
||||
|
||||
size_t rank_val = 0;
|
||||
size_t row = 0;
|
||||
|
||||
for (size_t col = 0; col < N; col++) {
|
||||
if (row >= M) break;
|
||||
|
||||
size_t pivot = row;
|
||||
for (size_t i = row + 1; i < M; i++) {
|
||||
if (std::abs(tmp[i][col]) > std::abs(tmp[pivot][col]))
|
||||
pivot = i;
|
||||
}
|
||||
|
||||
if (std::abs(tmp[pivot][col]) < this->epsilon) continue;
|
||||
|
||||
if (pivot != row) {
|
||||
for (size_t j = 0; j < N; j++)
|
||||
std::swap(tmp[row][j], tmp[pivot][j]);
|
||||
}
|
||||
|
||||
for (size_t i = row + 1; i < M; i++) {
|
||||
T factor = tmp[i][col] / tmp[row][col];
|
||||
for (size_t j = col + 1; j < N; j++)
|
||||
tmp[i][j] -= factor * tmp[row][j];
|
||||
tmp[i][col] = (T)0.0;
|
||||
}
|
||||
|
||||
rank_val++;
|
||||
row++;
|
||||
}
|
||||
|
||||
return rank_val;
|
||||
}
|
||||
|
||||
/// Identity matrix (only valid for square matrices).
|
||||
static Matrix
|
||||
identity()
|
||||
{
|
||||
static_assert(M == N, "identity() requires a square matrix");
|
||||
Matrix result;
|
||||
for (size_t i = 0; i < M; i++) result.m[i][i] = (T)1.0;
|
||||
wr::math::DefaultEpsilon(result.epsilon);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Determinant (only valid for square matrices).
|
||||
T
|
||||
det() const
|
||||
{
|
||||
static_assert(M == N, "det() requires a square matrix");
|
||||
if (M == 0) return (T)1.0;
|
||||
if (M == 1) return m[0][0];
|
||||
if (M == 2) return m[0][0] * m[1][1] - m[0][1] * m[1][0];
|
||||
if (M == 3) {
|
||||
return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1])
|
||||
- m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0])
|
||||
+ m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]);
|
||||
}
|
||||
return gaussianEliminationDet();
|
||||
}
|
||||
|
||||
/// Inverse (only valid for square matrices). Returns nullopt if singular.
|
||||
std::optional<Matrix>
|
||||
inv() const
|
||||
{
|
||||
static_assert(M == N, "inv() requires a square matrix");
|
||||
if (wr::math::AbsTolerance(this->det(), (T)0.0, this->epsilon))
|
||||
return std::nullopt;
|
||||
if (this->rank() != M)
|
||||
return std::nullopt;
|
||||
|
||||
T aug[M][M];
|
||||
Matrix result = Matrix::identity();
|
||||
result.setEpsilon(this->epsilon);
|
||||
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < M; j++)
|
||||
aug[i][j] = m[i][j];
|
||||
|
||||
for (size_t col = 0; col < M; col++) {
|
||||
size_t pivot = col;
|
||||
for (size_t row_p = col + 1; row_p < M; row_p++) {
|
||||
if (std::abs(aug[row_p][col]) > std::abs(aug[pivot][col]))
|
||||
pivot = row_p;
|
||||
}
|
||||
if (pivot != col) {
|
||||
for (size_t j = 0; j < M; j++) {
|
||||
std::swap(aug[col][j], aug[pivot][j]);
|
||||
std::swap(result.m[col][j], result.m[pivot][j]);
|
||||
}
|
||||
}
|
||||
T diag = aug[col][col];
|
||||
for (size_t j = 0; j < M; j++) {
|
||||
aug[col][j] /= diag;
|
||||
result.m[col][j] /= diag;
|
||||
}
|
||||
for (size_t row_e = 0; row_e < M; row_e++) {
|
||||
if (row_e == col) continue;
|
||||
T factor = aug[row_e][col];
|
||||
for (size_t j = 0; j < M; j++) {
|
||||
aug[row_e][j] -= factor * aug[col][j];
|
||||
result.m[row_e][j] -= factor * result.m[col][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// In-place transpose (only valid for square matrices).
|
||||
void
|
||||
transposeInplace()
|
||||
{
|
||||
static_assert(M == N, "transposeInplace() requires a square matrix");
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = i + 1; j < M; j++)
|
||||
std::swap(m[i][j], m[j][i]);
|
||||
}
|
||||
|
||||
/// Scalar multiplication.
|
||||
Matrix
|
||||
operator*(T k) const
|
||||
{
|
||||
Matrix result;
|
||||
result.epsilon = this->epsilon;
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < N; j++)
|
||||
result.m[i][j] = m[i][j] * k;
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Scalar division.
|
||||
Matrix
|
||||
operator/(T k) const
|
||||
{
|
||||
Matrix result;
|
||||
result.epsilon = this->epsilon;
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < N; j++)
|
||||
result.m[i][j] = m[i][j] / k;
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Matrix addition.
|
||||
Matrix
|
||||
operator+(const Matrix<T, M, N> &other) const
|
||||
{
|
||||
Matrix result;
|
||||
result.epsilon = std::min(this->epsilon, other.epsilon);
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < N; j++)
|
||||
result.m[i][j] = m[i][j] + other.m[i][j];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Matrix-vector multiplication: returns Vector<T, M>.
|
||||
Vector<T, M>
|
||||
operator*(const Vector<T, N> &vec) const
|
||||
{
|
||||
std::array<T, M> result_arr;
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
T sum = (T)0.0;
|
||||
for (size_t j = 0; j < N; j++)
|
||||
sum += m[i][j] * vec[j];
|
||||
result_arr[i] = sum;
|
||||
}
|
||||
return Vector<T, M>::fromArrayEps(result_arr, this->epsilon);
|
||||
}
|
||||
|
||||
/// Matrix-matrix multiplication.
|
||||
template <size_t P>
|
||||
Matrix<T, M, P>
|
||||
operator*(const Matrix<T, N, P> &other) const
|
||||
{
|
||||
Matrix<T, M, P> result;
|
||||
result.epsilon = std::min(this->epsilon, other.epsilon);
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < P; j++) {
|
||||
T sum = (T)0.0;
|
||||
for (size_t k = 0; k < N; k++)
|
||||
sum += m[i][k] * other.m[k][j];
|
||||
result.m[i][j] = sum;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Equality check within epsilon.
|
||||
bool
|
||||
operator==(const Matrix<T, M, N> &other) const
|
||||
{
|
||||
T eps = std::min(this->epsilon, other.epsilon);
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < N; j++)
|
||||
if (!wr::math::AbsTolerance(m[i][j], other.m[i][j], eps))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool
|
||||
operator!=(const Matrix<T, M, N> &other) const
|
||||
{
|
||||
return !(*this == other);
|
||||
}
|
||||
|
||||
/// Output in row-major format.
|
||||
friend std::ostream&
|
||||
operator<<(std::ostream& outs, const Matrix<T, M, N>& mat)
|
||||
{
|
||||
outs << "[";
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
outs << "[";
|
||||
for (size_t j = 0; j < N; j++) {
|
||||
outs << mat.m[i][j];
|
||||
if (j < N - 1) outs << ", ";
|
||||
}
|
||||
outs << "]";
|
||||
if (i < M - 1) outs << ", ";
|
||||
}
|
||||
outs << "]";
|
||||
return outs;
|
||||
}
|
||||
|
||||
private:
|
||||
T
|
||||
gaussianEliminationDet() const
|
||||
{
|
||||
T tmp[M][M];
|
||||
for (size_t i = 0; i < M; i++)
|
||||
for (size_t j = 0; j < M; j++)
|
||||
tmp[i][j] = m[i][j];
|
||||
|
||||
T det_val = (T)1.0;
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
size_t pivot = i;
|
||||
for (size_t j = i + 1; j < M; j++)
|
||||
if (std::abs(tmp[j][i]) > std::abs(tmp[pivot][i]))
|
||||
pivot = j;
|
||||
if (pivot != i) {
|
||||
for (size_t k = 0; k < M; k++)
|
||||
std::swap(tmp[i][k], tmp[pivot][k]);
|
||||
det_val *= (T)-1.0;
|
||||
}
|
||||
if (wr::math::AbsTolerance(tmp[i][i], (T)0.0, this->epsilon))
|
||||
return (T)0.0;
|
||||
det_val *= tmp[i][i];
|
||||
for (size_t j = i + 1; j < M; j++) {
|
||||
T factor = tmp[j][i] / tmp[i][i];
|
||||
for (size_t k = i + 1; k < M; k++)
|
||||
tmp[j][k] -= factor * tmp[i][k];
|
||||
}
|
||||
}
|
||||
return det_val;
|
||||
}
|
||||
};
|
||||
|
||||
// Type aliases
|
||||
typedef Matrix<float, 2, 2> Matrix2f;
|
||||
typedef Matrix<float, 3, 3> Matrix3f;
|
||||
typedef Matrix<float, 4, 4> Matrix4f;
|
||||
typedef Matrix<double, 2, 2> Matrix2d;
|
||||
typedef Matrix<double, 3, 3> Matrix3d;
|
||||
typedef Matrix<double, 4, 4> Matrix4d;
|
||||
|
||||
/// Convert a Vector<T, N> to a column matrix Matrix<T, N, 1>.
|
||||
template <typename T, size_t N>
|
||||
Matrix<T, N, 1>
|
||||
vecToMat(const Vector<T, N>& vec)
|
||||
{
|
||||
Matrix<T, N, 1> result;
|
||||
for (size_t i = 0; i < N; i++)
|
||||
result.m[i][0] = vec[i];
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Convert a Matrix<T, N, 1> to a Vector<T, N>.
|
||||
template <typename T, size_t N>
|
||||
Vector<T, N>
|
||||
matToVec(const Matrix<T, N, 1>& mat)
|
||||
{
|
||||
std::array<T, N> arr;
|
||||
for (size_t i = 0; i < N; i++)
|
||||
arr[i] = mat.m[i][0];
|
||||
return Vector<T, N>::fromArrayEps(arr, mat.epsilon);
|
||||
}
|
||||
|
||||
} // namespace geom
|
||||
} // namespace wr
|
||||
|
||||
#endif // __WRMATH_GEOM_MATRIX_H
|
||||
@@ -9,7 +9,8 @@
|
||||
#define __WRMATH_GEOM_ORIENTATION_H
|
||||
|
||||
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <optional>
|
||||
#include "wrmath/geom/vector.h"
|
||||
|
||||
|
||||
namespace wr {
|
||||
@@ -84,6 +85,27 @@ float Heading3f(Vector3f vec);
|
||||
double Heading3d(Vector3d vec);
|
||||
|
||||
|
||||
/// RBearing3d computes the relative bearing from origin to point, in [0, 2π].
|
||||
/// Returns nullopt if either vector is zero.
|
||||
std::optional<double> RBearing3d(Vector3d origin, Vector3d point);
|
||||
|
||||
/// RBearing3f computes the relative bearing from origin to point, in [0, 2π].
|
||||
std::optional<float> RBearing3f(Vector3f origin, Vector3f point);
|
||||
|
||||
/// ABearing3d computes the absolute bearing (angle from +y axis) to point.
|
||||
std::optional<double> ABearing3d(Vector3d point);
|
||||
|
||||
/// ABearing3f computes the absolute bearing (angle from +y axis) to point.
|
||||
std::optional<float> ABearing3f(Vector3f point);
|
||||
|
||||
/// CompassHeading3d returns the compass heading [0, 2π] of a vector.
|
||||
/// North is the +y axis; positive angles are clockwise.
|
||||
std::optional<double> CompassHeading3d(Vector3d v);
|
||||
|
||||
/// CompassHeading3f returns the compass heading [0, 2π] of a vector.
|
||||
std::optional<float> CompassHeading3f(Vector3f v);
|
||||
|
||||
|
||||
} // namespace geom
|
||||
} // namespace wr
|
||||
|
||||
|
||||
@@ -10,8 +10,9 @@
|
||||
#include <initializer_list>
|
||||
#include <iostream>
|
||||
#include <ostream>
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <wrmath/math.h>
|
||||
#include "wrmath/geom/vector.h"
|
||||
#include "wrmath/geom/matrix.h"
|
||||
#include "wrmath/math.h"
|
||||
|
||||
/// wr contains the wntrmute robotics code.
|
||||
namespace wr {
|
||||
@@ -257,6 +258,55 @@ public:
|
||||
}
|
||||
|
||||
|
||||
/// Linear interpolation between two quaternions.
|
||||
Quaternion
|
||||
lerp(const Quaternion<T> &other, T t) const
|
||||
{
|
||||
Quaternion<T> sum = *this + (other - *this) * t;
|
||||
return sum / sum.norm();
|
||||
}
|
||||
|
||||
/// Shortest-path spherical linear interpolation.
|
||||
Quaternion
|
||||
slerp(const Quaternion<T> &other, T t) const
|
||||
{
|
||||
return ShortestSLERP(*this, other, t);
|
||||
}
|
||||
|
||||
/// Compute the Jacobian matrix of this quaternion.
|
||||
/// Returns a 3x4 matrix.
|
||||
geom::Matrix<T, 3, 4>
|
||||
jacobian() const
|
||||
{
|
||||
T q1 = this->w;
|
||||
T q2 = this->v[0];
|
||||
T q3 = this->v[1];
|
||||
T q4 = this->v[2];
|
||||
|
||||
geom::Matrix<T, 3, 4> result;
|
||||
result.m[0][0] = -q3; result.m[0][1] = q4; result.m[0][2] = -q1; result.m[0][3] = q2;
|
||||
result.m[1][0] = q2; result.m[1][1] = q1; result.m[1][2] = q4; result.m[1][3] = q3;
|
||||
result.m[2][0] = (T)2.0*q1; result.m[2][1] = (T)2.0*q2;
|
||||
result.m[2][2] = (T)-2.0*q3; result.m[2][3] = (T)-2.0*q4;
|
||||
result.setEpsilon(this->eps);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Compute the direction vector of this quaternion.
|
||||
/// Returns the direction the quaternion is pointing.
|
||||
Vector<T, 3>
|
||||
direction() const
|
||||
{
|
||||
Quaternion<T> u = this->isUnitQuaternion() ? *this : (*this / this->norm());
|
||||
T ux = u.v[0], uy = u.v[1], uz = u.v[2], uw = u.w;
|
||||
return Vector<T, 3>{
|
||||
(T)2.0 * (ux * uz + uw * uy),
|
||||
(T)2.0 * (uy * uz + uw * ux),
|
||||
uw*uw - ux*ux - uy*uy
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/// Perform quaternion addition with another quaternion.
|
||||
///
|
||||
/// @param other The quaternion to be added with this one.
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
#include <ostream>
|
||||
#include <iostream>
|
||||
|
||||
#include <wrmath/math.h>
|
||||
#include "wrmath/math.h"
|
||||
|
||||
|
||||
// This implementation is essentially a C++ translation of a Python library
|
||||
@@ -57,7 +57,14 @@ public:
|
||||
|
||||
wr::math::DefaultEpsilon(this->epsilon);
|
||||
std::copy(ilst.begin(), ilst.end(), this->arr.begin());
|
||||
}
|
||||
}
|
||||
|
||||
/// Construct a vector from a std::array with default epsilon.
|
||||
explicit Vector(const std::array<T, N>& src)
|
||||
{
|
||||
wr::math::DefaultEpsilon(this->epsilon);
|
||||
this->arr = src;
|
||||
}
|
||||
|
||||
|
||||
/// Compute the length of the vector.
|
||||
@@ -133,7 +140,7 @@ public:
|
||||
|
||||
/// Determine whether two vectors are parallel.
|
||||
/// @param other Another vector
|
||||
/// @return True if the angle between the vectors is zero.
|
||||
/// @return True if the two vectors are parallel (same direction).
|
||||
bool
|
||||
isParallel(const Vector<T, N> &other) const
|
||||
{
|
||||
@@ -141,12 +148,10 @@ public:
|
||||
return true;
|
||||
}
|
||||
|
||||
T angle = this->angle(other);
|
||||
if (wr::math::WithinTolerance(angle, (T)0.0, this->epsilon)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
// Compare unit vectors directly instead of using angle()/acos(), which
|
||||
// produces NaN or ~0.0003 on macOS/arm64 due to dot products landing
|
||||
// just above 1.0 (e.g. 1.000000001) — outside the domain of acos.
|
||||
return this->unitVector() == other.unitVector();
|
||||
}
|
||||
|
||||
|
||||
@@ -353,6 +358,132 @@ public:
|
||||
return outs;
|
||||
}
|
||||
|
||||
/// Return a zero vector with default epsilon.
|
||||
static Vector
|
||||
zero()
|
||||
{
|
||||
Vector v;
|
||||
for (size_t i = 0; i < N; i++) v.arr[i] = (T)0.0;
|
||||
return v;
|
||||
}
|
||||
|
||||
/// Return a copy with a different epsilon.
|
||||
Vector
|
||||
withEpsilon(T eps) const
|
||||
{
|
||||
Vector v = *this;
|
||||
v.epsilon = eps;
|
||||
return v;
|
||||
}
|
||||
|
||||
/// Access internal array (read-only).
|
||||
const std::array<T, N>&
|
||||
asArray() const
|
||||
{
|
||||
return this->arr;
|
||||
}
|
||||
|
||||
/// Construct from array with default epsilon.
|
||||
static Vector
|
||||
fromArray(const std::array<T, N>& src)
|
||||
{
|
||||
Vector v;
|
||||
v.arr = src;
|
||||
return v;
|
||||
}
|
||||
|
||||
/// Construct from array with custom epsilon.
|
||||
static Vector
|
||||
fromArrayEps(const std::array<T, N>& src, T eps)
|
||||
{
|
||||
Vector v;
|
||||
v.arr = src;
|
||||
v.epsilon = eps;
|
||||
return v;
|
||||
}
|
||||
|
||||
/// Apply a function to each element and return the result.
|
||||
template <typename F>
|
||||
Vector
|
||||
map(F f) const
|
||||
{
|
||||
Vector v;
|
||||
v.epsilon = this->epsilon;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
v.arr[i] = f(this->arr[i]);
|
||||
}
|
||||
return v;
|
||||
}
|
||||
|
||||
/// Return true if all elements are NaN.
|
||||
bool
|
||||
isNaN() const
|
||||
{
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (!std::isnan(this->arr[i])) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/// Compute the signed angle between two vectors in [-pi, pi].
|
||||
/// Uses the 2D cross product of the first two components.
|
||||
T
|
||||
angle2(const Vector<T, N> &other) const
|
||||
{
|
||||
T cross = this->arr[0] * other.arr[1] - this->arr[1] * other.arr[0];
|
||||
T dot = *this * other;
|
||||
return std::atan2(cross, dot);
|
||||
}
|
||||
|
||||
/// Compute the Euclidean distance between two vectors.
|
||||
T
|
||||
euclidist(const Vector<T, N> &other) const
|
||||
{
|
||||
T result = 0;
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
T diff = this->arr[i] - other.arr[i];
|
||||
result += diff * diff;
|
||||
}
|
||||
return std::sqrt(result);
|
||||
}
|
||||
|
||||
/// Project this vector into a lower-dimensional space by taking
|
||||
/// the first M elements.
|
||||
template <size_t M>
|
||||
Vector<T, M>
|
||||
projectLower() const
|
||||
{
|
||||
static_assert(M <= N, "cannot project to a higher dimension");
|
||||
std::array<T, M> result_arr;
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
result_arr[i] = this->arr[i];
|
||||
}
|
||||
return Vector<T, M>::fromArrayEps(result_arr, this->epsilon);
|
||||
}
|
||||
|
||||
/// Project this vector into a lower-dimensional space by taking
|
||||
/// the last M elements.
|
||||
template <size_t M>
|
||||
Vector<T, M>
|
||||
projectLowerTail() const
|
||||
{
|
||||
static_assert(M <= N, "cannot project to a higher dimension");
|
||||
std::array<T, M> result_arr;
|
||||
for (size_t i = 0; i < M; i++) {
|
||||
result_arr[i] = this->arr[N - M + i];
|
||||
}
|
||||
return Vector<T, M>::fromArrayEps(result_arr, this->epsilon);
|
||||
}
|
||||
|
||||
/// Return the x component (index 0).
|
||||
T x() const { static_assert(N >= 1, "x() requires N >= 1"); return this->arr[0]; }
|
||||
|
||||
/// Return the y component (index 1).
|
||||
T y() const { static_assert(N >= 2, "y() requires N >= 2"); return this->arr[1]; }
|
||||
|
||||
/// Return the z component (index 2).
|
||||
T z() const { static_assert(N >= 3, "z() requires N >= 3"); return this->arr[2]; }
|
||||
|
||||
private:
|
||||
static const size_t dim = N;
|
||||
T epsilon;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include <wrmath/math.h>
|
||||
#include "wrmath/math.h"
|
||||
|
||||
|
||||
namespace wr {
|
||||
|
||||
@@ -61,6 +61,57 @@ WithinTolerance(T a, T b, T epsilon)
|
||||
}
|
||||
|
||||
|
||||
/// Epsilon3 is an absolute tolerance within 3 decimal places.
|
||||
static constexpr double Epsilon3 = 0.001;
|
||||
|
||||
/// Epsilon6 is an absolute tolerance within 6 decimal places.
|
||||
static constexpr double Epsilon6 = 0.000001;
|
||||
|
||||
/// EpsilonMax is a tolerance near the maximum precision for doubles.
|
||||
static constexpr double EpsilonMax = 0.000000000000001;
|
||||
|
||||
/// AbsTolerance checks equality within epsilon, handling NaN and Inf.
|
||||
template <typename T>
|
||||
static bool
|
||||
AbsTolerance(T a, T b, T epsilon)
|
||||
{
|
||||
if (std::isnan(a)) return std::isnan(b);
|
||||
if (std::isnan(b)) return false;
|
||||
if (std::isinf(a)) return std::isinf(b);
|
||||
if (std::isinf(b)) return false;
|
||||
return std::abs(a - b) < epsilon;
|
||||
}
|
||||
|
||||
/// AbsError returns the absolute difference between two values.
|
||||
template <typename T>
|
||||
static T
|
||||
AbsError(T a, T b)
|
||||
{
|
||||
return std::abs(a - b);
|
||||
}
|
||||
|
||||
/// RotateRadians wraps (theta0 + theta1) into [-pi, pi].
|
||||
template <typename T>
|
||||
static T
|
||||
RotateRadians(T theta0, T theta1)
|
||||
{
|
||||
const T pi = static_cast<T>(M_PI);
|
||||
const T tau = pi * (T)2.0;
|
||||
T delta = theta1 + theta0;
|
||||
if (delta > pi) return RotateRadians(delta - tau, (T)0.0);
|
||||
if (delta < -pi) return RotateRadians(delta + tau, (T)0.0);
|
||||
return delta;
|
||||
}
|
||||
|
||||
/// Circumference returns 2*pi*r.
|
||||
template <typename T>
|
||||
static T
|
||||
Circumference(T r)
|
||||
{
|
||||
return (T)2.0 * static_cast<T>(M_PI) * r;
|
||||
}
|
||||
|
||||
|
||||
} // namespace math
|
||||
} // namespace wr
|
||||
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
#include <wrmath/geom/vector.h>
|
||||
#include <wrmath/geom/orientation.h>
|
||||
#include <optional>
|
||||
#include <cmath>
|
||||
#include "wrmath/geom/vector.h"
|
||||
#include "wrmath/geom/orientation.h"
|
||||
|
||||
|
||||
namespace wr {
|
||||
@@ -36,5 +38,82 @@ Heading3d(Vector3d vec)
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
#include <iostream>
|
||||
#include <wrmath/geom/quaternion.h>
|
||||
#include "wrmath/geom/quaternion.h"
|
||||
|
||||
|
||||
namespace wr {
|
||||
|
||||
Reference in New Issue
Block a user