core/algebra/affine.hpp

0001 /*
0002  * SPDX-PackageName: "covfie, a part of the ACTS project"
0003  * SPDX-FileCopyrightText: 2022 CERN
0004  * SPDX-License-Identifier: MPL-2.0
0005  */
0006
0007 #pragma once
0008
0009 #include <cstddef>
0010
0011 #include <covfie/core/algebra/matrix.hpp>
0012 #include <covfie/core/algebra/vector.hpp>
0013 #include <covfie/core/array.hpp>
0014 #include <covfie/core/qualifiers.hpp>
0015
0016 namespace covfie::algebra {
0017 template <std::size_t N, typename T = float, typename I = std::size_t>
0018 struct affine : public matrix<N, N + 1, T, I> {
0019     affine() = default;
0020     affine(affine &&) = default;
0021     affine(const affine &) = default;
0022     affine & operator=(const affine &) = default;
0023     affine & operator=(affine &&) = default;
0024
0025     COVFIE_HOST_DEVICE affine(const matrix<N, N + 1, T, I> & o)
0026         : matrix<N, N + 1, T, I>(o)
0027     {
0028     }
0029
0030     COVFIE_HOST_DEVICE vector<N, T, I> operator*(const vector<N, T, I> & v
0031     ) const
0032     {
0033         vector<N + 1, T, I> r;
0034
0035         for (I i = 0; i < N; ++i) {
0036             r(i) = v(i);
0037         }
0038
0039         r(N) = static_cast<T>(1.);
0040
0041         return matrix<N, N + 1, T, I>::operator*(r);
0042     }
0043
0044     COVFIE_HOST_DEVICE affine<N, T, I> operator*(const affine<N, T, I> & m
0045     ) const
0046     {
0047         matrix<N + 1, N + 1, T, I> m1, m2;
0048
0049         for (I i = 0; i < N; ++i) {
0050             for (I j = 0; j < N + 1; ++j) {
0051                 m1(i, j) = this->operator()(i, j);
0052                 m2(i, j) = m.operator()(i, j);
0053             }
0054         }
0055
0056         for (I j = 0; j < N + 1; ++j) {
0057             if (j == N) {
0058                 m1(N, j) = 1.f;
0059                 m2(N, j) = 1.f;
0060             } else {
0061                 m1(N, j) = 0.f;
0062                 m2(N, j) = 0.f;
0063             }
0064         }
0065
0066         matrix<N + 1, N + 1, T, I> r = m1 * m2;
0067         matrix<N, N + 1, T, I> o;
0068
0069         for (I i = 0; i < N; ++i) {
0070             for (I j = 0; j < N + 1; ++j) {
0071                 o(i, j) = r(i, j);
0072             }
0073         }
0074
0075         return o;
0076     }
0077
0078     template <typename... Args>
0079     COVFIE_HOST_DEVICE static affine<N, T, I> translation(const Args &... args)
0080     {
0081         static_assert(
0082             (std::is_convertible_v<Args, T> && ...),
0083             "Translation arguments must be convertible to transformation "
0084             "matrix elements."
0085         );
0086         static_assert(
0087             sizeof...(Args) == N,
0088             "Translation must have exactly as many arguments as the dimensions "
0089             "of the matrix."
0090         );
0091
0092         array::array<T, N> arr{args...};
0093
0094         matrix<N, N + 1, T, I> result = matrix<N, N + 1, T, I>::identity();
0095
0096         for (I i = 0; i < N; ++i) {
0097             result(i, N) = arr[i];
0098         }
0099
0100         return result;
0101     }
0102
0103     template <typename... Args>
0104     COVFIE_HOST_DEVICE static affine<N, T, I> scaling(const Args &... args)
0105     {
0106         static_assert(
0107             (std::is_convertible_v<Args, T> && ...),
0108             "Scaling arguments must be convertible to transformation matrix "
0109             "elements."
0110         );
0111         static_assert(
0112             sizeof...(Args) == N,
0113             "Scaling must have exactly as many arguments as the dimensions of "
0114             "the matrix."
0115         );
0116
0117         array::array<T, N> arr{args...};
0118
0119         matrix<N, N + 1, T, I> result = matrix<N, N + 1, T, I>::identity();
0120
0121         for (I i = 0; i < N; ++i) {
0122             result(i, i) = arr[i];
0123         }
0124
0125         return result;
0126     }
0127 };
0128 }