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
// 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.
* @return Formatted string for this matrix.
*/
template <typename Derived>
std::string dumpString(const Eigen::MatrixBase<Derived> &mat,
std::string_view prefix = "",
unsigned int precision = 10,
DumpFormatType format_type = DumpFormatType::DEFAULT_FLOAT,
bool show_size = true) {
const Eigen::IOFormat prettyFormat(precision, 0, ", ", "\n", "[", "]", "", "");
// 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 << "=";
}
} 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) << "\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::MatrixBase<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 column 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>
std::vector<unsigned int> 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);
std::vector<unsigned int> result(indices.data(), indices.data() + fullrank);
return 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<std::vector<unsigned int>, std::vector<unsigned int>>
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);
std::vector<unsigned int> rows(rindices.data(), rindices.data() + fullrank);
std::vector<unsigned int> cols(cindices.data(), cindices.data() + fullrank);
return std::pair<std::vector<unsigned int>, std::vector<unsigned int>>(rows, cols);
}
/**
* @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