KOKO\Mimi
58 lines
1.9 KiB
C++
58 lines
1.9 KiB
C++
#ifndef FESA_MATH_VECTOR3_OPERATIONS_H_
|
|
#define FESA_MATH_VECTOR3_OPERATIONS_H_
|
|
|
|
#include <array>
|
|
#include <cstddef>
|
|
|
|
#include "fesa/math/matrix.h"
|
|
#include "fesa/math/vector3.h"
|
|
|
|
namespace fesa::math_internal {
|
|
|
|
/// @brief Reduces four source-ordered geometric vectors with scalar-left products.
|
|
inline Vector3 WeightedSum(const std::array<double, 4>& weights,
|
|
const std::array<Vector3, 4>& values) {
|
|
Vector3 result{};
|
|
for (std::size_t node = 0U; node < 4U; ++node) {
|
|
result = result + weights[node] * values[node];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/// @brief Cancels the first-node anchor before the same ordered reduction.
|
|
inline Vector3 DerivativeSum(const std::array<double, 4>& derivatives,
|
|
const std::array<Vector3, 4>& values) {
|
|
std::array<Vector3, 4> relative{};
|
|
for (std::size_t node = 0U; node < 4U; ++node) {
|
|
relative[node] = values[node] - values[0U];
|
|
}
|
|
return WeightedSum(derivatives, relative);
|
|
}
|
|
|
|
/// @brief Forms the positive cross-product matrix in fixed Cartesian coordinates.
|
|
inline Matrix Skew(const Vector3& vector) {
|
|
Matrix result{3U, 3U};
|
|
result(0U, 1U) = -vector.Z();
|
|
result(0U, 2U) = vector.Y();
|
|
result(1U, 0U) = vector.Z();
|
|
result(1U, 2U) = -vector.X();
|
|
result(2U, 0U) = -vector.Y();
|
|
result(2U, 1U) = vector.X();
|
|
return result;
|
|
}
|
|
|
|
/// @brief Applies a fixed 3-by-3 geometric frame with the original component graph.
|
|
inline Vector3 Multiply(const Matrix& matrix, const Vector3& vector) {
|
|
return Vector3{
|
|
matrix(0U, 0U) * vector.X() + matrix(0U, 1U) * vector.Y() +
|
|
matrix(0U, 2U) * vector.Z(),
|
|
matrix(1U, 0U) * vector.X() + matrix(1U, 1U) * vector.Y() +
|
|
matrix(1U, 2U) * vector.Z(),
|
|
matrix(2U, 0U) * vector.X() + matrix(2U, 1U) * vector.Y() +
|
|
matrix(2U, 2U) * vector.Z()};
|
|
}
|
|
|
|
} // namespace fesa::math_internal
|
|
|
|
#endif // FESA_MATH_VECTOR3_OPERATIONS_H_
|