Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp @ 238

Last change on this file since 238 was 216, checked in by mathiask, 18 years ago

[Physik] add ode-0.9
File size: 16.7 KB
Line 
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*/
Note: See TracBrowser for help on using the repository browser.