diff --git a/geometry/r3x3_matrix_test.cpp b/geometry/r3x3_matrix_test.cpp index 3513d00146..266a4eda82 100644 --- a/geometry/r3x3_matrix_test.cpp +++ b/geometry/r3x3_matrix_test.cpp @@ -65,9 +65,9 @@ TEST_F(R3x3MatrixTest, QRDecomposition) { EXPECT_THAT( q, AlmostEquals(R3x3Matrix( - {6.0 / 7.0, 3.0 / 7.0, 2.0 / 7.0}, - {-30.0 / Sqrt(3577.0), 34.0 / Sqrt(3577.0), 39.0 / Sqrt(3577.0)}, - {1.0 / Sqrt(73.0), -6.0 / Sqrt(73.0), 6.0 / Sqrt(73.0)}), 10)); + {6.0 / 7.0, -30.0 / Sqrt(3577.0), 1.0 / Sqrt(73.0)}, + {3.0 / 7.0, 34.0 / Sqrt(3577.0), -6.0 / Sqrt(73.0)}, + {2.0 / 7.0, 39.0 / Sqrt(3577.0), 6.0 / Sqrt(73.0)}), 569)); EXPECT_THAT( r, AlmostEquals(R3x3Matrix( diff --git a/numerics/fixed_arrays.hpp b/numerics/fixed_arrays.hpp index 21885606a4..4eed4af7e4 100644 --- a/numerics/fixed_arrays.hpp +++ b/numerics/fixed_arrays.hpp @@ -19,6 +19,8 @@ using namespace principia::numerics::_transposed_view; using namespace principia::quantities::_named_quantities; using namespace principia::quantities::_si; +// TODO(phl): This should support the same operations as unbounded_arrays.hpp. + template class FixedMatrix; @@ -221,10 +223,10 @@ template constexpr FixedVector Normalize( FixedVector const& vector); -template -constexpr FixedMatrix, lsize, rsize> SymmetricProduct( - FixedVector const& left, - FixedVector const& right); +template +constexpr FixedMatrix, size, size> SymmetricProduct( + FixedVector const& left, + FixedVector const& right); template constexpr FixedMatrix, size, size> SymmetricSquare( @@ -284,23 +286,23 @@ constexpr FixedMatrix& operator-=( template constexpr FixedVector, size> operator*( - LScalar const left, + LScalar const& left, FixedVector const& right); template constexpr FixedVector, size> operator*( FixedVector const& left, - RScalar const right); + RScalar const& right); template constexpr FixedMatrix, rows, columns> -operator*(LScalar const left, +operator*(LScalar const& left, FixedMatrix const& right); template constexpr FixedMatrix, rows, columns> operator*(FixedMatrix const& left, - RScalar const right); + RScalar const& right); template constexpr FixedVector, size> operator/( @@ -310,7 +312,27 @@ constexpr FixedVector, size> operator/( template constexpr FixedMatrix, rows, columns> operator/(FixedMatrix const& left, - RScalar const right); + RScalar const& right); + +template +constexpr FixedVector& operator*=( + FixedVector& left, + double right); + +template +constexpr FixedMatrix& operator*=( + FixedMatrix& left, + double right); + +template +constexpr FixedVector& operator/=( + FixedVector& left, + double right); + +template +constexpr FixedMatrix& operator/=( + FixedMatrix& left, + double right); // Hilbert space and algebra. @@ -341,6 +363,12 @@ constexpr FixedVector, rows> operator*( FixedMatrix const& left, FixedVector const& right); +// Use this operator to multiply a row vector with a matrix. +template +constexpr FixedVector, columns> operator*( + TransposedView> const& left, + FixedVector const& right); + // Ouput. template diff --git a/numerics/fixed_arrays_body.hpp b/numerics/fixed_arrays_body.hpp index 27cc2d2224..f519c2a574 100644 --- a/numerics/fixed_arrays_body.hpp +++ b/numerics/fixed_arrays_body.hpp @@ -83,7 +83,7 @@ Scalar FixedVector::Norm() const { } template -inline Square FixedVector::Norm²() const { +Square FixedVector::Norm²() const { return DotProduct::Compute(data_, data_); } @@ -397,12 +397,12 @@ constexpr FixedVector Normalize( return vector / vector.Norm(); } -template -constexpr FixedMatrix, lsize, rsize> SymmetricProduct( - FixedVector const& left, - FixedVector const& right) { - FixedMatrix, lsize, rsize> result(uninitialized); - for (int i = 0; i < lsize; ++i) { +template +constexpr FixedMatrix, size, size> SymmetricProduct( + FixedVector const& left, + FixedVector const& right) { + FixedMatrix, size, size> result(uninitialized); + for (int i = 0; i < size; ++i) { for (int j = 0; j < i; ++j) { auto const r = 0.5 * (left[i] * right[j] + left[j] * right[i]); result(i, j) = r; @@ -533,7 +533,7 @@ constexpr FixedMatrix& operator-=( template constexpr FixedVector, size> operator*( - LScalar const left, + LScalar const& left, FixedVector const& right) { std::array, size> result; for (int i = 0; i < size; ++i) { @@ -545,7 +545,7 @@ constexpr FixedVector, size> operator*( template constexpr FixedVector, size> operator*( FixedVector const& left, - RScalar const right) { + RScalar const& right) { std::array, size> result; for (int i = 0; i < size; ++i) { result[i] = left[i] * right; @@ -555,7 +555,7 @@ constexpr FixedVector, size> operator*( template constexpr FixedMatrix, rows, columns> operator*( - LScalar const left, + LScalar const& left, FixedMatrix const& right) { FixedMatrix, rows, columns> result( uninitialized); @@ -570,7 +570,7 @@ constexpr FixedMatrix, rows, columns> operator*( template constexpr FixedMatrix, rows, columns> operator*( FixedMatrix const& left, - RScalar const right) { + RScalar const& right) { FixedMatrix, rows, columns> result( uninitialized); for (int i = 0; i < rows; ++i) { @@ -595,7 +595,7 @@ constexpr FixedVector, size> operator/( template constexpr FixedMatrix, rows, columns> operator/( FixedMatrix const& left, - RScalar const right) { + RScalar const& right) { FixedMatrix, rows, columns> result( uninitialized); for (int i = 0; i < rows; ++i) { @@ -606,6 +606,32 @@ constexpr FixedMatrix, rows, columns> operator/( return result; } +template +constexpr FixedVector& operator*=(FixedVector& left, + double const right) { + return left = left * right; +} + +template +constexpr FixedMatrix& operator*=( + FixedMatrix& left, + double const right) { + return left = left * right; +} + +template +constexpr FixedVector& operator/=(FixedVector& left, + double const right) { + return left = left / right; +} + +template +constexpr FixedMatrix& operator/=( + FixedMatrix& left, + double const right) { + return left = left / right; +} + template constexpr Product operator*( LScalar* const left, @@ -664,6 +690,20 @@ constexpr FixedVector, rows> operator*( return FixedVector, rows>(std::move(result)); } +template +constexpr FixedVector, columns> operator*( + TransposedView> const& left, + FixedVector const& right) { + std::array, columns> result{}; + for (int j = 0; j < columns; ++j) { + auto& result_j = result[j]; + for (int i = 0; i < rows; ++i) { + result_j += left.transpose(i, j) * right[i]; + } + } + return FixedVector, columns>(std::move(result)); +} + template std::ostream& operator<<(std::ostream& out, FixedVector const& vector) { diff --git a/numerics/fixed_arrays_test.cpp b/numerics/fixed_arrays_test.cpp index c84b1dabf0..92dc97a2d7 100644 --- a/numerics/fixed_arrays_test.cpp +++ b/numerics/fixed_arrays_test.cpp @@ -130,6 +130,8 @@ TEST_F(FixedArraysTest, Algebra) { 14, -63, 5, -92})), m23_ * m34_); EXPECT_EQ(v3_, m34_ * v4_); + EXPECT_EQ((FixedVector({-486, -229, 333, 198})), + TransposedView{m34_} * v3_); // NOLINT } TEST_F(FixedArraysTest, VectorIndexing) { diff --git a/numerics/matrix_computations.hpp b/numerics/matrix_computations.hpp index 73ba275b03..3b9da3ecfa 100644 --- a/numerics/matrix_computations.hpp +++ b/numerics/matrix_computations.hpp @@ -29,6 +29,15 @@ struct ᵗRDRDecompositionGenerator; template struct SubstitutionGenerator; +// Declares: +// struct Result { +// (matrix) H; +// (matrix) U; +// } +// TODO(phl): Add support for U. +template +struct HessenbergDecompositionGenerator; + // Declares: // struct Result { // ⟨matrix⟩ rotation; @@ -81,6 +90,13 @@ typename SubstitutionGenerator::Result ForwardSubstitution(LowerTriangularMatrix const& L, Vector const& b); +// If A is a square matrix, returns U and H so that A = ᵗU H U, where H is an +// upper Hessenberg matrix. +// TODO(phl): Add support for returning U. +template +typename HessenbergDecompositionGenerator::Result +HessenbergDecomposition(Matrix const& A); + // Returns the eigensystem of A, which must be symmetric. // As a safety measure we limit the number of iterations. We prefer to exit // when the matrix is nearly diagonal, though. @@ -95,7 +111,8 @@ template typename RayleighQuotientGenerator::Result RayleighQuotient(Matrix const& A, Vector const& x); -// Returns the eigenvector closest to x and its eigenvalue. +// Returns the eigenvector closest to x and its eigenvalue. A must be +// symmetric. template typename RayleighQuotientIterationGenerator::Result RayleighQuotientIteration(Matrix const& A, Vector const& x); @@ -111,6 +128,7 @@ using internal::BackSubstitution; using internal::CholeskyDecomposition; using internal::ClassicalJacobi; using internal::ForwardSubstitution; +using internal::HessenbergDecomposition; using internal::RayleighQuotient; using internal::RayleighQuotientIteration; using internal::Solve; diff --git a/numerics/matrix_computations_body.hpp b/numerics/matrix_computations_body.hpp index 6673538b57..84a77c2086 100644 --- a/numerics/matrix_computations_body.hpp +++ b/numerics/matrix_computations_body.hpp @@ -8,6 +8,7 @@ #include "base/tags.hpp" #include "numerics/fixed_arrays.hpp" +#include "numerics/transposed_view.hpp" #include "numerics/unbounded_arrays.hpp" #include "quantities/elementary_functions.hpp" #include "quantities/si.hpp" @@ -19,10 +20,251 @@ namespace internal { using namespace principia::base::_tags; using namespace principia::numerics::_fixed_arrays; +using namespace principia::numerics::_transposed_view; using namespace principia::numerics::_unbounded_arrays; using namespace principia::quantities::_elementary_functions; using namespace principia::quantities::_si; +// TODO(phl): The view stuff should be (1) made completed, i.e., have all the +// operations that exist for fixed/unbounded vectors/matrices; (2) moved to a +// common place (probably together with TransposedView); (3) unified with +// fixed/unbounded arrays so that we don't have to write each algorithm N times. + +// A view of a column of a matrix. +// TODO(phl): Could we extract Scalar from Matrix? +template +struct ColumnView { + Matrix& matrix; + int first_row; + int last_row; + int column; + + Scalar Norm() const; + Square Norm²() const; + constexpr int size() const; + + // Constructs an unbounded vector by copying data from the view. Note that + // the result is unbounded even if the matrix being viewed is a FixedMatrix. + explicit operator UnboundedVector() const; + + constexpr Scalar& operator[](int index); + constexpr Scalar const& operator[](int index) const; + + ColumnView& operator/=(double right); +}; + +template +Square ColumnView::Norm²() const { + Square result{}; + for (int i = first_row; i <= last_row; ++i) { + result += Pow<2>(matrix(i, column)); + } + return result; +} + +template +Scalar ColumnView::Norm() const { + return Sqrt(Norm²()); +} + +template +constexpr int ColumnView::size() const { + return last_row - first_row + 1; +} + +template +ColumnView::operator UnboundedVector() const { + UnboundedVector result(size(), uninitialized); + for (int i = first_row; i <= last_row; ++i) { + result[i - first_row] = matrix(i, column); + } + return result; +} + +template +constexpr Scalar& ColumnView::operator[](int const index) { + DCHECK_LE(index, last_row - first_row); + return matrix(first_row + index, column); +} + +template +constexpr Scalar const& ColumnView::operator[]( + int const index) const { + DCHECK_LE(index, last_row - first_row); + return matrix(first_row + index, column); +} + +template +ColumnView& ColumnView::operator/=( + double const right) { + for (int i = first_row; i < last_row; ++i) { + matrix(i, column) /= right; + } +} + +template +UnboundedVector Normalize(ColumnView const& view) { + return UnboundedVector(view) / view.Norm(); +} + +template +std::ostream& operator<<(std::ostream& out, + ColumnView const& view) { + std::stringstream s; + for (int i = 0; i < view.size(); ++i) { + s << (i == 0 ? "{" : "") << view[i] + << (i == view.size() - 1 ? "}" : ", "); + } + out << s.str(); + return out; +} + +template +struct BlockView { + Matrix& matrix; + int first_row; + int last_row; + int first_column; + int last_column; + + constexpr int rows() const; + constexpr int columns() const; + + constexpr Scalar& operator()(int row, int column); + constexpr Scalar const& operator()(int row, int column) const; + + BlockView& operator-=(UnboundedMatrix const& right); +}; + +template +constexpr int BlockView::rows() const { + return last_row - first_row + 1; +} + +template +constexpr int BlockView::columns() const { + return last_column - first_column + 1; +} + +template +constexpr Scalar& BlockView::operator()(int const row, + int const column) { + DCHECK_LE(row, last_row - first_row); + DCHECK_LE(column, last_column - first_column); + return matrix(first_row + row, first_column + column); +} + +template +constexpr Scalar const& BlockView::operator()( + int const row, + int const column) const { + DCHECK_LE(row, last_row - first_row); + DCHECK_LE(column, last_column - first_column); + return matrix(first_row + row, first_column + column); +} + +template +BlockView& BlockView::operator-=( + UnboundedMatrix const& right) { + CHECK_EQ(rows(), right.rows()); + CHECK_EQ(columns(), right.columns()); + for (int i = 0; i < right.rows(); ++i) { + for (int j = 0; j < right.columns(); ++j) { + matrix(first_row + i, first_column + j) -= right(i, j); + } + } + return *this; +} + + +template +UnboundedVector> operator*( + BlockView const& left, + UnboundedVector const& right) { + CHECK_EQ(left.columns(), right.size()); + UnboundedVector> result(left.rows()); + for (int i = 0; i < left.rows(); ++i) { + auto& result_i = result[i]; + for (int j = 0; j < left.columns(); ++j) { + result_i += left(i, j) * right[j]; + } + } + return result; +} + +template +UnboundedVector> operator*( + TransposedView> const& left, + UnboundedVector const& right) { + CHECK_EQ(left.transpose.rows(), right.size()); + UnboundedVector> result(left.transpose.columns()); + for (int j = 0; j < left.transpose.columns(); ++j) { + auto& result_j = result[j]; + for (int i = 0; i < left.transpose.rows(); ++i) { + result_j += left.transpose(i, j) * right[i]; + } + } + return result; +} + +// As mentioned in [GV13] section 5.1.4, "It is critical to exploit structure +// when applying [the Householder reflection] to a matrix". +struct HouseholderReflection { + UnboundedVector v; + double β; +}; + +template +HouseholderReflection ComputeHouseholderReflection(Vector const& x) { + int const m = x.size(); + // In order to avoid issues with quantities, we start by normalizing x. This + // implies that μ is 1. + auto const normalized_x = Normalize(x); + HouseholderReflection result{.v = normalized_x, .β = 0}; + double const& x₁ = normalized_x[0]; + double& v₁ = result.v[0]; + auto x₂ₘ = normalized_x; + x₂ₘ[0] = 0; + auto const σ = x₂ₘ.Norm²(); + v₁ = 1; + if (σ == 0) { + if (x₁ < 0) { + result.β = -2; + } + } else { + static constexpr double μ = 1; + if (x₁ <= 0) { + v₁ = x₁ - μ; + } else { + v₁ = -σ / (x₁ + μ); + } + double const v₁² = Pow<2>(v₁); + result.β = 2 * v₁² / (σ + v₁²); + result.v /= v₁; + } + return result; +} + +// A becomes P A. +template +void Premultiply(HouseholderReflection const& P, Matrix& A) { + // We don't have a multiplication TransposedView * Matrix because the + // ownership of the result is problematic. Instead, we transpose twice. That + // costs essentially nothing. + auto const ᵗAv = TransposedView{A} * P.v; // NOLINT + auto const ᵗvA = TransposedView{ᵗAv}; // NOLINT + auto const βv = P.β * P.v; + A -= βv * ᵗvA; +} + +// A becomes A P. +template +void PostMultiply(Matrix& A, HouseholderReflection const& P) { + auto const βᵗv = TransposedView{P.β * P.v}; // NOLINT + auto const Av = A * P.v; + A -= Av * βᵗv; +} + // This is J(p, q, θ) in [GV13] section 8.5.1. This matrix is also called a // Givens rotation. As mentioned in [GV13] section 5.1.9, "It is critical that // the special structure of a Givens rotation matrix be exploited". @@ -145,6 +387,25 @@ struct SubstitutionGenerator, static Result Uninitialized(TriangularMatrix const& m); }; +template +struct HessenbergDecompositionGenerator> { + using Scalar = Scalar_; + using Reflector = UnboundedVector; + struct Result { + UnboundedMatrix H; + }; +}; + +template +struct HessenbergDecompositionGenerator< + FixedMatrix> { + using Scalar = Scalar_; + using Reflector = FixedVector; + struct Result { + FixedMatrix H; + }; +}; + template struct ClassicalJacobiGenerator> { using Scalar = Scalar_; @@ -433,6 +694,42 @@ ForwardSubstitution(LowerTriangularMatrix const& L, return x; } +template +typename HessenbergDecompositionGenerator::Result +HessenbergDecomposition(Matrix const& A) { + using G = HessenbergDecompositionGenerator; + using Scalar = typename G::Scalar; + typename HessenbergDecompositionGenerator::Result result{.H = A}; + auto& H = result.H; + int const n = A.rows(); + + // [GV13], Algorithm 7.4.2. + for (int k = 0; k < n - 2; ++k) { + auto const P = ComputeHouseholderReflection( + ColumnView{.matrix = H, + .first_row = k + 1, + .last_row = n - 1, + .column = k}); + { + auto block = BlockView{.matrix = H, + .first_row = k + 1, + .last_row = n - 1, + .first_column = k, + .last_column = n - 1}; + Premultiply(P, block); + } + { + auto block = BlockView{.matrix = H, + .first_row = 0, + .last_row = n - 1, + .first_column = k + 1, + .last_column = n - 1}; + PostMultiply(block, P); + } + } + return result; +} + template typename ClassicalJacobiGenerator::Result ClassicalJacobi(Matrix const& A, int max_iterations, double ε) { diff --git a/numerics/matrix_computations_test.cpp b/numerics/matrix_computations_test.cpp index 8ecc372523..f2dbbaf0b8 100644 --- a/numerics/matrix_computations_test.cpp +++ b/numerics/matrix_computations_test.cpp @@ -8,6 +8,7 @@ #include "numerics/unbounded_arrays.hpp" #include "quantities/elementary_functions.hpp" #include "testing_utilities/almost_equals.hpp" +#include "testing_utilities/vanishes_before.hpp" namespace principia { namespace numerics { @@ -17,6 +18,7 @@ using namespace principia::numerics::_matrix_computations; using namespace principia::numerics::_unbounded_arrays; using namespace principia::quantities::_elementary_functions; using namespace principia::testing_utilities::_almost_equals; +using namespace principia::testing_utilities::_vanishes_before; template class MatrixComputationsTest : public ::testing::Test { @@ -102,6 +104,39 @@ TYPED_TEST(MatrixComputationsTest, ForwardSubstitution) { EXPECT_THAT(x4_actual, AlmostEquals(x4_expected, 0)); } +TYPED_TEST(MatrixComputationsTest, HessenbergForm) { + using Matrix = typename std::tuple_element<3, TypeParam>::type; + Matrix const m4({1, 2, 3, -4, + 5, 6, 7, 8, + 9, 8, -7, 6, + 5, 4, 3, 2}); + Matrix const h4_expected({ 1, + 1.4852968963237645012, + -0.63177627362517020518, + 5.1375822981102002423, + + 11.445523142259597039, + 7.7328244274809160305, + 10.840153034634636509, + -4.4908563773743662120, + + 0, + 10.020455740333542876, + -3.9158079318984636532, + 0.14244486136993501485, + + 0, + 0, + 2.4140754086886336638, + -2.8170164955824523773}); + auto h4_actual = HessenbergDecomposition(m4).H; + // This component should really use VanishesBefore, but we don't have a good + // way to do that. + EXPECT_THAT(h4_actual(3, 1), VanishesBefore(1, 24)); + h4_actual(3, 1) = 0; + EXPECT_THAT(h4_actual, AlmostEquals(h4_expected, 14)); +} + TYPED_TEST(MatrixComputationsTest, ClassicalJacobi) { using Vector = typename std::tuple_element<0, TypeParam>::type; using Matrix = typename std::tuple_element<3, TypeParam>::type; diff --git a/numerics/unbounded_arrays.hpp b/numerics/unbounded_arrays.hpp index e623e842c3..bf67267edf 100644 --- a/numerics/unbounded_arrays.hpp +++ b/numerics/unbounded_arrays.hpp @@ -14,6 +14,8 @@ namespace numerics { namespace _unbounded_arrays { namespace internal { +// TODO(phl): This should support the same operations as fixed_arrays.hpp. + using namespace principia::base::_tags; using namespace principia::numerics::_transposed_view; using namespace principia::quantities::_named_quantities; @@ -50,6 +52,7 @@ class UnboundedVector final { void EraseToEnd(int begin_index); Scalar Norm() const; + Square Norm²() const; int size() const; @@ -196,25 +199,110 @@ class UnboundedUpperTriangularMatrix final { friend class Row; }; -template -UnboundedVector> operator/( - UnboundedVector const& left, - ScalarRight const& right); - -template -Product operator*( - TransposedView> const& left, - UnboundedVector const& right); - -template -UnboundedMatrix> operator*( - UnboundedMatrix const& left, - UnboundedMatrix const& right); - -template -UnboundedVector> operator*( - UnboundedMatrix const& left, - UnboundedVector const& right); +template +UnboundedVector Normalize(UnboundedVector const& vector); + +// Additive groups. + +template +constexpr UnboundedVector& operator+=( + UnboundedVector& left, + UnboundedVector const& right); + +template +constexpr UnboundedMatrix& operator+=( + UnboundedMatrix& left, + UnboundedMatrix const& right); + +template +constexpr UnboundedVector& operator-=( + UnboundedVector& left, + UnboundedVector const& right); + +template +constexpr UnboundedMatrix& operator-=( + UnboundedMatrix& left, + UnboundedMatrix const& right); + +// Vector spaces. + +template +UnboundedVector> operator*( + LScalar const& left, + UnboundedVector const& right); + +template +UnboundedVector> operator*( + UnboundedVector const& left, + RScalar const& right); + +template +UnboundedMatrix> +operator*(LScalar const& left, + UnboundedMatrix const& right); + +template +UnboundedMatrix> +operator*(UnboundedMatrix const& left, + RScalar const& right); + +template +UnboundedVector> operator/( + UnboundedVector const& left, + RScalar const& right); + +template +constexpr UnboundedMatrix> +operator/(UnboundedMatrix const& left, + RScalar const& right); + +template +constexpr UnboundedVector& operator*=( + UnboundedVector& left, + double right); + +template +constexpr UnboundedMatrix& operator*=( + UnboundedMatrix& left, + double right); + +template +constexpr UnboundedVector& operator/=( + UnboundedVector& left, + double right); + +template +constexpr UnboundedMatrix& operator/=( + UnboundedMatrix& left, + double right); + +// Hilbert space and algebra. + +template +Product operator*( + TransposedView> const& left, + UnboundedVector const& right); + +template +UnboundedMatrix> operator*( + UnboundedVector const& left, + TransposedView> const& right); + +template +UnboundedMatrix> operator*( + UnboundedMatrix const& left, + UnboundedMatrix const& right); + +template +UnboundedVector> operator*( + UnboundedMatrix const& left, + UnboundedVector const& right); + +// Use this operator to multiply a row vector with a matrix. +template +UnboundedVector> operator*( + TransposedView> const& left, + UnboundedVector const& right); template std::ostream& operator<<(std::ostream& out, diff --git a/numerics/unbounded_arrays_body.hpp b/numerics/unbounded_arrays_body.hpp index ee0c9c61d3..0e0f9f4943 100644 --- a/numerics/unbounded_arrays_body.hpp +++ b/numerics/unbounded_arrays_body.hpp @@ -63,11 +63,16 @@ void UnboundedVector::EraseToEnd(int const begin_index) { template Scalar UnboundedVector::Norm() const { + return Sqrt(Norm²()); +} + +template +Square UnboundedVector::Norm²() const { Square norm²{}; for (auto const c : data_) { norm² += c * c; } - return Sqrt(norm²); + return norm²; } template @@ -468,36 +473,177 @@ UnboundedUpperTriangularMatrix::Transpose( return result; } -template -UnboundedVector> operator/( - UnboundedVector const& left, - ScalarRight const& right) { - UnboundedVector> result(left.size(), - uninitialized); +template +UnboundedVector Normalize(UnboundedVector const& vector) { + return vector / vector.Norm(); +} + +template +constexpr UnboundedVector& operator+=( + UnboundedVector& left, + UnboundedVector const& right) { + return left = left + right; +} + +template +constexpr UnboundedMatrix& operator+=( + UnboundedMatrix& left, + UnboundedMatrix const& right) { + return left = left + right; +} + +template +constexpr UnboundedVector& operator-=( + UnboundedVector& left, + UnboundedVector const& right) { + return left = left - right; +} + +template +constexpr UnboundedMatrix& operator-=( + UnboundedMatrix& left, + UnboundedMatrix const& right) { + return left = left - right; +} + +template +UnboundedVector> operator*( + LScalar const& left, + UnboundedVector const& right) { + UnboundedVector> result(right.size(), + uninitialized); + for (int i = 0; i < right.size(); ++i) { + result[i] = left * right[i]; + } + return result; +} + +template +UnboundedVector> operator*( + UnboundedVector const& left, + RScalar const& right) { + UnboundedVector> result(left.size(), + uninitialized); + for (int i = 0; i < left.size(); ++i) { + result[i] = left[i] * right; + } + return result; +} + +template +UnboundedMatrix> operator*( + LScalar const& left, + UnboundedMatrix const& right) { + UnboundedMatrix> result(right.rows(), + right.columns(), + uninitialized); + for (int i = 0; i < right.rows(); ++i) { + for (int j = 0; j < right.columns(); ++j) { + result(i, j) = left * right(i, j); + } + } + return result; +} + +template +UnboundedMatrix> operator*( + UnboundedMatrix const& left, + RScalar const& right) { + UnboundedMatrix> result(left.rows(), + left.columns(), + uninitialized); + for (int i = 0; i < left.rows(); ++i) { + for (int j = 0; j < left.columns(); ++j) { + result(i, j) = left(i, j) * right; + } + } + return result; +} + +template +UnboundedVector> operator/( + UnboundedVector const& left, + RScalar const& right) { + UnboundedVector> result(left.size(), + uninitialized); for (int i = 0; i < left.size(); ++i) { result[i] = left[i] / right; } return result; } -template -Product operator*( - TransposedView> const& left, - UnboundedVector const& right) { +template +UnboundedMatrix> operator/( + UnboundedMatrix const& left, + RScalar const right) { + UnboundedMatrix> result(left.rows(), + left.columns(), + uninitialized); + for (int i = 0; i < left.rows(); ++i) { + for (int j = 0; j < left.columns(); ++j) { + result(i, j) = left(i, j) / right; + } + } + return result; +} + +template +constexpr UnboundedVector& operator*=(UnboundedVector& left, + double const right) { + return left = left * right; +} + +template +constexpr UnboundedMatrix& operator*=(UnboundedMatrix& left, + double const right) { + return left = left * right; +} + +template +constexpr UnboundedVector& operator/=(UnboundedVector& left, + double const right) { + return left = left / right; +} + +template +constexpr UnboundedMatrix& operator/=(UnboundedMatrix& left, + double const right) { + return left = left / right; +} + +template +Product operator*( + TransposedView> const& left, + UnboundedVector const& right) { CHECK_EQ(left.transpose.size(), right.size()); - Product result{}; + Product result{}; for (int i = 0; i < left.transpose.size(); ++i) { result += left.transpose[i] * right[i]; } return result; } -template -UnboundedMatrix> operator*( - UnboundedMatrix const& left, - UnboundedMatrix const& right) { +template +UnboundedMatrix> operator*( + UnboundedVector const& left, + TransposedView> const& right) { + UnboundedMatrix> result(left.size(), + right.transpose.size(), + uninitialized); + for (int i = 0; i < result.rows(); ++i) { + for (int j = 0; j < result.columns(); ++j) { + result(i, j) = left[i] * right.transpose[j]; + } + } + return result; +} + +template +UnboundedMatrix> operator*( + UnboundedMatrix const& left, + UnboundedMatrix const& right) { CHECK_EQ(left.columns(), right.rows()); - UnboundedMatrix> result(left.rows(), + UnboundedMatrix> result(left.rows(), right.columns()); for (int i = 0; i < left.rows(); ++i) { for (int j = 0; j < right.columns(); ++j) { @@ -509,12 +655,12 @@ UnboundedMatrix> operator*( return result; } -template -UnboundedVector> operator*( - UnboundedMatrix const& left, - UnboundedVector const& right) { +template +UnboundedVector> operator*( + UnboundedMatrix const& left, + UnboundedVector const& right) { CHECK_EQ(left.columns(), right.size()); - UnboundedVector> result(left.rows()); + UnboundedVector> result(left.rows()); for (int i = 0; i < left.rows(); ++i) { auto& result_i = result[i]; for (int j = 0; j < left.columns(); ++j) { @@ -524,6 +670,21 @@ UnboundedVector> operator*( return result; } +template +UnboundedVector> operator*( + TransposedView> const& left, + UnboundedVector const& right) { + CHECK_EQ(left.rows(), right.size()); + UnboundedVector> result(left.columns()); + for (int j = 0; j < left.columns(); ++j) { + auto& result_j = result[j]; + for (int i = 0; i < left.rows(); ++i) { + result_j += left(i, j) * right[i]; + } + } + return result; +} + template std::ostream& operator<<(std::ostream& out, UnboundedVector const& vector) { diff --git a/numerics/unbounded_arrays_test.cpp b/numerics/unbounded_arrays_test.cpp index 4f8350afd3..6811ed6c1d 100644 --- a/numerics/unbounded_arrays_test.cpp +++ b/numerics/unbounded_arrays_test.cpp @@ -108,6 +108,10 @@ TEST_F(UnboundedArraysTest, MultiplicationDivision) { 661611, 1070697, 1732308, 2803005}), m4_ * m4_); } +TEST_F(UnboundedArraysTest, Algebra) { + EXPECT_EQ(3270, v3_.Transpose() * v3_); +} + TEST_F(UnboundedArraysTest, VectorIndexing) { EXPECT_EQ(31, v3_[1]); v3_[2] = -666; diff --git a/testing_utilities/almost_equals_body.hpp b/testing_utilities/almost_equals_body.hpp index 96d2f3170f..301a61b6e3 100644 --- a/testing_utilities/almost_equals_body.hpp +++ b/testing_utilities/almost_equals_body.hpp @@ -166,7 +166,7 @@ bool AlmostEqualsMatcher::MatchAndExplain( int max_j = -1; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { - int const distance = NormalizedNaNULPDistance( + std::int64_t const distance = NormalizedNaNULPDistance( DoubleValue(actual(i, j)), DoubleValue(expected_(i, j))); if (distance > max_distance) { max_distance = distance; @@ -338,8 +338,8 @@ bool AlmostEqualsMatcher::MatchAndExplain( std::int64_t max_distance = -1; int max_i = -1; for (int i = 0; i < expected_.size(); ++i) { - int const distance = NormalizedNaNULPDistance(DoubleValue(actual[i]), - DoubleValue(expected_[i])); + std::int64_t const distance = NormalizedNaNULPDistance( + DoubleValue(actual[i]), DoubleValue(expected_[i])); if (distance > max_distance) { max_distance = distance; max_i = i; @@ -368,7 +368,7 @@ bool AlmostEqualsMatcher::MatchAndExplain( int max_j = -1; for (int i = 0; i < expected_.rows(); ++i) { for (int j = 0; j < expected_.columns(); ++j) { - int const distance = NormalizedNaNULPDistance( + std::int64_t const distance = NormalizedNaNULPDistance( DoubleValue(actual(i, j)), DoubleValue(expected_(i, j))); if (distance > max_distance) { max_distance = distance; @@ -400,7 +400,7 @@ bool AlmostEqualsMatcher::MatchAndExplain( int max_j = -1; for (int i = 0; i < expected_.rows(); ++i) { for (int j = 0; j <= i; ++j) { - int const distance = NormalizedNaNULPDistance( + std::int64_t const distance = NormalizedNaNULPDistance( DoubleValue(actual(i, j)), DoubleValue(expected_(i, j))); if (distance > max_distance) { max_distance = distance; @@ -432,7 +432,7 @@ bool AlmostEqualsMatcher::MatchAndExplain( int max_j = -1; for (int i = 0; i < expected_.columns(); ++i) { for (int j = i; j < expected_.columns(); ++j) { - int const distance = NormalizedNaNULPDistance( + std::int64_t const distance = NormalizedNaNULPDistance( DoubleValue(actual(i, j)), DoubleValue(expected_(i, j))); if (distance > max_distance) { max_distance = distance; @@ -466,8 +466,8 @@ bool AlmostEqualsMatcher::MatchAndExplain( std::int64_t max_distance = -1; int max_i = -1; for (int i = 0; i < expected_.size(); ++i) { - int const distance = NormalizedNaNULPDistance(DoubleValue(actual[i]), - DoubleValue(expected_[i])); + std::int64_t const distance = NormalizedNaNULPDistance( + DoubleValue(actual[i]), DoubleValue(expected_[i])); if (distance > max_distance) { max_distance = distance; max_i = i; @@ -500,7 +500,7 @@ bool AlmostEqualsMatcher::MatchAndExplain( int max_j = -1; for (int i = 0; i < expected_.rows(); ++i) { for (int j = 0; j < expected_.columns(); ++j) { - int const distance = NormalizedNaNULPDistance( + std::int64_t const distance = NormalizedNaNULPDistance( DoubleValue(actual(i, j)), DoubleValue(expected_(i, j))); if (distance > max_distance) { max_distance = distance; @@ -536,7 +536,7 @@ bool AlmostEqualsMatcher::MatchAndExplain( int max_j = -1; for (int i = 0; i < expected_.rows(); ++i) { for (int j = 0; j <= i; ++j) { - int const distance = NormalizedNaNULPDistance( + std::int64_t const distance = NormalizedNaNULPDistance( DoubleValue(actual(i, j)), DoubleValue(expected_(i, j))); if (distance > max_distance) { max_distance = distance; @@ -573,7 +573,7 @@ bool AlmostEqualsMatcher::MatchAndExplain( int max_j = -1; for (int i = 0; i < expected_.columns(); ++i) { for (int j = i; j < expected_.columns(); ++j) { - int const distance = NormalizedNaNULPDistance( + std::int64_t const distance = NormalizedNaNULPDistance( DoubleValue(actual(i, j)), DoubleValue(expected_(i, j))); if (distance > max_distance) { max_distance = distance;