/** * $Id$ * ***** BEGIN GPL LICENSE BLOCK ***** * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software Foundation, * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. * * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. * All rights reserved. * * The Original Code is: all of this file. * * Contributor(s): none yet. * * ***** END GPL LICENSE BLOCK ***** */ #include "MEM_guardedalloc.h" #include "BLI_math_vector.h" #include "KX_NavMeshObject.h" #include "RAS_MeshObject.h" #include "DNA_mesh_types.h" #include "DNA_meshdata_types.h" extern "C" { #include "BKE_scene.h" #include "BKE_customdata.h" #include "BKE_cdderivedmesh.h" #include "BKE_DerivedMesh.h" #include "BKE_navmesh_conversion.h" } #include "KX_PythonInit.h" #include "KX_PyMath.h" #include "Value.h" #include "Recast.h" #include "DetourStatNavMeshBuilder.h" #include "KX_ObstacleSimulation.h" static const int MAX_PATH_LEN = 256; static const float polyPickExt[3] = {2, 4, 2}; static void calcMeshBounds(const float* vert, int nverts, float* bmin, float* bmax) { bmin[0] = bmax[0] = vert[0]; bmin[1] = bmax[1] = vert[1]; bmin[2] = bmax[2] = vert[2]; for (int i=1; ivert[3*i+0]) bmin[0] = vert[3*i+0]; if (bmin[1]>vert[3*i+1]) bmin[1] = vert[3*i+1]; if (bmin[2]>vert[3*i+2]) bmin[2] = vert[3*i+2]; if (bmax[0]ProcessReplica(); return replica; } void KX_NavMeshObject::ProcessReplica() { KX_GameObject::ProcessReplica(); BuildNavMesh(); KX_Scene* scene = KX_GetActiveScene(); KX_ObstacleSimulation* obssimulation = scene->GetObstacleSimulation(); if (obssimulation) obssimulation->AddObstaclesForNavMesh(this); } bool KX_NavMeshObject::BuildVertIndArrays(float *&vertices, int& nverts, unsigned short* &polys, int& npolys, unsigned short *&dmeshes, float *&dvertices, int &ndvertsuniq, unsigned short *&dtris, int& ndtris, int &vertsPerPoly) { DerivedMesh* dm = mesh_create_derived_no_virtual(KX_GetActiveScene()->GetBlenderScene(), GetBlenderObject(), NULL, CD_MASK_MESH); int* recastData = (int*) dm->getFaceDataArray(dm, CD_RECAST); if (recastData) { int *dtrisToPolysMap=NULL, *dtrisToTrisMap=NULL, *trisToFacesMap=NULL; int nAllVerts = 0; float *allVerts = NULL; buildNavMeshDataByDerivedMesh(dm, &vertsPerPoly, &nAllVerts, &allVerts, &ndtris, &dtris, &npolys, &dmeshes, &polys, &dtrisToPolysMap, &dtrisToTrisMap, &trisToFacesMap); MEM_freeN(dtrisToPolysMap); MEM_freeN(dtrisToTrisMap); MEM_freeN(trisToFacesMap); unsigned short *verticesMap = new unsigned short[nAllVerts]; memset(verticesMap, 0xffff, sizeof(unsigned short)*nAllVerts); int curIdx = 0; //vertices - mesh verts //iterate over all polys and create map for their vertices first... for (int polyidx=0; polyidx0) { dvertices = new float[ndvertsuniq*3]; } for (int vi=0; vim_sharedvertex_map.size(); if (nverts >= 0xffff) return false; //calculate count of tris int nmeshpolys = meshobj->NumPolygons(); npolys = nmeshpolys; for (int p=0; pGetPolygon(p)->VertexCount(); npolys+=vertcount-3; } //create verts vertices = new float[nverts*3]; float* vert = vertices; for (int vi=0; vim_sharedvertex_map[vi].empty() ? meshobj->GetVertexLocation(vi) : NULL; if (pos) copy_v3_v3(vert, pos); else { memset(vert, 0, 3*sizeof(float)); //vertex isn't in any poly, set dummy zero coordinates } vert+=3; } //create tris polys = new unsigned short[npolys*3*2]; memset(polys, 0xff, sizeof(unsigned short)*3*2*npolys); unsigned short *poly = polys; RAS_Polygon* raspoly; for (int p=0; pGetPolygon(p); for (int v=0; vVertexCount()-2; v++) { poly[0]= raspoly->GetVertex(0)->getOrigIndex(); for (size_t i=1; i<3; i++) { poly[i]= raspoly->GetVertex(v+i)->getOrigIndex(); } poly += 6; } } dmeshes = NULL; dvertices = NULL; ndvertsuniq = 0; dtris = NULL; ndtris = npolys; } dm->release(dm); return true; } bool KX_NavMeshObject::BuildNavMesh() { if (m_navMesh) { delete m_navMesh; m_navMesh = NULL; } if (GetMeshCount()==0) { printf("Can't find mesh for navmesh object: %s \n", m_name.ReadPtr()); return false; } float *vertices = NULL, *dvertices = NULL; unsigned short *polys = NULL, *dtris = NULL, *dmeshes = NULL; int nverts = 0, npolys = 0, ndvertsuniq = 0, ndtris = 0; int vertsPerPoly = 0; if (!BuildVertIndArrays(vertices, nverts, polys, npolys, dmeshes, dvertices, ndvertsuniq, dtris, ndtris, vertsPerPoly ) || vertsPerPoly<3) { printf("Can't build navigation mesh data for object:%s \n", m_name.ReadPtr()); return false; } MT_Point3 pos; if (dmeshes==NULL) { for (int i=0; i((vertices[3*i+0]-bmin[0])*ics); vertsi[3*i+1] = static_cast((vertices[3*i+1]-bmin[1])*ics); vertsi[3*i+2] = static_cast((vertices[3*i+2]-bmin[2])*ics); } // Calculate data size const int headerSize = sizeof(dtStatNavMeshHeader); const int vertsSize = sizeof(float)*3*nverts; const int polysSize = sizeof(dtStatPoly)*npolys; const int nodesSize = sizeof(dtStatBVNode)*npolys*2; const int detailMeshesSize = sizeof(dtStatPolyDetail)*npolys; const int detailVertsSize = sizeof(float)*3*ndvertsuniq; const int detailTrisSize = sizeof(unsigned char)*4*ndtris; const int dataSize = headerSize + vertsSize + polysSize + nodesSize + detailMeshesSize + detailVertsSize + detailTrisSize; unsigned char* data = new unsigned char[dataSize]; if (!data) return false; memset(data, 0, dataSize); unsigned char* d = data; dtStatNavMeshHeader* header = (dtStatNavMeshHeader*)d; d += headerSize; float* navVerts = (float*)d; d += vertsSize; dtStatPoly* navPolys = (dtStatPoly*)d; d += polysSize; dtStatBVNode* navNodes = (dtStatBVNode*)d; d += nodesSize; dtStatPolyDetail* navDMeshes = (dtStatPolyDetail*)d; d += detailMeshesSize; float* navDVerts = (float*)d; d += detailVertsSize; unsigned char* navDTris = (unsigned char*)d; d += detailTrisSize; // Store header header->magic = DT_STAT_NAVMESH_MAGIC; header->version = DT_STAT_NAVMESH_VERSION; header->npolys = npolys; header->nverts = nverts; header->cs = cs; header->bmin[0] = bmin[0]; header->bmin[1] = bmin[1]; header->bmin[2] = bmin[2]; header->bmax[0] = bmax[0]; header->bmax[1] = bmax[1]; header->bmax[2] = bmax[2]; header->ndmeshes = npolys; header->ndverts = ndvertsuniq; header->ndtris = ndtris; // Store vertices for (int i = 0; i < nverts; ++i) { const unsigned short* iv = &vertsi[i*3]; float* v = &navVerts[i*3]; v[0] = bmin[0] + iv[0] * cs; v[1] = bmin[1] + iv[1] * cs; v[2] = bmin[2] + iv[2] * cs; } //memcpy(navVerts, vertices, nverts*3*sizeof(float)); // Store polygons const unsigned short* src = polys; for (int i = 0; i < npolys; ++i) { dtStatPoly* p = &navPolys[i]; p->nv = 0; for (int j = 0; j < vertsPerPoly; ++j) { if (src[j] == 0xffff) break; p->v[j] = src[j]; p->n[j] = src[vertsPerPoly+j]+1; p->nv++; } src += vertsPerPoly*2; } header->nnodes = createBVTree(vertsi, nverts, polys, npolys, vertsPerPoly, cs, cs, npolys*2, navNodes); if (dmeshes==NULL) { //create fake detail meshes for (int i = 0; i < npolys; ++i) { dtStatPolyDetail& dtl = navDMeshes[i]; dtl.vbase = 0; dtl.nverts = 0; dtl.tbase = i; dtl.ntris = 1; } // setup triangles. unsigned char* tri = navDTris; for(size_t i=0; iinit(data, dataSize, true); delete [] vertices; /* navmesh conversion is using C guarded alloc for memory allocaitons */ MEM_freeN(polys); if (dmeshes) MEM_freeN(dmeshes); if (dtris) MEM_freeN(dtris); if (dvertices) { delete [] dvertices; } return true; } dtStatNavMesh* KX_NavMeshObject::GetNavMesh() { return m_navMesh; } void KX_NavMeshObject::DrawNavMesh(NavMeshRenderMode renderMode) { if (!m_navMesh) return; MT_Vector3 color(0.f, 0.f, 0.f); switch (renderMode) { case RM_POLYS : case RM_WALLS : for (int pi=0; pigetPolyCount(); pi++) { const dtStatPoly* poly = m_navMesh->getPoly(pi); for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++) { if (poly->n[j] && renderMode==RM_WALLS) continue; const float* vif = m_navMesh->getVertex(poly->v[i]); const float* vjf = m_navMesh->getVertex(poly->v[j]); MT_Point3 vi(vif[0], vif[2], vif[1]); MT_Point3 vj(vjf[0], vjf[2], vjf[1]); vi = TransformToWorldCoords(vi); vj = TransformToWorldCoords(vj); KX_RasterizerDrawDebugLine(vi, vj, color); } } break; case RM_TRIS : for (int i = 0; i < m_navMesh->getPolyDetailCount(); ++i) { const dtStatPoly* p = m_navMesh->getPoly(i); const dtStatPolyDetail* pd = m_navMesh->getPolyDetail(i); for (int j = 0; j < pd->ntris; ++j) { const unsigned char* t = m_navMesh->getDetailTri(pd->tbase+j); MT_Point3 tri[3]; for (int k = 0; k < 3; ++k) { const float* v; if (t[k] < p->nv) v = m_navMesh->getVertex(p->v[t[k]]); else v = m_navMesh->getDetailVertex(pd->vbase+(t[k]-p->nv)); float pos[3]; vcopy(pos, v); flipAxes(pos); tri[k].setValue(pos); } for (int k=0; k<3; k++) tri[k] = TransformToWorldCoords(tri[k]); for (int k=0; k<3; k++) KX_RasterizerDrawDebugLine(tri[k], tri[(k+1)%3], color); } } break; default: /* pass */ break; } } MT_Point3 KX_NavMeshObject::TransformToLocalCoords(const MT_Point3& wpos) { MT_Matrix3x3 orientation = NodeGetWorldOrientation(); const MT_Vector3& scaling = NodeGetWorldScaling(); orientation.scale(scaling[0], scaling[1], scaling[2]); MT_Transform worldtr(NodeGetWorldPosition(), orientation); MT_Transform invworldtr; invworldtr.invert(worldtr); MT_Point3 lpos = invworldtr(wpos); return lpos; } MT_Point3 KX_NavMeshObject::TransformToWorldCoords(const MT_Point3& lpos) { MT_Matrix3x3 orientation = NodeGetWorldOrientation(); const MT_Vector3& scaling = NodeGetWorldScaling(); orientation.scale(scaling[0], scaling[1], scaling[2]); MT_Transform worldtr(NodeGetWorldPosition(), orientation); MT_Point3 wpos = worldtr(lpos); return wpos; } int KX_NavMeshObject::FindPath(const MT_Point3& from, const MT_Point3& to, float* path, int maxPathLen) { if (!m_navMesh) return 0; MT_Point3 localfrom = TransformToLocalCoords(from); MT_Point3 localto = TransformToLocalCoords(to); float spos[3], epos[3]; localfrom.getValue(spos); flipAxes(spos); localto.getValue(epos); flipAxes(epos); dtStatPolyRef sPolyRef = m_navMesh->findNearestPoly(spos, polyPickExt); dtStatPolyRef ePolyRef = m_navMesh->findNearestPoly(epos, polyPickExt); int pathLen = 0; if (sPolyRef && ePolyRef) { dtStatPolyRef* polys = new dtStatPolyRef[maxPathLen]; int npolys; npolys = m_navMesh->findPath(sPolyRef, ePolyRef, spos, epos, polys, maxPathLen); if (npolys) { pathLen = m_navMesh->findStraightPath(spos, epos, polys, npolys, path, maxPathLen); for (int i=0; ifindNearestPoly(spos, polyPickExt); float t=0; static dtStatPolyRef polys[MAX_PATH_LEN]; m_navMesh->raycast(sPolyRef, spos, epos, t, polys, MAX_PATH_LEN); return t; } void KX_NavMeshObject::DrawPath(const float *path, int pathLen, const MT_Vector3& color) { MT_Vector3 a,b; for (int i=0; i=0 && arg