aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/heightfield.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/heightfield.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/heightfield.cpp1812
1 files changed, 0 insertions, 1812 deletions
diff --git a/libraries/ode-0.9/ode/src/heightfield.cpp b/libraries/ode-0.9/ode/src/heightfield.cpp
deleted file mode 100644
index 36755fc..0000000
--- a/libraries/ode-0.9/ode/src/heightfield.cpp
+++ /dev/null
@@ -1,1812 +0,0 @@
1// dHeightfield Collider
2// Paul Cheyrou-Lagreze aka Tuan Kuranes 2006 Speed enhancements http://www.pop-3d.com
3// Martijn Buijs 2006 http://home.planet.nl/~buijs512/
4// Based on Terrain & Cone contrib by:
5// Benoit CHAPEROT 2003-2004 http://www.jstarlab.com
6// Some code inspired by Magic Software
7
8
9#include <ode/common.h>
10#include <ode/collision.h>
11#include <ode/matrix.h>
12#include <ode/rotation.h>
13#include <ode/odemath.h>
14#include "collision_kernel.h"
15#include "collision_std.h"
16#include "collision_util.h"
17#include "heightfield.h"
18
19
20
21#if dTRIMESH_ENABLED
22#include "collision_trimesh_internal.h"
23#endif // dTRIMESH_ENABLED
24
25#define TERRAINTOL REAL(0.0)
26
27#define dMIN(A,B) ((A)>(B) ? B : A)
28#define dMAX(A,B) ((A)>(B) ? A : B)
29
30
31// Three-way MIN and MAX
32#define dMIN3(A,B,C) ( (A)<(B) ? dMIN((A),(C)) : dMIN((B),(C)) )
33#define dMAX3(A,B,C) ( (A)>(B) ? dMAX((A),(C)) : dMAX((B),(C)) )
34
35#define dOPESIGN(a, op1, op2,b) \
36 (a)[0] op1 op2 ((b)[0]); \
37 (a)[1] op1 op2 ((b)[1]); \
38 (a)[2] op1 op2 ((b)[2]);
39
40#define dGeomRaySetNoNormalize(myRay, MyPoint, MyVector) { \
41 \
42 dVector3Copy (MyPoint, myRay.final_posr->pos); \
43 myRay.final_posr->R[2] = MyVector[0]; \
44 myRay.final_posr->R[6] = MyVector[1]; \
45 myRay.final_posr->R[10] = MyVector[2]; \
46 dGeomMoved (&myRay); \
47 }
48
49#define dGeomPlaneSetNoNormalize(MyPlane, MyPlaneDef) { \
50 \
51 MyPlane->p[0] = MyPlaneDef[0]; \
52 MyPlane->p[1] = MyPlaneDef[1]; \
53 MyPlane->p[2] = MyPlaneDef[2]; \
54 MyPlane->p[3] = MyPlaneDef[3]; \
55 dGeomMoved (MyPlane); \
56 }
57//////// Local Build Option ////////////////////////////////////////////////////
58
59// Uncomment this #define to use the (0,0) corner of the geom as the origin,
60// rather than the center. This was the way the original heightfield worked,
61// but as it does not match the way all other geometries work, so for constancy it
62// was changed to work like this.
63
64// #define DHEIGHTFIELD_CORNER_ORIGIN
65
66
67// Uncomment this #define to add heightfield triangles edge colliding
68// Code is not guaranteed and I didn't find the need to add that as
69// colliding planes triangles and edge triangles seems enough.
70// #define _HEIGHTFIELDEDGECOLLIDING
71
72
73//////// dxHeightfieldData /////////////////////////////////////////////////////////////
74
75// dxHeightfieldData constructor
76dxHeightfieldData::dxHeightfieldData()
77{
78 //
79}
80
81
82// build Heightfield data
83void dxHeightfieldData::SetData( int nWidthSamples, int nDepthSamples,
84 dReal fWidth, dReal fDepth,
85 dReal fScale, dReal fOffset, dReal fThickness,
86 int bWrapMode )
87{
88 dIASSERT( fWidth > REAL( 0.0 ) );
89 dIASSERT( fDepth > REAL( 0.0 ) );
90 dIASSERT( nWidthSamples > 0 );
91 dIASSERT( nDepthSamples > 0 );
92
93 // x,z bounds
94 m_fWidth = fWidth;
95 m_fDepth = fDepth;
96
97 // cache half x,z bounds
98 m_fHalfWidth = fWidth / REAL( 2.0 );
99 m_fHalfDepth = fDepth / REAL( 2.0 );
100
101 // scale and offset
102 m_fScale = fScale;
103 m_fOffset = fOffset;
104
105 // infinite min height bounds
106 m_fThickness = fThickness;
107
108 // number of vertices per side
109 m_nWidthSamples = nWidthSamples;
110 m_nDepthSamples = nDepthSamples;
111
112 m_fSampleWidth = m_fWidth / ( m_nWidthSamples - 1 );
113 m_fSampleDepth = m_fDepth / ( m_nDepthSamples - 1 );
114
115 m_fInvSampleWidth = 1 / m_fSampleWidth;
116 m_fInvSampleDepth = 1 / m_fSampleDepth;
117
118 // finite or repeated terrain?
119 m_bWrapMode = bWrapMode;
120}
121
122
123// recomputes heights bounds
124void dxHeightfieldData::ComputeHeightBounds()
125{
126 static int i;
127 static dReal h;
128 static unsigned char *data_byte;
129 static short *data_short;
130 static float *data_float;
131 static double *data_double;
132
133 switch ( m_nGetHeightMode )
134 {
135
136 // callback
137 case 0:
138 // change nothing, keep using default or user specified bounds
139 return;
140
141 // byte
142 case 1:
143 data_byte = (unsigned char*)m_pHeightData;
144 m_fMinHeight = dInfinity;
145 m_fMaxHeight = -dInfinity;
146
147 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
148 {
149 h = data_byte[i];
150 if (h < m_fMinHeight) m_fMinHeight = h;
151 if (h > m_fMaxHeight) m_fMaxHeight = h;
152 }
153
154 break;
155
156 // short
157 case 2:
158 data_short = (short*)m_pHeightData;
159 m_fMinHeight = dInfinity;
160 m_fMaxHeight = -dInfinity;
161
162 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
163 {
164 h = data_short[i];
165 if (h < m_fMinHeight) m_fMinHeight = h;
166 if (h > m_fMaxHeight) m_fMaxHeight = h;
167 }
168
169 break;
170
171 // float
172 case 3:
173 data_float = (float*)m_pHeightData;
174 m_fMinHeight = dInfinity;
175 m_fMaxHeight = -dInfinity;
176
177 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
178 {
179 h = data_float[i];
180 if (h < m_fMinHeight) m_fMinHeight = h;
181 if (h > m_fMaxHeight) m_fMaxHeight = h;
182 }
183
184 break;
185
186 // double
187 case 4:
188 data_double = (double*)m_pHeightData;
189 m_fMinHeight = dInfinity;
190 m_fMaxHeight = -dInfinity;
191
192 for (i=0; i<m_nWidthSamples*m_nDepthSamples; i++)
193 {
194 h = static_cast< dReal >( data_double[i] );
195 if (h < m_fMinHeight) m_fMinHeight = h;
196 if (h > m_fMaxHeight) m_fMaxHeight = h;
197 }
198
199 break;
200
201 }
202
203 // scale and offset
204 m_fMinHeight *= m_fScale;
205 m_fMaxHeight *= m_fScale;
206 m_fMinHeight += m_fOffset;
207 m_fMaxHeight += m_fOffset;
208
209 // add thickness
210 m_fMinHeight -= m_fThickness;
211}
212
213
214// returns whether point is over terrain Cell triangle?
215bool dxHeightfieldData::IsOnHeightfield ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const
216{
217 {
218 const dReal MaxX = CellOrigin[0] + m_fSampleWidth;
219 const dReal TolX = m_fSampleWidth * TERRAINTOL;
220 if ((pos[0]<CellOrigin[0]-TolX) || (pos[0]>MaxX+TolX))
221 return false;
222 }
223
224 {
225 const dReal MaxZ = CellOrigin[2] + m_fSampleDepth;
226 const dReal TolZ = m_fSampleDepth * TERRAINTOL;
227 if ((pos[2]<CellOrigin[2]-TolZ) || (pos[2]>MaxZ+TolZ))
228 return false;
229 }
230
231 // add X percentage position on cell and Z percentage position on cell
232 const dReal pctTotal = (pos[0] - CellOrigin[0]) * m_fInvSampleWidth
233 + (pos[2] - CellOrigin[2]) * m_fInvSampleDepth;
234
235 if (isABC)
236 {
237 if (pctTotal >= REAL(1.0) + TERRAINTOL)
238 return false;
239 else
240 return true;
241 }
242 else if (pctTotal <= REAL(1.0) - TERRAINTOL)
243 {
244 return false;
245 }
246 return true;
247}
248// returns whether point is over terrain Cell triangle?
249bool dxHeightfieldData::IsOnHeightfield2 ( const dReal * const CellOrigin, const dReal * const pos, const bool isABC) const
250{
251 dReal MaxX, MinX;
252 dReal MaxZ, MinZ;
253 if (isABC)
254 {
255 // point A
256 MinX = CellOrigin[0];
257 MaxX = CellOrigin[0] + m_fSampleWidth;
258
259 MinZ = CellOrigin[2];
260 MaxZ = CellOrigin[2] + m_fSampleDepth;
261 }
262 else
263 {
264 // point D
265 MinX = CellOrigin[0] - m_fSampleWidth;
266 MaxX = CellOrigin[0];
267
268 MinZ = CellOrigin[2] - m_fSampleDepth;
269 MaxZ = CellOrigin[2];
270 }
271
272 // check if inside CELL
273 {
274 const dReal TolX = m_fSampleWidth * TERRAINTOL;
275 if ((pos[0]<MinX-TolX) || (pos[0]>MaxX+TolX))
276 return false;
277 }
278
279 {
280 const dReal TolZ = m_fSampleDepth * TERRAINTOL;
281 if ((pos[2]<MinZ-TolZ) || (pos[2]>MaxZ+TolZ))
282 return false;
283 }
284
285 // Sum up X percentage position on cell and Z percentage position on cell
286 const dReal pctTotal = (pos[0] - MinX) * m_fInvSampleWidth
287 + (pos[2] - MinZ) * m_fInvSampleDepth;
288
289 // check if inside respective Triangle of Cell
290 if (isABC)
291 {
292 if (pctTotal >= REAL(1.0) + TERRAINTOL)
293 return false;
294 else
295 return true;
296 }
297 else if (pctTotal <= REAL(1.0) - TERRAINTOL)
298 {
299 return false;
300 }
301 return true;
302}
303
304
305// returns height at given sample coordinates
306dReal dxHeightfieldData::GetHeight( int x, int z )
307{
308 static dReal h;
309 static unsigned char *data_byte;
310 static short *data_short;
311 static float *data_float;
312 static double *data_double;
313
314 if ( m_bWrapMode == 0 )
315 {
316 // Finite
317 if ( x < 0 ) x = 0;
318 if ( z < 0 ) z = 0;
319 if ( x > m_nWidthSamples - 1 ) x = m_nWidthSamples - 1;
320 if ( z > m_nDepthSamples - 1 ) z = m_nDepthSamples - 1;
321 }
322 else
323 {
324 // Infinite
325 x %= m_nWidthSamples - 1;
326 z %= m_nDepthSamples - 1;
327 if ( x < 0 ) x += m_nWidthSamples - 1;
328 if ( z < 0 ) z += m_nDepthSamples - 1;
329 }
330
331 switch ( m_nGetHeightMode )
332 {
333
334 // callback (dReal)
335 case 0:
336 h = (*m_pGetHeightCallback)(m_pUserData, x, z);
337 break;
338
339 // byte
340 case 1:
341 data_byte = (unsigned char*)m_pHeightData;
342 h = data_byte[x+(z * m_nWidthSamples)];
343 break;
344
345 // short
346 case 2:
347 data_short = (short*)m_pHeightData;
348 h = data_short[x+(z * m_nWidthSamples)];
349 break;
350
351 // float
352 case 3:
353 data_float = (float*)m_pHeightData;
354 h = data_float[x+(z * m_nWidthSamples)];
355 break;
356
357 // double
358 case 4:
359 data_double = (double*)m_pHeightData;
360 h = static_cast< dReal >( data_double[x+(z * m_nWidthSamples)] );
361 break;
362 }
363
364 return (h * m_fScale) + m_fOffset;
365}
366
367
368// returns height at given coordinates
369dReal dxHeightfieldData::GetHeight( dReal x, dReal z )
370{
371 dReal dnX = dFloor( x * m_fInvSampleWidth );
372 dReal dnZ = dFloor( z * m_fInvSampleDepth );
373
374 dReal dx = ( x - ( dnX * m_fSampleWidth ) ) * m_fInvSampleWidth;
375 dReal dz = ( z - ( dnZ * m_fSampleDepth ) ) * m_fInvSampleDepth;
376
377 int nX = int( dnX );
378 int nZ = int( dnZ );
379
380 //dIASSERT( ( dx + dEpsilon >= 0.0f ) && ( dx - dEpsilon <= 1.0f ) );
381 //dIASSERT( ( dz + dEpsilon >= 0.0f ) && ( dz - dEpsilon <= 1.0f ) );
382
383 dReal y, y0;
384
385 if ( dx + dz < REAL( 1.0 ) )
386 {
387 y0 = GetHeight( nX, nZ );
388
389 y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * dx
390 + ( GetHeight( nX, nZ + 1 ) - y0 ) * dz;
391 }
392 else
393 {
394 y0 = GetHeight( nX + 1, nZ + 1 );
395
396 y = y0 + ( GetHeight( nX + 1, nZ ) - y0 ) * ( REAL(1.0) - dz ) +
397 ( GetHeight( nX, nZ + 1 ) - y0 ) * ( REAL(1.0) - dx );
398 }
399
400 return y;
401}
402
403
404// dxHeightfieldData destructor
405dxHeightfieldData::~dxHeightfieldData()
406{
407 static unsigned char *data_byte;
408 static short *data_short;
409 static float *data_float;
410 static double *data_double;
411
412 dIASSERT( m_pHeightData );
413
414 if ( m_bCopyHeightData )
415 {
416 switch ( m_nGetHeightMode )
417 {
418
419 // callback
420 case 0:
421 // do nothing
422 break;
423
424 // byte
425 case 1:
426 data_byte = (unsigned char*)m_pHeightData;
427 delete [] data_byte;
428 break;
429
430 // short
431 case 2:
432 data_short = (short*)m_pHeightData;
433 delete [] data_short;
434 break;
435
436 // float
437 case 3:
438 data_float = (float*)m_pHeightData;
439 delete [] data_float;
440 break;
441
442 // double
443 case 4:
444 data_double = (double*)m_pHeightData;
445 delete [] data_double;
446 break;
447
448 }
449 }
450}
451
452
453//////// dxHeightfield /////////////////////////////////////////////////////////////////
454
455
456// dxHeightfield constructor
457dxHeightfield::dxHeightfield( dSpaceID space,
458 dHeightfieldDataID data,
459 int bPlaceable ) :
460 dxGeom( space, bPlaceable ),
461 tempPlaneBuffer(0),
462 tempPlaneInstances(0),
463 tempPlaneBufferSize(0),
464 tempTriangleBuffer(0),
465 tempTriangleBufferSize(0),
466 tempHeightBuffer(0),
467 tempHeightInstances(0),
468 tempHeightBufferSizeX(0),
469 tempHeightBufferSizeZ(0)
470{
471 type = dHeightfieldClass;
472 this->m_p_data = data;
473}
474
475
476// compute axis aligned bounding box
477void dxHeightfield::computeAABB()
478{
479 const dxHeightfieldData *d = m_p_data;
480
481 if ( d->m_bWrapMode == 0 )
482 {
483 // Finite
484 if ( gflags & GEOM_PLACEABLE )
485 {
486 dReal dx[6], dy[6], dz[6];
487
488 // Y-axis
489 dy[0] = ( final_posr->R[ 1] * d->m_fMinHeight );
490 dy[1] = ( final_posr->R[ 5] * d->m_fMinHeight );
491 dy[2] = ( final_posr->R[ 9] * d->m_fMinHeight );
492 dy[3] = ( final_posr->R[ 1] * d->m_fMaxHeight );
493 dy[4] = ( final_posr->R[ 5] * d->m_fMaxHeight );
494 dy[5] = ( final_posr->R[ 9] * d->m_fMaxHeight );
495
496#ifdef DHEIGHTFIELD_CORNER_ORIGIN
497
498 // X-axis
499 dx[0] = 0; dx[3] = ( final_posr->R[ 0] * d->m_fWidth );
500 dx[1] = 0; dx[4] = ( final_posr->R[ 4] * d->m_fWidth );
501 dx[2] = 0; dx[5] = ( final_posr->R[ 8] * d->m_fWidth );
502
503 // Z-axis
504 dz[0] = 0; dz[3] = ( final_posr->R[ 2] * d->m_fDepth );
505 dz[1] = 0; dz[4] = ( final_posr->R[ 6] * d->m_fDepth );
506 dz[2] = 0; dz[5] = ( final_posr->R[10] * d->m_fDepth );
507
508#else // DHEIGHTFIELD_CORNER_ORIGIN
509
510 // X-axis
511 dx[0] = ( final_posr->R[ 0] * -d->m_fHalfWidth );
512 dx[1] = ( final_posr->R[ 4] * -d->m_fHalfWidth );
513 dx[2] = ( final_posr->R[ 8] * -d->m_fHalfWidth );
514 dx[3] = ( final_posr->R[ 0] * d->m_fHalfWidth );
515 dx[4] = ( final_posr->R[ 4] * d->m_fHalfWidth );
516 dx[5] = ( final_posr->R[ 8] * d->m_fHalfWidth );
517
518 // Z-axis
519 dz[0] = ( final_posr->R[ 2] * -d->m_fHalfDepth );
520 dz[1] = ( final_posr->R[ 6] * -d->m_fHalfDepth );
521 dz[2] = ( final_posr->R[10] * -d->m_fHalfDepth );
522 dz[3] = ( final_posr->R[ 2] * d->m_fHalfDepth );
523 dz[4] = ( final_posr->R[ 6] * d->m_fHalfDepth );
524 dz[5] = ( final_posr->R[10] * d->m_fHalfDepth );
525
526#endif // DHEIGHTFIELD_CORNER_ORIGIN
527
528 // X extents
529 aabb[0] = final_posr->pos[0] +
530 dMIN3( dMIN( dx[0], dx[3] ), dMIN( dy[0], dy[3] ), dMIN( dz[0], dz[3] ) );
531 aabb[1] = final_posr->pos[0] +
532 dMAX3( dMAX( dx[0], dx[3] ), dMAX( dy[0], dy[3] ), dMAX( dz[0], dz[3] ) );
533
534 // Y extents
535 aabb[2] = final_posr->pos[1] +
536 dMIN3( dMIN( dx[1], dx[4] ), dMIN( dy[1], dy[4] ), dMIN( dz[1], dz[4] ) );
537 aabb[3] = final_posr->pos[1] +
538 dMAX3( dMAX( dx[1], dx[4] ), dMAX( dy[1], dy[4] ), dMAX( dz[1], dz[4] ) );
539
540 // Z extents
541 aabb[4] = final_posr->pos[2] +
542 dMIN3( dMIN( dx[2], dx[5] ), dMIN( dy[2], dy[5] ), dMIN( dz[2], dz[5] ) );
543 aabb[5] = final_posr->pos[2] +
544 dMAX3( dMAX( dx[2], dx[5] ), dMAX( dy[2], dy[5] ), dMAX( dz[2], dz[5] ) );
545 }
546 else
547 {
548
549#ifdef DHEIGHTFIELD_CORNER_ORIGIN
550
551 aabb[0] = 0; aabb[1] = d->m_fWidth;
552 aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
553 aabb[4] = 0; aabb[5] = d->m_fDepth;
554
555#else // DHEIGHTFIELD_CORNER_ORIGIN
556
557 aabb[0] = -d->m_fHalfWidth; aabb[1] = +d->m_fHalfWidth;
558 aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
559 aabb[4] = -d->m_fHalfDepth; aabb[5] = +d->m_fHalfDepth;
560
561#endif // DHEIGHTFIELD_CORNER_ORIGIN
562
563 }
564 }
565 else
566 {
567 // Infinite
568 if ( gflags & GEOM_PLACEABLE )
569 {
570 aabb[0] = -dInfinity; aabb[1] = +dInfinity;
571 aabb[2] = -dInfinity; aabb[3] = +dInfinity;
572 aabb[4] = -dInfinity; aabb[5] = +dInfinity;
573 }
574 else
575 {
576 aabb[0] = -dInfinity; aabb[1] = +dInfinity;
577 aabb[2] = d->m_fMinHeight; aabb[3] = d->m_fMaxHeight;
578 aabb[4] = -dInfinity; aabb[5] = +dInfinity;
579 }
580 }
581
582}
583
584
585// dxHeightfield destructor
586dxHeightfield::~dxHeightfield()
587{
588 resetTriangleBuffer();
589 resetPlaneBuffer();
590 resetHeightBuffer();
591}
592
593void dxHeightfield::allocateTriangleBuffer(size_t numTri)
594{
595 size_t alignedNumTri = AlignBufferSize(numTri, TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT);
596 tempTriangleBufferSize = alignedNumTri;
597 tempTriangleBuffer = new HeightFieldTriangle[alignedNumTri];
598}
599
600void dxHeightfield::resetTriangleBuffer()
601{
602 delete[] tempTriangleBuffer;
603}
604
605void dxHeightfield::allocatePlaneBuffer(size_t numTri)
606{
607 size_t alignedNumTri = AlignBufferSize(numTri, TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT);
608 tempPlaneBufferSize = alignedNumTri;
609 tempPlaneBuffer = new HeightFieldPlane *[alignedNumTri];
610 tempPlaneInstances = new HeightFieldPlane[alignedNumTri];
611
612 HeightFieldPlane *ptrPlaneMatrix = tempPlaneInstances;
613 for (size_t indexTri = 0; indexTri != alignedNumTri; indexTri++)
614 {
615 tempPlaneBuffer[indexTri] = ptrPlaneMatrix;
616 ptrPlaneMatrix += 1;
617 }
618}
619
620void dxHeightfield::resetPlaneBuffer()
621{
622 delete[] tempPlaneInstances;
623 delete[] tempPlaneBuffer;
624}
625
626void dxHeightfield::allocateHeightBuffer(size_t numX, size_t numZ)
627{
628 size_t alignedNumX = AlignBufferSize(numX, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X);
629 size_t alignedNumZ = AlignBufferSize(numZ, TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z);
630 tempHeightBufferSizeX = alignedNumX;
631 tempHeightBufferSizeZ = alignedNumZ;
632 tempHeightBuffer = new HeightFieldVertex *[alignedNumX];
633 size_t numCells = alignedNumX * alignedNumZ;
634 tempHeightInstances = new HeightFieldVertex [numCells];
635
636 HeightFieldVertex *ptrHeightMatrix = tempHeightInstances;
637 for (size_t indexX = 0; indexX != alignedNumX; indexX++)
638 {
639 tempHeightBuffer[indexX] = ptrHeightMatrix;
640 ptrHeightMatrix += alignedNumZ;
641 }
642}
643
644void dxHeightfield::resetHeightBuffer()
645{
646 delete[] tempHeightInstances;
647 delete[] tempHeightBuffer;
648}
649//////// Heightfield data interface ////////////////////////////////////////////////////
650
651
652dHeightfieldDataID dGeomHeightfieldDataCreate()
653{
654 return new dxHeightfieldData();
655}
656
657
658void dGeomHeightfieldDataBuildCallback( dHeightfieldDataID d,
659 void* pUserData, dHeightfieldGetHeight* pCallback,
660 dReal width, dReal depth, int widthSamples, int depthSamples,
661 dReal scale, dReal offset, dReal thickness, int bWrap )
662{
663 dUASSERT( d, "argument not Heightfield data" );
664 dIASSERT( pCallback );
665 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
666 dIASSERT( depthSamples >= 2 );
667
668 // callback
669 d->m_nGetHeightMode = 0;
670 d->m_pUserData = pUserData;
671 d->m_pGetHeightCallback = pCallback;
672
673 // set info
674 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
675
676 // default bounds
677 d->m_fMinHeight = -dInfinity;
678 d->m_fMaxHeight = dInfinity;
679}
680
681
682void dGeomHeightfieldDataBuildByte( dHeightfieldDataID d,
683 const unsigned char *pHeightData, int bCopyHeightData,
684 dReal width, dReal depth, int widthSamples, int depthSamples,
685 dReal scale, dReal offset, dReal thickness, int bWrap )
686{
687 dUASSERT( d, "Argument not Heightfield data" );
688 dIASSERT( pHeightData );
689 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
690 dIASSERT( depthSamples >= 2 );
691
692 // set info
693 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
694 d->m_nGetHeightMode = 1;
695 d->m_bCopyHeightData = bCopyHeightData;
696
697 if ( d->m_bCopyHeightData == 0 )
698 {
699 // Data is referenced only.
700 d->m_pHeightData = pHeightData;
701 }
702 else
703 {
704 // We own the height data, allocate storage
705 d->m_pHeightData = new unsigned char[ d->m_nWidthSamples * d->m_nDepthSamples ];
706 dIASSERT( d->m_pHeightData );
707
708 // Copy data.
709 memcpy( (void*)d->m_pHeightData, pHeightData,
710 sizeof( unsigned char ) * d->m_nWidthSamples * d->m_nDepthSamples );
711 }
712
713 // Find height bounds
714 d->ComputeHeightBounds();
715}
716
717
718void dGeomHeightfieldDataBuildShort( dHeightfieldDataID d,
719 const short* pHeightData, int bCopyHeightData,
720 dReal width, dReal depth, int widthSamples, int depthSamples,
721 dReal scale, dReal offset, dReal thickness, int bWrap )
722{
723 dUASSERT( d, "Argument not Heightfield data" );
724 dIASSERT( pHeightData );
725 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
726 dIASSERT( depthSamples >= 2 );
727
728 // set info
729 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
730 d->m_nGetHeightMode = 2;
731 d->m_bCopyHeightData = bCopyHeightData;
732
733 if ( d->m_bCopyHeightData == 0 )
734 {
735 // Data is referenced only.
736 d->m_pHeightData = pHeightData;
737 }
738 else
739 {
740 // We own the height data, allocate storage
741 d->m_pHeightData = new short[ d->m_nWidthSamples * d->m_nDepthSamples ];
742 dIASSERT( d->m_pHeightData );
743
744 // Copy data.
745 memcpy( (void*)d->m_pHeightData, pHeightData,
746 sizeof( short ) * d->m_nWidthSamples * d->m_nDepthSamples );
747 }
748
749 // Find height bounds
750 d->ComputeHeightBounds();
751}
752
753
754void dGeomHeightfieldDataBuildSingle( dHeightfieldDataID d,
755 const float *pHeightData, int bCopyHeightData,
756 dReal width, dReal depth, int widthSamples, int depthSamples,
757 dReal scale, dReal offset, dReal thickness, int bWrap )
758{
759 dUASSERT( d, "Argument not Heightfield data" );
760 dIASSERT( pHeightData );
761 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
762 dIASSERT( depthSamples >= 2 );
763
764 // set info
765 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
766 d->m_nGetHeightMode = 3;
767 d->m_bCopyHeightData = bCopyHeightData;
768
769 if ( d->m_bCopyHeightData == 0 )
770 {
771 // Data is referenced only.
772 d->m_pHeightData = pHeightData;
773 }
774 else
775 {
776 // We own the height data, allocate storage
777 d->m_pHeightData = new float[ d->m_nWidthSamples * d->m_nDepthSamples ];
778 dIASSERT( d->m_pHeightData );
779
780 // Copy data.
781 memcpy( (void*)d->m_pHeightData, pHeightData,
782 sizeof( float ) * d->m_nWidthSamples * d->m_nDepthSamples );
783 }
784
785 // Find height bounds
786 d->ComputeHeightBounds();
787}
788
789void dGeomHeightfieldDataBuildDouble( dHeightfieldDataID d,
790 const double *pHeightData, int bCopyHeightData,
791 dReal width, dReal depth, int widthSamples, int depthSamples,
792 dReal scale, dReal offset, dReal thickness, int bWrap )
793{
794 dUASSERT( d, "Argument not Heightfield data" );
795 dIASSERT( pHeightData );
796 dIASSERT( widthSamples >= 2 ); // Ensure we're making something with at least one cell.
797 dIASSERT( depthSamples >= 2 );
798
799 // set info
800 d->SetData( widthSamples, depthSamples, width, depth, scale, offset, thickness, bWrap );
801 d->m_nGetHeightMode = 4;
802 d->m_bCopyHeightData = bCopyHeightData;
803
804 if ( d->m_bCopyHeightData == 0 )
805 {
806 // Data is referenced only.
807 d->m_pHeightData = pHeightData;
808 }
809 else
810 {
811 // We own the height data, allocate storage
812 d->m_pHeightData = new double[ d->m_nWidthSamples * d->m_nDepthSamples ];
813 dIASSERT( d->m_pHeightData );
814
815 // Copy data.
816 memcpy( (void*)d->m_pHeightData, pHeightData,
817 sizeof( double ) * d->m_nWidthSamples * d->m_nDepthSamples );
818 }
819
820 // Find height bounds
821 d->ComputeHeightBounds();
822}
823
824
825
826
827void dGeomHeightfieldDataSetBounds( dHeightfieldDataID d, dReal minHeight, dReal maxHeight )
828{
829 dUASSERT(d, "Argument not Heightfield data");
830 d->m_fMinHeight = ( minHeight * d->m_fScale ) + d->m_fOffset - d->m_fThickness;
831 d->m_fMaxHeight = ( maxHeight * d->m_fScale ) + d->m_fOffset;
832}
833
834
835void dGeomHeightfieldDataDestroy( dHeightfieldDataID d )
836{
837 dUASSERT(d, "argument not Heightfield data");
838 delete d;
839}
840
841
842//////// Heightfield geom interface ////////////////////////////////////////////////////
843
844
845dGeomID dCreateHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable )
846{
847 return new dxHeightfield( space, data, bPlaceable );
848}
849
850
851void dGeomHeightfieldSetHeightfieldData( dGeomID g, dHeightfieldDataID d )
852{
853 dxHeightfield* geom = (dxHeightfield*) g;
854 geom->data = d;
855}
856
857
858dHeightfieldDataID dGeomHeightfieldGetHeightfieldData( dGeomID g )
859{
860 dxHeightfield* geom = (dxHeightfield*) g;
861 return geom->m_p_data;
862}
863
864//////// dxHeightfield /////////////////////////////////////////////////////////////////
865
866
867// Typedef for generic 'get point depth' function
868typedef dReal dGetDepthFn( dGeomID g, dReal x, dReal y, dReal z );
869
870
871#define DMESS(A) \
872 dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).", \
873 x,z,A, \
874 pContact->depth, \
875 dGeomSphereGetRadius(o2), \
876 pContact->pos[0], \
877 pContact->pos[1], \
878 pContact->pos[2], \
879 pContact->normal[0], \
880 pContact->normal[1], \
881 pContact->normal[2]);
882
883static inline bool DescendingTriangleSort(const HeightFieldTriangle * const A, const HeightFieldTriangle * const B)
884{
885 return ((A->maxAAAB - B->maxAAAB) > dEpsilon);
886}
887static inline bool DescendingPlaneSort(const HeightFieldPlane * const A, const HeightFieldPlane * const B)
888{
889 return ((A->maxAAAB - B->maxAAAB) > dEpsilon);
890}
891
892void dxHeightfield::sortPlanes(const size_t numPlanes)
893{
894 bool has_swapped = true;
895 do
896 {
897 has_swapped = false;//reset flag
898 for (size_t i = 0; i < numPlanes - 1; i++)
899 {
900 //if they are in the wrong order
901 if (DescendingPlaneSort(tempPlaneBuffer[i], tempPlaneBuffer[i + 1]))
902 {
903 //exchange them
904 HeightFieldPlane * tempPlane = tempPlaneBuffer[i];
905 tempPlaneBuffer[i] = tempPlaneBuffer[i + 1];
906 tempPlaneBuffer[i + 1] = tempPlane;
907
908 //we have swapped at least once, list may not be sorted yet
909 has_swapped = true;
910 }
911 }
912 } //if no swaps were made during this pass, the list has been sorted
913 while (has_swapped);
914}
915
916static inline dReal DistancePointToLine(const dVector3 &_point,
917 const dVector3 &_pt0,
918 const dVector3 &_Edge,
919 const dReal _Edgelength)
920{
921 dVector3 v;
922 dVector3Subtract(_point, _pt0, v);
923 dVector3 s;
924 dVector3Copy (_Edge, s);
925 const dReal dot = dVector3Dot(v, _Edge) / _Edgelength;
926 dVector3Scale(s, dot);
927 dVector3Subtract(v, s, v);
928 return dVector3Length(v);
929}
930
931
932
933
934int dxHeightfield::dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,
935 dxGeom* o2, const int numMaxContactsPossible,
936 int flags, dContactGeom* contact,
937 int skip )
938{
939 dContactGeom *pContact = 0;
940 int x, z;
941 // check if not above or inside terrain first
942 // while filling a heightmap partial temporary buffer
943 const unsigned int numX = (maxX - minX) + 1;
944 const unsigned int numZ = (maxZ - minZ) + 1;
945 const dReal minO2Height = o2->aabb[2];
946 const dReal maxO2Height = o2->aabb[3];
947 unsigned int x_local, z_local;
948 dReal maxY = - dInfinity;
949 dReal minY = dInfinity;
950 // localize and const for faster access
951 const dReal cfSampleWidth = m_p_data->m_fSampleWidth;
952 const dReal cfSampleDepth = m_p_data->m_fSampleDepth;
953 {
954 if (tempHeightBufferSizeX < numX || tempHeightBufferSizeZ < numZ)
955 {
956 resetHeightBuffer();
957 allocateHeightBuffer(numX, numZ);
958 }
959
960 dReal Xpos, Ypos;
961 Xpos = minX * cfSampleWidth;
962
963
964 for ( x = minX, x_local = 0; x_local < numX; x++, x_local++)
965 {
966 const dReal c_Xpos = Xpos;
967 HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
968 Ypos = minZ * cfSampleDepth;
969 for ( z = minZ, z_local = 0; z_local < numZ; z++, z_local++)
970 {
971 const dReal h = m_p_data->GetHeight(x, z);
972 HeightFieldRow[z_local].vertex[0] = c_Xpos;
973 HeightFieldRow[z_local].vertex[1] = h;
974 HeightFieldRow[z_local].vertex[2] = Ypos;
975
976
977 maxY = dMAX(maxY, h);
978 minY = dMIN(minY, h);
979
980
981 Ypos += cfSampleDepth;
982 }
983 Xpos += cfSampleWidth;
984 }
985 if (minO2Height - maxY > -dEpsilon )
986 {
987 //totally above heightfield
988 return 0;
989 }
990 if (minY - maxO2Height > -dEpsilon )
991 {
992 // totally under heightfield
993 pContact = CONTACT(contact, 0);
994
995 pContact->pos[0] = o2->final_posr->pos[0];
996 pContact->pos[1] = minY;
997 pContact->pos[2] = o2->final_posr->pos[2];
998
999 pContact->normal[0] = 0;
1000 pContact->normal[1] = - 1;
1001 pContact->normal[2] = 0;
1002
1003 pContact->depth = minY - maxO2Height;
1004
1005 return 1;
1006 }
1007 }
1008 // get All Planes that could collide against.
1009 dColliderFn *geomRayNCollider;
1010 dColliderFn *geomNPlaneCollider;
1011 dGetDepthFn *geomNDepthGetter;
1012
1013 // int max_collisionContact = numMaxContactsPossible; -- not used
1014 switch (o2->type)
1015 {
1016 case dRayClass:
1017 geomRayNCollider = NULL;
1018 geomNPlaneCollider = dCollideRayPlane;
1019 geomNDepthGetter = NULL;
1020 //max_collisionContact = 1;
1021 break;
1022
1023 case dSphereClass:
1024 geomRayNCollider = dCollideRaySphere;
1025 geomNPlaneCollider = dCollideSpherePlane;
1026 geomNDepthGetter = dGeomSpherePointDepth;
1027 //max_collisionContact = 3;
1028 break;
1029
1030 case dBoxClass:
1031 geomRayNCollider = dCollideRayBox;
1032 geomNPlaneCollider = dCollideBoxPlane;
1033 geomNDepthGetter = dGeomBoxPointDepth;
1034 //max_collisionContact = 8;
1035 break;
1036
1037 case dCapsuleClass:
1038 geomRayNCollider = dCollideRayCapsule;
1039 geomNPlaneCollider = dCollideCapsulePlane;
1040 geomNDepthGetter = dGeomCapsulePointDepth;
1041 // max_collisionContact = 3;
1042 break;
1043
1044 case dCylinderClass:
1045 geomRayNCollider = dCollideRayCylinder;
1046 geomNPlaneCollider = dCollideCylinderPlane;
1047 geomNDepthGetter = NULL;// TODO: dGeomCCylinderPointDepth
1048 //max_collisionContact = 3;
1049 break;
1050
1051 case dConvexClass:
1052 geomRayNCollider = dCollideRayConvex;
1053 geomNPlaneCollider = dCollideConvexPlane;
1054 geomNDepthGetter = NULL;// TODO: dGeomConvexPointDepth;
1055 //max_collisionContact = 3;
1056 break;
1057
1058#if dTRIMESH_ENABLED
1059
1060 case dTriMeshClass:
1061 geomRayNCollider = dCollideRayTrimesh;
1062 geomNPlaneCollider = dCollideTrimeshPlane;
1063 geomNDepthGetter = NULL;// TODO: dGeomTrimeshPointDepth;
1064 //max_collisionContact = 3;
1065 break;
1066
1067#endif // dTRIMESH_ENABLED
1068
1069 default:
1070 dIASSERT(0); // Shouldn't ever get here.
1071 break;
1072
1073 }
1074
1075 dxPlane myplane(0,0,0,0,0);
1076 dxPlane* sliding_plane = &myplane;
1077 dReal triplane[4];
1078 int i;
1079
1080 // check some trivial case.
1081 // Vector Up plane
1082 if (maxY - minY < dEpsilon)
1083 {
1084 // it's a single plane.
1085 triplane[0] = 0;
1086 triplane[1] = 1;
1087 triplane[2] = 0;
1088 triplane[3] = minY;
1089 dGeomPlaneSetNoNormalize (sliding_plane, triplane);
1090 // find collision and compute contact points
1091 const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip);
1092 dIASSERT(numTerrainContacts <= numMaxContactsPossible);
1093 for (i = 0; i < numTerrainContacts; i++)
1094 {
1095 pContact = CONTACT(contact, i*skip);
1096 dOPESIGN(pContact->normal, =, -, triplane);
1097 }
1098 return numTerrainContacts;
1099 }
1100 // unique plane
1101 {
1102 // check for very simple plane heightfield
1103 dReal minXHeightDelta = dInfinity, maxXHeightDelta = - dInfinity;
1104 dReal minZHeightDelta = dInfinity, maxZHeightDelta = - dInfinity;
1105
1106
1107 dReal lastXHeight = tempHeightBuffer[0][0].vertex[1];
1108 for ( x_local = 1; x_local < numX; x_local++)
1109 {
1110 HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
1111
1112 const dReal deltaX = HeightFieldRow[0].vertex[1] - lastXHeight;
1113
1114 maxXHeightDelta = dMAX (maxXHeightDelta, deltaX);
1115 minXHeightDelta = dMIN (minXHeightDelta, deltaX);
1116
1117 dReal lastZHeight = HeightFieldRow[0].vertex[1];
1118 for ( z_local = 1; z_local < numZ; z_local++)
1119 {
1120 const dReal deltaZ = (HeightFieldRow[z_local].vertex[1] - lastZHeight);
1121
1122 maxZHeightDelta = dMAX (maxZHeightDelta, deltaZ);
1123 minZHeightDelta = dMIN (minZHeightDelta, deltaZ);
1124
1125 }
1126 }
1127
1128 if (maxZHeightDelta - minZHeightDelta < dEpsilon &&
1129 maxXHeightDelta - minXHeightDelta < dEpsilon )
1130 {
1131 // it's a single plane.
1132 const dVector3 &A = tempHeightBuffer[0][0].vertex;
1133 const dVector3 &B = tempHeightBuffer[1][0].vertex;
1134 const dVector3 &C = tempHeightBuffer[0][1].vertex;
1135
1136 // define 2 edges and a point that will define collision plane
1137 {
1138 dVector3 Edge1, Edge2;
1139 dVector3Subtract(C, A, Edge1);
1140 dVector3Subtract(B, A, Edge2);
1141 dVector3Cross(Edge1, Edge2, triplane);
1142 }
1143
1144 // Define Plane
1145 // Normalize plane normal
1146 const dReal dinvlength = REAL(1.0) / dVector3Length(triplane);
1147 triplane[0] *= dinvlength;
1148 triplane[1] *= dinvlength;
1149 triplane[2] *= dinvlength;
1150 // get distance to origin from plane
1151 triplane[3] = dVector3Dot(triplane, A);
1152
1153 dGeomPlaneSetNoNormalize (sliding_plane, triplane);
1154 // find collision and compute contact points
1155 const int numTerrainContacts = geomNPlaneCollider (o2, sliding_plane, flags, contact, skip);
1156 dIASSERT(numTerrainContacts <= numMaxContactsPossible);
1157 for (i = 0; i < numTerrainContacts; i++)
1158 {
1159 pContact = CONTACT(contact, i*skip);
1160 dOPESIGN(pContact->normal, =, -, triplane);
1161 }
1162 return numTerrainContacts;
1163 }
1164 }
1165
1166
1167 int numTerrainContacts = 0;
1168 dContactGeom *PlaneContact = m_p_data->m_contacts;
1169
1170 const unsigned int numTriMax = (maxX - minX) * (maxZ - minZ) * 2;
1171 if (tempTriangleBufferSize < numTriMax)
1172 {
1173 resetTriangleBuffer();
1174 allocateTriangleBuffer(numTriMax);
1175 }
1176
1177 // Sorting triangle/plane resulting from heightfield zone
1178 // Perhaps that would be necessary in case of too much limited
1179 // maximum contact point...
1180 // or in complex mesh case (trimesh and convex)
1181 // need some test or insights on this before enabling this.
1182 const bool isContactNumPointsLimited =
1183 true;
1184 // (numMaxContacts < 8)
1185 // || o2->type == dConvexClass
1186 // || o2->type == dTriMeshClass
1187 // || (numMaxContacts < (int)numTriMax)
1188
1189
1190
1191 // if small heightfield triangle related to O2 colliding
1192 // or no Triangle colliding at all.
1193 bool needFurtherPasses = (o2->type == dTriMeshClass);
1194 //compute Ratio between Triangle size and O2 aabb size
1195 // no FurtherPasses are needed in ray class
1196 if (o2->type != dRayClass && needFurtherPasses == false)
1197 {
1198 const dReal xratio = (o2->aabb[1] - o2->aabb[0]) * m_p_data->m_fInvSampleWidth;
1199 if (xratio > REAL(1.5))
1200 needFurtherPasses = true;
1201 else
1202 {
1203 const dReal zratio = (o2->aabb[5] - o2->aabb[4]) * m_p_data->m_fInvSampleDepth;
1204 if (zratio > REAL(1.5))
1205 needFurtherPasses = true;
1206 }
1207
1208 }
1209
1210 unsigned int numTri = 0;
1211 HeightFieldVertex *A, *B, *C, *D;
1212 /* (y is up)
1213 A--------B-...x
1214 | /|
1215 | / |
1216 | / |
1217 | / |
1218 | / |
1219 | / |
1220 | / |
1221 |/ |
1222 C--------D
1223 .
1224 .
1225 .
1226 z
1227 */
1228 // keep only triangle that does intersect geom
1229 for ( x = minX, x_local = 0; x < maxX; x++, x_local++)
1230 {
1231 HeightFieldVertex *HeightFieldRow = tempHeightBuffer[x_local];
1232 HeightFieldVertex *HeightFieldNextRow = tempHeightBuffer[x_local + 1];
1233
1234 // First A
1235 C = &HeightFieldRow [0];
1236 // First B
1237 D = &HeightFieldNextRow[0];
1238 for ( z = minZ, z_local = 0; z < maxZ; z++, z_local++)
1239 {
1240 A = C;
1241 B = D;
1242
1243 C = &HeightFieldRow [z_local + 1];
1244 D = &HeightFieldNextRow[z_local + 1];
1245
1246 const dReal AHeight = A->vertex[1];
1247 const dReal BHeight = B->vertex[1];
1248 const dReal CHeight = C->vertex[1];
1249 const dReal DHeight = D->vertex[1];
1250
1251 const bool isACollide = 0 < AHeight - minO2Height;
1252 const bool isBCollide = 0 < BHeight - minO2Height;
1253 const bool isCCollide = 0 < CHeight - minO2Height;
1254 const bool isDCollide = 0 < DHeight - minO2Height;
1255
1256 A->state = !(isACollide);
1257 B->state = !(isBCollide);
1258 C->state = !(isCCollide);
1259 D->state = !(isCCollide);
1260
1261 if (isACollide || isBCollide || isCCollide)
1262 {
1263 HeightFieldTriangle * const CurrTriUp = &tempTriangleBuffer[numTri++];
1264
1265 CurrTriUp->state = false;
1266
1267 // changing point order here implies to change it in isOnHeightField
1268 CurrTriUp->vertices[0] = A;
1269 CurrTriUp->vertices[1] = B;
1270 CurrTriUp->vertices[2] = C;
1271
1272 if (isContactNumPointsLimited)
1273 CurrTriUp->setMinMax();
1274 CurrTriUp->isUp = true;
1275 }
1276
1277 if (isBCollide || isCCollide || isDCollide)
1278 {
1279 HeightFieldTriangle * const CurrTriDown = &tempTriangleBuffer[numTri++];
1280
1281 CurrTriDown->state = false;
1282 // changing point order here implies to change it in isOnHeightField
1283
1284 CurrTriDown->vertices[0] = D;
1285 CurrTriDown->vertices[1] = B;
1286 CurrTriDown->vertices[2] = C;
1287
1288
1289 if (isContactNumPointsLimited)
1290 CurrTriDown->setMinMax();
1291 CurrTriDown->isUp = false;
1292 }
1293
1294
1295 if (needFurtherPasses &&
1296 (isBCollide || isCCollide)
1297 &&
1298 (AHeight - CHeight > 0 &&
1299 AHeight - BHeight > 0 &&
1300 DHeight - CHeight > 0 &&
1301 DHeight - BHeight > 0))
1302 {
1303 // That means Edge BC is concave, therefore
1304 // BC Edge and B and C vertices cannot collide
1305
1306 B->state = true;
1307 C->state = true;
1308 }
1309 // should find a way to check other edges (AB, BD, CD) too for concavity
1310 }
1311 }
1312
1313 // at least on triangle should intersect geom
1314 dIASSERT (numTri != 0);
1315 // pass1: VS triangle as Planes
1316 // Group Triangle by same plane definition
1317 // as Terrain often has many triangles using same plane definition
1318 // then collide against that list of triangles.
1319 {
1320
1321 dVector3 Edge1, Edge2;
1322 //compute all triangles normals.
1323 for (unsigned int k = 0; k < numTri; k++)
1324 {
1325 HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
1326
1327 // define 2 edges and a point that will define collision plane
1328 dVector3Subtract(itTriangle->vertices[2]->vertex, itTriangle->vertices[0]->vertex, Edge1);
1329 dVector3Subtract(itTriangle->vertices[1]->vertex, itTriangle->vertices[0]->vertex, Edge2);
1330
1331 // find a perpendicular vector to the triangle
1332 if (itTriangle->isUp)
1333 dVector3Cross(Edge1, Edge2, triplane);
1334 else
1335 dVector3Cross(Edge2, Edge1, triplane);
1336
1337 // Define Plane
1338 // Normalize plane normal
1339 const dReal dinvlength = REAL(1.0) / dVector3Length(triplane);
1340 triplane[0] *= dinvlength;
1341 triplane[1] *= dinvlength;
1342 triplane[2] *= dinvlength;
1343 // get distance to origin from plane
1344 triplane[3] = dVector3Dot(triplane, itTriangle->vertices[0]->vertex);
1345
1346 // saves normal for collision check (planes, triangles, vertices and edges.)
1347 dVector3Copy(triplane, itTriangle->planeDef);
1348 // saves distance for collision check (planes, triangles, vertices and edges.)
1349 itTriangle->planeDef[3] = triplane[3];
1350 }
1351
1352 // group by Triangles by Planes sharing shame plane definition
1353 if (tempPlaneBufferSize < numTri)
1354 {
1355 resetPlaneBuffer();
1356 allocatePlaneBuffer(numTri);
1357 }
1358 unsigned int numPlanes = 0;
1359 for (unsigned int k = 0; k < numTri; k++)
1360 {
1361 HeightFieldTriangle * const tri_base = &tempTriangleBuffer[k];
1362
1363 if (tri_base->state == true)
1364 continue;// already tested or added to plane list.
1365
1366 HeightFieldPlane * const currPlane = tempPlaneBuffer[numPlanes];
1367 currPlane->resetTriangleListSize(numTri - k);
1368 currPlane->addTriangle(tri_base);
1369 // saves normal for collision check (planes, triangles, vertices and edges.)
1370 dVector3Copy(tri_base->planeDef, currPlane->planeDef);
1371 // saves distance for collision check (planes, triangles, vertices and edges.)
1372 currPlane->planeDef[3]= tri_base->planeDef[3];
1373
1374 const dReal normx = tri_base->planeDef[0];
1375 const dReal normy = tri_base->planeDef[1];
1376 const dReal normz = tri_base->planeDef[2];
1377 const dReal dist = tri_base->planeDef[3];
1378
1379 for (unsigned int m = k + 1; m < numTri; m++)
1380 {
1381
1382 HeightFieldTriangle * const tri_test = &tempTriangleBuffer[m];
1383 if (tri_test->state == true)
1384 continue;// already tested or added to plane list.
1385
1386 // normals and distance are the same.
1387 if (
1388 dFabs(normy - tri_test->planeDef[1]) < dEpsilon &&
1389 dFabs(dist - tri_test->planeDef[3]) < dEpsilon &&
1390 dFabs(normx - tri_test->planeDef[0]) < dEpsilon &&
1391 dFabs(normz - tri_test->planeDef[2]) < dEpsilon
1392 )
1393 {
1394 currPlane->addTriangle (tri_test);
1395 tri_test->state = true;
1396 }
1397 }
1398
1399 tri_base->state = true;
1400 if (isContactNumPointsLimited)
1401 currPlane->setMinMax();
1402
1403 numPlanes++;
1404 }
1405
1406 // sort planes
1407 if (isContactNumPointsLimited)
1408 sortPlanes(numPlanes);
1409
1410#if !defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
1411 /*
1412 Note by Oleh_Derevenko:
1413 It seems to be incorrect to limit contact count by some particular value
1414 since some of them (and even all of them) may be culled in following condition.
1415 However I do not see an easy way to fix this.
1416 If not that culling the flags modification should be changed here and
1417 additionally repeated after some contacts have been generated (in "if (didCollide)").
1418 The maximum of contacts in flags would then be set to minimum of contacts
1419 remaining and HEIGHTFIELDMAXCONTACTPERCELL.
1420 */
1421 int planeTestFlags = (flags & ~NUMC_MASK) | HEIGHTFIELDMAXCONTACTPERCELL;
1422 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1423#else // if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
1424 int numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1425 int planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane;
1426 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1427#endif
1428
1429 for (unsigned int k = 0; k < numPlanes; k++)
1430 {
1431 HeightFieldPlane * const itPlane = tempPlaneBuffer[k];
1432
1433 //set Geom
1434 dGeomPlaneSetNoNormalize (sliding_plane, itPlane->planeDef);
1435 //dGeomPlaneSetParams (sliding_plane, triangle_Plane[0], triangle_Plane[1], triangle_Plane[2], triangle_Plane[3]);
1436 // find collision and compute contact points
1437 bool didCollide = false;
1438 const int numPlaneContacts = geomNPlaneCollider (o2, sliding_plane, planeTestFlags, PlaneContact, sizeof(dContactGeom));
1439 const size_t planeTriListSize = itPlane->trianglelistCurrentSize;
1440 for (i = 0; i < numPlaneContacts; i++)
1441 {
1442 // Check if contact point found in plane is inside Triangle.
1443 const dVector3 &pCPos = PlaneContact[i].pos;
1444 for (size_t b = 0; planeTriListSize > b; b++)
1445 {
1446 if (m_p_data->IsOnHeightfield2 (itPlane->trianglelist[b]->vertices[0]->vertex,
1447 pCPos,
1448 itPlane->trianglelist[b]->isUp))
1449 {
1450 pContact = CONTACT(contact, numTerrainContacts*skip);
1451 dVector3Copy(pCPos, pContact->pos);
1452 dOPESIGN(pContact->normal, =, -, itPlane->planeDef);
1453 pContact->depth = PlaneContact[i].depth;
1454 numTerrainContacts++;
1455 if ( numTerrainContacts == numMaxContactsPossible )
1456 return numTerrainContacts;
1457
1458 didCollide = true;
1459 break;
1460 }
1461 }
1462 }
1463 if (didCollide)
1464 {
1465#if defined(NO_CONTACT_CULLING_BY_ISONHEIGHTFIELD2)
1466 /* Note by Oleh_Derevenko:
1467 This code is not used - see another note above
1468 */
1469 numMaxContactsPerPlane = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1470 planeTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerPlane;
1471 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1472#endif
1473 for (size_t b = 0; planeTriListSize > b; b++)
1474 {
1475 // flag Triangles Vertices as collided
1476 // to prevent any collision test of those
1477 for (i = 0; i < 3; i++)
1478 itPlane->trianglelist[b]->vertices[i]->state = true;
1479 }
1480 }
1481 else
1482 {
1483 // flag triangle as not collided so that Vertices or Edge
1484 // of that triangles will be checked.
1485 for (size_t b = 0; planeTriListSize > b; b++)
1486 {
1487 itPlane->trianglelist[b]->state = false;
1488 }
1489 }
1490 }
1491 }
1492
1493
1494
1495 // pass2: VS triangle vertices
1496 if (needFurtherPasses)
1497 {
1498 dxRay tempRay(0, 1);
1499 dReal depth;
1500 bool vertexCollided;
1501
1502 // Only one contact is necessary for ray test
1503 int rayTestFlags = (flags & ~NUMC_MASK) | 1;
1504 dIASSERT((1 & ~NUMC_MASK) == 0);
1505 //
1506 // Find Contact Penetration Depth of each vertices
1507 //
1508 for (unsigned int k = 0; k < numTri; k++)
1509 {
1510 const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
1511 if (itTriangle->state == true)
1512 continue;// plane triangle did already collide.
1513
1514 for (size_t i = 0; i < 3; i++)
1515 {
1516 HeightFieldVertex *vertex = itTriangle->vertices[i];
1517 if (vertex->state == true)
1518 continue;// vertice did already collide.
1519
1520 vertexCollided = false;
1521 const dVector3 &triVertex = vertex->vertex;
1522 if ( geomNDepthGetter )
1523 {
1524 depth = geomNDepthGetter( o2,
1525 triVertex[0], triVertex[1], triVertex[2] );
1526 if (depth + dEpsilon < 0)
1527 vertexCollided = true;
1528 }
1529 else
1530 {
1531 // We don't have a GetDepth function, so do a ray cast instead.
1532 // NOTE: This isn't ideal, and a GetDepth function should be
1533 // written for all geom classes.
1534 tempRay.length = (minO2Height - triVertex[1]) * REAL(1000.0);
1535
1536 //dGeomRaySet( &tempRay, pContact->pos[0], pContact->pos[1], pContact->pos[2],
1537 // - itTriangle->Normal[0], - itTriangle->Normal[1], - itTriangle->Normal[2] );
1538 dGeomRaySetNoNormalize(tempRay, triVertex, itTriangle->planeDef);
1539
1540 if ( geomRayNCollider( &tempRay, o2, rayTestFlags, PlaneContact, sizeof( dContactGeom ) ) )
1541 {
1542 depth = PlaneContact[0].depth;
1543 vertexCollided = true;
1544 }
1545 }
1546 if (vertexCollided)
1547 {
1548 pContact = CONTACT(contact, numTerrainContacts*skip);
1549 //create contact using vertices
1550 dVector3Copy (triVertex, pContact->pos);
1551 //create contact using Plane Normal
1552 dOPESIGN(pContact->normal, =, -, itTriangle->planeDef);
1553
1554 pContact->depth = depth;
1555
1556 numTerrainContacts++;
1557 if ( numTerrainContacts == numMaxContactsPossible )
1558 return numTerrainContacts;
1559
1560 vertex->state = true;
1561 }
1562 }
1563 }
1564 }
1565
1566#ifdef _HEIGHTFIELDEDGECOLLIDING
1567 // pass3: VS triangle Edges
1568 if (needFurtherPasses)
1569 {
1570 dVector3 Edge;
1571 dxRay edgeRay(0, 1);
1572
1573 int numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1574 int triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri;
1575 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1576
1577 for (unsigned int k = 0; k < numTri; k++)
1578 {
1579 const HeightFieldTriangle * const itTriangle = &tempTriangleBuffer[k];
1580
1581 if (itTriangle->state == true)
1582 continue;// plane did already collide.
1583
1584 for (size_t m = 0; m < 3; m++)
1585 {
1586 const size_t next = (m + 1) % 3;
1587 HeightFieldVertex *vertex0 = itTriangle->vertices[m];
1588 HeightFieldVertex *vertex1 = itTriangle->vertices[next];
1589
1590 // not concave or under the AABB
1591 // nor triangle already collided against vertices
1592 if (vertex0->state == true && vertex1->state == true)
1593 continue;// plane did already collide.
1594
1595 dVector3Subtract(vertex1->vertex, vertex0->vertex, Edge);
1596 edgeRay.length = dVector3Length (Edge);
1597 dGeomRaySetNoNormalize(edgeRay, vertex1->vertex, Edge);
1598 int prevTerrainContacts = numTerrainContacts;
1599 pContact = CONTACT(contact, prevTerrainContacts*skip);
1600 const int numCollision = geomRayNCollider(&edgeRay,o2,triTestFlags,pContact,skip);
1601 dIASSERT(numCollision <= numMaxContactsPerTri);
1602
1603 if (numCollision)
1604 {
1605 numTerrainContacts += numCollision;
1606
1607 do
1608 {
1609 pContact = CONTACT(contact, prevTerrainContacts*skip);
1610
1611 //create contact using Plane Normal
1612 dOPESIGN(pContact->normal, =, -, itTriangle->planeDef);
1613
1614 pContact->depth = DistancePointToLine(pContact->pos, vertex1->vertex, Edge, edgeRay.length);
1615 }
1616 while (++prevTerrainContacts != numTerrainContacts);
1617
1618 if ( numTerrainContacts == numMaxContactsPossible )
1619 return numTerrainContacts;
1620
1621 numMaxContactsPerTri = dMIN(numMaxContactsPossible - numTerrainContacts, HEIGHTFIELDMAXCONTACTPERCELL);
1622 triTestFlags = (flags & ~NUMC_MASK) | numMaxContactsPerTri;
1623 dIASSERT((HEIGHTFIELDMAXCONTACTPERCELL & ~NUMC_MASK) == 0);
1624 }
1625 }
1626
1627 itTriangle->vertices[0]->state = true;
1628 itTriangle->vertices[1]->state = true;
1629 itTriangle->vertices[2]->state = true;
1630 }
1631 }
1632#endif // _HEIGHTFIELDEDGECOLLIDING
1633 return numTerrainContacts;
1634}
1635
1636int dCollideHeightfield( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contact, int skip )
1637{
1638 dIASSERT( skip >= (int)sizeof(dContactGeom) );
1639 dIASSERT( o1->type == dHeightfieldClass );
1640 dIASSERT((flags & NUMC_MASK) >= 1);
1641
1642 int i;
1643
1644 // if ((flags & NUMC_MASK) == 0) -- An assertion check is made on entry
1645 // { flags = (flags & ~NUMC_MASK) | 1; dIASSERT((1 & ~NUMC_MASK) == 0); }
1646
1647 int numMaxTerrainContacts = (flags & NUMC_MASK);
1648
1649 dxHeightfield *terrain = (dxHeightfield*) o1;
1650
1651 dVector3 posbak;
1652 dMatrix3 Rbak;
1653 dReal aabbbak[6];
1654 int gflagsbak;
1655 dVector3 pos0,pos1;
1656 dMatrix3 R1;
1657
1658 int numTerrainContacts = 0;
1659
1660 //@@ Should find a way to set reComputeAABB to false in default case
1661 // aka DHEIGHTFIELD_CORNER_ORIGIN not defined and terrain not PLACEABLE
1662 // so that we can free some memory and speed up things a bit
1663 // while saving some precision loss
1664#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1665 const bool reComputeAABB = true;
1666#else
1667 const bool reComputeAABB = ( terrain->gflags & GEOM_PLACEABLE ) ? true : false;
1668#endif //DHEIGHTFIELD_CORNER_ORIGIN
1669
1670 //
1671 // Transform O2 into Heightfield Space
1672 //
1673 if (reComputeAABB)
1674 {
1675 // Backup original o2 position, rotation and AABB.
1676 dVector3Copy( o2->final_posr->pos, posbak );
1677 dMatrix3Copy( o2->final_posr->R, Rbak );
1678 memcpy( aabbbak, o2->aabb, sizeof( dReal ) * 6 );
1679 gflagsbak = o2->gflags;
1680 }
1681
1682 if ( terrain->gflags & GEOM_PLACEABLE )
1683 {
1684 // Transform o2 into heightfield space.
1685 dOP( pos0, -, o2->final_posr->pos, terrain->final_posr->pos );
1686 dMULTIPLY1_331( pos1, terrain->final_posr->R, pos0 );
1687 dMULTIPLY1_333( R1, terrain->final_posr->R, o2->final_posr->R );
1688
1689 // Update o2 with transformed position and rotation.
1690 dVector3Copy( pos1, o2->final_posr->pos );
1691 dMatrix3Copy( R1, o2->final_posr->R );
1692 }
1693
1694#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1695 o2->final_posr->pos[ 0 ] += terrain->m_p_data->m_fHalfWidth;
1696 o2->final_posr->pos[ 2 ] += terrain->m_p_data->m_fHalfDepth;
1697#endif // DHEIGHTFIELD_CORNER_ORIGIN
1698
1699 // Rebuild AABB for O2
1700 if (reComputeAABB)
1701 o2->computeAABB();
1702
1703 //
1704 // Collide
1705 //
1706
1707 //check if inside boundaries
1708 // using O2 aabb
1709 // aabb[6] is (minx, maxx, miny, maxy, minz, maxz)
1710 const bool wrapped = terrain->m_p_data->m_bWrapMode != 0;
1711
1712 int nMinX;
1713 int nMaxX;
1714 int nMinZ;
1715 int nMaxZ;
1716
1717 if ( !wrapped )
1718 {
1719 if ( o2->aabb[0] > terrain->m_p_data->m_fWidth //MinX
1720 && o2->aabb[4] > terrain->m_p_data->m_fDepth)//MinZ
1721 goto dCollideHeightfieldExit;
1722
1723 if ( o2->aabb[1] < 0 //MaxX
1724 && o2->aabb[5] < 0) //MaxZ
1725 goto dCollideHeightfieldExit;
1726
1727 }
1728
1729 nMinX = int(dFloor(o2->aabb[0] * terrain->m_p_data->m_fInvSampleWidth));
1730 nMaxX = int(dFloor(o2->aabb[1] * terrain->m_p_data->m_fInvSampleWidth)) + 1;
1731 nMinZ = int(dFloor(o2->aabb[4] * terrain->m_p_data->m_fInvSampleDepth));
1732 nMaxZ = int(dFloor(o2->aabb[5] * terrain->m_p_data->m_fInvSampleDepth)) + 1;
1733
1734 if ( !wrapped )
1735 {
1736 nMinX = dMAX( nMinX, 0 );
1737 nMaxX = dMIN( nMaxX, terrain->m_p_data->m_nWidthSamples - 1 );
1738 nMinZ = dMAX( nMinZ, 0 );
1739 nMaxZ = dMIN( nMaxZ, terrain->m_p_data->m_nDepthSamples - 1 );
1740
1741 dIASSERT ((nMinX < nMaxX) || (nMinZ < nMaxZ))
1742 }
1743
1744
1745
1746 numTerrainContacts += terrain->dCollideHeightfieldZone(
1747 nMinX,nMaxX,nMinZ,nMaxZ,o2,numMaxTerrainContacts - numTerrainContacts,
1748 flags,CONTACT(contact,numTerrainContacts*skip),skip );
1749
1750 dIASSERT( numTerrainContacts <= numMaxTerrainContacts );
1751
1752 dContactGeom *pContact;
1753 for ( i = 0; i < numTerrainContacts; ++i )
1754 {
1755 pContact = CONTACT(contact,i*skip);
1756 pContact->g1 = o1;
1757 pContact->g2 = o2;
1758 }
1759
1760
1761 //------------------------------------------------------------------------------
1762
1763dCollideHeightfieldExit:
1764
1765 if (reComputeAABB)
1766 {
1767 // Restore o2 position, rotation and AABB
1768 dVector3Copy( posbak, o2->final_posr->pos );
1769 dMatrix3Copy( Rbak, o2->final_posr->R );
1770 memcpy( o2->aabb, aabbbak, sizeof(dReal)*6 );
1771 o2->gflags = gflagsbak;
1772
1773 //
1774 // Transform Contacts to World Space
1775 //
1776 if ( terrain->gflags & GEOM_PLACEABLE )
1777 {
1778 for ( i = 0; i < numTerrainContacts; ++i )
1779 {
1780 pContact = CONTACT(contact,i*skip);
1781 dOPE( pos0, =, pContact->pos );
1782
1783#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1784 pos0[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
1785 pos0[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
1786#endif // !DHEIGHTFIELD_CORNER_ORIGIN
1787
1788 dMULTIPLY0_331( pContact->pos, terrain->final_posr->R, pos0 );
1789
1790 dOP( pContact->pos, +, pContact->pos, terrain->final_posr->pos );
1791 dOPE( pos0, =, pContact->normal );
1792
1793 dMULTIPLY0_331( pContact->normal, terrain->final_posr->R, pos0 );
1794 }
1795 }
1796#ifndef DHEIGHTFIELD_CORNER_ORIGIN
1797 else
1798 {
1799 for ( i = 0; i < numTerrainContacts; ++i )
1800 {
1801 pContact = CONTACT(contact,i*skip);
1802 pContact->pos[ 0 ] -= terrain->m_p_data->m_fHalfWidth;
1803 pContact->pos[ 2 ] -= terrain->m_p_data->m_fHalfDepth;
1804 }
1805 }
1806#endif // !DHEIGHTFIELD_CORNER_ORIGIN
1807 }
1808 // Return contact count.
1809 return numTerrainContacts;
1810}
1811
1812
generated by cgit v1.1 at 2024-11-09 23:01:34 +0000