cpp-mathmatic-matrix/include/math/matrix/matrix_helpers.hpp

//
//  matrix_helpers.hpp
//  math
//
//  Created by Sam Jaffe on 8/20/16.
//

#pragma once

#include "matrix.hpp"

namespace math { namespace matrix {

  template <typename T, std::size_t N> matrix<T, N, N> identity() {
    matrix<T, N, N> rval;
    VECTOR_FOR_EACH(i) { rval.at(i, i) = 1; }
    return rval;
  }

  template <typename T, std::size_t N>
  matrix<T, N, N> diagonal(vector::vector<T, N> const & vec) {
    matrix<T, N, N> rval = identity<T, N>();
    VECTOR_FOR_EACH(i) { rval.at(i, i) = vec[i]; }
    return rval;
  }

  template <typename T, std::size_t N>
  matrix<T, N + 1, N + 1> translation(vector::vector<T, N> const & vec) {
    matrix<T, N + 1, N + 1> rval = identity<T, N + 1>();
    VECTOR_FOR_EACH(i) { rval.at(i, N) = vec[i]; }
    return rval;
  }

  template <typename T, std::size_t N>
  matrix<T, N + 1, N + 1> scalar(vector::vector<T, N> const & vec) {
    matrix<T, N + 1, N + 1> rval = identity<T, N + 1>();
    VECTOR_FOR_EACH(i) { rval.at(i, i) = vec[i]; }
    return rval;
  }

  template <size_t N> struct rotation_t {
    constexpr rotation_t(size_t f, size_t s) : first(f), second(s) {}
    size_t first, second;
  };

  namespace rotate {
    constexpr rotation_t<3> const X_AXIS{1, 2};
    constexpr rotation_t<3> const Y_AXIS{2, 0};
    constexpr rotation_t<3> const Z_AXIS{0, 1};

    constexpr rotation_t<3> const ROLL{1, 2};
    constexpr rotation_t<3> const PITCH{2, 0};
    constexpr rotation_t<3> const YAW{0, 1};

    constexpr rotation_t<2> const ROT_2D{0, 1};
  }

  template <typename T, size_t D>
  auto rotation(T theta, rotation_t<D> r)
      -> matrix<decltype(sin(theta)), D, D> {
    static_assert(D >= 2, "cannot rotate with 1D matrix");
    using G = decltype(sin(theta));
    using std::cos;
    using std::sin;
    matrix<G, D, D> rval = identity<G, D>();
    G const vsin = sin(theta);
    G const vcos = cos(theta);
    rval.at(r.first, r.first) = vcos;
    rval.at(r.second, r.second) = vcos;
    rval.at(r.first, r.second) = -vsin;
    rval.at(r.second, r.first) = vsin;
    return rval;
  }

  template <size_t D, typename T, size_t RD>
  auto rotation(T theta, rotation_t<RD> r)
      -> matrix<decltype(sin(theta)), D, D> {
    static_assert(D > RD,
                  "rotation<D> has to increase the number of dimensions");
    using G = decltype(sin(theta));
    return rotation(theta, r).concat(identity<G, D - RD>(),
                                     concat_strategy::diagonal);
  }

  template <typename T, size_t N>
  vector::vector<T, N> operator*(matrix<T, N + 1, N + 1> const & lhs,
                                 vector::vector<T, N> const & rhs) {
    vector::vector<T, N + 1> tmp(rhs);
    tmp[N] = 1;
    return vector::vector<T, N>(lhs * tmp);
  }

}}