/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
* 
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* 
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "dgPhysicsStdafx.h"
#include "dgBody.h"
#include "dgContact.h"
#include "dgCollisionEllipse.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


dgCollisionEllipse::dgCollisionEllipse(dgMemoryAllocator* const allocator, dgUnsigned32 signature, dgFloat32 rx, dgFloat32 ry, dgFloat32 rz, const dgMatrix& offsetMatrix)
	:dgCollisionSphere(allocator, signature, dgFloat32 (1.0f), offsetMatrix), 
	m_scale (rx, ry, rz, dgFloat32 (0.0f)),
	m_invScale (dgFloat32 (1.0f) / rx, dgFloat32 (1.0f) / ry, dgFloat32 (1.0f) / rz, dgFloat32 (0.0f))
{
	m_rtti |= dgCollisionEllipse_RTTI;
	m_collsionId = m_ellipseCollision;
}

dgCollisionEllipse::dgCollisionEllipse(dgWorld* const world, dgDeserialize deserialization, void* const userData)
	:dgCollisionSphere(world, deserialization, userData) 
{
	dgVector size;

	m_rtti |= dgCollisionEllipse_RTTI;
	deserialization (userData, &m_scale, sizeof (dgVector));
	m_invScale.m_x = dgFloat32 (1.0f) / m_scale.m_x; 
	m_invScale.m_y = dgFloat32 (1.0f) / m_scale.m_y; 
	m_invScale.m_z = dgFloat32 (1.0f) / m_scale.m_z; 
	m_invScale.m_w = dgFloat32 (0.0f); 
}

dgCollisionEllipse::~dgCollisionEllipse()
{
}

dgInt32 dgCollisionEllipse::CalculateSignature () const
{
	dgUnsigned32 buffer[2 * sizeof (dgMatrix) / sizeof(dgInt32)];

	memset (buffer, 0, sizeof (buffer));
	buffer[0] = m_ellipseCollision;
	buffer[1] = Quantize (m_scale.m_x);
	buffer[2] = Quantize (m_scale.m_y);
	buffer[3] = Quantize (m_scale.m_z);
	memcpy (&buffer[4], &m_offset, sizeof (dgMatrix));
	return dgInt32 (MakeCRC(buffer, sizeof (buffer)));
}

void dgCollisionEllipse::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__)
{
	_ASSERTE (0);
}


void dgCollisionEllipse::CalcAABB (const dgMatrix &matrix, dgVector &p0, dgVector &p1) const
{
	dgMatrix mat (matrix);
	mat.m_front = mat.m_front.Scale (m_scale.m_x);
	mat.m_up = mat.m_up.Scale (m_scale.m_y);
	mat.m_right = mat.m_right.Scale (m_scale.m_z);
	dgCollisionConvex::CalcAABB (mat, p0, p1);
}

void dgCollisionEllipse::CalcAABBSimd (const dgMatrix &matrix, dgVector& p0, dgVector& p1) const
{
	dgMatrix mat (matrix);
	mat.m_front = mat.m_front.Scale (m_scale.m_x);
	mat.m_up = mat.m_up.Scale (m_scale.m_y);
	mat.m_right = mat.m_right.Scale (m_scale.m_z);
	dgCollisionConvex::CalcAABBSimd (mat, p0, p1);

#if 0
	dgVector xxx0;
	dgVector xxx1;
	CalcAABB (matrix, xxx0, xxx1);
	_ASSERTE (dgAbsf(xxx0.m_x - p0.m_x) < 1.0e-3f);
	_ASSERTE (dgAbsf(xxx0.m_y - p0.m_y) < 1.0e-3f);
	_ASSERTE (dgAbsf(xxx0.m_z - p0.m_z) < 1.0e-3f);
	_ASSERTE (dgAbsf(xxx1.m_x - p1.m_x) < 1.0e-3f);
	_ASSERTE (dgAbsf(xxx1.m_y - p1.m_y) < 1.0e-3f);
	_ASSERTE (dgAbsf(xxx1.m_z - p1.m_z) < 1.0e-3f);
#endif

}

dgVector dgCollisionEllipse::SupportVertex (const dgVector& dir) const
{
	_ASSERTE ((dir % dir) > dgFloat32 (0.999f));
	dgVector dir1 (dir.m_x * m_scale.m_x, dir.m_y * m_scale.m_y, dir.m_z * m_scale.m_z, dgFloat32 (0.0f));

	dir1 = dir1.Scale (dgRsqrt (dir1 % dir1));

	dgVector p (dgCollisionSphere::SupportVertex (dir1));
	return dgVector (p.m_x * m_scale.m_x, p.m_y * m_scale.m_y, p.m_z * m_scale.m_z, dgFloat32 (0.0f)); 
}

dgVector dgCollisionEllipse::SupportVertexSimd (const dgVector& dir) const
{
#ifdef DG_BUILD_SIMD_CODE
	_ASSERTE ((dir % dir) > dgFloat32 (0.999f));
	_ASSERTE ((dgUnsigned64(&dir) & 0x0f) == 0);
	_ASSERTE ((dgUnsigned64(&m_scale) & 0x0f) == 0);

	dgVector dir1;
	simd_type n;
	simd_type tmp;
	simd_type mag2;

//	dir1 = dgVector (dir.m_x * m_scale.m_x, dir.m_y * m_scale.m_y, dir.m_z * m_scale.m_z, dgFloat32 (0.0f));
	n = simd_mul_v (*(simd_type*)&dir, *(simd_type*)&m_scale); 

//	dir1 = dir1.Scale (dgRsqrt (dir1 % dir1));
	mag2 = simd_mul_v (n, n);
	mag2 = simd_add_s (simd_add_v (mag2, simd_move_hl_v (mag2, mag2)), simd_permut_v (mag2, mag2, PURMUT_MASK (3,3,3,1)));
	tmp = simd_rsqrt_s(mag2);
	mag2 = simd_mul_s (simd_mul_s(*(simd_type*)&m_nrh0p5, tmp), simd_mul_sub_s (*(simd_type*)&m_nrh3p0, simd_mul_s (mag2, tmp), tmp));
	(*(simd_type*)&dir1) = simd_mul_v (n, simd_permut_v (mag2, mag2, PURMUT_MASK (3,0,0,0)));

	dgVector p (dgCollisionSphere::SupportVertexSimd (dir1));
	return dgVector (p.m_x * m_scale.m_x, p.m_y * m_scale.m_y, p.m_z * m_scale.m_z, dgFloat32 (0.0f)); 


#else
	return dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
#endif
}


dgInt32 dgCollisionEllipse::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const
{
	_ASSERTE ((normal % normal) > dgFloat32 (0.999f));
//	contactsOut[0] = point;

	dgVector n (normal.m_x * m_scale.m_x, normal.m_y * m_scale.m_y, normal.m_z * m_scale.m_z, dgFloat32 (0.0f));
	n = n.Scale ((normal % point) / (n % n));
	contactsOut[0] = dgVector (n.m_x * m_scale.m_x, n.m_y * m_scale.m_y, n.m_z * m_scale.m_z, dgFloat32 (0.0f)); 
	return 1;
}

dgInt32 dgCollisionEllipse::CalculatePlaneIntersectionSimd (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const
{
#ifdef DG_BUILD_SIMD_CODE	
	_ASSERTE ((normal % normal) > dgFloat32 (0.999f));
	dgVector n (normal.m_x * m_scale.m_x, normal.m_y * m_scale.m_y, normal.m_z * m_scale.m_z, dgFloat32 (0.0f));
	n = n.Scale ((normal % point) / (n % n));
	contactsOut[0] = dgVector (n.m_x * m_scale.m_x, n.m_y * m_scale.m_y, n.m_z * m_scale.m_z, dgFloat32 (0.0f)); 
	return 1;

#else
	return 0;
#endif
}


void dgCollisionEllipse::DebugCollision (const dgMatrix& matrixPtr, OnDebugCollisionMeshCallback callback, void* const userData) const
{
	dgMatrix mat (GetOffsetMatrix() * matrixPtr);  
	mat.m_front = mat.m_front.Scale (m_scale.m_x);
	mat.m_up = mat.m_up.Scale (m_scale.m_y);
	mat.m_right = mat.m_right.Scale (m_scale.m_z);
	mat = GetOffsetMatrix().Inverse() * mat;
	dgCollisionSphere::DebugCollision (mat, callback, userData);
}


dgFloat32 dgCollisionEllipse::RayCast (const dgVector& p0, const dgVector& p1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	dgFloat32 t;

	if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
		return dgFloat32 (1.2f);
	}

	dgVector q0 (p0.m_x * m_invScale.m_x, p0.m_y * m_invScale.m_y, p0.m_z * m_invScale.m_z, dgFloat32 (0.0f)); 
	dgVector q1 (p1.m_x * m_invScale.m_x, p1.m_y * m_invScale.m_y, p1.m_z * m_invScale.m_z, dgFloat32 (0.0f));
	t = dgCollisionSphere::RayCast (q0, q1, contactOut, NULL, NULL, NULL);
	return t;
}

dgFloat32 dgCollisionEllipse::RayCastSimd (const dgVector& p0, const dgVector& p1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	dgFloat32 t;

	if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
		return dgFloat32 (1.2f);
	}

	dgVector q0 (p0.m_x * m_invScale.m_x, p0.m_y * m_invScale.m_y, p0.m_z * m_invScale.m_z, dgFloat32 (0.0f)); 
	dgVector q1 (p1.m_x * m_invScale.m_x, p1.m_y * m_invScale.m_y, p1.m_z * m_invScale.m_z, dgFloat32 (0.0f));
	t = dgCollisionSphere::RayCastSimd (q0, q1, contactOut, NULL, NULL, NULL);
	return t;
}



dgFloat32 dgCollisionEllipse::CalculateMassProperties (dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const
{
	return dgCollisionConvex::CalculateMassProperties (inertia, crossInertia, centerOfMass);
}

void dgCollisionEllipse::GetCollisionInfo(dgCollisionInfo* info) const
{
	dgCollisionConvex::GetCollisionInfo(info);

	info->m_sphere.m_r0 = m_scale.m_x;
	info->m_sphere.m_r1 = m_scale.m_y;
	info->m_sphere.m_r2 = m_scale.m_z;
	info->m_offsetMatrix = GetOffsetMatrix();
//	strcpy (info->m_collisionType, "sphere");
	info->m_collisionType = m_sphereCollision;

}

void dgCollisionEllipse::Serialize(dgSerialize callback, void* const userData) const
{
	dgCollisionSphere::Serialize(callback, userData);
	callback (userData, &m_scale, sizeof (dgVector));
}