Program Listing for File SpatialVector.h#
↰ Return to documentation for file (include/Karana/Math/SpatialVector.h)
/*
* Copyright (c) 2024-2026 Karana Dynamics Pty Ltd. All rights reserved.
*
* NOTICE TO USER:
*
* This source code and/or documentation (the "Licensed Materials") is
* the confidential and proprietary information of Karana Dynamics Inc.
* Use of these Licensed Materials is governed by the terms and conditions
* of a separate software license agreement between Karana Dynamics and the
* Licensee ("License Agreement"). Unless expressly permitted under that
* agreement, any reproduction, modification, distribution, or disclosure
* of the Licensed Materials, in whole or in part, to any third party
* without the prior written consent of Karana Dynamics is strictly prohibited.
*
* THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
* KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
* BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
* FITNESS FOR A PARTICULAR PURPOSE.
*
* IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
* INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
* ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
* OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
*
* U.S. Government End Users: The Licensed Materials are a "commercial item"
* as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
* only as a commercial end item under the terms of this license.
*
* Any use of the Licensed Materials in individual or commercial software must
* include, in the user documentation and internal source code comments,
* this Notice, Disclaimer, and U.S. Government Use Provision.
*/
/**
* @file
* @brief Contains the declarations for the SpatialVector class.
*/
#pragma once
#include "Karana/Math/Defs.h"
#include <boost/type_traits/integral_constant.hpp>
namespace Karana::Math {
// Accepts expressions that could represent an N-by-1 column vector.
template <typename Derived, Eigen::Index N>
concept CompatibleEigenColumnVector =
(Derived::RowsAtCompileTime == N or Derived::RowsAtCompileTime == Eigen::Dynamic) and
(Derived::ColsAtCompileTime == 1 or Derived::ColsAtCompileTime == Eigen::Dynamic);
/**
* @class SpatialVector
* @brief Represents a 6D spatial vector, split into angular (w) and linear (v) components.
*
* See \sref{spatial_notation_sec} section for more discussion on
* spatial notation.
*/
class SpatialVector {
// Constructors
public:
/**
* @brief Construct initialized to zero.
*/
SpatialVector();
/**
* @brief Construct using a 6 vector. The first 3 components will be put into w and
* the last 3 components will be put into v.
*
* @tparam Derived The derived Eigen type.
* @param sv The Eigen expression to copy into the SpatialVector.
* This should be a 6-element vector.
*/
template <typename Derived>
requires CompatibleEigenColumnVector<Derived, 6>
explicit SpatialVector(const Eigen::MatrixBase<Derived> &sv) {
// Validate only dimensions that were not established at compile time.
if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic ||
Derived::ColsAtCompileTime == Eigen::Dynamic) {
if (sv.rows() != 6 || sv.cols() != 1) {
throw std::invalid_argument("SpatialVector requires a 6x1 vector");
}
}
_v = sv;
}
/**
* @brief Construct using two 3-element vectors. The first will become w and the
* second will become v.
*
* @tparam WDerived The derived eigen type for w.
* @tparam VDerived The derived eigen type for v.
* @param w Angular portion of the spatial vector.
* @param v Linear portion of the spatial vector.
*/
template <typename WDerived, typename VDerived>
requires CompatibleEigenColumnVector<WDerived, 3> &&
CompatibleEigenColumnVector<VDerived, 3>
explicit SpatialVector(const Eigen::MatrixBase<WDerived> &w,
const Eigen::MatrixBase<VDerived> &v) {
// Validate the angular expression only when it has dynamic dimensions.
if constexpr (WDerived::RowsAtCompileTime == Eigen::Dynamic ||
WDerived::ColsAtCompileTime == Eigen::Dynamic) {
if (w.rows() != 3 || w.cols() != 1) {
throw std::invalid_argument(
"SpatialVector angular component requires a 3x1 vector");
}
}
// Validate the linear expression only when it has dynamic dimensions.
if constexpr (VDerived::RowsAtCompileTime == Eigen::Dynamic ||
VDerived::ColsAtCompileTime == Eigen::Dynamic) {
if (v.rows() != 3 || v.cols() != 1) {
throw std::invalid_argument(
"SpatialVector linear component requires a 3x1 vector");
}
}
// Evaluate each expression directly into its corresponding component.
_v.template head<3>() = w;
_v.template tail<3>() = v;
}
// Copy and move operations
public:
/**
* @brief Copy constructor.
*
* @param other The SpatialVector to copy from.
*/
SpatialVector(const SpatialVector &other);
/**
* @brief Copy assignment operator.
*
* @param other The SpatialVector to assign from.
* @return Reference to this object.
*/
SpatialVector &operator=(const SpatialVector &other);
/**
* @brief Copy assignment operator.
*
* @tparam Derived The derived Eigen type.
* @param expression The Eigen expression to copy into the SpatialVector.
* @return A reference to this SpatialVector.
*/
template <class Derived>
requires(
(Derived::RowsAtCompileTime == 6 || Derived::RowsAtCompileTime == Eigen::Dynamic) &&
(Derived::ColsAtCompileTime == 1 || Derived::ColsAtCompileTime == Eigen::Dynamic))
SpatialVector &operator=(const Eigen::MatrixBase<Derived> &expression) {
// Assign the expression directly without creating an intermediate Vec6.
_v = expression;
return *this;
}
/**
* @brief Move constructor.
*
* @param other The SpatialVector to move from.
*/
SpatialVector(SpatialVector &&other) noexcept;
/**
* @brief Move assignment operator.
*
* @param other The SpatialVector to move from.
* @return Reference to this object.
*/
SpatialVector &operator=(SpatialVector &&other) noexcept;
/**
* @brief Return object type as a string.
*
* @return String "SpatialVector".
*/
std::string_view typeString() const;
/**
* @brief Dump the object as a formatted string.
*
* @param prefix Optional prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
* @param skip_new_line If true, do not add a new line at the end
* @return Formatted string representation.
*/
std::string dumpString(std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT,
bool skip_new_line = false) const;
/**
* @brief Write dumpString to std::cout.
*
* @param prefix Optional prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
*/
void dump(std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT) const;
/**
* @brief Return true if the spatial vector is zero.
*
* @param prec Tolerance to use when comparing with zero.
* @return True if both angular and linear components are zero.
*/
bool isZero(double prec = MATH_EPSILON) const;
/**
* @brief Zero out the spatial vector's values.
*/
void setZero();
/**
* @brief Mark the spatial vector as not ready.
*/
void makeNotReady();
/**
* @brief Check if the spatial vector is initialized.
*
* @return True if both angular and linear components are initialized.
*/
bool isReady() const;
/**
* @brief noalias method to avoid Eigen temporary objects when possible.
*
* @return The noalias of the underlying km::Vec6
*/
[[nodiscard]] decltype(auto) noalias() noexcept { return _v.noalias(); }
// Operators
public:
/**
* @brief Equality operator.
*
* @param other Another SpatialVector to compare with.
* @return True if both components are equal.
*/
bool operator==(const SpatialVector &other) const;
/**
* @brief Scalar multiplication.
*
* @param scale Scalar value to multiply by.
* @return Scaled SpatialVector.
*/
SpatialVector operator*(double scale) const;
/**
* @brief Dot product with another spatial vector.
*
* @param other The SpatialVector to compute the dot product with.
* @return Dot product result.
*/
double operator*(const SpatialVector &other) const;
/**
* @brief Component-wise addition.
*
* @param other The SpatialVector to add to this one.
* @return Resulting SpatialVector sum.
*/
SpatialVector operator+(const SpatialVector &other) const;
/**
* @brief In-place component-wise addition.
*
* @param other The SpatialVector to add to this one in-place.
* @return Reference to this object.
*/
SpatialVector &operator+=(const SpatialVector &other);
/**
* @brief Negation operator.
*
* @return Negated SpatialVector.
*/
SpatialVector operator-() const;
/**
* @brief Subtract another SpatialVector.
*
* @param other The SpatialVector to subtract from this one.
* @return Resulting SpatialVector.
*/
SpatialVector operator-(const SpatialVector &other) const;
/**
* @brief In-place subtraction.
*
* @param other The SpatialVector to subtract from this one.
* @return Reference to this object.
*/
SpatialVector &operator-=(const SpatialVector &other);
/**
* @brief Add a Vec6 to this spatial vector.
*
* @param vec The 6D vector to add to this SpatialVector.
* @return Resulting SpatialVector.
*/
SpatialVector operator+(const Vec6 &vec) const;
/**
* @brief Subtract a Vec6 from this spatial vector.
*
* @param vec The 6D vector to subtract from this SpatialVector.
* @return Resulting SpatialVector.
*/
SpatialVector operator-(const Vec6 &vec) const;
// SpatialVector methods
public:
/**
* @brief Check if another spatial vector is approximately equal within precision.
*
* @param other The SpatialVector to compare to this one.
* @param prec Precision tolerance.
* @return True if approximately equal.
*/
bool isApprox(const SpatialVector &other, double prec = MATH_EPSILON) const;
/**
* @brief Get the angular portion of the spatial vector.
*
* @return Const reference to angular vector.
*/
Eigen::VectorBlock<const Vec6, 3> getw() const noexcept { return _v.head<3>(); }
/**
* @brief Set the angular portion of the spatial vector.
*
* @param w New angular vector.
*/
void setw(const Vec3 &w) { _v.head<3>() = w; }
/**
* @brief Get the linear portion of the spatial vector.
*
* @return Const reference to linear vector.
*/
Eigen::VectorBlock<const Vec6, 3> getv() const noexcept { return _v.tail<3>(); }
/**
* @brief Set the linear portion of the spatial vector.
*
* @param v New linear vector.
*/
void setv(const Vec3 &v) { _v.tail<3>() = v; }
/**
* @brief Convert to a 6-vector by combining the w and v parts as [w;v].
*
* @return Combined 6D vector.
*/
const Vec6 &toVector6() const noexcept { return _v; }
/**
* @brief Convert to a 6-vector by combining the w and v parts as [w;v].
*
* @return Combined 6D vector.
*/
Vec6 &toVector6() noexcept { return _v; }
/**
* @brief Compute 6-D cross product with another spatial vector.
*
* \f[
* A \times B = \tilde A B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 & \tilde A_w
* \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix}
* \f]
*
* @param other The SpatialVector to compute the cross product with.
* @return Resulting SpatialVector.
*/
SpatialVector cross(const SpatialVector &other);
/**
* @brief Compute 6-D cross product with another spatial vector.
*
* \f[
* \tilde A = \begin{bmatrix} \tilde A_w & 0 \\ \tilde A_v & \tilde A_w
* \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix}
* \f]
*
* @return Resulting SpatialVector.
*/
Mat66 tilde() const;
/**
* @brief Compute the bar product with another spatial vector.
*
* \f[ \bar A B = - {\tilde A}^* B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 &
* \tilde A_w \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix} \f]
*
* @param other The SpatialVector to compute the bar product with.
* @return Resulting SpatialVector.
*/
SpatialVector barprod(const SpatialVector &other);
/**
* @brief Multiply from the vector from left with a matrix, i.e v * mat.
*
* @param mat Matrix to multiply.
* @return Resulting SpatialVector.
*/
SpatialVector multiplyFromLeft(const Mat66 &mat) const;
protected:
/// The 6 element spatial vector
Vec6 _v;
};
/**
* @class SpatialVelocity
* @brief A velocity SpatialVector.
*/
class SpatialVelocity : public SpatialVector {
public:
using SpatialVector::SpatialVector;
/**
* @brief Copy constructor.
*
* @param other The SpatialVelocity to copy from.
*/
SpatialVelocity(const SpatialVelocity &other);
/**
* @brief Copy assignment operator.
*
* @param other The SpatialVelocity to assign from.
* @return Reference to this object.
*/
SpatialVelocity &operator=(const SpatialVelocity &other);
/**
* @brief Copy assignment operator.
*
* @tparam Derived The derived Eigen type.
* @param expression The Eigen expression to copy into the SpatialVelocity.
* @return A reference to this SpatialVelocity.
*/
template <class Derived>
requires(
(Derived::RowsAtCompileTime == 6 || Derived::RowsAtCompileTime == Eigen::Dynamic) &&
(Derived::ColsAtCompileTime == 1 || Derived::ColsAtCompileTime == Eigen::Dynamic))
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialVelocity &operator=(const Eigen::MatrixBase<Derived> &expression) {
// Assign the expression directly without creating an intermediate Vec6.
_v = expression;
return *this;
}
/**
* @brief Move constructor.
*
* @param other The SpatialVelocity to move from.
*/
SpatialVelocity(SpatialVelocity &&other) noexcept;
/**
* @brief Move assignment operator.
*
* @param other The SpatialVelocity to move from.
* @return Reference to this object.
*/
SpatialVelocity &operator=(SpatialVelocity &&other) noexcept;
/**
* @brief Scalar multiplication.
*
* @param scale Scalar value to multiply by.
* @return Scaled SpatialVelocity.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialVelocity operator*(double scale) const;
/**
* @brief Component-wise addition.
*
* @param other The SpatialVelocity to add to this one.
* @return Resulting SpatialVelocity sum.
*/
SpatialVelocity operator+(const SpatialVelocity &other) const;
/**
* @brief In-place component-wise addition.
*
* @param other The SpatialVelocity to add to this one in-place.
* @return Reference to this object.
*/
SpatialVelocity &operator+=(const SpatialVelocity &other);
/**
* @brief Negation operator.
*
* @return Negated SpatialVelocity.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialVelocity operator-() const;
/**
* @brief Subtract another SpatialVelocity.
*
* @param other The SpatialVelocity to subtract from this one.
* @return Resulting SpatialVelocity.
*/
SpatialVelocity operator-(const SpatialVelocity &other) const;
/**
* @brief In-place subtraction.
*
* @param other The SpatialVelocity to subtract from this one.
* @return Reference to this object.
*/
SpatialVelocity &operator-=(const SpatialVelocity &other);
/**
* @brief Add a Vec6 to this spatial velocity.
*
* @param vec The 6D velocity to add to this SpatialVelocity.
* @return Resulting SpatialVelocity.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialVelocity operator+(const Vec6 &vec) const;
/**
* @brief Subtract a Vec6 from this spatial velocity.
*
* @param vec The 6D velocity to subtract from this SpatialVelocity.
* @return Resulting SpatialVelocity.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialVelocity operator-(const Vec6 &vec) const;
/**
* @brief Compute 6-D cross product with another spatial velocity.
*
* \f[
* A \times B = \tilde A B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 & \tilde A_w
* \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix}
* \f]
*
* @param other The SpatialVelocity to compute the cross product with.
* @return Resulting SpatialVelocity.
*/
SpatialVelocity cross(const SpatialVelocity &other);
/**
* @brief Compute the bar product with another spatial velocity.
*
* \f[ \bar A B = - {\tilde A}^* B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 &
* \tilde A_w \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix} \f]
*
* @param other The SpatialVelocity to compute the bar product with.
* @return Resulting SpatialVelocity.
*/
SpatialVelocity barprod(const SpatialVelocity &other);
/**
* @brief Multiply the velocity from left with a matrix, i.e v * mat.
*
* @param mat Matrix to multiply.
* @return Resulting SpatialVelocity.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialVelocity multiplyFromLeft(const Mat66 &mat) const;
};
/**
* @class SpatialAcceleration
* @brief A velocity SpatialVector.
*/
class SpatialAcceleration : public SpatialVector {
public:
using SpatialVector::SpatialVector;
/**
* @brief Copy constructor.
*
* @param other The SpatialAcceleration to copy from.
*/
SpatialAcceleration(const SpatialAcceleration &other);
/**
* @brief Copy assignment operator.
*
* @param other The SpatialAcceleration to assign from.
* @return Reference to this object.
*/
SpatialAcceleration &operator=(const SpatialAcceleration &other);
/**
* @brief Copy assignment operator.
*
* @tparam Derived The derived Eigen type.
* @param expression The Eigen expression to copy into the SpatialAcceleration.
* @return A reference to this SpatialAcceleration.
*/
template <class Derived>
requires(
(Derived::RowsAtCompileTime == 6 || Derived::RowsAtCompileTime == Eigen::Dynamic) &&
(Derived::ColsAtCompileTime == 1 || Derived::ColsAtCompileTime == Eigen::Dynamic))
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialAcceleration &operator=(const Eigen::MatrixBase<Derived> &expression) {
// Assign the expression directly without creating an intermediate Vec6.
_v = expression;
return *this;
}
/**
* @brief Move constructor.
*
* @param other The SpatialAcceleration to move from.
*/
SpatialAcceleration(SpatialAcceleration &&other) noexcept;
/**
* @brief Move assignment operator.
*
* @param other The SpatialAcceleration to move from.
* @return Reference to this object.
*/
SpatialAcceleration &operator=(SpatialAcceleration &&other) noexcept;
/**
* @brief Scalar multiplication.
*
* @param scale Scalar value to multiply by.
* @return Scaled SpatialAcceleration.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialAcceleration operator*(double scale) const;
/**
* @brief Component-wise addition.
*
* @param other The SpatialAcceleration to add to this one.
* @return Resulting SpatialAcceleration sum.
*/
SpatialAcceleration operator+(const SpatialAcceleration &other) const;
/**
* @brief In-place component-wise addition.
*
* @param other The SpatialAcceleration to add to this one in-place.
* @return Reference to this object.
*/
SpatialAcceleration &operator+=(const SpatialAcceleration &other);
/**
* @brief Negation operator.
*
* @return Negated SpatialAcceleration.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialAcceleration operator-() const;
/**
* @brief Subtract another SpatialAcceleration.
*
* @param other The SpatialAcceleration to subtract from this one.
* @return Resulting SpatialAcceleration.
*/
SpatialAcceleration operator-(const SpatialAcceleration &other) const;
/**
* @brief In-place subtraction.
*
* @param other The SpatialAcceleration to subtract from this one.
* @return Reference to this object.
*/
SpatialAcceleration &operator-=(const SpatialAcceleration &other);
/**
* @brief Add a Vec6 to this spatial acceleration.
*
* @param vec The 6D acceleration to add to this SpatialAcceleration.
* @return Resulting SpatialAcceleration.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialAcceleration operator+(const Vec6 &vec) const;
/**
* @brief Subtract a Vec6 from this spatial acceleration.
*
* @param vec The 6D acceleration to subtract from this SpatialAcceleration.
* @return Resulting SpatialAcceleration.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialAcceleration operator-(const Vec6 &vec) const;
/**
* @brief Compute 6-D cross product with another spatial acceleration.
*
* \f[
* A \times B = \tilde A B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 & \tilde A_w
* \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix}
* \f]
*
* @param other The SpatialAcceleration to compute the cross product with.
* @return Resulting SpatialAcceleration.
*/
SpatialAcceleration cross(const SpatialAcceleration &other);
/**
* @brief Compute the bar product with another spatial acceleration.
*
* \f[ \bar A B = - {\tilde A}^* B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 &
* \tilde A_w \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix} \f]
*
* @param other The SpatialAcceleration to compute the bar product with.
* @return Resulting SpatialAcceleration.
*/
SpatialAcceleration barprod(const SpatialAcceleration &other);
/**
* @brief Multiply the acceleration from left with a matrix, i.e v * mat.
*
* @param mat Matrix to multiply.
* @return Resulting SpatialAcceleration.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialAcceleration multiplyFromLeft(const Mat66 &mat) const;
};
/**
* @class SpatialForce
* @brief A force SpatialVector.
*/
class SpatialForce : public SpatialVector {
public:
using SpatialVector::SpatialVector;
/**
* @brief Copy constructor.
*
* @param other The SpatialForce to copy from.
*/
SpatialForce(const SpatialForce &other);
/**
* @brief Copy assignment operator.
*
* @param other The SpatialForce to assign from.
* @return Reference to this object.
*/
SpatialForce &operator=(const SpatialForce &other);
/**
* @brief Copy assignment operator.
*
* @tparam Derived The derived Eigen type.
* @param expression The Eigen expression to copy into the SpatialForce.
* @return A reference to this SpatialForce.
*/
template <class Derived>
requires(
(Derived::RowsAtCompileTime == 6 || Derived::RowsAtCompileTime == Eigen::Dynamic) &&
(Derived::ColsAtCompileTime == 1 || Derived::ColsAtCompileTime == Eigen::Dynamic))
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialForce &operator=(const Eigen::MatrixBase<Derived> &expression) {
// Assign the expression directly without creating an intermediate Vec6.
_v = expression;
return *this;
}
/**
* @brief Move constructor.
*
* @param other The SpatialForce to move from.
*/
SpatialForce(SpatialForce &&other) noexcept;
/**
* @brief Move assignment operator.
*
* @param other The SpatialForce to move from.
* @return Reference to this object.
*/
SpatialForce &operator=(SpatialForce &&other) noexcept;
/**
* @brief Scalar multiplication.
*
* @param scale Scalar value to multiply by.
* @return Scaled SpatialForce.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialForce operator*(double scale) const;
/**
* @brief Component-wise addition.
*
* @param other The SpatialForce to add to this one.
* @return Resulting SpatialForce sum.
*/
SpatialForce operator+(const SpatialForce &other) const;
/**
* @brief In-place component-wise addition.
*
* @param other The SpatialForce to add to this one in-place.
* @return Reference to this object.
*/
SpatialForce &operator+=(const SpatialForce &other);
/**
* @brief Negation operator.
*
* @return Negated SpatialForce.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialForce operator-() const;
/**
* @brief Subtract another SpatialForce.
*
* @param other The SpatialForce to subtract from this one.
* @return Resulting SpatialForce.
*/
SpatialForce operator-(const SpatialForce &other) const;
/**
* @brief In-place subtraction.
*
* @param other The SpatialForce to subtract from this one.
* @return Reference to this object.
*/
SpatialForce &operator-=(const SpatialForce &other);
/**
* @brief Add a Vec6 to this spatial force.
*
* @param vec The 6D force to add to this SpatialForce.
* @return Resulting SpatialForce.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialForce operator+(const Vec6 &vec) const;
/**
* @brief Subtract a Vec6 from this spatial force.
*
* @param vec The 6D force to subtract from this SpatialForce.
* @return Resulting SpatialForce.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialForce operator-(const Vec6 &vec) const;
/**
* @brief Compute 6-D cross product with another spatial force.
*
* \f[
* A \times B = \tilde A B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 & \tilde A_w
* \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix}
* \f]
*
* @param other The SpatialForce to compute the cross product with.
* @return Resulting SpatialForce.
*/
SpatialForce cross(const SpatialForce &other);
/**
* @brief Compute the bar product with another spatial force.
*
* \f[ \bar A B = - {\tilde A}^* B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 &
* \tilde A_w \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix} \f]
*
* @param other The SpatialForce to compute the bar product with.
* @return Resulting SpatialForce.
*/
SpatialForce barprod(const SpatialForce &other);
/**
* @brief Multiply the force from left with a matrix, i.e v * mat.
*
* @param mat Matrix to multiply.
* @return Resulting SpatialForce.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialForce multiplyFromLeft(const Mat66 &mat) const;
};
/**
* @class SpatialMomentum
* @brief A momentum SpatialVector.
*/
class SpatialMomentum : public SpatialVector {
public:
using SpatialVector::SpatialVector;
/**
* @brief Copy constructor.
*
* @param other The SpatialMomentum to copy from.
*/
SpatialMomentum(const SpatialMomentum &other);
/**
* @brief Copy assignment operator.
*
* @param other The SpatialMomentum to assign from.
* @return Reference to this object.
*/
SpatialMomentum &operator=(const SpatialMomentum &other);
/**
* @brief Copy assignment operator.
*
* @tparam Derived The derived Eigen type.
* @param expression The Eigen expression to copy into the SpatialMomentum.
* @return A reference to this SpatialMomentum.
*/
template <class Derived>
requires(
(Derived::RowsAtCompileTime == 6 || Derived::RowsAtCompileTime == Eigen::Dynamic) &&
(Derived::ColsAtCompileTime == 1 || Derived::ColsAtCompileTime == Eigen::Dynamic))
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialMomentum &operator=(const Eigen::MatrixBase<Derived> &expression) {
// Assign the expression directly without creating an intermediate Vec6.
_v = expression;
return *this;
}
/**
* @brief Move constructor.
*
* @param other The SpatialMomentum to move from.
*/
SpatialMomentum(SpatialMomentum &&other) noexcept;
/**
* @brief Move assignment operator.
*
* @param other The SpatialMomentum to move from.
* @return Reference to this object.
*/
SpatialMomentum &operator=(SpatialMomentum &&other) noexcept;
/**
* @brief Scalar multiplication.
*
* @param scale Scalar value to multiply by.
* @return Scaled SpatialMomentum.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialMomentum operator*(double scale) const;
/**
* @brief Component-wise addition.
*
* @param other The SpatialMomentum to add to this one.
* @return Resulting SpatialMomentum sum.
*/
SpatialMomentum operator+(const SpatialMomentum &other) const;
/**
* @brief In-place component-wise addition.
*
* @param other The SpatialMomentum to add to this one in-place.
* @return Reference to this object.
*/
SpatialMomentum &operator+=(const SpatialMomentum &other);
/**
* @brief Negation operator.
*
* @return Negated SpatialMomentum.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialMomentum operator-() const;
/**
* @brief Subtract another SpatialMomentum.
*
* @param other The SpatialMomentum to subtract from this one.
* @return Resulting SpatialMomentum.
*/
SpatialMomentum operator-(const SpatialMomentum &other) const;
/**
* @brief In-place subtraction.
*
* @param other The SpatialMomentum to subtract from this one.
* @return Reference to this object.
*/
SpatialMomentum &operator-=(const SpatialMomentum &other);
/**
* @brief Add a Vec6 to this spatial momentum.
*
* @param vec The 6D momentum to add to this SpatialMomentum.
* @return Resulting SpatialMomentum.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialMomentum operator+(const Vec6 &vec) const;
/**
* @brief Subtract a Vec6 from this spatial momentum.
*
* @param vec The 6D momentum to subtract from this SpatialMomentum.
* @return Resulting SpatialMomentum.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialMomentum operator-(const Vec6 &vec) const;
/**
* @brief Compute 6-D cross product with another spatial momentum.
*
* \f[
* A \times B = \tilde A B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 & \tilde A_w
* \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix}
* \f]
*
* @param other The SpatialMomentum to compute the cross product with.
* @return Resulting SpatialMomentum.
*/
SpatialMomentum cross(const SpatialMomentum &other);
/**
* @brief Compute the bar product with another spatial momentum.
*
* \f[ \bar A B = - {\tilde A}^* B = \begin{bmatrix} \tilde A_w & \tilde A_v \\ 0 &
* \tilde A_w \end{bmatrix} \begin{bmatrix} B_w \\ B_v \end{bmatrix} \f]
*
* @param other The SpatialMomentum to compute the bar product with.
* @return Resulting SpatialMomentum.
*/
SpatialMomentum barprod(const SpatialMomentum &other);
/**
* @brief Multiply the momentum from left with a matrix, i.e v * mat.
*
* @param mat Matrix to multiply.
* @return Resulting SpatialMomentum.
*/
// Adding suppression, as CodeChecker complains this method has the same name as a method in
// SpatialVector. This is just a CodeChecker false positive.
// codechecker_suppress [cppcheck-duplInheritedMember]
SpatialMomentum multiplyFromLeft(const Mat66 &mat) const;
};
} // namespace Karana::Math