From 4931f7dc0207f1bfb3b7e85c56a07ec6194a9ab5 Mon Sep 17 00:00:00 2001
From: g18gaudi <gregory.gaudin@imt-atlantique.fr>
Date: Mon, 17 Mar 2025 19:06:24 +0100
Subject: [PATCH] Fixed header errors after docstring generation
---
build/.empty | 0
include/grid.hpp | 60 ++--
include/pose.hpp | 799 ++++++++++++++++++++++++++++++++++++-----------
setup-linux.py | 2 +
setup.py | 2 +
5 files changed, 643 insertions(+), 220 deletions(-)
create mode 100644 build/.empty
diff --git a/build/.empty b/build/.empty
new file mode 100644
index 0000000..e69de29
diff --git a/include/grid.hpp b/include/grid.hpp
index e406740..dccf14d 100644
--- a/include/grid.hpp
+++ b/include/grid.hpp
@@ -18,7 +18,6 @@ namespace QOSM::Tools{
ZY
};
-
/**
* @class Grid
* @brief Represents a 2D grid of points in a specified plane.
@@ -27,7 +26,7 @@ namespace QOSM::Tools{
* ranges. It supports various grid generation methods and transformations such
* as rotation and scaling.
*/
- class Grid {
+ class Grid{
protected:
/**
* @brief Creates grid points within the specified parameter ranges.
@@ -37,11 +36,7 @@ namespace QOSM::Tools{
* @param n A scaling factor.
* @param plane The plane type in which the grid is defined.
*/
- void createPoints(const std::array<double,2>& uRange,
- const std::array<double,2>& vRange,
- const std::array<int,2>& numPoints,
- const double& n,
- const PlaneType& plane);
+ void createPoints(const std::array<double,2> &uRange, const std::array<double,2> &vRange, const std::array<int,2> &numPoints, const double &n, const PlaneType &plane);
public:
std::array<unsigned int, 2> shape{0, 0}; ///< Grid dimensions (rows, cols).
@@ -49,7 +44,7 @@ namespace QOSM::Tools{
nc::NdArray<double> v; ///< V-coordinates of grid points.
VectorNx3 points; ///< 3D coordinates of grid points.
PlaneType type; ///< Type of plane where the grid is defined.
-
+
/**
* @brief Constructs a Grid with adaptive step size.
* @param uRange Range and step for the u-coordinate (min, max, step).
@@ -58,10 +53,10 @@ namespace QOSM::Tools{
* @param plane The plane type in which the grid is defined.
*/
Grid(const std::array<double,3>& uRange,
- const std::array<double,3>& vRange,
- const double& n,
- const PlaneType& plane);
-
+ const std::array<double,3>& vRange,
+ const double& n,
+ const PlaneType& plane);
+
/**
* @brief Constructs a Grid with a fixed number of points.
* @param uRange Range of the u-coordinate (min, max).
@@ -70,59 +65,58 @@ namespace QOSM::Tools{
* @param n A scaling factor.
* @param plane The plane type in which the grid is defined.
*/
- Grid(const std::array<double,2>& uRange,
- const std::array<double,2>& vRange,
- const std::array<int,2>& numPoints,
- const double& n,
- const PlaneType& plane);
-
+ Grid(const std::array<double,2> &uRange,
+ const std::array<double,2> &vRange,
+ const std::array<int,2> &numPoints,
+ const double &n,
+ const PlaneType &plane);
+
/**
* @brief Rotates the grid around the U-axis.
* @param centre The center of rotation.
* @param angle The rotation angle (in radians by default).
* @param deg If true, the angle is interpreted in degrees.
*/
- void rotateAlongU(const Vec3& centre, const double& angle, const bool& deg = false);
-
+ void rotateAlongU(const Vec3 ¢re, const double &angle, const bool °=false);
+
/**
* @brief Rotates the grid around the V-axis.
* @param centre The center of rotation.
* @param angle The rotation angle (in radians by default).
* @param deg If true, the angle is interpreted in degrees.
*/
- void rotateAlongV(const Vec3& centre, const double& angle, const bool& deg = false);
-
+ void rotateAlongV(const Vec3 ¢re, const double &angle, const bool °=false);
+
/**
* @brief Rotates the grid using a quaternion.
* @param centre The center of rotation.
* @param rot The quaternion representing the rotation.
*/
- void rotate(const Vec3& centre, const Quaternion& rot);
-
+ void rotate(const Vec3 ¢re, const Quaternion &rot);
+
/**
* @brief Scales the grid.
* @param scale The scaling factor.
*/
- void operator*=(const double& scale);
-
+ void operator*=(const double &scale);
+
/**
* @brief Uniformly scales down the grid.
* @param scale The scaling factor.
*/
- void operator/=(const double& scale);
-
+ void operator/=(const double &scale);
+
/**
* @brief Translates the grid by a given offset.
* @param offset The translation vector.
*/
- void operator+=(const Vec3& offset);
-
+ void operator+=(const Vec3 &offset);
+
/**
* @brief Translates the grid in the opposite direction of a given offset.
* @param offset The translation vector.
*/
- void operator-=(const Vec3& offset);
- };
-
+ void operator-=(const Vec3 &offset);
+ };
}
#endif
\ No newline at end of file
diff --git a/include/pose.hpp b/include/pose.hpp
index 8290f95..6defda2 100644
--- a/include/pose.hpp
+++ b/include/pose.hpp
@@ -15,17 +15,11 @@ namespace QOSM::Pose{
using MatrixX3d = nc::NdArray<double>;
/**
- * @class VectorNx3
- * @brief Represents a collection of N 3D vectors stored as a matrix with N rows and 3 columns.
- *
- * This class extends MatrixX3d and provides various constructors for initializing vectors,
- * as well as operations for manipulating and processing sets of 3D vectors.
- * It supports element-wise arithmetic, row and column extraction, dot and cross products,
- * normalization, and transformations.
+ * @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.
*
- * @note Each row represents a distinct 3D vector, which can be a position vector, unit vector, or Poynting vector.
*/
- class VectorNx3 : public MatrixX3d {
+ class VectorNx3 : public MatrixX3d{
protected:
/// Number of points (number of rows)
size_type N;
@@ -34,152 +28,223 @@ namespace QOSM::Pose{
/**
* @brief Default constructor, initializes a 1x3 vector.
*/
- VectorNx3();
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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;
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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();
-
+ 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();
-
+ 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();
-
+ 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;
-
+ 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);
-
+ 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);
-
+ 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();
-
+ VectorNx3 flip(){
+ MatrixX3d res = *this * -1.;
+ return VectorNx3(res);
+ }
+
/** @brief Scalar multiplication. */
- friend VectorNx3 operator*(VectorNx3 lhs, const double &rhs);
- VectorNx3 &operator*=(const double &rhs);
-
+ friend VectorNx3 operator*(VectorNx3 lhs, const double &rhs){
+ auto res = lhs * rhs;
+ return VectorNx3(res);
+ }
+
/** @brief Scalar division. */
- friend VectorNx3 operator/(VectorNx3 lhs, const double &rhs);
- VectorNx3 &operator/=(const double &rhs);
-
+ VectorNx3 &operator*=(const double &rhs){
+ *this *= rhs;
+ return *this;
+ }
+
/** @brief Scalar addition. */
- friend VectorNx3 operator+(VectorNx3 lhs, const double &rhs);
- VectorNx3 &operator+=(const double &rhs);
-
+ friend VectorNx3 operator/(VectorNx3 lhs, const double &rhs){
+ auto res = lhs / rhs;
+ return VectorNx3(res);
+ }
+
/** @brief Scalar subtraction. */
- friend VectorNx3 operator-(VectorNx3 lhs, const double &rhs);
- VectorNx3 &operator-=(const double &rhs);
- };
+ 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
@@ -188,183 +253,385 @@ namespace QOSM::Pose{
/**
- * @class Vec3
- * @brief Represents a 3D vector with various arithmetic and utility operations.
- * It can represent a position vector, a unit vector or a Poyinting vector.
+ * @brief
+ * 3D Vector of doubles (x, y, z). It can represent a position vector, a unit vector or a Poyinting vector.
*
- * This class supports initialization from different data structures, arithmetic operations,
- * and vector operations like dot product, cross product, and normalization.
*/
- class Vec3 {
+ 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:
+ public:
/**
* @brief Default constructor initializing the vector to (0,0,0).
*/
- Vec3(double x=0., double y=0., double z=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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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);
-
+ 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;
-
+ 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;
-
+ 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;
-
+ 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();
-
+ 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;
-
+ 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;
-
- /**
- * @brief Overloaded arithmetic operators.
- */
- inline Vec3& operator+=(const Vec3& rhl);
- inline Vec3& operator-=(const Vec3& rhl);
- inline Vec3& operator*=(const Vec3& rhl);
- inline Vec3& operator/=(const Vec3& rhl);
- inline Vec3& operator+=(const double& rhl);
- inline Vec3& operator-=(const double& rhl);
- inline Vec3& operator*=(const double& rhl);
- inline Vec3& operator/=(const double& rhl);
- inline double &operator[](short dim);
- inline double operator[](short dim) const;
-
+ 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;
-
+ 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;
-
+ 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/maximum between two vectors.
- * @return A vector containing the element-wise min/max.
+ * @brief Computes the minimum between two vectors.
+ * @return A vector containing the element-wise min.
*/
- Vec3 minimum(const Vec3& other) const;
- Vec3 maximum(const Vec3& other) const;
-
+ 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 Finds the index of the minimum/maximum element.
- * @return Index of the minimum/maximum value.
+ * @brief Computes the maximum between two vectors.
+ * @return A vector containing the element-wise max.
*/
- int argmin() const;
- int argmax() const;
-
+ 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 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();
- double& gety();
- double& getz();
-
+ 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);
- void sety(double &val);
- void setz(double &val);
-
+ 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;
-
+ 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);
-
+ 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;
- };
-
+ 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).
*
@@ -375,11 +642,17 @@ namespace QOSM::Pose{
*
* @note The class uses a four-element vector (v4df) to store the quaternion components.
*/
- class Quaternion {
+ 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:
+ public:
/**
* @brief Default constructor, initializes a quaternion.
* @param qw Scalar (real) component, default is 1.
@@ -387,14 +660,24 @@ namespace QOSM::Pose{
* @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.);
-
+ 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);
+ 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.
@@ -402,118 +685,259 @@ namespace QOSM::Pose{
* @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);
+ 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);
+ 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);
+ 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);
-
+ 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);
-
+ 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);
-
+ 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;
-
+ inline Vec3 toVec3() const{
+ return {data[1], data[2], data[3]};
+ }
+
/** @brief Accessor for the scalar (real) part of the quaternion. */
- double& getw();
+ double& getw(){
+ return data[0];
+ }
+
/** @brief Accessor for the x-component of the quaternion. */
- double& getx();
+ double& getx(){
+ return data[1];
+ }
+
/** @brief Accessor for the y-component of the quaternion. */
- double& gety();
+ double& gety(){
+ return data[2];
+ }
+
/** @brief Accessor for the z-component of the quaternion. */
- double& getz();
-
+ 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;
-
+ 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;
-
+ 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;
-
+ 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;
-
+ 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();
-
+ 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;
-
+ 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;
-
+ 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;
-
+ 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;
-
+ 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;
- };
-
+ 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
@@ -523,7 +947,7 @@ namespace QOSM::Pose{
* orientation quaternion (`q_ref_ori`). It provides multiple constructors
* for initialization using initializer lists, arrays, or explicit parameters.
*/
- struct Pose {
+ struct Pose{
Vec3 ref_r_ref_ori; ///< Position vector (x, y, z) in reference frame.
Quaternion q_ref_ori; ///< Orientation as a quaternion.
@@ -555,11 +979,12 @@ namespace QOSM::Pose{
* @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) {}
+ 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
@@ -570,7 +995,7 @@ namespace QOSM::Pose{
/**
* @brief
- * Grid of point on a planar surface
+ * Grid of points on a planar surface
*/
class MeshGrid{
private:
diff --git a/setup-linux.py b/setup-linux.py
index b91a197..0a4988a 100644
--- a/setup-linux.py
+++ b/setup-linux.py
@@ -44,6 +44,8 @@ setup(
name="qosm_core",
cmdclass={"build_ext": build_ext},
ext_modules=ext_modules,
+ zip_safe=False,
+ python_requires=">=3.11",
)
# USAGE:
diff --git a/setup.py b/setup.py
index d759c0e..f43cc9b 100644
--- a/setup.py
+++ b/setup.py
@@ -39,6 +39,8 @@ setup(
name="qosm_core",
cmdclass={"build_ext": build_ext},
ext_modules=ext_modules,
+ zip_safe=False,
+ python_requires=">=3.11",
)
# USAGE:
--
GitLab