Menu
▾
▴
[Opende-svn] SF.net SVN: opende: [1003] trunk
|
From: <hid...@us...> - 2006-07-06 13:29:24
|
Revision: 1003 Author: hidden_asbestos Date: 2006-07-06 06:29:04 -0700 (Thu, 06 Jul 2006) ViewCVS: http://svn.sourceforge.net/opende/?rev=1003&view=rev Log Message: ----------- New heightfield Geom class 'dHeightField'. See ode/test/test_heightfield.cpp for a simple demonstration. Modified Paths: -------------- trunk/CHANGELOG.txt trunk/include/ode/collision.h trunk/include/ode/odemath.h trunk/ode/src/collision_kernel.cpp trunk/ode/src/collision_std.h Added Paths: ----------- trunk/ode/doc/readme-heightfield.txt trunk/ode/src/heightfield.cpp trunk/ode/src/heightfield.h trunk/ode/test/test_heightfield.cpp Modified: trunk/CHANGELOG.txt =================================================================== --- trunk/CHANGELOG.txt 2006-07-06 00:34:30 UTC (rev 1002) +++ trunk/CHANGELOG.txt 2006-07-06 13:29:04 UTC (rev 1003) @@ -9,6 +9,11 @@ ------------------------------------------------------------------------------ +07/06/06 david + + * Added heightfield primitive collision code. Simple test available in + ode/test/test_heightfield + 04/03/06 rodrigo * Added Convex primitive collision code, Modified: trunk/include/ode/collision.h =================================================================== --- trunk/include/ode/collision.h 2006-07-06 00:34:30 UTC (rev 1002) +++ trunk/include/ode/collision.h 2006-07-06 13:29:04 UTC (rev 1003) @@ -664,6 +664,7 @@ dConvexClass, dGeomTransformClass, dTriMeshClass, + dHeightfieldClass, dFirstSpaceClass, dSimpleSpaceClass = dFirstSpaceClass, @@ -749,7 +750,61 @@ ODE_API void dGeomTransformSetInfo (dGeomID g, int mode); ODE_API int dGeomTransformGetInfo (dGeomID g); + /* ************************************************************************ */ +/* heightfield functions */ + + +// Callback prototype +typedef dReal dHeightfieldGetHeight( void* pUserData, int x, int z ); + +// Data storage for triangle meshes. +struct dxHeightfieldData; +typedef struct dxHeightfieldData* dHeightfieldDataID; + +ODE_API dGeomID dCreateHeightfield( dSpaceID space, + dHeightfieldDataID Data, int bPlaceable ); + +ODE_API dHeightfieldDataID dGeomHeightfieldDataCreate(); + +ODE_API void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d, + void *pUserData, dHeightfieldGetHeight *Callback, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ); + +ODE_API void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d, + unsigned char *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ); + +ODE_API void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d, + unsigned char *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ); + +ODE_API void dGeomHeightfieldDataBuildFloat( dHeightfieldDataID d, + dReal *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ); + +ODE_API void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, + dReal vMinHeight, dReal vMaxHeight ); + +ODE_API void dGeomHeightfieldDataDestroy( dHeightfieldDataID d ); + +ODE_API void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID Data ); + +ODE_API dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g ); + +ODE_API dReal dGeomHeightfieldPointDepth( dGeomID g, dReal x, dReal y, dReal z ); + + + +/* ************************************************************************ */ /* utility functions */ ODE_API void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2, Modified: trunk/include/ode/odemath.h =================================================================== --- trunk/include/ode/odemath.h 2006-07-06 00:34:30 UTC (rev 1002) +++ trunk/include/ode/odemath.h 2006-07-06 13:29:04 UTC (rev 1003) @@ -45,7 +45,53 @@ #define dVALIDMAT(m) (!(dIsNan(m[0]) | dIsNan(m[2]) | dIsNan(m[2]) | dIsNan(m[3]) | dIsNan(m[4]) | dIsNan(m[5]) | dIsNan(m[6]) | dIsNan(m[7]) | dIsNan(m[8]) | dIsNan(m[9]) | dIsNan(m[10]) | dIsNan(m[11]))) + + + /* + * General purpose vector operations with other vectors or constants. + */ + +#define dOP(a,op,b,c) \ + (a)[0] = ((b)[0]) op ((c)[0]); \ + (a)[1] = ((b)[1]) op ((c)[1]); \ + (a)[2] = ((b)[2]) op ((c)[2]); +#define dOPC(a,op,b,c) \ + (a)[0] = ((b)[0]) op (c); \ + (a)[1] = ((b)[1]) op (c); \ + (a)[2] = ((b)[2]) op (c); +#define dOPE(a,op,b) \ + (a)[0] op ((b)[0]); \ + (a)[1] op ((b)[1]); \ + (a)[2] op ((b)[2]); +#define dOPEC(a,op,c) \ + (a)[0] op (c); \ + (a)[1] op (c); \ + (a)[2] op (c); + + +/* + * Length, and squared length helpers. dLENGTH returns the length of a dVector3. + * dLENGTHSQUARED return the squared length of a dVector3. + */ + +#define dLENGTHSQUARED(a) (((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2])) + +#ifdef __cplusplus + +PURE_INLINE dReal dLENGTH (const dReal *a) { return dSqrt(dLENGTHSQUARED(a)); } + +#else + +#define dLENGTH(a) ( dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) ) ) + +#endif /* __cplusplus */ + + + + + +/* * 3-way dot product. dDOTpq means that elements of `a' and `b' are spaced * p and q indexes apart respectively. dDOT() means dDOT11. * in C++ we could use function templates to get all the versions of these Added: trunk/ode/doc/readme-heightfield.txt =================================================================== --- trunk/ode/doc/readme-heightfield.txt (rev 0) +++ trunk/ode/doc/readme-heightfield.txt 2006-07-06 13:29:04 UTC (rev 1003) @@ -0,0 +1,239 @@ +README.TXT + +dHeightfield collider for ODE + + Created by Martijn Buijs + April 2006 + + +Introduction + + dHeightfield is a heightfield collider for ODE, based on the "Terrain & Cone" contrib by Benoit Chaperot. + It removes the terrain dimension limitations (supports non-square fields & grid spacing, non-power of two + dimensions) and allows deformable terrains. It should work for animated water surfaces as well -- the reason + I named it "dHeightfield" rather than "dTerrain". A bunch of fixes are included (AABB patch, callback patch), + and the geom creation interface has been changed to mimic the trimesh collider. + + +Notes: + +* This collider is NOT final. It wes tested under a limited number of situations. The Short sample + path has not been tested, allthough it should work okay. Optimizations may be made. I encourage + anyone using this collider to take a brief look at the code. +* Works fine alongside the Terrain & Cone contrib. +* Can confirm it works fine with sphere, box, capsule, cone colliders, not sure about rays. +* The short sample data path has not been tested, but I think it should work ok. +* I used the word "depth" for the Z dimension throughout the code to avoid confusion with the + vertical "height" dimension. It is unrelated to other uses of the word troughout ODE. +* The vWidth and vDepth parameters in the dGeomHeightfieldDataBuild* are the world space dimensions + of the heightfield. For example, if you have 30 meter sample data then vWidth = vWidthSamples * 30. +* The surface thickness can be specified, rather than treating the parameter as flag and setting the + AABB minimum height bound to -dInfinity. This allows bodies to move underneath the terrain (for + example in tunnels and overhangs modeled with trimesh colliders). +* With dGeomHeightfieldDataSetBounds the min and max height bounds can be set, this is usefull if + you are using the callback path (and thus the bounds cannot be automatically computed) or have + dynamic heightfields. When using the callback, the min and max bounds are initially set to + inifinity, set them manually to speed things up. +* The min/max values passed to dGeomHeightfieldDataSetBounds are in sample space. It is scaled and + offset (just like all other sample data) internally. + + +Open issues/wishlist/todo list: + +* needs some work to make heightfields work with Z-up systems +* 'real' surface thickness (right now it only adds to the bottom of the AABB) +* a bit flag map to disable samples (to create holes in terrains) +* allow variable sample interpolation methods (preferably per callback) +* allow arbitrary heightfield orientations +* configuration independent floating point sample path +* DrawStuff draw function +* test_heightfield demo program + + +Bugs: + +* There's one extra row of quads along the border of finite heightfields (presumably to make terrains + seamlessly tileable?). This should be easy to fix by removing that feature. Rather than dealing with + it internally, the sample data itself should be made tileable. I believe that is more correct. + + +New data types: + + dGeomHeightfieldDataID + + +New functions: + + dHeightfieldDataID dGeomHeightfieldDataCreate() + + void dGeomHeightfieldDataBuildCallback(dHeightfieldDataID d, + void *pUserData, dHeightfieldGetHeight *Callback, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness) + + void dGeomHeightfieldDataBuildByte(dHeightfieldDataID d, + unsigned char *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness) + + void dGeomHeightfieldDataBuildShort(dHeightfieldDataID d, + unsigned char *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness) + + void dGeomHeightfieldDataBuildFloat(dHeightfieldDataID d, + dReal *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness) + + void dGeomHeightfieldDataSetBounds(dHeightfieldDataID d, dReal vMinHeight, dReal vMaxHeight) + + void dGeomHeightfieldDataDestroy(dHeightfieldDataID d) + + dGeomID dCreateHeightfield(dSpaceID space, dHeightfieldDataID Data, int bPlaceable) + + void dGeomHeightfieldSetHeightfieldData(dGeomID g, dHeightfieldDataID Data) + + dHeightfieldDataID dGeomHeightfieldGetHeightfieldData(dGeomID g) + + dReal dGeomHeightfieldPointDepth(dGeomID g, dReal x, dReal y, dReal z) + + +Usage (ODE 0.5): + +1) Add to folder "ode\src": + + dHeightfield.h + dHeightfield.cpp + +2) Add in "include\ode\collision.h": + + enum { + dSphereClass = 0, + dBoxClass, + dCCylinderClass, + dCylinderClass, + dPlaneClass, + dRayClass, + dGeomTransformClass, + dTriMeshClass, + + dHeightfieldClass, // <------- add this line + + dFirstSpaceClass, + dSimpleSpaceClass = dFirstSpaceClass, + dHashSpaceClass, + dQuadTreeSpaceClass, + + dLastSpaceClass = dQuadTreeSpaceClass, + + dFirstUserClass, + dLastUserClass = dFirstUserClass + dMaxUserClasses - 1, + dGeomNumClasses + }; + + // <------- and from here... + struct dxHeightfieldData; + typedef struct dxHeightfieldData* dHeightfieldDataID; + typedef float dHeightfieldGetHeight(void* pUserData, int x, int y); + + dHeightfieldDataID dGeomHeightfieldDataCreate(); + + void dGeomHeightfieldDataBuildCallback(dHeightfieldDataID d, + void *pUserData, dHeightfieldGetHeight *Callback, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness); + + void dGeomHeightfieldDataBuildByte(dHeightfieldDataID d, + unsigned char *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness); + + void dGeomHeightfieldDataBuildShort(dHeightfieldDataID d, + unsigned char *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness); + + void dGeomHeightfieldDataBuildFloat(dHeightfieldDataID d, + dReal *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness); + + void dGeomHeightfieldDataSetBounds(dHeightfieldDataID d, dReal vMinHeight, dReal vMaxHeight); + + void dGeomHeightfieldDataDestroy(dHeightfieldDataID d); + + dGeomID dCreateHeightfield(dSpaceID space, dHeightfieldDataID Data, int bPlaceable); + + void dGeomHeightfieldSetHeightfieldData(dGeomID g, dHeightfieldDataID Data); + + dHeightfieldDataID dGeomHeightfieldGetHeightfieldData(dGeomID g); + + dReal dGeomHeightfieldPointDepth(dGeomID g, dReal x, dReal y, dReal z); + // <------- to here + +3) Add in "include\ode\odemath.h" (this can be skipped if the "Terrain & Cone" contrib is already applied): + + #define dOP(a,op,b,c) \ + (a)[0] = ((b)[0]) op ((c)[0]); \ + (a)[1] = ((b)[1]) op ((c)[1]); \ + (a)[2] = ((b)[2]) op ((c)[2]); + #define dOPC(a,op,b,c) \ + (a)[0] = ((b)[0]) op (c); \ + (a)[1] = ((b)[1]) op (c); \ + (a)[2] = ((b)[2]) op (c); + #define dOPE(a,op,b) \ + (a)[0] op ((b)[0]); \ + (a)[1] op ((b)[1]); \ + (a)[2] op ((b)[2]); + #define dOPEC(a,op,c) \ + (a)[0] op (c); \ + (a)[1] op (c); \ + (a)[2] op (c); + #define dLENGTH(a) \ + (dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) )); + #define dLENGTHSQUARED(a) \ + (((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2])); + +4) Add in "ode\src\collision_kernel.cpp" line 137: + + setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dCCylinderClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dConeClass,&dCollideHeightfield); + +5) Add in "ode\src\collision_std.h": + + int dCollideHeightfield(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip); + +6) Add "dHeightfield.cpp" to the ODE makefile, workspace etc. + + +Acknowledgements: + + Based on Terrain & Cone by: + Benoit CHAPEROT 2003-2004 + http://www.jstarlab.com + + Geoff Carlton + AABB patch for Terrain & Cone + dTerrainZ height callback + + +Contact: + + buijs512 AT planet DOT nl + http://home.planet.nl/~buijs512/ + + + +END OF FILE Modified: trunk/ode/src/collision_kernel.cpp =================================================================== --- trunk/ode/src/collision_kernel.cpp 2006-07-06 00:34:30 UTC (rev 1002) +++ trunk/ode/src/collision_kernel.cpp 2006-07-06 13:29:04 UTC (rev 1003) @@ -200,6 +200,13 @@ setCollider (dRayClass,dConvexClass,&dCollideRayConvex); //<-- Convex Collision +//--> dHeightfield Collision + setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield); + setCollider (dHeightfieldClass,dCapsuleClass,&dCollideHeightfield); +//<-- dHeightfield Collision + setAllColliders (dGeomTransformClass,&dCollideTransform); } Modified: trunk/ode/src/collision_std.h =================================================================== --- trunk/ode/src/collision_std.h 2006-07-06 00:34:30 UTC (rev 1002) +++ trunk/ode/src/collision_std.h 2006-07-06 13:29:04 UTC (rev 1003) @@ -89,6 +89,10 @@ dContactGeom *contact, int skip); //<-- Convex Collision +// dHeightfield +int dCollideHeightfield( dxGeom *o1, dxGeom *o2, + int flags, dContactGeom *contact, int skip ); + //**************************************************************************** // the basic geometry objects Added: trunk/ode/src/heightfield.cpp =================================================================== --- trunk/ode/src/heightfield.cpp (rev 0) +++ trunk/ode/src/heightfield.cpp 2006-07-06 13:29:04 UTC (rev 1003) @@ -0,0 +1,930 @@ +// dHeightfield Collider +// 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/matrix.h> +#include <ode/rotation.h> +#include <ode/odemath.h> +#include "collision_kernel.h" +#include "collision_std.h" +#include "collision_util.h" +#include "heightfield.h" + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) +#define MAXCONTACT 10 +#define TERRAINTOL 0.0f + +#if _MSC_VER <= 1200 +#define dMIN(A,B) ((A)>(B) ? B : A) +#define dMAX(A,B) ((A)>(B) ? A : B) +#else +#define dMIN(A,B) std::min(A,B) +#define dMAX(A,B) std::max(A,B) +#endif + +// 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)) ) + + +//////// dxHeightfieldData ///////////////////////////////////////////////////////////// + +// dxHeightfieldData constructor +dxHeightfieldData::dxHeightfieldData() +{ + // +} + + +// build Heightfield data +void dxHeightfieldData::SetData(int nWidthSamples, int nDepthSamples, + dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness) +{ + dIASSERT(vWidth > 0.0f); + dIASSERT(vDepth > 0.0f); + dIASSERT(nWidthSamples > 0); + dIASSERT(nDepthSamples > 0); + + // x,z bounds + m_vWidth = vWidth; + m_vDepth = vDepth; + + // scale and offset + m_vScale = vScale; + m_vOffset = vOffset; + + // number of vertices per side + m_nWidthSamples = nWidthSamples; + m_nDepthSamples = nDepthSamples; + + m_vSampleWidth = m_vWidth / m_nWidthSamples; + m_vSampleDepth = m_vDepth / m_nDepthSamples; + + // infinite min height bounds + m_vThickness = vThickness; + + // finite or repeated terrain + m_nWrapMode = nWrapMode; +} + + +// recomputes heights bounds +void dxHeightfieldData::ComputeHeightBounds() +{ + static int i; + static dReal h; + static unsigned char *data_byte; + static short *data_short; + static dReal *data_float; + + 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_vMinHeight = dInfinity; + m_vMaxHeight = -dInfinity; + + for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++) + { + h = data_byte[i]; + if (h < m_vMinHeight) m_vMinHeight = h; + if (h > m_vMaxHeight) m_vMaxHeight = h; + } + + break; + + // short + case 2: + data_short = (short*)m_pHeightData; + m_vMinHeight = dInfinity; + m_vMaxHeight = -dInfinity; + + for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++) + { + h = data_short[i]; + if (h < m_vMinHeight) m_vMinHeight = h; + if (h > m_vMaxHeight) m_vMaxHeight = h; + } + + break; + + // float + case 3: + data_float = (dReal*)m_pHeightData; + m_vMinHeight = dInfinity; + m_vMaxHeight = -dInfinity; + + for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++) + { + h = data_float[i]; + if (h < m_vMinHeight) m_vMinHeight = h; + if (h > m_vMaxHeight) m_vMaxHeight = h; + } + + break; + + } + + // scale and offset + m_vMinHeight *= m_vScale; + m_vMaxHeight *= m_vScale; + m_vMinHeight += m_vOffset; + m_vMaxHeight += m_vOffset; + + // add thickness + m_vMinHeight -= m_vThickness; +} + + +// returns whether point is over terrain? +bool dxHeightfieldData::IsOnHeightfield( int nx, int nz, int w, dReal *pos ) +{ + dVector3 Min,Max; + Min[0] = nx * m_vSampleWidth; + Min[2] = nz * m_vSampleDepth; + Max[0] = (nx+1) * m_vSampleWidth; + Max[2] = (nz+1) * m_vSampleDepth; + dReal TolX = m_vSampleWidth * TERRAINTOL; + dReal TolZ = m_vSampleDepth * TERRAINTOL; + + if ((pos[0]<Min[0]-TolX) || (pos[0]>Max[0]+TolX)) return false; + if ((pos[2]<Min[2]-TolZ) || (pos[2]>Max[2]+TolZ)) return false; + + dReal dx = (pos[0] - (dReal(nx) * m_vSampleWidth)) / m_vSampleWidth; + dReal dz = (pos[2] - (dReal(nz) * m_vSampleDepth)) / m_vSampleDepth; + + if ((w == 0) && (dx + dz > 1.f+TERRAINTOL)) return false; + if ((w == 1) && (dx + dz < 1.f-TERRAINTOL)) return false; + + return true; +} + + +// returns height at given sample coordinates +dReal dxHeightfieldData::GetHeight( int x, int z ) +{ + static dReal h; + static unsigned char *data_byte; + static short *data_short; + static dReal *data_float; + + if (m_nWrapMode == 0) + { + 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 + { + x %= m_nWidthSamples; + z %= m_nDepthSamples; + if (x < 0) x+=m_nWidthSamples; + if (z < 0) z+=m_nDepthSamples; + } + + 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; + + // dReal + case 3: + data_float = (dReal*)m_pHeightData; + h = data_float[x+(z * m_nWidthSamples)]; + break; + } + + return (h * m_vScale) + m_vOffset; +} + + +// returns height at given coordinates +dReal dxHeightfieldData::GetHeight( dReal x, dReal z ) +{ + int nX = int(floor(x / m_vSampleWidth)); + int nZ = int(floor(z / m_vSampleDepth)); + dReal dx = (x - (dReal(nX) * m_vSampleWidth)) / m_vSampleWidth; + dReal dz = (z - (dReal(nZ) * m_vSampleDepth)) / m_vSampleDepth; + dIASSERT((dx >= 0.f) && (dx <= 1.f)); + dIASSERT((dz >= 0.f) && (dz <= 1.f)); + + dReal y, y0; + + if (dx + dz < 1.f) + { + 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) * (1.f - dz) + + (GetHeight(nX,nZ+1) - y0) * (1.f - dx); + } + + return y; +} + + +// dxHeightfieldData destructor +dxHeightfieldData::~dxHeightfieldData() +{ + if (m_bCopyHeightData) + { + dIASSERT(m_pHeightData); + delete [] m_pHeightData; + } +} + + +//////// dxHeightfield ///////////////////////////////////////////////////////////////// + + +// dxHeightfield constructor +dxHeightfield::dxHeightfield( dSpaceID space, + dHeightfieldDataID Data, + int bPlaceable ) : dxGeom( space, bPlaceable ) +{ + type = dHeightfieldClass; + this->Data = Data; +} + + +// compute axis aligned bounding box +void dxHeightfield::computeAABB() +{ + const dxHeightfieldData *d = Data; + + if ( d->m_nWrapMode == 0 ) + { + // Finite + if ( gflags & GEOM_PLACEABLE ) + { + dReal dx[3], dy[6], dz[3]; + + dx[0] = ( final_posr->R[ 0] * d->m_vWidth ); + dx[1] = ( final_posr->R[ 4] * d->m_vWidth ); + dx[2] = ( final_posr->R[ 8] * d->m_vWidth ); + + dy[0] = ( final_posr->R[ 1] * d->m_vMinHeight ); + dy[1] = ( final_posr->R[ 5] * d->m_vMinHeight ); + dy[2] = ( final_posr->R[ 9] * d->m_vMinHeight ); + dy[3] = ( final_posr->R[ 1] * d->m_vMaxHeight ); + dy[4] = ( final_posr->R[ 5] * d->m_vMaxHeight ); + dy[5] = ( final_posr->R[ 9] * d->m_vMaxHeight ); + + dz[0] = ( final_posr->R[ 2] * d->m_vDepth ); + dz[1] = ( final_posr->R[ 6] * d->m_vDepth ); + dz[2] = ( final_posr->R[10] * d->m_vDepth ); + + // X extents + aabb[0] = final_posr->pos[0] + + dMIN3( dMIN( REAL( 0.0 ), dx[0] ), dMIN( dy[0], dy[3] ), dMIN( REAL( 0.0 ), dz[0] ) ); + aabb[1] = final_posr->pos[0] + + dMAX3( dMAX( REAL( 0.0 ), dx[0] ), dMAX( dy[0], dy[3] ), dMAX( REAL( 0.0 ), dz[0] ) ); + + // Y extents + aabb[2] = final_posr->pos[1] + + dMIN3( dMIN( REAL( 0.0 ), dx[1] ), dMIN( dy[1], dy[4] ), dMIN( REAL( 0.0 ), dz[1] ) ); + aabb[3] = final_posr->pos[1] + + dMAX3( dMAX( REAL( 0.0 ), dx[1] ), dMAX( dy[1], dy[4] ), dMAX( REAL( 0.0 ), dz[1] ) ); + + // Z extents + aabb[4] = final_posr->pos[2] + + dMIN3( dMIN( REAL( 0.0 ), dx[2] ), dMIN( dy[2], dy[5] ), dMIN( REAL( 0.0 ), dz[2] ) ); + aabb[5] = final_posr->pos[2] + + dMAX3( dMAX( REAL( 0.0 ), dx[2] ), dMAX( dy[2], dy[5] ), dMAX( REAL( 0.0 ), dz[2] ) ); + } + else + { + aabb[0] = 0; + aabb[1] = d->m_vWidth; + aabb[2] = d->m_vMinHeight; + aabb[3] = d->m_vMaxHeight; + aabb[4] = 0; + aabb[5] = d->m_vDepth; + } + } + 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_vMinHeight; + aabb[3] = d->m_vMaxHeight; + aabb[4] = -dInfinity; + aabb[5] = dInfinity; + } + } +} + + +// dxHeightfield destructor +dxHeightfield::~dxHeightfield() +{ + // +} + + +//////// Heightfield data interface //////////////////////////////////////////////////// + + +dHeightfieldDataID dGeomHeightfieldDataCreate() +{ + return new dxHeightfieldData(); +} + + +void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d, + void *pUserData, dHeightfieldGetHeight *Callback, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ) +{ + dUASSERT( d, "argument not Heightfield data" ); + dIASSERT( Callback ); + + // callback + d->m_nGetHeightMode = 0; + d->m_pUserData = pUserData; + d->m_pGetHeightCallback = Callback; + + // set info + d->SetData(nWidthSamples, nDepthSamples, vWidth, vDepth, vScale, vOffset, nWrapMode, vThickness); + + // default bounds + d->m_vMinHeight = -dInfinity; + d->m_vMaxHeight = dInfinity; +} + + +void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d, + unsigned char *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ) +{ + dUASSERT( d, "Argument not Heightfield data" ); + dIASSERT( pHeightData ); + + // set info + d->SetData(nWidthSamples, nDepthSamples, vWidth, vDepth, vScale, vOffset, nWrapMode, vThickness); + d->m_nGetHeightMode = 1; + d->m_bCopyHeightData = bCopyHeightData; + + if (!d->m_bCopyHeightData) + { + d->m_pHeightData = pHeightData; + } + else + { + // copy data + d->m_pHeightData = new unsigned char[d->m_nWidthSamples * d->m_nDepthSamples]; + dIASSERT(d->m_pHeightData); + memcpy(d->m_pHeightData, pHeightData, sizeof(unsigned char)*d->m_nWidthSamples*d->m_nDepthSamples); + } + + // height bounds + d->ComputeHeightBounds(); +} + + +void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d, + short *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ) +{ + dUASSERT(d, "argument not Heightfield data"); + dIASSERT(pHeightData); + + // set info + d->SetData(nWidthSamples, nDepthSamples, vWidth, vDepth, vScale, vOffset, nWrapMode, vThickness); + d->m_nGetHeightMode = 2; + d->m_bCopyHeightData = bCopyHeightData; + if (!d->m_bCopyHeightData) + { + d->m_pHeightData = pHeightData; + } + else + { + // copy data + d->m_pHeightData = new short[nWidthSamples * nDepthSamples]; + dIASSERT(d->m_pHeightData); + memcpy(d->m_pHeightData, pHeightData, sizeof(short)*nWidthSamples*nDepthSamples); + } + + // height bounds + d->ComputeHeightBounds(); +} + + +void dGeomHeightfieldDataBuildFloat( dHeightfieldDataID d, + dReal *pHeightData, int bCopyHeightData, + int nWidthSamples, int nDepthSamples, dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int nWrapMode, dReal vThickness ) +{ + dUASSERT(d, "argument not Heightfield data"); + dIASSERT(pHeightData); + + // set info + d->SetData(nWidthSamples, nDepthSamples, vWidth, vDepth, vScale, vOffset, nWrapMode, vThickness); + d->m_nGetHeightMode = 3; + d->m_bCopyHeightData = bCopyHeightData; + + if (!d->m_bCopyHeightData) + { + d->m_pHeightData = pHeightData; + } + else + { + // copy data + d->m_pHeightData = new dReal[nWidthSamples * nDepthSamples]; + dIASSERT(d->m_pHeightData); + memcpy(d->m_pHeightData, pHeightData, sizeof(dReal)*nWidthSamples*nDepthSamples); + } + + // height bounds + d->ComputeHeightBounds(); +} + + +void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal vMinHeight, dReal vMaxHeight ) +{ + dUASSERT(d, "argument not Heightfield data"); + d->m_vMinHeight = (vMinHeight * d->m_vScale) + d->m_vOffset - d->m_vThickness; + d->m_vMaxHeight = (vMaxHeight * d->m_vScale) + d->m_vOffset; +} + + +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 Data ) +{ + dxHeightfield* Geom = (dxHeightfield*) g; + Geom->Data = Data; +} + + +dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g ) +{ + dxHeightfield* Geom = (dxHeightfield*) g; + return Geom->Data; +} + + +dReal dGeomHeightfieldPointDepth( dGeomID g, dReal x, dReal y, dReal z ) +{ + dUASSERT (g && g->type == dHeightfieldClass,"argument not a dHeightfield"); + dxHeightfield *t = (dxHeightfield*) g; + return t->Data->GetHeight(x,z) - y; +} + + +//////// dxHeightfield ///////////////////////////////////////////////////////////////// + + +typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z); +#define RECOMPUTE_RAYNORMAL +//#define DO_RAYDEPTH + +#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]); +/* +(y is up) + +A-B-E.x +|/| +C-D +| +F +. +z +*/ +int dxHeightfield::dCollideHeightfieldUnit(int x, int z, dxGeom *o2, int numMaxContacts, + int flags, dContactGeom *contact, int skip) +{ + dColliderFn *CollideRayN; + dColliderFn *CollideNPlane; + dGetDepthFn *GetDepth; + int numContacts = 0; + int numPlaneContacts = 0; + int i; + + if (numContacts == numMaxContacts) return numContacts; + + dContactGeom PlaneContact[MAXCONTACT]; + flags = (flags & 0xffff0000) | MAXCONTACT; + + switch (o2->type) + { + case dRayClass: + CollideRayN = NULL; + CollideNPlane = dCollideRayPlane; + GetDepth = NULL; + break; + case dSphereClass: + CollideRayN = dCollideRaySphere; + CollideNPlane = dCollideSpherePlane; + GetDepth = dGeomSpherePointDepth; + break; + case dBoxClass: + CollideRayN = dCollideRayBox; + CollideNPlane = dCollideBoxPlane; + GetDepth = dGeomBoxPointDepth; + break; + case dCapsuleClass: + CollideRayN = dCollideRayCapsule; + CollideNPlane = dCollideCapsulePlane; + GetDepth = dGeomCapsulePointDepth; + break; +/* case dCylinderClass: + CollideRayN = dCollideRayCylinder; + CollideNPlane = dCollideCylinderPlane; + GetDepth = dGeomCylinderPointDepth; + break;*/ +/* case dConvexClass: + CollideRayN = dCollideRayConvex; + CollideNPlane = dCollideConvexPlane; + GetDepth = dGeomConvexPointDepth; + break;*/ +/* case dConeClass: + CollideRayN = dCollideRayCone; + CollideNPlane = dCollideConePlane; + GetDepth = dGeomConePointDepth; + break;*/ + default: + dIASSERT(0); + } + + dReal Plane[4],lBD,lCD,lBC; + dVector3 A,B,C,D,BD,CD,BC,AB,AC; + A[0] = x * Data->m_vSampleWidth; + A[2] = z * Data->m_vSampleDepth; + A[1] = Data->GetHeight(x,z); + + B[0] = (x+1) * Data->m_vSampleWidth; + B[2] = z * Data->m_vSampleDepth; + B[1] = Data->GetHeight(x+1,z); + + C[0] = x * Data->m_vSampleWidth; + C[2] = (z+1) * Data->m_vSampleDepth; + C[1] = Data->GetHeight(x,z+1); + + D[0] = (x+1) * Data->m_vSampleWidth; + D[2] = (z+1) * Data->m_vSampleDepth; + D[1] = Data->GetHeight(x+1,z+1); + + dOP(BC,-,C,B); + lBC = dLENGTH(BC); + dOPEC(BC,/=,lBC); + + dOP(BD,-,D,B); + lBD = dLENGTH(BD); + dOPEC(BD,/=,lBD); + + dOP(CD,-,D,C); + lCD = dLENGTH(CD); + dOPEC(CD,/=,lCD); + + dOP(AB,-,B,A); + dNormalize3(AB); + + dOP(AC,-,C,A); + dNormalize3(AC); + + if (CollideRayN) + { +#ifdef RECOMPUTE_RAYNORMAL + dVector3 E,F; + dVector3 CE,FB,AD; + dVector3 Normal[3]; + E[0] = (x+2) * Data->m_vSampleWidth; + E[2] = z * Data->m_vSampleDepth; + E[1] = Data->GetHeight(x+2,z); + F[0] = x * Data->m_vSampleWidth; + F[2] = (z+2) * Data->m_vSampleDepth; + F[1] = Data->GetHeight(x,z+2); + dOP(AD,-,D,A); + dNormalize3(AD); + dOP(CE,-,E,C); + dNormalize3(CE); + dOP(FB,-,B,F); + dNormalize3(FB); + + //BC + dCROSS(Normal[0],=,BC,AD); + dNormalize3(Normal[0]); + + //BD + dCROSS(Normal[1],=,BD,CE); + dNormalize3(Normal[1]); + + //CD + dCROSS(Normal[2],=,CD,FB); + dNormalize3(Normal[2]); +#endif + int nA[3],nB[3]; + dContactGeom ContactA[3],ContactB[3]; + dxRay rayBC(0,lBC); + dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]); + nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom)); + dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]); + nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom)); + + dxRay rayBD(0,lBD); + dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]); + nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom)); + dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]); + nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom)); + + dxRay rayCD(0,lCD); + dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]); + nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom)); + dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]); + nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom)); + + for (i=0;i<3;i++) + { + if (nA[i] & nB[i]) + { + dContactGeom *pContact = CONTACT(contact,numContacts*skip); + pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2; + pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2; + pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2; +#ifdef RECOMPUTE_RAYNORMAL + pContact->normal[0] = -Normal[i][0]; + pContact->normal[1] = -Normal[i][1]; + pContact->normal[2] = -Normal[i][2]; +#else + pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f; + pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f; + pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f; + dNormalize3(pContact->normal); +#endif +#ifdef DO_RAYDEPTH + dxRay rayV(0,1000.f); + dGeomRaySet(&rayV, pContact->pos[0], + pContact->pos[1], + pContact->pos[2], + -pContact->normal[0], + -pContact->normal[1], + -pContact->normal[2]); + + dContactGeom ContactV; + if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom))) + { + pContact->depth = ContactV.depth; + numContacts++; + } +#else + pContact->depth = GetDepth(o2, + pContact->pos[0], + pContact->pos[1], + pContact->pos[2]); + numContacts++; +#endif + if (numContacts == numMaxContacts) return numContacts; + + } + } + } + + dCROSS(Plane,=,AC,AB); + dNormalize3(Plane); + Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2]; + dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;i<numPlaneContacts;i++) + { + if (Data->IsOnHeightfield(x,z,0,PlaneContact[i].pos)) + { + dContactGeom *pContact = CONTACT(contact,numContacts*skip); + pContact->pos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + + //DMESS(0); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + dCROSS(Plane,=,BD,CD); + dNormalize3(Plane); + Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2]; + dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]); + numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom)); + + for (i=0;i<numPlaneContacts;i++) + { + if (Data->IsOnHeightfield(x,z,1,PlaneContact[i].pos)) + { + dContactGeom *pContact = CONTACT(contact,numContacts*skip); + pContact->pos[0] = PlaneContact[i].pos[0]; + pContact->pos[1] = PlaneContact[i].pos[1]; + pContact->pos[2] = PlaneContact[i].pos[2]; + pContact->normal[0] = -PlaneContact[i].normal[0]; + pContact->normal[1] = -PlaneContact[i].normal[1]; + pContact->normal[2] = -PlaneContact[i].normal[2]; + pContact->depth = PlaneContact[i].depth; + //DMESS(1); + numContacts++; + + if (numContacts == numMaxContacts) + return numContacts; + } + } + + return numContacts; +} + + +int dCollideHeightfield(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (o1->type == dHeightfieldClass); + int i,j; + + if ((flags & 0xffff) == 0) + flags = (flags & 0xffff0000) | 1; + + int numMaxTerrainContacts = (flags & 0xffff); + dxHeightfield *terrain = (dxHeightfield*) o1; + + dVector3 posbak; + dMatrix3 Rbak; + dReal aabbbak[6]; + int gflagsbak; + + dVector3 pos0,pos1; + dMatrix3 R1; + int numTerrainContacts = 0; + + if (terrain->gflags & GEOM_PLACEABLE) + { + dOP(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); + dVector3Copy(o2->final_posr->pos,posbak); + dMatrix3Copy(o2->final_posr->R,Rbak); + dVector3Copy(pos1,o2->final_posr->pos); + dMatrix3Copy(R1,o2->final_posr->R); + memcpy(aabbbak,o2->aabb,sizeof(dReal)*6); + gflagsbak = o2->gflags; + o2->computeAABB(); + } + + int nMinX = int(floor(o2->aabb[0] / terrain->Data->m_vSampleWidth)); + int nMaxX = int(floor(o2->aabb[1] / terrain->Data->m_vSampleWidth)) + 1; + int nMinZ = int(floor(o2->aabb[4] / terrain->Data->m_vSampleDepth)); + int nMaxZ = int(floor(o2->aabb[5] / terrain->Data->m_vSampleDepth)) + 1; + + + if (terrain->Data->m_nWrapMode == 0) + { + nMinX = dMAX(nMinX,0); + nMaxX = dMIN(nMaxX,terrain->Data->m_nWidthSamples); + nMinZ = dMAX(nMinZ,0); + nMaxZ = dMIN(nMaxZ,terrain->Data->m_nDepthSamples); + + if ((nMinX >= nMaxX) || (nMinZ >= nMaxZ)) goto dCollideHeightfieldExit; + } + + + dVector3 AabbTop; + AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2; + AabbTop[2] = (o2->aabb[4]+o2->aabb[5]) / 2; + AabbTop[1] = o2->aabb[3]; + if (o2->type != dRayClass) + { + dReal AabbTopDepth = terrain->Data->GetHeight(AabbTop[0],AabbTop[2]) - AabbTop[1]; + if (AabbTopDepth > 0.f) + { + contact->depth = AabbTopDepth; + dReal MaxDepth = (o2->aabb[3]-o2->aabb[2]) / 2; + if (contact->depth > MaxDepth) contact->depth = MaxDepth; + contact->g1 = o1; + contact->g2 = o2; + dOPE(contact->pos,=,AabbTop); + contact->normal[0] = 0.f; + contact->normal[1] = -1.f; + contact->normal[2] = 0.f; + + numTerrainContacts = 1; + goto dCollideHeightfieldExit; + } + } + + for (i=nMinX;i<nMaxX;i++) + { + for (j=nMinZ;j<nMaxZ;j++) + { + numTerrainContacts += terrain->dCollideHeightfieldUnit( + i,j,o2,numMaxTerrainContacts - numTerrainContacts, + flags,CONTACT(contact,numTerrainContacts*skip),skip ); + } + } + + dIASSERT(numTerrainContacts <= numMaxTerrainContacts); + + for (i=0; i<numTerrainContacts; i++) + { + CONTACT(contact,i*skip)->g1 = o1; + CONTACT(contact,i*skip)->g2 = o2; + } + +dCollideHeightfieldExit: + + if (terrain->gflags & GEOM_PLACEABLE) + { + dVector3Copy(posbak,o2->final_posr->pos); + dMatrix3Copy(Rbak,o2->final_posr->R); + memcpy(o2->aabb,aabbbak,sizeof(dReal)*6); + o2->gflags = gflagsbak; + + for (i=0; i<numTerrainContacts; i++) + { + dOPE(pos0,=,CONTACT(contact,i*skip)->pos); + dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0); + dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos); + + dOPE(pos0,=,CONTACT(contact,i*skip)->normal); + dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0); + } + } + + return numTerrainContacts; +} + + Added: trunk/ode/src/heightfield.h =================================================================== --- trunk/ode/src/heightfield.h (rev 0) +++ trunk/ode/src/heightfield.h 2006-07-06 13:29:04 UTC (rev 1003) @@ -0,0 +1,63 @@ +// dHeightfield Collider +// Martijn Buijs 2006 http://home.planet.nl/~buijs512/ +// Based on Terrain & Cone contrib by: +// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com + +#ifndef _DHEIGHTFIELD_H_ +#define _DHEIGHTFIELD_H_ + +#include <ode/common.h> +#include "collision_kernel.h" + +// Heightfield Data structure +struct dxHeightfieldData +{ + dReal m_vWidth; // world space heightfield dimension on X axis + dReal m_vDepth; // world space heightfield dimension on Z axis + dReal m_vMinHeight; // min sample height value (scaled and offset) + dReal m_vMaxHeight; // max sample height value (scaled and offset) + dReal m_vSampleWidth; // sample spacing on X axis (== m_vWidth / m_nWidthSamples) + dReal m_vSampleDepth; // sample spacing on Z axis (== m_vDepth / m_nDepthSamples) + dReal m_vThickness; // surface thickness (currently only added to bottom AABB) + dReal m_vScale; // sample value multiplier + dReal m_vOffset; // vertical sample offset + int m_nWidthSamples; // number of samples on X axis + int m_nDepthSamples; // number of samples on Z axis + int m_bCopyHeightData; // copy sample data flag + int m_nWrapMode; // heightfield wrapping mode (0=finite, 1=infinite) + int m_nGetHeightMode; // getheight mode (0=callback, 1=byte, 2=short, 3=float) + void *m_pHeightData; // sample data array + void *m_pUserData; // callback user data + + dHeightfieldGetHeight* m_pGetHeightCallback; + + dxHeightfieldData(); + ~dxHeightfieldData(); + + void SetData( int nWidthSamples, int nDepthSamples, + dReal vWidth, dReal vDepth, + dReal vScale, dReal vOffset, + int bFinite, dReal vThickness ); + + void ComputeHeightBounds(); + + bool IsOnHeightfield(int nx, int nz, int w, dReal *pos); + dReal GetHeight(int x, int z); + dReal GetHeight(dReal x, dReal z); +}; + + +// Heightfield geom structure +struct dxHeightfield : public dxGeom +{ + dxHeightfieldData *Data; + + dxHeightfield( dSpaceID space, dHeightfieldDataID Data, int bPlaceable ); + ~dxHeightfield(); + + void computeAABB(); + int dCollideHeightfieldUnit( int x, int z, dxGeom *o2, int numMaxContacts, int flags, dContactGeom *contact, int skip ); +}; + + +#endif //_DHEIGHTFIELD_H_ Added: trunk/ode/test/test_heightfield.cpp =================================================================== --- trunk/ode/test/test_heightfield.cpp (rev 0) +++ trunk/ode/test/test_heightfield.cpp 2006-07-06 13:29:04 UTC (rev 1003) @@ -0,0 +1,574 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * + * All rights reserved. Email: ru...@q1... 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. * + * * + *************************************************************************/ + +#include <ode/ode.h> +#include <drawstuff/drawstuff.h> + +#ifdef _MSC_VER +#pragma warning(disable:4244 4305) // for VC++, no precision loss complaints +#endif + +#define DEGTORAD 0.01745329251994329577f //!< PI / 180.0, convert degrees to radians + +dGeomID gheight; + +//<---- Convex Object +dReal planes[]= // planes for a cube + { + 1.0f ,0.0f ,0.0f ,0.25f, + 0.0f ,1.0f ,0.0f ,0.25f, + 0.0f ,0.0f ,1.0f ,0.25f, + 0.0f ,0.0f ,-1.0f,0.25f, + 0.0f ,-1.0f,0.0f ,0.25f, + -1.0f,0.0f ,0.0f ,0.25f + /* + 1.0f ,0.0f ,0.0f ,2.0f, + 0.0f ,1.0f ,0.0f ,1.0f, + 0.0f ,0.0f ,1.0f ,1.0f, + 0.0f ,0.0f ,-1.0f,1.0f, + 0.0f ,-1.0f,0.0f ,1.0f, + -1.0f,0.0f ,0.0f ,0.0f + */ + }; +const unsigned int planecount=6; + +dReal points[]= // points for a cube + { + 0.25f,0.25f,0.25f, // point 0 + -0.25f,0.25f,0.25f, // point 1 + + 0.25f,-0.25f,0.25f, // point 2 + -0.25f,-0.25f,0.25f,// point 3 + + 0.25f,0.25f,-0.25f, // point 4 + -0.25f,0.25f,-0.25f,// point 5 + + 0.25f,-0.25f,-0.25f,// point 6 + -0.25f,-0.25f,-0.25f,// point 7 + }; +const unsigned int pointcount=8; +unsigned int polygons[] = //Polygons for a cube (6 squares) + { + 4,0,2,6,4, // positive X + 4,1,0,4,5, // positive Y + 4,0,1,3,2, // positive Z + 4,3,1,5,7, // negative X + 4,2,3,7,6, // negative Y + 4,5,4,6,7, // negative Z + }; +//----> Convex Object + +// select correct drawing functions + +#ifdef dDOUBLE +#define dsDrawBox dsDrawBoxD +#define dsDrawSphere dsDrawSphereD +#define dsDrawCylinder dsDrawCylinderD +#define dsDrawCapsule dsDrawCapsuleD +#define dsDrawConvex dsDrawConvexD +#endif + + +// some constants + +#define NUM 100 // max number of objects +#define DENSITY (5.0) // density of all objects +#define GPB 3 // maximum number of geometries per body +#define MAX_CONTACTS 8 // maximum number of contact points per body + + +// dynamics and collision objects + +struct MyObject { + dBodyID body; // the body + dGeomID geom[GPB]; // geometries representing this body +}; + +static int num=0; // number of objects in simulation +static int nextobj=0; // next object to recycle if num==NUM +static dWorldID world; +static dSpaceID space; +static MyObject obj[NUM]; +static dJointGroupID contactgroup; +static int selected = -1; // selected object +static int show_aabb = 0; // show geom AABBs? +static int show_contacts = 0; // show contact points? +static int random_pos = 1; // drop objects from random position? +static int write_world = 0; + + + +#define HFIELD_WSTEP 32 +#define HFIELD_DSTEP 32 + +#define HFIELD_WIDTH REAL( 16.0 ) +#define HFIELD_DEPTH REAL( 16.0 ) + +dReal heightfield_callback( void* pUserData, int x, int z ) +{ + dReal fx = ( ((dReal)x) - HFIELD_WSTEP/2 ) / (dReal)HFIELD_WSTEP; + dReal fz = ( ((dReal)z) - HFIELD_DSTEP/2 ) / (dReal)HFIELD_DSTEP; + + // Create an interesting 'bowl' shape + dReal h = ( REAL( 64.0 ) * ( fx*fx + fz*fz ) ); + + return h; +} + + + + +// this is called by dSpaceCollide when two objects in space are +// potentially colliding. + +static void nearCallback (void *data, dGeomID o1, dGeomID o2) +{ + int i; + // if (o1->body && o2->body) return; + + // exit without doing anything if the two bodies are connected by a joint + dBodyID b1 = dGeomGetBody(o1); + dBodyID b2 = dGeomGetBody(o2); + if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return; + + dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box + for (i=0; i<MAX_CONTACTS; i++) { + contact[i].surface.mode = dContactBounce | dContactSoftCFM; + contact[i].surface.mu = dInfinity; + contact[i].surface.mu2 = 0; + contact[i].surface.bounce = 0.1; + contact[i].surface.bounce_vel = 0.1; + contact[i].surface.soft_cfm = 0.01; + } + if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom, + sizeof(dContact))) { + dMatrix3 RI; + dRSetIdentity (RI); + const dReal ss[3] = {0.02,0.02,0.02}; + for (i=0; i<numc; i++) { + dJointID c = dJointCreateContact (world,contactgroup,contact+i); + dJointAttach (c,b1,b2); + if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss); + } + } +} + + +// start simulation - set viewpoint + +static void start() +{ + static float xyz[3] = {2.1640f,-1.3079f,1.7600f}; + static float hpr[3] = {125.5000f,-17.0000f,0.0000f}; + dsSetViewpoint (xyz,hpr); + printf ("To drop another object, press:\n"); + printf (" b for box.\n"); + printf (" s for sphere.\n"); + printf (" c for capsule.\n"); + printf (" y for cylinder.\n"); + printf (" v for a convex object.\n"); + printf (" x for a composite object.\n"); + printf ("To select an object, press space.\n"); + printf ("To disable the selected object, press d.\n"); + printf ("To enable the selected object, press e.\n"); + printf ("To toggle showing the geom AABBs, press a.\n"); + printf ("To toggle showing the contact points, press t.\n"); + printf ("To toggle dropping from random position/orientation, press r.\n"); + printf ("To save the current state to 'state.dif', press 1.\n"); +} + + +char locase (char c) +{ + if (c >= 'A' && c <= 'Z') return c - ('a'-'A'); + else return c; +} + + +// called when a key pressed + +static void command (int cmd) +{ + size_t i; + int j,k; + dReal sides[3]; + dMass m; + + cmd = locase (cmd); + if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') + { + if (num < NUM) { + i = num; + num++; + } + else { + i = nextobj; + nextobj++; + if (nextobj >= num) nextobj = 0; + + // destroy the body and geoms for slot i + dBodyDestroy (obj[i].body); + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); + } + memset (&obj[i],0,sizeof(obj[i])); + } + + obj[i].body = dBodyCreate (world); + for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; + + dMatrix3 R; + if (random_pos) { + dBodySetPosition (obj[i].body, + (dRandReal()*HFIELD_WIDTH*0.125), + (dRandReal()*HFIELD_DEPTH*0.125), + dRandReal() + 4 ); + dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + } + else { + dReal maxheight = 0; + for (k=0; k<num; k++) { + const dReal *pos = dBodyGetPosition (obj[k].body); + if (pos[2] > maxheight) maxheight = pos[2]; + } + dBodySetPosition (obj[i].body, 0,maxheight+1,0); + dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); + } + dBodySetRotation (obj[i].body,R); + dBodySetData (obj[i].body,(void*) i); + + if (cmd == 'b') { + dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); + obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); + } + else if (cmd == 'c') { + sides[0] *= 0.5; + dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); + } + //<---- Convex Object + else if (cmd == 'v') + { + dMassSetBox (&m,DENSITY,0.25,0.25,0.25); + obj[i].geom[0] = dCreateConvex (space, + planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + else if (cmd == 'y') { + dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); + obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); + } + else if (cmd == 's') { + sides[0] *= 0.5; + dMassSetSphere (&m,DENSITY,sides[0]); + obj[i].geom[0] = dCreateSphere (space,sides[0]); + } + else if (cmd == 'x') { + dGeomID g2[GPB]; // encapsulated geometries + dReal dpos[GPB][3]; // delta-positions for encapsulated geometries + + // start accumulating masses for the encapsulated geometries + dMass m2; + dMassSetZero (&m); + + // set random delta positions + for (j=0; j<GPB; j++) { + for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; + } + + for (k=0; k<GPB; k++) { + obj[i].geom[k] = dCreateGeomTransform (space); + dGeomTransformSetCleanup (obj[i].geom[k],1); + if (k==0) { + dReal radius = dRandReal()*0.25+0.05; + g2[k] = dCreateSphere (0,radius); + dMassSetSphere (&m2,DENSITY,radius); + } + else if (k==1) { + g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); + dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); + } + else { + dReal radius = dRandReal()*0.1+0.05; + dReal length = dRandReal()*1.0+0.1; + g2[k] = dCreateCapsule (0,radius,length); + dMassSetCapsule (&m2,DENSITY,3,radius,length); + } + dGeomTransformSetGeom (obj[i].geom[k],g2[k]); + + // set the transformation (adjust the mass too) + dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); + dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); + dMatrix3 Rtx; + dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, + dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); + dGeomSetRotation (g2[k],Rtx); + dMassRotate (&m2,Rtx); + + // add to the total mass + dMassAdd (&m,&m2); + } + + // move all encapsulated objects so that the center of mass is (0,0,0) + for (k=0; k<2; k++) { + dGeomSetPosition (g2[k], + dpos[k][0]-m.c[0], + dpos[k][1]-m.c[1], + dpos[k][2]-m.c[2]); + } + dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); + } + + for (k=0; k < GPB; k++) { + if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); + } + + dBodySetMass (obj[i].body,&m); + } + + if (cmd == ' ') { + selected++; + if (selected >= num) selected = 0; + if (selected < 0) selected = 0; + } + else if (cmd == 'd' && selected >= 0 && selected < num) { + dBodyDisable (obj[selected].body); + } + else if (cmd == 'e' && selected >= 0 && selected < num) { + dBodyEnable (obj[selected].body); + } + else if (cmd == 'a') { + show_aabb ^= 1; + } + else if (cmd == 't') { + show_contacts ^= 1; + } + else if (cmd == 'r') { + random_pos ^= 1; + } + else if (cmd == '1') { + write_world = 1; + } +} + + +// draw a geom + +void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb) +{ + int i; + + if (!g) return; + if (!pos) pos = dGeomGetPosition (g); + if (!R) R = dGeomGetRotation (g); + + int type = dGeomGetClass (g); + if (type == dBoxClass) { + dVector3 sides; + dGeomBoxGetLengths (g,sides); + dsDrawBox (pos,R,sides); + } + else if (type == dSphereClass) { + dsDrawSphere (pos,R,dGeomSphereGetRadius (g)); + } + else if (type == dCapsuleClass) { + dReal radius,length; + dGeomCapsuleGetParams (g,&radius,&length); + dsDrawCapsule (pos,R,length,radius); + } + //<---- Convex Object + else if (type == dConvexClass) + { + //dVector3 sides={0.50,0.50,0.50}; + dsDrawConvex(pos,R,planes, + planecount, + points, + pointcount, + polygons); + } + //----> Convex Object + else if (type == dCylinderClass) { + dReal radius,length; + dGeomCylinderGetParams (g,&radius,&length); + dsDrawCylinder (pos,R,length,radius); + } + else if (type == dGeomTransformClass) { + dGeomID g2 = dGeomTransformGetGeom (g); + const dReal *pos2 = dGeomGetPosition (g2); + const dReal *R2 = dGeomGetRotation (g2); + dVector3 actual_pos; + dMatrix3 actual_R; + dMULTIPLY0_331 (actual_pos,R,pos2); + actual_pos[0] += pos[0]; + actual_pos[1] += pos[1]; + actual_pos[2] += pos[2]; + dMULTIPLY0_333 (actual_R,R,R2); + drawGeom (g2,actual_pos,actual_R,0); + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (g,aabb); + dVector3 bbpos; + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } + +} + + +// simulation loop + +static void simLoop (int pause) +{ + dsSetColor (0,0,2); + dSpaceCollide (space,0,&nearCallback); + if (!pause) dWorldQuickStep (world,0.02); + + if (write_world) { + FILE *f = fopen ("state.dif","wt"); + if (f) { + dWorldExportDIF (world,f,"X"); + fclose (f); + } + write_world = 0; + } + + // remove all contact joints + dJointGroupEmpty (contactgroup); + + dsSetColor (1,1,0); + dsSetTexture (DS_WOOD); + for (int i=0; i<num; i++) { + for (int j=0; j < GPB; j++) { + if (i==selected) { + dsSetColor (0,0.7,1); + } + else if (! dBodyIsEnabled (obj[i].body)) { + dsSetColor (1,0.8,0); + } + else { + dsSetColor (1,1,0); + } + drawGeom (obj[i].geom[j],0,0,show_aabb); + } + } + + if (show_aabb) { + // draw the bounding box for this geom + dReal aabb[6]; + dGeomGetAABB (gheight,aabb); + dVector3 bbpos; + for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]); + dVector3 bbsides; + for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2]; + dMatrix3 RI; + dRSetIdentity (RI); + dsSetColorAlpha (1,0,0,0.5); + dsDrawBox (bbpos,RI,bbsides); + } +} + + +int main (int argc, char **argv) +{ + // setup pointers to drawstuff callback functions + dsFunctions fn; + fn.version = DS_VERSION; + fn.start = &start; + fn.step = &simLoop; + fn.command = &command; + fn.stop = 0; + fn.path_to_textures = "../../drawstuff/textures"; + if(argc==2) + { + fn.path_to_textures = argv[1]; + } + + // create world + + world = dWorldCreate(); + space = dHashSpaceCreate (0); + contactgroup = dJointGroupCreate (0); + dWorldSetGravity (world,0,0,-0.5); + dWorldSetCFM (world,1e-5); + dWorldSetAutoDisableFlag (world,1); + dWorldSetContactMaxCorrectingVel (world,0.1); + dWorldSetContactSurfaceLayer (world,0.001); + memset (obj,0,sizeof(obj)); + + + + + // our heightfield floor + + dHeightfieldDataID heightid = dGeomHeightfieldDataCreate(); + + // Create a single non repeating heightfield. + dGeomHeightfieldDataBuildCallback( heightid, NULL, heightfield_callback, + HFIELD_WSTEP, HFIELD_DSTEP, HFIELD_WIDTH, HFIELD_DEPTH, + REAL( 1.0 ), REAL( 0.0 ), 0, REAL( 10.0 ) ); + + // Give some very bounds which, while conservative, + // makes AABB computation more accurate than +/-INF. + dGeomHeightfieldDataSetBounds( heightid, REAL( -100.0 ), REAL( 100.0 ) ); + + + gheight = dCreateHeightfield( space, heightid, 1 ); + + dVector3 pos; + pos[ 0 ] = -HFIELD_WIDTH / 2; + pos[ 1 ] = HFIELD_DEPTH / 2; + pos[ 2 ] = 0; + + // Rotate so Z is up, not Y (which is the default orientation) + dMatrix3 R; + dRSetIdentity( R ); + dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 ); + + // Place it. + dGeomSetRotation( gheight, R ); + dGeomSetPosition( gheight, pos[0], ... [truncated message content] |
