#pragma once
#ifndef POSE_HPP
#define POSE_HPP 1
#define NUMCPP_NO_USE_BOOST ON
#include <iostream>
#include <numeric>
#include <iostream>
#include <algorithm> // For std::min_element, std::max_element
#include "NumCpp.hpp"
#ifdef _MSC_VER
#include <valarray>
#endif
namespace QOSM::Pose{
using MatrixX3d = nc::NdArray<double>;
/**
* @brief
* Vector of Nx3 points. Each point as 3 components (x, y, z) and can represent a position vector, a unit vector or a Poyinting vector.
*
*/
class VectorNx3 : public MatrixX3d{
protected:
/// Number of points (number of rows)
size_type N;
public:
/**
* @brief Default constructor, initializes a 1x3 vector.
*/
VectorNx3() : NdArray(1, 3), N(1){};
/**
* @brief Constructs a vector with N rows and 3 columns, initializing all elements to a specific value.
* @param _N Number of rows (vectors).
* @param value Initial value for all components (default: 0).
*/
VectorNx3(const size_type _N, double value = 0.0) : NdArray(_N, 3), N(_N){
fill(double{ value });
};
/**
* @brief Constructs a vector from an existing matrix of size Nx3.
* @param data Matrix of values.
*/
VectorNx3(const MatrixX3d data) : NdArray(data), N(data.numRows()){};
/**
* @brief Constructs a vector from a pointer to an array of doubles.
* @param ptr Pointer to data.
* @param size Total number of elements (must be a multiple of 3).
*/
VectorNx3(const double* ptr, uint32_t size) : NdArray(ptr, size){
N = size / 3;
}
/**
* @brief Returns the number of vectors (rows).
* @return Number of vectors.
*/
inline nc::uint32 rows() const{
return N;
}
/**
* @brief Retrieves a column at a specific index.
* @param idx Index of the column.
* @return Column as an Nx1 array.
*/
inline auto col(nc::uint32 idx){
return (*this)(rSlice(), idx);
}
/**
* @brief Retrieves a row at a specific index.
* @param idx Index of the row.
* @return Row as a 1x3 array.
*/
inline auto row(nc::uint32 idx){
return (*this)(idx, cSlice());
}
/**
* @brief Extracts a column and returns it as a separate VectorNx3 instance.
* @param idx Index of the column.
* @return New VectorNx3 containing the column.
*/
inline auto vcol(nc::uint32 idx){
return VectorNx3((*this)(rSlice(), idx));
}
/**
* @brief Extracts a row and returns it as a separate VectorNx3 instance.
* @param idx Index of the row.
* @param N Number of repetitions (default: 1).
* @return New VectorNx3 containing the repeated row.
*/
inline auto vrow(nc::uint32 idx, nc::uint32 N = 1){
return VectorNx3((*this)(idx, cSlice()).repeat(N, 1));
}
/**
* @brief Repeats a row N times.
* @param idx Index of the row.
* @return New instance with the row repeated.
*/
inline auto rowN(nc::uint32 idx){
return (*this)(idx, cSlice()).repeat(N, 1);
}
/**
* @brief Inserts a column vector at a specified index.
* @param idx Column index.
* @param data Column data (Nx1 matrix).
*/
inline void putCol(nc::uint32 idx, MatrixX3d &data){
put(rSlice(), idx, data);
}
/**
* @brief Inserts a row vector at a specified index.
* @param idx Row index.
* @param data Row data (1x3 matrix).
*/
inline void putRow(nc::uint32 idx, MatrixX3d &data){
put(idx, cSlice(), data);
}
/**
* @brief Creates a VectorNx3 from an existing MatrixX3d.
* @param data Matrix of values.
* @return New VectorNx3 instance.
*/
static VectorNx3 CreateVectorNx3(const MatrixX3d data){
return VectorNx3(NdArray(data));
};
/**
* @brief Computes the norm of each row.
* @return Nx1 matrix containing norms.
*/
inline MatrixX3d norm(){
return nc::sqrt<double>(nc::sum<double>((*this)*(*this), nc::Axis::COL));
}
/**
* @brief Computes the squared norm of each row.
* @return Nx1 matrix containing squared norms.
*/
inline MatrixX3d norm2(){
return nc::sum<double>((*this)*(*this), nc::Axis::COL);
}
/**
* @brief Normalizes each row in place.
* @return Nx1 matrix of normalization factors.
*/
nc::NdArray<double> normalise(){
const auto norm = (nc::sqrt<double>(nc::sum<double>((*this)*(*this), nc::Axis::COL))).reshape(N, 1);
*this /= norm;
return norm;
}
/**
* @brief Returns a normalized copy of the vector.
* @return New VectorNx3 instance with normalized rows.
*/
VectorNx3 normalised() const{
auto n = nc::sqrt<double>(nc::sum<double>((*this)*(*this), nc::Axis::COL)).reshape(N, 1);
return VectorNx3((*this) / n);
}
/**
* @brief Computes the dot product of each row with another VectorNx3.
* @warning Both vectors must have the same number of rows.
* @return Nx1 matrix of dot product values.
*/
inline MatrixX3d dot(const VectorNx3 &b){
return nc::sum<double>((*this)*b, nc::Axis::COL);
}
/**
* @brief Computes the cross product of each row with another VectorNx3.
* @warning Both vectors must have the same number of rows.
* @return New VectorNx3 containing cross product results.
*/
VectorNx3 cross(const VectorNx3 &v2){
const VectorNx3 &v1 = *this;
auto res = VectorNx3(v1.numRows());
auto r = v1.rSlice();
res.put(r, 0, v1(r, 1) * v2(r, 2) - v1(r, 2) * v2(r, 1));
res.put(r, 1, v1(r, 2) * v2(r, 0) - v1(r, 0) * v2(r, 2));
res.put(r, 2, v1(r, 0) * v2(r, 1) - v1(r, 1) * v2(r, 0));
return res;
}
/**
* @brief Flips the direction of all vectors.
* @return New VectorNx3 with reversed directions.
*/
VectorNx3 flip(){
MatrixX3d res = *this * -1.;
return VectorNx3(res);
}
/** @brief Scalar multiplication. */
friend VectorNx3 operator*(VectorNx3 lhs, const double &rhs){
auto res = lhs * rhs;
return VectorNx3(res);
}
/** @brief Scalar division. */
VectorNx3 &operator*=(const double &rhs){
*this *= rhs;
return *this;
}
/** @brief Scalar addition. */
friend VectorNx3 operator/(VectorNx3 lhs, const double &rhs){
auto res = lhs / rhs;
return VectorNx3(res);
}
/** @brief Scalar subtraction. */
VectorNx3 &operator/=(const double &rhs){
*this /= rhs;
return *this;
}
friend VectorNx3 operator+(VectorNx3 lhs, const double &rhs){
auto res = lhs + rhs;
return VectorNx3(res);
}
VectorNx3 &operator+=(const double &rhs){
*this += rhs;
return *this;
}
friend VectorNx3 operator-(VectorNx3 lhs, const double &rhs){
auto res = lhs - rhs;
return VectorNx3(res);
}
VectorNx3 &operator-=(const double &rhs){
*this -= rhs;
return *this;
}
};
#ifndef _MSC_VER
typedef double v4df __attribute__ ((vector_size (32)));
#else
using v4df = std::valarray<double>;
#endif
/**
* @brief
* 3D Vector of doubles (x, y, z). It can represent a position vector, a unit vector or a Poyinting vector.
*
*/
class Vec3{
protected:
v4df data = {0., 0., 0., 0.};
Vec3(const v4df &v){
data[0] = v[0];
data[1] = v[1];
data[2] = v[2];
}
public:
/**
* @brief Default constructor initializing the vector to (0,0,0).
*/
Vec3(double x=0., double y=0., double z=0.){
data[0] = x;
data[1] = y;
data[2] = z;
}
/**
* @brief Constructor initializing from an nc::NdArray.
* @param inVector Input array.
* @param idx Row index to extract the vector from.
*/
Vec3(const nc::NdArray<double> &inVector, const std::size_t idx){
data[0] = inVector(idx, 0);
data[1] = inVector(idx, 1);
data[2] = inVector(idx, 2);
}
/**
* @brief Constructor initializing from a VectorNx3.
* @param inVector Input matrix.
* @param idx Row index to extract the vector from.
*/
Vec3(const VectorNx3 &inVector, const std::size_t idx){
data[0] = inVector(idx, 0);
data[1] = inVector(idx, 1);
data[2] = inVector(idx, 2);
}
/**
* @brief Constructor initializing from a 3-element std::array.
* @param v Input array.
*/
Vec3(const std::array<double, 3> &v){
auto v_ptr = v.data();
data[0] = v_ptr[0];
data[1] = v_ptr[1];
data[2] = v_ptr[2];
}
/**
* @brief Constructor initializing from a std::vector.
* @param vec Input vector (must have at least 3 elements).
* @throws std::runtime_error if the vector has less than 3 elements.
*/
Vec3(const std::vector<double>& vec) {
if (vec.size() < 3)
throw std::runtime_error("Invalid vector size (<3)");
auto it = vec.begin();
data[0] = *it++;
data[1] = *it++;
data[2] = *it;
}
/**
* @brief Constructor initializing from an initializer list.
* @param values List of 3 values.
* @throws std::runtime_error if the list has less than 3 elements.
*/
Vec3(std::initializer_list<double> values) {
if (values.size() < 3)
throw std::runtime_error("Invalid list size (<3)");
auto it = values.begin();
data[0] = *it++;
data[1] = *it++;
data[2] = *it;
}
/**
* @brief Assigns values using an initializer list.
* @param values List of 3 values.
* @throws std::runtime_error if the list has less than 3 elements.
*/
void assign(std::initializer_list<double> values) {
if (values.size() < 3)
throw std::runtime_error("Invalid list size (<3)");
auto it = values.begin();
data[0] = *it++;
data[1] = *it++;
data[2] = *it;
}
/**
* @brief Computes the dot product with another vector.
* @param rhl The other vector.
* @return The dot product value.
*/
inline double dot(const Vec3 &rhl) const{
return data[0]*rhl.data[0] + data[1]*rhl.data[1] + data[2]*rhl.data[2];
}
/**
* @brief Computes the squared norm of the vector.
* @return The squared norm value.
*/
inline double norm2() const{
return data[0]*data[0] + data[1]*data[1] + data[2]*data[2];
}
/**
* @brief Computes the norm (magnitude) of the vector.
* @return The norm value.
*/
inline double norm() const{
return std::sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2]);
}
/**
* @brief Normalizes the vector in place.
* @return The original norm before normalization.
*/
inline double normalise(){
const double norm = std::sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2]);
if (norm != 0.) data /= norm;
return norm;
}
/**
* @brief Returns a normalized copy of the vector.
* @return A normalized vector.
*/
inline Vec3 normalised() const{
const double n = std::sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2]);
return n != 0. ? Vec3(data / n) : Vec3();
}
/**
* @brief Computes the cross product with another vector.
* @param rhl The other vector.
* @return The resulting cross product vector.
*/
Vec3 cross(const Vec3 &rhl) const{
return Vec3(
data[1]*rhl.data[2] - data[2]*rhl.data[1],
data[2]*rhl.data[0] - data[0]*rhl.data[2],
data[0]*rhl.data[1] - data[1]*rhl.data[0]
);
}
inline Vec3& operator+=(const Vec3& rhl){
data += rhl.data;
return *this;
}
inline Vec3& operator-=(const Vec3& rhl){
data -= rhl.data;
return *this;
}
inline Vec3& operator*=(const Vec3& rhl){
data *= rhl.data;
return *this;
}
inline Vec3& operator/=(const Vec3& rhl){
data /= rhl.data;
return *this;
}
inline Vec3& operator+=(const double& rhl){
data += rhl;
return *this;
}
inline Vec3& operator-=(const double& rhl){
data -= rhl;
return *this;
}
inline Vec3& operator*=(const double& rhl){
data *= rhl;
return *this;
}
inline Vec3& operator/=(const double& rhl){
data /= rhl;
return *this;
}
inline double &operator[](short dim){
return data[dim];
}
inline double operator[](short dim) const{
return data[dim];
}
friend Vec3 operator+(const Vec3& lhl, const Vec3& rhl){
return Vec3(lhl.data + rhl.data);
}
friend Vec3 operator-(const Vec3& lhl, const Vec3& rhl){
return Vec3(lhl.data - rhl.data);
}
friend Vec3 operator*(const Vec3& lhl, const Vec3& rhl){
return Vec3(lhl.data * rhl.data);
}
friend Vec3 operator/(const Vec3& lhl, const Vec3& rhl){
return Vec3(lhl.data / rhl.data);
}
friend Vec3 operator+(const Vec3& lhl, const double rhl){
return Vec3(lhl.data + rhl);
}
friend Vec3 operator-(const Vec3& lhl, const double rhl){
return Vec3(lhl.data - rhl);
}
friend Vec3 operator*(const Vec3& lhl, const double rhl){
return Vec3(lhl.data * rhl);
}
friend Vec3 operator/(const Vec3& lhl, const double rhl){
return Vec3(lhl.data / rhl);
}
friend Vec3 operator+(const double lhl, const Vec3& rhl){
return Vec3(lhl + rhl.data);
}
friend Vec3 operator-(const double lhl, const Vec3& rhl){
return Vec3(lhl - rhl.data);
}
friend Vec3 operator*(const double lhl, const Vec3& rhl){
return Vec3(lhl * rhl.data);
}
friend Vec3 operator/(const double lhl, const Vec3& rhl){
return Vec3(lhl / rhl.data);
}
/**
* @brief Computes the minimum component of the vector.
* @return The minimum value.
*/
double min() const {
return (data[0] < data[1])
? (data[0] < data[2] ? data[0] : data[1])
: (data[1] < data[2] ? data[1] : data[2]);
}
/**
* @brief Computes the maximum component of the vector.
* @return The maximum value.
*/
double max() const {
return (data[0] > data[1])
? (data[0] > data[2] ? data[0] : data[1])
: (data[1] > data[2] ? data[1] : data[2]);
}
/**
* @brief Computes the minimum between two vectors.
* @return A vector containing the element-wise min.
*/
Vec3 minimum(const Vec3& other) const {
return {
std::min(data[0], other.data[0]),
std::min(data[1], other.data[1]),
std::min(data[2], other.data[2])
};
}
/**
* @brief Computes the maximum between two vectors.
* @return A vector containing the element-wise max.
*/
Vec3 maximum(const Vec3& other) const {
return {
std::max(data[0], other.data[0]),
std::max(data[1], other.data[1]),
std::max(data[2], other.data[2])
};
}
/**
* @brief Finds the index of the minimum element.
* @return Index of the minimum value.
*/
int argmin() const {
return (data[0] < data[1])
? (data[0] < data[2] ? 0 : 1)
: (data[1] < data[2] ? 1 : 2);
}
/**
* @brief Finds the index of the maximum element.
* @return Index of the maximum value.
*/
int argmax() const {
return (data[0] > data[1])
? (data[0] > data[2] ? 0 : 1)
: (data[1] > data[2] ? 1 : 2);
}
/**
* @brief Converts the vector to a VectorNx3 representation.
* @param N Number of rows.
* @return A VectorNx3 with repeated rows.
*/
VectorNx3 toNd(const std::size_t N) const{
VectorNx3 res(N);
res.put(res.rSlice(), 0, data[0]);
res.put(res.rSlice(), 1, data[1]);
res.put(res.rSlice(), 2, data[2]);
return res;
}
/**
* @brief Accessors for individual components.
*/
double& getx(){
return data[0];
}
/**
* @brief Accessors for individual components.
*/
double& gety(){
return data[1];
}
/**
* @brief Accessors for individual components.
*/
double& getz(){
return data[2];
}
/**
* @brief Mutators for individual components.
*/
void setx(double &val){
data[0] = val;
}
/**
* @brief Mutators for individual components.
*/
void sety(double &val){
data[1] = val;
}
/**
* @brief Mutators for individual components.
*/
void setz(double &val){
data[2] = val;
}
/**
* @brief Creates a copy of the vector.
* @return A new Vec3 instance with the same values.
*/
Vec3 copy() const{
return Vec3(data[0], data[1], data[2]);
}
/**
* @brief Overloaded stream output operator.
*/
friend std::ostream& operator<<(std::ostream& os, const Vec3& o){
os << "Vector: [" << o.data[0] << ", " << o.data[1] << ", " << o.data[2] << "]";
return os;
}
/**
* @brief Converts the vector to a string representation.
* @return A formatted string representing the vector.
*/
std::string __str__() const {
return "Vector: [" + std::to_string(data[0]) + ", " + std::to_string(data[1]) + ", " + std::to_string(data[2]) + "] ";
}
};
/**
* @class Quaternion
* @brief Represents a quaternion, using the European convention (w, x, y, z).
*
* Quaternions are used for representing rotations in 3D space and provide
* an efficient way to handle rotations without suffering from gimbal lock.
* This class supports various constructors, arithmetic operations, and
* rotation transformations.
*
* @note The class uses a four-element vector (v4df) to store the quaternion components.
*/
class Quaternion{
protected:
v4df data = {0., 0., 0., 0.};
Quaternion(const v4df &q){
data[0] = q[0];
data[1] = q[1];
data[2] = q[2];
data[3] = q[3];
}
public:
/**
* @brief Default constructor, initializes a quaternion.
* @param qw Scalar (real) component, default is 1.
* @param qx X-component, default is 0.
* @param qy Y-component, default is 0.
* @param qz Z-component, default is 0.
*/
Quaternion(const double& qw=1., const double& qx=0., const double& qy=0., const double& qz=0.){
data[0] = qw;
data[1] = qx;
data[2] = qy;
data[3] = qz;
}
/**
* @brief Constructs a quaternion from a scalar and a 3D vector.
* @param w Scalar component.
* @param v 3D vector representing the imaginary part.
*/
Quaternion(const double& w, const Vec3& v){
data[0] = w;
data[1] = v[0];
data[2] = v[1];
data[3] = v[2];
}
/**
* @brief Constructs a quaternion from an axis-angle representation.
* @param axis Rotation axis (must be normalized).
* @param angle Rotation angle in radians (or degrees if deg=true).
* @param deg Whether the angle is given in degrees (default: false).
*/
Quaternion(const Vec3& axis, const double& angle, const bool& deg=false){
const double _angle = (deg ? angle * nc::constants::pi / 180. : angle) * .5;
const double sAngle = std::sin(_angle);
data[0] = std::cos(_angle);
data[1] = axis[0] * sAngle;
data[2] = axis[1] * sAngle;
data[3] = axis[2] * sAngle;
}
/**
* @brief Constructs a quaternion from a rotation vector.
* @param rotVec A vector representing the axis and angle of rotation.
* @param deg Whether the rotation vector is in degrees (default: false).
*/
Quaternion(const Vec3& rotVec, const bool& deg=false){
const double angle = rotVec.norm() * (deg ? nc::constants::pi / 180. : 1.) * .5;
const double sAngle = std::sin(angle);
const Vec3 axis = rotVec.normalised();
data[0] = std::cos(angle);
data[1] = axis[0] * sAngle;
data[2] = axis[1] * sAngle;
data[3] = axis[2] * sAngle;
}
/**
* @brief Constructs a quaternion from a 4-element array.
* @param q An array of 4 double values.
*/
Quaternion(const std::array<double, 4> &q){
auto q_ptr = q.data();
data[0] = q_ptr[0];
data[1] = q_ptr[1];
data[2] = q_ptr[2];
data[3] = q_ptr[3];
}
/**
* @brief Constructs a quaternion from a vector of at least 4 elements.
* @param q A std::vector of doubles (must have at least 4 elements).
* @throws std::runtime_error if the vector size is < 4.
*/
Quaternion(const std::vector<double>& q) {
if (q.size() < 4)
throw std::runtime_error("Invalid quaternion size (<4)");
auto it = q.begin();
data[0] = *it++;
data[1] = *it++;
data[2] = *it++;
data[3] = *it;
}
/**
* @brief Constructs a quaternion from an initializer list.
* @param values A list of 4 double values.
* @throws std::runtime_error if the list size is < 4.
*/
Quaternion(std::initializer_list<double> values) {
if (values.size() < 4)
throw std::runtime_error("Invalid list size (<4)");
auto it = values.begin();
data[0] = *it++;
data[1] = *it++;
data[2] = *it++;
data[3] = *it;
}
/**
* @brief Assigns new values to the quaternion from an initializer list.
* @param values A list of 4 double values.
* @throws std::runtime_error if the list size is < 4.
*/
void assign(std::initializer_list<double> values) {
if (values.size() < 4)
throw std::runtime_error("Invalid list size (<4)");
auto it = values.begin();
data[0] = *it++;
data[1] = *it++;
data[2] = *it++;
data[3] = *it;
}
/**
* @brief Returns the vector (imaginary) part of the quaternion.
* @return Vec3 The imaginary part (x, y, z).
*/
inline Vec3 toVec3() const{
return {data[1], data[2], data[3]};
}
/** @brief Accessor for the scalar (real) part of the quaternion. */
double& getw(){
return data[0];
}
/** @brief Accessor for the x-component of the quaternion. */
double& getx(){
return data[1];
}
/** @brief Accessor for the y-component of the quaternion. */
double& gety(){
return data[2];
}
/** @brief Accessor for the z-component of the quaternion. */
double& getz(){
return data[3];
}
/**
* @brief Computes the conjugate of the quaternion.
* @return Quaternion with the same real part but negated imaginary parts.
*/
inline Quaternion conj() const{
return {data[0], -data[1], -data[2], -data[3]};
}
/**
* @brief Computes the dot product with another quaternion.
* @param rhl The other quaternion.
* @return Dot product value.
*/
inline double dot(const Quaternion &rhl) const{
return data[0]*rhl.data[0] + data[1]*rhl.data[1] + data[2]*rhl.data[2];
}
/**
* @brief Computes the squared norm of the quaternion.
* @return The squared norm.
*/
inline double norm2() const{
return data[0]*data[0] + data[1]*data[1] + data[2]*data[2] + data[3]*data[3];
}
/**
* @brief Computes the norm (magnitude) of the quaternion.
* @return The norm of the quaternion.
*/
inline double norm() const{
return std::sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2] + data[3]*data[3]);
}
/**
* @brief Normalizes the quaternion in place.
*/
inline void normalise(){
data /= std::sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2] + data[3]*data[3]);
}
/**
* @brief Returns a normalized copy of the quaternion.
* @return A normalized quaternion.
*/
inline Quaternion normalised() const{
const double n = std::sqrt(data[0]*data[0] + data[1]*data[1] + data[2]*data[2] + data[3]*data[3]);
return Quaternion(data / n);
}
/**
* @brief Rotates a 3D vector using the quaternion.
* @param v The vector to rotate.
* @return The rotated vector.
*/
inline Vec3 rotate(const Vec3& v) const{
const Quaternion qv(0., v[0], v[1], v[2]);
const Quaternion res = (*this) * (qv * (*this).conj());
return {res[1], res[2], res[3]};
}
/**
* @brief Overloaded rotation method for modifying the input vector.
*/
inline Vec3 rotate(Vec3& v) const{
const Quaternion qv(0., v[0], v[1], v[2]);
const Quaternion res = (*this) * (qv * (*this).conj());
return {res[1], res[2], res[3]};
}
/**
* @brief Rotates a set of 3D vectors.
* @param v The vectors to rotate.
* @return The rotated vectors.
*/
inline VectorNx3 rotate(const VectorNx3& v) const{
const auto N = v.numRows();
VectorNx3 res(N);
for(int i = 0; i < N; i++){
const Quaternion qv(0., v(i, 0), v(i, 1), v(i, 2));
const Quaternion qres = (*this) * (qv * (*this).conj());
MatrixX3d a = {qres[1], qres[2], qres[3]};
res.putRow(i, a);
}
return res;
}
/**
* @brief Overloaded rotation method for modifying a set of vectors.
*/
inline VectorNx3 rotate(VectorNx3& v) const{
const auto N = v.numRows();
VectorNx3 res(N);
for(int i = 0; i < N; i++){
const Quaternion qv(0., v(i, 0), v(i, 1), v(i, 2));
const Quaternion qres = (*this) * (qv * (*this).conj());
MatrixX3d a = {qres[1], qres[2], qres[3]};
res.putRow(i, a);
}
return res;
}
inline double &operator[](short dim){
return data[dim];
}
inline double operator[](short dim) const{
return data[dim];
}
friend Quaternion operator+(const Quaternion& lhl, const Quaternion& rhl){
return Quaternion(lhl.data + rhl.data);
}
friend Quaternion operator-(const Quaternion& lhl, const Quaternion& rhl){
return Quaternion(lhl.data - rhl.data);
}
friend Quaternion operator*(const Quaternion& lhl, const Quaternion& rhl){
return {
lhl.data[0] * rhl.data[0] - lhl.data[1] * rhl.data[1] - lhl.data[2] * rhl.data[2] - lhl.data[3] * rhl.data[3], // w
lhl.data[0] * rhl.data[1] + lhl.data[1] * rhl.data[0] + lhl.data[2] * rhl.data[3] - lhl.data[3] * rhl.data[2], // x
lhl.data[0] * rhl.data[2] - lhl.data[1] * rhl.data[3] + lhl.data[2] * rhl.data[0] + lhl.data[3] * rhl.data[1], // y
lhl.data[0] * rhl.data[3] + lhl.data[1] * rhl.data[2] - lhl.data[2] * rhl.data[1] + lhl.data[3] * rhl.data[0] // z
};
}
friend Quaternion operator*(const Quaternion& lhl, const double rhl){
return Quaternion(lhl.data * rhl);
}
friend Quaternion operator/(const Quaternion& lhl, const double rhl){
return Quaternion(lhl.data / rhl);
}
friend Quaternion operator*(const double lhl, const Quaternion& rhl){
return Quaternion(lhl * rhl.data);
}
friend std::ostream& operator<<(std::ostream& os, const Quaternion& o){
os << "Quaternion: [" << o.data[0] << ", " << o.data[1] << ", " << o.data[2] << ", " << o.data[3] << "]";
return os;
}
// Define the __str__() method for printing the object
std::string __str__() const {
return "Quaternion: [" + std::to_string(data[0]) + ", " + std::to_string(data[1]) + ", " + std::to_string(data[2]) + ", " + std::to_string(data[3]) + "] ";
}
};
/**
* @struct Pose
* @brief Represents a 3D pose consisting of position and orientation.
*
* The Pose structure combines a position vector (`ref_r_ref_ori`) and an
* orientation quaternion (`q_ref_ori`). It provides multiple constructors
* for initialization using initializer lists, arrays, or explicit parameters.
*/
struct Pose{
Vec3 ref_r_ref_ori; ///< Position vector (x, y, z) in reference frame.
Quaternion q_ref_ori; ///< Orientation as a quaternion.
/**
* @brief Constructs a Pose from an initializer list.
* @param values A list of 7 double values (3 for position, 4 for quaternion).
* @throws std::runtime_error if the list size is not 7.
*/
Pose(const std::initializer_list<double>& values) {
if (values.size() != 7)
throw std::runtime_error("Invalid list size (must be 7)");
auto it = values.begin();
ref_r_ref_ori = {*it++, *it++, *it++};
q_ref_ori = {*it++, *it++, *it++, *it++};
}
/**
* @brief Constructs a Pose from a 7-element array.
* @param values A std::array of 7 doubles (3 for position, 4 for quaternion).
*/
Pose(const std::array<double, 7>& values) {
auto it = values.begin();
ref_r_ref_ori = {*it++, *it++, *it++};
q_ref_ori = {*it++, *it++, *it++, *it++};
}
/**
* @brief Constructs a Pose from explicit position and quaternion values.
* @param v The position vector (default: zero vector).
* @param q The orientation quaternion (default: identity quaternion).
*/
Pose(Vec3 v = Vec3(), Quaternion q = Quaternion()) {
ref_r_ref_ori = v;
q_ref_ori = q;
}
};
/**
* Write the pose of b (written in frame ref) wrt the frame a
* @param pose_ref_a Pose of a wrt frame ref
* @param pose_ref_b Pose of b wrt frame ref
* @return pose_a_b Pose of b wrt frale a
*/
Pose changeFrame(const Pose &pose_ref_a, const Pose &pose_ref_b);
/**
* @brief
* Grid of points on a planar surface
*/
class MeshGrid{
private:
/// @brief Scale to apply on the points coordinates
double scale;
public:
/// @brief Matrices of points (meshgrid)
MatrixX3d U, V;
/// @brief number of point along u and v axis
long nu, nv;
/**
* Create a Meshgrid
* @param u Meshgrid for u axis
* @param v Meshgrid for v axis
* @param scale Scale to apply to the grid
*/
MeshGrid(MatrixX3d u, MatrixX3d v, const double scale=1.) : scale(scale){
nu = u.size();
nv = v.size();
auto grid = nc::meshgrid(u, v);
U = grid.first * scale;
V = grid.second * scale;
}
/**
* Get the number of points in the grid
* @return the number of points as an integer
*/
inline auto size(){
return nu * nv;
}
/**
* Get an 1-D vector of the meshgrid U
* @return the flatten vector of the mesgrid U
*/
MatrixX3d flattenU(){
return U.reshape(nu*nv, 1);
}
/**
* Get an 1-D vector of the meshgrid V
* @return the flatten vector of the mesgrid V
*/
MatrixX3d flattenV(){
return V.reshape(nu*nv, 1);
}
/**
* Return a Vector of points for an XY plane located at z
* @param z position along the z-axis of the XY plane
* @return a Vector of points lying in the XY plane
*/
VectorNx3 xyPlane(double z=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, U.reshape(nu*nv, 1));
r_pts.put(rows, 1, V.reshape(nu*nv, 1));
r_pts.put(rows, 2, z * scale);
return r_pts;
}
/**
* Return a Vector of points for an XZ plane located at y
* @param y position along the y-axis of the XY plane
* @return a Vector of points lying in the XZ plane
*/
VectorNx3 xzPlane(double y=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, U.reshape(nu*nv, 1));
r_pts.put(rows, 1, y * scale);
r_pts.put(rows, 2, V.reshape(nu*nv, 1));
return r_pts;
}
/**
* Return a Vector of points for an YZ plane located at x
* @param x position along the x-axis of the XY plane
* @return a Vector of points lying in the YZ plane
*/
VectorNx3 yzPlane(double x=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, x * scale);
r_pts.put(rows, 1, U.reshape(nu*nv, 1));
r_pts.put(rows, 2, V.reshape(nu*nv, 1));
return r_pts;
}
/**
* Return a Vector of points for an YX plane located at z
* @param z position along the z-axis of the YX plane
* @return a Vector of points lying in the YX plane
*/
VectorNx3 yxPlane(double z=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, V.reshape(nu*nv, 1));
r_pts.put(rows, 1, U.reshape(nu*nv, 1));
r_pts.put(rows, 2, z * scale);
return r_pts;
}
/**
* Return a Vector of points for an ZY plane located at x
* @param x position along the z-axis of the XY plane
* @return a Vector of points lying in the ZY plane
*/
VectorNx3 zyPlane(double x=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, x * scale);
r_pts.put(rows, 1, V.reshape(nu*nv, 1));
r_pts.put(rows, 2, U.reshape(nu*nv, 1));
return r_pts;
}
/**
* Return a Vector of points for an ZX plane located at z
* @param y position along the z-axis of the ZX plane
* @return a Vector of points lying in the ZX plane
*/
VectorNx3 zxPlane(double y=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, V.reshape(nu*nv, 1));
r_pts.put(rows, 1, y * scale);
r_pts.put(rows, 2, U.reshape(nu*nv, 1));
return r_pts;
}
};
}
#endif