#include "CoordinateSpace.h"

namespace Dubious {
namespace Math {

//////////////////////////////////////////////////////////////
// This usually comes in handy:
// http://www.cs.princeton.edu/~gewang/projects/darth/stuff/quat_faq.html

//////////////////////////////////////////////////////////////
CoordinateSpace::CoordinateSpace()
	: m_Transform( btQuaternion( btVector3( 0, 0, 1 ), 0 ) )
{
}

//////////////////////////////////////////////////////////////
CoordinateSpace::CoordinateSpace( const btTransform& Transform )
	: m_Transform( Transform )
{
}

//////////////////////////////////////////////////////////////
CoordinateSpace::~CoordinateSpace()
{
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::SetPosition( const Vector& V )
{
	m_Transform.setOrigin( V.Vector() );
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::SetPosition( const LocalVector& V )
{
	SetPosition( Transform( V ) );
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::Translate( const Vector& V )
{
	m_Transform.setOrigin( m_Transform.getOrigin() + V.Vector());
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::Translate( const LocalVector& V )
{
	Translate( Transform( V ) );
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::Rotate( const Rotation& R )
{
	m_Transform.setRotation( R.Quaternion() * m_Transform.getRotation() );
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::Rotate( const LocalRotation& R )
{
	m_Transform.setRotation( m_Transform.getRotation() * R.Quaternion() );
}

//////////////////////////////////////////////////////////////
const Vector CoordinateSpace::Position() const
{
	return Vector( m_Transform.getOrigin() );
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::Axes( Vector& X, Vector& Y, Vector& Z ) const
{
	// The proper math would be to do:
	// X = Transform( LocalVector( 1, 0, 0 ) );
	// however there's a thing about rotation matrixes, their
	// columns already hold the X, Y, and Z axes.  So rather
	// then do the multiplications, I just copy them
	X = Vector( m_Transform.getBasis().getColumn( 0 ) );
	Y = Vector( m_Transform.getBasis().getColumn( 1 ) );
	Z = Vector( m_Transform.getBasis().getColumn( 2 ) );
}

//////////////////////////////////////////////////////////////
const Vector CoordinateSpace::Transform( const LocalVector& V ) const
{
	return m_Transform.getBasis() * V.Vector();
}

//////////////////////////////////////////////////////////////
const LocalVector CoordinateSpace::Transform( const Vector& V ) const
{
	// The correct answer to this is
	// V' = Mi * V
	// (where Mi is the Matrix inverse)
	// Since the determinent of a rotation matrix is 1, the
	// inverse is the same as the transpose.  And since the
	// transpose is so much easier to find, we can use
	// V' = Mt * V
	return m_Transform.getBasis().transpose() * V.Vector();
}

//////////////////////////////////////////////////////////////
void CoordinateSpace::GLMatrix( float m[16] ) const
{
	m_Transform.getOpenGLMatrix( m );
}

}
}
