source/blender/blenkernel/intern/navmesh_conversion.c

   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 /** \file blender/blenkernel/intern/navmesh_conversion.c
  29  *  \ingroup bke
  30  */
  31
  32 #include <math.h>
  33 #include <stdlib.h>
  34
  35 #include "MEM_guardedalloc.h"
  36
  37 #include "DNA_meshdata_types.h"
  38
  39 #include "BKE_navmesh_conversion.h"
  40 #include "BKE_cdderivedmesh.h"
  41
  42 #include "BLI_utildefines.h"
  43 #include "BLI_math.h"
  44
  45 #include "recast-capi.h"
  46
  47 BM_INLINE float area2(const float* a, const float* b, const float* c)
  48 {
  49         return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
  50 }
  51
  52 BM_INLINE int left(const float* a, const float* b, const float* c)
  53 {
  54         return area2(a, b, c) < 0;
  55 }
  56
  57 int polyNumVerts(const unsigned short* p, const int vertsPerPoly)
  58 {
  59         int i, nv = 0;
  60         for (i=0; i<vertsPerPoly; i++)
  61         {
  62                 if (p[i]==0xffff)
  63                         break;
  64                 nv++;
  65         }
  66         return nv;
  67 }
  68
  69 int polyIsConvex(const unsigned short* p, const int vertsPerPoly, const float* verts)
  70 {
  71         int j, nv = polyNumVerts(p, vertsPerPoly);
  72         if (nv<3)
  73                 return 0;
  74         for (j=0; j<nv; j++)
  75         {
  76                 const float* v = &verts[3*p[j]];
  77                 const float* v_next = &verts[3*p[(j+1)%nv]];
  78                 const float* v_prev = &verts[3*p[(nv+j-1)%nv]];
  79                 if (!left(v_prev, v, v_next))
  80                         return 0;
  81
  82         }
  83         return 1;
  84 }
  85
  86 float distPointToSegmentSq(const float* point, const float* a, const float* b)
  87 {
  88         float abx[3], dx[3];
  89         float d, t;
  90
  91         sub_v3_v3v3(abx, b,a);
  92         sub_v3_v3v3(dx, point,a);
  93
  94         d = abx[0]*abx[0]+abx[2]*abx[2];
  95         t = abx[0]*dx[0]+abx[2]*dx[2];
  96
  97         if (d > 0)
  98                 t /= d;
  99         if (t < 0)
 100                 t = 0;
 101         else if (t > 1)
 102                 t = 1;
 103         dx[0] = a[0] + t*abx[0] - point[0];
 104         dx[2] = a[2] + t*abx[2] - point[2];
 105
 106         return dx[0]*dx[0] + dx[2]*dx[2];
 107 }
 108
 109 int buildRawVertIndicesData(DerivedMesh* dm, int *nverts_r, float **verts_r,
 110                                                                         int *ntris_r, unsigned short **tris_r, int **trisToFacesMap_r,
 111                                                                         int **recastData)
 112 {
 113         int vi, fi, triIdx;
 114         int nverts, ntris;
 115         int *trisToFacesMap;
 116         float *verts;
 117         unsigned short *tris, *tri;
 118         int nfaces;
 119         MFace *faces;
 120
 121         nverts = dm->getNumVerts(dm);
 122         if (nverts>=0xffff)
 123         {
 124                 printf("Converting navmesh: Error! Too many vertices. Max number of vertices %d\n", 0xffff);
 125                 return 0;
 126         }
 127         verts = MEM_callocN(sizeof(float)*3*nverts, "buildRawVertIndicesData verts");
 128         dm->getVertCos(dm, (float(*)[3])verts);
 129
 130         //flip coordinates
 131         for (vi=0; vi<nverts; vi++)
 132         {
 133                 SWAP(float, verts[3*vi+1], verts[3*vi+2]);
 134         }
 135
 136         //calculate number of tris
 137         nfaces = dm->getNumTessFaces(dm);
 138         faces = dm->getTessFaceArray(dm);
 139         ntris = nfaces;
 140         for (fi=0; fi<nfaces; fi++)
 141         {
 142                 MFace* face = &faces[fi];
 143                 if (face->v4)
 144                         ntris++;
 145         }
 146
 147         //copy and transform to triangles (reorder on the run)
 148         trisToFacesMap = MEM_callocN(sizeof(int)*ntris, "buildRawVertIndicesData trisToFacesMap");
 149         tris = MEM_callocN(sizeof(unsigned short)*3*ntris, "buildRawVertIndicesData tris");
 150         tri = tris;
 151         triIdx = 0;
 152         for (fi=0; fi<nfaces; fi++)
 153         {
 154                 MFace* face = &faces[fi];
 155                 tri[3*triIdx+0] = (unsigned short) face->v1;
 156                 tri[3*triIdx+1] = (unsigned short) face->v3;
 157                 tri[3*triIdx+2] = (unsigned short) face->v2;
 158                 trisToFacesMap[triIdx++]=fi;
 159                 if (face->v4)
 160                 {
 161                         tri[3*triIdx+0] = (unsigned short) face->v1;
 162                         tri[3*triIdx+1] = (unsigned short) face->v4;
 163                         tri[3*triIdx+2] = (unsigned short) face->v3;
 164                         trisToFacesMap[triIdx++]=fi;
 165                 }
 166         }
 167
 168         //carefully, recast data is just reference to data in derived mesh
 169         *recastData = (int*)CustomData_get_layer(&dm->faceData, CD_RECAST);
 170
 171         *nverts_r = nverts;
 172         *verts_r = verts;
 173         *ntris_r = ntris;
 174         *tris_r = tris;
 175         *trisToFacesMap_r = trisToFacesMap;
 176
 177         return 1;
 178 }
 179
 180 int buildPolygonsByDetailedMeshes(const int vertsPerPoly, const int npolys,
 181                                                                                   unsigned short* polys, const unsigned short* dmeshes,
 182                                                                                   const float* verts, const unsigned short* dtris,
 183                                                                                   const int* dtrisToPolysMap)
 184 {
 185         int polyidx;
 186         int capacity = vertsPerPoly;
 187         unsigned short* newPoly = MEM_callocN(sizeof(unsigned short)*capacity, "buildPolygonsByDetailedMeshes newPoly");
 188         memset(newPoly, 0xff, sizeof(unsigned short)*capacity);
 189
 190         for (polyidx=0; polyidx<npolys; polyidx++)
 191         {
 192                 size_t i;
 193                 int j, k;
 194                 int nv = 0;
 195                 //search border
 196                 int tri, btri = -1;
 197                 int edge, bedge = -1;
 198                 int dtrisNum = dmeshes[polyidx*4+3];
 199                 int dtrisBase = dmeshes[polyidx*4+2];
 200                 unsigned char *traversedTris = MEM_callocN(sizeof(unsigned char)*dtrisNum, "buildPolygonsByDetailedMeshes traversedTris");
 201                 unsigned short* adjustedPoly;
 202                 int adjustedNv;
 203                 int allBorderTraversed;
 204
 205                 for (j=0; j<dtrisNum && btri==-1;j++)
 206                 {
 207                         int curpolytri = dtrisBase+j;
 208                         for (k=0; k<3; k++)
 209                         {
 210                                 unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
 211                                 if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
 212                                 {
 213                                         btri = curpolytri;
 214                                         bedge = k;
 215                                         break;
 216                                 }
 217                         }
 218                 }
 219                 if (btri==-1 || bedge==-1)
 220                 {
 221                         //can't find triangle with border edge
 222                         MEM_freeN(traversedTris);
 223                         MEM_freeN(newPoly);
 224
 225                         return 0;
 226                 }
 227
 228                 newPoly[nv++] = dtris[btri*3*2+bedge];
 229                 tri = btri;
 230                 edge = (bedge+1)%3;
 231                 traversedTris[tri-dtrisBase] = 1;
 232                 while (tri!=btri || edge!=bedge)
 233                 {
 234                         int neighbortri = dtris[tri*3*2+3+edge];
 235                         if (neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
 236                         {
 237                                 if (nv==capacity)
 238                                 {
 239                                         unsigned short* newPolyBig;
 240                                         capacity += vertsPerPoly;
 241                                         newPolyBig = MEM_callocN(sizeof(unsigned short)*capacity, "buildPolygonsByDetailedMeshes newPolyBig");
 242                                         memset(newPolyBig, 0xff, sizeof(unsigned short)*capacity);
 243                                         memcpy(newPolyBig, newPoly, sizeof(unsigned short)*nv);
 244                                         MEM_freeN(newPoly);
 245                                         newPoly = newPolyBig;
 246                                 }
 247                                 newPoly[nv++] = dtris[tri*3*2+edge];
 248                                 //move to next edge
 249                                 edge = (edge+1)%3;
 250                         }
 251                         else
 252                         {
 253                                 //move to next tri
 254                                 int twinedge = -1;
 255                                 for (k=0; k<3; k++)
 256                                 {
 257                                         if (dtris[neighbortri*3*2+3+k] == tri)
 258                                         {
 259                                                 twinedge = k;
 260                                                 break;
 261                                         }
 262                                 }
 263                                 if (twinedge==-1)
 264                                 {
 265                                         printf("Converting navmesh: Error! Can't find neighbor edge - invalid adjacency info\n");
 266                                         MEM_freeN(traversedTris);
 267                                         goto returnLabel;
 268                                 }
 269                                 tri = neighbortri;
 270                                 edge = (twinedge+1)%3;
 271                                 traversedTris[tri-dtrisBase] = 1;
 272                         }
 273                 }
 274
 275                 adjustedPoly = MEM_callocN(sizeof(unsigned short)*nv, "buildPolygonsByDetailedMeshes adjustedPoly");
 276                 adjustedNv = 0;
 277                 for (i=0; i<nv; i++)
 278                 {
 279                         unsigned short prev = newPoly[(nv+i-1)%nv];
 280                         unsigned short cur = newPoly[i];
 281                         unsigned short next = newPoly[(i+1)%nv];
 282                         float distSq = distPointToSegmentSq(&verts[3*cur], &verts[3*prev], &verts[3*next]);
 283                         static const float tolerance = 0.001f;
 284                         if (distSq>tolerance)
 285                                 adjustedPoly[adjustedNv++] = cur;
 286                 }
 287                 memcpy(newPoly, adjustedPoly, adjustedNv*sizeof(unsigned short));
 288                 MEM_freeN(adjustedPoly);
 289                 nv = adjustedNv;
 290
 291                 allBorderTraversed = 1;
 292                 for (i=0; i<dtrisNum; i++)
 293                 {
 294                         if (traversedTris[i]==0)
 295                         {
 296                                 //check whether it has border edges
 297                                 int curpolytri = dtrisBase+i;
 298                                 for (k=0; k<3; k++)
 299                                 {
 300                                         unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
 301                                         if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
 302                                         {
 303                                                 allBorderTraversed = 0;
 304                                                 break;
 305                                         }
 306                                 }
 307                         }
 308                 }
 309
 310                 if (nv<=vertsPerPoly && allBorderTraversed)
 311                 {
 312                         for (i=0; i<nv; i++)
 313                         {
 314                                 polys[polyidx*vertsPerPoly*2+i] = newPoly[i];
 315                         }
 316                 }
 317
 318                 MEM_freeN(traversedTris);
 319         }
 320
 321 returnLabel:
 322         MEM_freeN(newPoly);
 323
 324         return 1;
 325 }
 326
 327 struct SortContext
 328 {
 329         const int* recastData;
 330         const int* trisToFacesMap;
 331 };
 332
 333 static int compareByData(void *ctx, const void * a, const void * b)
 334 {
 335         return (((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int*)a]] -
 336                         ((struct SortContext *)ctx)->recastData[((struct SortContext *)ctx)->trisToFacesMap[*(int*)b]] );
 337 }
 338
 339 int buildNavMeshData(const int nverts, const float* verts,
 340                                                          const int ntris, const unsigned short *tris,
 341                                                          const int* recastData, const int* trisToFacesMap,
 342                                                          int *ndtris_r, unsigned short **dtris_r,
 343                                                          int *npolys_r, unsigned short **dmeshes_r, unsigned short **polys_r,
 344                                                          int *vertsPerPoly_r, int **dtrisToPolysMap_r, int **dtrisToTrisMap_r)
 345
 346 {
 347         int *trisMapping = MEM_callocN(sizeof(int)*ntris, "buildNavMeshData trisMapping");
 348         int i;
 349         struct SortContext context;
 350         int validTriStart, prevPolyIdx, curPolyIdx, newPolyIdx, prevpolyidx;
 351         unsigned short *dmesh;
 352
 353         int ndtris, npolys, vertsPerPoly;
 354         unsigned short *dtris, *dmeshes, *polys;
 355         int *dtrisToPolysMap, *dtrisToTrisMap;
 356
 357         if (!recastData)
 358         {
 359                 printf("Converting navmesh: Error! Can't find recast custom data\n");
 360                 return 0;
 361         }
 362
 363         //sort the triangles by polygon idx
 364         for (i=0; i<ntris; i++)
 365                 trisMapping[i]=i;
 366         context.recastData = recastData;
 367         context.trisToFacesMap = trisToFacesMap;
 368         recast_qsort(trisMapping, ntris, sizeof(int), &context, compareByData);
 369
 370         //search first valid triangle - triangle of convex polygon
 371         validTriStart = -1;
 372         for (i=0; i< ntris; i++)
 373         {
 374                 if (recastData[trisToFacesMap[trisMapping[i]]]>0)
 375                 {
 376                         validTriStart = i;
 377                         break;
 378                 }
 379         }
 380
 381         if (validTriStart<0)
 382         {
 383                 printf("Converting navmesh: Error! No valid polygons in mesh\n");
 384                 MEM_freeN(trisMapping);
 385                 return 0;
 386         }
 387
 388         ndtris = ntris-validTriStart;
 389         //fill dtris to faces mapping
 390         dtrisToTrisMap = MEM_callocN(sizeof(int)*ndtris, "buildNavMeshData dtrisToTrisMap");
 391         memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris*sizeof(int));
 392         MEM_freeN(trisMapping);
 393
 394         //create detailed mesh triangles  - copy only valid triangles
 395         //and reserve memory for adjacency info
 396         dtris = MEM_callocN(sizeof(unsigned short)*3*2*ndtris, "buildNavMeshData dtris");
 397         memset(dtris, 0xffff, sizeof(unsigned short)*3*2*ndtris);
 398         for (i=0; i<ndtris; i++)
 399         {
 400                 memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
 401         }
 402
 403         //create new recast data corresponded to dtris and renumber for continuous indices
 404         prevPolyIdx = -1;
 405         newPolyIdx = 0;
 406         dtrisToPolysMap = MEM_callocN(sizeof(int)*ndtris, "buildNavMeshData dtrisToPolysMap");
 407         for (i=0; i<ndtris; i++)
 408         {
 409                 curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
 410                 if (curPolyIdx!=prevPolyIdx)
 411                 {
 412                         newPolyIdx++;
 413                         prevPolyIdx=curPolyIdx;
 414                 }
 415                 dtrisToPolysMap[i] = newPolyIdx;
 416         }
 417
 418
 419         //build adjacency info for detailed mesh triangles
 420         recast_buildMeshAdjacency(dtris, ndtris, nverts, 3);
 421
 422         //create detailed mesh description for each navigation polygon
 423         npolys = dtrisToPolysMap[ndtris-1];
 424         dmeshes = MEM_callocN(sizeof(unsigned short)*npolys*4, "buildNavMeshData dmeshes");
 425         memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
 426         dmesh = NULL;
 427         prevpolyidx = 0;
 428         for (i=0; i<ndtris; i++)
 429         {
 430                 int curpolyidx = dtrisToPolysMap[i];
 431                 if (curpolyidx!=prevpolyidx)
 432                 {
 433                         if (curpolyidx!=prevpolyidx+1)
 434                         {
 435                                 printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
 436                                 return 0;
 437                         }
 438                         dmesh = dmesh==NULL ? dmeshes : dmesh+4;
 439                         dmesh[2] = (unsigned short)i;   //tbase
 440                         dmesh[3] = 0;   //tnum
 441                         prevpolyidx = curpolyidx;
 442                 }
 443                 dmesh[3]++;
 444         }
 445
 446         //create navigation polygons
 447         vertsPerPoly = 6;
 448         polys = MEM_callocN(sizeof(unsigned short)*npolys*vertsPerPoly*2, "buildNavMeshData polys");
 449         memset(polys, 0xff, sizeof(unsigned short)*vertsPerPoly*2*npolys);
 450
 451         buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
 452
 453         *ndtris_r = ndtris;
 454         *npolys_r = npolys;
 455         *vertsPerPoly_r = vertsPerPoly;
 456         *dtris_r = dtris;
 457         *dmeshes_r = dmeshes;
 458         *polys_r = polys;
 459         *dtrisToPolysMap_r = dtrisToPolysMap;
 460         *dtrisToTrisMap_r = dtrisToTrisMap;
 461
 462         return 1;
 463 }
 464
 465
 466 int buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int *vertsPerPoly,
 467                                                                                   int *nverts, float **verts,
 468                                                                                   int *ndtris, unsigned short **dtris,
 469                                                                                   int *npolys, unsigned short **dmeshes,
 470                                                                                   unsigned short **polys, int **dtrisToPolysMap,
 471                                                                                   int **dtrisToTrisMap, int **trisToFacesMap)
 472 {
 473         int res = 1;
 474         int ntris = 0, *recastData=NULL;
 475         unsigned short *tris=NULL;
 476
 477         res = buildRawVertIndicesData(dm, nverts, verts, &ntris, &tris, trisToFacesMap, &recastData);
 478         if (!res)
 479         {
 480                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
 481                 goto exit;
 482         }
 483
 484         res = buildNavMeshData(*nverts, *verts, ntris, tris, recastData, *trisToFacesMap,
 485                 ndtris, dtris, npolys, dmeshes,polys, vertsPerPoly,
 486                 dtrisToPolysMap, dtrisToTrisMap);
 487         if (!res)
 488         {
 489                 printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
 490                 goto exit;
 491         }
 492
 493 exit:
 494         if (tris)
 495                 MEM_freeN(tris);
 496
 497         return res;
 498 }
 499
 500 int polyFindVertex(const unsigned short* p, const int vertsPerPoly, unsigned short vertexIdx)
 501 {
 502         int i, res = -1;
 503         for(i=0; i<vertsPerPoly; i++)
 504         {
 505                 if (p[i]==0xffff)
 506                         break;
 507                 if (p[i]==vertexIdx)
 508                 {
 509                         res = i;
 510                         break;
 511                 }
 512         }
 513         return res;
 514 }