KOKO\Mimi
40 lines
1.7 KiB
C++
40 lines
1.7 KiB
C++
#ifndef FESA_SRC_FESA_MATH_JET_ARITHMETIC_H_
|
|
#define FESA_SRC_FESA_MATH_JET_ARITHMETIC_H_
|
|
|
|
#include <array>
|
|
#include <cstddef>
|
|
#include <utility>
|
|
|
|
namespace fesa::math_internal {
|
|
/// @brief Multiplies fixed Jet matrices in row/column/index order.
|
|
/// @note The caller retains its Jet precision, derivative storage, product rule
|
|
/// and constant provenance. Each result starts at its actual constant zero.
|
|
template <class Jet, class Constant>
|
|
std::array<std::array<Jet, 3>, 3> MultiplyJetMatrix(
|
|
const std::array<std::array<Jet, 3>, 3>& left,
|
|
const std::array<std::array<Jet, 3>, 3>& right, Constant constant) {
|
|
std::array<std::array<Jet, 3>, 3> result{};
|
|
for (std::size_t row = 0; row < 3; ++row)
|
|
for (std::size_t column = 0; column < 3; ++column) {
|
|
result[row][column] = constant(0.0);
|
|
for (std::size_t index = 0; index < 3; ++index)
|
|
result[row][column] = std::move(result[row][column]) +
|
|
left[row][index] * right[index][column];
|
|
}
|
|
return result;
|
|
}
|
|
/// @brief Multiplies a fixed Jet matrix and actual vector RHS in row order.
|
|
/// @note The zero-start component accumulation matches both Jet consumers.
|
|
template <class Jet, class Constant>
|
|
std::array<Jet, 3> MultiplyJetMatrixVector(
|
|
const std::array<std::array<Jet, 3>, 3>& matrix,
|
|
const std::array<Jet, 3>& vector, Constant constant) {
|
|
std::array<Jet, 3> result{constant(0.0),constant(0.0),constant(0.0)};
|
|
for (std::size_t row = 0; row < 3; ++row)
|
|
for (std::size_t column = 0; column < 3; ++column)
|
|
result[row] = std::move(result[row]) + matrix[row][column] * vector[column];
|
|
return result;
|
|
}
|
|
} // namespace fesa::math_internal
|
|
#endif // FESA_SRC_FESA_MATH_JET_ARITHMETIC_H_
|