path: root/extern/bullet/LinearMath/SimdQuaternion.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 SIMD__QUATERNION_H_
#define SIMD__QUATERNION_H_

#include "SimdVector3.h"

class SimdQuaternion : public SimdQuadWord {
public:
	SimdQuaternion() {}

	//		template <typename SimdScalar>
	//		explicit Quaternion(const SimdScalar *v) : Tuple4<SimdScalar>(v) {}

	SimdQuaternion(const SimdScalar& x, const SimdScalar& y, const SimdScalar& z, const SimdScalar& w) 
		: SimdQuadWord(x, y, z, w) 
	{}

	SimdQuaternion(const SimdVector3& axis, const SimdScalar& angle) 
	{ 
		setRotation(axis, angle); 
	}

	SimdQuaternion(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
	{ 
		setEuler(yaw, pitch, roll); 
	}

	void setRotation(const SimdVector3& axis, const SimdScalar& angle)
	{
		SimdScalar d = axis.length();
		assert(d != SimdScalar(0.0));
		SimdScalar s = SimdSin(angle * SimdScalar(0.5)) / d;
		setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
			SimdCos(angle * SimdScalar(0.5)));
	}

	void setEuler(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
	{
		SimdScalar halfYaw = SimdScalar(yaw) * SimdScalar(0.5);  
		SimdScalar halfPitch = SimdScalar(pitch) * SimdScalar(0.5);  
		SimdScalar halfRoll = SimdScalar(roll) * SimdScalar(0.5);  
		SimdScalar cosYaw = SimdCos(halfYaw);
		SimdScalar sinYaw = SimdSin(halfYaw);
		SimdScalar cosPitch = SimdCos(halfPitch);
		SimdScalar sinPitch = SimdSin(halfPitch);
		SimdScalar cosRoll = SimdCos(halfRoll);
		SimdScalar sinRoll = SimdSin(halfRoll);
		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
			cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
			sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
			cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
	}

	SimdQuaternion& operator+=(const SimdQuaternion& q)
	{
		m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q[3];
		return *this;
	}

	SimdQuaternion& operator-=(const SimdQuaternion& q) 
	{
		m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q[3];
		return *this;
	}

	SimdQuaternion& operator*=(const SimdScalar& s)
	{
		m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
		return *this;
	}


	SimdQuaternion& operator*=(const SimdQuaternion& q)
	{
		setValue(m_unusedW * q.x() + m_x * q[3] + m_y * q.z() - m_z * q.y(),
			m_unusedW * q.y() + m_y * q[3] + m_z * q.x() - m_x * q.z(),
			m_unusedW * q.z() + m_z * q[3] + m_x * q.y() - m_y * q.x(),
			m_unusedW * q[3] - m_x * q.x() - m_y * q.y() - m_z * q.z());
		return *this;
	}

	SimdScalar dot(const SimdQuaternion& q) const
	{
		return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q[3];
	}

	SimdScalar length2() const
	{
		return dot(*this);
	}

	SimdScalar length() const
	{
		return SimdSqrt(length2());
	}

	SimdQuaternion& normalize() 
	{
		return *this /= length();
	}

	SIMD_FORCE_INLINE SimdQuaternion
	operator*(const SimdScalar& s) const
	{
		return SimdQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
	}



	SimdQuaternion operator/(const SimdScalar& s) const
	{
		assert(s != SimdScalar(0.0));
		return *this * (SimdScalar(1.0) / s);
	}


	SimdQuaternion& operator/=(const SimdScalar& s) 
	{
		assert(s != SimdScalar(0.0));
		return *this *= SimdScalar(1.0) / s;
	}


	SimdQuaternion normalized() const 
	{
		return *this / length();
	} 

	SimdScalar angle(const SimdQuaternion& q) const 
	{
		SimdScalar s = SimdSqrt(length2() * q.length2());
		assert(s != SimdScalar(0.0));
		return SimdAcos(dot(q) / s);
	}

	SimdScalar getAngle() const 
	{
		SimdScalar s = 2.f * SimdAcos(m_unusedW);
		return s;
	}



	SimdQuaternion inverse() const
	{
		return SimdQuaternion(m_x, m_y, m_z, -m_unusedW);
	}

	SIMD_FORCE_INLINE SimdQuaternion
	operator+(const SimdQuaternion& q2) const
	{
		const SimdQuaternion& q1 = *this;
		return SimdQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1[3] + q2[3]);
	}

	SIMD_FORCE_INLINE SimdQuaternion
	operator-(const SimdQuaternion& q2) const
	{
		const SimdQuaternion& q1 = *this;
		return SimdQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1[3] - q2[3]);
	}

	SIMD_FORCE_INLINE SimdQuaternion operator-() const
	{
		const SimdQuaternion& q2 = *this;
		return SimdQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2[3]);
	}

	SIMD_FORCE_INLINE SimdQuaternion farthest( const SimdQuaternion& qd) const 
	{
		SimdQuaternion diff,sum;
		diff = *this - qd;
		sum = *this + qd;
		if( diff.dot(diff) > sum.dot(sum) )
			return qd;
		return (-qd);
	}

	SimdQuaternion slerp(const SimdQuaternion& q, const SimdScalar& t) const
	{
		SimdScalar theta = angle(q);
		if (theta != SimdScalar(0.0))
		{
			SimdScalar d = SimdScalar(1.0) / SimdSin(theta);
			SimdScalar s0 = SimdSin((SimdScalar(1.0) - t) * theta);
			SimdScalar s1 = SimdSin(t * theta);   
			return SimdQuaternion((m_x * s0 + q.x() * s1) * d,
				(m_y * s0 + q.y() * s1) * d,
				(m_z * s0 + q.z() * s1) * d,
				(m_unusedW * s0 + q[3] * s1) * d);
		}
		else
		{
			return *this;
		}
	}

	

};



SIMD_FORCE_INLINE SimdQuaternion
operator-(const SimdQuaternion& q)
{
	return SimdQuaternion(-q.x(), -q.y(), -q.z(), -q[3]);
}




SIMD_FORCE_INLINE SimdQuaternion
operator*(const SimdQuaternion& q1, const SimdQuaternion& q2) {
	return SimdQuaternion(q1[3] * q2.x() + q1.x() * q2[3] + q1.y() * q2.z() - q1.z() * q2.y(),
		q1[3] * q2.y() + q1.y() * q2[3] + q1.z() * q2.x() - q1.x() * q2.z(),
		q1[3] * q2.z() + q1.z() * q2[3] + q1.x() * q2.y() - q1.y() * q2.x(),
		q1[3] * q2[3] - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
}

SIMD_FORCE_INLINE SimdQuaternion
operator*(const SimdQuaternion& q, const SimdVector3& w)
{
	return SimdQuaternion( q[3] * w.x() + q.y() * w.z() - q.z() * w.y(),
		q[3] * w.y() + q.z() * w.x() - q.x() * w.z(),
		q[3] * w.z() + q.x() * w.y() - q.y() * w.x(),
		-q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
}

SIMD_FORCE_INLINE SimdQuaternion
operator*(const SimdVector3& w, const SimdQuaternion& q)
{
	return SimdQuaternion( w.x() * q[3] + w.y() * q.z() - w.z() * q.y(),
		w.y() * q[3] + w.z() * q.x() - w.x() * q.z(),
		w.z() * q[3] + w.x() * q.y() - w.y() * q.x(),
		-w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
}

SIMD_FORCE_INLINE SimdScalar 
dot(const SimdQuaternion& q1, const SimdQuaternion& q2) 
{ 
	return q1.dot(q2); 
}


SIMD_FORCE_INLINE SimdScalar
length(const SimdQuaternion& q) 
{ 
	return q.length(); 
}

SIMD_FORCE_INLINE SimdScalar
angle(const SimdQuaternion& q1, const SimdQuaternion& q2) 
{ 
	return q1.angle(q2); 
}


SIMD_FORCE_INLINE SimdQuaternion
inverse(const SimdQuaternion& q) 
{
	return q.inverse();
}

SIMD_FORCE_INLINE SimdQuaternion
slerp(const SimdQuaternion& q1, const SimdQuaternion& q2, const SimdScalar& t) 
{
	return q1.slerp(q2, t);
}


#endif