#include "SM_MathPCH.h"



Matrix3X3* ToTransform(Matrix3X3* pDest, const Quaternion* q)
{
  float R[3][3];

  q->ToRotationMatrix (R);
  
  pDest->m_11=R[0][0]; pDest->m_12=R[0][1]; pDest->m_13=R[0][2]; 
  pDest->m_21=R[1][0]; pDest->m_22=R[1][1]; pDest->m_23=R[1][2]; 
  pDest->m_31=R[2][0]; pDest->m_32=R[2][1]; pDest->m_33=R[2][2]; 
  return (pDest);

}



Matrix4X4* ToTransform(Matrix4X4* pDest, const Vector3D* v,const Quaternion* q)
{
  float R[3][3];

  q->ToRotationMatrix (R);
  
  pDest->m_11=R[0][0]; pDest->m_12=R[0][1]; pDest->m_13=R[0][2]; pDest->m_14=0.0f;
  pDest->m_21=R[1][0]; pDest->m_22=R[1][1]; pDest->m_23=R[1][2]; pDest->m_24=0.0f;
  pDest->m_31=R[2][0]; pDest->m_32=R[2][1]; pDest->m_33=R[2][2]; pDest->m_34=0.0f;
  pDest->m_41=v->x   ; pDest->m_42=v->y   ; pDest->m_43=v->z   ; pDest->m_44=1.0f;
  return (pDest);
}

Matrix4X4* ToTransform(Matrix4X4* pDest, const Vector3D* v, const Quaternion* q, float fScale)
{
  float R[3][3];

  q->ToRotationMatrix (R);
  
  pDest->m_11=R[0][0]*fScale; pDest->m_12=R[0][1]*fScale; pDest->m_13=R[0][2]*fScale; pDest->m_14=0.0f;
  pDest->m_21=R[1][0]*fScale; pDest->m_22=R[1][1]*fScale; pDest->m_23=R[1][2]*fScale; pDest->m_24=0.0f;
  pDest->m_31=R[2][0]*fScale; pDest->m_32=R[2][1]*fScale; pDest->m_33=R[2][2]*fScale; pDest->m_34=0.0f;
  pDest->m_41=v->x          ; pDest->m_42=v->y          ; pDest->m_43=v->z          ; pDest->m_44=1.0f;
  return (pDest);
}

Matrix4X4* ToInverseTransform(Matrix4X4* pDest, const Vector3D* v, const Quaternion* q)
{
  float R[3][3];

  q->ToRotationMatrix (R);

	pDest->m_11=R[0][0]; pDest->m_12=R[1][0]; pDest->m_13=R[2][0]; pDest->m_14=0.0f; 
	pDest->m_21=R[0][1]; pDest->m_22=R[1][1]; pDest->m_23=R[2][1]; pDest->m_24=0.0f; 
	pDest->m_31=R[0][2]; pDest->m_32=R[1][2]; pDest->m_33=R[2][2]; pDest->m_34=0.0f;

	pDest->m_41=-(v->x*pDest->m_11+v->y*pDest->m_21+v->z*pDest->m_31);
	pDest->m_42=-(v->x*pDest->m_12+v->y*pDest->m_22+v->z*pDest->m_32);
	pDest->m_43=-(v->x*pDest->m_13+v->y*pDest->m_23+v->z*pDest->m_33);  
  pDest->m_44=1.0f;

  return (pDest);
}

void  GenerateFrustumPlanes(Matrix4X4* pView, const Matrix4X4* pProjection, Plane* pPlanes)
{
  Matrix4X4 WorldToCamera;

  Matrix4X4::Mult(&WorldToCamera, pView, pProjection);

  // Left clipping plane 
  pPlanes[0].a = WorldToCamera.m_14 + WorldToCamera.m_11; 
  pPlanes[0].b = WorldToCamera.m_24 + WorldToCamera.m_21; 
  pPlanes[0].c = WorldToCamera.m_34 + WorldToCamera.m_31; 
  pPlanes[0].d = WorldToCamera.m_44 + WorldToCamera.m_41;

  // Right clipping plane 
  pPlanes[1].a = WorldToCamera.m_14 - WorldToCamera.m_11; 
  pPlanes[1].b = WorldToCamera.m_24 - WorldToCamera.m_21; 
  pPlanes[1].c = WorldToCamera.m_34 - WorldToCamera.m_31; 
  pPlanes[1].d = WorldToCamera.m_44 - WorldToCamera.m_41;

  // Top clipping plane 
  pPlanes[2].a = WorldToCamera.m_14 - WorldToCamera.m_12; 
  pPlanes[2].b = WorldToCamera.m_24 - WorldToCamera.m_22; 
  pPlanes[2].c = WorldToCamera.m_34 - WorldToCamera.m_32; 
  pPlanes[2].d = WorldToCamera.m_44 - WorldToCamera.m_42;

  // Bottom clipping plane 
  pPlanes[3].a = WorldToCamera.m_14 + WorldToCamera.m_12; 
  pPlanes[3].b = WorldToCamera.m_24 + WorldToCamera.m_22; 
  pPlanes[3].c = WorldToCamera.m_34 + WorldToCamera.m_32; 
  pPlanes[3].d = WorldToCamera.m_44 + WorldToCamera.m_42;

  // Near clipping plane 
  pPlanes[4].a = WorldToCamera.m_13; 
  pPlanes[4].b = WorldToCamera.m_23; 
  pPlanes[4].c = WorldToCamera.m_33; 
  pPlanes[4].d = WorldToCamera.m_43;

  // Far clipping plane 
  pPlanes[5].a = WorldToCamera.m_14 - WorldToCamera.m_13; 
  pPlanes[5].b = WorldToCamera.m_24 - WorldToCamera.m_23; 
  pPlanes[5].c = WorldToCamera.m_34 - WorldToCamera.m_33; 
  pPlanes[5].d = WorldToCamera.m_44 - WorldToCamera.m_43;

  int i;

  for (i=0 ; i<6 ; i++)
  {
    pPlanes[i].Normalize();
  }  
}

void GetFrame(Vector3D& v3dVPN, Vector3D& v3dUp, Vector3D& v3dRight)
{
  v3dVPN.Normalize();
  v3dRight=Vector3D::Cross(v3dVPN, Vector3D(0.0f, 1.0f, 0.0f));
  v3dRight.Normalize();
  v3dUp=Vector3D::Cross(v3dVPN, v3dRight);
}
