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


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

dgConvexSimplexEdge dgCollisionBox::m_edgeArray[24] = {
	{1, &m_edgeArray[3], &m_edgeArray[12], &m_edgeArray[5]},
	{2, &m_edgeArray[6], &m_edgeArray[3], &m_edgeArray[7]},
	{7, &m_edgeArray[12], &m_edgeArray[6], &m_edgeArray[14]},
	{0, &m_edgeArray[0], &m_edgeArray[9], &m_edgeArray[1]},
	{3, &m_edgeArray[9], &m_edgeArray[15], &m_edgeArray[11]},
	{6, &m_edgeArray[15], &m_edgeArray[0], &m_edgeArray[16]},
	{0, &m_edgeArray[1], &m_edgeArray[18], &m_edgeArray[2]},
	{3, &m_edgeArray[10], &m_edgeArray[1], &m_edgeArray[9]},
	{5, &m_edgeArray[18], &m_edgeArray[10], &m_edgeArray[20]},
	{1, &m_edgeArray[4], &m_edgeArray[7], &m_edgeArray[3]},
	{2, &m_edgeArray[7], &m_edgeArray[21], &m_edgeArray[8]},
	{4, &m_edgeArray[21], &m_edgeArray[4], &m_edgeArray[22]},
	{0, &m_edgeArray[2], &m_edgeArray[16], &m_edgeArray[0]},
	{6, &m_edgeArray[16], &m_edgeArray[19], &m_edgeArray[17]},
	{5, &m_edgeArray[19], &m_edgeArray[2], &m_edgeArray[18]},
	{1, &m_edgeArray[5], &m_edgeArray[22], &m_edgeArray[4]},
	{7, &m_edgeArray[13], &m_edgeArray[5], &m_edgeArray[12]},
	{4, &m_edgeArray[22], &m_edgeArray[13], &m_edgeArray[23]},
	{2, &m_edgeArray[8], &m_edgeArray[14], &m_edgeArray[6]},
	{7, &m_edgeArray[14], &m_edgeArray[23], &m_edgeArray[13]},
	{4, &m_edgeArray[23], &m_edgeArray[8], &m_edgeArray[21]},
	{3, &m_edgeArray[11], &m_edgeArray[20], &m_edgeArray[10]},
	{6, &m_edgeArray[17], &m_edgeArray[11], &m_edgeArray[15]},
	{5, &m_edgeArray[20], &m_edgeArray[17], &m_edgeArray[19]},
};

dgCollisionBox::dgCollisionBox(dgMemoryAllocator *allocator,
                               dgUnsigned32 signature, dgFloat32 size_x, dgFloat32 size_y,
                               dgFloat32 size_z, const dgMatrix &matrix) : dgCollisionConvex(allocator, signature, matrix, m_boxCollision) {
	m_destructionImpulse = dgFloat32(1.0e20f);
	Init(size_x, size_y, size_z);
}

dgCollisionBox::dgCollisionBox(dgWorld *const world,
                               dgDeserialize deserialization, void *const userData) : dgCollisionConvex(world, deserialization, userData) {
	dgVector size;
	deserialization(userData, &size, sizeof(dgVector));
	m_destructionImpulse = size.m_w;
	Init(size.m_x, size.m_y, size.m_z);
}

void dgCollisionBox::Init(dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z) {
	m_rtti |= dgCollisionBox_RTTI;
	m_size[0].m_x = dgAbsf(size_x) * dgFloat32(0.5f);
	m_size[0].m_y = dgAbsf(size_y) * dgFloat32(0.5f);
	m_size[0].m_z = dgAbsf(size_z) * dgFloat32(0.5f);
	m_size[0].m_w = dgFloat32(0.0f);

	m_size[1].m_x = -m_size[0].m_x;
	m_size[1].m_y = -m_size[0].m_y;
	m_size[1].m_z = -m_size[0].m_z;
	m_size[1].m_w = dgFloat32(0.0f);

	m_edgeCount = 24;
	m_vertexCount = 8;

	m_vertex[0] = dgVector(m_size[0].m_x, m_size[0].m_y, m_size[0].m_z,
	                       dgFloat32(1.0f));
	m_vertex[1] = dgVector(-m_size[0].m_x, m_size[0].m_y, m_size[0].m_z,
	                       dgFloat32(1.0f));
	m_vertex[2] = dgVector(m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z,
	                       dgFloat32(1.0f));
	m_vertex[3] = dgVector(-m_size[0].m_x, -m_size[0].m_y, m_size[0].m_z,
	                       dgFloat32(1.0f));
	m_vertex[4] = dgVector(-m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z,
	                       dgFloat32(1.0f));
	m_vertex[5] = dgVector(m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z,
	                       dgFloat32(1.0f));
	m_vertex[6] = dgVector(-m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z,
	                       dgFloat32(1.0f));
	m_vertex[7] = dgVector(m_size[0].m_x, m_size[0].m_y, -m_size[0].m_z,
	                       dgFloat32(1.0f));

	m_vertex_sse[0] = dgVector(m_vertex[0].m_x, m_vertex[1].m_x, m_vertex[2].m_x,
	                           m_vertex[3].m_x);
	m_vertex_sse[1] = dgVector(m_vertex[0].m_y, m_vertex[1].m_y, m_vertex[2].m_y,
	                           m_vertex[3].m_y);
	m_vertex_sse[2] = dgVector(m_vertex[0].m_z, m_vertex[1].m_z, m_vertex[2].m_z,
	                           m_vertex[3].m_z);
	m_vertex_sse[3] = dgVector(m_vertex[4].m_x, m_vertex[5].m_x, m_vertex[6].m_x,
	                           m_vertex[7].m_x);
	m_vertex_sse[4] = dgVector(m_vertex[4].m_y, m_vertex[5].m_y, m_vertex[6].m_y,
	                           m_vertex[7].m_y);
	m_vertex_sse[5] = dgVector(m_vertex[4].m_z, m_vertex[5].m_z, m_vertex[6].m_z,
	                           m_vertex[7].m_z);

	dgCollisionConvex::m_vertex = m_vertex;
	dgCollisionConvex::m_simplex = m_edgeArray;
	SetVolumeAndCG();
}

dgCollisionBox::~dgCollisionBox() {
	//  m_shapeRefCount --;
	//  NEWTON_ASSERT (m_shapeRefCount >= 0);

	dgCollisionConvex::m_simplex = NULL;
	dgCollisionConvex::m_vertex = NULL;
}

void dgCollisionBox::SetCollisionBBox(const dgVector &p0__,
                                      const dgVector &p1__) {
	NEWTON_ASSERT(0);
}

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

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

dgVector dgCollisionBox::SupportVertexSimd(const dgVector &dir) const {
#ifdef DG_BUILD_SIMD_CODE
#if 0
	dgFloatSign *const ptr = (dgFloatSign *) &dir;

	dgInt32 x = -(ptr[0].m_integer.m_iVal >> 31);
	dgInt32 y = -(ptr[1].m_integer.m_iVal >> 31);
	dgInt32 z = -(ptr[2].m_integer.m_iVal >> 31);
	return dgVector(m_size[x].m_x, m_size[y].m_y, m_size[z].m_z, dgFloat32(0.0f));

#else
	// according to Intel in latest possessors this is better, because read after write are very, very expensive
	return dgVector(dir.m_x < dgFloat32(0.0f) ? m_size[1].m_x : m_size[0].m_x,
	                dir.m_y < dgFloat32(0.0f) ? m_size[1].m_y : m_size[0].m_y,
	                dir.m_z < dgFloat32(0.0f) ? m_size[1].m_z : m_size[0].m_z,
	                dgFloat32(0.0f));
#endif

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

dgVector dgCollisionBox::SupportVertex(const dgVector &dir) const {
	NEWTON_ASSERT(dgAbsf(dir % dir - dgFloat32(1.0f)) < dgFloat32(1.0e-3f));

#if 0
	dgFloatSign *const ptr = (dgFloatSign *) &dir;

	dgInt32 x = -(ptr[0].m_integer.m_iVal >> 31);
	dgInt32 y = -(ptr[1].m_integer.m_iVal >> 31);
	dgInt32 z = -(ptr[2].m_integer.m_iVal >> 31);
	return dgVector(m_size[x].m_x, m_size[y].m_y, m_size[z].m_z, dgFloat32(0.0f));

#else
	// according to Intel in latest possessors this is better, because read after write are very, very expensive
	return dgVector(dir.m_x < dgFloat32(0.0f) ? m_size[1].m_x : m_size[0].m_x,
	                dir.m_y < dgFloat32(0.0f) ? m_size[1].m_y : m_size[0].m_y,
	                dir.m_z < dgFloat32(0.0f) ? m_size[1].m_z : m_size[0].m_z,
	                dgFloat32(0.0f));
#endif
}

void dgCollisionBox::CalcAABB(const dgMatrix &matrix, dgVector &p0,
                              dgVector &p1) const {
	dgFloat32 x = m_size[0].m_x * dgAbsf(matrix[0][0]) + m_size[0].m_y * dgAbsf(matrix[1][0]) + m_size[0].m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING;
	dgFloat32 y = m_size[0].m_x * dgAbsf(matrix[0][1]) + m_size[0].m_y * dgAbsf(matrix[1][1]) + m_size[0].m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING;
	dgFloat32 z = m_size[0].m_x * dgAbsf(matrix[0][2]) + m_size[0].m_y * dgAbsf(matrix[1][2]) + m_size[0].m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING;

	p0.m_x = matrix[3][0] - x;
	p1.m_x = matrix[3][0] + x;

	p0.m_y = matrix[3][1] - y;
	p1.m_y = matrix[3][1] + y;

	p0.m_z = matrix[3][2] - z;
	p1.m_z = matrix[3][2] + z;

	p0.m_w = dgFloat32(1.0f);
	p1.m_w = dgFloat32(1.0f);
}

void dgCollisionBox::CalcAABBSimd(const dgMatrix &matrix, dgVector &p0,
                                  dgVector &p1) const {
#ifdef DG_BUILD_SIMD_CODE
	//  dgFloat32 x;
	//  dgFloat32 y;
	//  dgFloat32 z;
	simd_type tmp;

	//  x = m_size[0].m_x * dgAbsf(matrix[0][0]) + m_size[0].m_y * dgAbsf(matrix[1][0]) + m_size[0].m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING;
	//  y = m_size[0].m_x * dgAbsf(matrix[0][1]) + m_size[0].m_y * dgAbsf(matrix[1][1]) + m_size[0].m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING;
	//  z = m_size[0].m_x * dgAbsf(matrix[0][2]) + m_size[0].m_y * dgAbsf(matrix[1][2]) + m_size[0].m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING;
	tmp =
	    simd_mul_add_v(
	        simd_mul_add_v(
	            simd_mul_add_v((simd_type &)m_aabb_padd, (simd_type &)m_size_x, simd_and_v((simd_type &)m_signMask, (simd_type &)matrix[0])),
	            (simd_type &)m_size_y, simd_and_v((simd_type &)m_signMask, (simd_type &)matrix[1])),
	        (simd_type &)m_size_z, simd_and_v((simd_type &)m_signMask, (simd_type &)matrix[2]));

	//  p0.m_x = matrix[3][0] - x;
	//  p1.m_x = matrix[3][0] + x;
	//  p0.m_y = matrix[3][1] - y;
	//  p1.m_y = matrix[3][1] + y;
	//  p0.m_z = matrix[3][2] - z;
	//  p1.m_z = matrix[3][2] + z;
	//  p0.m_w = dgFloat32 (1.0f);
	//  p1.m_w = dgFloat32 (1.0f);
	(simd_type &)p0 = simd_sub_v((simd_type &)matrix[3], tmp);
	(simd_type &)p1 = simd_add_v((simd_type &)matrix[3], tmp);

#else

#endif
}

dgFloat32 dgCollisionBox::RayCast(const dgVector &localP0,
                                  const dgVector &localP1, dgContactPoint &contactOut,
                                  OnRayPrecastAction preFilter, const dgBody *const body,
                                  void *const userData) const {
	if (PREFILTER_RAYCAST(preFilter, reinterpret_cast<const NewtonBody *>(body), reinterpret_cast<const NewtonCollision *>(this), userData)) {
		return dgFloat32(1.2f);
	}

#if 0
	bool cut;
	dgFloat32 t;
	dgFloat32 s;
	dgFloat32 s1;

	cut = false;
	s = dgFloat32(0.0f);

	dgVector p0(localP0);
	dgVector p1(localP1);
	for (int i = 0; i < 3; i ++) {
		dgFloatSign tmp0;
		dgFloatSign tmp1;

		tmp0.m_fVal = m_size[0][i] - p0[i];
		if (tmp0.m_integer.m_iVal > 0) {
			tmp1.m_fVal = m_size[0][i] - p1[i];
			if (tmp1.m_integer.m_iVal < 0) {
				t = tmp0.m_fVal / (p1[i] - p0[i]);
				p1 = p0 + (p1 - p0).Scale(t);

			}
		} else {
			tmp1.m_fVal = m_size[0][i] - p1[i];
			if (tmp1.m_integer.m_iVal > 0) {
				s1 = (m_size[0][i] - localP0[i]) / (localP1[i] - localP0[i]);
				if (s1 > s) {
					s = s1;
					cut = true;
					contactOut.m_normal = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
					contactOut.m_normal[i] = dgFloat32(1.0f);
					contactOut.m_userId = SetUserDataID();
				}
				t = tmp0.m_fVal / (p1[i] - p0[i]);
				p0 = p0 + (p1 - p0).Scale(t);
			} else {
				return dgFloat32(1.2f);
			}
		}

		tmp0.m_fVal = -m_size[0][i] - p0[i];
		if (tmp0.m_integer.m_iVal < 0) {
			tmp1.m_fVal = -m_size[0][i] - p1[i];
			if (tmp1.m_integer.m_iVal > 0) {
				t = tmp0.m_fVal / (p1[i] - p0[i]);
				p1 = p0 + (p1 - p0).Scale(t);
			}
		} else {
			tmp1.m_fVal = -m_size[0][i] - p1[i];
			if (tmp1.m_integer.m_iVal < 0) {
				s1 = (-m_size[0][i] - localP0[i]) / (localP1[i] - localP0[i]);
				if (s1 > s) {
					s = s1;
					cut = true;
					contactOut.m_normal = dgVector(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
					contactOut.m_normal[i] = -dgFloat32(1.0f);
					contactOut.m_userId = SetUserDataID();
				}

				t = tmp0.m_fVal / (p1[i] - p0[i]);
				p0 = p0 + (p1 - p0).Scale(t);
			} else {
				return dgFloat32(1.2f);
			}
		}
	}
	return cut ? s : 1.2f;
#else

	dgInt32 index = 0;
	dgFloat32 signDir = dgFloat32(0.0f);
	dgFloat32 tmin = dgFloat32(0.0f);
	dgFloat32 tmax = dgFloat32(1.0f);
	for (dgInt32 i = 0; i < 3; i++) {
		dgFloat32 dp = localP1[i] - localP0[i];
		if (dgAbsf(dp) < dgFloat32(1.0e-8f)) {
			if (localP0[i] <= m_size[1][i] || localP0[i] >= m_size[0][i]) {
				return dgFloat32(1.2f);
			}
		} else {
			dp = dgFloat32(1.0f) / dp;
			dgFloat32 t1 = (m_size[1][i] - localP0[i]) * dp;
			dgFloat32 t2 = (m_size[0][i] - localP0[i]) * dp;

			dgFloat32 sign = dgFloat32(-1.0f);
			if (t1 > t2) {
				sign = 1;
				Swap(t1, t2);
			}
			if (t1 > tmin) {
				signDir = sign;
				index = i;
				tmin = t1;
			}
			if (t2 < tmax) {
				tmax = t2;
			}
			if (tmin > tmax) {
				return dgFloat32(1.2f);
			}
		}
	}

	if (tmin >= dgFloat32(0.0f)) {
		NEWTON_ASSERT(tmin < 1.0f);
		contactOut.m_normal = dgVector(dgFloat32(0.0f), dgFloat32(0.0f),
		                               dgFloat32(0.0f), dgFloat32(0.0f));
		contactOut.m_normal[index] = signDir;
		// contactOut.m_userId = SetUserData();
		contactOut.m_userId = SetUserDataID();
	} else {
		tmin = dgFloat32(1.2f);
	}
	return tmin;
#endif
}

dgFloat32 dgCollisionBox::RayCastSimd(const dgVector &localP0,
                                      const dgVector &localP1, dgContactPoint &contactOut,
                                      OnRayPrecastAction preFilter, const dgBody *const body,
                                      void *const userData) const {
	return RayCast(localP0, localP1, contactOut, preFilter, body, userData);
}

dgFloat32 dgCollisionBox::CalculateMassProperties(dgVector &inertia,
        dgVector &crossInertia, dgVector &centerOfMass) const {
	//  volume = dgCollisionConvex::CalculateMassProperties (inertia, crossInertia, centerOfMass);

	centerOfMass = GetOffsetMatrix().m_posit;
	dgFloat32 volume = dgFloat32(8.0f) * m_size[0].m_x * m_size[0].m_y * m_size[0].m_z;

	dgVector II(
	    dgFloat32(1.0f / 3.0f) * volume * (m_size[0].m_y * m_size[0].m_y + m_size[0].m_z * m_size[0].m_z),
	    dgFloat32(1.0f / 3.0f) * volume * (m_size[0].m_x * m_size[0].m_x + m_size[0].m_z * m_size[0].m_z),
	    dgFloat32(1.0f / 3.0f) * volume * (m_size[0].m_x * m_size[0].m_x + m_size[0].m_y * m_size[0].m_y),
	    dgFloat32(0.0f));

	dgMatrix inertiaTensor(dgGetIdentityMatrix());

	inertiaTensor[0][0] = II.m_x;
	inertiaTensor[1][1] = II.m_y;
	inertiaTensor[2][2] = II.m_z;

	inertiaTensor = GetOffsetMatrix().Inverse() * inertiaTensor * GetOffsetMatrix();

	crossInertia.m_x = inertiaTensor[1][2] - volume * centerOfMass.m_y * centerOfMass.m_z;
	crossInertia.m_y = inertiaTensor[0][2] - volume * centerOfMass.m_z * centerOfMass.m_x;
	crossInertia.m_z = inertiaTensor[0][1] - volume * centerOfMass.m_x * centerOfMass.m_y;
	crossInertia.m_w = dgFloat32(0.0f);

	dgVector central(centerOfMass.CompProduct(centerOfMass));
	inertia.m_x = inertiaTensor[0][0] + volume * (central.m_y + central.m_z);
	inertia.m_y = inertiaTensor[1][1] + volume * (central.m_z + central.m_x);
	inertia.m_z = inertiaTensor[2][2] + volume * (central.m_x + central.m_y);
	inertia.m_w = dgFloat32(0.0f);

	centerOfMass = centerOfMass.Scale(volume);
	return volume;
}

void dgCollisionBox::SetBreakImpulse(dgFloat32 force) {
	m_destructionImpulse = force;
}

dgFloat32 dgCollisionBox::GetBreakImpulse() const {
	return m_destructionImpulse;
}

dgInt32 dgCollisionBox::CalculatePlaneIntersection(const dgVector &normal,
        const dgVector &point, dgVector *const contactsOut) const {
	dgFloat32 test[8];
	dgPlane plane(normal, -(normal % point));

	dgConvexSimplexEdge *edge = NULL;
	{
		dgFloat32 side1 = dgFloat32(1.0e20f);
		for (dgInt32 i = 0; i < 8; i++) {
			dgFloat32 side0 = plane.Evalue(m_vertex[i]);
			test[i] = side0;
			if (side0 > dgFloat32(0.0f)) {
				if (side0 < side1) {
					side1 = side0;
					edge = m_supportVertexStarCuadrant[i];
				}
			}
		}
	}

	dgInt32 count = 0;
	if (edge) {
		NEWTON_ASSERT(test[edge->m_vertex] > dgFloat32(0.0f));

		dgConvexSimplexEdge *ptr = edge;
		dgConvexSimplexEdge *firstEdge = NULL;
		dgFloat32 side0 = test[edge->m_vertex];
		do {
			NEWTON_ASSERT(m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32(1.0f));
			dgFloat32 side1 = test[ptr->m_twin->m_vertex];
			if (side1 < side0) {
				if (side1 < dgFloat32(0.0f)) {
					firstEdge = ptr;
					break;
				}

				side0 = side1;
				edge = ptr->m_twin;
				ptr = edge;
			}
			ptr = ptr->m_twin->m_next;
		} while (ptr != edge);

		if (firstEdge) {
			edge = firstEdge;
			ptr = edge;
			do {
				dgVector dp(m_vertex[ptr->m_twin->m_vertex] - m_vertex[ptr->m_vertex]);

				dgFloat32 t = plane % dp;
				if (t >= dgFloat32(-1.e-24f)) {
					t = dgFloat32(0.0f);
				} else {
					t = test[ptr->m_vertex] / t;
					if (t > dgFloat32(0.0f)) {
						t = dgFloat32(0.0f);
					}
					if (t < dgFloat32(-1.0f)) {
						t = dgFloat32(-1.0f);
					}
				}

				NEWTON_ASSERT(t <= dgFloat32(0.01f));
				NEWTON_ASSERT(t >= dgFloat32(-1.05f));
				contactsOut[count] = m_vertex[ptr->m_vertex] - dp.Scale(t);
				count++;

				dgConvexSimplexEdge *ptr1 = ptr->m_next;
				for (; ptr1 != ptr; ptr1 = ptr1->m_next) {
					dgInt32 index0 = ptr1->m_twin->m_vertex;
					if (test[index0] >= dgFloat32(0.0f)) {
						NEWTON_ASSERT(test[ptr1->m_vertex] <= dgFloat32(0.0f));
						break;
					}
				}
				NEWTON_ASSERT(ptr != ptr1);
				ptr = ptr1->m_twin;

			} while ((ptr != edge) && (count < 8));
		}
	}

	if (count > 1) {
		count = RectifyConvexSlice(count, normal, contactsOut);
	}
	return count;
}

dgInt32 dgCollisionBox::CalculatePlaneIntersectionSimd(const dgVector &normal,
        const dgVector &point, dgVector *const contactsOut) const {
#ifdef DG_BUILD_SIMD_CODE
	dgInt32 i;
	dgInt32 count;
	dgInt32 index0;
	dgInt32 index1;
	dgFloat32 side0;
	dgFloat32 side1;
	dgFloat32 *test;
	dgConvexSimplexEdge *ptr;
	dgConvexSimplexEdge *ptr1;
	dgConvexSimplexEdge *edge;
	dgConvexSimplexEdge *firstEdge;
	simd_type dot;
	simd_type den;
	simd_type p1p0;
	//  simd_type minVal;
	//  simd_type edgePtr;
	simd_type plane_a;
	simd_type plane_b;
	simd_type plane_c;
	simd_type plane_d;
	simd_type side[2];

	test = (dgFloat32 *)&side[0];
	dgPlane plane(normal, -(normal % point));

	plane_a = simd_set1(plane.m_x);
	plane_b = simd_set1(plane.m_y);
	plane_c = simd_set1(plane.m_z);
	plane_d = simd_set1(plane.m_w);

	side[0] =
	    simd_mul_add_v(simd_mul_add_v(simd_mul_add_v(plane_d, *((simd_type *)&m_vertex_sse[0]), plane_a),
	                                  *((simd_type *)&m_vertex_sse[1]), plane_b),
	                   *((simd_type *)&m_vertex_sse[2]), plane_c);
	side[1] =
	    simd_mul_add_v(simd_mul_add_v(simd_mul_add_v(plane_d, *((simd_type *)&m_vertex_sse[3]), plane_a),
	                                  *((simd_type *)&m_vertex_sse[4]), plane_b),
	                   *((simd_type *)&m_vertex_sse[5]), plane_c);

	//  edgePtr = *((simd_type*) &m_zero);
	//  minVal = simd_mul_s(simd_load_s(m_huge.m_x), *((simd_type*) &m_nrh0p5));
	//  for (i = 0; i < 8; i ++) {
	//      den = simd_load_s(test[i]);
	//      dot = simd_cmpgt_s(den, *((simd_type*) &m_zero));
	//      den = simd_or_v(simd_and_v (den, dot), simd_andnot_v (*((simd_type*) &m_huge), dot));
	//      dot = simd_cmplt_s(den, minVal);
	//      minVal = simd_min_s(den, minVal);
	//      edgePtr = simd_or_v(simd_and_v (simd_load_s (*((float*) &m_supportVertexStarCuadrant[i])), dot), simd_andnot_v (edgePtr, dot));
	//  }
	//  simd_store_s(edgePtr, (dgFloat32 *)&edge);

	edge = NULL;
	side1 = dgFloat32(1.0e20f);
	for (i = 0; i < 8; i++) {
		//      side0 = plane.Evalue (m_vertex[i]);
		//      test[i] = side0;
		side0 = test[i];
		if (side0 > dgFloat32(0.0f)) {
			if (side0 < side1) {
				side1 = side0;
				edge = m_supportVertexStarCuadrant[i];
			}
		}
	}

	count = 0;
	if (edge) {

		ptr = edge;
		firstEdge = NULL;
		side0 = test[edge->m_vertex];
		do {
			NEWTON_ASSERT(m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32(1.0f));
			side1 = test[ptr->m_twin->m_vertex];
			if (side1 < side0) {
				if (side1 < dgFloat32(0.0f)) {
					firstEdge = ptr;
					break;
				}

				side0 = side1;
				edge = ptr->m_twin;
				ptr = edge;
			}
			ptr = ptr->m_twin->m_next;
		} while (ptr != edge);

		if (firstEdge) {
			edge = firstEdge;
			ptr = edge;
			do {
				index0 = ptr->m_vertex;
				index1 = ptr->m_twin->m_vertex;
				//          dgVector dp (m_vertex[index1] - m_vertex[index0]);
				//          contactsOut[count] = m_vertex[index0] - dp.Scale (test[index0] / (plane % dp));

				NEWTON_ASSERT(m_vertex[index0].m_w == dgFloat32(1.0f));
				NEWTON_ASSERT(m_vertex[index1].m_w == dgFloat32(1.0f));
				p1p0 =
				    simd_sub_v(*(simd_type *)&m_vertex[index1], *(simd_type *)&m_vertex[index0]);
				dot = simd_mul_v(p1p0, *(simd_type *)&plane);
				dot =
				    simd_add_s(simd_add_v(dot, simd_move_hl_v(dot, dot)), simd_permut_v(dot, dot, PURMUT_MASK(3, 3, 3, 1)));
				den = simd_rcp_s(dot);
				den =
				    simd_mul_s(simd_load_s(test[index0]), simd_mul_sub_s(simd_add_s(den, den), simd_mul_s(den, dot), den));
				den =
				    simd_min_s(simd_max_s(den, *(simd_type *)&m_negOne), *(simd_type *)&m_zero);

				NEWTON_ASSERT(((dgFloat32 *)&den)[0] <= dgFloat32(0.0f));
				NEWTON_ASSERT(((dgFloat32 *)&den)[0] >= dgFloat32(-1.0f));
				(*(simd_type *)&contactsOut[count]) =
				    simd_mul_sub_v(*(simd_type *)&m_vertex[index0], p1p0, simd_permut_v(den, den, PURMUT_MASK(3, 0, 0, 0)));

				count++;
				for (ptr1 = ptr->m_next; ptr1 != ptr; ptr1 = ptr1->m_next) {
					index0 = ptr1->m_twin->m_vertex;
					if (test[index0] >= dgFloat32(0.0f)) {
						NEWTON_ASSERT(test[ptr1->m_vertex] <= dgFloat32(0.0f));
						break;
					}
				}
				NEWTON_ASSERT(ptr != ptr1);
				ptr = ptr1->m_twin;
			} while ((ptr != edge) && (count < 8));
		}
	}

	if (count > 1) {
		count = RectifyConvexSlice(count, normal, contactsOut);
	}
	return count;

#else
	return 0;
#endif
}

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

	info->m_box.m_x = m_size[0].m_x * dgFloat32(2.0f);
	info->m_box.m_y = m_size[0].m_y * dgFloat32(2.0f);
	info->m_box.m_z = m_size[0].m_z * dgFloat32(2.0f);
	info->m_offsetMatrix = GetOffsetMatrix();
	//  strcpy (info->m_collisionType, "box");
	info->m_collisionType = m_collsionId;
}

void dgCollisionBox::Serialize(dgSerialize callback, void *const userData) const {
	dgVector size(m_size[0].Scale(dgFloat32(2.0f)));
	size.m_w = m_destructionImpulse;

	SerializeLow(callback, userData);
	callback(userData, &size, sizeof(dgVector));
}