diff --git a/CMakeLists.txt b/CMakeLists.txt
index acbcc79..3f9d512 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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)
diff --git a/test/madgwick_test.cc b/test/madgwick_test.cc
index eeb190a..6dd494b 100644
--- a/test/madgwick_test.cc
+++ b/test/madgwick_test.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;
diff --git a/test/orientation_test.cc b/test/orientation_test.cc
index ac57eae..50d8212 100644
--- a/test/orientation_test.cc
+++ b/test/orientation_test.cc
@@ -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;
diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc
index 33c6afc..7c58d28 100644
--- a/test/quaternion_test.cc
+++ b/test/quaternion_test.cc
@@ -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;
diff --git a/test/vector_test.cc b/test/vector_test.cc
index fdd0ff8..32340d3 100644
--- a/test/vector_test.cc
+++ b/test/vector_test.cc
@@ -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;
diff --git a/wrmath/estim/imu.h b/wrmath/estim/imu.h
new file mode 100644
index 0000000..1165855
--- /dev/null
+++ b/wrmath/estim/imu.h
@@ -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
diff --git a/wrmath/filter.h b/wrmath/filter.h
index cf0f4e4..92d7484 100644
--- a/wrmath/filter.h
+++ b/wrmath/filter.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
diff --git a/wrmath/filter/madgwick.h b/wrmath/filter/madgwick.h
index f556df4..56725b8 100644
--- a/wrmath/filter/madgwick.h
+++ b/wrmath/filter/madgwick.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;
 };
diff --git a/wrmath/geom.h b/wrmath/geom.h
index c108653..a88983e 100644
--- a/wrmath/geom.h
+++ b/wrmath/geom.h
@@ -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
diff --git a/wrmath/geom/coord2d.h b/wrmath/geom/coord2d.h
new file mode 100644
index 0000000..8ce5337
--- /dev/null
+++ b/wrmath/geom/coord2d.h
@@ -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
diff --git a/wrmath/geom/coord3d.h b/wrmath/geom/coord3d.h
new file mode 100644
index 0000000..22bad7b
--- /dev/null
+++ b/wrmath/geom/coord3d.h
@@ -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
diff --git a/wrmath/geom/matrix.h b/wrmath/geom/matrix.h
new file mode 100644
index 0000000..175e976
--- /dev/null
+++ b/wrmath/geom/matrix.h
@@ -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
diff --git a/wrmath/geom/orientation.h b/wrmath/geom/orientation.h
index d0449fa..86f4ace 100644
--- a/wrmath/geom/orientation.h
+++ b/wrmath/geom/orientation.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
 
diff --git a/wrmath/geom/quaternion.h b/wrmath/geom/quaternion.h
index 66fbb10..7bca565 100644
--- a/wrmath/geom/quaternion.h
+++ b/wrmath/geom/quaternion.h
@@ -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.
diff --git a/wrmath/geom/vector.h b/wrmath/geom/vector.h
index a86e65c..36c1b32 100644
--- a/wrmath/geom/vector.h
+++ b/wrmath/geom/vector.h
@@ -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;
diff --git a/wrmath/math.cc b/wrmath/math.cc
index 3deea7b..4860b9f 100644
--- a/wrmath/math.cc
+++ b/wrmath/math.cc
@@ -1,4 +1,4 @@
-#include <wrmath/math.h>
+#include "wrmath/math.h"
 
 
 namespace wr {
diff --git a/wrmath/math.h b/wrmath/math.h
index daca2ec..39b618b 100644
--- a/wrmath/math.h
+++ b/wrmath/math.h
@@ -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
 
diff --git a/wrmath/orientation.cc b/wrmath/orientation.cc
index 0a8462e..360dee0 100644
--- a/wrmath/orientation.cc
+++ b/wrmath/orientation.cc
@@ -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
diff --git a/wrmath/quaternion.cc b/wrmath/quaternion.cc
index f035ae6..9145b64 100644
--- a/wrmath/quaternion.cc
+++ b/wrmath/quaternion.cc
@@ -1,5 +1,5 @@
 #include <iostream>
-#include <wrmath/geom/quaternion.h>
+#include "wrmath/geom/quaternion.h"
 
 
 namespace wr {