#include "util.h"
#include "XString.h"

DXVector3 DecomposeRollPitchYaw(D3DXMATRIX mat) 
{ 
	DXVector3 rot;

	rot.x = sin(-mat._32); 

	float threshold = 0.001f;

	float test = cos(rot.x); 

	if(test > threshold)
	{ 
		rot.z = atan2(mat._12, mat._22);
		rot.y = atan2(mat._31, mat._33); 
	} 
	else 
	{ 
		rot.z = atan2(-mat._21, mat._11); 
		rot.y = 0.0; 
	} 

	return rot;
} 


void OrthonormalizeOrientation(D3DXMATRIX&mat)
{
    DXVector3 x(mat._11,mat._21,mat._31);
	DXVector3 y(mat._12,mat._22,mat._32);
	DXVector3 z;

	D3DXVec3Normalize(x, x);

	D3DXVec3Normalize(z, D3DXVec3Cross(z, x, y));

	D3DXVec3Normalize(y, D3DXVec3Cross(y, z, x));

	mat._11 = x.x; mat._12 = y.x; mat._13 = z.x; mat._14 = 0;
	mat._21 = x.y; mat._22 = y.y; mat._23 = z.y; mat._24 = 0;
	mat._31 = x.z; mat._32 = y.z; mat._33 = z.z; mat._34 = 0;
	mat._41 = 0;   mat._42 = 0;   mat._43 = 0;   mat._44 = 1;
}