Program Listing for File Defs.h#
↰ Return to documentation for file (include/Karana/Math/Defs.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 Basic typedefs and definitions for the Math module
*/
#pragma once
#include <bit>
#include <cmath>
#include <ios>
#include <iostream>
/* for use of gcc specific fenableexcept */
#if defined(PYBIND11_MKDOC_SKIP) || (defined(__GNUC__) && !defined(__clang__))
#include <fenv.h>
#endif
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <chrono>
namespace Karana::Math {
/** the type to use for time values */
using Ktime = std::chrono::nanoseconds;
/**
* @brief Convert a seconds (double) time value to Ktime
*
* @param t The time in seconds to convert.
* @return The input time in seconds converted to a Ktime.
* */
Ktime secondsToKtime(double t);
/**
* @brief Convert a Ktime value to a double (units in seconds).
*
* @param kt The Ktime value to convert.
* @return The converted time.
* */
double ktimeToSeconds(const Ktime &kt);
/**
* @brief Create a NaN double with a particular mantissa payload
*
* @param payload The payload
* @return The NaN double with given mantissa payload
* */
constexpr double buildNaNWithPayload(uint64_t payload) {
return std::bit_cast<double>((0x7FFull << 52) | (1ull << 51) |
(payload & ((1ull << 51) - 1)));
}
/**
* @brief Whether a double is a NaN with a given mantissa payload
*
* @param candidate The candidate double to check.
* @param payload The payload to check for.
* @return Whether candidate is a NaN with the given payload.
* */
constexpr bool isNaNWithPayload(const double &candidate, uint64_t payload) {
if (!std::isnan(candidate)) {
return false;
}
auto bits = std::bit_cast<uint64_t>(candidate);
constexpr uint64_t payload_mask = (1ull << 51) - 1;
return (bits & payload_mask) == (payload & payload_mask);
}
/**
* A sentinel payload value to distinguished NaNs indicating an
* unspecified value from other NaNs. The value of 1954 was chosen
* to match the one used for NA constants in the R language.
*/
inline constexpr uint64_t notReadyPayload = 1954;
/// NaN with a specific mantissa payload indicating a not ready value
inline constexpr double notReadyNaN = buildNaNWithPayload(notReadyPayload);
/**
* @brief Whether a double is a NaN indicating a not ready value
*
* @param candidate The candidate double to check.
* @return Whether candidate is a NaN indicating a not ready value.
* */
constexpr bool isNotReadyNaN(const double &candidate) {
return isNaNWithPayload(candidate, notReadyPayload);
}
/**
* @brief Uninitialize a Ktime value.
*
* @param kt The Ktime value to makeNotReady.
*/
void makeNotReady(Ktime &kt);
/**
* @brief Check if a Ktime value is initialized.
*
* @param kt The Ktime value to check for initialization.
* @return `true` if the input Ktime is not ready, `false` otherwise.
*/
bool isReady(const Ktime &kt);
/// Controls how floating-point values are formatted when dumping.
enum class DumpFormatType {
/// Use std::defaultfloat, which uses as many meaningful digits as needed up to the set
/// precision.
/// This will print in scientific or fixed notation, depending on the number.
DEFAULT_FLOAT,
FIXED, ///< Use std::fixed, which prints a fixed number of digits.
SCIENTIFIC, ///< Use std::scientific, which prints the numbers in scientific notation.
};
using namespace Eigen;
// Vector definitions
using Vec = VectorXd; ///< Variable length double vector
using Vec3 = Vector3d; ///< 3-vector of type double
using Vec4 = Vector4d; ///< 4-vector of type double
using Vec6 = Matrix<double, 6, 1>; ///< 6-vector of type double
using VecInt = VectorXi; ///< Variable length integers vector
// Matrix definitions. We use RowMajor, since this is standard for C++.
using Mat = Matrix<double, Dynamic, Dynamic, RowMajor>; ///< Variable length double matrix
using Mat33 = Matrix<double, 3, 3, RowMajor>; ///< Size 3x3 double matrix
using Mat44 = Matrix<double, 4, 4, RowMajor>; ///< Size 4x4 double matrix
using Mat66 = Matrix<double, 6, 6, RowMajor>; ///< Size 6x6 double matrix
using Mat6n = Matrix<double, 6, Dynamic, RowMajor>; ///< Size 6xn double matrix
// Vector array definitions
using ArrayVec = ArrayXd; ///< Variable length double array
using Array = Array<double, Dynamic, Dynamic, RowMajor>; ///< Variable length double array
// Slice definitions
// These should be preferred instead of & or const& to support slices without creating a copy
using VecSlice = Ref<Vec>; ///< Slice of a Vec
using ConstVecSlice = const Ref<const Vec>; ///< Read-only slice of a Vec
using Vec3Slice = Ref<Vec3>; ///< Slice of a Vec3
using ConstVec3Slice = const Ref<const Vec3>; ///< Read-only slice of a Vec3
using MatSlice = Ref<Mat>; ///< Slice of a Mat
using ConstMatSlice = const Ref<const Mat>; ///< Read-only slice of a Mat
/// Default tolerance value used, equal to Eigen's dummy_precision
#define MATH_EPSILON Eigen::NumTraits<double>::dummy_precision()
/**
* @brief Calculate the 3x3 skew cross-product matrix for the input vector.
*
* @param v The vector to compute the skew cross-product matrix for.
* @return The skew cross-product matrix.
*/
Mat33 tilde(const Vec3 &v);
// Run only with g++ or when pybind11-mkdoc is running. Skip clang otherwise.
#if defined(PYBIND11_MKDOC_SKIP) || (defined(__GNUC__) && !defined(__clang__))
/**
* @brief Function to enable/disable exceptions when there are floating point errors
*
* This method is handy for tracking down the source of NaNs by
* using a source debugger to break on exceptions. This method is
* specifically for gcc compiled code.
*
* @param enable If true, floating point exceptions will be raised, else disabled
*/
void floatingPointExceptions(bool enable);
#endif
/**
* @brief Set the elements of the DenseBase with NAN values to set it to not ready state.
*
* @param base The Eigen DenseBase object to makeNotReady.
*/
template <typename Derived> void makeNotReady(Eigen::DenseBase<Derived> &base) {
base.setConstant(notReadyNaN);
}
/**
* @brief Check if the incoming matrix is initialized.
*
* @param mat The matrix to check if is initialized.
* @return `true` if the matrix is initialized, `false` otherwise.
*/
template <typename Derived> bool isReady(const Eigen::MatrixBase<Derived> &mat) {
return not mat.array().unaryExpr([](double x) { return isNotReadyNaN(x); }).any();
}
/**
* @brief Check if the incoming matrix is initialized.
*
* @param array The ArrayBase to check if is initialized.
* @return `true` if the matrix is initialized, `false` otherwise.
*/
template <typename Derived> bool isReady(const Eigen::ArrayBase<Derived> &array) {
return not array.unaryExpr([](double x) { return isNotReadyNaN(x); }).any();
}
/**
* @brief LCP solver based on Projected Gauss-Siedel (PGS) method
*
* @param M the matrix
* @param q the vector
* @param max_iter the maximum number of iterations
* @param tolerance the tolerance value
* @return the solution vector
*/
Vec
solveLcpPgs(const Mat &M, const Vec &q, unsigned int max_iter = 1000, double tolerance = 1e-6);
/**
* @brief LCP solver based on Projected Successive Over-Relaxation (PSOR) method
*
* @param M the matrix
* @param q the vector
* @param omega Relaxation parameter (typically 1.0 < omega < 2.0)
* @param max_iter the maximum number of iterations
* @param tolerance the tolerance value
* @return the solution vector
*/
Vec solveLcpPsor(const Mat &M,
const Vec &q,
double omega = 1.2,
unsigned int max_iter = 1000,
double tolerance = 1e-6);
/**
* @brief Return a pretty formatted string with values from the vector/matrix.
* A newline is not added at the end is added only for matrices and not for vectors.
*
* @param mat The matrix to get the formatted string for.
* @param prefix String prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
* @param show_size If true, the size of the array is shown.
* @param skip_new_line If true, the new line at the end of matrix output is skipped
* @return Formatted string for this matrix.
*/
template <typename Derived>
std::string dumpString(const Eigen::DenseBase<Derived> &mat,
std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT,
bool show_size = true,
bool skip_new_line = false) {
std::string row_prefix = "["; // Vector
std::string mat_prefix = ""; // Vector
int alignment = Eigen::DontAlignCols;
if (mat.cols() > 1) {
// Matrix
row_prefix = std::format("{} [", prefix);
// mat_prefix = "=";
alignment = 0;
}
/*
precision number of digits for floating point values, or one of the
special constants StreamPrecision and FullPrecision. The default is the
special value StreamPrecision which means to use the stream's own
precision setting, as set for instance using cout.precision(3). The other
special value FullPrecision means that the number of digits will be
computed to match the full precision of each floating-point type.
flags an OR-ed combination of flags, the default value is 0, the only
currently available flag is DontAlignCols which allows to disable the
alignment of columns, resulting in faster code.
coeffSeparator string printed between two coefficients of the same row
rowSeparator string printed between two rows
rowPrefix string printed at the beginning of each row
rowSuffix string printed at the end of each row
matPrefix string printed at the beginning of the matrix
matSuffix string printed at the end of the matrix
fill character printed to fill the empty space in aligned columns
*/
const Eigen::IOFormat prettyFormat(
precision, alignment, ", ", "\n", row_prefix, "]", mat_prefix, "");
// const Eigen::IOFormat prettyFormat(precision, 0, ", ", "\n",
// std::format("{}[", prefix),
// "]", "", "");
std::ostringstream oss;
switch (format_type) {
case DumpFormatType::DEFAULT_FLOAT:
oss << std::defaultfloat;
break;
case DumpFormatType::SCIENTIFIC:
oss << std::scientific;
break;
case DumpFormatType::FIXED:
oss << std::fixed;
break;
default:
throw std::logic_error("Unexpected DumpFormatType value in "
"Eigen::MatrixBase dumpString\n");
}
if (not show_size) {
if (!prefix.empty()) {
// oss << prefix << "=";
oss << prefix;
if (not skip_new_line) {
/* backdoor to get rid of '=' from Var_T dump output for vectors and
* matrices */
oss << "=";
}
}
} else // showing size
{
if (not prefix.empty()) // prefix
{
oss << prefix;
if (mat.cols() == 1) // Vector
oss << std::format(" <{}>: ", mat.rows());
else
oss << std::format(" <{}x{}>: ", mat.rows(), mat.cols());
} else // no prefix
{
if (mat.cols() == 1) // Vector
oss << std::format("<{}>: ", mat.rows());
else
oss << std::format("<{}x{}>: ", mat.rows(), mat.cols());
}
}
if (mat.cols() == 1) // Vector
oss << mat.transpose().format(prettyFormat); // << "\n";
else {
// Matrix
oss << "\\ \n" << mat.format(prettyFormat);
if (not skip_new_line)
oss << "\n";
}
// oss << std::setfill('-') << std::setw(80) << "-" << "\n";
return oss.str();
}
/**
* @brief Print dumpString to std::cout.
*
* @param mat The matrix to print dumpString for.
* @param prefix String prefix for each line.
* @param precision Number of digits to use for floating point values.
* @param format_type The format type to use.
* @param show_size If true, the size of the array is shown.
*/
template <typename Derived>
void dump(const Eigen::DenseBase<Derived> &mat,
std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT,
bool show_size = true) {
std::cout << dumpString(mat, prefix, precision, format_type, show_size);
if (mat.cols() == 1)
std::cout << std::endl;
}
/**
* @brief Return the rank of a matrix
*
* @param mat The input matrix
* @param threshold the threshold to use for rank check
* @return the matrix rank
*/
template <typename Derived>
unsigned int getRank(const Eigen::MatrixBase<Derived> &mat, double threshold = 1e-12) {
// Perform QR decomposition with full pivoting
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(mat);
qr.setThreshold(threshold);
return qr.rank();
}
/**
* @brief Return the list of indices of independent columns in the input matrix
*
* @param mat The input matrix
* @param threshold the threshold to use for rank check
* @return the list of indices of independent columns
*/
template <typename Derived>
VecInt getIndepColIndices(const Eigen::MatrixBase<Derived> &mat, double threshold = 1e-12) {
// Perform QR decomposition with column pivoting
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(mat);
qr.setThreshold(threshold);
unsigned int fullrank = qr.rank();
auto indices = qr.colsPermutation().indices().head(fullrank);
// VecInt result(indices.data(), indices.data() + fullrank);
return indices; // result;
}
/**
* @brief Return the pair of indices lists of indices of independent rows and columns in the
* input matrix
*
* @param mat The input matrix
* @param threshold the threshold to use for rank check
* @return the pair of indices lists of independent rows and columns
*/
template <typename Derived>
std::pair<VecInt, VecInt> getIndepRowColIndices(const Eigen::MatrixBase<Derived> &mat,
double threshold = 1e-12) {
// Perform LU decomposition with column pivoting
Eigen::FullPivLU<Eigen::MatrixXd> lu(mat);
lu.setThreshold(threshold);
unsigned int fullrank = lu.rank();
auto rindices = lu.permutationP().indices().head(fullrank);
auto cindices = lu.permutationQ().indices().head(fullrank);
// VecInt rows(rindices.data(), rindices.data() + fullrank);
// VecInt cols(cindices.data(), cindices.data() + fullrank);
// return std::pair<VecInt, VecInt>(rows, cols);
return std::pair<VecInt, VecInt>(rindices, cindices);
}
/**
* @brief Return the singular values for a matrix
*
* @param mat The input matrix
* @return the singular values
*/
template <typename Derived> Vec getSingularValues(const Eigen::MatrixBase<Derived> &mat) {
// Perform QR decomposition with column pivoting
Eigen::JacobiSVD<Eigen::MatrixXd> svd(mat, Eigen::ComputeThinU | Eigen::ComputeFullV);
return svd.singularValues();
}
} // namespace Karana::Math