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