/* 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 "dgCollisionCapsule.h"

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

dgInt32 dgCollisionCapsule::m_shapeRefCount = 0;
dgConvexSimplexEdge dgCollisionCapsule::m_edgeArray[DG_CAPSULE_SEGMENTS
    * (6 + 8 * (DG_CAP_SEGMENTS - 1))];

dgCollisionCapsule::dgCollisionCapsule(dgMemoryAllocator* allocator,
    dgUnsigned32 signature, dgFloat32 radius, dgFloat32 height,
    const dgMatrix& matrix) :
    dgCollisionConvex(allocator, signature, matrix, m_capsuleCollision)
{
  Init(radius, height);
}

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

dgCollisionCapsule::~dgCollisionCapsule()
{
  m_shapeRefCount--;
  _ASSERTE(m_shapeRefCount >= 0);

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

void dgCollisionCapsule::Init(dgFloat32 radius, dgFloat32 height)
{
//	dgInt32 i;
//	dgInt32 j;
//	dgInt32 i0;
//	dgInt32 i1;
//	dgFloat32 x;
//	dgFloat32 y;
//	dgFloat32 z;
//	dgFloat32 r;
//	dgFloat32 angle;
//	dgEdge *edge;

  m_rtti |= dgCollisionCapsule_RTTI;

  dgInt32 i0 = 0;
  dgInt32 i1 = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;

  m_radius = dgAbsf(radius);
  m_height[0] = GetMax(dgFloat32(0.01f),
      dgAbsf(height * dgFloat32(0.5f)) - m_radius);
  m_height[1] = -m_height[0];

  m_silhuette[0] = dgVector(m_height[0], -m_radius, dgFloat32(0.0f),
      dgFloat32(0.0f));
  m_silhuette[1] = dgVector(-m_height[0], -m_radius, dgFloat32(0.0f),
      dgFloat32(0.0f));
  m_silhuette[2] = dgVector(-m_height[0], m_radius, dgFloat32(0.0f),
      dgFloat32(0.0f));
  m_silhuette[3] = dgVector(m_height[0], m_radius, dgFloat32(0.0f),
      dgFloat32(0.0f));

  m_tethaStep = GetDiscretedAngleStep(m_radius);
  m_tethaStepInv = dgFloat32(1.0f) / m_tethaStep;
  m_delCosTetha = dgCos (m_tethaStep);
  m_delSinTetha = dgSin (m_tethaStep);

//	dgFloat32 x = dgFloat32 (0.5f) * m_radius / DG_CAP_SEGMENTS;
  for (dgInt32 j = 0; j < DG_CAP_SEGMENTS; j++)
  {
    dgFloat32 angle = dgFloat32(0.0f);
    dgFloat32 x = (DG_CAP_SEGMENTS - j - 1) * m_radius / DG_CAP_SEGMENTS;
    dgFloat32 r = dgSqrt (m_radius * m_radius - x * x);
    i1 -= DG_CAPSULE_SEGMENTS;
    for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i++)
    {
      dgFloat32 z = dgSin (angle) * r;
      dgFloat32 y = dgCos (angle) * r;
      m_vertex[i0] = dgVector(-(m_height[0] + x), y, z, dgFloat32(1.0f));
      m_vertex[i1] = dgVector((m_height[0] + x), y, z, dgFloat32(1.0f));
      i0++;
      i1++;
      angle += dgPI2 / DG_CAPSULE_SEGMENTS;
    }
    i1 -= DG_CAPSULE_SEGMENTS;
  }

  m_vertexCount = DG_CAPSULE_SEGMENTS * DG_CAP_SEGMENTS * 2;
  m_edgeCount = DG_CAPSULE_SEGMENTS * (6 + 8 * (DG_CAP_SEGMENTS - 1));
  dgCollisionConvex::m_vertex = m_vertex;

  if (!m_shapeRefCount)
  {
    dgPolyhedra polyhedra(m_allocator);
    dgInt32 wireframe[DG_CAPSULE_SEGMENTS + 10];

    i1 = 0;
    i0 = DG_CAPSULE_SEGMENTS - 1;
    polyhedra.BeginFace();
    for (dgInt32 j = 0; j < DG_CAP_SEGMENTS * 2 - 1; j++)
    {
      for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i++)
      {
        wireframe[0] = i0;
        wireframe[1] = i1;
        wireframe[2] = i1 + DG_CAPSULE_SEGMENTS;
        wireframe[3] = i0 + DG_CAPSULE_SEGMENTS;
        i0 = i1;
        i1++;
        polyhedra.AddFace(4, wireframe);
      }
      i0 = i1 + DG_CAPSULE_SEGMENTS - 1;
    }

    for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i++)
    {
      wireframe[i] = DG_CAPSULE_SEGMENTS - 1 - i;
    }
    polyhedra.AddFace(DG_CAPSULE_SEGMENTS, wireframe);

    for (dgInt32 i = 0; i < DG_CAPSULE_SEGMENTS; i++)
    {
      wireframe[i] = i + DG_CAPSULE_SEGMENTS * (DG_CAP_SEGMENTS * 2 - 1);
    }
    polyhedra.AddFace(DG_CAPSULE_SEGMENTS, wireframe);
    polyhedra.EndFace();

    _ASSERTE(SanityCheck (polyhedra));

    dgUnsigned64 i = 0;
    dgPolyhedra::Iterator iter(polyhedra);
    for (iter.Begin(); iter; iter++)
    {
      dgEdge* const edge = &(*iter);
      edge->m_userData = i;
      i++;
    }

    for (iter.Begin(); iter; iter++)
    {
      dgEdge* const edge = &(*iter);

      dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];

      ptr->m_vertex = edge->m_incidentVertex;
      ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
      ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
      ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
    }

  }

  m_shapeRefCount++;
  dgCollisionConvex::m_simplex = m_edgeArray;

  SetVolumeAndCG();

  dgVector inertia;
  dgVector centerOfMass;
  dgVector crossInertia;
  m_volume.m_w = CalculateMassProperties(inertia, crossInertia, centerOfMass);
}

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

  memset(buffer, 0, sizeof(buffer));
  buffer[0] = m_capsuleCollision;
  buffer[1] = dgCollision::Quantize(m_radius);
  buffer[2] = dgCollision::Quantize(m_height[0]);
  memcpy(&buffer[3], &m_offset, sizeof(dgMatrix));
  return dgInt32(dgCollision::MakeCRC(buffer, sizeof(buffer)));
}

void dgCollisionCapsule::TesselateTriangle(dgInt32 level, dgFloat32 side,
    const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count,
    dgVector* ouput) const
{
  if (level)
  {
    _ASSERTE(dgAbsf (p0 % p0 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
    _ASSERTE(dgAbsf (p1 % p1 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
    _ASSERTE(dgAbsf (p2 % p2 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
    dgVector p01(p0 + p1);
    dgVector p12(p1 + p2);
    dgVector p20(p2 + p0);

    p01 = p01.Scale(dgFloat32(1.0f) / dgSqrt(p01 % p01));
    p12 = p12.Scale(dgFloat32(1.0f) / dgSqrt(p12 % p12));
    p20 = p20.Scale(dgFloat32(1.0f) / dgSqrt(p20 % p20));

    _ASSERTE(dgAbsf (p01 % p01 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
    _ASSERTE(dgAbsf (p12 % p12 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
    _ASSERTE(dgAbsf (p20 % p20 - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));

    TesselateTriangle(level - 1, side, p0, p01, p20, count, ouput);
    TesselateTriangle(level - 1, side, p1, p12, p01, count, ouput);
    TesselateTriangle(level - 1, side, p2, p20, p12, count, ouput);
    TesselateTriangle(level - 1, side, p01, p12, p20, count, ouput);

  }
  else
  {
    ouput[count + 0] = p0.Scale(m_radius);
    ouput[count + 1] = p1.Scale(m_radius);
    ouput[count + 2] = p2.Scale(m_radius);
    ouput[count + 0].m_x += side;
    ouput[count + 1].m_x += side;
    ouput[count + 2].m_x += side;
    count += 3;
  }
}

void dgCollisionCapsule::DebugCollision(const dgMatrix& matrixPtr,
    OnDebugCollisionMeshCallback callback, void* const userData) const
{
  dgInt32 i0;
  dgInt32 i1;
  dgInt32 j0;
  dgInt32 j1;

  dgInt32 count;
  dgFloat32 y;
  dgFloat32 z;
  dgFloat32 angle;
#define POWER 2
#define STEPS (4 * (1 << POWER))

  dgTriplex face[32];
  dgTriplex pool[1024];
  dgVector tmpVectex[1024];

  angle = dgFloat32(0.0f);
  for (i0 = 0; i0 < STEPS; i0++)
  {
    z = dgSin (angle) * m_radius;
    y = dgCos (angle) * m_radius;
    tmpVectex[i0].m_x = -m_height[0];
    tmpVectex[i0].m_y = y;
    tmpVectex[i0].m_z = z;
    tmpVectex[i0 + STEPS].m_x = m_height[0];
    tmpVectex[i0 + STEPS].m_y = y;
    tmpVectex[i0 + STEPS].m_z = z;
    angle += dgPI2 / dgFloat32(STEPS);
  }

  dgVector p0(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f),
      dgFloat32(0.0f));
  dgVector p1(-dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f),
      dgFloat32(0.0f));
  dgVector p2(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f),
      dgFloat32(0.0f));
  dgVector p3(dgFloat32(0.0f), -dgFloat32(1.0f), dgFloat32(0.0f),
      dgFloat32(0.0f));
  dgVector p4(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(1.0f),
      dgFloat32(0.0f));
  dgVector p5(dgFloat32(0.0f), dgFloat32(0.0f), -dgFloat32(1.0f),
      dgFloat32(0.0f));

  count = STEPS * 2;
  TesselateTriangle(POWER, m_height[0], p0, p2, p4, count, tmpVectex);
  TesselateTriangle(POWER, m_height[0], p0, p4, p3, count, tmpVectex);
  TesselateTriangle(POWER, m_height[0], p0, p3, p5, count, tmpVectex);
  TesselateTriangle(POWER, m_height[0], p0, p5, p2, count, tmpVectex);

  TesselateTriangle(POWER, -m_height[0], p1, p4, p2, count, tmpVectex);
  TesselateTriangle(POWER, -m_height[0], p1, p3, p4, count, tmpVectex);
  TesselateTriangle(POWER, -m_height[0], p1, p5, p3, count, tmpVectex);
  TesselateTriangle(POWER, -m_height[0], p1, p2, p5, count, tmpVectex);

  dgMatrix matrix(GetOffsetMatrix() * matrixPtr);
  matrix.TransformTriplex(&pool[0].m_x, sizeof(dgTriplex), &tmpVectex[0].m_x,
      sizeof(dgVector), count);

  i0 = STEPS - 1;
  j1 = STEPS;
  j0 = STEPS + STEPS - 1;
  for (i1 = 0; i1 < STEPS; i1++)
  {
    face[0] = pool[i0];
    face[1] = pool[i1];
    face[2] = pool[j1];
    face[3] = pool[j0];
    callback(userData, 4, &face[0].m_x, 0);
    i0 = i1;
    j0 = j1;
    j1++;
  }

  for (i1 = STEPS * 2; i1 < count; i1 += 3)
  {
    callback(userData, 3, &pool[i1].m_x, 0);
  }
}

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

dgVector dgCollisionCapsule::SupportVertexSimd(const dgVector& dir) const
{
#ifdef DG_BUILD_SIMD_CODE

  dgInt32 index;
  dgFloatSign *ptr;

  _ASSERTE(dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

  ptr = (dgFloatSign*) &dir;
  index = -(ptr[0].m_integer.m_iVal >> 31);
  dgVector p(dir.Scale(m_radius));
  p.m_x += m_height[index];
  return p;

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

dgVector dgCollisionCapsule::SupportVertex(const dgVector& dir) const
{
  dgInt32 index;
  dgFloatSign *ptr;

  _ASSERTE(dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));

  ptr = (dgFloatSign*) &dir;
  index = -(ptr[0].m_integer.m_iVal >> 31);
  dgVector p(dir.Scale(m_radius));
  p.m_x += m_height[index];
  return p;
}

dgFloat32 dgCollisionCapsule::RayCast (const dgVector& q0, const dgVector& q1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
		return dgFloat32 (1.2f);
	}


	dgFloat32 t = dgFloat32 (1.2f);

	dgVector p0 (q0);
	p0.m_x = dgFloat32 (0.0f);
	dgFloat32 radius = m_radius;

	dgFloat32 c = (p0 % p0) - radius * radius;
	if (c > dgFloat32 (0.0f)) {
		dgVector p1 (q1);
		p1.m_x = dgFloat32 (0.0f);

		dgVector dp (p1 - p0);

		dgFloat32 a = dp % dp;
		dgFloat32 b = dgFloat32 (2.0f) * (p0 % dp);
		dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
		if (desc > dgFloat32 (1.0e-8f)) {
			desc = dgSqrt (desc);
			a = dgFloat32 (1.0f) / (dgFloat32 (2.0f) * a);
			dgFloat32 t1 = GetMin ((- b + desc) * a, (- b - desc) * a);
			if ((t1 >= dgFloat32 (0.0f)) && t1 < dgFloat32 (1.0f)) {
				dgFloat32 x = q0.m_x + (q1.m_x - q0.m_x) * t1;
				if (x > m_height[0]) {
					dgVector h (m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
					dgVector dq (q1 - q0);
					dgVector dqp0 (q0 - h);
					a = dq % dq;
					b = dgFloat32 (2.0f) * (dqp0 % dq);
					c = dqp0 % dqp0 - radius * radius;
					desc = b * b - dgFloat32 (4.0f) * a * c;
					if (desc > dgFloat32 (1.0e-8f)) {
						desc = dgSqrt (desc);
						a = dgFloat32 (1.0f) / (dgFloat32 (2.0f) * a);
						t1 = GetMin ((- b + desc) * a, (- b - desc) * a);
						if ((t1 >= dgFloat32 (0.0f)) && t1 < dgFloat32 (1.0f)) {
							t = t1;
						dgVector n (q0 + dq.Scale (t) - h); 
						contactOut.m_normal = n.Scale (dgRsqrt (n % n));
						contactOut.m_userId = SetUserDataID();
					}
					}

				} else if (x < -m_height[0]) {
					dgVector h (-m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
					dgVector dq (q1 - q0);
					dgVector dqp0 (q0 - h);
					a = dq % dq;
					b = dgFloat32 (2.0f) * (dqp0 % dq);
					c = dqp0 % dqp0 - radius * radius;
					desc = b * b - dgFloat32 (4.0f) * a * c;
					if (desc > dgFloat32 (1.0e-8f)) {
						desc = dgSqrt (desc);
						a = dgFloat32 (1.0f) / (dgFloat32 (2.0f) * a);
						t1 = GetMin ((- b + desc) * a, (- b - desc) * a);
						if ((t1 >= dgFloat32 (0.0f)) && t1 < dgFloat32 (1.0f)) {
							t = t1;
							dgVector n (q0 + dq.Scale (t) - h); 
							contactOut.m_normal = n.Scale (dgRsqrt (n % n));
							contactOut.m_userId = SetUserDataID();
						}
					}
				} else { 
					t = t1;
					dgVector n (p0 + dp.Scale (t));
					contactOut.m_normal = n.Scale (dgRsqrt (n % n));
					contactOut.m_userId = SetUserDataID();
				}
			}
		}
	} else {
		if (q0.m_x > m_height[0]) {
			dgVector h (m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
			dgVector dq (q1 - q0);
			dgVector dqp0 (q0 - h);
			dgFloat32 a = dq % dq;
			dgFloat32 b = dgFloat32 (2.0f) * (dqp0 % dq);
			dgFloat32 c = dqp0 % dqp0 - radius * radius;
			if (c > dgFloat32 (0.0f)) {
				dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
				if (desc > dgFloat32 (1.0e-8f)) {
					dgFloat32 t1;
					desc = dgSqrt (desc);
					a = dgFloat32 (1.0f) / (dgFloat32 (2.0f) * a);
					t1 = GetMin ((- b + desc) * a, (- b - desc) * a);
					if (t1 >= dgFloat32 (0.0f)) {
						t = t1;
						dgVector n (q0 + dq.Scale (t) - h); 
						contactOut.m_normal = n.Scale (dgRsqrt (n % n));
						contactOut.m_userId = SetUserDataID();
					}
				}
			}

		} else if (q0.m_x < -m_height[0]) {
			dgVector h (-m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
			dgVector dq (q1 - q0);
			dgVector dqp0 (q0 - h);
			dgFloat32 a = dq % dq;
			dgFloat32 b = dgFloat32 (2.0f) * (dqp0 % dq);
			dgFloat32 c = dqp0 % dqp0 - radius * radius;
			if (c > dgFloat32 (0.0f)) {
				dgFloat32 desc = b * b - dgFloat32 (4.0f) * a * c;
				if (desc > dgFloat32 (1.0e-8f)) {
					dgFloat32 t1;
					desc = dgSqrt (desc);
					a = dgFloat32 (1.0f) / (dgFloat32 (2.0f) * a);
					t1 = GetMin ((- b + desc) * a, (- b - desc) * a);
					if (t1 >= dgFloat32 (0.0f)) {
						t = t1;
						dgVector n (q0 + dq.Scale (t) - h); 
						contactOut.m_normal = n.Scale (dgRsqrt (n % n));
						contactOut.m_userId = SetUserDataID();
					}
				}
			}
		}
	}

	return t;
}


dgFloat32 dgCollisionCapsule::RayCastSimd(const dgVector& q0,
    const dgVector& q1, dgContactPoint& contactOut,
    OnRayPrecastAction preFilter, const dgBody* const body,
    void* const userData) const
{
  return RayCast(q0, q1, contactOut, preFilter, body, userData);
}

dgFloat32 dgCollisionCapsule::CalculateMassProperties(dgVector& inertia,
    dgVector& crossInertia, dgVector& centerOfMass) const
{
  dgFloat32 volume;
  dgFloat32 inertiaxx;
  dgFloat32 inertiayyzz;
  dgFloat32 cylVolume;
  dgFloat32 sphVolume;
  dgFloat32 cylInertiaxx;
  dgFloat32 sphInertiaxx;
  dgFloat32 cylInertiayyzz;
  dgFloat32 sphInertiayyzz;

  centerOfMass = GetOffsetMatrix().m_posit;
  cylVolume = dgFloat32(3.1616f * 2.0f) * m_radius * m_radius * m_height[0];
  sphVolume = dgFloat32(3.1616f * 4.0f / 3.0f) * m_radius * m_radius * m_radius;

  cylInertiaxx = (dgFloat32(0.5f) * m_radius * m_radius) * cylVolume;
  sphInertiaxx = (dgFloat32(2.0f / 5.0f) * m_radius * m_radius) * sphVolume;

  cylInertiayyzz = (dgFloat32(0.25f) * m_radius * m_radius
      + dgFloat32(1.0f / 3.0f) * m_height[0] * m_height[0]) * cylVolume;
  sphInertiayyzz = sphInertiaxx + m_height[0] * m_height[0] * sphVolume;

  volume = cylVolume + sphVolume;
  inertiaxx = cylInertiaxx + sphInertiaxx;
  inertiayyzz = cylInertiayyzz + sphInertiayyzz;

  dgMatrix inertiaTensor(dgGetIdentityMatrix());

  inertiaTensor[0][0] = inertiaxx;
  inertiaTensor[1][1] = inertiayyzz;
  inertiaTensor[2][2] = inertiayyzz;

  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;

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

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

dgInt32 dgCollisionCapsule::CalculatePlaneIntersectionSimd(
    const dgVector& normal, const dgVector& origin,
    dgVector* const contactsOut) const
{
#ifdef DG_BUILD_SIMD_CODE
  return dgCollisionCapsule::CalculatePlaneIntersection(normal, origin,
      contactsOut);
#else
  return 0;
#endif
}

dgInt32 dgCollisionCapsule::CalculatePlaneIntersection(const dgVector& normal,
    const dgVector& origin, dgVector* const contactsOut) const
{
  dgInt32 count = 0;
  if (dgAbsf(normal.m_x) > dgFloat32(0.999f))
  {
		dgVector center1 (-m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
		dgVector p1 (center1 - normal.Scale ((center1 - origin) % normal));
		if (p1.m_x < (-m_height[0])) {
			dgFloat32 t = normal % (p1 - origin);
			contactsOut[0] = p1 - normal.Scale (t);
			return 1;
		} else {
			dgVector center0 (m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
			dgVector p0 (center0 - normal.Scale ((center0 - origin) % normal));
			if (p0.m_x > m_height[0]) {
				dgFloat32 t = normal % (p0 - origin);
				contactsOut[0] = p0 - normal.Scale (t);
				return 1;
			}
		}
  }
  else
  {
		dgFloat32 magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
    dgFloat32 cosAng = normal.m_y * magInv;
    dgFloat32 sinAng = normal.m_z * magInv;
		_ASSERTE(dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
		dgVector normal1(normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng,	dgFloat32(0.0f), dgFloat32(0.0f));
    dgVector origin1(origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
        origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32(0.0f));
    dgPlane plane(normal1, -(normal1 % origin1));
		dgVector maxDir((normal1.m_x > dgFloat32(0.0f)) ? m_silhuette[3].m_x : -m_silhuette[3].m_x,	
						(normal1.m_y > dgFloat32(0.0f)) ? m_silhuette[3].m_y : -m_silhuette[3].m_y, dgFloat32(0.0f), dgFloat32(0.0f));

    dgFloat32 test0 = plane.Evalue(maxDir);
    dgFloat32 test1 = plane.Evalue(maxDir.Scale(dgFloat32(-1.0f)));
		if ((test0 * test1) > dgFloat32(0.0f)) {
			dgVector center1 (-m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
			dgVector p1 (center1 - normal.Scale ((center1 - origin) % normal));
			if (p1.m_x < (-m_height[0])) {
				dgFloat32 t = normal % (p1 - origin);
				contactsOut[0] = p1 - normal.Scale (t);
				return 1;
			} else {
				dgVector center0 (m_height[0], dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
				dgVector p0 (center0 - normal.Scale ((center0 - origin) % normal));
				if (p0.m_x > m_height[0]) {
					dgFloat32 t = normal % (p0 - origin);
					contactsOut[0] = p0 - normal.Scale (t);
					return 1;
        }
      }
    }
    else
    {
      dgVector dp(m_silhuette[1] - m_silhuette[0]);
      dgFloat32 den = normal1 % dp;
      if (dgAbsf(den) > dgFloat32(0.0f))
      {
        test0 = -plane.Evalue(m_silhuette[0]) / den;
        if ((test0 <= dgFloat32(1.0)) && (test0 >= dgFloat32(0.0f)))
        {
          contactsOut[count] = m_silhuette[0] + dp.Scale(test0);
          count++;
        }
      }

      if (count < 2)
      {
        test0 = plane.m_w - plane.m_x * m_height[0];
        if (dgAbsf(test0) < m_radius)
        {
          dgFloat32 r = -m_height[0];
          dgFloat32 d = plane.m_w + r * plane.m_x;

          dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
          dgFloat32 b = dgFloat32(2.0f) * plane.m_x * d;
          dgFloat32 c = d * d - m_radius * m_radius * plane.m_y * plane.m_y;
          dgFloat32 desc = b * b - dgFloat32(4.0f) * a * c;
          if (desc > dgFloat32(0.0f))
          {
            _ASSERTE(dgAbsf (a) > dgFloat32 (0.0f));
            desc = dgSqrt (desc);
            a = -dgFloat32(0.5f) * b / a;
            dgFloat32 x0 = a + desc;
            dgFloat32 x1 = a - desc;
            if (x0 > dgFloat32(0.0f))
            {
              x0 = x1;
            }
            if (x0 < 0.0f)
            {
              _ASSERTE(x0 <= dgFloat32 (0.0f));
              _ASSERTE(dgAbsf (plane.m_y) > dgFloat32 (0.0f));
              dgFloat32 y = -(plane.m_x * x0 + d) / plane.m_y;
              contactsOut[count] = dgVector(x0 + r, y, dgFloat32(0.0f),
                  dgFloat32(0.0f));
              count++;
            }
          }
        }
      }

      if (count < 2)
      {
        dgVector dp(m_silhuette[3] - m_silhuette[2]);
        den = normal1 % dp;
        if (dgAbsf(den) > dgFloat32(0.0f))
        {
          test0 = -plane.Evalue(m_silhuette[2]) / den;
          if ((test0 <= dgFloat32(1.0)) && (test0 >= dgFloat32(0.0f)))
          {
            contactsOut[count] = m_silhuette[2] + dp.Scale(test0);
            count++;
          }
        }
      }

      if (count < 2)
      {
        test0 = plane.m_w + plane.m_x * m_height[0];
        if (dgAbsf(test0) < m_radius)
        {
          dgFloat32 r = m_height[0];
          dgFloat32 d = plane.m_w + r * plane.m_x;

          dgFloat32 a = plane.m_x * plane.m_x + plane.m_y * plane.m_y;
          dgFloat32 b = dgFloat32(2.0f) * plane.m_x * d;
          dgFloat32 c = d * d - m_radius * m_radius * plane.m_y * plane.m_y;
          dgFloat32 desc = b * b - dgFloat32(4.0f) * a * c;
          if (desc > dgFloat32(0.0f))
          {
            _ASSERTE(dgAbsf (a) > dgFloat32 (0.0f));
            desc = dgSqrt (desc);
            a = -dgFloat32(0.5f) * b / a;
            dgFloat32 x0 = a + desc;
            dgFloat32 x1 = a - desc;
            if (x0 < dgFloat32(0.0f))
            {
              x0 = x1;
            }
            if (x0 > 0.0f)
            {
              _ASSERTE(x0 >= dgFloat32 (0.0f));
              _ASSERTE(dgAbsf (plane.m_y) > dgFloat32 (0.0f));
              dgFloat32 y = -(plane.m_x * x0 + d) / plane.m_y;
              contactsOut[count] = dgVector(x0 + r, y, dgFloat32(0.0f),
                  dgFloat32(0.0f));
              count++;
            }
          }
        }
      }
    }

    for (dgInt32 i = 0; i < count; i++)
    {
      dgFloat32 y = contactsOut[i].m_y;
      dgFloat32 z = contactsOut[i].m_z;
      contactsOut[i].m_y = y * cosAng - z * sinAng;
      contactsOut[i].m_z = z * cosAng + y * sinAng;
    }
  }
  return count;
}

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

  info->m_capsule.m_r0 = m_radius;
  info->m_capsule.m_r1 = m_radius;
  info->m_capsule.m_height = dgFloat32(2.0f) * (m_radius + m_height[0]);
  info->m_offsetMatrix = GetOffsetMatrix();
//	strcpy (info->m_collisionType, "capsule");
  info->m_collisionType = m_collsionId;

}

void dgCollisionCapsule::Serialize(dgSerialize callback,
    void* const userData) const
{
  dgVector size(m_radius, dgFloat32(2.0f) * (m_radius + m_height[0]),
      dgFloat32(0.0f), dgFloat32(0.0f));

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