core_math/tjp/core/math/Matrix3.inl

//
//  Math.inl
//  amoeba
//
//  Created by Timothy Prepscius on 12/30/16.
//  Copyright © 2016 Timothy Prepscius. All rights reserved.
//
#pragma once

#include "Matrix3.hpp"
#include "HPoint.hpp"
#include "Zero.h"

#include <cmath>


#include "HMatrix.inl"

namespace tjp {
namespace core {
namespace math {

//// inline
//template<typename Real>
//Matrix3<Real>::Matrix3(std::initializer_list<Real> l)
//{
//	int i=0;
//	for (auto &r : l)
//		v[i++] = r;
//}

/*
// inline
template<typename Real>
Matrix3<Real>::Matrix3<Real> (const Vector3<Real> &a, const Vector3<Real> &b)
{
	// http://gamedev.stackexchange.com/questions/20097/how-to-calculate-a-3x3-rotation-matrix-from-2-direction-vectors
	
	auto x = a.normalize();
	auto y = b.normalize();
	
	Matrix3<Real> &m = *this;
	
	Vector3<Real> *mx = (Vector3<Real> *)m[0];
	Vector3<Real> *my = (Vector3<Real> *)m[1];
	Vector3<Real> *mz = (Vector3<Real> *)m[2];
	
    *mx = x;
    *mz = (x^y).normalize();
	*my = (*mz ^ x).normalize();
}
*/

template<typename Real>
Matrix3<Real>::Matrix3 (const Vector3<Real> &a, const Vector3<Real> &b, const Vector3<Real> &c)
{
	v[0] = a[0];
	v[1] = a[1];
	v[2] = a[2];
	v[3] = b[0];
	v[4] = b[1];
	v[5] = b[2];
	v[6] = c[0];
	v[7] = c[1];
	v[8] = c[2];
}


// inline
template<typename Real>
Matrix3<Real>::Matrix3 (const Vector3<Real> &a, const Vector3<Real> &b)
{
	// http://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
	
	auto acb = a ^ b;
	auto s = acb.length();
	auto c = a * b;
	
	Real vn[] {
		c, -s, 0,
		s, c, 0,
		0, 0, 1
	};
	
	for (int i=0; i<9; ++i)
		v[i] = vn[i];
}

template<typename Real>
Matrix3<Real> rotationMatrix(const Vector3<Real> &axis, const Real &angle)
{
    Real cosTheta = std::cos(angle);
    Real sinTheta = std::sin(angle);
    Real oneMinusCosTheta = 1 - cosTheta;

    Real x = axis[0];
    Real y = axis[1];
    Real z = axis[2];

	Matrix3<Real> result;
    result[0][0] = cosTheta + x * x * oneMinusCosTheta;
    result[0][1] = x * y * oneMinusCosTheta - z * sinTheta;
    result[0][2] = x * z * oneMinusCosTheta + y * sinTheta;

    result[1][0] = y * x * oneMinusCosTheta + z * sinTheta;
    result[1][1] = cosTheta + y * y * oneMinusCosTheta;
    result[1][2] = y * z * oneMinusCosTheta - x * sinTheta;

    result[2][0] = z * x * oneMinusCosTheta - y * sinTheta;
    result[2][1] = z * y * oneMinusCosTheta + x * sinTheta;
    result[2][2] = cosTheta + z * z * oneMinusCosTheta;
    
    return result;
}

// inline
template<typename Real>
Matrix3<Real> Matrix3<Real>::transpose() const
{
    return Matrix3<Real>({
        v[0],
        v[3],
        v[6],
        v[1],
        v[4],
        v[7],
        v[2],
        v[5],
        v[8]
    });
}

// inline
template<typename Real>
Matrix3<Real> Matrix3<Real>::inverse(const Real epsilon) const
{
    // Invert a 3x3 using cofactors.  This is faster than using a generic
    // Gaussian elimination because of the loop overhead of such a method.

    Matrix3 inverse;

    // Compute the adjoint.
    inverse.v[0] = v[4]*v[8] - v[5]*v[7];
    inverse.v[1] = v[2]*v[7] - v[1]*v[8];
    inverse.v[2] = v[1]*v[5] - v[2]*v[4];
    inverse.v[3] = v[5]*v[6] - v[3]*v[8];
    inverse.v[4] = v[0]*v[8] - v[2]*v[6];
    inverse.v[5] = v[2]*v[3] - v[0]*v[5];
    inverse.v[6] = v[3]*v[7] - v[4]*v[6];
    inverse.v[7] = v[1]*v[6] - v[0]*v[7];
    inverse.v[8] = v[0]*v[4] - v[1]*v[3];

    Real det = v[0]*inverse.v[0] + v[1]*inverse.v[3] +
        v[2]*inverse.v[6];

    if (std::abs(det) > epsilon)
    {
        Real invDet = ((Real)1)/det;
        inverse.v[0] *= invDet;
        inverse.v[1] *= invDet;
        inverse.v[2] *= invDet;
        inverse.v[3] *= invDet;
        inverse.v[4] *= invDet;
        inverse.v[5] *= invDet;
        inverse.v[6] *= invDet;
        inverse.v[7] *= invDet;
        inverse.v[8] *= invDet;
        return inverse;
    }

    return Zero;
}

// inline
template<typename Real>
Vector3<Real> operator *(const Matrix3<Real> &m, const Vector3<Real> &p)
{
	HMatrix<Real> hm(m, Vector3<Real>::Zero, { 1, 1, 1 });
	HPoint<Real> hp(p);
	auto v = hm * hp;
	return { v.v[0], v.v[1], v.v[2] };
}

// inline
template<typename Real>
const Real* Matrix3<Real>::operator[] (size_t row) const
{
    return &v[3*row];
}

// inline
template<typename Real>
Real* Matrix3<Real>::operator[] (size_t row)
{
    return &v[3*row];
}

} // namespace
} // namespace
} // namespace