2 * ***** BEGIN GPL LICENSE BLOCK *****
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.
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.
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.
18 * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
19 * All rights reserved.
21 * The Original Code is: all of this file.
23 * Contributor(s): none yet.
25 * ***** END GPL LICENSE BLOCK *****
28 #include "MEM_guardedalloc.h"
30 #include "BLI_math_vector.h"
31 #include "KX_NavMeshObject.h"
32 #include "RAS_MeshObject.h"
34 #include "DNA_mesh_types.h"
35 #include "DNA_meshdata_types.h"
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"
45 #include "KX_PythonInit.h"
46 #include "KX_PyMath.h"
49 #include "DetourStatNavMeshBuilder.h"
50 #include "KX_ObstacleSimulation.h"
52 static const int MAX_PATH_LEN = 256;
53 static const float polyPickExt[3] = {2, 4, 2};
55 static void calcMeshBounds(const float* vert, int nverts, float* bmin, float* bmax)
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++)
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];
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];
72 inline void flipAxes(float* vec)
74 std::swap(vec[1],vec[2]);
76 KX_NavMeshObject::KX_NavMeshObject(void* sgReplicationInfo, SG_Callbacks callbacks)
77 : KX_GameObject(sgReplicationInfo, callbacks)
83 KX_NavMeshObject::~KX_NavMeshObject()
89 CValue* KX_NavMeshObject::GetReplica()
91 KX_NavMeshObject* replica = new KX_NavMeshObject(*this);
92 replica->ProcessReplica();
96 void KX_NavMeshObject::ProcessReplica()
98 KX_GameObject::ProcessReplica();
101 KX_Scene* scene = KX_GetActiveScene();
102 KX_ObstacleSimulation* obssimulation = scene->GetObstacleSimulation();
104 obssimulation->AddObstaclesForNavMesh(this);
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)
113 DerivedMesh* dm = mesh_create_derived_no_virtual(KX_GetActiveScene()->GetBlenderScene(), GetBlenderObject(),
115 int* recastData = (int*) dm->getTessFaceDataArray(dm, CD_RECAST);
118 int *dtrisToPolysMap=NULL, *dtrisToTrisMap=NULL, *trisToFacesMap=NULL;
120 float *allVerts = NULL;
121 buildNavMeshDataByDerivedMesh(dm, &vertsPerPoly, &nAllVerts, &allVerts, &ndtris, &dtris,
122 &npolys, &dmeshes, &polys, &dtrisToPolysMap, &dtrisToTrisMap, &trisToFacesMap);
124 MEM_freeN(dtrisToPolysMap);
125 MEM_freeN(dtrisToTrisMap);
126 MEM_freeN(trisToFacesMap);
128 unsigned short *verticesMap = new unsigned short[nAllVerts];
129 memset(verticesMap, 0xffff, sizeof(unsigned short)*nAllVerts);
131 //vertices - mesh verts
132 //iterate over all polys and create map for their vertices first...
133 for (int polyidx=0; polyidx<npolys; polyidx++)
135 unsigned short* poly = &polys[polyidx*vertsPerPoly*2];
136 for (int i=0; i<vertsPerPoly; i++)
138 unsigned short idx = poly[i];
141 if (verticesMap[idx]==0xffff)
143 verticesMap[idx] = curIdx++;
145 poly[i] = verticesMap[idx];
149 //...then iterate over detailed meshes
150 //transform indices to local ones (for each navigation polygon)
151 for (int polyidx=0; polyidx<npolys; polyidx++)
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++)
161 unsigned short* dtri = &dtris[(tribase+j)*3*2];
162 for (int k=0; k<3; k++)
164 int newVertexIdx = verticesMap[dtri[k]];
165 if (newVertexIdx==0xffff)
167 newVertexIdx = curIdx++;
168 verticesMap[dtri[k]] = newVertexIdx;
171 if (newVertexIdx<nverts)
173 //it's polygon vertex ("shared")
174 int idxInPoly = polyFindVertex(poly, vertsPerPoly, newVertexIdx);
177 printf("Building NavMeshObject: Error! Can't find vertex in polygon\n");
184 dtri[k] = newVertexIdx - vbase + nv;
188 dmesh[0] = vbase-nverts; //verts base
189 dmesh[1] = curIdx-vbase; //verts num
192 vertices = new float[nverts*3];
193 ndvertsuniq = curIdx - nverts;
196 dvertices = new float[ndvertsuniq*3];
198 for (int vi=0; vi<nAllVerts; vi++)
200 int newIdx = verticesMap[vi];
205 //navigation mesh vertex
206 memcpy(vertices+3*newIdx, allVerts+3*vi, 3*sizeof(float));
210 //detailed mesh vertex
211 memcpy(dvertices+3*(newIdx-nverts), allVerts+3*vi, 3*sizeof(float));
220 //create from RAS_MeshObject (detailed mesh is fake)
221 RAS_MeshObject* meshobj = GetMesh(0);
223 nverts = meshobj->m_sharedvertex_map.size();
224 if (nverts >= 0xffff)
226 //calculate count of tris
227 int nmeshpolys = meshobj->NumPolygons();
229 for (int p=0; p<nmeshpolys; p++)
231 int vertcount = meshobj->GetPolygon(p)->VertexCount();
236 vertices = new float[nverts*3];
237 float* vert = vertices;
238 for (int vi=0; vi<nverts; vi++)
240 const float* pos = !meshobj->m_sharedvertex_map[vi].empty() ? meshobj->GetVertexLocation(vi) : NULL;
242 copy_v3_v3(vert, pos);
245 memset(vert, 0, 3*sizeof(float)); //vertex isn't in any poly, set dummy zero coordinates
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++)
257 raspoly = meshobj->GetPolygon(p);
258 for (int v=0; v<raspoly->VertexCount()-2; v++)
260 poly[0]= raspoly->GetVertex(0)->getOrigIndex();
261 for (size_t i=1; i<3; i++)
263 poly[i]= raspoly->GetVertex(v+i)->getOrigIndex();
280 bool KX_NavMeshObject::BuildNavMesh()
288 if (GetMeshCount()==0)
290 printf("Can't find mesh for navmesh object: %s \n", m_name.ReadPtr());
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 )
302 printf("Can't build navigation mesh data for object:%s \n", m_name.ReadPtr());
309 for (int i=0; i<nverts; i++)
311 flipAxes(&vertices[i*3]);
313 for (int i=0; i<ndvertsuniq; i++)
315 flipAxes(&dvertices[i*3]);
319 buildMeshAdjacency(polys, npolys, nverts, vertsPerPoly);
323 if (!nverts || !npolys)
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];
331 for (int i=0; i<nverts; i++)
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);
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;
347 const int dataSize = headerSize + vertsSize + polysSize + nodesSize +
348 detailMeshesSize + detailVertsSize + detailTrisSize;
349 unsigned char* data = new unsigned char[dataSize];
352 memset(data, 0, dataSize);
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;
364 header->magic = DT_STAT_NAVMESH_MAGIC;
365 header->version = DT_STAT_NAVMESH_VERSION;
366 header->npolys = npolys;
367 header->nverts = nverts;
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;
380 for (int i = 0; i < nverts; ++i)
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;
388 //memcpy(navVerts, vertices, nverts*3*sizeof(float));
391 const unsigned short* src = polys;
392 for (int i = 0; i < npolys; ++i)
394 dtStatPoly* p = &navPolys[i];
396 for (int j = 0; j < vertsPerPoly; ++j)
398 if (src[j] == 0xffff) break;
400 p->n[j] = src[vertsPerPoly+j]+1;
403 src += vertsPerPoly*2;
406 header->nnodes = createBVTree(vertsi, nverts, polys, npolys, vertsPerPoly,
407 cs, cs, npolys*2, navNodes);
412 //create fake detail meshes
413 for (int i = 0; i < npolys; ++i)
415 dtStatPolyDetail& dtl = navDMeshes[i];
422 unsigned char* tri = navDTris;
423 for(size_t i=0; i<ndtris; i++)
425 for (size_t j=0; j<3; j++)
432 memcpy(navDVerts, dvertices, ndvertsuniq*3*sizeof(float));
434 unsigned char* tri = navDTris;
435 for(size_t i=0; i<ndtris; i++)
437 for (size_t j=0; j<3; j++)
438 tri[4*i+j] = dtris[6*i+j];
441 for (int i = 0; i < npolys; ++i)
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];
451 m_navMesh = new dtStatNavMesh;
452 m_navMesh->init(data, dataSize, true);
456 /* navmesh conversion is using C guarded alloc for memory allocaitons */
458 if (dmeshes) MEM_freeN(dmeshes);
459 if (dtris) MEM_freeN(dtris);
469 dtStatNavMesh* KX_NavMeshObject::GetNavMesh()
474 void KX_NavMeshObject::DrawNavMesh(NavMeshRenderMode renderMode)
478 MT_Vector3 color(0.f, 0.f, 0.f);
484 for (int pi=0; pi<m_navMesh->getPolyCount(); pi++)
486 const dtStatPoly* poly = m_navMesh->getPoly(pi);
488 for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
490 if (poly->n[j] && renderMode==RM_WALLS)
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);
503 for (int i = 0; i < m_navMesh->getPolyDetailCount(); ++i)
505 const dtStatPoly* p = m_navMesh->getPoly(i);
506 const dtStatPolyDetail* pd = m_navMesh->getPolyDetail(i);
508 for (int j = 0; j < pd->ntris; ++j)
510 const unsigned char* t = m_navMesh->getDetailTri(pd->tbase+j);
512 for (int k = 0; k < 3; ++k)
516 v = m_navMesh->getVertex(p->v[t[k]]);
518 v = m_navMesh->getDetailVertex(pd->vbase+(t[k]-p->nv));
522 tri[k].setValue(pos);
525 for (int k=0; k<3; k++)
526 tri[k] = TransformToWorldCoords(tri[k]);
528 for (int k=0; k<3; k++)
529 KX_RasterizerDrawDebugLine(tri[k], tri[(k+1)%3], color);
539 MT_Point3 KX_NavMeshObject::TransformToLocalCoords(const MT_Point3& wpos)
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);
551 MT_Point3 KX_NavMeshObject::TransformToWorldCoords(const MT_Point3& lpos)
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);
561 int KX_NavMeshObject::FindPath(const MT_Point3& from, const MT_Point3& to, float* path, int maxPathLen)
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);
574 if (sPolyRef && ePolyRef)
576 dtStatPolyRef* polys = new dtStatPolyRef[maxPathLen];
578 npolys = m_navMesh->findPath(sPolyRef, ePolyRef, spos, epos, polys, maxPathLen);
581 pathLen = m_navMesh->findStraightPath(spos, epos, polys, npolys, path, maxPathLen);
582 for (int i=0; i<pathLen; i++)
584 flipAxes(&path[i*3]);
585 MT_Point3 waypoint(&path[i*3]);
586 waypoint = TransformToWorldCoords(waypoint);
587 waypoint.getValue(&path[i*3]);
595 float KX_NavMeshObject::Raycast(const MT_Point3& from, const MT_Point3& to)
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);
606 static dtStatPolyRef polys[MAX_PATH_LEN];
607 m_navMesh->raycast(sPolyRef, spos, epos, t, polys, MAX_PATH_LEN);
611 void KX_NavMeshObject::DrawPath(const float *path, int pathLen, const MT_Vector3& color)
614 for (int i=0; i<pathLen-1; i++)
616 a.setValue(&path[3*i]);
617 b.setValue(&path[3*(i+1)]);
618 KX_RasterizerDrawDebugLine(a, b, color);
624 //----------------------------------------------------------------------------
627 PyTypeObject KX_NavMeshObject::Type = {
628 PyVarObject_HEAD_INIT(NULL, 0)
630 sizeof(PyObjectPlus_Proxy),
642 Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE,
647 &KX_GameObject::Type,
652 PyAttributeDef KX_NavMeshObject::Attributes[] = {
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
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")
669 PyObject *ob_from, *ob_to;
670 if (!PyArg_ParseTuple(args,"OO:getPath",&ob_from,&ob_to))
673 if (!PyVecTo(ob_from, from) || !PyVecTo(ob_to, to))
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++)
681 MT_Point3 point(&path[3*i]);
682 PyList_SET_ITEM(pathList, i, PyObjectFrom(point));
688 KX_PYMETHODDEF_DOC(KX_NavMeshObject, raycast,
689 "raycast(start, goal): raycast from start to goal points\n"
690 "Returns hit factor)\n")
692 PyObject *ob_from, *ob_to;
693 if (!PyArg_ParseTuple(args,"OO:getPath",&ob_from,&ob_to))
696 if (!PyVecTo(ob_from, from) || !PyVecTo(ob_to, to))
698 float hit = Raycast(from, to);
699 return PyFloat_FromDouble(hit);
702 KX_PYMETHODDEF_DOC(KX_NavMeshObject, draw,
703 "draw(mode): navigation mesh debug drawing\n"
704 "mode: WALLS, POLYS, TRIS\n")
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);
714 KX_PYMETHODDEF_DOC_NOARGS(KX_NavMeshObject, rebuild,
715 "rebuild(): rebuild navigation mesh\n")
721 #endif // WITH_PYTHON