/* 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 "dgCollisionBox.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 --;
//	_ASSERTE (m_shapeRefCount >= 0);

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

void dgCollisionBox::SetCollisionBBox(const dgVector& p0__,
    const dgVector& p1__)
{
  _ASSERTE(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
{
  _ASSERTE(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, body, 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))
  {
    _ASSERTE(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)
  {
    _ASSERTE(test[edge->m_vertex] > dgFloat32 (0.0f));

    dgConvexSimplexEdge* ptr = edge;
    dgConvexSimplexEdge* firstEdge = NULL;
    dgFloat32 side0 = test[edge->m_vertex];
    do
    {
      _ASSERTE(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);
          }
        }

        _ASSERTE(t <= dgFloat32 (0.01f));
        _ASSERTE(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))
          {
            _ASSERTE(test[ptr1->m_vertex] <= dgFloat32 (0.0f));
            break;
          }
        }_ASSERTE(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
    {
      _ASSERTE(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));

        _ASSERTE(m_vertex[index0].m_w == dgFloat32 (1.0f));
        _ASSERTE(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);

        _ASSERTE(((dgFloat32*)&den)[0] <= dgFloat32 (0.0f));
        _ASSERTE(((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))
          {
            _ASSERTE(test[ptr1->m_vertex] <= dgFloat32 (0.0f));
            break;
          }
        }_ASSERTE(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));
}

