Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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