blender/extern/bullet/LinearMath/SimdMatrix3x3.h

/*

Copyright (c) 2005 Gino van den Bergen / Erwin Coumans http://continuousphysics.com

Permission is hereby granted, free of charge, to any person or organization
obtaining a copy of the software and accompanying documentation covered by
this license (the "Software") to use, reproduce, display, distribute,
execute, and transmit the Software, and to prepare derivative works of the
Software, and to permit third-parties to whom the Software is furnished to
do so, all subject to the following:

The copyright notices in the Software and this entire statement, including
the above license grant, this restriction and the following disclaimer,
must be included in all copies of the Software, in whole or in part, and
all derivative works of the Software, unless such copies or derivative
works are solely in the form of machine-executable object code generated by
a source language processor.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
*/

#ifndef SimdMatrix3x3_H
#define SimdMatrix3x3_H

#include "SimdScalar.h"

#include "SimdVector3.h"
#include "SimdQuaternion.h"


class SimdMatrix3x3 {
	public:
		SimdMatrix3x3 () {}
		
//		explicit SimdMatrix3x3(const SimdScalar *m) { setFromOpenGLSubMatrix(m); }
		
		explicit SimdMatrix3x3(const SimdQuaternion& q) { setRotation(q); }
		/*
		template <typename SimdScalar>
		Matrix3x3(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
		{ 
			setEulerYPR(yaw, pitch, roll);
		}
		*/
		SimdMatrix3x3(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz,
				  const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz,
				  const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
		{ 
			setValue(xx, xy, xz, 
					 yx, yy, yz, 
					 zx, zy, zz);
		}
		
		SimdVector3 getColumn(int i) const
		{
			return SimdVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
		}

		const SimdVector3& getRow(int i) const
		{
			return m_el[i];
		}


		SIMD_FORCE_INLINE SimdVector3&  operator[](int i)
		{ 
			assert(0 <= i && i < 3);
			return m_el[i]; 
		}
		
		const SimdVector3& operator[](int i) const
		{
			assert(0 <= i && i < 3);
			return m_el[i]; 
		}
		
		SimdMatrix3x3& operator*=(const SimdMatrix3x3& m); 
		
	
	void setFromOpenGLSubMatrix(const SimdScalar *m)
		{
			m_el[0][0] = (m[0]); 
			m_el[1][0] = (m[1]); 
			m_el[2][0] = (m[2]);
			m_el[0][1] = (m[4]); 
			m_el[1][1] = (m[5]); 
			m_el[2][1] = (m[6]);
			m_el[0][2] = (m[8]); 
			m_el[1][2] = (m[9]); 
			m_el[2][2] = (m[10]);
		}

		void setValue(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz, 
					  const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz, 
					  const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
		{
			m_el[0][0] = SimdScalar(xx); 
			m_el[0][1] = SimdScalar(xy); 
			m_el[0][2] = SimdScalar(xz);
			m_el[1][0] = SimdScalar(yx); 
			m_el[1][1] = SimdScalar(yy); 
			m_el[1][2] = SimdScalar(yz);
			m_el[2][0] = SimdScalar(zx); 
			m_el[2][1] = SimdScalar(zy); 
			m_el[2][2] = SimdScalar(zz);
		}
  
		void setRotation(const SimdQuaternion& q) 
		{
			SimdScalar d = q.length2();
			assert(d != SimdScalar(0.0));
			SimdScalar s = SimdScalar(2.0) / d;
			SimdScalar xs = q[0] * s,   ys = q[1] * s,   zs = q[2] * s;
			SimdScalar wx = q[3] * xs,  wy = q[3] * ys,  wz = q[3] * zs;
			SimdScalar xx = q[0] * xs,  xy = q[0] * ys,  xz = q[0] * zs;
			SimdScalar yy = q[1] * ys,  yz = q[1] * zs,  zz = q[2] * zs;
			setValue(SimdScalar(1.0) - (yy + zz), xy - wz, xz + wy,
					 xy + wz, SimdScalar(1.0) - (xx + zz), yz - wx,
					 xz - wy, yz + wx, SimdScalar(1.0) - (xx + yy));
		}
		


		void setEulerYPR(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll) 
		{

			SimdScalar cy(SimdCos(yaw)); 
			SimdScalar  sy(SimdSin(yaw)); 
			SimdScalar  cp(SimdCos(pitch)); 
			SimdScalar  sp(SimdSin(pitch)); 
			SimdScalar  cr(SimdCos(roll));
			SimdScalar  sr(SimdSin(roll));
			SimdScalar  cc = cy * cr; 
			SimdScalar  cs = cy * sr; 
			SimdScalar  sc = sy * cr; 
			SimdScalar  ss = sy * sr;
			setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
                     cp * sr,       cp * cr,      -sp,
					 sc + sp * cs, -ss + sp * cc,  cy * cp);
		
		}

	/**
	 * setEulerZYX
	 * @param euler a const reference to a SimdVector3 of euler angles
	 * These angles are used to produce a rotation matrix. The euler
	 * angles are applied in ZYX order. I.e a vector is first rotated 
	 * about X then Y and then Z
	 **/
	
	void setEulerZYX(SimdScalar eulerX,SimdScalar eulerY,SimdScalar eulerZ) {
		SimdScalar ci ( SimdCos(eulerX)); 
		SimdScalar cj ( SimdCos(eulerY)); 
		SimdScalar ch ( SimdCos(eulerZ)); 
		SimdScalar si ( SimdSin(eulerX)); 
		SimdScalar sj ( SimdSin(eulerY)); 
		SimdScalar sh ( SimdSin(eulerZ)); 
		SimdScalar cc = ci * ch; 
		SimdScalar cs = ci * sh; 
		SimdScalar sc = si * ch; 
		SimdScalar ss = si * sh;
		
		setValue(cj * ch, sj * sc - cs, sj * cc + ss,
				 cj * sh, sj * ss + cc, sj * cs - sc, 
	       			 -sj,      cj * si,      cj * ci);
	}

		void setIdentity()
		{ 
			setValue(SimdScalar(1.0), SimdScalar(0.0), SimdScalar(0.0), 
					 SimdScalar(0.0), SimdScalar(1.0), SimdScalar(0.0), 
					 SimdScalar(0.0), SimdScalar(0.0), SimdScalar(1.0)); 
		}
    
		void getOpenGLSubMatrix(SimdScalar *m) const 
		{
			m[0]  = SimdScalar(m_el[0][0]); 
			m[1]  = SimdScalar(m_el[1][0]);
			m[2]  = SimdScalar(m_el[2][0]);
			m[3]  = SimdScalar(0.0); 
			m[4]  = SimdScalar(m_el[0][1]);
			m[5]  = SimdScalar(m_el[1][1]);
			m[6]  = SimdScalar(m_el[2][1]);
			m[7]  = SimdScalar(0.0); 
			m[8]  = SimdScalar(m_el[0][2]); 
			m[9]  = SimdScalar(m_el[1][2]);
			m[10] = SimdScalar(m_el[2][2]);
			m[11] = SimdScalar(0.0); 
		}

		void getRotation(SimdQuaternion& q) const
		{
			SimdScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
			
			if (trace > SimdScalar(0.0)) 
			{
				SimdScalar s = SimdSqrt(trace + SimdScalar(1.0));
				q[3] = s * SimdScalar(0.5);
				s = SimdScalar(0.5) / s;
				
				q[0] = (m_el[2][1] - m_el[1][2]) * s;
				q[1] = (m_el[0][2] - m_el[2][0]) * s;
				q[2] = (m_el[1][0] - m_el[0][1]) * s;
			} 
			else 
			{
				int i = m_el[0][0] < m_el[1][1] ? 
					(m_el[1][1] < m_el[2][2] ? 2 : 1) :
					(m_el[0][0] < m_el[2][2] ? 2 : 0); 
				int j = (i + 1) % 3;  
				int k = (i + 2) % 3;
				
				SimdScalar s = SimdSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + SimdScalar(1.0));
				q[i] = s * SimdScalar(0.5);
				s = SimdScalar(0.5) / s;
				
				q[3] = (m_el[k][j] - m_el[j][k]) * s;
				q[j] = (m_el[j][i] + m_el[i][j]) * s;
				q[k] = (m_el[k][i] + m_el[i][k]) * s;
			}
		}
	

		
		void getEuler(SimdScalar& yaw, SimdScalar& pitch, SimdScalar& roll) const
		{
			pitch = SimdScalar(SimdAsin(-m_el[2][0]));
			if (pitch < SIMD_2_PI)
			{
				if (pitch > SIMD_2_PI)
				{
					yaw = SimdScalar(SimdAtan2(m_el[1][0], m_el[0][0]));
					roll = SimdScalar(SimdAtan2(m_el[2][1], m_el[2][2]));
				}
				else 
				{
					yaw = SimdScalar(-SimdAtan2(-m_el[0][1], m_el[0][2]));
					roll = SimdScalar(0.0);
				}
			}
			else
			{
				yaw = SimdScalar(SimdAtan2(-m_el[0][1], m_el[0][2]));
				roll = SimdScalar(0.0);
			}
		}

		SimdVector3 getScaling() const
		{
			return SimdVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0],
								   m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1],
								   m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]);
		}
		
		
		SimdMatrix3x3 scaled(const SimdVector3& s) const
		{
			return SimdMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2],
									 m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2],
									 m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]);
		}

		SimdScalar            determinant() const;
		SimdMatrix3x3 adjoint() const;
		SimdMatrix3x3 absolute() const;
		SimdMatrix3x3 transpose() const;
		SimdMatrix3x3 inverse() const; 
		
		SimdMatrix3x3 transposeTimes(const SimdMatrix3x3& m) const;
		SimdMatrix3x3 timesTranspose(const SimdMatrix3x3& m) const;
		
		SimdScalar tdot(int c, const SimdVector3& v) const 
		{
			return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
		}
		
	protected:
		SimdScalar cofac(int r1, int c1, int r2, int c2) const 
		{
			return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
		}

		SimdVector3 m_el[3];
	};
	
	SIMD_FORCE_INLINE SimdMatrix3x3& 
	SimdMatrix3x3::operator*=(const SimdMatrix3x3& m)
	{
		setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
				 m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
				 m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
		return *this;
	}
	
	SIMD_FORCE_INLINE SimdScalar 
	SimdMatrix3x3::determinant() const
	{ 
		return triple((*this)[0], (*this)[1], (*this)[2]);
	}
	

	SIMD_FORCE_INLINE SimdMatrix3x3 
	SimdMatrix3x3::absolute() const
	{
		return SimdMatrix3x3(
			SimdFabs(m_el[0][0]), SimdFabs(m_el[0][1]), SimdFabs(m_el[0][2]),
			SimdFabs(m_el[1][0]), SimdFabs(m_el[1][1]), SimdFabs(m_el[1][2]),
			SimdFabs(m_el[2][0]), SimdFabs(m_el[2][1]), SimdFabs(m_el[2][2]));
	}

	SIMD_FORCE_INLINE SimdMatrix3x3 
	SimdMatrix3x3::transpose() const 
	{
		return SimdMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
								 m_el[0][1], m_el[1][1], m_el[2][1],
								 m_el[0][2], m_el[1][2], m_el[2][2]);
	}
	
	SIMD_FORCE_INLINE SimdMatrix3x3 
	SimdMatrix3x3::adjoint() const 
	{
		return SimdMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
								 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
								 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
	}
	
	SIMD_FORCE_INLINE SimdMatrix3x3 
	SimdMatrix3x3::inverse() const
	{
		SimdVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
		SimdScalar det = (*this)[0].dot(co);
		assert(det != SimdScalar(0.0f));
		SimdScalar s = SimdScalar(1.0f) / det;
		return SimdMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
								 co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
								 co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
	}
	
	SIMD_FORCE_INLINE SimdMatrix3x3 
	SimdMatrix3x3::transposeTimes(const SimdMatrix3x3& m) const
	{
		return SimdMatrix3x3(
			m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0],
			m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1],
			m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2],
			m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0],
			m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1],
			m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2],
			m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0],
			m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1],
			m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]);
	}
	
	SIMD_FORCE_INLINE SimdMatrix3x3 
	SimdMatrix3x3::timesTranspose(const SimdMatrix3x3& m) const
	{
		return SimdMatrix3x3(
			m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
			m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
			m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
		
	}

	SIMD_FORCE_INLINE SimdVector3 
	operator*(const SimdMatrix3x3& m, const SimdVector3& v) 
	{
		return SimdVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
	}
	

	SIMD_FORCE_INLINE SimdVector3
	operator*(const SimdVector3& v, const SimdMatrix3x3& m)
	{
		return SimdVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
	}

	SIMD_FORCE_INLINE SimdMatrix3x3 
	operator*(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2)
	{
		return SimdMatrix3x3(
			m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
			m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
			m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
	}


	SIMD_FORCE_INLINE SimdMatrix3x3 SimdMultTransposeLeft(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2) {
    return SimdMatrix3x3(
        m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
        m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
        m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
        m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
        m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
        m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
        m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
        m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
        m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
}


#endif