e6f8d7175dc57de8090f9ab4c13ce48d06931275
[blender.git] / source / gameengine / Ketsji / KX_NavMeshObject.cpp
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 #include "MEM_guardedalloc.h"
29
30 #include "BLI_math_vector.h"
31 #include "KX_NavMeshObject.h"
32 #include "RAS_MeshObject.h"
33
34 #include "DNA_mesh_types.h"
35 #include "DNA_meshdata_types.h"
36
37 extern "C" {
38 #include "BKE_scene.h"
39 #include "BKE_customdata.h"
40 #include "BKE_cdderivedmesh.h"
41 #include "BKE_DerivedMesh.h"
42 #include "BKE_navmesh_conversion.h"
43 }
44
45 #include "KX_PythonInit.h"
46 #include "KX_PyMath.h"
47 #include "Value.h"
48 #include "Recast.h"
49 #include "DetourStatNavMeshBuilder.h"
50 #include "KX_ObstacleSimulation.h"
51
52 static const int MAX_PATH_LEN = 256;
53 static const float polyPickExt[3] = {2, 4, 2};
54
55 static void calcMeshBounds(const float* vert, int nverts, float* bmin, float* bmax)
56 {
57         bmin[0] = bmax[0] = vert[0];
58         bmin[1] = bmax[1] = vert[1];
59         bmin[2] = bmax[2] = vert[2];
60         for (int i=1; i<nverts; i++)
61         {
62                 if (bmin[0]>vert[3*i+0]) bmin[0] = vert[3*i+0];
63                 if (bmin[1]>vert[3*i+1]) bmin[1] = vert[3*i+1];
64                 if (bmin[2]>vert[3*i+2]) bmin[2] = vert[3*i+2];
65
66                 if (bmax[0]<vert[3*i+0]) bmax[0] = vert[3*i+0];
67                 if (bmax[1]<vert[3*i+1]) bmax[1] = vert[3*i+1];
68                 if (bmax[2]<vert[3*i+2]) bmax[2] = vert[3*i+2];
69         }
70 }
71
72 inline void flipAxes(float* vec)
73 {
74         std::swap(vec[1],vec[2]);
75 }
76 KX_NavMeshObject::KX_NavMeshObject(void* sgReplicationInfo, SG_Callbacks callbacks)
77 :       KX_GameObject(sgReplicationInfo, callbacks)
78 ,       m_navMesh(NULL)
79 {
80         
81 }
82
83 KX_NavMeshObject::~KX_NavMeshObject()
84 {
85         if (m_navMesh)
86                 delete m_navMesh;
87 }
88
89 CValue* KX_NavMeshObject::GetReplica()
90 {
91         KX_NavMeshObject* replica = new KX_NavMeshObject(*this);
92         replica->ProcessReplica();
93         return replica;
94 }
95
96 void KX_NavMeshObject::ProcessReplica()
97 {
98         KX_GameObject::ProcessReplica();
99
100         BuildNavMesh();
101         KX_Scene* scene = KX_GetActiveScene();
102         KX_ObstacleSimulation* obssimulation = scene->GetObstacleSimulation();
103         if (obssimulation)
104                 obssimulation->AddObstaclesForNavMesh(this);
105
106 }
107
108 bool KX_NavMeshObject::BuildVertIndArrays(float *&vertices, int& nverts,
109                                                                            unsigned short* &polys, int& npolys, unsigned short *&dmeshes,
110                                                                            float *&dvertices, int &ndvertsuniq, unsigned short *&dtris, 
111                                                                            int& ndtris, int &vertsPerPoly)
112 {
113         DerivedMesh* dm = mesh_create_derived_no_virtual(KX_GetActiveScene()->GetBlenderScene(), GetBlenderObject(), 
114                                                                                                         NULL, CD_MASK_MESH);
115         int* recastData = (int*) dm->getTessFaceDataArray(dm, CD_RECAST);
116         if (recastData)
117         {
118                 int *dtrisToPolysMap=NULL, *dtrisToTrisMap=NULL, *trisToFacesMap=NULL;
119                 int nAllVerts = 0;
120                 float *allVerts = NULL;
121                 buildNavMeshDataByDerivedMesh(dm, &vertsPerPoly, &nAllVerts, &allVerts, &ndtris, &dtris,
122                         &npolys, &dmeshes, &polys, &dtrisToPolysMap, &dtrisToTrisMap, &trisToFacesMap);
123
124                 MEM_freeN(dtrisToPolysMap);
125                 MEM_freeN(dtrisToTrisMap);
126                 MEM_freeN(trisToFacesMap);
127
128                 unsigned short *verticesMap = new unsigned short[nAllVerts];
129                 memset(verticesMap, 0xffff, sizeof(unsigned short)*nAllVerts);
130                 int curIdx = 0;
131                 //vertices - mesh verts
132                 //iterate over all polys and create map for their vertices first...
133                 for (int polyidx=0; polyidx<npolys; polyidx++)
134                 {
135                         unsigned short* poly = &polys[polyidx*vertsPerPoly*2];
136                         for (int i=0; i<vertsPerPoly; i++)
137                         {
138                                 unsigned short idx = poly[i];
139                                 if (idx==0xffff)
140                                         break;
141                                 if (verticesMap[idx]==0xffff)
142                                 {
143                                         verticesMap[idx] = curIdx++;
144                                 }
145                                 poly[i] = verticesMap[idx];
146                         }
147                 }
148                 nverts = curIdx;
149                 //...then iterate over detailed meshes
150                 //transform indices to local ones (for each navigation polygon)
151                 for (int polyidx=0; polyidx<npolys; polyidx++)
152                 {
153                         unsigned short *poly = &polys[polyidx*vertsPerPoly*2];
154                         int nv = polyNumVerts(poly, vertsPerPoly);
155                         unsigned short *dmesh = &dmeshes[4*polyidx];
156                         unsigned short tribase = dmesh[2];
157                         unsigned short trinum = dmesh[3];
158                         unsigned short vbase = curIdx;
159                         for (int j=0; j<trinum; j++)
160                         {
161                                 unsigned short* dtri = &dtris[(tribase+j)*3*2];
162                                 for (int k=0; k<3; k++)
163                                 {
164                                         int newVertexIdx = verticesMap[dtri[k]];
165                                         if (newVertexIdx==0xffff)
166                                         {
167                                                 newVertexIdx = curIdx++;
168                                                 verticesMap[dtri[k]] = newVertexIdx;
169                                         }
170
171                                         if (newVertexIdx<nverts)
172                                         {
173                                                 //it's polygon vertex ("shared")
174                                                 int idxInPoly = polyFindVertex(poly, vertsPerPoly, newVertexIdx);
175                                                 if (idxInPoly==-1)
176                                                 {
177                                                         printf("Building NavMeshObject: Error! Can't find vertex in polygon\n");
178                                                         return false;
179                                                 }
180                                                 dtri[k] = idxInPoly;
181                                         }
182                                         else
183                                         {
184                                                 dtri[k] = newVertexIdx - vbase + nv;
185                                         }
186                                 }
187                         }
188                         dmesh[0] = vbase-nverts; //verts base
189                         dmesh[1] = curIdx-vbase; //verts num
190                 }
191
192                 vertices = new float[nverts*3];
193                 ndvertsuniq = curIdx - nverts;
194                 if (ndvertsuniq>0)
195                 {
196                         dvertices = new float[ndvertsuniq*3];
197                 }
198                 for (int vi=0; vi<nAllVerts; vi++)
199                 {
200                         int newIdx = verticesMap[vi];
201                         if (newIdx!=0xffff)
202                         {
203                                 if (newIdx<nverts)
204                                 {
205                                         //navigation mesh vertex
206                                         memcpy(vertices+3*newIdx, allVerts+3*vi, 3*sizeof(float));
207                                 }
208                                 else
209                                 {
210                                         //detailed mesh vertex
211                                         memcpy(dvertices+3*(newIdx-nverts), allVerts+3*vi, 3*sizeof(float));
212                                 }                               
213                         }
214                 }
215
216                 MEM_freeN(allVerts);
217         }
218         else
219         {
220                 //create from RAS_MeshObject (detailed mesh is fake)
221                 RAS_MeshObject* meshobj = GetMesh(0);
222                 vertsPerPoly = 3;
223                 nverts = meshobj->m_sharedvertex_map.size();
224                 if (nverts >= 0xffff)
225                         return false;
226                 //calculate count of tris
227                 int nmeshpolys = meshobj->NumPolygons();
228                 npolys = nmeshpolys;
229                 for (int p=0; p<nmeshpolys; p++)
230                 {
231                         int vertcount = meshobj->GetPolygon(p)->VertexCount();
232                         npolys+=vertcount-3;
233                 }
234
235                 //create verts
236                 vertices = new float[nverts*3];
237                 float* vert = vertices;
238                 for (int vi=0; vi<nverts; vi++)
239                 {
240                         const float* pos = !meshobj->m_sharedvertex_map[vi].empty() ? meshobj->GetVertexLocation(vi) : NULL;
241                         if (pos)
242                                 copy_v3_v3(vert, pos);
243                         else
244                         {
245                                 memset(vert, 0, 3*sizeof(float)); //vertex isn't in any poly, set dummy zero coordinates
246                         }
247                         vert+=3;                
248                 }
249
250                 //create tris
251                 polys = (unsigned short*)MEM_callocN(sizeof(unsigned short)*3*2*npolys, "BuildVertIndArrays polys");
252                 memset(polys, 0xff, sizeof(unsigned short)*3*2*npolys);
253                 unsigned short *poly = polys;
254                 RAS_Polygon* raspoly;
255                 for (int p=0; p<nmeshpolys; p++)
256                 {
257                         raspoly = meshobj->GetPolygon(p);
258                         for (int v=0; v<raspoly->VertexCount()-2; v++)
259                         {
260                                 poly[0]= raspoly->GetVertex(0)->getOrigIndex();
261                                 for (size_t i=1; i<3; i++)
262                                 {
263                                         poly[i]= raspoly->GetVertex(v+i)->getOrigIndex();
264                                 }
265                                 poly += 6;
266                         }
267                 }
268                 dmeshes = NULL;
269                 dvertices = NULL;
270                 ndvertsuniq = 0;
271                 dtris = NULL;
272                 ndtris = npolys;
273         }
274         dm->release(dm);
275         
276         return true;
277 }
278
279
280 bool KX_NavMeshObject::BuildNavMesh()
281 {
282         if (m_navMesh)
283         {
284                 delete m_navMesh;
285                 m_navMesh = NULL;
286         }
287
288         if (GetMeshCount()==0)
289         {
290                 printf("Can't find mesh for navmesh object: %s \n", m_name.ReadPtr());
291                 return false;
292         }
293
294         float *vertices = NULL, *dvertices = NULL;
295         unsigned short *polys = NULL, *dtris = NULL, *dmeshes = NULL;
296         int nverts = 0, npolys = 0, ndvertsuniq = 0, ndtris = 0;        
297         int vertsPerPoly = 0;
298         if (!BuildVertIndArrays(vertices, nverts, polys, npolys, 
299                                                         dmeshes, dvertices, ndvertsuniq, dtris, ndtris, vertsPerPoly ) 
300                         || vertsPerPoly<3)
301         {
302                 printf("Can't build navigation mesh data for object:%s \n", m_name.ReadPtr());
303                 return false;
304         }
305         
306         MT_Point3 pos;
307         if (dmeshes==NULL)
308         {
309                 for (int i=0; i<nverts; i++)
310                 {
311                         flipAxes(&vertices[i*3]);
312                 }
313                 for (int i=0; i<ndvertsuniq; i++)
314                 {
315                         flipAxes(&dvertices[i*3]);
316                 }
317         }
318
319         buildMeshAdjacency(polys, npolys, nverts, vertsPerPoly);
320         
321         float cs = 0.2f;
322
323         if (!nverts || !npolys)
324                 return false;
325
326         float bmin[3], bmax[3];
327         calcMeshBounds(vertices, nverts, bmin, bmax);
328         //quantize vertex pos
329         unsigned short* vertsi = new unsigned short[3*nverts];
330         float ics = 1.f/cs;
331         for (int i=0; i<nverts; i++)
332         {
333                 vertsi[3*i+0] = static_cast<unsigned short>((vertices[3*i+0]-bmin[0])*ics);
334                 vertsi[3*i+1] = static_cast<unsigned short>((vertices[3*i+1]-bmin[1])*ics);
335                 vertsi[3*i+2] = static_cast<unsigned short>((vertices[3*i+2]-bmin[2])*ics);
336         }
337
338         // Calculate data size
339         const int headerSize = sizeof(dtStatNavMeshHeader);
340         const int vertsSize = sizeof(float)*3*nverts;
341         const int polysSize = sizeof(dtStatPoly)*npolys;
342         const int nodesSize = sizeof(dtStatBVNode)*npolys*2;
343         const int detailMeshesSize = sizeof(dtStatPolyDetail)*npolys;
344         const int detailVertsSize = sizeof(float)*3*ndvertsuniq;
345         const int detailTrisSize = sizeof(unsigned char)*4*ndtris;
346
347         const int dataSize = headerSize + vertsSize + polysSize + nodesSize +
348                 detailMeshesSize + detailVertsSize + detailTrisSize;
349         unsigned char* data = new unsigned char[dataSize];
350         if (!data)
351                 return false;
352         memset(data, 0, dataSize);
353
354         unsigned char* d = data;
355         dtStatNavMeshHeader* header = (dtStatNavMeshHeader*)d; d += headerSize;
356         float* navVerts = (float*)d; d += vertsSize;
357         dtStatPoly* navPolys = (dtStatPoly*)d; d += polysSize;
358         dtStatBVNode* navNodes = (dtStatBVNode*)d; d += nodesSize;
359         dtStatPolyDetail* navDMeshes = (dtStatPolyDetail*)d; d += detailMeshesSize;
360         float* navDVerts = (float*)d; d += detailVertsSize;
361         unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize;
362
363         // Store header
364         header->magic = DT_STAT_NAVMESH_MAGIC;
365         header->version = DT_STAT_NAVMESH_VERSION;
366         header->npolys = npolys;
367         header->nverts = nverts;
368         header->cs = cs;
369         header->bmin[0] = bmin[0];
370         header->bmin[1] = bmin[1];
371         header->bmin[2] = bmin[2];
372         header->bmax[0] = bmax[0];
373         header->bmax[1] = bmax[1];
374         header->bmax[2] = bmax[2];
375         header->ndmeshes = npolys;
376         header->ndverts = ndvertsuniq;
377         header->ndtris = ndtris;
378
379         // Store vertices
380         for (int i = 0; i < nverts; ++i)
381         {
382                 const unsigned short* iv = &vertsi[i*3];
383                 float* v = &navVerts[i*3];
384                 v[0] = bmin[0] + iv[0] * cs;
385                 v[1] = bmin[1] + iv[1] * cs;
386                 v[2] = bmin[2] + iv[2] * cs;
387         }
388         //memcpy(navVerts, vertices, nverts*3*sizeof(float));
389
390         // Store polygons
391         const unsigned short* src = polys;
392         for (int i = 0; i < npolys; ++i)
393         {
394                 dtStatPoly* p = &navPolys[i];
395                 p->nv = 0;
396                 for (int j = 0; j < vertsPerPoly; ++j)
397                 {
398                         if (src[j] == 0xffff) break;
399                         p->v[j] = src[j];
400                         p->n[j] = src[vertsPerPoly+j]+1;
401                         p->nv++;
402                 }
403                 src += vertsPerPoly*2;
404         }
405
406         header->nnodes = createBVTree(vertsi, nverts, polys, npolys, vertsPerPoly,
407                                                                 cs, cs, npolys*2, navNodes);
408         
409         
410         if (dmeshes==NULL)
411         {
412                 //create fake detail meshes
413                 for (int i = 0; i < npolys; ++i)
414                 {
415                         dtStatPolyDetail& dtl = navDMeshes[i];
416                         dtl.vbase = 0;
417                         dtl.nverts = 0;
418                         dtl.tbase = i;
419                         dtl.ntris = 1;
420                 }
421                 // setup triangles.
422                 unsigned char* tri = navDTris;
423                 for(size_t i=0; i<ndtris; i++)
424                 {
425                         for (size_t j=0; j<3; j++)
426                                 tri[4*i+j] = j;
427                 }
428         }
429         else
430         {
431                 //verts
432                 memcpy(navDVerts, dvertices, ndvertsuniq*3*sizeof(float));
433                 //tris
434                 unsigned char* tri = navDTris;
435                 for(size_t i=0; i<ndtris; i++)
436                 {
437                         for (size_t j=0; j<3; j++)
438                                 tri[4*i+j] = dtris[6*i+j];
439                 }
440                 //detailed meshes
441                 for (int i = 0; i < npolys; ++i)
442                 {
443                         dtStatPolyDetail& dtl = navDMeshes[i];
444                         dtl.vbase = dmeshes[i*4+0];
445                         dtl.nverts = dmeshes[i*4+1];
446                         dtl.tbase = dmeshes[i*4+2];
447                         dtl.ntris = dmeshes[i*4+3];
448                 }               
449         }
450
451         m_navMesh = new dtStatNavMesh;
452         m_navMesh->init(data, dataSize, true);  
453
454         delete [] vertices;
455
456         /* navmesh conversion is using C guarded alloc for memory allocaitons */
457         MEM_freeN(polys);
458         if (dmeshes) MEM_freeN(dmeshes);
459         if (dtris) MEM_freeN(dtris);
460
461         if (dvertices)
462         {
463                 delete [] dvertices;
464         }
465
466         return true;
467 }
468
469 dtStatNavMesh* KX_NavMeshObject::GetNavMesh()
470 {
471         return m_navMesh;
472 }
473
474 void KX_NavMeshObject::DrawNavMesh(NavMeshRenderMode renderMode)
475 {
476         if (!m_navMesh)
477                 return;
478         MT_Vector3 color(0.f, 0.f, 0.f);
479         
480         switch (renderMode)
481         {
482         case RM_POLYS :
483         case RM_WALLS : 
484                 for (int pi=0; pi<m_navMesh->getPolyCount(); pi++)
485                 {
486                         const dtStatPoly* poly = m_navMesh->getPoly(pi);
487
488                         for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
489                         {       
490                                 if (poly->n[j] && renderMode==RM_WALLS) 
491                                         continue;
492                                 const float* vif = m_navMesh->getVertex(poly->v[i]);
493                                 const float* vjf = m_navMesh->getVertex(poly->v[j]);
494                                 MT_Point3 vi(vif[0], vif[2], vif[1]);
495                                 MT_Point3 vj(vjf[0], vjf[2], vjf[1]);
496                                 vi = TransformToWorldCoords(vi);
497                                 vj = TransformToWorldCoords(vj);
498                                 KX_RasterizerDrawDebugLine(vi, vj, color);
499                         }
500                 }
501                 break;
502         case RM_TRIS : 
503                 for (int i = 0; i < m_navMesh->getPolyDetailCount(); ++i)
504                 {
505                         const dtStatPoly* p = m_navMesh->getPoly(i);
506                         const dtStatPolyDetail* pd = m_navMesh->getPolyDetail(i);
507
508                         for (int j = 0; j < pd->ntris; ++j)
509                         {
510                                 const unsigned char* t = m_navMesh->getDetailTri(pd->tbase+j);
511                                 MT_Point3 tri[3];
512                                 for (int k = 0; k < 3; ++k)
513                                 {
514                                         const float* v;
515                                         if (t[k] < p->nv)
516                                                 v = m_navMesh->getVertex(p->v[t[k]]);
517                                         else
518                                                 v =  m_navMesh->getDetailVertex(pd->vbase+(t[k]-p->nv));
519                                         float pos[3];
520                                         rcVcopy(pos, v);
521                                         flipAxes(pos);
522                                         tri[k].setValue(pos);
523                                 }
524
525                                 for (int k=0; k<3; k++)
526                                         tri[k] = TransformToWorldCoords(tri[k]);
527
528                                 for (int k=0; k<3; k++)
529                                         KX_RasterizerDrawDebugLine(tri[k], tri[(k+1)%3], color);
530                         }
531                 }
532                 break;
533         default:
534                 /* pass */
535                 break;
536         }
537 }
538
539 MT_Point3 KX_NavMeshObject::TransformToLocalCoords(const MT_Point3& wpos)
540 {
541         MT_Matrix3x3 orientation = NodeGetWorldOrientation();
542         const MT_Vector3& scaling = NodeGetWorldScaling();
543         orientation.scale(scaling[0], scaling[1], scaling[2]);
544         MT_Transform worldtr(NodeGetWorldPosition(), orientation); 
545         MT_Transform invworldtr;
546         invworldtr.invert(worldtr);
547         MT_Point3 lpos = invworldtr(wpos);
548         return lpos;
549 }
550
551 MT_Point3 KX_NavMeshObject::TransformToWorldCoords(const MT_Point3& lpos)
552 {
553         MT_Matrix3x3 orientation = NodeGetWorldOrientation();
554         const MT_Vector3& scaling = NodeGetWorldScaling();
555         orientation.scale(scaling[0], scaling[1], scaling[2]);
556         MT_Transform worldtr(NodeGetWorldPosition(), orientation); 
557         MT_Point3 wpos = worldtr(lpos);
558         return wpos;
559 }
560
561 int KX_NavMeshObject::FindPath(const MT_Point3& from, const MT_Point3& to, float* path, int maxPathLen)
562 {
563         if (!m_navMesh)
564                 return 0;
565         MT_Point3 localfrom = TransformToLocalCoords(from);
566         MT_Point3 localto = TransformToLocalCoords(to); 
567         float spos[3], epos[3];
568         localfrom.getValue(spos); flipAxes(spos);
569         localto.getValue(epos); flipAxes(epos);
570         dtStatPolyRef sPolyRef = m_navMesh->findNearestPoly(spos, polyPickExt);
571         dtStatPolyRef ePolyRef = m_navMesh->findNearestPoly(epos, polyPickExt);
572
573         int pathLen = 0;
574         if (sPolyRef && ePolyRef)
575         {
576                 dtStatPolyRef* polys = new dtStatPolyRef[maxPathLen];
577                 int npolys;
578                 npolys = m_navMesh->findPath(sPolyRef, ePolyRef, spos, epos, polys, maxPathLen);
579                 if (npolys)
580                 {
581                         pathLen = m_navMesh->findStraightPath(spos, epos, polys, npolys, path, maxPathLen);
582                         for (int i=0; i<pathLen; i++)
583                         {
584                                 flipAxes(&path[i*3]);
585                                 MT_Point3 waypoint(&path[i*3]);
586                                 waypoint = TransformToWorldCoords(waypoint);
587                                 waypoint.getValue(&path[i*3]);
588                         }
589                 }
590         }
591
592         return pathLen;
593 }
594
595 float KX_NavMeshObject::Raycast(const MT_Point3& from, const MT_Point3& to)
596 {
597         if (!m_navMesh)
598                 return 0.f;
599         MT_Point3 localfrom = TransformToLocalCoords(from);
600         MT_Point3 localto = TransformToLocalCoords(to); 
601         float spos[3], epos[3];
602         localfrom.getValue(spos); flipAxes(spos);
603         localto.getValue(epos); flipAxes(epos);
604         dtStatPolyRef sPolyRef = m_navMesh->findNearestPoly(spos, polyPickExt);
605         float t=0;
606         static dtStatPolyRef polys[MAX_PATH_LEN];
607         m_navMesh->raycast(sPolyRef, spos, epos, t, polys, MAX_PATH_LEN);
608         return t;
609 }
610
611 void KX_NavMeshObject::DrawPath(const float *path, int pathLen, const MT_Vector3& color)
612 {
613         MT_Vector3 a,b;
614         for (int i=0; i<pathLen-1; i++)
615         {
616                 a.setValue(&path[3*i]);
617                 b.setValue(&path[3*(i+1)]);
618                 KX_RasterizerDrawDebugLine(a, b, color);
619         }
620 }
621
622
623 #ifndef DISABLE_PYTHON
624 //----------------------------------------------------------------------------
625 //Python
626
627 PyTypeObject KX_NavMeshObject::Type = {
628         PyVarObject_HEAD_INIT(NULL, 0)
629         "KX_NavMeshObject",
630         sizeof(PyObjectPlus_Proxy),
631         0,
632         py_base_dealloc,
633         0,
634         0,
635         0,
636         0,
637         py_base_repr,
638         0,
639         0,
640         0,
641         0,0,0,0,0,0,
642         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
643         0,0,0,0,0,0,0,
644         Methods,
645         0,
646         0,
647         &KX_GameObject::Type,
648         0,0,0,0,0,0,
649         py_base_new
650 };
651
652 PyAttributeDef KX_NavMeshObject::Attributes[] = {
653         { NULL }        //Sentinel
654 };
655
656 //KX_PYMETHODTABLE_NOARGS(KX_GameObject, getD),
657 PyMethodDef KX_NavMeshObject::Methods[] = {
658         KX_PYMETHODTABLE(KX_NavMeshObject, findPath),
659         KX_PYMETHODTABLE(KX_NavMeshObject, raycast),
660         KX_PYMETHODTABLE(KX_NavMeshObject, draw),
661         KX_PYMETHODTABLE(KX_NavMeshObject, rebuild),
662         {NULL,NULL} //Sentinel
663 };
664
665 KX_PYMETHODDEF_DOC(KX_NavMeshObject, findPath,
666                                    "findPath(start, goal): find path from start to goal points\n"
667                                    "Returns a path as list of points)\n")
668 {
669         PyObject *ob_from, *ob_to;
670         if (!PyArg_ParseTuple(args,"OO:getPath",&ob_from,&ob_to))
671                 return NULL;
672         MT_Point3 from, to;
673         if (!PyVecTo(ob_from, from) || !PyVecTo(ob_to, to))
674                 return NULL;
675         
676         float path[MAX_PATH_LEN*3];
677         int pathLen = FindPath(from, to, path, MAX_PATH_LEN);
678         PyObject *pathList = PyList_New( pathLen );
679         for (int i=0; i<pathLen; i++)
680         {
681                 MT_Point3 point(&path[3*i]);
682                 PyList_SET_ITEM(pathList, i, PyObjectFrom(point));
683         }
684
685         return pathList;
686 }
687
688 KX_PYMETHODDEF_DOC(KX_NavMeshObject, raycast,
689                                    "raycast(start, goal): raycast from start to goal points\n"
690                                    "Returns hit factor)\n")
691 {
692         PyObject *ob_from, *ob_to;
693         if (!PyArg_ParseTuple(args,"OO:getPath",&ob_from,&ob_to))
694                 return NULL;
695         MT_Point3 from, to;
696         if (!PyVecTo(ob_from, from) || !PyVecTo(ob_to, to))
697                 return NULL;
698         float hit = Raycast(from, to);
699         return PyFloat_FromDouble(hit);
700 }
701
702 KX_PYMETHODDEF_DOC(KX_NavMeshObject, draw,
703                                    "draw(mode): navigation mesh debug drawing\n"
704                                    "mode: WALLS, POLYS, TRIS\n")
705 {
706         int arg;
707         NavMeshRenderMode renderMode = RM_TRIS;
708         if (PyArg_ParseTuple(args,"i:rebuild",&arg) && arg>=0 && arg<RM_MAX)
709                 renderMode = (NavMeshRenderMode)arg;
710         DrawNavMesh(renderMode);
711         Py_RETURN_NONE;
712 }
713
714 KX_PYMETHODDEF_DOC_NOARGS(KX_NavMeshObject, rebuild,
715                                                   "rebuild(): rebuild navigation mesh\n")
716 {
717         BuildNavMesh();
718         Py_RETURN_NONE;
719 }
720
721 #endif // DISABLE_PYTHON