f7527e2d5d5a812aa3277d82ab638f77dfb6741f
[blender.git] / source / blender / editors / object / object_navmesh.cpp
1 /**
2 * $Id:$
3 *
4 * ***** BEGIN GPL LICENSE BLOCK *****
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software Foundation,
18 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19 *
20 * The Original Code is Copyright (C) 2004 by Blender Foundation
21 * All rights reserved.
22 *
23 * The Original Code is: all of this file.
24 *
25 * Contributor(s): none yet.
26 *
27 * ***** END GPL LICENSE BLOCK *****
28 */
29 #include <math.h>
30 #include "Recast.h"
31
32 extern "C"
33 {
34 #include "MEM_guardedalloc.h"
35
36 #include "DNA_scene_types.h"
37 #include "DNA_object_types.h"
38 #include "DNA_meshdata_types.h"
39 #include "DNA_modifier_types.h"
40 #include "DNA_ID.h"
41
42 #include "BKE_library.h"
43 #include "BKE_depsgraph.h"
44 #include "BKE_context.h"
45 #include "BKE_mesh.h"
46 #include "BKE_DerivedMesh.h"
47 #include "BKE_cdderivedmesh.h"
48 #include "BLI_editVert.h"
49 #include "BLI_listbase.h"
50 #include "ED_object.h"
51 #include "BLI_math_vector.h"
52
53 #include "RNA_access.h"
54
55 #include "ED_mesh.h"
56
57 /*mesh/mesh_intern.h */
58 extern struct EditVert *addvertlist(EditMesh *em, float *vec, struct EditVert *example);
59 extern struct EditFace *addfacelist(EditMesh *em, struct EditVert *v1, struct EditVert *v2, struct EditVert *v3, struct EditVert *v4, struct EditFace *example, struct EditFace *exampleEdges);
60
61 #include "WM_api.h"
62 #include "WM_types.h"
63
64 static void createVertsTrisData(bContext *C, LinkNode* obs, int& nverts, float*& verts, int &ntris, int*& tris)
65 {
66         MVert *mvert;
67         int nfaces = 0, *tri, i, curnverts, basenverts, curnfaces;
68         MFace *mface;
69         float co[3], wco[3];
70         Object *ob;
71         LinkNode *oblink, *dmlink;
72         DerivedMesh *dm;
73         Scene* scene = CTX_data_scene(C);
74         LinkNode* dms = NULL;
75
76         nverts = 0;
77         ntris = 0;
78         //calculate number of verts and tris
79         for (oblink = obs; oblink; oblink = oblink->next) 
80         {
81                 ob = (Object*) oblink->link;    
82                 DerivedMesh *dm = mesh_create_derived_no_virtual(scene, ob, NULL, CD_MASK_MESH);
83                 BLI_linklist_append(&dms, (void*)dm);
84
85                 nverts += dm->getNumVerts(dm);
86                 nfaces = dm->getNumFaces(dm);
87                 ntris += nfaces;
88
89                 //resolve quad faces
90                 mface = dm->getFaceArray(dm);
91                 for (i=0; i<nfaces; i++)
92                 {
93                         MFace* mf = &mface[i];
94                         if (mf->v4)
95                                 ntris+=1;
96                 }
97         }
98
99         //create data
100         verts = (float*) MEM_mallocN(sizeof(float)*3*nverts, "verts");
101         tris = (int*) MEM_mallocN(sizeof(int)*3*ntris, "faces");
102
103         basenverts = 0;
104         tri = tris;
105         for (oblink = obs, dmlink = dms; oblink && dmlink; 
106                         oblink = oblink->next, dmlink = dmlink->next)
107         {
108                 ob = (Object*) oblink->link;
109                 dm = (DerivedMesh*) dmlink->link;
110
111                 curnverts = dm->getNumVerts(dm);
112                 mvert = dm->getVertArray(dm);
113                 //copy verts    
114                 for (i=0; i<curnverts; i++)
115                 {
116                         MVert *v = &mvert[i];
117                         copy_v3_v3(co, v->co);
118                         mul_v3_m4v3(wco, ob->obmat, co);
119                         verts[3*(basenverts+i)+0] = wco[0];
120                         verts[3*(basenverts+i)+1] = wco[2];
121                         verts[3*(basenverts+i)+2] = wco[1];
122                 }
123
124                 //create tris
125                 curnfaces = dm->getNumFaces(dm);
126                 mface = dm->getFaceArray(dm);
127                 for (i=0; i<curnfaces; i++)
128                 {
129                         MFace* mf = &mface[i]; 
130                         tri[0]= basenverts + mf->v1; tri[1]= basenverts + mf->v3;       tri[2]= basenverts + mf->v2; 
131                         tri += 3;
132                         if (mf->v4)
133                         {
134                                 tri[0]= basenverts + mf->v1; tri[1]= basenverts + mf->v4; tri[2]= basenverts + mf->v3; 
135                                 tri += 3;
136                         }
137                 }
138                 basenverts += curnverts;
139         }
140
141         //release derived mesh
142         for (dmlink = dms; dmlink; dmlink = dmlink->next)
143         {
144                 dm = (DerivedMesh*) dmlink->link;
145                 dm->release(dm);
146         }
147         BLI_linklist_free(dms, NULL);
148 }
149
150 static bool buildNavMesh(const RecastData& recastParams, int nverts, float* verts, int ntris, int* tris,
151                                                                  rcPolyMesh*& pmesh, rcPolyMeshDetail*& dmesh)
152 {
153         float bmin[3], bmax[3];
154         rcHeightfield* solid;
155         unsigned char *triflags;
156         rcCompactHeightfield* chf;
157         rcContourSet *cset;
158
159         rcCalcBounds(verts, nverts, bmin, bmax);
160
161         //
162         // Step 1. Initialize build config.
163         //
164         rcConfig cfg;
165         memset(&cfg, 0, sizeof(cfg));
166         {
167 /*
168                 float cellsize = 0.3f;
169                 float cellheight = 0.2f;
170                 float agentmaxslope = M_PI/4;
171                 float agentmaxclimb = 0.9f;
172                 float agentheight = 2.0f;
173                 float agentradius = 0.6f;
174                 float edgemaxlen = 12.0f;
175                 float edgemaxerror = 1.3f;
176                 float regionminsize = 50.f;
177                 float regionmergesize = 20.f;
178                 int vertsperpoly = 6;
179                 float detailsampledist = 6.0f;
180                 float detailsamplemaxerror = 1.0f;
181                 cfg.cs = cellsize;
182                 cfg.ch = cellheight;
183                 cfg.walkableSlopeAngle = agentmaxslope/M_PI*180.f;
184                 cfg.walkableHeight = (int)ceilf(agentheight/ cfg.ch);
185                 cfg.walkableClimb = (int)floorf(agentmaxclimb / cfg.ch);
186                 cfg.walkableRadius = (int)ceilf(agentradius / cfg.cs);
187                 cfg.maxEdgeLen = (int)(edgemaxlen/cellsize);
188                 cfg.maxSimplificationError = edgemaxerror;
189                 cfg.minRegionSize = (int)rcSqr(regionminsize);
190                 cfg.mergeRegionSize = (int)rcSqr(regionmergesize);
191                 cfg.maxVertsPerPoly = vertsperpoly;
192                 cfg.detailSampleDist = detailsampledist< 0.9f ? 0 : cellsize * detailsampledist;
193                 cfg.detailSampleMaxError = cellheight * detailsamplemaxerror;
194 */
195                 cfg.cs = recastParams.cellsize;
196                 cfg.ch = recastParams.cellheight;
197                 cfg.walkableSlopeAngle = recastParams.agentmaxslope/((float)M_PI)*180.f;
198                 cfg.walkableHeight = (int)ceilf(recastParams.agentheight/ cfg.ch);
199                 cfg.walkableClimb = (int)floorf(recastParams.agentmaxclimb / cfg.ch);
200                 cfg.walkableRadius = (int)ceilf(recastParams.agentradius / cfg.cs);
201                 cfg.maxEdgeLen = (int)(recastParams.edgemaxlen/recastParams.cellsize);
202                 cfg.maxSimplificationError = recastParams.edgemaxerror;
203                 cfg.minRegionSize = (int)rcSqr(recastParams.regionminsize);
204                 cfg.mergeRegionSize = (int)rcSqr(recastParams.regionmergesize);
205                 cfg.maxVertsPerPoly = recastParams.vertsperpoly;
206                 cfg.detailSampleDist = recastParams.detailsampledist< 0.9f ? 0 : 
207                                                                 recastParams.cellsize * recastParams.detailsampledist;
208                 cfg.detailSampleMaxError = recastParams.cellheight * recastParams.detailsamplemaxerror;
209
210         }
211
212         // Set the area where the navigation will be build.
213         vcopy(cfg.bmin, bmin);
214         vcopy(cfg.bmax, bmax);
215         rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
216
217         //
218         // Step 2. Rasterize input polygon soup.
219         //
220         // Allocate voxel heightfield where we rasterize our input data to.
221         solid = new rcHeightfield;
222         if (!solid)
223                 return false;
224
225         if (!rcCreateHeightfield(*solid, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch))
226                 return false;
227
228         // Allocate array that can hold triangle flags.
229         triflags = (unsigned char*) MEM_mallocN(sizeof(unsigned char)*ntris, "triflags");
230         if (!triflags)
231                 return false;
232         // Find triangles which are walkable based on their slope and rasterize them.
233         memset(triflags, 0, ntris*sizeof(unsigned char));
234         rcMarkWalkableTriangles(cfg.walkableSlopeAngle, verts, nverts, tris, ntris, triflags);
235         rcRasterizeTriangles(verts, nverts, tris, triflags, ntris, *solid);
236         MEM_freeN(triflags);
237         MEM_freeN(verts);
238         MEM_freeN(tris);
239
240         //
241         // Step 3. Filter walkables surfaces.
242         //
243         rcFilterLedgeSpans(cfg.walkableHeight, cfg.walkableClimb, *solid);
244         rcFilterWalkableLowHeightSpans(cfg.walkableHeight, *solid);
245
246         //
247         // Step 4. Partition walkable surface to simple regions.
248         //
249
250         chf = new rcCompactHeightfield;
251         if (!chf)
252                 return false;
253         if (!rcBuildCompactHeightfield(cfg.walkableHeight, cfg.walkableClimb, RC_WALKABLE, *solid, *chf))
254                 return false;
255
256         delete solid; 
257
258         // Prepare for region partitioning, by calculating distance field along the walkable surface.
259         if (!rcBuildDistanceField(*chf))
260                 return false;
261
262         // Partition the walkable surface into simple regions without holes.
263         if (!rcBuildRegions(*chf, cfg.walkableRadius, cfg.borderSize, cfg.minRegionSize, cfg.mergeRegionSize))
264                 return false;
265
266         //
267         // Step 5. Trace and simplify region contours.
268         //
269         // Create contours.
270         cset = new rcContourSet;
271         if (!cset)
272                 return false;
273
274         if (!rcBuildContours(*chf, cfg.maxSimplificationError, cfg.maxEdgeLen, *cset))
275                 return false;
276
277         //
278         // Step 6. Build polygons mesh from contours.
279         //
280         pmesh = new rcPolyMesh;
281         if (!pmesh)
282                 return false;
283         if (!rcBuildPolyMesh(*cset, cfg.maxVertsPerPoly, *pmesh))
284                 return false;
285
286
287         //
288         // Step 7. Create detail mesh which allows to access approximate height on each polygon.
289         //
290
291         dmesh = new rcPolyMeshDetail;
292         if (!dmesh)
293                 return false;
294
295         if (!rcBuildPolyMeshDetail(*pmesh, *chf, cfg.detailSampleDist, cfg.detailSampleMaxError, *dmesh))
296                 return false;
297
298         delete chf;
299         delete cset;
300
301         return true;
302 }
303
304 static Object* createRepresentation(bContext *C, rcPolyMesh*& pmesh, rcPolyMeshDetail*& dmesh)
305 {
306         Object *obedit;
307         float co[3], rot[3];
308         EditMesh *em;
309         int i,j, k, polyverts;
310         unsigned short* v;
311         int face[3];
312         Scene *scene= CTX_data_scene(C);
313
314         zero_v3(co);
315         zero_v3(rot);
316         obedit = ED_object_add_type(C, OB_MESH, co, rot, FALSE, 1);
317         ED_object_enter_editmode(C, EM_DO_UNDO|EM_IGNORE_LAYER);
318
319         em = BKE_mesh_get_editmesh(((Mesh *)obedit->data));
320
321         //create verts for polygon mesh
322         for(i = 0; i < pmesh->nverts; i++) {
323                 v = &pmesh->verts[3*i];
324                 co[0] = pmesh->bmin[0] + v[0]*pmesh->cs;
325                 co[1] = pmesh->bmin[1] + v[1]*pmesh->ch;
326                 co[2] = pmesh->bmin[2] + v[2]*pmesh->cs;
327                 SWAP(float, co[1], co[2]);
328                 addvertlist(em, co, NULL);
329         }
330         polyverts = pmesh->nverts;
331
332         //create custom data layer to save polygon idx
333         CustomData_add_layer_named(&em->fdata, CD_PROP_INT, CD_CALLOC, NULL, 0, "recastData");
334
335         //create verts and faces for detailed mesh
336         for (i=0; i<dmesh->nmeshes; i++)
337         {
338                 int uniquevbase = em->totvert;
339                 unsigned short vbase = dmesh->meshes[4*i+0];
340                 unsigned short ndv = dmesh->meshes[4*i+1];
341                 unsigned short tribase = dmesh->meshes[4*i+2];
342                 unsigned short trinum = dmesh->meshes[4*i+3];
343                 const unsigned short* p = &pmesh->polys[i*pmesh->nvp*2];
344                 int nv = 0;
345                 for (j = 0; j < pmesh->nvp; ++j)
346                 {
347                         if (p[j] == 0xffff) break;
348                         nv++;
349                 }
350                 //create unique verts 
351                 for (j=nv; j<ndv; j++)
352                 {
353                         copy_v3_v3(co, &dmesh->verts[3*(vbase + j)]);
354                         SWAP(float, co[1], co[2]);
355                         addvertlist(em, co, NULL);
356                 }
357
358                 EM_init_index_arrays(em, 1, 0, 0);
359                 
360                 //create faces
361                 for (j=0; j<trinum; j++)
362                 {
363                         unsigned char* tri = &dmesh->tris[4*(tribase+j)];
364                         EditFace* newFace;
365                         for (k=0; k<3; k++)
366                         {
367                                 if (tri[k]<nv)
368                                         face[k] = p[tri[k]]; //shared vertex
369                                 else
370                                         face[k] = uniquevbase+tri[k]-nv; //unique vertex
371                         }
372                         newFace = addfacelist(em, EM_get_vert_for_index(face[0]), EM_get_vert_for_index(face[2]), 
373                                                                         EM_get_vert_for_index(face[1]), NULL, NULL, NULL);
374
375                         //set navigation polygon idx to the custom layer
376                         int* polygonIdx = (int*)CustomData_em_get(&em->fdata, newFace->data, CD_PROP_INT);
377                         *polygonIdx = i+1; //add 1 to avoid zero idx
378                 }
379                 
380                 EM_free_index_arrays();
381         }
382
383         delete pmesh; pmesh = NULL;
384         delete dmesh; dmesh = NULL;
385
386         BKE_mesh_end_editmesh((Mesh*)obedit->data, em);
387         
388         DAG_id_flush_update((ID*)obedit->data, OB_RECALC_DATA);
389         WM_event_add_notifier(C, NC_GEOM|ND_DATA, obedit->data);
390
391
392         ED_object_exit_editmode(C, EM_FREEDATA); 
393         WM_event_add_notifier(C, NC_OBJECT|ND_DRAW, obedit);
394
395         obedit->gameflag &= ~OB_COLLISION;
396         obedit->gameflag |= OB_NAVMESH;
397         obedit->body_type = OB_BODY_TYPE_NAVMESH;
398         rename_id((ID *)obedit, "Navmesh");
399
400         ED_object_modifier_add(NULL, scene, obedit, NULL, eModifierType_NavMesh);
401         //ModifierData *md= modifiers_findByType(ob, eModifierType_NavMesh);
402         return obedit;
403 }
404
405 static int create_navmesh_exec(bContext *C, wmOperator *op)
406 {
407         Scene* scene = CTX_data_scene(C);
408         int nverts, ntris;
409         float* verts;
410         int* tris;
411         rcPolyMesh* pmesh;
412         rcPolyMeshDetail* dmesh;
413         LinkNode* obs = NULL;
414         CTX_DATA_BEGIN(C, Base*, base, selected_editable_bases)
415         {
416                 BLI_linklist_append(&obs, (void*)base->object);
417         }               
418         CTX_DATA_END;
419         createVertsTrisData(C, obs, nverts, verts, ntris, tris);
420         BLI_linklist_free(obs, NULL);
421         buildNavMesh(scene->gm.recastData, nverts, verts, ntris, tris, pmesh, dmesh);
422         createRepresentation(C, pmesh, dmesh);
423
424         return OPERATOR_FINISHED;
425 }
426
427 void OBJECT_OT_create_navmesh(wmOperatorType *ot)
428 {
429         /* identifiers */
430         ot->name= "Create navigation mesh";
431         ot->description= "Create navigation mesh for selected objects";
432         ot->idname= "OBJECT_OT_create_navmesh";
433
434         /* api callbacks */
435         ot->exec= create_navmesh_exec;
436
437         /* flags */
438         ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
439 }
440
441 static int assign_navpolygon_poll(bContext *C)
442 {
443         Object *ob= (Object *)CTX_data_pointer_get_type(C, "object", &RNA_Object).data;
444         if (!ob || !ob->data)
445                 return 0;
446         return (((Mesh*)ob->data)->edit_mesh != NULL);
447 }
448
449 static int assign_navpolygon_exec(bContext *C, wmOperator *op)
450 {
451         Object *obedit= CTX_data_edit_object(C);
452         EditMesh *em= BKE_mesh_get_editmesh((Mesh *)obedit->data);
453
454         //do work here
455         int targetPolyIdx = -1;
456         EditFace *ef, *efa;
457         efa = EM_get_actFace(em, 0);
458         if (efa) 
459         {
460                 if (CustomData_has_layer(&em->fdata, CD_PROP_INT))
461                 {
462                         targetPolyIdx = *(int*)CustomData_em_get(&em->fdata, efa->data, CD_PROP_INT);
463                         targetPolyIdx = targetPolyIdx>=0? targetPolyIdx : -targetPolyIdx;
464                         if (targetPolyIdx>0)
465                         {
466                                 //set target poly idx to other selected faces
467                                 ef = (EditFace*)em->faces.last;
468                                 while(ef) 
469                                 {
470                                         if((ef->f & SELECT )&& ef!=efa) 
471                                         {
472                                                 int* recastDataBlock = (int*)CustomData_em_get(&em->fdata, ef->data, CD_PROP_INT);
473                                                 *recastDataBlock = targetPolyIdx;
474                                         }
475                                         ef = ef->prev;
476                                 }
477                         }
478                 }               
479         }
480         
481         DAG_id_flush_update((ID*)obedit->data, OB_RECALC_DATA);
482         WM_event_add_notifier(C, NC_GEOM|ND_DATA, obedit->data);
483
484         BKE_mesh_end_editmesh((Mesh*)obedit->data, em);
485         return OPERATOR_FINISHED;
486 }
487
488 void OBJECT_OT_assign_navpolygon(struct wmOperatorType *ot)
489 {
490         /* identifiers */
491         ot->name= "Assign polygon index ";
492         ot->description= "Assign polygon index to face by active face";
493         ot->idname= "OBJECT_OT_assign_navpolygon";
494
495         /* api callbacks */
496         ot->poll = assign_navpolygon_poll;
497         ot->exec= assign_navpolygon_exec;
498
499         /* flags */
500         ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
501 }
502
503 static int compare(const void * a, const void * b){  
504         return ( *(int*)a - *(int*)b );
505 }
506 static int findFreeNavPolyIndex(EditMesh* em)
507 {
508         //construct vector of indices
509         int numfaces = em->totface;
510         int* indices = new int[numfaces];
511         EditFace* ef = (EditFace*)em->faces.last;
512         int idx = 0;
513         while(ef) 
514         {
515                 int polyIdx = *(int*)CustomData_em_get(&em->fdata, ef->data, CD_PROP_INT);
516                 indices[idx] = polyIdx;
517                 idx++;
518                 ef = ef->prev;
519         }
520         qsort(indices, numfaces, sizeof(int), compare);
521         //search first free index
522         int freeIdx = 1;
523         int maxIdx = indices[numfaces-1];
524         for (int i=0; i<numfaces; i++)
525         {
526                 if (indices[i]==freeIdx)
527                         freeIdx++;
528                 else if (indices[i]>freeIdx)
529                         break;
530         }
531         delete indices;
532         return freeIdx;
533 }
534
535 static int assign_new_navpolygon_exec(bContext *C, wmOperator *op)
536 {
537         Object *obedit= CTX_data_edit_object(C);
538         EditMesh *em= BKE_mesh_get_editmesh((Mesh *)obedit->data);
539
540         EditFace *ef;
541         if (CustomData_has_layer(&em->fdata, CD_PROP_INT))
542         {
543                 int targetPolyIdx = findFreeNavPolyIndex(em);
544                 if (targetPolyIdx>0)
545                 {
546                         //set target poly idx to selected faces
547                         ef = (EditFace*)em->faces.last;
548                         while(ef) 
549                         {
550                                 if(ef->f & SELECT ) 
551                                 {
552                                         int* recastDataBlock = (int*)CustomData_em_get(&em->fdata, ef->data, CD_PROP_INT);
553                                         *recastDataBlock = targetPolyIdx;
554                                 }
555                                 ef = ef->prev;
556                         }
557                 }
558         }               
559
560         DAG_id_flush_update((ID*)obedit->data, OB_RECALC_DATA);
561         WM_event_add_notifier(C, NC_GEOM|ND_DATA, obedit->data);
562
563         BKE_mesh_end_editmesh((Mesh*)obedit->data, em);
564         return OPERATOR_FINISHED;
565 }
566
567 void OBJECT_OT_assign_new_navpolygon(struct wmOperatorType *ot)
568 {
569         /* identifiers */
570         ot->name= "Assign new polygon index ";
571         ot->description= "Assign new polygon index to face";
572         ot->idname= "OBJECT_OT_assign_new_navpolygon";
573
574         /* api callbacks */
575         ot->poll = assign_navpolygon_poll;
576         ot->exec= assign_new_navpolygon_exec;
577
578         /* flags */
579         ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
580 }
581 }