Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/ode/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp @ 254

Last change on this file since 254 was 216, checked in by mathiask, 18 years ago
[Physik] add ode-0.9
File size: 16.7 KB

Rev	Line
[216]	1	//Benoit CHAPEROT 2003-2004 www.jstarlab.com
	2	//some code inspired by Magic Software
	3	#include <ode/common.h>
	4	#include <ode/collision.h>
	5	#include <ode/matrix.h>
	6	#include <ode/rotation.h>
	7	#include <ode/odemath.h>
	8	#include "collision_kernel.h"
	9	#include "collision_std.h"
	10	#include "collision_std_internal.h"
	11	#include "collision_util.h"
	12	//#include <drawstuff/drawstuff.h>
	13	#include "windows.h"
	14	#include "ode\ode.h"
	15
	16	#define CONTACT(p,skip) ((dContactGeom) (((char)p) + (skip)))
	17	#define MAXCONTACT 10
	18	#define TERRAINTOL 0.0f
	19
	20	static bool IsAPowerOfTwo(int f)
	21	{
	22	dAASSERT(f!=0);
	23	while ((f&1) != 1)
	24	f >>= 1;
	25
	26	return (f == 1);
	27	}
	28
	29	static int GetPowerOfTwo(int f)
	30	{
	31	dAASSERT(f!=0);
	32	int n = 0;
	33	while ((f&1) != 1)
	34	{
	35	n++;
	36	f >>= 1;
	37	}
	38
	39	return n;
	40	}
	41
	42	dxTerrainY::dxTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
	43	dxGeom (space,bPlaceable)
	44	{
	45	dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
	46	dIASSERT(pHeights);
	47	dIASSERT(vLength > 0.f);
	48	dIASSERT(nNumNodesPerSide > 0);
	49	type = dTerrainYClass;
	50	m_vLength = vLength;
	51	m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
	52	dIASSERT(m_pHeights);
	53	m_nNumNodesPerSide = nNumNodesPerSide;
	54	m_vNodeLength = m_vLength / m_nNumNodesPerSide;
	55	m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
	56	m_nNumNodesPerSideMask = m_nNumNodesPerSide - 1;
	57	m_vMinHeight = dInfinity;
	58	m_vMaxHeight = -dInfinity;
	59	m_bFinite = bFinite;
	60
	61	for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
	62	{
	63	m_pHeights[i] = pHeights[i];
	64	if (m_pHeights[i] < m_vMinHeight) m_vMinHeight = m_pHeights[i];
	65	if (m_pHeights[i] > m_vMaxHeight) m_vMaxHeight = m_pHeights[i];
	66	}
	67	}
	68
	69	dxTerrainY::~dxTerrainY()
	70	{
	71	dIASSERT(m_pHeights);
	72	delete [] m_pHeights;
	73	}
	74
	75	void dxTerrainY::computeAABB()
	76	{
	77	if (m_bFinite)
	78	{
	79	if (gflags & GEOM_PLACEABLE)
	80	{
	81	dReal dx[6],dy[6],dz[6];
	82	dx[0] = 0;
	83	dx[1] = final_posr->R[0] * m_vLength;
	84	dx[2] = final_posr->R[1] * m_vMinHeight;
	85	dx[3] = final_posr->R[1] * m_vMaxHeight;
	86	dx[4] = 0;
	87	dx[5] = final_posr->R[2] * m_vLength;
	88
	89	dy[0] = 0;
	90	dy[1] = final_posr->R[4] * m_vLength;
	91	dy[2] = final_posr->R[5] * m_vMinHeight;
	92	dy[3] = final_posr->R[5] * m_vMaxHeight;
	93	dy[4] = 0;
	94	dy[5] = final_posr->R[6] * m_vLength;
	95
	96	dz[0] = 0;
	97	dz[1] = final_posr->R[8] * m_vLength;
	98	dz[2] = final_posr->R[9] * m_vMinHeight;
	99	dz[3] = final_posr->R[9] * m_vMaxHeight;
	100	dz[4] = 0;
	101	dz[5] = final_posr->R[10] * m_vLength;
	102
	103	aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
	104	aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
	105	aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
	106	aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
	107	aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
	108	aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
	109	}
	110	else
	111	{
	112	aabb[0] = 0;
	113	aabb[1] = m_vLength;
	114	aabb[2] = m_vMinHeight;
	115	aabb[3] = m_vMaxHeight;
	116	aabb[4] = 0;
	117	aabb[5] = m_vLength;
	118	}
	119	}
	120	else
	121	{
	122	if (gflags & GEOM_PLACEABLE)
	123	{
	124	aabb[0] = -dInfinity;
	125	aabb[1] = dInfinity;
	126	aabb[2] = -dInfinity;
	127	aabb[3] = dInfinity;
	128	aabb[4] = -dInfinity;
	129	aabb[5] = dInfinity;
	130	}
	131	else
	132	{
	133	aabb[0] = -dInfinity;
	134	aabb[1] = dInfinity;
	135	aabb[2] = m_vMinHeight;
	136	aabb[3] = m_vMaxHeight;
	137	aabb[4] = -dInfinity;
	138	aabb[5] = dInfinity;
	139	}
	140	}
	141	}
	142
	143	dReal dxTerrainY::GetHeight(int x,int z)
	144	{
	145	return m_pHeights[ (((unsigned int)(z) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
	146	+ ((unsigned int)(x) & m_nNumNodesPerSideMask)];
	147	}
	148
	149	dReal dxTerrainY::GetHeight(dReal x,dReal z)
	150	{
	151	int nX = int(floor(x / m_vNodeLength));
	152	int nZ = int(floor(z / m_vNodeLength));
	153	dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
	154	dReal dz = (z - (dReal(nZ) * m_vNodeLength)) / m_vNodeLength;
	155	dIASSERT((dx >= 0.f) && (dx <= 1.f));
	156	dIASSERT((dz >= 0.f) && (dz <= 1.f));
	157
	158	dReal y,y0;
	159
	160	if (dx + dz < 1.f)
	161	{
	162	y0 = GetHeight(nX,nZ);
	163	y = y0
	164	+ (GetHeight(nX+1,nZ) - y0) * dx
	165	+ (GetHeight(nX,nZ+1) - y0) * dz;
	166	}
	167	else
	168	{
	169	y0 = GetHeight(nX+1,nZ+1);
	170	y = y0
	171	+ (GetHeight(nX+1,nZ) - y0) * (1.f - dz)
	172	+ (GetHeight(nX,nZ+1) - y0) * (1.f - dx);
	173	}
	174
	175	return y;
	176	}
	177
	178	bool dxTerrainY::IsOnTerrain(int nx,int nz,int w,dReal *pos)
	179	{
	180	dVector3 Min,Max;
	181	Min[0] = nx * m_vNodeLength;
	182	Min[2] = nz * m_vNodeLength;
	183	Max[0] = (nx+1) * m_vNodeLength;
	184	Max[2] = (nz+1) * m_vNodeLength;
	185	dReal Tol = m_vNodeLength * TERRAINTOL;
	186
	187	if ((pos[0]<Min[0]-Tol) \|\| (pos[0]>Max[0]+Tol))
	188	return false;
	189
	190	if ((pos[2]<Min[2]-Tol) \|\| (pos[2]>Max[2]+Tol))
	191	return false;
	192
	193	dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
	194	dReal dz = (pos[2] - (dReal(nz) * m_vNodeLength)) / m_vNodeLength;
	195
	196	if ((w == 0) && (dx + dz > 1.f+TERRAINTOL))
	197	return false;
	198
	199	if ((w == 1) && (dx + dz < 1.f-TERRAINTOL))
	200	return false;
	201
	202	return true;
	203	}
	204
	205	dGeomID dCreateTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
	206	{
	207	return new dxTerrainY(space, pHeights,vLength,nNumNodesPerSide,bFinite,bPlaceable);
	208	}
	209
	210	dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z)
	211	{
	212	dUASSERT (g && g->type == dTerrainYClass,"argument not a terrain");
	213	g->recomputePosr();
	214	dxTerrainY t = (dxTerrainY) g;
	215	return t->GetHeight(x,z) - y;
	216	}
	217
	218	typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z);
	219	#define RECOMPUTE_RAYNORMAL
	220	//#define DO_RAYDEPTH
	221
	222	#define DMESS(A) \
	223	dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
	224	x,z,A, \
	225	pContact->depth, \
	226	dGeomSphereGetRadius(o2), \
	227	pContact->pos[0], \
	228	pContact->pos[1], \
	229	pContact->pos[2], \
	230	pContact->normal[0], \
	231	pContact->normal[1], \
	232	pContact->normal[2]);
	233	/*
	234	(y is up)
	235
	236	A-B-E.x
	237	\|/\|
	238	C-D
	239	\|
	240	F
	241	.
	242	z
	243	*/
	244	int dxTerrainY::dCollideTerrainUnit(
	245	int x,int z,dxGeom *o2,int numMaxContacts,
	246	int flags,dContactGeom *contact, int skip)
	247	{
	248	dColliderFn *CollideRayN;
	249	dColliderFn *CollideNPlane;
	250	dGetDepthFn *GetDepth;
	251	int numContacts = 0;
	252	int numPlaneContacts = 0;
	253	int i;
	254
	255	if (numContacts == numMaxContacts)
	256	return numContacts;
	257
	258	dContactGeom PlaneContact[MAXCONTACT];
	259	flags = (flags & 0xffff0000) \| MAXCONTACT;
	260
	261	switch (o2->type)
	262	{
	263	case dSphereClass:
	264	CollideRayN = dCollideRaySphere;
	265	CollideNPlane = dCollideSpherePlane;
	266	GetDepth = dGeomSpherePointDepth;
	267	break;
	268	case dBoxClass:
	269	CollideRayN = dCollideRayBox;
	270	CollideNPlane = dCollideBoxPlane;
	271	GetDepth = dGeomBoxPointDepth;
	272	break;
	273	case dCCylinderClass:
	274	CollideRayN = dCollideRayCCylinder;
	275	CollideNPlane = dCollideCCylinderPlane;
	276	GetDepth = dGeomCCylinderPointDepth;
	277	break;
	278	case dRayClass:
	279	CollideRayN = NULL;
	280	CollideNPlane = dCollideRayPlane;
	281	GetDepth = NULL;
	282	break;
	283	case dConeClass:
	284	CollideRayN = dCollideRayCone;
	285	CollideNPlane = dCollideConePlane;
	286	GetDepth = dGeomConePointDepth;
	287	break;
	288	default:
	289	dIASSERT(0);
	290	}
	291
	292	dReal Plane[4],lBD,lCD,lBC;
	293	dVector3 A,B,C,D,BD,CD,BC,AB,AC;
	294	A[0] = x * m_vNodeLength;
	295	A[2] = z* m_vNodeLength;
	296	A[1] = GetHeight(x,z);
	297	B[0] = (x+1) * m_vNodeLength;
	298	B[2] = z * m_vNodeLength;
	299	B[1] = GetHeight(x+1,z);
	300	C[0] = x * m_vNodeLength;
	301	C[2] = (z+1) * m_vNodeLength;
	302	C[1] = GetHeight(x,z+1);
	303	D[0] = (x+1) * m_vNodeLength;
	304	D[2] = (z+1) * m_vNodeLength;
	305	D[1] = GetHeight(x+1,z+1);
	306
	307	dOP(BC,-,C,B);
	308	lBC = dLENGTH(BC);
	309	dOPEC(BC,/=,lBC);
	310
	311	dOP(BD,-,D,B);
	312	lBD = dLENGTH(BD);
	313	dOPEC(BD,/=,lBD);
	314
	315	dOP(CD,-,D,C);
	316	lCD = dLENGTH(CD);
	317	dOPEC(CD,/=,lCD);
	318
	319	dOP(AB,-,B,A);
	320	dNormalize3(AB);
	321
	322	dOP(AC,-,C,A);
	323	dNormalize3(AC);
	324
	325	if (CollideRayN)
	326	{
	327	#ifdef RECOMPUTE_RAYNORMAL
	328	dVector3 E,F;
	329	dVector3 CE,FB,AD;
	330	dVector3 Normal[3];
	331	E[0] = (x+2) * m_vNodeLength;
	332	E[2] = z * m_vNodeLength;
	333	E[1] = GetHeight(x+2,z);
	334	F[0] = x * m_vNodeLength;
	335	F[2] = (z+2) * m_vNodeLength;
	336	F[1] = GetHeight(x,z+2);
	337	dOP(AD,-,D,A);
	338	dNormalize3(AD);
	339	dOP(CE,-,E,C);
	340	dNormalize3(CE);
	341	dOP(FB,-,B,F);
	342	dNormalize3(FB);
	343
	344	//BC
	345	dCROSS(Normal[0],=,BC,AD);
	346	dNormalize3(Normal[0]);
	347
	348	//BD
	349	dCROSS(Normal[1],=,BD,CE);
	350	dNormalize3(Normal[1]);
	351
	352	//CD
	353	dCROSS(Normal[2],=,CD,FB);
	354	dNormalize3(Normal[2]);
	355	#endif
	356	int nA[3],nB[3];
	357	dContactGeom ContactA[3],ContactB[3];
	358	dxRay rayBC(0,lBC);
	359	dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
	360	nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom));
	361	dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
	362	nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom));
	363
	364	dxRay rayBD(0,lBD);
	365	dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
	366	nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom));
	367	dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
	368	nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom));
	369
	370	dxRay rayCD(0,lCD);
	371	dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
	372	nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom));
	373	dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
	374	nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom));
	375
	376	for (i=0;i<3;i++)
	377	{
	378	if (nA[i] & nB[i])
	379	{
	380	dContactGeom pContact = CONTACT(contact,numContactsskip);
	381	pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2;
	382	pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2;
	383	pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2;
	384	#ifdef RECOMPUTE_RAYNORMAL
	385	pContact->normal[0] = -Normal[i][0];
	386	pContact->normal[1] = -Normal[i][1];
	387	pContact->normal[2] = -Normal[i][2];
	388	#else
	389	pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2; //0.f;
	390	pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2; //0.f;
	391	pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2; //-1.f;
	392	dNormalize3(pContact->normal);
	393	#endif
	394	#ifdef DO_RAYDEPTH
	395	dxRay rayV(0,1000.f);
	396	dGeomRaySet(&rayV, pContact->pos[0],
	397	pContact->pos[1],
	398	pContact->pos[2],
	399	-pContact->normal[0],
	400	-pContact->normal[1],
	401	-pContact->normal[2]);
	402
	403	dContactGeom ContactV;
	404	if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
	405	{
	406	pContact->depth = ContactV.depth;
	407	numContacts++;
	408	}
	409	#else
	410
	411	if (GetDepth == NULL)
	412	{
	413	dxRay rayV(0,1000.f);
	414	dGeomRaySet(&rayV, pContact->pos[0],
	415	pContact->pos[1],
	416	pContact->pos[2],
	417	-pContact->normal[0],
	418	-pContact->normal[1],
	419	-pContact->normal[2]);
	420
	421	dContactGeom ContactV;
	422
	423	if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
	424	{
	425	pContact->depth = ContactV.depth;
	426	numContacts++;
	427	}
	428	}
	429	else
	430	{
	431	pContact->depth = GetDepth(o2,
	432	pContact->pos[0],
	433	pContact->pos[1],
	434	pContact->pos[2]);
	435	numContacts++;
	436	}
	437
	438	#endif
	439	if (numContacts == numMaxContacts)
	440	return numContacts;
	441
	442	}
	443	}
	444	}
	445
	446	dCROSS(Plane,=,AC,AB);
	447	dNormalize3(Plane);
	448	Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
	449	dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
	450	numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
	451
	452	for (i=0;i<numPlaneContacts;i++)
	453	{
	454	if (IsOnTerrain(x,z,0,PlaneContact[i].pos))
	455	{
	456	dContactGeom pContact = CONTACT(contact,numContactsskip);
	457	pContact->pos[0] = PlaneContact[i].pos[0];
	458	pContact->pos[1] = PlaneContact[i].pos[1];
	459	pContact->pos[2] = PlaneContact[i].pos[2];
	460	pContact->normal[0] = -PlaneContact[i].normal[0];
	461	pContact->normal[1] = -PlaneContact[i].normal[1];
	462	pContact->normal[2] = -PlaneContact[i].normal[2];
	463	pContact->depth = PlaneContact[i].depth;
	464
	465	//DMESS(0);
	466	numContacts++;
	467
	468	if (numContacts == numMaxContacts)
	469	return numContacts;
	470	}
	471	}
	472
	473	dCROSS(Plane,=,BD,CD);
	474	dNormalize3(Plane);
	475	Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
	476	dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
	477	numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
	478
	479	for (i=0;i<numPlaneContacts;i++)
	480	{
	481	if (IsOnTerrain(x,z,1,PlaneContact[i].pos))
	482	{
	483	dContactGeom pContact = CONTACT(contact,numContactsskip);
	484	pContact->pos[0] = PlaneContact[i].pos[0];
	485	pContact->pos[1] = PlaneContact[i].pos[1];
	486	pContact->pos[2] = PlaneContact[i].pos[2];
	487	pContact->normal[0] = -PlaneContact[i].normal[0];
	488	pContact->normal[1] = -PlaneContact[i].normal[1];
	489	pContact->normal[2] = -PlaneContact[i].normal[2];
	490	pContact->depth = PlaneContact[i].depth;
	491	//DMESS(1);
	492	numContacts++;
	493
	494	if (numContacts == numMaxContacts)
	495	return numContacts;
	496	}
	497	}
	498
	499	return numContacts;
	500	}
	501
	502	int dCollideTerrainY(dxGeom o1, dxGeom o2, int flags,dContactGeom *contact, int skip)
	503	{
	504	dIASSERT (skip >= (int)sizeof(dContactGeom));
	505	dIASSERT (o1->type == dTerrainYClass);
	506	int i,j;
	507
	508	if ((flags & 0xffff) == 0)
	509	flags = (flags & 0xffff0000) \| 1;
	510
	511	int numMaxTerrainContacts = (flags & 0xffff);
	512	dxTerrainY terrain = (dxTerrainY) o1;
	513
	514	dReal aabbbak[6];
	515	int gflagsbak;
	516
	517	dVector3 pos0;
	518	int numTerrainContacts = 0;
	519
	520	dxPosR *bak;
	521	dxPosR X1;
	522
	523	if (terrain->gflags & GEOM_PLACEABLE)
	524	{
	525	dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
	526	dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
	527	dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
	528	bak = o2->final_posr;
	529	o2->final_posr = &X1;
	530	memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
	531	gflagsbak = o2->gflags;
	532	o2->computeAABB();
	533	}
	534
	535	int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
	536	int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
	537	int nMinZ = int(floor(o2->aabb[4] / terrain->m_vNodeLength));
	538	int nMaxZ = int(floor(o2->aabb[5] / terrain->m_vNodeLength)) + 1;
	539
	540	if (terrain->m_bFinite)
	541	{
	542	nMinX = MAX(nMinX,0);
	543	nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
	544	nMinZ = MAX(nMinZ,0);
	545	nMaxZ = MIN(nMaxZ,terrain->m_nNumNodesPerSide);
	546
	547	if ((nMinX >= nMaxX) \|\| (nMinZ >= nMaxZ))
	548	goto dCollideTerrainYExit;
	549	}
	550
	551	dVector3 AabbTop;
	552	AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
	553	AabbTop[2] = (o2->aabb[4]+o2->aabb[5]) / 2;
	554	AabbTop[1] = o2->aabb[3];
	555	if (o2->type != dRayClass)
	556	{
	557	dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[2]) - AabbTop[1];
	558	if (AabbTopDepth > 0.f)
	559	{
	560	contact->depth = AabbTopDepth;
	561	dReal MaxDepth = (o2->aabb[3]-o2->aabb[2]) / 2;
	562	if (contact->depth > MaxDepth)
	563	contact->depth = MaxDepth;
	564	contact->g1 = o1;
	565	contact->g2 = o2;
	566	dOPE(contact->pos,=,AabbTop);
	567	contact->normal[0] = 0.f;
	568	contact->normal[1] = -1.f;
	569	contact->normal[2] = 0.f;
	570
	571	numTerrainContacts = 1;
	572	goto dCollideTerrainYExit;
	573	}
	574	}
	575
	576	for (i=nMinX;i<nMaxX;i++)
	577	{
	578	for (j=nMinZ;j<nMaxZ;j++)
	579	{
	580	numTerrainContacts += terrain->dCollideTerrainUnit(
	581	i,j,o2,numMaxTerrainContacts - numTerrainContacts,
	582	flags,CONTACT(contact,numTerrainContacts*skip),skip );
	583	}
	584	}
	585
	586	dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
	587
	588	for (i=0; i<numTerrainContacts; i++)
	589	{
	590	CONTACT(contact,i*skip)->g1 = o1;
	591	CONTACT(contact,i*skip)->g2 = o2;
	592	}
	593
	594	dCollideTerrainYExit:
	595
	596	if (terrain->gflags & GEOM_PLACEABLE)
	597	{
	598	o2->final_posr = bak;
	599	memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
	600	o2->gflags = gflagsbak;
	601
	602	for (i=0; i<numTerrainContacts; i++)
	603	{
	604	dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
	605	dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
	606	dOP(CONTACT(contact,iskip)->pos,+,CONTACT(contact,iskip)->pos,terrain->final_posr->pos);
	607
	608	dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
	609	dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
	610	}
	611	}
	612
	613	return numTerrainContacts;
	614	}
	615	/*
	616	void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float pHeights,const float pR,const float *ppos)
	617	{
	618	float A[3],B[3],C[3],D[3];
	619	float R[12];
	620	float pos[3];
	621	if (pR)
	622	memcpy(R,pR,sizeof(R));
	623	else
	624	{
	625	memset(R,0,sizeof(R));
	626	R[0] = 1.f;
	627	R[5] = 1.f;
	628	R[10] = 1.f;
	629	}
	630
	631	if (ppos)
	632	memcpy(pos,ppos,sizeof(pos));
	633	else
	634	memset(pos,0,sizeof(pos));
	635
	636	float vx,vz;
	637	vx = vLength * x;
	638	vz = vLength * z;
	639
	640	int i;
	641	for (i=0;i<nNumNodesPerSide;i++)
	642	{
	643	for (int j=0;j<nNumNodesPerSide;j++)
	644	{
	645	A[0] = i * vNodeLength + vx;
	646	A[2] = j * vNodeLength + vz;
	647	A[1] = GetHeight(i,j,nNumNodesPerSide,pHeights);
	648	B[0] = (i+1) * vNodeLength + vx;
	649	B[2] = j * vNodeLength + vz;
	650	B[1] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
	651	C[0] = i * vNodeLength + vx;
	652	C[2] = (j+1) * vNodeLength + vz;
	653	C[1] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
	654	D[0] = (i+1) * vNodeLength + vx;
	655	D[2] = (j+1) * vNodeLength + vz;
	656	D[1] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
	657	dsDrawTriangle(pos,R,C,B,A,1);
	658	dsDrawTriangle(pos,R,D,B,C,1);
	659	}
	660	}
	661	}
	662	*/

Note: See TracBrowser for help on using the repository browser.

Download in other formats: