#include "physics.h"
#include "math3d.h"

PxCastResult_t IPxWorld::BoxCast( vec3 size, vec3 origin, vec3 destination, vec3 rotation )
{
	vec3 velocity;
	PxCastResult_t result = {};
	px_cast_result r = {};

	for ( int i = 0; i < 3; i++ )
		velocity[i] = destination[i] - origin[i];

	r = px_box_cast(px, size[0], size[1], size[2], PX_GLM_VEC3(origin), PX_GLM_VEC3(rotation), PX_GLM_VEC3(velocity), 1);


	result.bHit = r.hit;
	result.fTime = r.time;
	for ( int i = 0; i < 3; i++ )
	{
		result.position[i] = origin[i] + velocity[i] * result.fTime;
		result.normal[i] = r.normal1.m[i];
		result.normal2[i] = r.normal2.m[i];
	}


	return result;
}

void CPxCollider::Spawn( float fFriction )
{

};
void CPxCollider::Destroy()
{

};

void CPxBallMesh::Spawn( float fFriction )
{
	m_pCollider = px_ball(m_fRadius, {.friction = fFriction});
};

void CPxBallMesh::Destroy()
{

};

void CPxBoxMesh::Spawn( float fFriction )
{
	m_pCollider = px_box(m_fRadius[0], m_fRadius[1], m_fRadius[2], {.friction = fFriction});
};

void CPxBoxMesh::Destroy()
{

};

void CPxTriangleMesh::Spawn( float fFriction )
{

};

void CPxTriangleMesh::Destroy()
{

};

void CPxRigidKinematicPosition::Spawn( CPxCollider *pCollider, px_matrix matrix, px_rigidbody_params params )
{
	m_pRigidBody = px_kinematic_position_body(px, pCollider->m_pCollider, matrix, params);
};

void CPxRigidKinematicPosition::SetPosition( px_vec3 position )
{
	px_setposition(px, m_pRigidBody, position);
};
px_vec3 CPxRigidKinematicPosition::GetPosition( void )
{
	return px_getposition(px, m_pRigidBody);
};
px_matrix CPxRigidKinematicPosition::GetMatrix ( void )
{
	return px_getmatrix(px, m_pRigidBody);
};

void CPxRigidKinematicPosition::Destroy()
{

};

void CPxRigidBody::Spawn( CPxCollider *pCollider, px_matrix matrix, px_rigidbody_params params )
{
	m_pRigidBody = px_rigidbody(px, pCollider->m_pCollider, matrix, params);
};
void CPxRigidBody::SetPosition( px_vec3 position )
{
	px_setposition(px, m_pRigidBody, position);
}

void CPxRigidBody::SetVelocity( px_vec3 velocity )
{
	px_setvelocity(px, m_pRigidBody, velocity);
}

px_vec3 CPxRigidBody::GetPosition( void )
{
	return px_getposition(px, m_pRigidBody);
};

px_vec3 CPxRigidBody::GetVelocity( void )
{
	return px_getvelocity(px, m_pRigidBody);
};

px_matrix CPxRigidBody::GetMatrix ( void )
{
	return px_getmatrix(px, m_pRigidBody);
};

void CPxRigidBody::Destroy()
{

};

void CPxStaticBody::Spawn( CPxCollider *pCollider, px_matrix matrix, px_rigidbody_params params )
{
	px_staticbody(px, pCollider->m_pCollider, matrix);
};
px_vec3 CPxStaticBody::GetPosition( void )
{
	return px_getposition(px, m_pCollider);
};
px_matrix CPxStaticBody::GetMatrix ( void )
{
	return px_getmatrix(px, m_pCollider);
};

void CPxStaticBody::Destroy()
{

}

;
void CPxFixedBody::Spawn( CPxCollider *pCollider, px_matrix matrix, px_rigidbody_params params )
{
	px_fixedbody(px, pCollider->m_pCollider);
};
px_vec3 CPxFixedBody::GetPosition( void )
{
	return px_getposition(px, m_pCollider);
};
px_matrix CPxFixedBody::GetMatrix ( void )
{
	return px_getmatrix(px, m_pCollider);
};

void CPxFixedBody::Destroy()
{

};