watchrobot/libraries/AP_Math/matrix4.cpp

/*
 * matrix3.cpp
 * Copyright (C) Andrew Tridgell 2012
 *
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma GCC optimize("O2")

#include "AP_Math.h"


// multiplication by a vector
template <typename T>
Vector4<T> Matrix4<T>::operator *(const Vector4<T> &v) const
{
    return Vector4<T>(a.M0 * v.M0 + a.M1 * v.M1 + a.M2 * v.M2 + a.M3 * v.M3,
                      b.M0 * v.M0 + b.M1 * v.M1 + b.M2 * v.M2 + b.M3 * v.M3,
                      c.M0 * v.M0 + c.M1 * v.M1 + c.M2 * v.M2 + c.M3 * v.M3,
                      d.M0 * v.M0 + d.M1 * v.M1 + d.M2 * v.M2 + d.M3 * v.M3);
}


template <typename T>
T Matrix4<T>::det() const
{
	Matrix3<T> m11(b.M1, b.M2, b.M3,
		           c.M1, c.M2, c.M3,
		           d.M1, d.M2, d.M3);
	
	Matrix3<T> m12(b.M0, b.M2, b.M3,
		           c.M0, c.M2, c.M3,
		           d.M0, d.M2, d.M3);
	
	Matrix3<T> m13(b.M0, b.M1, b.M3,
		           c.M0, c.M1, c.M3,
		           d.M0, d.M1, d.M3);
	
	Matrix3<T> m14(b.M0, b.M1, b.M2,
		           c.M0, c.M1, c.M2,
		           d.M0, d.M1, d.M2);
	
    T Matrix_det = a.M0*(m11.det())
    			 -a.M1*(m12.det())
    			 +a.M2*(m13.det())
    			 -a.M3*(m14.det());
	return Matrix_det;

}

template <typename T>
bool Matrix4<T>::inverse(Matrix4<T>& inv) const
{
    const T d_val = det();

    if (is_zero(d_val)) {
        return false;
    }
	//1 lie
	Matrix3<T> m11(b.M1, b.M2, b.M3,
		           c.M1, c.M2, c.M3,
		           d.M1, d.M2, d.M3);
	inv.a.M0 = m11.det()/d_val;
	
	Matrix3<T> m12(b.M0, b.M2, b.M3,
		           c.M0, c.M2, c.M3,
		           d.M0, d.M2, d.M3);
	inv.b.M0 = -m12.det()/d_val;
	
	Matrix3<T> m13(b.M0, b.M1, b.M3,
		           c.M0, c.M1, c.M3,
		           d.M0, d.M1, d.M3);
	inv.c.M0 = m13.det()/d_val;
	Matrix3<T> m14(b.M0, b.M1, b.M2,
		           c.M0, c.M1, c.M2,
		           d.M0, d.M1, d.M2);
	inv.d.M0 = -m14.det()/d_val;
	//2 lie----------------

	Matrix3<T> m21(a.M1, a.M2, a.M3,
		           c.M1, c.M2, c.M3,
		           d.M1, d.M2, d.M3);
	inv.a.M1 = -m21.det()/d_val;
	
	Matrix3<T> m22(a.M0, a.M2, a.M3,
		           c.M0, c.M2, c.M3,
		           d.M0, d.M2, d.M3);
	inv.b.M1 = m22.det()/d_val;
	
	Matrix3<T> m23(a.M0, a.M1, a.M3,
		           c.M0, c.M1, c.M3,
		           d.M0, d.M1, d.M3);
	inv.c.M1 = -m23.det()/d_val;
	Matrix3<T> m24(a.M0, a.M1, a.M2,
		           c.M0, c.M1, c.M2,
		           d.M0, d.M1, d.M2);
	inv.d.M1 = m24.det()/d_val;
	//3 lie---------------------
	Matrix3<T> m31(a.M1, a.M2, a.M3,
		           b.M1, b.M2, b.M3,
		           d.M1, d.M2, d.M3);
	inv.a.M2 = m31.det()/d_val;
	
	Matrix3<T> m32(a.M0, a.M2, a.M3,
		           b.M0, b.M2, b.M3,
		           d.M0, d.M2, d.M3);
	inv.b.M2 = -m32.det()/d_val;
	
	Matrix3<T> m33(a.M0, a.M1, a.M3,
		           b.M0, b.M1, b.M3,
		           d.M0, d.M1, d.M3);
	inv.c.M2 = m33.det()/d_val;
	Matrix3<T> m34(a.M0, a.M1, a.M2,
		           b.M0, b.M1, b.M2,
		           d.M0, d.M1, d.M2);
	inv.d.M2 = -m34.det()/d_val;
	//4 lie---------------------
	Matrix3<T> m41(a.M1, a.M2, a.M3,
		           b.M1, b.M2, b.M3,
		           c.M1, c.M2, c.M3);
	inv.a.M3 = -m41.det()/d_val;
	
	Matrix3<T> m42(a.M0, a.M2, a.M3,
		           b.M0, b.M2, b.M3,
		           c.M0, c.M2, c.M3);
	inv.b.M3 = m42.det()/d_val;
	
	Matrix3<T> m43(a.M0, a.M1, a.M3,
		           b.M0, b.M1, b.M3,
		           c.M0, c.M1, c.M3);
	inv.c.M3 = -m43.det()/d_val;
	Matrix3<T> m44(a.M0, a.M1, a.M2,
		           b.M0, b.M1, b.M2,
		           c.M0, c.M1, c.M2);
	inv.d.M3 = m44.det()/d_val;


    return true;
}


template <typename T>
void Matrix4<T>::zero(void)
{
    a.M0 = a.M1 = a.M2 = a.M3 = 0;
    b.M0 = b.M1 = b.M2 = b.M3 = 0;
    c.M0 = c.M1 = c.M2 = c.M3 = 0;
	d.M0 = d.M1 = d.M2 = d.M3 = 0;
	
}




// only define for float
template void Matrix4<float>::zero(void);
template Vector4<float> Matrix4<float>::operator *(const Vector4<float> &v) const;
template float Matrix4<float>::det() const;
template bool Matrix4<float>::inverse(Matrix4<float>& inv) const;