dterraincallback.cpp

赫赫大名的 OGRE 游戏引擎

// TERRAINGEOMETRY - 

// Modified Terrain collider, by Ed Jones

// Original by;
//Benoit CHAPEROT 2003-2004 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_std_internal.h"

struct dxSphere : public dxGeom {
  dReal radius;		// sphere radius
  dxSphere (dSpaceID space, dReal _radius);
  void computeAABB();
};


struct dxBox : public dxGeom {
  dVector3 side;	// side lengths (x,y,z)
  dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
  void computeAABB();
};


struct dxCCylinder : public dxGeom {
  dReal radius,lz;	// radius, length along z axis
  dxCCylinder (dSpaceID space, dReal _radius, dReal _length);
  void computeAABB();
};

struct dxPlane : public dxGeom {
  dReal p[4];
  dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
  void computeAABB();
};

struct dxCylinder : public dxGeom {
  dReal radius,lz;	// radius, length along z axis
  dxCylinder (dSpaceID space, dReal _radius, dReal _length);
  void computeAABB();
};

struct dxRay : public dxGeom {
  dReal length;
  dxRay (dSpaceID space, dReal _length);
  void computeAABB();
};

struct dxTerrainCallback : public dxGeom {
  dReal m_vLength;
  dReal *m_pHeights;
  dReal m_vMinHeight;
  dReal m_vMaxHeight;
  dReal m_vNodeLength;
  int	m_nNumNodesPerSide;
  int	m_nNumNodesPerSideShift;
  int	m_nNumNodesPerSideMask;
  int	m_bFinite;
  dTerrainHeightCallback* m_pHeightCallback;  
  dxTerrainCallback(dSpaceID space, dReal *pHeights,dReal vHeight,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
  ~dxTerrainCallback();
  void computeAABB();
  dReal GetHeight(dReal x,dReal z);
  dReal GetHeight(int x,int z);
  int dCollideTerrainUnit(int x,int z,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip);
  bool IsOnTerrain(int nx,int nz,int w,dReal *pos);
};
#ifndef MIN
#define MIN(a,b)	((a<b)?a:b)
#endif

#ifndef MAX
#define MAX(a,b)	((a>b)?a:b)
#endif

#include "collision_util.h"
#include "windows.h"
#include "ode\ode.h"

#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
#define MAXCONTACT 10
#define TERRAINTOL 0.0f

static bool IsAPowerOfTwo(int f)
{
	dAASSERT(f!=0);
	while ((f&1) != 1)	
		f >>= 1;

	return (f == 1);
}

static int GetPowerOfTwo(int f)
{
	dAASSERT(f!=0);
	int n = 0;
	while ((f&1) != 1)
	{
		n++;
		f >>= 1;
	}
	
	return n;
}

dxTerrainCallback::dxTerrainCallback (dSpaceID space, dReal *pHeights,dReal vHeight,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
dxGeom (space,bPlaceable)
{
//	dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
//	dIASSERT(pHeights);
	dIASSERT(vLength > 0.f);
	dIASSERT(nNumNodesPerSide > 0);
	type = dTerrainCallbackClass;
	m_vLength = vLength;
	m_pHeights = 0;
	if(pHeights)
    {
    	dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
        m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
	   dIASSERT(m_pHeights);
    }
	m_nNumNodesPerSide = nNumNodesPerSide;
	m_vNodeLength = m_vLength / m_nNumNodesPerSide;
	m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
	m_nNumNodesPerSideMask  = m_nNumNodesPerSide - 1;
	m_vMinHeight = dInfinity;
	m_vMaxHeight = -dInfinity;
	m_bFinite = bFinite;

    if((m_pHeights)&&(pHeights))
    {
        for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
	   {
	       m_pHeights[i] = pHeights[i];
    	   if (m_pHeights[i] < m_vMinHeight)	m_vMinHeight = m_pHeights[i];
	   	   if (m_pHeights[i] > m_vMaxHeight)	m_vMaxHeight = m_pHeights[i];
	   }
    }
    else
    {
        m_vMinHeight = 0.0f;
	   	m_vMaxHeight = vHeight;
    }
	
	m_pHeightCallback = 0;
}

dxTerrainCallback::~dxTerrainCallback()
{
// 	dIASSERT(m_pHeights);
	delete [] m_pHeights;
}

void dxTerrainCallback::computeAABB()
{
	if (m_bFinite)
	{
		if (gflags & GEOM_PLACEABLE)
		{
			dReal dx[6],dy[6],dz[6];
			dx[0] = 0;
			dx[1] = R[0] * m_vLength;
			dx[2] = R[1] * m_vMinHeight;
			dx[3] = R[1] * m_vMaxHeight;
			dx[4] = 0;
			dx[5] = R[2] * m_vLength;

			dy[0] = 0;
			dy[1] = R[4] * m_vLength;
			dy[2] = R[5] * m_vMinHeight;
			dy[3] = R[5] * m_vMaxHeight;
			dy[4] = 0;
			dy[5] = R[6] * m_vLength;

			dz[0]  = 0;
			dz[1]  = R[8] * m_vLength;
			dz[2]  = R[9] * m_vMinHeight;
			dz[3]  = R[9] * m_vMaxHeight;
			dz[4]  = 0;
			dz[5]  = R[10] * m_vLength;

			aabb[0] = pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
			aabb[1] = pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
			aabb[2] = pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
			aabb[3] = pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
			aabb[4] = pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
			aabb[5] = pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
		}
		else
		{
			aabb[0] = 0;
			aabb[1] = m_vLength;
			aabb[2] = m_vMinHeight;
			aabb[3] = m_vMaxHeight;
			aabb[4] = 0;
			aabb[5] = m_vLength;
		}
	}
	else
	{
		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] = m_vMinHeight;
			aabb[3] = m_vMaxHeight;
			aabb[4] = -dInfinity;
			aabb[5] = dInfinity;
		}
	}
}

dReal dxTerrainCallback::GetHeight(int x,int z)
{
    if(m_pHeightCallback)
    {
        return GetHeight((dReal)x * m_vNodeLength,(dReal)z * m_vNodeLength);
    }
    else
    {
	   return m_pHeights[	(((unsigned int)(z) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
	       				+	 ((unsigned int)(x) & m_nNumNodesPerSideMask)];
    }
}

dReal dxTerrainCallback::GetHeight(dReal x,dReal z)
{
    if(m_pHeightCallback)
    {
        return (*m_pHeightCallback)(data,x,z);
    }
    else
    {
	int nX		= int(floor(x / m_vNodeLength));
	int nZ		= int(floor(z / m_vNodeLength));
	dReal dx	= (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
	dReal dz	= (z - (dReal(nZ) * m_vNodeLength)) / m_vNodeLength;
	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;	
    }
}

bool dxTerrainCallback::IsOnTerrain(int nx,int nz,int w,dReal *pos)
{
	dVector3 Min,Max;
	Min[0] = nx * m_vNodeLength;
	Min[2] = nz * m_vNodeLength;
	Max[0] = (nx+1) * m_vNodeLength;
	Max[2] = (nz+1) * m_vNodeLength;
	dReal Tol = m_vNodeLength * TERRAINTOL;
	
	if ((pos[0]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
		return false;

	if ((pos[2]<Min[2]-Tol) || (pos[2]>Max[2]+Tol))
		return false;

	dReal dx	= (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
	dReal dz	= (pos[2] - (dReal(nz) * m_vNodeLength)) / m_vNodeLength;

	if ((w == 0) && (dx + dz > 1.f+TERRAINTOL))
		return false;

	if ((w == 1) && (dx + dz < 1.f-TERRAINTOL))
		return false;

	return true;
}

dGeomID dCreateTerrainCallback(dSpaceID space, dReal *pHeights,dReal vHeight,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
{
	return new dxTerrainCallback(space, pHeights,vHeight,vLength,nNumNodesPerSide,bFinite,bPlaceable);
}

dReal dGeomTerrainCallbackPointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
	dUASSERT (g && g->type == dTerrainCallbackClass,"argument not a terrain");
	dxTerrainCallback *t = (dxTerrainCallback*) g;
	return t->GetHeight(x,z) - y;
}

void dGeomTerrainCallbackSetHeightCallback(dGeomID g,dTerrainHeightCallback* callback)
{
    ((dxTerrainCallback*)g)->m_pHeightCallback = callback;
}


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 dxTerrainCallback::dCollideTerrainUnit(
	int x,int z,dxGeom *o2,int numMaxContacts,
	int flags,dContactGeom *contact, int skip)
{