/* 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 "dgWorld.h"
#include "dgCollisionHeightField.h"

#define DG_HIGHTFILD_DATA_ID 0x45AF5E07

dgCollisionHeightField::dgCollisionHeightField(dgWorld* const world,
    dgInt32 width, dgInt32 height, dgInt32 contructionMode,
    const dgUnsigned16* const elevationMap, const dgInt8* const atributeMap,
    dgFloat32 horizontalScale, dgFloat32 vertcalScale) :
    dgCollisionMesh(world->GetAllocator(), m_heightField)
{
  dgFloat32 y0;
  dgFloat32 y1;

  m_userRayCastCallback = NULL;
  m_rtti |= dgCollisionHeightField_RTTI;
  m_width = width;
  m_height = height;
  m_diagonalMode = contructionMode;
  m_verticalScale = vertcalScale;
  m_horizontalScale = horizontalScale;

  m_elevationMap = (dgUnsigned16 *) dgMallocStack(
      m_width * m_height * sizeof(dgUnsigned16));
  memcpy(m_elevationMap, elevationMap,
      m_width * m_height * sizeof(dgUnsigned16));

  m_atributeMap = (dgInt8 *) dgMallocStack(m_width * m_height * sizeof(dgInt8));
  memcpy(m_atributeMap, atributeMap, m_width * m_height * sizeof(dgInt8));

  y0 = dgFloat32(0.0f);
  y1 = dgFloat32(-dgFloat32(1.0e10f));
  for (dgInt32 i = 0; i < m_width * m_height; i++)
  {
    y1 = GetMax(y1, dgFloat32(m_elevationMap[i]));
  }

  m_minBox = dgVector(dgFloat32(dgFloat32(0.0f)), y0 * m_verticalScale,
      dgFloat32(dgFloat32(0.0f)), dgFloat32(dgFloat32(1.0f)));
  m_maxBox = dgVector(dgFloat32(m_width - 1) * m_horizontalScale,
      y1 * m_verticalScale, dgFloat32(m_height - 1) * m_horizontalScale,
      dgFloat32(dgFloat32(1.0f)));

//	m_verticalScaleInv = dgFloat32 (1.0f) / m_verticalScale;
  m_horizontalScaleInv = dgFloat32(1.0f) / m_horizontalScale;

  dgTree<void*, unsigned>::dgTreeNode* nodeData = world->m_perInstanceData.Find(
      DG_HIGHTFILD_DATA_ID);
  if (!nodeData)
  {
    m_instanceData = (dgPerIntanceData*) dgMallocStack(
        sizeof(dgPerIntanceData));
    m_instanceData->m_refCount = 0;
    m_instanceData->m_world = world;
    for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i++)
    {
      m_instanceData->m_vertex[i] = NULL;
      m_instanceData->m_vertexCount[i] = 8 * 8;
      AllocateVertex(world, i);
    }
    nodeData = world->m_perInstanceData.Insert(m_instanceData,
        DG_HIGHTFILD_DATA_ID);
  }
  m_instanceData = (dgPerIntanceData*) nodeData->GetInfo();

  m_instanceData->m_refCount++;
  SetCollisionBBox(m_minBox, m_maxBox);
}

dgCollisionHeightField::dgCollisionHeightField(dgWorld* const world,
    dgDeserialize deserialization, void* const userData) :
    dgCollisionMesh(world, deserialization, userData)
{
  m_rtti |= dgCollisionHeightField_RTTI;

  m_userRayCastCallback = NULL;
  deserialization(userData, &m_width, sizeof(dgInt32));
  deserialization(userData, &m_height, sizeof(dgInt32));
  deserialization(userData, &m_diagonalMode, sizeof(dgInt32));
  deserialization(userData, &m_verticalScale, sizeof(dgFloat32));
  deserialization(userData, &m_horizontalScale, sizeof(dgFloat32));
  deserialization(userData, &m_minBox.m_x, sizeof(dgVector));
  deserialization(userData, &m_maxBox.m_x, sizeof(dgVector));

  m_elevationMap = (dgUnsigned16 *) dgMallocStack(
      m_width * m_height * sizeof(dgUnsigned16));
  m_atributeMap = (dgInt8 *) dgMallocStack(m_width * m_height * sizeof(dgInt8));

  deserialization(userData, m_elevationMap,
      m_width * m_height * sizeof(dgUnsigned16));
  deserialization(userData, m_atributeMap, m_width * m_height * sizeof(dgInt8));

//	m_verticalScaleInv = dgFloat32 (1.0f) / m_verticalScale;
  m_horizontalScaleInv = dgFloat32(1.0f) / m_horizontalScale;

  dgTree<void*, unsigned>::dgTreeNode* nodeData = world->m_perInstanceData.Find(
      DG_HIGHTFILD_DATA_ID);
  if (!nodeData)
  {
    m_instanceData = (dgPerIntanceData*) dgMallocStack(
        sizeof(dgPerIntanceData));
    m_instanceData->m_refCount = 0;
    m_instanceData->m_world = world;
    for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i++)
    {
      m_instanceData->m_vertex[i] = NULL;
      m_instanceData->m_vertexCount[i] = 8 * 8;
      AllocateVertex(world, i);
    }
    nodeData = world->m_perInstanceData.Insert(m_instanceData,
        DG_HIGHTFILD_DATA_ID);
  }
  m_instanceData = (dgPerIntanceData*) nodeData->GetInfo();

  m_instanceData->m_refCount++;
  SetCollisionBBox(m_minBox, m_maxBox);
}

dgCollisionHeightField::~dgCollisionHeightField(void)
{
  m_instanceData->m_refCount--;
  if (!m_instanceData->m_refCount)
  {
    dgWorld* world = m_instanceData->m_world;

    for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i++)
    {
      dgFreeStack(m_instanceData->m_vertex[i]);
    }
    dgFreeStack(m_instanceData);
    world->m_perInstanceData.Remove(DG_HIGHTFILD_DATA_ID);
  }
  dgFreeStack(m_elevationMap);
  dgFreeStack(m_atributeMap);
}

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

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

  callback(userData, &m_width, sizeof(dgInt32));
  callback(userData, &m_height, sizeof(dgInt32));
  callback(userData, &m_diagonalMode, sizeof(dgInt32));
  callback(userData, &m_verticalScale, sizeof(dgFloat32));
  callback(userData, &m_horizontalScale, sizeof(dgFloat32));
  callback(userData, &m_minBox.m_x, sizeof(dgVector));
  callback(userData, &m_maxBox.m_x, sizeof(dgVector));

  callback(userData, m_elevationMap, m_width * m_height * sizeof(dgInt16));
  callback(userData, m_atributeMap, m_width * m_height * sizeof(dgInt8));
}

void dgCollisionHeightField::SetCollisionRayCastCallback(
    dgCollisionHeightFieldRayCastCallback rayCastCallback)
{
  m_userRayCastCallback = rayCastCallback;
}

void dgCollisionHeightField::AllocateVertex(dgWorld* const world,
    dgInt32 threadIndex) const
{
  dgVector *vertex;
  vertex = (dgVector *) dgMallocStack(
      2 * m_instanceData->m_vertexCount[threadIndex] * sizeof(dgVector));
  if (m_instanceData->m_vertex[threadIndex])
  {
    memcpy(vertex, m_instanceData->m_vertex[threadIndex],
        m_instanceData->m_vertexCount[threadIndex] * sizeof(dgVector));
    dgFreeStack(m_instanceData->m_vertex[threadIndex]);
  }

  m_instanceData->m_vertexCount[threadIndex] *= 2;
  m_instanceData->m_vertex[threadIndex] = vertex;
}

void dgCollisionHeightField::GetCollisionInfo(dgCollisionInfo* info) const
{
  dgCollision::GetCollisionInfo(info);

  info->m_offsetMatrix = GetOffsetMatrix();
  info->m_collisionType = m_collsionId;

  dgCollisionInfo::dgHeightMapCollisionData& data = info->m_heightFieldCollision;
  data.m_width = m_width;
  data.m_height = m_height;
  data.m_gridsDiagonals = m_diagonalMode;
  data.m_verticalScale = m_verticalScale;
  data.m_horizonalScale = m_horizontalScale;
  data.m_atributes = m_atributeMap;
  data.m_elevation = m_elevationMap;
}

void dgCollisionHeightField::CalculateMinExtend2d(const dgVector& p0,
    const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const
{
  dgFloat32 x0 = GetMin(p0.m_x, p1.m_x) - dgFloat32(1.0e-3f);
  dgFloat32 z0 = GetMin(p0.m_z, p1.m_z) - dgFloat32(1.0e-3f);

  dgFloat32 x1 = GetMax(p0.m_x, p1.m_x) + dgFloat32(1.0e-3f);
  dgFloat32 z1 = GetMax(p0.m_z, p1.m_z) + dgFloat32(1.0e-3f);

  x0 = m_horizontalScale * dgFloor(x0 * m_horizontalScaleInv);
  z0 = m_horizontalScale * dgFloor(z0 * m_horizontalScaleInv);
  x1 = m_horizontalScale * dgFloor(x1 * m_horizontalScaleInv)
      + m_horizontalScale;
  z1 = m_horizontalScale * dgFloor(z1 * m_horizontalScaleInv)
      + m_horizontalScale;

  boxP0.m_x = GetMax(x0, m_minBox.m_x);
  boxP0.m_z = GetMax(z0, m_minBox.m_z);
  boxP0.m_y = -dgFloat32(1.0e10f);
  boxP0.m_w = dgFloat32(0.0f);

  boxP1.m_x = GetMin(x1, m_maxBox.m_x);
  boxP1.m_z = GetMin(z1, m_maxBox.m_z);
  boxP1.m_y = dgFloat32(1.0e10f);
  boxP1.m_w = dgFloat32(0.0f);
}

void dgCollisionHeightField::CalculateMinExtend3d(const dgVector& p0,
    const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const
{
  _ASSERTE(p0.m_x <= p1.m_x);
  _ASSERTE(p0.m_y <= p1.m_y);
  _ASSERTE(p0.m_z <= p1.m_z);

  dgFloat32 x0 = m_horizontalScale
      * dgFloor((p0.m_x - dgFloat32(1.0e-3f)) * m_horizontalScaleInv);
  dgFloat32 z0 = m_horizontalScale
      * dgFloor((p0.m_z - dgFloat32(1.0e-3f)) * m_horizontalScaleInv);
  dgFloat32 x1 = m_horizontalScale
      * dgFloor((p1.m_x + dgFloat32(1.0e-3f)) * m_horizontalScaleInv)
      + m_horizontalScale;
  dgFloat32 z1 = m_horizontalScale
      * dgFloor((p1.m_z + dgFloat32(1.0e-3f)) * m_horizontalScaleInv)
      + m_horizontalScale;

  boxP0.m_x = GetMax(x0, m_minBox.m_x);
  boxP0.m_z = GetMax(z0, m_minBox.m_z);
  boxP0.m_y = p0.m_y - dgFloat32(1.0e-3f);
  boxP0.m_w = dgFloat32(0.0f);

  boxP1.m_x = GetMin(x1, m_maxBox.m_x);
  boxP1.m_z = GetMin(z1, m_maxBox.m_z);
  boxP1.m_y = p1.m_y + dgFloat32(1.0e-3f);
  boxP1.m_w = dgFloat32(0.0f);
}

dgFloat32 dgCollisionHeightField::RayCastCellSimd(const FastRayTest& ray,
    dgInt32 xIndex0, dgInt32 zIndex0, dgVector& normalOut) const
{
  dgFloat32 t;
  dgInt32 base;
  dgInt32 triangle[3];
  dgVector points[4];

  // get the 3d point at the corner of the cell

  if ((xIndex0 < 0) || (zIndex0 < 0) || (xIndex0 >= (m_width - 1))
      || (zIndex0 >= (m_height - 1)))
  {
    return dgFloat32(1.2f);
  }

  base = zIndex0 * m_width + xIndex0;

  points[0 * 2 + 0] = dgVector((xIndex0 + 0) * m_horizontalScale,
      dgFloat32(m_elevationMap[base]) * m_verticalScale,
      (zIndex0 + 0) * m_horizontalScale, dgFloat32(0.0f));
  points[0 * 2 + 1] = dgVector((xIndex0 + 1) * m_horizontalScale,
      dgFloat32(m_elevationMap[base + 1]) * m_verticalScale,
      (zIndex0 + 0) * m_horizontalScale, dgFloat32(0.0f));
  points[1 * 2 + 1] = dgVector((xIndex0 + 1) * m_horizontalScale,
      dgFloat32(m_elevationMap[base + m_width + 1]) * m_verticalScale,
      (zIndex0 + 1) * m_horizontalScale, dgFloat32(0.0f));
  points[1 * 2 + 0] = dgVector((xIndex0 + 0) * m_horizontalScale,
      dgFloat32(m_elevationMap[base + m_width + 0]) * m_verticalScale,
      (zIndex0 + 1) * m_horizontalScale, dgFloat32(0.0f));

  t = dgFloat32(1.2f);
  if (!m_diagonalMode)
  {
    triangle[0] = 1;
    triangle[1] = 2;
    triangle[2] = 3;

    dgVector e10(points[2] - points[1]);
    dgVector e20(points[3] - points[1]);
    dgVector normal(e10 * e20);
    t = ray.PolygonIntersectSimd(normal, &points[0].m_x, sizeof(dgVector),
        triangle, 3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }

    triangle[0] = 1;
    triangle[1] = 0;
    triangle[2] = 2;

    dgVector e30(points[0] - points[1]);
    normal = e30 * e10;
    t = ray.PolygonIntersectSimd(normal, &points[0].m_x, sizeof(dgVector),
        triangle, 3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }

  }
  else
  {

    triangle[0] = 0;
    triangle[1] = 2;
    triangle[2] = 3;

    dgVector e10(points[2] - points[0]);
    dgVector e20(points[3] - points[0]);
    dgVector normal(e10 * e20);
    t = ray.PolygonIntersectSimd(normal, &points[0].m_x, sizeof(dgVector),
        triangle, 3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }

    triangle[0] = 0;
    triangle[1] = 3;
    triangle[2] = 1;

    dgVector e30(points[1] - points[0]);
    normal = e20 * e30;
    t = ray.PolygonIntersectSimd(normal, &points[0].m_x, sizeof(dgVector),
        triangle, 3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }
  }

  return t;
}

dgFloat32 dgCollisionHeightField::RayCastCell(const FastRayTest& ray,
    dgInt32 xIndex0, dgInt32 zIndex0, dgVector& normalOut) const
{
  dgFloat32 t;
  dgInt32 base;
  dgInt32 triangle[3];
  dgVector points[4];

  // get the 3d point at the corner of the cell
  if ((xIndex0 < 0) || (zIndex0 < 0) || (xIndex0 >= (m_width - 1))
      || (zIndex0 >= (m_height - 1)))
  {
    return dgFloat32(1.2f);
  }

  base = zIndex0 * m_width + xIndex0;

  points[0 * 2 + 0] = dgVector((xIndex0 + 0) * m_horizontalScale,
      dgFloat32(m_elevationMap[base]) * m_verticalScale,
      (zIndex0 + 0) * m_horizontalScale, dgFloat32(0.0f));
  points[0 * 2 + 1] = dgVector((xIndex0 + 1) * m_horizontalScale,
      dgFloat32(m_elevationMap[base + 1]) * m_verticalScale,
      (zIndex0 + 0) * m_horizontalScale, dgFloat32(0.0f));
  points[1 * 2 + 1] = dgVector((xIndex0 + 1) * m_horizontalScale,
      dgFloat32(m_elevationMap[base + m_width + 1]) * m_verticalScale,
      (zIndex0 + 1) * m_horizontalScale, dgFloat32(0.0f));
  points[1 * 2 + 0] = dgVector((xIndex0 + 0) * m_horizontalScale,
      dgFloat32(m_elevationMap[base + m_width + 0]) * m_verticalScale,
      (zIndex0 + 1) * m_horizontalScale, dgFloat32(0.0f));

  t = dgFloat32(1.2f);
  if (!m_diagonalMode)
  {
    triangle[0] = 1;
    triangle[1] = 2;
    triangle[2] = 3;

    dgVector e10(points[2] - points[1]);
    dgVector e20(points[3] - points[1]);
    dgVector normal(e10 * e20);
    t = ray.PolygonIntersect(normal, &points[0].m_x, sizeof(dgVector), triangle,
        3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }

    triangle[0] = 1;
    triangle[1] = 0;
    triangle[2] = 2;

    dgVector e30(points[0] - points[1]);
    normal = e30 * e10;
    t = ray.PolygonIntersect(normal, &points[0].m_x, sizeof(dgVector), triangle,
        3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }

  }
  else
  {

    triangle[0] = 0;
    triangle[1] = 2;
    triangle[2] = 3;

    dgVector e10(points[2] - points[0]);
    dgVector e20(points[3] - points[0]);
    dgVector normal(e10 * e20);
    t = ray.PolygonIntersect(normal, &points[0].m_x, sizeof(dgVector), triangle,
        3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }

    triangle[0] = 0;
    triangle[1] = 3;
    triangle[2] = 1;

    dgVector e30(points[1] - points[0]);
    normal = e20 * e30;
    t = ray.PolygonIntersect(normal, &points[0].m_x, sizeof(dgVector), triangle,
        3);
    if (t < dgFloat32(1.0f))
    {
      normalOut = normal;
      return t;
    }
  }
  return t;
}

dgFloat32 dgCollisionHeightField::RayCastSimd(const dgVector& q0,
    const dgVector& q1, dgContactPoint& contactOut,
    OnRayPrecastAction preFilter, const dgBody* const body,
    void* const userData) const
{
  dgVector boxP0;
  dgVector boxP1;

  // set the debug line counter to zero
//	debugRayCast = 0;

// calculate the ray bounding box
  CalculateMinExtend2d(q0, q1, boxP0, boxP1);

  dgVector dq(q1 - q0);
  dgVector padding(
      dq.Scale(
          m_horizontalScale * dgFloat32(10.0f)
              / (dgSqrt (dq % dq) + dgFloat32(1.0e-6f))));

  // make sure the line segment crosses the original segment box
  dgVector p0(q0 - padding);
  dgVector p1(q1 + padding);

  // clip the line against the bounding box
  if (dgRayBoxClip(p0, p1, boxP0, boxP1))
  {
    dgVector dp(p1 - p0);
    dgVector normalOut(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
        dgFloat32(0.0f));

    dgFloat32 scale = m_horizontalScale;
    dgFloat32 invScale = m_horizontalScaleInv;
    dgInt32 ix0 = dgFastInt(p0.m_x * invScale);
    dgInt32 iz0 = dgFastInt(p0.m_z * invScale);

    // implement a 3ddda line algorithm 
    dgInt32 xInc;
    dgFloat32 tx;
    dgFloat32 stepX;
    if (dp.m_x > dgFloat32(0.0f))
    {
      xInc = 1;
      dgFloat32 val = dgFloat32(1.0f) / dp.m_x;
      stepX = scale * val;
      tx = (scale * (ix0 + dgFloat32(1.0f)) - p0.m_x) * val;
    }
    else if (dp.m_x < dgFloat32(0.0f))
    {
      xInc = -1;
      dgFloat32 val = -dgFloat32(1.0f) / dp.m_x;
      stepX = scale * val;
      tx = -(scale * ix0 - p0.m_x) * val;
    }
    else
    {
      xInc = 0;
      stepX = dgFloat32(0.0f);
      tx = dgFloat32(1.0e10f);
    }

    dgInt32 zInc;
    dgFloat32 stepZ;
    dgFloat32 tz;
    if (dp.m_z > dgFloat32(0.0f))
    {
      zInc = 1;
      dgFloat32 val = dgFloat32(1.0f) / dp.m_z;
      stepZ = scale * val;
      tz = (scale * (iz0 + dgFloat32(1.0f)) - p0.m_z) * val;
    }
    else if (dp.m_z < dgFloat32(0.0f))
    {
      zInc = -1;
      dgFloat32 val = -dgFloat32(1.0f) / dp.m_z;
      stepZ = scale * val;
      tz = -(scale * iz0 - p0.m_z) * val;
    }
    else
    {
      zInc = 0;
      stepZ = dgFloat32(0.0f);
      tz = dgFloat32(1.0e10f);
    }

    dgFloat32 txAcc = tx;
    dgFloat32 tzAcc = tz;
    dgInt32 xIndex0 = ix0;
    dgInt32 zIndex0 = iz0;
    FastRayTest ray(q0, q1);

    // for each cell touched by the line
    do
    {
      dgFloat32 t = RayCastCellSimd(ray, xIndex0, zIndex0, normalOut);
      if (t < dgFloat32(1.0f))
      {
        // bail out at the first intersection and copy the data into the descriptor
        contactOut.m_normal = normalOut.Scale(
            dgFloat32(1.0f) / dgSqrt (normalOut % normalOut));
        contactOut.m_userId = m_atributeMap[zIndex0 * m_width + xIndex0];

        if (m_userRayCastCallback)
        {
          dgVector normal(
              body->GetCollisionMatrix().RotateVector(contactOut.m_normal));
          m_userRayCastCallback(body, this, t, xIndex0, zIndex0, &normal,
              dgInt32(contactOut.m_userId), userData);
        }

        return t;
      }

      if (txAcc < tzAcc)
      {
        xIndex0 += xInc;
        tx = txAcc;
        txAcc += stepX;
      }
      else
      {
        zIndex0 += zInc;
        tz = txAcc;
        tzAcc += stepZ;
      }
    } while ((tx <= dgFloat32(1.0f)) || (tz <= dgFloat32(1.0f)));
  }

  // if no cell was hit, return a large value
  return dgFloat32(1.2f);
}

dgFloat32 dgCollisionHeightField::RayCast(const dgVector& q0,
    const dgVector& q1, dgContactPoint& contactOut,
    OnRayPrecastAction preFilter, const dgBody* const body,
    void* const userData) const
{
  dgVector boxP0;
  dgVector boxP1;

  // set the debug line counter to zero
//	debugRayCast = 0;

// calculate the ray bounding box
  CalculateMinExtend2d(q0, q1, boxP0, boxP1);

//	dgVector dq (q1 - q0);
//	dgVector padding (dq.Scale (m_horizontalScale * dgFloat32 (10.0f) / (dgSqrt (dq % dq) + dgFloat32 (1.0e-6f))));

// make sure the line segment crosses the original segment box
//	dgVector p0 (q0 - padding);
//	dgVector p1 (q1 + padding);

  dgVector p0(q0);
  dgVector p1(q1);

  // clip the line against the bounding box
  if (dgRayBoxClip(p0, p1, boxP0, boxP1))
  {
    dgVector dp(p1 - p0);
    dgVector normalOut(dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f),
        dgFloat32(0.0f));

    dgFloat32 scale = m_horizontalScale;
    dgFloat32 invScale = m_horizontalScaleInv;
    dgInt32 ix0 = dgFastInt(p0.m_x * invScale);
    dgInt32 iz0 = dgFastInt(p0.m_z * invScale);

    // implement a 3ddda line algorithm 
    dgInt32 xInc;
    dgFloat32 tx;
    dgFloat32 stepX;
    if (dp.m_x > dgFloat32(0.0f))
    {
      xInc = 1;
      dgFloat32 val = dgFloat32(1.0f) / dp.m_x;
      stepX = scale * val;
      tx = (scale * (ix0 + dgFloat32(1.0f)) - p0.m_x) * val;
    }
    else if (dp.m_x < dgFloat32(0.0f))
    {
      xInc = -1;
      dgFloat32 val = -dgFloat32(1.0f) / dp.m_x;
      stepX = scale * val;
      tx = -(scale * ix0 - p0.m_x) * val;
    }
    else
    {
      xInc = 0;
      stepX = dgFloat32(0.0f);
      tx = dgFloat32(1.0e10f);
    }

    dgInt32 zInc;
    dgFloat32 stepZ;
    dgFloat32 tz;
    if (dp.m_z > dgFloat32(0.0f))
    {
      zInc = 1;
      dgFloat32 val = dgFloat32(1.0f) / dp.m_z;
      stepZ = scale * val;
      tz = (scale * (iz0 + dgFloat32(1.0f)) - p0.m_z) * val;
    }
    else if (dp.m_z < dgFloat32(0.0f))
    {
      zInc = -1;
      dgFloat32 val = -dgFloat32(1.0f) / dp.m_z;
      stepZ = scale * val;
      tz = -(scale * iz0 - p0.m_z) * val;
    }
    else
    {
      zInc = 0;
      stepZ = dgFloat32(0.0f);
      tz = dgFloat32(1.0e10f);
    }

    dgFloat32 txAcc = tx;
    dgFloat32 tzAcc = tz;
    dgInt32 xIndex0 = ix0;
    dgInt32 zIndex0 = iz0;
    FastRayTest ray(q0, q1);

    // for each cell touched by the line
    do
    {
      dgFloat32 t = RayCastCell(ray, xIndex0, zIndex0, normalOut);
      if (t < dgFloat32(1.0f))
      {
        // bail out at the first intersection and copy the data into the descriptor
        contactOut.m_normal = normalOut.Scale(
            dgFloat32(1.0f) / dgSqrt (normalOut % normalOut));
        contactOut.m_userId = m_atributeMap[zIndex0 * m_width + xIndex0];

        if (m_userRayCastCallback)
        {
          dgVector normal(
              body->GetCollisionMatrix().RotateVector(contactOut.m_normal));
          m_userRayCastCallback(body, this, t, xIndex0, zIndex0, &normal,
              dgInt32(contactOut.m_userId), userData);
        }

        return t;
      }

      if (txAcc < tzAcc)
      {
        xIndex0 += xInc;
        tx = txAcc;
        txAcc += stepX;
      }
      else
      {
        zIndex0 += zInc;
        tz = txAcc;
        tzAcc += stepZ;
      }
    } while ((tx <= dgFloat32(1.0f)) || (tz <= dgFloat32(1.0f)));
  }

  // if no cell was hit, return a large value
  return dgFloat32(1.2f);
}

void dgCollisionHeightField::GetVertexListIndexList(const dgVector& p0,
    const dgVector& p1, dgGetVertexListIndexList &data) const
{
  _ASSERTE(0);
  data.m_vertexCount = 0;
}

struct dgCollisionHeightFieldShowPolyContext
{
  dgMatrix m_matrix;
  void* m_userData;
  OnDebugCollisionMeshCallback m_callback;
};

void dgCollisionHeightField::DebugCollision(const dgMatrix& matrix,
    OnDebugCollisionMeshCallback callback, void* const userData) const
{
  dgInt32 base;
  dgVector points[4];

  base = 0;
  for (dgInt32 z = 0; z < m_height - 1; z++)
  {
    points[0 * 2 + 0] = matrix.TransformVector(
        dgVector((0 + 0) * m_horizontalScale,
            dgFloat32(m_elevationMap[base + 0]) * m_verticalScale,
            (z + 0) * m_horizontalScale, dgFloat32(0.0f)));
    points[1 * 2 + 0] = matrix.TransformVector(
        dgVector((0 + 0) * m_horizontalScale,
            dgFloat32(m_elevationMap[base + 0 + m_width + 0]) * m_verticalScale,
            (z + 1) * m_horizontalScale, dgFloat32(0.0f)));

    for (dgInt32 x = 0; x < m_width - 1; x++)
    {
      dgTriplex triangle[3];
      points[0 * 2 + 1] = matrix.TransformVector(
          dgVector((x + 1) * m_horizontalScale,
              dgFloat32(m_elevationMap[base + x + 1]) * m_verticalScale,
              (z + 0) * m_horizontalScale, dgFloat32(0.0f)));
      points[1 * 2 + 1] = matrix.TransformVector(
          dgVector(
              (x + 1) * m_horizontalScale,
              dgFloat32(m_elevationMap[base + x + m_width + 1])
                  * m_verticalScale, (z + 1) * m_horizontalScale,
              dgFloat32(0.0f)));

      if (m_diagonalMode)
      {
        triangle[0].m_x = points[0].m_x;
        triangle[0].m_y = points[0].m_y;
        triangle[0].m_z = points[0].m_z;

        triangle[1].m_x = points[2].m_x;
        triangle[1].m_y = points[2].m_y;
        triangle[1].m_z = points[2].m_z;

        triangle[2].m_x = points[1].m_x;
        triangle[2].m_y = points[1].m_y;
        triangle[2].m_z = points[1].m_z;
        callback(userData, 3, &triangle[0].m_x, m_atributeMap[base]);

        triangle[0].m_x = points[1].m_x;
        triangle[0].m_y = points[1].m_y;
        triangle[0].m_z = points[1].m_z;

        triangle[1].m_x = points[2].m_x;
        triangle[1].m_y = points[2].m_y;
        triangle[1].m_z = points[2].m_z;

        triangle[2].m_x = points[3].m_x;
        triangle[2].m_y = points[3].m_y;
        triangle[2].m_z = points[3].m_z;
        callback(userData, 3, &triangle[0].m_x, m_atributeMap[base]);

      }
      else
      {
        triangle[0].m_x = points[0].m_x;
        triangle[0].m_y = points[0].m_y;
        triangle[0].m_z = points[0].m_z;

        triangle[1].m_x = points[2].m_x;
        triangle[1].m_y = points[2].m_y;
        triangle[1].m_z = points[2].m_z;

        triangle[2].m_x = points[3].m_x;
        triangle[2].m_y = points[3].m_y;
        triangle[2].m_z = points[3].m_z;
        callback(userData, 3, &triangle[0].m_x, m_atributeMap[base]);

        triangle[0].m_x = points[0].m_x;
        triangle[0].m_y = points[0].m_y;
        triangle[0].m_z = points[0].m_z;

        triangle[1].m_x = points[3].m_x;
        triangle[1].m_y = points[3].m_y;
        triangle[1].m_z = points[3].m_z;

        triangle[2].m_x = points[1].m_x;
        triangle[2].m_y = points[1].m_y;
        triangle[2].m_z = points[1].m_z;
        callback(userData, 3, &triangle[0].m_x, m_atributeMap[base]);
      }
      points[0 * 2 + 0] = points[0 * 2 + 1];
      points[1 * 2 + 0] = points[1 * 2 + 1];
    }
    base += m_width;
  }
}

void dgCollisionHeightField::GetLocalAABB(const dgVector& p0,
    const dgVector& p1, dgVector& boxP0, dgVector& boxP1) const
{
  dgInt32 x0;
  dgInt32 x1;
  dgInt32 z0;
  dgInt32 z1;
  dgInt32 base;
  dgInt32 minHeight;
  dgInt32 maxHeight;

  // the user data is the pointer to the collision geometry
  CalculateMinExtend3d(p0, p1, boxP0, boxP1);

  x0 = dgFastInt(boxP0.m_x * m_horizontalScaleInv);
  x1 = dgFastInt(boxP1.m_x * m_horizontalScaleInv);
  z0 = dgFastInt(boxP0.m_z * m_horizontalScaleInv);
  z1 = dgFastInt(boxP1.m_z * m_horizontalScaleInv);

  minHeight = 0x7fffffff;
  maxHeight = -0x7fffffff;
  base = z0 * m_width;
  for (dgInt32 z = z0; z <= z1; z++)
  {
    for (dgInt32 x = x0; x <= x1; x++)
    {
      dgInt32 high;
      high = m_elevationMap[base + x];
      if (high < minHeight)
      {
        minHeight = high;
      }
      if (high > maxHeight)
      {
        maxHeight = high;
      }
    }
    base += m_width;
  }

  boxP0.m_y = m_verticalScale * minHeight;
  boxP1.m_y = m_verticalScale * maxHeight;
}

void dgCollisionHeightField::GetCollidingFacesSimd(
    dgPolygonMeshDesc* const data) const
{
  GetCollidingFaces(data);
}

void dgCollisionHeightField::GetCollidingFaces(
    dgPolygonMeshDesc* const data) const
{
  dgInt32 x0;
  dgInt32 x1;
  dgInt32 z0;
  dgInt32 z1;
  dgInt32 base;
  dgInt32 vertexIndex;
  dgVector boxP0;
  dgVector boxP1;
  dgWorld* world;

  // the user data is the pointer to the collision geometry
  CalculateMinExtend3d(data->m_boxP0, data->m_boxP1, boxP0, boxP1);

  world = data->m_objBody->GetWorld();
  x0 = dgFastInt(boxP0.m_x * m_horizontalScaleInv);
  x1 = dgFastInt(boxP1.m_x * m_horizontalScaleInv);
  z0 = dgFastInt(boxP0.m_z * m_horizontalScaleInv);
  z1 = dgFastInt(boxP1.m_z * m_horizontalScaleInv);

  dgInt32 minHeight;
  dgInt32 maxHeight;

  minHeight = 0x7fffffff;
  maxHeight = -0x7fffffff;

  base = z0 * m_width;
  for (dgInt32 z = z0; z <= z1; z++)
  {
    for (dgInt32 x = x0; x <= x1; x++)
    {
      dgInt32 high;
      high = m_elevationMap[base + x];
      if (high < minHeight)
      {
        minHeight = high;
      }
      if (high > maxHeight)
      {
        maxHeight = high;
      }
    }
    base += m_width;
  }

  if (!(((m_verticalScale * maxHeight) < boxP0.m_y)
      || ((m_verticalScale * minHeight) > boxP1.m_y)))
  {
    dgInt32 step;
    dgInt32 index;
    dgInt32 faceCount;
    dgInt32 normalBase;
    dgInt32 normalIndex;

    // scan the vertices's intersected by the box extend
    base = (z1 - z0 + 1) * (x1 - x0 + 1) + 2 * (z1 - z0) * (x1 - x0);
    while (base > m_instanceData->m_vertexCount[data->m_threadNumber])
    {
      AllocateVertex(world, data->m_threadNumber);
    }

    vertexIndex = 0;
    base = z0 * m_width;
    dgVector* const vertex = m_instanceData->m_vertex[data->m_threadNumber];

    for (dgInt32 z = z0; z <= z1; z++)
    {
      for (dgInt32 x = x0; x <= x1; x++)
      {
        vertex[vertexIndex] = dgVector(m_horizontalScale * x,
            m_verticalScale * dgFloat32(m_elevationMap[base + x]),
            m_horizontalScale * z, dgFloat32(0.0f));
        vertexIndex++;
        _ASSERTE(
            vertexIndex <= m_instanceData->m_vertexCount[data->m_threadNumber]);
      }
      base += m_width;
    }

    normalBase = vertexIndex;
    index = 0;
    faceCount = 0;
    vertexIndex = 0;
    normalIndex = 0;
    step = x1 - x0 + 1;

    dgInt32* const indices = data->m_globalFaceVertexIndex;
    dgInt32* const attributes = data->m_globalUserAttribute;
    dgInt32* const faceIndexCount = data->m_globalFaceIndexCount;
    dgInt32* const normalIndexCount = data->m_globalFaceNormalIndex;
    dgInt32* const faceAdjencentEdgeNormal = data->m_globalAdjencentEdgeNormal;
    dgFloat32* const facefaceMaxSize = data->m_globalFaceMaxSize;
    dgFloat32 faceSize = GetMax(m_horizontalScale * dgFloat32(2.0f),
        dgFloat32(64.0f));

    if (!m_diagonalMode)
    {
      for (dgInt32 z = z0; z < z1; z++)
      {
        dgInt32 zStep;
        zStep = z * m_width;

        for (dgInt32 x = x0; x < x1; x++)
        {
          dgInt32 i0;
          dgInt32 i1;
          dgInt32 i2;
          dgInt32 i3;

          i0 = vertexIndex;
          i1 = vertexIndex + step;
          i2 = vertexIndex + 1;
          i3 = vertexIndex + step + 1;

          faceIndexCount[faceCount] = 3;
          facefaceMaxSize[faceCount] = faceSize;
          attributes[faceCount] = m_atributeMap[zStep + x];
          indices[index + 0] = i0;
          indices[index + 1] = i1;
          indices[index + 2] = i2;
          index += 3;
          faceCount++;

          faceIndexCount[faceCount] = 3;
          attributes[faceCount] = m_atributeMap[zStep + x];
          facefaceMaxSize[faceCount] = faceSize;
          indices[index + 0] = i2;
          indices[index + 1] = i1;
          indices[index + 2] = i3;
          index += 3;
          faceCount++;
          vertexIndex++;

          // calculate the the normal
          dgVector e0(vertex[i0] - vertex[i2]);
          dgVector e1(vertex[i1] - vertex[i2]);
          dgVector e2(vertex[i3] - vertex[i2]);
          dgVector n0(e0 * e1);
          dgVector n1(e1 * e2);

          //normalBase 
          vertex[normalBase] = n0.Scale(dgRsqrt(n0 % n0));
          normalIndexCount[normalIndex] = normalBase;
          normalIndex++;
          normalBase++;

          vertex[normalBase] = n1.Scale(dgRsqrt(n1 % n1));
          normalIndexCount[normalIndex] = normalBase;
          normalIndex++;
          normalBase++;
        }
        vertexIndex++;
      }

      base = 0;
      index = 0;
      step = x1 - x0;
      for (dgInt32 z = z0; z < z1; z++)
      {
        dgInt32 z0Flag;
        dgInt32 z1Flag;

        z0Flag = ((z - z0 - 1) >> 31);
        z1Flag = ((z1 - z - 2) >> 31);
        for (dgInt32 x = x0; x < x1; x++)
        {
          dgInt32 xA0;
          dgInt32 xA1;
          dgInt32 zA0;
          dgInt32 zA1;
          dgInt32 zxA;
          dgInt32 x0Flag;
          dgInt32 x1Flag;
          dgFloat32 side;
          dgFloat32 diagSide;

          x0Flag = ((x - x0 - 1) >> 31);
          x1Flag = ((x1 - x - 2) >> 31);

          const dgVector& point = vertex[indices[base + 1]];
          const dgVector& n = vertex[normalIndexCount[index * 2]];

          xA0 = ((~x0Flag) & (index * 2 - 1))
              | (x0Flag & ((index - x + x0) * 2));
          side = n % (vertex[indices[xA0 * 3 + 1]] - point);
          faceAdjencentEdgeNormal[base + 0] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[xA0] : -1;

          zxA = index * 2 + 1;
          diagSide = n % (vertex[indices[xA0 * 3 + 2]] - point);
          faceAdjencentEdgeNormal[base + 1] =
              (diagSide < dgFloat32(-1.0e-5f)) ? normalIndexCount[zxA] : -1;

          zA0 = ((~z0Flag) & ((index - step) * 2 + 1)) | (z0Flag & (index * 2));
          side = n % (vertex[indices[zA0 * 3]] - point);
          faceAdjencentEdgeNormal[base + 2] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[zA0] : -1;

          const dgVector& n1 = vertex[normalIndexCount[index * 2 + 1]];
          xA1 = ((~x1Flag) & (index * 2 + 2)) | (x1Flag & (index * 2 + 1));
          side = n1 % (vertex[indices[xA1 * 3 + 2]] - point);
          faceAdjencentEdgeNormal[base + 5] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[xA1] : -1;

          zxA = index * 2;
          faceAdjencentEdgeNormal[base + 3] =
              (diagSide < dgFloat32(-1.0e-5f)) ? normalIndexCount[zxA] : -1;
          ;

          zA1 = ((~z1Flag) & ((index + step) * 2)) | (z1Flag & (index * 2 + 1));
          side = n % (vertex[indices[zA1 * 3 + 1]] - point);
          faceAdjencentEdgeNormal[base + 4] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[zA1] : -1;
          index++;
          base += 6;
        }
      }
    }
    else
    {

      for (dgInt32 z = z0; z < z1; z++)
      {
        dgInt32 zStep;
        zStep = z * m_width;
        for (dgInt32 x = x0; x < x1; x++)
        {

          dgInt32 i0;
          dgInt32 i1;
          dgInt32 i2;
          dgInt32 i3;

          i0 = vertexIndex;
          i1 = vertexIndex + step + 1;
          i2 = vertexIndex + 1;
          i3 = vertexIndex + step;

          faceIndexCount[faceCount] = 3;
          facefaceMaxSize[faceCount] = faceSize;
          attributes[faceCount] = m_atributeMap[zStep + x];
          indices[index + 0] = i0;
          indices[index + 1] = i1;
          indices[index + 2] = i2;
          index += 3;
          faceCount++;

          faceIndexCount[faceCount] = 3;
          facefaceMaxSize[faceCount] = faceSize;
          attributes[faceCount] = m_atributeMap[zStep + x];
          indices[index + 0] = i0;
          indices[index + 1] = i3;
          indices[index + 2] = i1;
          index += 3;
          faceCount++;
          vertexIndex++;

          // calculate the the normal
          dgVector e0(vertex[i3] - vertex[i0]);
          dgVector e1(vertex[i1] - vertex[i0]);
          dgVector e2(vertex[i2] - vertex[i0]);
          dgVector n0(e0 * e1);
          dgVector n1(e1 * e2);

          vertex[normalBase] = n0.Scale(dgRsqrt(n0 % n0));
          normalIndexCount[normalIndex] = normalBase;
          normalIndex++;
          normalBase++;

          vertex[normalBase] = n1.Scale(dgRsqrt(n1 % n1));
          normalIndexCount[normalIndex] = normalBase;
          normalIndex++;
          normalBase++;
        }
        vertexIndex++;
      }

      base = 0;
      index = 0;
      step = x1 - x0;
      for (dgInt32 z = z0; z < z1; z++)
      {
        dgInt32 z0Flag;
        dgInt32 z1Flag;

        z0Flag = ((z - z0 - 1) >> 31);
        z1Flag = ((z1 - z - 2) >> 31);
        for (dgInt32 x = x0; x < x1; x++)
        {
          dgInt32 xA0;
          dgInt32 xA1;
          dgInt32 zA0;
          dgInt32 zA1;
          dgInt32 zxA;
          dgInt32 x0Flag;
          dgInt32 x1Flag;
          dgFloat32 side;
          dgFloat32 diagSide;

          x1Flag = ((x - x0 - 1) >> 31);
          x0Flag = ((x1 - x - 2) >> 31);

          const dgVector& point = vertex[indices[base]];
          const dgVector& n = vertex[normalIndexCount[index * 2]];
          xA0 = ((~x0Flag) & (index * 2 + 3)) | (x0Flag & (index * 2));
          side = n % (vertex[indices[xA0 * 3 + 2]] - point);
          faceAdjencentEdgeNormal[base + 1] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[xA0] : -1;

          zxA = index * 2 + 1;
          diagSide = n % (vertex[indices[xA0 * 3 + 1]] - point);
          faceAdjencentEdgeNormal[base + 0] =
              (diagSide < dgFloat32(-1.0e-5f)) ? normalIndexCount[zxA] : -1;

          zA0 = ((~z0Flag) & ((index - step) * 2 + 1)) | (z0Flag & (index * 2));
          side = n % (vertex[indices[zA0 * 3]] - point);
          faceAdjencentEdgeNormal[base + 2] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[zA0] : -1;

          const dgVector& n1 = vertex[normalIndexCount[index * 2 + 1]];
          xA1 = ((~x1Flag) & (index * 2 - 2))
              | (x1Flag & ((index - x + x0) * 2 + 1));
          side = n1 % (vertex[indices[xA1 * 3]] - point);
          faceAdjencentEdgeNormal[base + 3] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[xA1] : -1;

          zxA = index * 2;
          faceAdjencentEdgeNormal[base + 5] =
              (diagSide < dgFloat32(-1.0e-5f)) ? normalIndexCount[zxA] : -1;
          ;

          zA1 = ((~z1Flag) & ((index + step) * 2)) | (z1Flag & (index * 2 + 1));
          side = n % (vertex[indices[zA1 * 3 + 1]] - point);
          faceAdjencentEdgeNormal[base + 4] =
              (side < dgFloat32(-1.0e-5f)) ? normalIndexCount[zA1] : -1;

          index++;
          base += 6;
        }
      }
    }

    data->m_faceCount = faceCount;

    // initialize the callback data structure
    data->m_vertexStrideInBytes = sizeof(dgVector);
    data->m_faceVertexIndex = indices;
    data->m_faceNormalIndex = normalIndexCount;
    data->m_faceMaxSize = facefaceMaxSize;
    data->m_faceAdjencentEdgeNormal = faceAdjencentEdgeNormal;
    data->m_userAttribute = attributes;
    data->m_faceIndexCount = faceIndexCount;
    data->m_vertex = &vertex[0].m_x;

    if (GetDebugCollisionCallback())
    {
      dgTriplex triplex[3];
      const dgMatrix& matrix = data->m_polySoupBody->GetCollisionMatrix();
      for (dgInt32 i = 0; i < faceCount; i++)
      {
        for (dgInt32 j = 0; j < 3; j++)
        {
          dgVector p(matrix.TransformVector(vertex[indices[i * 3 + j]]));
          triplex[j].m_x = p.m_x;
          triplex[j].m_y = p.m_y;
          triplex[j].m_z = p.m_z;
        }
        GetDebugCollisionCallback()(data->m_polySoupBody, data->m_objBody,
            attributes[i], 3, &triplex[0].m_x, sizeof(dgTriplex));
      }
    }
  }
}

