aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:24:38 +0000
committerdan miller2007-10-19 05:24:38 +0000
commitf205de7847da7ae1c10212d82e7042d0100b4ce0 (patch)
tree9acc9608a6880502aaeda43af52c33e278e95b9c /libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
parenttrying to fix my screwup part deux (diff)
downloadopensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.zip
opensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.gz
opensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.bz2
opensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.xz
from the start... checking in ode-0.9
Diffstat (limited to 'libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp')
-rw-r--r--libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp659
1 files changed, 659 insertions, 0 deletions
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
new file mode 100644
index 0000000..d47c402
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
@@ -0,0 +1,659 @@
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
42dxTerrainZ::dxTerrainZ (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 = dTerrainZClass;
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
69dxTerrainZ::~dxTerrainZ()
70{
71 dIASSERT(m_pHeights);
72 delete [] m_pHeights;
73}
74
75void dxTerrainZ::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] = 0;
85 dx[3] = final_posr->R[1] * m_vLength;
86 dx[4] = final_posr->R[2] * m_vMinHeight;
87 dx[5] = final_posr->R[2] * m_vMaxHeight;
88
89 dy[0] = 0;
90 dy[1] = final_posr->R[4] * m_vLength;
91 dy[2] = 0;
92 dy[3] = final_posr->R[5] * m_vLength;
93 dy[4] = final_posr->R[6] * m_vMinHeight;
94 dy[5] = final_posr->R[6] * m_vMaxHeight;
95
96 dz[0] = 0;
97 dz[1] = final_posr->R[8] * m_vLength;
98 dz[2] = 0;
99 dz[3] = final_posr->R[9] * m_vLength;
100 dz[4] = final_posr->R[10] * m_vMinHeight;
101 dz[5] = final_posr->R[10] * m_vMaxHeight;
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] = 0;
115 aabb[3] = m_vLength;
116 aabb[4] = m_vMinHeight;
117 aabb[5] = m_vMaxHeight;
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] = -dInfinity;
136 aabb[3] = dInfinity;
137 aabb[4] = m_vMinHeight;
138 aabb[5] = m_vMaxHeight;
139 }
140 }
141}
142
143dReal dxTerrainZ::GetHeight(int x,int y)
144{
145 return m_pHeights[ (((unsigned int)(y) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
146 + ((unsigned int)(x) & m_nNumNodesPerSideMask)];
147}
148
149dReal dxTerrainZ::GetHeight(dReal x,dReal y)
150{
151 int nX = int(floor(x / m_vNodeLength));
152 int nY = int(floor(y / m_vNodeLength));
153 dReal dx = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
154 dReal dy = (y - (dReal(nY) * m_vNodeLength)) / m_vNodeLength;
155 dIASSERT((dx >= 0.f) && (dx <= 1.f));
156 dIASSERT((dy >= 0.f) && (dy <= 1.f));
157
158 dReal z,z0;
159
160 if (dx + dy < 1.f)
161 {
162 z0 = GetHeight(nX,nY);
163 z = z0
164 + (GetHeight(nX+1,nY) - z0) * dx
165 + (GetHeight(nX,nY+1) - z0) * dy;
166 }
167 else
168 {
169 z0 = GetHeight(nX+1,nY+1);
170 z = z0
171 + (GetHeight(nX+1,nY) - z0) * (1.f - dy)
172 + (GetHeight(nX,nY+1) - z0) * (1.f - dx);
173 }
174
175 return z;
176}
177
178bool dxTerrainZ::IsOnTerrain(int nx,int ny,int w,dReal *pos)
179{
180 dVector3 Min,Max;
181 Min[0] = nx * m_vNodeLength;
182 Min[1] = ny * m_vNodeLength;
183 Max[0] = (nx+1) * m_vNodeLength;
184 Max[1] = (ny+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[1]<Min[1]-Tol) || (pos[1]>Max[1]+Tol))
191 return false;
192
193 dReal dx = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
194 dReal dy = (pos[1] - (dReal(ny) * m_vNodeLength)) / m_vNodeLength;
195
196 if ((w == 0) && (dx + dy > 1.f+TERRAINTOL))
197 return false;
198
199 if ((w == 1) && (dx + dy < 1.f-TERRAINTOL))
200 return false;
201
202 return true;
203}
204
205dGeomID dCreateTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
206{
207 return new dxTerrainZ(space, pHeights,vLength,nNumNodesPerSide, bFinite, bPlaceable);
208}
209
210dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z)
211{
212 dUASSERT (g && g->type == dTerrainZClass,"argument not a terrain");
213 g->recomputePosr();
214 dxTerrainZ *t = (dxTerrainZ*) g;
215 return t->GetHeight(x,y) - z;
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,y,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(z is up)
235
236y
237.
238F
239|
240C-D
241|\|
242A-B-E.x
243*/
244int dxTerrainZ::dCollideTerrainUnit(
245 int x,int y,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[1] = y * m_vNodeLength;
296 A[2] = GetHeight(x,y);
297 B[0] = (x+1) * m_vNodeLength;
298 B[1] = y * m_vNodeLength;
299 B[2] = GetHeight(x+1,y);
300 C[0] = x * m_vNodeLength;
301 C[1] = (y+1) * m_vNodeLength;
302 C[2] = GetHeight(x,y+1);
303 D[0] = (x+1) * m_vNodeLength;
304 D[1] = (y+1) * m_vNodeLength;
305 D[2] = GetHeight(x+1,y+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[1] = y * m_vNodeLength;
333 E[2] = GetHeight(x+2,y);
334 F[0] = x * m_vNodeLength;
335 F[1] = (y+2) * m_vNodeLength;
336 F[2] = GetHeight(x,y+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],=,AD,BC);
346 dNormalize3(Normal[0]);
347
348 //BD
349 dCROSS(Normal[1],=,CE,BD);
350 dNormalize3(Normal[1]);
351
352 //CD
353 dCROSS(Normal[2],=,FB,CD);
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 if (GetDepth == NULL)
411 {
412 dxRay rayV(0,1000.f);
413 dGeomRaySet(&rayV, pContact->pos[0],
414 pContact->pos[1],
415 pContact->pos[2],
416 -pContact->normal[0],
417 -pContact->normal[1],
418 -pContact->normal[2]);
419
420 dContactGeom ContactV;
421 if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
422 {
423 pContact->depth = ContactV.depth;
424 numContacts++;
425 }
426 }
427 else
428 {
429 pContact->depth = GetDepth(o2,
430 pContact->pos[0],
431 pContact->pos[1],
432 pContact->pos[2]);
433 numContacts++;
434 }
435#endif
436 if (numContacts == numMaxContacts)
437 return numContacts;
438
439 }
440 }
441 }
442
443 dCROSS(Plane,=,AB,AC);
444 dNormalize3(Plane);
445 Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
446 dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
447 numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
448
449 for (i=0;i<numPlaneContacts;i++)
450 {
451 if (IsOnTerrain(x,y,0,PlaneContact[i].pos))
452 {
453 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
454 pContact->pos[0] = PlaneContact[i].pos[0];
455 pContact->pos[1] = PlaneContact[i].pos[1];
456 pContact->pos[2] = PlaneContact[i].pos[2];
457 pContact->normal[0] = -PlaneContact[i].normal[0];
458 pContact->normal[1] = -PlaneContact[i].normal[1];
459 pContact->normal[2] = -PlaneContact[i].normal[2];
460 pContact->depth = PlaneContact[i].depth;
461
462 //DMESS(0);
463 numContacts++;
464
465 if (numContacts == numMaxContacts)
466 return numContacts;
467 }
468 }
469
470 dCROSS(Plane,=,CD,BD);
471 dNormalize3(Plane);
472 Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
473 dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
474 numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
475
476 for (i=0;i<numPlaneContacts;i++)
477 {
478 if (IsOnTerrain(x,y,1,PlaneContact[i].pos))
479 {
480 dContactGeom *pContact = CONTACT(contact,numContacts*skip);
481 pContact->pos[0] = PlaneContact[i].pos[0];
482 pContact->pos[1] = PlaneContact[i].pos[1];
483 pContact->pos[2] = PlaneContact[i].pos[2];
484 pContact->normal[0] = -PlaneContact[i].normal[0];
485 pContact->normal[1] = -PlaneContact[i].normal[1];
486 pContact->normal[2] = -PlaneContact[i].normal[2];
487 pContact->depth = PlaneContact[i].depth;
488 //DMESS(1);
489 numContacts++;
490
491 if (numContacts == numMaxContacts)
492 return numContacts;
493 }
494 }
495
496 return numContacts;
497}
498
499int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip)
500{
501 dIASSERT (skip >= (int)sizeof(dContactGeom));
502 dIASSERT (o1->type == dTerrainZClass);
503 int i,j;
504
505 if ((flags & 0xffff) == 0)
506 flags = (flags & 0xffff0000) | 1;
507
508 int numMaxTerrainContacts = (flags & 0xffff);
509 dxTerrainZ *terrain = (dxTerrainZ*) o1;
510
511 dReal aabbbak[6];
512 int gflagsbak;
513
514 dVector3 pos0;
515 int numTerrainContacts = 0;
516
517 dxPosR *bak;
518 dxPosR X1;
519
520 if (terrain->gflags & GEOM_PLACEABLE)
521 {
522 dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
523 dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
524 dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
525 bak = o2->final_posr;
526 o2->final_posr = &X1;
527 memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
528 gflagsbak = o2->gflags;
529 o2->computeAABB();
530 }
531
532 int nMinX = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
533 int nMaxX = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
534 int nMinY = int(floor(o2->aabb[2] / terrain->m_vNodeLength));
535 int nMaxY = int(floor(o2->aabb[3] / terrain->m_vNodeLength)) + 1;
536
537 if (terrain->m_bFinite)
538 {
539 nMinX = MAX(nMinX,0);
540 nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
541 nMinY = MAX(nMinY,0);
542 nMaxY = MIN(nMaxY,terrain->m_nNumNodesPerSide);
543
544 if ((nMinX >= nMaxX) || (nMinY >= nMaxY))
545 goto dCollideTerrainZExit;
546 }
547
548 dVector3 AabbTop;
549 AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
550 AabbTop[1] = (o2->aabb[2]+o2->aabb[3]) / 2;
551 AabbTop[2] = o2->aabb[5];
552 if (o2->type != dRayClass)
553 {
554 dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[1]) - AabbTop[2];
555 if (AabbTopDepth > 0.f)
556 {
557 contact->depth = AabbTopDepth;
558 dReal MaxDepth = (o2->aabb[5]-o2->aabb[4]) / 2;
559 if (contact->depth > MaxDepth)
560 contact->depth = MaxDepth;
561 contact->g1 = o1;
562 contact->g2 = o2;
563 dOPE(contact->pos,=,AabbTop);
564 contact->normal[0] = 0.f;
565 contact->normal[1] = 0.f;
566 contact->normal[2] = -1.f;
567
568 numTerrainContacts = 1;
569 goto dCollideTerrainZExit;
570 }
571 }
572
573 for (i=nMinX;i<nMaxX;i++)
574 {
575 for (j=nMinY;j<nMaxY;j++)
576 {
577 numTerrainContacts += terrain->dCollideTerrainUnit(
578 i,j,o2,numMaxTerrainContacts - numTerrainContacts,
579 flags,CONTACT(contact,numTerrainContacts*skip),skip );
580 }
581 }
582
583 dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
584
585 for (i=0; i<numTerrainContacts; i++)
586 {
587 CONTACT(contact,i*skip)->g1 = o1;
588 CONTACT(contact,i*skip)->g2 = o2;
589 }
590
591dCollideTerrainZExit:
592
593 if (terrain->gflags & GEOM_PLACEABLE)
594 {
595 o2->final_posr = bak;
596 memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
597 o2->gflags = gflagsbak;
598
599 for (i=0; i<numTerrainContacts; i++)
600 {
601 dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
602 dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
603 dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos);
604
605 dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
606 dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
607 }
608 }
609
610 return numTerrainContacts;
611}
612/*
613void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
614{
615 float A[3],B[3],C[3],D[3];
616 float R[12];
617 float pos[3];
618 if (pR)
619 memcpy(R,pR,sizeof(R));
620 else
621 {
622 memset(R,0,sizeof(R));
623 R[0] = 1.f;
624 R[5] = 1.f;
625 R[10] = 1.f;
626 }
627
628 if (ppos)
629 memcpy(pos,ppos,sizeof(pos));
630 else
631 memset(pos,0,sizeof(pos));
632
633 float vx,vz;
634 vx = vLength * x;
635 vz = vLength * z;
636
637 int i;
638 for (i=0;i<nNumNodesPerSide;i++)
639 {
640 for (int j=0;j<nNumNodesPerSide;j++)
641 {
642 A[0] = i * vNodeLength + vx;
643 A[1] = j * vNodeLength + vz;
644 A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
645 B[0] = (i+1) * vNodeLength + vx;
646 B[1] = j * vNodeLength + vz;
647 B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
648 C[0] = i * vNodeLength + vx;
649 C[1] = (j+1) * vNodeLength + vz;
650 C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
651 D[0] = (i+1) * vNodeLength + vx;
652 D[1] = (j+1) * vNodeLength + vz;
653 D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
654 dsDrawTriangle(pos,R,C,A,B,1);
655 dsDrawTriangle(pos,R,D,C,B,1);
656 }
657 }
658}
659*/
generated by cgit v1.1 at 2024-09-21 23:21:55 +0000