aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:22:23 +0000
committerdan miller2007-10-19 05:22:23 +0000
commit1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43 (patch)
tree51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
parentone more for the gipper (diff)
downloadopensim-SC_OLD-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.zip
opensim-SC_OLD-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.gz
opensim-SC_OLD-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.bz2
opensim-SC_OLD-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.xz
trying to fix my screwup, please hold on
Diffstat (limited to 'libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp')
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp662
1 files changed, 0 insertions, 662 deletions
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
deleted file mode 100644
index ff779ac..0000000
--- a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
+++ /dev/null
@@ -1,662 +0,0 @@
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
20static bool IsAPowerOfTwo(int f)
21{
22 dAASSERT(f!=0);
23 while ((f&1) != 1)
24 f >>= 1;
25
26 return (f == 1);
27}
28
29static 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
42dxTerrainY::dxTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
43dxGeom (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
69dxTerrainY::~dxTerrainY()
70{
71 dIASSERT(m_pHeights);
72 delete [] m_pHeights;
73}
74
75void 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
143dReal 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
149dReal 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
178bool 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
205dGeomID 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
210dReal 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
218typedef 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
236A-B-E.x
237|/|
238C-D
239|
240F
241.
242z
243*/
244int 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,numContacts*skip);
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,numContacts*skip);
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,numContacts*skip);
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
502int 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
594dCollideTerrainYExit:
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,i*skip)->pos,+,CONTACT(contact,i*skip)->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/*
616void 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*/
generated by cgit v1.1 at 2024-11-08 09:52:56 +0000