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

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

#pragma once

#include "math/matrix/matrix.hpp"

#include "math/matrix/macro.h"

namespace math::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);
}

}

#include "math/matrix/undef.h"