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