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 #ifdef WITH_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 // WITH_PYTHON