1877 lines
62 KiB
C++
1877 lines
62 KiB
C++
|
/*************************************************************************
|
||
|
|
* *
|
||
|
|
* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
|
||
|
|
* All rights reserved. Email: russ@q12.org Web: www.q12.org *
|
||
|
|
* *
|
||
|
|
* This library is free software; you can redistribute it and/or *
|
||
|
|
* modify it under the terms of EITHER: *
|
||
|
|
* (1) The GNU Lesser General Public License as published by the Free *
|
||
|
|
* Software Foundation; either version 2.1 of the License, or (at *
|
||
|
|
* your option) any later version. The text of the GNU Lesser *
|
||
|
|
* General Public License is included with this library in the *
|
||
|
|
* file LICENSE.TXT. *
|
||
|
|
* (2) The BSD-style license that is included with this library in *
|
||
|
|
* the file LICENSE-BSD.TXT. *
|
||
|
|
* *
|
||
|
|
* This library is distributed in the hope that it will be useful, *
|
||
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
|
||
|
|
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
|
||
|
|
* *
|
||
|
|
*************************************************************************/
|
||
|
|
|
||
|
|
// dHeightfield Collider
|
||
|
|
// Paul Cheyrou-Lagreze aka Tuan Kuranes 2006 Speed enhancements http://www.pop-3d.com
|
||
|
|
// Martijn Buijs 2006 http://home.planet.nl/~buijs512/
|
||
|
|
// Based on Terrain & Cone contrib by:
|
||
|
|
// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com
|
||
|
|
// Some code inspired by Magic Software
|
||
|
|
|
||
|
|
|
||
|
|
#include <ode/common.h>
|
||
|
|
#include <ode/collision.h>
|
||
|
|
#include <ode/rotation.h>
|
||
|
|
#include "config.h"
|
||
|
|
#include "matrix.h"
|
||
|
|
#include "odemath.h"
|
||
|
|
#include "collision_kernel.h"
|
||
|
|
#include "collision_std.h"
|
||
|
|
#include "collision_util.h"
|
||
|
|
#include "heightfield.h"
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
#if dTRIMESH_ENABLED
|
||
|
|
#include "collision_trimesh_colliders.h"
|
||
|
|
#endif // dTRIMESH_ENABLED
|
||
|
|
|
||
|
|
#define dMIN(A,B) ((A)>(B) ? (B) : (A))
|
||
|
|
#define dMAX(A,B) ((A)>(B) ? (A) : (B))
|
||
|
|
|
||
|
|
|
||
|
|
// Three-way MIN and MAX
|
||
|
|
#define dMIN3(A,B,C) ( (A)<(B) ? dMIN((A),(C)) : dMIN((B),(C)) )
|
||
|
|
#define dMAX3(A,B,C) ( (A)>(B) ? dMAX((A),(C)) : dMAX((B),(C)) )
|
||
|
|
|
||
|
|
#define dOPESIGN(a, op1, op2,b) \
|
||
|
|
(a)[0] op1 op2 ((b)[0]); \
|
||
|
|
(a)[1] op1 op2 ((b)[1]); \
|
||
|
|
(a)[2] op1 op2 ((b)[2]);
|
||
|
|
|
||
|
|
#define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) { \
|
||
|
|
\
|
||
|
|
dVector3Copy (MyPoint, (myRay).final_posr->pos); \
|
||
|
|
(myRay).final_posr->R[2] = (MyVector)[0]; \
|
||
|
|
(myRay).final_posr->R[6] = (MyVector)[1]; \
|
||
|
|
(myRay).final_posr->R[10] = (MyVector)[2]; \
|
||
|
|
dGeomMoved (&myRay); \
|
||
|
|
}
|
||
|
|
|
||
|
|
#define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \
|
||
|
|
\
|
||
|
|
(MyPlane)->p[0] = (MyPlaneDef)[0]; \
|
||
|
|
(MyPlane)->p[1] = (MyPlaneDef)[1]; \
|
||
|
|
(MyPlane)->p[2] = (MyPlaneDef)[2]; \
|
||
|
|
(MyPlane)->p[3] = (MyPlaneDef)[3]; \
|
||
|
|
dGeomMoved (MyPlane); \
|
||
|
|
}
|
||
|
|
//////// Local Build Option ////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
// Uncomment this #define to use the (0,0) corner of the geom as the origin,
|
||
|
|
// rather than the center. This was the way the original heightfield worked,
|
||
|
|
// but as it does not match the way all other geometries work, so, for consistency, it
|
||
|
|
// was changed to work like this.
|
||
|
|
|
||
|
|
// #define DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
|
||
|
|
// Uncomment this #define to add heightfield triangles edge colliding
|
||
|
|
// Code is not guaranteed and I didn't find the need to add that as
|
||
|
|
// colliding planes triangles and edge triangles seems enough.
|
||
|
|
// #define _HEIGHTFIELDEDGECOLLIDING
|
||
|
|
|
||
|
|
|
||
|
|
//////// dxHeightfieldData /////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
// dxHeightfieldData constructor
|
||
|
|
dxHeightfieldData::dxHeightfieldData():
|
||
|
|
m_fWidth( 0 ),
|
||
|
|
m_fDepth( 0 ),
|
||
|
|
m_fSampleWidth( 0 ),
|
||
|
|
m_fSampleDepth( 0 ),
|
||
|
|
m_fSampleZXAspect( 0 ),
|
||
|
|
m_fInvSampleWidth( 0 ),
|
||
|
|
m_fInvSampleDepth( 0 ),
|
||
|
|
|
||
|
|
m_fHalfWidth( 0 ),
|
||
|
|
m_fHalfDepth( 0 ),
|
||
|
|
|
||
|
|
m_fMinHeight( 0 ),
|
||
|
|
m_fMaxHeight( 0 ),
|
||
|
|
m_fThickness( 0 ),
|
||
|
|
m_fScale( 0 ),
|
||
|
|
m_fOffset( 0 ),
|
||
|
|
|
||
|
|
m_nWidthSamples( 0 ),
|
||
|
|
m_nDepthSamples( 0 ),
|
||
|
|
m_bCopyHeightData( 0 ),
|
||
|
|
m_bWrapMode( 0 ),
|
||
|
|
m_nGetHeightMode( 0 ),
|
||
|
|
|
||
|
|
m_pHeightData( NULL ),
|
||
|
|
m_pUserData( NULL ),
|
||
|
|
|
||
|
|
m_pGetHeightCallback( NULL )
|
||
|
|
{
|
||
|
|
memset( m_contacts, 0, sizeof( m_contacts ) );
|
||
|
|
}
|
||
|
|
|
||
|
|
// build Heightfield data
|
||
|
|
void dxHeightfieldData::SetData( int nWidthSamples, int nDepthSamples,
|
||
|
|
dReal fWidth, dReal fDepth,
|
||
|
|
dReal fScale, dReal fOffset, dReal fThickness,
|
||
|
|
int bWrapMode )
|
||
|
|
{
|
||
|
|
dIASSERT( fWidth > REAL( 0.0 ) );
|
||
|
|
dIASSERT( fDepth > REAL( 0.0 ) );
|
||
|
|
dIASSERT( nWidthSamples > 0 );
|
||
|
|
dIASSERT( nDepthSamples > 0 );
|
||
|
|
|
||
|
|
// x,z bounds
|
||
|
|
m_fWidth = fWidth;
|
||
|
|
m_fDepth = fDepth;
|
||
|
|
|
||
|
|
// cache half x,z bounds
|
||
|
|
m_fHalfWidth = fWidth / REAL( 2.0 );
|
||
|
|
m_fHalfDepth = fDepth / REAL( 2.0 );
|
||
|
|
|
||
|
|
// scale and offset
|
||
|
|
m_fScale = fScale;
|
||
|
|
m_fOffset = fOffset;
|
||
|
|
|
||
|
|
// infinite min height bounds
|
||
|
|
m_fThickness = fThickness;
|
||
|
|
|
||
|
|
// number of vertices per side
|
||
|
|
m_nWidthSamples = nWidthSamples;
|
||
|
|
m_nDepthSamples = nDepthSamples;
|
||
|
|
|
||
|
|
m_fSampleWidth = m_fWidth / ( m_nWidthSamples - REAL( 1.0 ) );
|
||
|
|
m_fSampleDepth = m_fDepth / ( m_nDepthSamples - REAL( 1.0 ) );
|
||
|
|
|
||
|
|
m_fSampleZXAspect = m_fSampleDepth / m_fSampleWidth;
|
||
|
|
|
||
|
|
m_fInvSampleWidth = REAL( 1.0 ) / m_fSampleWidth;
|
||
|
|
m_fInvSampleDepth = REAL( 1.0 ) / m_fSampleDepth;
|
||
|
|
|
||
|
|
// finite or repeated terrain?
|
||
|
|
m_bWrapMode = bWrapMode;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// recomputes heights bounds
|
||
|
|
void dxHeightfieldData::ComputeHeightBounds()
|
||
|
|
{
|
||
|
|
int i;
|
||
|
|
dReal h;
|
||
|
|
unsigned char *data_byte;
|
||
|
|
short *data_short;
|
||
|
|
float *data_float;
|
||
|
|
double *data_double;
|
||
|
|
|
||
|
|
switch ( m_nGetHeightMode )
|
||
|
|
{
|
||
|
|
|
||
|
|
// callback
|
||
|
|
case 0:
|
||
|
|
// change nothing, keep using default or user specified bounds
|
||
|
|
return;
|
||
|
|
|
||
|
|
// byte
|
||
|
|
case 1:
|
||
|
|
data_byte = (unsigned char*)m_pHeightData;
|
||
|
|
m_fMinHeight = dInfinity;
|
||
|
|
m_fMaxHeight = -dInfinity;
|
||
|
|
|
||
|
|
for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
|
||
|
|
{
|
||
|
|
h = data_byte[i];
|
||
|
|
if (h < m_fMinHeight) m_fMinHeight = h;
|
||
|
|
if (h > m_fMaxHeight) m_fMaxHeight = h;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
// short
|
||
|
|
case 2:
|
||
|
|
data_short = (short*)m_pHeightData;
|
||
|
|
m_fMinHeight = dInfinity;
|
||
|
|
m_fMaxHeight = -dInfinity;
|
||
|
|
|
||
|
|
for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
|
||
|
|
{
|
||
|
|
h = data_short[i];
|
||
|
|
if (h < m_fMinHeight) m_fMinHeight = h;
|
||
|
|
if (h > m_fMaxHeight) m_fMaxHeight = h;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
// float
|
||
|
|
case 3:
|
||
|
|
data_float = (float*)m_pHeightData;
|
||
|
|
m_fMinHeight = dInfinity;
|
||
|
|
m_fMaxHeight = -dInfinity;
|
||
|
|
|
||
|
|
for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
|
||
|
|
{
|
||
|
|
h = data_float[i];
|
||
|
|
if (h < m_fMinHeight) m_fMinHeight = h;
|
||
|
|
if (h > m_fMaxHeight) m_fMaxHeight = h;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
// double
|
||
|
|
case 4:
|
||
|
|
data_double = (double*)m_pHeightData;
|
||
|
|
m_fMinHeight = dInfinity;
|
||
|
|
m_fMaxHeight = -dInfinity;
|
||
|
|
|
||
|
|
for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
|
||
|
|
{
|
||
|
|
h = static_cast< dReal >( data_double[i] );
|
||
|
|
if (h < m_fMinHeight) m_fMinHeight = h;
|
||
|
|
if (h > m_fMaxHeight) m_fMaxHeight = h;
|
||
|
|
}
|
||
|
|
|
||
|
|
break;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
// scale and offset
|
||
|
|
m_fMinHeight *= m_fScale;
|
||
|
|
m_fMaxHeight *= m_fScale;
|
||
|
|
m_fMinHeight += m_fOffset;
|
||
|
|
m_fMaxHeight += m_fOffset;
|
||
|
|
|
||
|
|
// add thickness
|
||
|
|
m_fMinHeight -= m_fThickness;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// returns whether point is over terrain Cell triangle?
|
||
|
|
bool dxHeightfieldData::IsOnHeightfield2 ( const HeightFieldVertex * const CellCorner,
|
||
|
|
const dReal * const pos, const bool isABC) const
|
||
|
|
{
|
||
|
|
// WARNING!!!
|
||
|
|
// This function must be written in the way to make sure that every point on
|
||
|
|
// XZ plane falls in one and only one triangle. Keep that in mind if you
|
||
|
|
// intend to change the code.
|
||
|
|
// Also remember about computational errors and possible mismatches in
|
||
|
|
// values if they are calculated differently in different places in the code.
|
||
|
|
// Currently both the implementation has been optimized and effects of
|
||
|
|
// computational errors have been eliminated.
|
||
|
|
|
||
|
|
dReal MaxX, MinX;
|
||
|
|
dReal MaxZ, MinZ;
|
||
|
|
|
||
|
|
if (isABC)
|
||
|
|
{
|
||
|
|
// point A
|
||
|
|
MinX = CellCorner->vertex[0];
|
||
|
|
if (pos[0] < MinX)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
MaxX = (CellCorner->coords[0] + 1) * m_fSampleWidth;
|
||
|
|
if (pos[0] >= MaxX)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
MinZ = CellCorner->vertex[2];
|
||
|
|
if (pos[2] < MinZ)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
MaxZ = (CellCorner->coords[1] + 1) * m_fSampleDepth;
|
||
|
|
if (pos[2] >= MaxZ)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
return (MaxZ - pos[2]) > (pos[0] - MinX) * m_fSampleZXAspect;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// point D
|
||
|
|
MaxX = CellCorner->vertex[0];
|
||
|
|
if (pos[0] >= MaxX)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
MinX = (CellCorner->coords[0] - 1) * m_fSampleWidth;
|
||
|
|
if (pos[0] < MinX)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
MaxZ = CellCorner->vertex[2];
|
||
|
|
if (pos[2] >= MaxZ)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
MinZ = (CellCorner->coords[1] - 1) * m_fSampleDepth;
|
||
|
|
if (pos[2] < MinZ)
|
||
|
|
return false;
|
||
|
|
|
||
|
|
return (MaxZ - pos[2]) <= (pos[0] - MinX) * m_fSampleZXAspect;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// returns height at given sample coordinates
|
||
|
|
dReal dxHeightfieldData::GetHeight( int x, int z )
|
||
|
|
{
|
||
|
|
dReal h=0;
|
||
|
|
unsigned char *data_byte;
|
||
|
|
short *data_short;
|
||
|
|
float *data_float;
|
||
|
|
double *data_double;
|
||
|
|
|
||
|
|
if ( m_bWrapMode == 0 )
|
||
|
|
{
|
||
|
|
// Finite
|
||
|
|
if ( x < 0 ) x = 0;
|
||
|
|
if ( z < 0 ) z = 0;
|
||
|
|
if ( x > m_nWidthSamples - 1 ) x = m_nWidthSamples - 1;
|
||
|
|
if ( z > m_nDepthSamples - 1 ) z = m_nDepthSamples - 1;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// Infinite
|
||
|
|
x %= m_nWidthSamples - 1;
|
||
|
|
z %= m_nDepthSamples - 1;
|
||
|
|
if ( x < 0 ) x += m_nWidthSamples - 1;
|
||
|
|
if ( z < 0 ) z += m_nDepthSamples - 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
switch ( m_nGetHeightMode )
|
||
|
|
{
|
||
|
|
|
||
|
|
// callback (dReal)
|
||
|
|
case 0:
|
||
|
|
h = (*m_pGetHeightCallback)(m_pUserData, x, z);
|
||
|
|
break;
|
||
|
|
|
||
|
|
// byte
|
||
|
|
case 1:
|
||
|
|
data_byte = (unsigned char*)m_pHeightData;
|
||
|
|
h = data_byte[x+(z * m_nWidthSamples)];
|
||
|
|
break;
|
||
|
|
|
||
|
|
// short
|
||
|
|
case 2:
|
||
|
|
data_short = (short*)m_pHeightData;
|
||
|
|
h = data_short[x+(z * m_nWidthSamples)];
|
||
|
|
break;
|
||
|
|
|
||
|
|
// float
|
||
|
|
case 3:
|
||
|
|
data_float = (float*)m_pHeightData;
|
||
|
|
h = data_float[x+(z * m_nWidthSamples)];
|
||
|
|
break;
|
||
|
|
|
||
|
|
// double
|
||
|
|
case 4:
|
||
|
|
data_double = (double*)m_pHeightData;
|
||
|
|
h = (dReal)( data_double[x+(z * m_nWidthSamples)] );
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
|
||
|
|
return (h * m_fScale) + m_fOffset;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// returns height at given coordinates
|
||
|
|
dReal dxHeightfieldData::GetHeight( dReal x, dReal z )
|
||
|
|
{
|
||
|
|
dReal dnX = dFloor( x * m_fInvSampleWidth );
|
||
|
|
dReal dnZ = dFloor( z * m_fInvSampleDepth );
|
||
|
|
|
||
|
|
dReal dx = ( x - ( dnX * m_fSampleWidth ) ) * m_fInvSampleWidth;
|
||
|
|
dReal dz = ( z - ( dnZ * m_fSampleDepth ) ) * m_fInvSampleDepth;
|
||
|
|
|
||
|
|
int nX = int( dnX );
|
||
|
|
int nZ = int( dnZ );
|
||
|
|
|
||
|
|
//dIASSERT( ( dx + dEpsilon >= 0.0f ) && ( dx - dEpsilon <= 1.0f ) );
|
||
|
|
//dIASSERT( ( dz + dEpsilon >= 0.0f ) && ( dz - dEpsilon <= 1.0f ) );
|
||
|
|
|
||
|
|
dReal y, y0;
|
||
|
|
|
||
|
|
if ( dx + dz <= REAL( 1.0 ) ) // Use <= comparison to prefer simpler branch
|
||
|
|
{
|
||
|
|
y0 = GetHeight( nX, nZ );
|
||
|
|
|
||
|
|
y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * dx
|
||
|
|
+ ( GetHeight( nX, nZ + 1 ) - y0 ) * dz;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
y0 = GetHeight( nX + 1, nZ + 1 );
|
||
|
|
|
||
|
|
y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * ( REAL(1.0) - dz ) +
|
||
|
|
( GetHeight( nX, nZ + 1 ) - y0 ) * ( REAL(1.0) - dx );
|
||
|
|
}
|
||
|
|
|
||
|
|
return y;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// dxHeightfieldData destructor
|
||
|
|
dxHeightfieldData::~dxHeightfieldData()
|
||
|
|
{
|
||
|
|
unsigned char *data_byte;
|
||
|
|
short *data_short;
|
||
|
|
float *data_float;
|
||
|
|
double *data_double;
|
||
|
|
|
||
|
|
if ( m_bCopyHeightData )
|
||
|
|
{
|
||
|
|
switch ( m_nGetHeightMode )
|
||
|
|
{
|
||
|
|
|
||
|
|
// callback
|
||
|
|
case 0:
|
||
|
|
// do nothing
|
||
|
|
break;
|
||
|
|
|
||
|
|
// byte
|
||
|
|
case 1:
|
||
|
|
dIASSERT( m_pHeightData );
|
||
|
|
data_byte = (unsigned char*)m_pHeightData;
|
||
|
|
delete [] data_byte;
|
||
|
|
break;
|
||
|
|
|
||
|
|
// short
|
||
|
|
case 2:
|
||
|
|
dIASSERT( m_pHeightData );
|
||
|
|
data_short = (short*)m_pHeightData;
|
||
|
|
delete [] data_short;
|
||
|
|
break;
|
||
|
|
|
||
|
|
// float
|
||
|
|
case 3:
|
||
|
|
dIASSERT( m_pHeightData );
|
||
|
|
data_float = (float*)m_pHeightData;
|
||
|
|
delete [] data_float;
|
||
|
|
break;
|
||
|
|
|
||
|
|
// double
|
||
|
|
case 4:
|
||
|
|
dIASSERT( m_pHeightData );
|
||
|
|
data_double = (double*)m_pHeightData;
|
||
|
|
delete [] data_double;
|
||
|
|
break;
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////// dxHeightfield /////////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
|
||
|
|
// dxHeightfield constructor
|
||
|
|
dxHeightfield::dxHeightfield( dSpaceID space,
|
||
|
|
dHeightfieldDataID data,
|
||
|
|
int bPlaceable ) :
|
||
|
|
dxGeom( space, bPlaceable ),
|
||
|
|
tempPlaneBuffer(0),
|
||
|
|
tempPlaneInstances(0),
|
||
|
|
tempPlaneBufferSize(0),
|
||
|
|
tempTriangleBuffer(0),
|
||
|
|
tempTriangleBufferSize(0),
|
||
|
|
tempHeightBuffer(0),
|
||
|
|
tempHeightInstances(0),
|
||
|
|
tempHeightBufferSizeX(0),
|
||
|
|
tempHeightBufferSizeZ(0)
|
||
|
|
{
|
||
|
|
type = dHeightfieldClass;
|
||
|
|
this->m_p_data = data;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// compute axis aligned bounding box
|
||
|
|
void dxHeightfield::computeAABB()
|
||
|
|
{
|
||
|
|
const dxHeightfieldData *d = m_p_data;
|
||
|
|
|
||
|
|
if ( d->m_bWrapMode == 0 )
|
||
|
|
{
|
||
|
|
// Finite
|
||
|
|
if ( gflags & GEOM_PLACEABLE )
|
||
|
|
{
|
||
|
|
dReal dx[6], dy[6], dz[6];
|
||
|
|
|
||
|
|
// Y-axis
|
||
|
|
if (d->m_fMinHeight != -dInfinity)
|
||
|
|
{
|
||
|
|
dy[0] = ( final_posr->R[ 1] * d->m_fMinHeight );
|
||
|
|
dy[1] = ( final_posr->R[ 5] * d->m_fMinHeight );
|
||
|
|
dy[2] = ( final_posr->R[ 9] * d->m_fMinHeight );
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// Multiplication is performed to obtain infinity of correct sign
|
||
|
|
dy[0] = ( final_posr->R[ 1] ? final_posr->R[ 1] * -dInfinity : REAL(0.0) );
|
||
|
|
dy[1] = ( final_posr->R[ 5] ? final_posr->R[ 5] * -dInfinity : REAL(0.0) );
|
||
|
|
dy[2] = ( final_posr->R[ 9] ? final_posr->R[ 9] * -dInfinity : REAL(0.0) );
|
||
|
|
}
|
||
|
|
|
||
|
|
if (d->m_fMaxHeight != dInfinity)
|
||
|
|
{
|
||
|
|
dy[3] = ( final_posr->R[ 1] * d->m_fMaxHeight );
|
||
|
|
dy[4] = ( final_posr->R[ 5] * d->m_fMaxHeight );
|
||
|
|
dy[5] = ( final_posr->R[ 9] * d->m_fMaxHeight );
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
dy[3] = ( final_posr->R[ 1] ? final_posr->R[ 1] * dInfinity : REAL(0.0) );
|
||
|
|
dy[4] = ( final_posr->R[ 5] ? final_posr->R[ 5] * dInfinity : REAL(0.0) );
|
||
|
|
dy[5] = ( final_posr->R[ 9] ? final_posr->R[ 9] * dInfinity : REAL(0.0) );
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
// X-axis
|
||
|
|
dx[0] = 0; dx[3] = ( final_posr->R[ 0] * d->m_fWidth );
|
||
|
|
dx[1] = 0; dx[4] = ( final_posr->R[ 4] * d->m_fWidth );
|
||
|
|
dx[2] = 0; dx[5] = ( final_posr->R[ 8] * d->m_fWidth );
|
||
|
|
|
||
|
|
// Z-axis
|
||
|
|
dz[0] = 0; dz[3] = ( final_posr->R[ 2] * d->m_fDepth );
|
||
|
|
dz[1] = 0; dz[4] = ( final_posr->R[ 6] * d->m_fDepth );
|
||
|
|
dz[2] = 0; dz[5] = ( final_posr->R[10] * d->m_fDepth );
|
||
|
|
|
||
|
|
#else // DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
// X-axis
|
||
|
|
dx[0] = ( final_posr->R[ 0] * -d->m_fHalfWidth );
|
||
|
|
dx[1] = ( final_posr->R[ 4] * -d->m_fHalfWidth );
|
||
|
|
dx[2] = ( final_posr->R[ 8] * -d->m_fHalfWidth );
|
||
|
|
dx[3] = ( final_posr->R[ 0] * d->m_fHalfWidth );
|
||
|
|
dx[4] = ( final_posr->R[ 4] * d->m_fHalfWidth );
|
||
|
|
dx[5] = ( final_posr->R[ 8] * d->m_fHalfWidth );
|
||
|
|
|
||
|
|
// Z-axis
|
||
|
|
dz[0] = ( final_posr->R[ 2] * -d->m_fHalfDepth );
|
||
|
|
dz[1] = ( final_posr->R[ 6] * -d->m_fHalfDepth );
|
||
|
|
dz[2] = ( final_posr->R[10] * -d->m_fHalfDepth );
|
||
|
|
dz[3] = ( final_posr->R[ 2] * d->m_fHalfDepth );
|
||
|
|
dz[4] = ( final_posr->R[ 6] * d->m_fHalfDepth );
|
||
|
|
dz[5] = ( final_posr->R[10] * d->m_fHalfDepth );
|
||
|
|
|
||
|
|
#endif // DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
// X extents
|
||
|
|
aabb[0] = final_posr->pos[0] +
|
||
|
|
dMIN3( dMIN( dx[0], dx[3] ), dMIN( dy[0], dy[3] ), dMIN( dz[0], dz[3] ) );
|
||
|
|
aabb[1] = final_posr->pos[0] +
|
||
|
|
dMAX3( dMAX( dx[0], dx[3] ), dMAX( dy[0], dy[3] ), dMAX( dz[0], dz[3] ) );
|
||
|
|
|
||
|
|
// Y extents
|
||
|
|
aabb[2] = final_posr->pos[1] +
|
||
|
|
dMIN3( dMIN( dx[1], dx[4] ), dMIN( dy[1], dy[4] ), dMIN( dz[1], dz[4] ) );
|
||
|
|
aabb[3] = final_posr->pos[1] +
|
||
|
|
dMAX3( dMAX( dx[1], dx[4] ), dMAX( dy[1], dy[4] ), dMAX( dz[1], dz[4] ) );
|
||
|
|
|
||
|
|
// Z extents
|
||
|
|
aabb[4] = final_posr->pos[2] +
|
||
|
|
dMIN3( dMIN( dx[2], dx[5] ), dMIN( dy[2], dy[5] ), dMIN( dz[2], dz[5] ) );
|
||
|
|
aabb[5] = final_posr->pos[2] +
|
||
|
|
dMAX3( dMAX( dx[2], dx[5] ), dMAX( dy[2], dy[5] ), dMAX( dz[2], dz[5] ) );
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
|
||
|
|
#ifdef DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
aabb[0] = 0; aabb[1] = d->m_fWidth;
|
||
|
|
aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
|
||
|
|
aabb[4] = 0; aabb[5] = d->m_fDepth;
|
||
|
|
|
||
|
|
#else // DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
aabb[0] = -d->m_fHalfWidth; aabb[1] = +d->m_fHalfWidth;
|
||
|
|
aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
|
||
|
|
aabb[4] = -d->m_fHalfDepth; aabb[5] = +d->m_fHalfDepth;
|
||
|
|
|
||
|
|
#endif // DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// Infinite
|
||
|
|
if ( gflags & GEOM_PLACEABLE )
|
||
|
|
{
|
||
|
|
aabb[0] = -dInfinity; aabb[1] = +dInfinity;
|
||
|
|
aabb[2] = -dInfinity; aabb[3] = +dInfinity;
|
||
|
|
aabb[4] = -dInfinity; aabb[5] = +dInfinity;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
aabb[0] = -dInfinity; aabb[1] = +dInfinity;
|
||
|
|
aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
|
||
|
|
aabb[4] = -dInfinity; aabb[5] = +dInfinity;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// dxHeightfield destructor
|
||
|
|
dxHeightfield::~dxHeightfield()
|
||
|
|
{
|
||
|
|
resetTriangleBuffer();
|
||
|
|
resetPlaneBuffer();
|
||
|
|
resetHeightBuffer();
|
||
|
|
}
|
||
|
|
|
||
|
|
void dxHeightfield::allocateTriangleBuffer(sizeint numTri)
|
||
|
|
{
|
||
|
|
sizeint alignedNumTri = AlignBufferSize(numTri, TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT);
|
||
|
|
tempTriangleBufferSize = alignedNumTri;
|
||
|
|
tempTriangleBuffer = new HeightFieldTriangle[alignedNumTri];
|
||
|
|
}
|
||
|
|
|
||
|
|
void dxHeightfield::resetTriangleBuffer()
|
||
|
|
{
|
||
|
|
delete[] tempTriangleBuffer;
|
||
|
|
}
|
||
|
|
|
||
|
|
void dxHeightfield::allocatePlaneBuffer(sizeint numTri)
|
||
|
|
{
|
||
|
|
sizeint alignedNumTri = AlignBufferSize(numTri, TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT);
|
||
|
|
tempPlaneBufferSize = alignedNumTri;
|
||
|
|
tempPlaneBuffer = new HeightFieldPlane *[alignedNumTri];
|
||
|
|
tempPlaneInstances = new HeightFieldPlane[alignedNumTri];
|
||
|
|
|
||
|
|
HeightFieldPlane *ptrPlaneMatrix = tempPlaneInstances;
|
||
|
|
for (sizeint indexTri = 0; indexTri != alignedNumTri; indexTri++)
|
||
|
|
{
|
||
|
|
tempPlaneBuffer[indexTri] = ptrPlaneMatrix;
|
||
|
|
ptrPlaneMatrix += 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void dxHeightfield::resetPlaneBuffer()
|
||
|
|
{
|
||
|
|
delete[] tempPlaneInstances;
|
||
|
|
delete[] tempPlaneBuffer;
|
||
|
|
}
|
||
|
|
|
||
|
|
void dxHeightfield::allocateHeightBuffer(sizeint numX, sizeint numZ)
|
||
|
|
{
|
||
|
|
sizeint alignedNumX = AlignBufferSize(numX, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X);
|
||
|
|
sizeint alignedNumZ = AlignBufferSize(numZ, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z);
|
||
|
|
tempHeightBufferSizeX = alignedNumX;
|
||
|
|
tempHeightBufferSizeZ = alignedNumZ;
|
||
|
|
tempHeightBuffer = new HeightFieldVertex *[alignedNumX];
|
||
|
|
sizeint numCells = alignedNumX * alignedNumZ;
|
||
|
|
tempHeightInstances = new HeightFieldVertex [numCells];
|
||
|
|
|
||
|
|
HeightFieldVertex *ptrHeightMatrix = tempHeightInstances;
|
||
|
|
for (sizeint indexX = 0; indexX != alignedNumX; indexX++)
|
||
|
|
{
|
||
|
|
tempHeightBuffer[indexX] = ptrHeightMatrix;
|
||
|
|
ptrHeightMatrix += alignedNumZ;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void dxHeightfield::resetHeightBuffer()
|
||
|
|
{
|
||
|
|
delete[] tempHeightInstances;
|
||
|
|
delete[] tempHeightBuffer;
|
||
|
|
}
|
||
|
|
//////// Heightfield data interface ////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
|
||
|
|
dHeightfieldDataID dGeomHeightfieldDataCreate()
|
||
|
|
{
|
||
|
|
return new dxHeightfieldData();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d,
|
||
|
|
void* pUserData, dHeightfieldGetHeight* pCallback,
|
||
|
|
dReal width, dReal depth, int widthSamples, int depthSamples,
|
||
|
|
dReal scale, dReal offset, dReal thickness, int bWrap )
|
||
|
|
{
|
||
|
|
dUASSERT( d, "argument not Heightfield data" );
|
||
|
|
dIASSERT( pCallback );
|
||
|
|
dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
|
||
|
|
dIASSERT( depthSamples >= 2 );
|
||
|
|
|
||
|
|
// callback
|
||
|
|
d->m_nGetHeightMode = 0;
|
||
|
|
d->m_pUserData = pUserData;
|
||
|
|
d->m_pGetHeightCallback = pCallback;
|
||
|
|
|
||
|
|
// set info
|
||
|
|
d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
|
||
|
|
|
||
|
|
// default bounds
|
||
|
|
d->m_fMinHeight = -dInfinity;
|
||
|
|
d->m_fMaxHeight = dInfinity;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d,
|
||
|
|
const unsigned char *pHeightData, int bCopyHeightData,
|
||
|
|
dReal width, dReal depth, int widthSamples, int depthSamples,
|
||
|
|
dReal scale, dReal offset, dReal thickness, int bWrap )
|
||
|
|
{
|
||
|
|
dUASSERT( d, "Argument not Heightfield data" );
|
||
|
|
dIASSERT( pHeightData );
|
||
|
|
dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
|
||
|
|
dIASSERT( depthSamples >= 2 );
|
||
|
|
|
||
|
|
// set info
|
||
|
|
d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
|
||
|
|
d->m_nGetHeightMode = 1;
|
||
|
|
d->m_bCopyHeightData = bCopyHeightData;
|
||
|
|
|
||
|
|
if ( d->m_bCopyHeightData == 0 )
|
||
|
|
{
|
||
|
|
// Data is referenced only.
|
||
|
|
d->m_pHeightData = pHeightData;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// We own the height data, allocate storage
|
||
|
|
d->m_pHeightData = new unsigned char[ d->m_nWidthSamples * d->m_nDepthSamples ];
|
||
|
|
dIASSERT( d->m_pHeightData );
|
||
|
|
|
||
|
|
// Copy data.
|
||
|
|
memcpy( (void*)d->m_pHeightData, pHeightData,
|
||
|
|
sizeof( unsigned char ) * d->m_nWidthSamples * d->m_nDepthSamples );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Find height bounds
|
||
|
|
d->ComputeHeightBounds();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d,
|
||
|
|
const short* pHeightData, int bCopyHeightData,
|
||
|
|
dReal width, dReal depth, int widthSamples, int depthSamples,
|
||
|
|
dReal scale, dReal offset, dReal thickness, int bWrap )
|
||
|
|
{
|
||
|
|
dUASSERT( d, "Argument not Heightfield data" );
|
||
|
|
dIASSERT( pHeightData );
|
||
|
|
dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
|
||
|
|
dIASSERT( depthSamples >= 2 );
|
||
|
|
|
||
|
|
// set info
|
||
|
|
d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
|
||
|
|
d->m_nGetHeightMode = 2;
|
||
|
|
d->m_bCopyHeightData = bCopyHeightData;
|
||
|
|
|
||
|
|
if ( d->m_bCopyHeightData == 0 )
|
||
|
|
{
|
||
|
|
// Data is referenced only.
|
||
|
|
d->m_pHeightData = pHeightData;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// We own the height data, allocate storage
|
||
|
|
d->m_pHeightData = new short[ d->m_nWidthSamples * d->m_nDepthSamples ];
|
||
|
|
dIASSERT( d->m_pHeightData );
|
||
|
|
|
||
|
|
// Copy data.
|
||
|
|
memcpy( (void*)d->m_pHeightData, pHeightData,
|
||
|
|
sizeof( short ) * d->m_nWidthSamples * d->m_nDepthSamples );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Find height bounds
|
||
|
|
d->ComputeHeightBounds();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d,
|
||
|
|
const float *pHeightData, int bCopyHeightData,
|
||
|
|
dReal width, dReal depth, int widthSamples, int depthSamples,
|
||
|
|
dReal scale, dReal offset, dReal thickness, int bWrap )
|
||
|
|
{
|
||
|
|
dUASSERT( d, "Argument not Heightfield data" );
|
||
|
|
dIASSERT( pHeightData );
|
||
|
|
dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
|
||
|
|
dIASSERT( depthSamples >= 2 );
|
||
|
|
|
||
|
|
// set info
|
||
|
|
d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
|
||
|
|
d->m_nGetHeightMode = 3;
|
||
|
|
d->m_bCopyHeightData = bCopyHeightData;
|
||
|
|
|
||
|
|
if ( d->m_bCopyHeightData == 0 )
|
||
|
|
{
|
||
|
|
// Data is referenced only.
|
||
|
|
d->m_pHeightData = pHeightData;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// We own the height data, allocate storage
|
||
|
|
d->m_pHeightData = new float[ d->m_nWidthSamples * d->m_nDepthSamples ];
|
||
|
|
dIASSERT( d->m_pHeightData );
|
||
|
|
|
||
|
|
// Copy data.
|
||
|
|
memcpy( (void*)d->m_pHeightData, pHeightData,
|
||
|
|
sizeof( float ) * d->m_nWidthSamples * d->m_nDepthSamples );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Find height bounds
|
||
|
|
d->ComputeHeightBounds();
|
||
|
|
}
|
||
|
|
|
||
|
|
void dGeomHeightfieldDataBuildDouble( dHeightfieldDataID d,
|
||
|
|
const double *pHeightData, int bCopyHeightData,
|
||
|
|
dReal width, dReal depth, int widthSamples, int depthSamples,
|
||
|
|
dReal scale, dReal offset, dReal thickness, int bWrap )
|
||
|
|
{
|
||
|
|
dUASSERT( d, "Argument not Heightfield data" );
|
||
|
|
dIASSERT( pHeightData );
|
||
|
|
dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
|
||
|
|
dIASSERT( depthSamples >= 2 );
|
||
|
|
|
||
|
|
// set info
|
||
|
|
d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
|
||
|
|
d->m_nGetHeightMode = 4;
|
||
|
|
d->m_bCopyHeightData = bCopyHeightData;
|
||
|
|
|
||
|
|
if ( d->m_bCopyHeightData == 0 )
|
||
|
|
{
|
||
|
|
// Data is referenced only.
|
||
|
|
d->m_pHeightData = pHeightData;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// We own the height data, allocate storage
|
||
|
|
d->m_pHeightData = new double[ d->m_nWidthSamples * d->m_nDepthSamples ];
|
||
|
|
dIASSERT( d->m_pHeightData );
|
||
|
|
|
||
|
|
// Copy data.
|
||
|
|
memcpy( (void*)d->m_pHeightData, pHeightData,
|
||
|
|
sizeof( double ) * d->m_nWidthSamples * d->m_nDepthSamples );
|
||
|
|
}
|
||
|
|
|
||
|
|
// Find height bounds
|
||
|
|
d->ComputeHeightBounds();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight )
|
||
|
|
{
|
||
|
|
dUASSERT(d, "Argument not Heightfield data");
|
||
|
|
d->m_fMinHeight = ( minHeight * d->m_fScale ) + d->m_fOffset - d->m_fThickness;
|
||
|
|
d->m_fMaxHeight = ( maxHeight * d->m_fScale ) + d->m_fOffset;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void dGeomHeightfieldDataDestroy( dHeightfieldDataID d )
|
||
|
|
{
|
||
|
|
dUASSERT(d, "argument not Heightfield data");
|
||
|
|
delete d;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////// Heightfield geom interface ////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
|
||
|
|
dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable )
|
||
|
|
{
|
||
|
|
return new dxHeightfield( space, data, bPlaceable );
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d )
|
||
|
|
{
|
||
|
|
dxHeightfield* geom = (dxHeightfield*) g;
|
||
|
|
geom->m_p_data = d;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g )
|
||
|
|
{
|
||
|
|
dxHeightfield* geom = (dxHeightfield*) g;
|
||
|
|
return geom->m_p_data;
|
||
|
|
}
|
||
|
|
|
||
|
|
//////// dxHeightfield /////////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
|
||
|
|
// Typedef for generic 'get point depth' function
|
||
|
|
typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z );
|
||
|
|
|
||
|
|
|
||
|
|
#define DMESS(A) \
|
||
|
|
dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
|
||
|
|
x,z,(A), \
|
||
|
|
pContact->depth, \
|
||
|
|
dGeomSphereGetRadius(o2), \
|
||
|
|
pContact->pos[0], \
|
||
|
|
pContact->pos[1], \
|
||
|
|
pContact->pos[2], \
|
||
|
|
pContact->normal[0], \
|
||
|
|
pContact->normal[1], \
|
||
|
|
pContact->normal[2]);
|
||
|
|
|
||
|
|
static inline bool DescendingTriangleSort(const HeightFieldTriangle * const A, const HeightFieldTriangle * const B)
|
||
|
|
{
|
||
|
|
return ((A->maxAAAB - B->maxAAAB) > dEpsilon);
|
||
|
|
}
|
||
|
|
static inline bool DescendingPlaneSort(const HeightFieldPlane * const A, const HeightFieldPlane * const B)
|
||
|
|
{
|
||
|
|
return ((A->maxAAAB - B->maxAAAB) > dEpsilon);
|
||
|
|
}
|
||
|
|
|
||
|
|
void dxHeightfield::sortPlanes(const sizeint numPlanes)
|
||
|
|
{
|
||
|
|
bool has_swapped = true;
|
||
|
|
do
|
||
|
|
{
|
||
|
|
has_swapped = false;//reset flag
|
||
|
|
for (sizeint i = 0; i < numPlanes - 1; i++)
|
||
|
|
{
|
||
|
|
//if they are in the wrong order
|
||
|
|
if (DescendingPlaneSort(tempPlaneBuffer[i], tempPlaneBuffer[i + 1]))
|
||
|
|
{
|
||
|
|
//exchange them
|
||
|
|
HeightFieldPlane * tempPlane = tempPlaneBuffer[i];
|
||
|
|
tempPlaneBuffer[i] = tempPlaneBuffer[i + 1];
|
||
|
|
tempPlaneBuffer[i + 1] = tempPlane;
|
||
|
|
|
||
|
|
//we have swapped at least once, list may not be sorted yet
|
||
|
|
has_swapped = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
} //if no swaps were made during this pass, the list has been sorted
|
||
|
|
while (has_swapped);
|
||
|
|
}
|
||
|
|
|
||
|
|
static inline dReal DistancePointToLine(const dVector3 &_point,
|
||
|
|
const dVector3 &_pt0,
|
||
|
|
const dVector3 &_Edge,
|
||
|
|
const dReal _Edgelength)
|
||
|
|
{
|
||
|
|
dVector3 v;
|
||
|
|
dVector3Subtract(_point, _pt0, v);
|
||
|
|
dVector3 s;
|
||
|
|
dVector3Copy (_Edge, s);
|
||
|
|
const dReal dot = dVector3Dot(v, _Edge) / _Edgelength;
|
||
|
|
dVector3Scale(s, dot);
|
||
|
|
dVector3Subtract(v, s, v);
|
||
|
|
return dVector3Length(v);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
|
||
|
|
dxGeom* o2, const int numMaxContactsPossible,
|
||
|
|
int flags, dContactGeom* contact,
|
||
|
|
int skip )
|
||
|
|
{
|
||
|
|
dContactGeom *pContact = 0;
|
||
|
|
int x, z;
|
||
|
|
// check if not above or inside terrain first
|
||
|
|
// while filling a heightmap partial temporary buffer
|
||
|
|
const unsigned int numX = (maxX - minX) + 1;
|
||
|
|
const unsigned int numZ = (maxZ - minZ) + 1;
|
||
|
|
const dReal minO2Height = o2->aabb[2];
|
||
|
|
const dReal maxO2Height = o2->aabb[3];
|
||
|
|
unsigned int x_local, z_local;
|
||
|
|
dReal maxY = - dInfinity;
|
||
|
|
dReal minY = dInfinity;
|
||
|
|
// localize and const for faster access
|
||
|
|
const dReal cfSampleWidth = m_p_data->m_fSampleWidth;
|
||
|
|
const dReal cfSampleDepth = m_p_data->m_fSampleDepth;
|
||
|
|
{
|
||
|
|
if (tempHeightBufferSizeX < numX || tempHeightBufferSizeZ < numZ)
|
||
|
|
{
|
||
|
|
resetHeightBuffer();
|
||
|
|
allocateHeightBuffer(numX, numZ);
|
||
|
|
}
|
||
|
|
|
||
|
|
dReal Xpos, Ypos;
|
||
|
|
|
||
|
|
for ( x = minX, x_local = 0; x_local < numX; x++, x_local++)
|
||
|
|
{
|
||
|
|
Xpos = x * cfSampleWidth; // Always calculate pos via multiplication to avoid computational error accumulation during multiple additions
|
||
|
|
|
||
|
|
const dReal c_Xpos = Xpos;
|
||
|
|
HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
|
||
|
|
for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++)
|
||
|
|
{
|
||
|
|
Ypos = z * cfSampleDepth; // Always calculate pos via multiplication to avoid computational error accumulation during multiple additions
|
||
|
|
|
||
|
|
const dReal h = m_p_data->GetHeight(x, z);
|
||
|
|
HeightFieldRow[z_local].vertex[0] = c_Xpos;
|
||
|
|
HeightFieldRow[z_local].vertex[1] = h;
|
||
|
|
HeightFieldRow[z_local].vertex[2] = Ypos;
|
||
|
|
HeightFieldRow[z_local].coords[0] = x;
|
||
|
|
HeightFieldRow[z_local].coords[1] = z;
|
||
|
|
|
||
|
|
maxY = dMAX(maxY, h);
|
||
|
|
minY = dMIN(minY, h);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (minO2Height - maxY > -dEpsilon )
|
||
|
|
{
|
||
|
|
//totally above heightfield
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
if (minY - maxO2Height > -dEpsilon )
|
||
|
|
{
|
||
|
|
// totally under heightfield
|
||
|
|
pContact = CONTACT(contact, 0);
|
||
|
|
|
||
|
|
pContact->pos[0] = o2->final_posr->pos[0];
|
||
|
|
pContact->pos[1] = minY;
|
||
|
|
pContact->pos[2] = o2->final_posr->pos[2];
|
||
|
|
|
||
|
|
pContact->normal[0] = 0;
|
||
|
|
pContact->normal[1] = - 1;
|
||
|
|
pContact->normal[2] = 0;
|
||
|
|
|
||
|
|
pContact->depth = minY - maxO2Height;
|
||
|
|
|
||
|
|
pContact->side1 = -1;
|
||
|
|
pContact->side2 = -1;
|
||
|
|
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
// get All Planes that could collide against.
|
||
|
|
dColliderFn *geomRayNCollider=0;
|
||
|
|
dColliderFn *geomNPlaneCollider=0;
|
||
|
|
dGetDepthFn *geomNDepthGetter=0;
|
||
|
|
|
||
|
|
// int max_collisionContact = numMaxContactsPossible; -- not used
|
||
|
|
switch (o2->type)
|
||
|
|
{
|
||
|
|
case dRayClass:
|
||
|
|
geomRayNCollider = NULL;
|
||
|
|
geomNPlaneCollider = dCollideRayPlane;
|
||
|
|
geomNDepthGetter = NULL;
|
||
|
|
//max_collisionContact = 1;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case dSphereClass:
|
||
|
|
geomRayNCollider = dCollideRaySphere;
|
||
|
|
geomNPlaneCollider = dCollideSpherePlane;
|
||
|
|
geomNDepthGetter = dGeomSpherePointDepth;
|
||
|
|
//max_collisionContact = 3;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case dBoxClass:
|
||
|
|
geomRayNCollider = dCollideRayBox;
|
||
|
|
geomNPlaneCollider = dCollideBoxPlane;
|
||
|
|
geomNDepthGetter = dGeomBoxPointDepth;
|
||
|
|
//max_collisionContact = 8;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case dCapsuleClass:
|
||
|
|
geomRayNCollider = dCollideRayCapsule;
|
||
|
|
geomNPlaneCollider = dCollideCapsulePlane;
|
||
|
|
geomNDepthGetter = dGeomCapsulePointDepth;
|
||
|
|
// max_collisionContact = 3;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case dCylinderClass:
|
||
|
|
geomRayNCollider = dCollideRayCylinder;
|
||
|
|
geomNPlaneCollider = dCollideCylinderPlane;
|
||
|
|
geomNDepthGetter = NULL;// TODO: dGeomCCylinderPointDepth
|
||
|
|
//max_collisionContact = 3;
|
||
|
|
break;
|
||
|
|
|
||
|
|
case dConvexClass:
|
||
|
|
geomRayNCollider = dCollideRayConvex;
|
||
|
|
geomNPlaneCollider = dCollideConvexPlane;
|
||
|
|
geomNDepthGetter = NULL;// TODO: dGeomConvexPointDepth;
|
||
|
|
//max_collisionContact = 3;
|
||
|
|
break;
|
||
|
|
|
||
|
|
#if dTRIMESH_ENABLED
|
||
|
|
|
||
|
|
case dTriMeshClass:
|
||
|
|
geomRayNCollider = dCollideRayTrimesh;
|
||
|
|
geomNPlaneCollider = dCollideTrimeshPlane;
|
||
|
|
geomNDepthGetter = NULL;// TODO: dGeomTrimeshPointDepth;
|
||
|
|
//max_collisionContact = 3;
|
||
|
|
break;
|
||
|
|
|
||
|
|
#endif // dTRIMESH_ENABLED
|
||
|
|
|
||
|
|
default:
|
||
|
|
dIASSERT(0); // Shouldn't ever get here.
|
||
|
|
break;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
dxPlane myplane(0,0,0,0,0);
|
||
|
|
dxPlane* sliding_plane = &myplane;
|
||
|
|
dReal triplane[4];
|
||
|
|
int i;
|
||
|
|
|
||
|
|
// check some trivial case.
|
||
|
|
// Vector Up plane
|
||
|
|
if (maxY - minY < dEpsilon)
|
||
|
|
{
|
||
|
|
// it's a single plane.
|
||
|
|
triplane[0] = 0;
|
||
|
|
triplane[1] = 1;
|
||
|
|
triplane[2] = 0;
|
||
|
|
triplane[3] = minY;
|
||
|
|
dGeomPlaneSetNoNormalize (sliding_plane, triplane);
|
||
|
|
// find collision and compute contact points
|
||
|
|
const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip);
|
||
|
|
dIASSERT(numTerrainContacts <= numMaxContactsPossible);
|
||
|
|
for (i = 0; i < numTerrainContacts; i++)
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact, i*skip);
|
||
|
|
dOPESIGN(pContact->normal, =, -, triplane);
|
||
|
|
}
|
||
|
|
return numTerrainContacts;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* -- This block is invalid as per Martijn Buijs <buijs512@planet.nl>
|
||
|
|
|
||
|
|
The problem seems to be based on the erroneously assumption that if two of
|
||
|
|
the four vertices of a 'grid' are at the same height, the entire grid can be
|
||
|
|
represented as a single plane. It works for an axis aligned slope, but fails
|
||
|
|
on all 4 grids of a 3x3 spike feature. Since the plane normal is constructed
|
||
|
|
from only 3 vertices (only one of the two triangles) this often results in
|
||
|
|
discontinuities at the grid edges (causing small jumps when the contact
|
||
|
|
point moves from one grid to another).
|
||
|
|
|
||
|
|
// unique plane
|
||
|
|
{
|
||
|
|
// check for very simple plane heightfield
|
||
|
|
dReal minXHeightDelta = dInfinity, maxXHeightDelta = - dInfinity;
|
||
|
|
dReal minZHeightDelta = dInfinity, maxZHeightDelta = - dInfinity;
|
||
|
|
|
||
|
|
|
||
|
|
dReal lastXHeight = tempHeightBuffer[0][0].vertex[1];
|
||
|
|
for ( x_local = 1; x_local < numX; x_local++)
|
||
|
|
{
|
||
|
|
HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
|
||
|
|
|
||
|
|
const dReal deltaX = HeightFieldRow[0].vertex[1] - lastXHeight;
|
||
|
|
|
||
|
|
maxXHeightDelta = dMAX (maxXHeightDelta, deltaX);
|
||
|
|
minXHeightDelta = dMIN (minXHeightDelta, deltaX);
|
||
|
|
|
||
|
|
dReal lastZHeight = HeightFieldRow[0].vertex[1];
|
||
|
|
for ( z_local = 1; z_local < numZ; z_local++)
|
||
|
|
{
|
||
|
|
const dReal deltaZ = (HeightFieldRow[z_local].vertex[1] - lastZHeight);
|
||
|
|
|
||
|
|
maxZHeightDelta = dMAX (maxZHeightDelta, deltaZ);
|
||
|
|
minZHeightDelta = dMIN (minZHeightDelta, deltaZ);
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (maxZHeightDelta - minZHeightDelta < dEpsilon &&
|
||
|
|
maxXHeightDelta - minXHeightDelta < dEpsilon )
|
||
|
|
{
|
||
|
|
// it's a single plane.
|
||
|
|
const dVector3 &A = tempHeightBuffer[0][0].vertex;
|
||
|
|
const dVector3 &B = tempHeightBuffer[1][0].vertex;
|
||
|
|
const dVector3 &C = tempHeightBuffer[0][1].vertex;
|
||
|
|
|
||
|
|
// define 2 edges and a point that will define collision plane
|
||
|
|
{
|
||
|
|
dVector3 Edge1, Edge2;
|
||
|
|
dVector3Subtract(C, A, Edge1);
|
||
|
|
dVector3Subtract(B, A, Edge2);
|
||
|
|
dVector3Cross(Edge1, Edge2, triplane);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Define Plane
|
||
|
|
// Normalize plane normal
|
||
|
|
const dReal dinvlength = REAL(1.0) / dVector3Length(triplane);
|
||
|
|
triplane[0] *= dinvlength;
|
||
|
|
triplane[1] *= dinvlength;
|
||
|
|
triplane[2] *= dinvlength;
|
||
|
|
// get distance to origin from plane
|
||
|
|
triplane[3] = dVector3Dot(triplane, A);
|
||
|
|
|
||
|
|
dGeomPlaneSetNoNormalize (sliding_plane, triplane);
|
||
|
|
// find collision and compute contact points
|
||
|
|
const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip);
|
||
|
|
dIASSERT(numTerrainContacts <= numMaxContactsPossible);
|
||
|
|
for (i = 0; i < numTerrainContacts; i++)
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact, i*skip);
|
||
|
|
dOPESIGN(pContact->normal, =, -, triplane);
|
||
|
|
}
|
||
|
|
return numTerrainContacts;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
*/
|
||
|
|
|
||
|
|
int numTerrainContacts = 0;
|
||
|
|
dContactGeom *PlaneContact = m_p_data->m_contacts;
|
||
|
|
|
||
|
|
const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2;
|
||
|
|
if (tempTriangleBufferSize < numTriMax)
|
||
|
|
{
|
||
|
|
resetTriangleBuffer();
|
||
|
|
allocateTriangleBuffer(numTriMax);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Sorting triangle/plane resulting from heightfield zone
|
||
|
|
// Perhaps that would be necessary in case of too much limited
|
||
|
|
// maximum contact point...
|
||
|
|
// or in complex mesh case (trimesh and convex)
|
||
|
|
// need some test or insights on this before enabling this.
|
||
|
|
const bool isContactNumPointsLimited =
|
||
|
|
true;
|
||
|
|
// (numMaxContacts < 8)
|
||
|
|
// || o2->type == dConvexClass
|
||
|
|
// || o2->type == dTriMeshClass
|
||
|
|
// || (numMaxContacts < (int)numTriMax)
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// if small heightfield triangle related to O2 colliding
|
||
|
|
// or no Triangle colliding at all.
|
||
|
|
bool needFurtherPasses = (o2->type == dTriMeshClass);
|
||
|
|
//compute Ratio between Triangle size and O2 aabb size
|
||
|
|
// no FurtherPasses are needed in ray class
|
||
|
|
if (o2->type != dRayClass && needFurtherPasses == false)
|
||
|
|
{
|
||
|
|
const dReal xratio = (o2->aabb[1] - o2->aabb[0]) * m_p_data->m_fInvSampleWidth;
|
||
|
|
if (xratio > REAL(1.5))
|
||
|
|
needFurtherPasses = true;
|
||
|
|
else
|
||
|
|
{
|
||
|
|
const dReal zratio = (o2->aabb[5] - o2->aabb[4]) * m_p_data->m_fInvSampleDepth;
|
||
|
|
if (zratio > REAL(1.5))
|
||
|
|
needFurtherPasses = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
unsigned int numTri = 0;
|
||
|
|
HeightFieldVertex *A, *B, *C, *D;
|
||
|
|
/* (y is up)
|
||
|
|
A--------B-...x
|
||
|
|
| /|
|
||
|
|
| / |
|
||
|
|
| / |
|
||
|
|
| / |
|
||
|
|
| / |
|
||
|
|
| / |
|
||
|
|
| / |
|
||
|
|
|/ |
|
||
|
|
C--------D
|
||
|
|
.
|
||
|
|
.
|
||
|
|
.
|
||
|
|
z
|
||
|
|
*/
|
||
|
|
// keep only triangle that does intersect geom
|
||
|
|
|
||
|
|
const unsigned int maxX_local = maxX - minX;
|
||
|
|
const unsigned int maxZ_local = maxZ - minZ;
|
||
|
|
|
||
|
|
for ( x_local = 0; x_local < maxX_local; x_local++)
|
||
|
|
{
|
||
|
|
HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
|
||
|
|
HeightFieldVertex *HeightFieldNextRow = tempHeightBuffer[x_local + 1];
|
||
|
|
|
||
|
|
// First A
|
||
|
|
C = &HeightFieldRow [0];
|
||
|
|
// First B
|
||
|
|
D = &HeightFieldNextRow[0];
|
||
|
|
|
||
|
|
for ( z_local = 0; z_local < maxZ_local; z_local++)
|
||
|
|
{
|
||
|
|
A = C;
|
||
|
|
B = D;
|
||
|
|
|
||
|
|
C = &HeightFieldRow [z_local + 1];
|
||
|
|
D = &HeightFieldNextRow[z_local + 1];
|
||
|
|
|
||
|
|
const dReal AHeight = A->vertex[1];
|
||
|
|
const dReal BHeight = B->vertex[1];
|
||
|
|
const dReal CHeight = C->vertex[1];
|
||
|
|
const dReal DHeight = D->vertex[1];
|
||
|
|
|
||
|
|
const bool isACollide = AHeight > minO2Height;
|
||
|
|
const bool isBCollide = BHeight > minO2Height;
|
||
|
|
const bool isCCollide = CHeight > minO2Height;
|
||
|
|
const bool isDCollide = DHeight > minO2Height;
|
||
|
|
|
||
|
|
A->state = !(isACollide);
|
||
|
|
B->state = !(isBCollide);
|
||
|
|
C->state = !(isCCollide);
|
||
|
|
D->state = !(isDCollide);
|
||
|
|
|
||
|
|
if (isACollide || isBCollide || isCCollide)
|
||
|
|
{
|
||
|
|
HeightFieldTriangle * const CurrTriUp = &tempTriangleBuffer[numTri++];
|
||
|
|
|
||
|
|
CurrTriUp->state = false;
|
||
|
|
|
||
|
|
// changing point order here implies to change it in isOnHeightField
|
||
|
|
CurrTriUp->vertices[0] = A;
|
||
|
|
CurrTriUp->vertices[1] = B;
|
||
|
|
CurrTriUp->vertices[2] = C;
|
||
|
|
|
||
|
|
if (isContactNumPointsLimited)
|
||
|
|
CurrTriUp->setMinMax();
|
||
|
|
CurrTriUp->isUp = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (isBCollide || isCCollide || isDCollide)
|
||
|
|
{
|
||
|
|
HeightFieldTriangle * const CurrTriDown = &tempTriangleBuffer[numTri++];
|
||
|
|
|
||
|
|
CurrTriDown->state = false;
|
||
|
|
// changing point order here implies to change it in isOnHeightField
|
||
|
|
|
||
|
|
CurrTriDown->vertices[0] = D;
|
||
|
|
CurrTriDown->vertices[1] = B;
|
||
|
|
CurrTriDown->vertices[2] = C;
|
||
|
|
|
||
|
|
|
||
|
|
if (isContactNumPointsLimited)
|
||
|
|
CurrTriDown->setMinMax();
|
||
|
|
CurrTriDown->isUp = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
if (needFurtherPasses &&
|
||
|
|
(isBCollide || isCCollide)
|
||
|
|
&&
|
||
|
|
(AHeight > CHeight &&
|
||
|
|
AHeight > BHeight &&
|
||
|
|
DHeight > CHeight &&
|
||
|
|
DHeight > BHeight))
|
||
|
|
{
|
||
|
|
// That means Edge BC is concave, therefore
|
||
|
|
// BC Edge and B and C vertices cannot collide
|
||
|
|
|
||
|
|
B->state = true;
|
||
|
|
C->state = true;
|
||
|
|
}
|
||
|
|
// should find a way to check other edges (AB, BD, CD) too for concavity
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// at least on triangle should intersect geom
|
||
|
|
dIASSERT (numTri != 0);
|
||
|
|
// pass1: VS triangle as Planes
|
||
|
|
// Group Triangle by same plane definition
|
||
|
|
// as Terrain often has many triangles using same plane definition
|
||
|
|
// then collide against that list of triangles.
|
||
|
|
{
|
||
|
|
|
||
|
|
dVector3 Edge1, Edge2;
|
||
|
|
//compute all triangles normals.
|
||
|
|
for (unsigned int k = 0; k < numTri; k++)
|
||
|
|
{
|
||
|
|
HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
|
||
|
|
|
||
|
|
// define 2 edges and a point that will define collision plane
|
||
|
|
dVector3Subtract(itTriangle->vertices[2]->vertex, itTriangle->vertices[0]->vertex, Edge1);
|
||
|
|
dVector3Subtract(itTriangle->vertices[1]->vertex, itTriangle->vertices[0]->vertex, Edge2);
|
||
|
|
|
||
|
|
// find a perpendicular vector to the triangle
|
||
|
|
if (itTriangle->isUp)
|
||
|
|
dVector3Cross(Edge1, Edge2, triplane);
|
||
|
|
else
|
||
|
|
dVector3Cross(Edge2, Edge1, triplane);
|
||
|
|
|
||
|
|
// Define Plane
|
||
|
|
// Normalize plane normal
|
||
|
|
const dReal dinvlength = REAL(1.0) / dVector3Length(triplane);
|
||
|
|
triplane[0] *= dinvlength;
|
||
|
|
triplane[1] *= dinvlength;
|
||
|
|
triplane[2] *= dinvlength;
|
||
|
|
// get distance to origin from plane
|
||
|
|
triplane[3] = dVector3Dot(triplane, itTriangle->vertices[0]->vertex);
|
||
|
|
|
||
|
|
// saves normal for collision check (planes, triangles, vertices and edges.)
|
||
|
|
dVector3Copy(triplane, itTriangle->planeDef);
|
||
|
|
// saves distance for collision check (planes, triangles, vertices and edges.)
|
||
|
|
itTriangle->planeDef[3] = triplane[3];
|
||
|
|
}
|
||
|
|
|
||
|
|
// group by Triangles by Planes sharing same plane definition
|
||
|
|
if (tempPlaneBufferSize < numTri)
|
||
|
|
{
|
||
|
|
resetPlaneBuffer();
|
||
|
|
allocatePlaneBuffer(numTri);
|
||
|
|
}
|
||
|
|
|
||
|
|
unsigned int numPlanes = 0;
|
||
|
|
for (unsigned int k = 0; k < numTri; k++)
|
||
|
|
{
|
||
|
|
HeightFieldTriangle * const tri_base = &tempTriangleBuffer[k];
|
||
|
|
|
||
|
|
if (tri_base->state == true)
|
||
|
|
continue;// already tested or added to plane list.
|
||
|
|
|
||
|
|
HeightFieldPlane * const currPlane = tempPlaneBuffer[numPlanes];
|
||
|
|
currPlane->resetTriangleListSize(numTri - k);
|
||
|
|
currPlane->addTriangle(tri_base);
|
||
|
|
// saves normal for collision check (planes, triangles, vertices and edges.)
|
||
|
|
dVector3Copy(tri_base->planeDef, currPlane->planeDef);
|
||
|
|
// saves distance for collision check (planes, triangles, vertices and edges.)
|
||
|
|
currPlane->planeDef[3]= tri_base->planeDef[3];
|
||
|
|
|
||
|
|
const dReal normx = tri_base->planeDef[0];
|
||
|
|
const dReal normy = tri_base->planeDef[1];
|
||
|
|
const dReal normz = tri_base->planeDef[2];
|
||
|
|
const dReal dist = tri_base->planeDef[3];
|
||
|
|
|
||
|
|
for (unsigned int m = k + 1; m < numTri; m++)
|
||
|
|
{
|
||
|
|
|
||
|
|
HeightFieldTriangle * const tri_test = &tempTriangleBuffer[m];
|
||
|
|
if (tri_test->state == true)
|
||
|
|
continue;// already tested or added to plane list.
|
||
|
|
|
||
|
|
// normals and distance are the same.
|
||
|
|
if (
|
||
|
|
dFabs(normy - tri_test->planeDef[1]) < dEpsilon &&
|
||
|
|
dFabs(dist - tri_test->planeDef[3]) < dEpsilon &&
|
||
|
|
dFabs(normx - tri_test->planeDef[0]) < dEpsilon &&
|
||
|
|
dFabs(normz - tri_test->planeDef[2]) < dEpsilon
|
||
|
|
)
|
||
|
|
{
|
||
|
|
currPlane->addTriangle (tri_test);
|
||
|
|
tri_test->state = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
tri_base->state = true;
|
||
|
|
if (isContactNumPointsLimited)
|
||
|
|
currPlane->setMinMax();
|
||
|
|
|
||
|
|
numPlanes++;
|
||
|
|
}
|
||
|
|
|
||
|
|
// sort planes
|
||
|
|
if (isContactNumPointsLimited)
|
||
|
|
sortPlanes(numPlanes);
|
||
|
|
|
||
|
|
#if !defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
|
||
|
|
/*
|
||
|
|
Note by Oleh_Derevenko:
|
||
|
|
It seems to be incorrect to limit contact count by some particular value
|
||
|
|
since some of them (and even all of them) may be culled in following condition.
|
||
|
|
However I do not see an easy way to fix this.
|
||
|
|
If not that culling the flags modification should be changed here and
|
||
|
|
additionally repeated after some contacts have been generated (in "if (didCollide)").
|
||
|
|
The maximum of contacts in flags would then be set to minimum of contacts
|
||
|
|
remaining and HEIGHTFIELDMAXCONTACTPERCELL.
|
||
|
|
*/
|
||
|
|
int planeTestFlags = (flags & ~NUMC_MASK) | HEIGHTFIELDMAXCONTACTPERCELL;
|
||
|
|
dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
|
||
|
|
#else // if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
|
||
|
|
int numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
|
||
|
|
int planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane;
|
||
|
|
dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
for (unsigned int k = 0; k < numPlanes; k++)
|
||
|
|
{
|
||
|
|
HeightFieldPlane * const itPlane = tempPlaneBuffer[k];
|
||
|
|
|
||
|
|
//set Geom
|
||
|
|
dGeomPlaneSetNoNormalize (sliding_plane, itPlane->planeDef);
|
||
|
|
//dGeomPlaneSetParams (sliding_plane, triangle_Plane[0], triangle_Plane[1], triangle_Plane[2], triangle_Plane[3]);
|
||
|
|
// find collision and compute contact points
|
||
|
|
bool didCollide = false;
|
||
|
|
const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, planeTestFlags, PlaneContact, sizeof(dContactGeom));
|
||
|
|
const sizeint planeTriListSize = itPlane->trianglelistCurrentSize;
|
||
|
|
for (i = 0; i < numPlaneContacts; i++)
|
||
|
|
{
|
||
|
|
dContactGeom *planeCurrContact = PlaneContact + i;
|
||
|
|
// Check if contact point found in plane is inside Triangle.
|
||
|
|
const dVector3 &pCPos = planeCurrContact->pos;
|
||
|
|
for (sizeint b = 0; planeTriListSize > b; b++)
|
||
|
|
{
|
||
|
|
if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0],
|
||
|
|
pCPos,
|
||
|
|
itPlane->trianglelist[b]->isUp))
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact, numTerrainContacts*skip);
|
||
|
|
dVector3Copy(pCPos, pContact->pos);
|
||
|
|
dOPESIGN(pContact->normal, =, -, itPlane->planeDef);
|
||
|
|
pContact->depth = planeCurrContact->depth;
|
||
|
|
pContact->side1 = planeCurrContact->side1;
|
||
|
|
pContact->side2 = planeCurrContact->side2;
|
||
|
|
numTerrainContacts++;
|
||
|
|
if ( numTerrainContacts == numMaxContactsPossible )
|
||
|
|
return numTerrainContacts;
|
||
|
|
|
||
|
|
didCollide = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (didCollide)
|
||
|
|
{
|
||
|
|
#if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
|
||
|
|
/* Note by Oleh_Derevenko:
|
||
|
|
This code is not used - see another note above
|
||
|
|
*/
|
||
|
|
numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
|
||
|
|
planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane;
|
||
|
|
dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
|
||
|
|
#endif
|
||
|
|
for (sizeint b = 0; planeTriListSize > b; b++)
|
||
|
|
{
|
||
|
|
// flag Triangles Vertices as collided
|
||
|
|
// to prevent any collision test of those
|
||
|
|
for (i = 0; i < 3; i++)
|
||
|
|
itPlane->trianglelist[b]->vertices[i]->state = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// flag triangle as not collided so that Vertices or Edge
|
||
|
|
// of that triangles will be checked.
|
||
|
|
for (sizeint b = 0; planeTriListSize > b; b++)
|
||
|
|
{
|
||
|
|
itPlane->trianglelist[b]->state = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// pass2: VS triangle vertices
|
||
|
|
if (needFurtherPasses)
|
||
|
|
{
|
||
|
|
dxRay tempRay(0, 1);
|
||
|
|
dReal depth;
|
||
|
|
bool vertexCollided;
|
||
|
|
|
||
|
|
// Only one contact is necessary for ray test
|
||
|
|
int rayTestFlags = (flags & ~NUMC_MASK) | 1;
|
||
|
|
dIASSERT((1 & ~NUMC_MASK) == 0);
|
||
|
|
//
|
||
|
|
// Find Contact Penetration Depth of each vertices
|
||
|
|
//
|
||
|
|
for (unsigned int k = 0; k < numTri; k++)
|
||
|
|
{
|
||
|
|
const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
|
||
|
|
if (itTriangle->state == true)
|
||
|
|
continue;// plane triangle did already collide.
|
||
|
|
|
||
|
|
for (sizeint i = 0; i < 3; i++)
|
||
|
|
{
|
||
|
|
HeightFieldVertex *vertex = itTriangle->vertices[i];
|
||
|
|
if (vertex->state == true)
|
||
|
|
continue;// vertice did already collide.
|
||
|
|
|
||
|
|
vertexCollided = false;
|
||
|
|
const dVector3 &triVertex = vertex->vertex;
|
||
|
|
if ( geomNDepthGetter )
|
||
|
|
{
|
||
|
|
depth = geomNDepthGetter( o2,
|
||
|
|
triVertex[0], triVertex[1], triVertex[2] );
|
||
|
|
if (depth > dEpsilon)
|
||
|
|
vertexCollided = true;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
// We don't have a GetDepth function, so do a ray cast instead.
|
||
|
|
// NOTE: This isn't ideal, and a GetDepth function should be
|
||
|
|
// written for all geom classes.
|
||
|
|
tempRay.length = (minO2Height - triVertex[1]) * REAL(1000.0);
|
||
|
|
|
||
|
|
//dGeomRaySet( &tempRay, pContact->pos[0], pContact->pos[1], pContact->pos[2],
|
||
|
|
// - itTriangle->Normal[0], - itTriangle->Normal[1], - itTriangle->Normal[2] );
|
||
|
|
dGeomRaySetNoNormalize(tempRay, triVertex, itTriangle->planeDef);
|
||
|
|
|
||
|
|
if ( geomRayNCollider( &tempRay, o2, rayTestFlags, PlaneContact, sizeof( dContactGeom ) ) )
|
||
|
|
{
|
||
|
|
depth = PlaneContact[0].depth;
|
||
|
|
vertexCollided = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (vertexCollided)
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact, numTerrainContacts*skip);
|
||
|
|
//create contact using vertices
|
||
|
|
dVector3Copy (triVertex, pContact->pos);
|
||
|
|
//create contact using Plane Normal
|
||
|
|
dOPESIGN(pContact->normal, =, -, itTriangle->planeDef);
|
||
|
|
|
||
|
|
pContact->depth = depth;
|
||
|
|
pContact->side1 = -1;
|
||
|
|
pContact->side2 = -1;
|
||
|
|
|
||
|
|
numTerrainContacts++;
|
||
|
|
if ( numTerrainContacts == numMaxContactsPossible )
|
||
|
|
return numTerrainContacts;
|
||
|
|
|
||
|
|
vertex->state = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef _HEIGHTFIELDEDGECOLLIDING
|
||
|
|
// pass3: VS triangle Edges
|
||
|
|
if (needFurtherPasses)
|
||
|
|
{
|
||
|
|
dVector3 Edge;
|
||
|
|
dxRay edgeRay(0, 1);
|
||
|
|
|
||
|
|
int numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
|
||
|
|
int triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri;
|
||
|
|
dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
|
||
|
|
|
||
|
|
for (unsigned int k = 0; k < numTri; k++)
|
||
|
|
{
|
||
|
|
const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
|
||
|
|
|
||
|
|
if (itTriangle->state == true)
|
||
|
|
continue;// plane did already collide.
|
||
|
|
|
||
|
|
for (sizeint m = 0; m < 3; m++)
|
||
|
|
{
|
||
|
|
const sizeint next = (m + 1) % 3;
|
||
|
|
HeightFieldVertex *vertex0 = itTriangle->vertices[m];
|
||
|
|
HeightFieldVertex *vertex1 = itTriangle->vertices[next];
|
||
|
|
|
||
|
|
// not concave or under the AABB
|
||
|
|
// nor triangle already collided against vertices
|
||
|
|
if (vertex0->state == true && vertex1->state == true)
|
||
|
|
continue;// plane did already collide.
|
||
|
|
|
||
|
|
dVector3Subtract(vertex1->vertex, vertex0->vertex, Edge);
|
||
|
|
edgeRay.length = dVector3Length (Edge);
|
||
|
|
dGeomRaySetNoNormalize(edgeRay, vertex1->vertex, Edge);
|
||
|
|
int prevTerrainContacts = numTerrainContacts;
|
||
|
|
pContact = CONTACT(contact, prevTerrainContacts*skip);
|
||
|
|
const int numCollision = geomRayNCollider(&edgeRay,o2,triTestFlags,pContact,skip);
|
||
|
|
dIASSERT(numCollision <= numMaxContactsPerTri);
|
||
|
|
|
||
|
|
if (numCollision)
|
||
|
|
{
|
||
|
|
numTerrainContacts += numCollision;
|
||
|
|
|
||
|
|
do
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact, prevTerrainContacts*skip);
|
||
|
|
|
||
|
|
//create contact using Plane Normal
|
||
|
|
dOPESIGN(pContact->normal, =, -, itTriangle->planeDef);
|
||
|
|
|
||
|
|
pContact->depth = DistancePointToLine(pContact->pos, vertex1->vertex, Edge, edgeRay.length);
|
||
|
|
}
|
||
|
|
while (++prevTerrainContacts != numTerrainContacts);
|
||
|
|
|
||
|
|
if ( numTerrainContacts == numMaxContactsPossible )
|
||
|
|
return numTerrainContacts;
|
||
|
|
|
||
|
|
numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
|
||
|
|
triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri;
|
||
|
|
dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
itTriangle->vertices[0]->state = true;
|
||
|
|
itTriangle->vertices[1]->state = true;
|
||
|
|
itTriangle->vertices[2]->state = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif // _HEIGHTFIELDEDGECOLLIDING
|
||
|
|
return numTerrainContacts;
|
||
|
|
}
|
||
|
|
|
||
|
|
int dCollideHeightfield( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contact, int skip )
|
||
|
|
{
|
||
|
|
dIASSERT( skip >= (int)sizeof(dContactGeom) );
|
||
|
|
dIASSERT( o1->type == dHeightfieldClass );
|
||
|
|
dIASSERT((flags & NUMC_MASK) >= 1);
|
||
|
|
|
||
|
|
int i;
|
||
|
|
|
||
|
|
// if ((flags & NUMC_MASK) == 0) -- An assertion check is made on entry
|
||
|
|
// { flags = (flags & ~NUMC_MASK) | 1; dIASSERT((1 & ~NUMC_MASK) == 0); }
|
||
|
|
|
||
|
|
int numMaxTerrainContacts = (flags & NUMC_MASK);
|
||
|
|
|
||
|
|
dxHeightfield *terrain = (dxHeightfield*) o1;
|
||
|
|
|
||
|
|
dVector3 posbak;
|
||
|
|
dMatrix3 Rbak;
|
||
|
|
dReal aabbbak[6];
|
||
|
|
int gflagsbak;
|
||
|
|
dVector3 pos0,pos1;
|
||
|
|
dMatrix3 R1;
|
||
|
|
|
||
|
|
int numTerrainContacts = 0;
|
||
|
|
int numTerrainOrigContacts = 0;
|
||
|
|
|
||
|
|
//@@ Should find a way to set reComputeAABB to false in default case
|
||
|
|
// aka DHEIGHTFIELD_CORNER_ORIGIN not defined and terrain not PLACEABLE
|
||
|
|
// so that we can free some memory and speed up things a bit
|
||
|
|
// while saving some precision loss
|
||
|
|
#ifndef DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
const bool reComputeAABB = true;
|
||
|
|
#else
|
||
|
|
const bool reComputeAABB = ( terrain->gflags & GEOM_PLACEABLE ) ? true : false;
|
||
|
|
#endif //DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
//
|
||
|
|
// Transform O2 into Heightfield Space
|
||
|
|
//
|
||
|
|
if (reComputeAABB)
|
||
|
|
{
|
||
|
|
// Backup original o2 position, rotation and AABB.
|
||
|
|
dVector3Copy( o2->final_posr->pos, posbak );
|
||
|
|
dMatrix3Copy( o2->final_posr->R, Rbak );
|
||
|
|
memcpy( aabbbak, o2->aabb, sizeof( dReal ) * 6 );
|
||
|
|
gflagsbak = o2->gflags;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ( terrain->gflags & GEOM_PLACEABLE )
|
||
|
|
{
|
||
|
|
// Transform o2 into heightfield space.
|
||
|
|
dSubtractVectors3( pos0, o2->final_posr->pos, terrain->final_posr->pos );
|
||
|
|
dMultiply1_331( pos1, terrain->final_posr->R, pos0 );
|
||
|
|
dMultiply1_333( R1, terrain->final_posr->R, o2->final_posr->R );
|
||
|
|
|
||
|
|
// Update o2 with transformed position and rotation.
|
||
|
|
dVector3Copy( pos1, o2->final_posr->pos );
|
||
|
|
dMatrix3Copy( R1, o2->final_posr->R );
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifndef DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
o2->final_posr->pos[ 0 ] += terrain->m_p_data->m_fHalfWidth;
|
||
|
|
o2->final_posr->pos[ 2 ] += terrain->m_p_data->m_fHalfDepth;
|
||
|
|
#endif // DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
// Rebuild AABB for O2
|
||
|
|
if (reComputeAABB)
|
||
|
|
o2->computeAABB();
|
||
|
|
|
||
|
|
//
|
||
|
|
// Collide
|
||
|
|
//
|
||
|
|
|
||
|
|
//check if inside boundaries
|
||
|
|
// using O2 aabb
|
||
|
|
// aabb[6] is (minx, maxx, miny, maxy, minz, maxz)
|
||
|
|
const bool wrapped = terrain->m_p_data->m_bWrapMode != 0;
|
||
|
|
|
||
|
|
if ( !wrapped )
|
||
|
|
{
|
||
|
|
if ( o2->aabb[0] > terrain->m_p_data->m_fWidth //MinX
|
||
|
|
|| o2->aabb[4] > terrain->m_p_data->m_fDepth)//MinZ
|
||
|
|
goto dCollideHeightfieldExit;
|
||
|
|
|
||
|
|
if ( o2->aabb[1] < 0 //MaxX
|
||
|
|
|| o2->aabb[5] < 0)//MaxZ
|
||
|
|
goto dCollideHeightfieldExit;
|
||
|
|
}
|
||
|
|
|
||
|
|
{ // To narrow scope of following variables
|
||
|
|
const dReal fInvSampleWidth = terrain->m_p_data->m_fInvSampleWidth;
|
||
|
|
int nMinX = (int)dFloor(dNextAfter(o2->aabb[0] * fInvSampleWidth, -dInfinity));
|
||
|
|
int nMaxX = (int)dCeil(dNextAfter(o2->aabb[1] * fInvSampleWidth, dInfinity));
|
||
|
|
const dReal fInvSampleDepth = terrain->m_p_data->m_fInvSampleDepth;
|
||
|
|
int nMinZ = (int)dFloor(dNextAfter(o2->aabb[4] * fInvSampleDepth, -dInfinity));
|
||
|
|
int nMaxZ = (int)dCeil(dNextAfter(o2->aabb[5] * fInvSampleDepth, dInfinity));
|
||
|
|
|
||
|
|
if ( !wrapped )
|
||
|
|
{
|
||
|
|
nMinX = dMAX( nMinX, 0 );
|
||
|
|
nMaxX = dMIN( nMaxX, terrain->m_p_data->m_nWidthSamples - 1 );
|
||
|
|
nMinZ = dMAX( nMinZ, 0 );
|
||
|
|
nMaxZ = dMIN( nMaxZ, terrain->m_p_data->m_nDepthSamples - 1 );
|
||
|
|
|
||
|
|
dIASSERT ((nMinX < nMaxX) && (nMinZ < nMaxZ));
|
||
|
|
}
|
||
|
|
|
||
|
|
numTerrainOrigContacts = numTerrainContacts;
|
||
|
|
numTerrainContacts += terrain->dCollideHeightfieldZone(
|
||
|
|
nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts,
|
||
|
|
flags,CONTACT(contact,numTerrainContacts*skip),skip );
|
||
|
|
dIASSERT( numTerrainContacts <= numMaxTerrainContacts );
|
||
|
|
}
|
||
|
|
|
||
|
|
dContactGeom *pContact;
|
||
|
|
for ( i = numTerrainOrigContacts; i != numTerrainContacts; ++i )
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact,i*skip);
|
||
|
|
pContact->g1 = o1;
|
||
|
|
pContact->g2 = o2;
|
||
|
|
// pContact->side1 = -1; -- Oleh_Derevenko: sides must not be erased here as they are set by respective colliders during ray/plane tests
|
||
|
|
// pContact->side2 = -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//------------------------------------------------------------------------------
|
||
|
|
|
||
|
|
dCollideHeightfieldExit:
|
||
|
|
|
||
|
|
if (reComputeAABB)
|
||
|
|
{
|
||
|
|
// Restore o2 position, rotation and AABB
|
||
|
|
dVector3Copy( posbak, o2->final_posr->pos );
|
||
|
|
dMatrix3Copy( Rbak, o2->final_posr->R );
|
||
|
|
memcpy( o2->aabb, aabbbak, sizeof(dReal)*6 );
|
||
|
|
o2->gflags = gflagsbak;
|
||
|
|
|
||
|
|
//
|
||
|
|
// Transform Contacts to World Space
|
||
|
|
//
|
||
|
|
if ( terrain->gflags & GEOM_PLACEABLE )
|
||
|
|
{
|
||
|
|
for ( i = 0; i < numTerrainContacts; ++i )
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact,i*skip);
|
||
|
|
dCopyVector3( pos0, pContact->pos );
|
||
|
|
|
||
|
|
#ifndef DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
pos0[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
|
||
|
|
pos0[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
|
||
|
|
#endif // !DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
|
||
|
|
dMultiply0_331( pContact->pos, terrain->final_posr->R, pos0 );
|
||
|
|
|
||
|
|
dAddVectors3( pContact->pos, pContact->pos, terrain->final_posr->pos );
|
||
|
|
dCopyVector3( pos0, pContact->normal );
|
||
|
|
|
||
|
|
dMultiply0_331( pContact->normal, terrain->final_posr->R, pos0 );
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#ifndef DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
else
|
||
|
|
{
|
||
|
|
for ( i = 0; i < numTerrainContacts; ++i )
|
||
|
|
{
|
||
|
|
pContact = CONTACT(contact,i*skip);
|
||
|
|
pContact->pos[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
|
||
|
|
pContact->pos[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif // !DHEIGHTFIELD_CORNER_ORIGIN
|
||
|
|
}
|
||
|
|
// Return contact count.
|
||
|
|
return numTerrainContacts;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|