/* 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 "dgCollisionEllipse.h"
#include "dgBody.h"
#include "dgContact.h"
#include "hpl1/engine/libraries/newton/core/dg.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) {

	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__) {
	NEWTON_ASSERT(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);
	NEWTON_ASSERT(dgAbsf(xxx0.m_x - p0.m_x) < 1.0e-3f);
	NEWTON_ASSERT(dgAbsf(xxx0.m_y - p0.m_y) < 1.0e-3f);
	NEWTON_ASSERT(dgAbsf(xxx0.m_z - p0.m_z) < 1.0e-3f);
	NEWTON_ASSERT(dgAbsf(xxx1.m_x - p1.m_x) < 1.0e-3f);
	NEWTON_ASSERT(dgAbsf(xxx1.m_y - p1.m_y) < 1.0e-3f);
	NEWTON_ASSERT(dgAbsf(xxx1.m_z - p1.m_z) < 1.0e-3f);
#endif
}

dgVector dgCollisionEllipse::SupportVertex(const dgVector &dir) const {
	NEWTON_ASSERT((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
	NEWTON_ASSERT((dir % dir) > dgFloat32(0.999f));
	NEWTON_ASSERT((dgUnsigned64(&dir) & 0x0f) == 0);
	NEWTON_ASSERT((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 {
	NEWTON_ASSERT((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
	NEWTON_ASSERT((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, reinterpret_cast<const NewtonBody *>(body), reinterpret_cast<const NewtonCollision *>(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 {
	return RayCast(p0, p1, contactOut, preFilter, body, userData);
}

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));
}