svn merge ^/trunk/blender -r40644:40720
[blender-staging.git] / extern / recastnavigation / Recast / Source / Recast.cpp
1 //
2 // Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
3 //
4 // This software is provided 'as-is', without any express or implied
5 // warranty.  In no event will the authors be held liable for any damages
6 // arising from the use of this software.
7 // Permission is granted to anyone to use this software for any purpose,
8 // including commercial applications, and to alter it and redistribute it
9 // freely, subject to the following restrictions:
10 // 1. The origin of this software must not be misrepresented; you must not
11 //    claim that you wrote the original software. If you use this software
12 //    in a product, an acknowledgment in the product documentation would be
13 //    appreciated but is not required.
14 // 2. Altered source versions must be plainly marked as such, and must not be
15 //    misrepresented as being the original software.
16 // 3. This notice may not be removed or altered from any source distribution.
17 //
18
19 #include <float.h>
20 #define _USE_MATH_DEFINES
21 #include <math.h>
22 #include <string.h>
23 #include <stdlib.h>
24 #include <stdio.h>
25 #include <stdarg.h>
26 #include "Recast.h"
27 #include "RecastAlloc.h"
28 #include "RecastAssert.h"
29
30 float rcSqrt(float x)
31 {
32         return sqrtf(x);
33 }
34
35 /// @class rcContext
36 /// @par
37 ///
38 /// This class does not provide logging or timer functionality on its 
39 /// own.  Both must be provided by a concrete implementation 
40 /// by overriding the protected member functions.  Also, this class does not 
41 /// provide an interface for extracting log messages. (Only adding them.) 
42 /// So concrete implementations must provide one.
43 ///
44 /// If no logging or timers are required, just pass an instance of this 
45 /// class through the Recast build process.
46 ///
47
48 /// @par
49 ///
50 /// Example:
51 /// @code
52 /// // Where ctx is an instance of rcContext and filepath is a char array.
53 /// ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath);
54 /// @endcode
55 void rcContext::log(const rcLogCategory category, const char* format, ...)
56 {
57         if (!m_logEnabled)
58                 return;
59         static const int MSG_SIZE = 512;
60         char msg[MSG_SIZE];
61         va_list ap;
62         va_start(ap, format);
63         int len = vsnprintf(msg, MSG_SIZE, format, ap);
64         if (len >= MSG_SIZE)
65         {
66                 len = MSG_SIZE-1;
67                 msg[MSG_SIZE-1] = '\0';
68         }
69         va_end(ap);
70         doLog(category, msg, len);
71 }
72
73 rcHeightfield* rcAllocHeightfield()
74 {
75         rcHeightfield* hf = (rcHeightfield*)rcAlloc(sizeof(rcHeightfield), RC_ALLOC_PERM);
76         memset(hf, 0, sizeof(rcHeightfield));
77         return hf;
78 }
79
80 void rcFreeHeightField(rcHeightfield* hf)
81 {
82         if (!hf) return;
83         // Delete span array.
84         rcFree(hf->spans);
85         // Delete span pools.
86         while (hf->pools)
87         {
88                 rcSpanPool* next = hf->pools->next;
89                 rcFree(hf->pools);
90                 hf->pools = next;
91         }
92         rcFree(hf);
93 }
94
95 rcCompactHeightfield* rcAllocCompactHeightfield()
96 {
97         rcCompactHeightfield* chf = (rcCompactHeightfield*)rcAlloc(sizeof(rcCompactHeightfield), RC_ALLOC_PERM);
98         memset(chf, 0, sizeof(rcCompactHeightfield));
99         return chf;
100 }
101
102 void rcFreeCompactHeightfield(rcCompactHeightfield* chf)
103 {
104         if (!chf) return;
105         rcFree(chf->cells);
106         rcFree(chf->spans);
107         rcFree(chf->dist);
108         rcFree(chf->areas);
109         rcFree(chf);
110 }
111
112
113 rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet()
114 {
115         rcHeightfieldLayerSet* lset = (rcHeightfieldLayerSet*)rcAlloc(sizeof(rcHeightfieldLayerSet), RC_ALLOC_PERM);
116         memset(lset, 0, sizeof(rcHeightfieldLayerSet));
117         return lset;
118 }
119
120 void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* lset)
121 {
122         if (!lset) return;
123         for (int i = 0; i < lset->nlayers; ++i)
124         {
125                 rcFree(lset->layers[i].heights);
126                 rcFree(lset->layers[i].areas);
127                 rcFree(lset->layers[i].cons);
128         }
129         rcFree(lset->layers);
130         rcFree(lset);
131 }
132
133
134 rcContourSet* rcAllocContourSet()
135 {
136         rcContourSet* cset = (rcContourSet*)rcAlloc(sizeof(rcContourSet), RC_ALLOC_PERM);
137         memset(cset, 0, sizeof(rcContourSet));
138         return cset;
139 }
140
141 void rcFreeContourSet(rcContourSet* cset)
142 {
143         if (!cset) return;
144         for (int i = 0; i < cset->nconts; ++i)
145         {
146                 rcFree(cset->conts[i].verts);
147                 rcFree(cset->conts[i].rverts);
148         }
149         rcFree(cset->conts);
150         rcFree(cset);
151 }
152
153 rcPolyMesh* rcAllocPolyMesh()
154 {
155         rcPolyMesh* pmesh = (rcPolyMesh*)rcAlloc(sizeof(rcPolyMesh), RC_ALLOC_PERM);
156         memset(pmesh, 0, sizeof(rcPolyMesh));
157         return pmesh;
158 }
159
160 void rcFreePolyMesh(rcPolyMesh* pmesh)
161 {
162         if (!pmesh) return;
163         rcFree(pmesh->verts);
164         rcFree(pmesh->polys);
165         rcFree(pmesh->regs);
166         rcFree(pmesh->flags);
167         rcFree(pmesh->areas);
168         rcFree(pmesh);
169 }
170
171 rcPolyMeshDetail* rcAllocPolyMeshDetail()
172 {
173         rcPolyMeshDetail* dmesh = (rcPolyMeshDetail*)rcAlloc(sizeof(rcPolyMeshDetail), RC_ALLOC_PERM);
174         memset(dmesh, 0, sizeof(rcPolyMeshDetail));
175         return dmesh;
176 }
177
178 void rcFreePolyMeshDetail(rcPolyMeshDetail* dmesh)
179 {
180         if (!dmesh) return;
181         rcFree(dmesh->meshes);
182         rcFree(dmesh->verts);
183         rcFree(dmesh->tris);
184         rcFree(dmesh);
185 }
186
187 void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax)
188 {
189         // Calculate bounding box.
190         rcVcopy(bmin, verts);
191         rcVcopy(bmax, verts);
192         for (int i = 1; i < nv; ++i)
193         {
194                 const float* v = &verts[i*3];
195                 rcVmin(bmin, v);
196                 rcVmax(bmax, v);
197         }
198 }
199
200 void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h)
201 {
202         *w = (int)((bmax[0] - bmin[0])/cs+0.5f);
203         *h = (int)((bmax[2] - bmin[2])/cs+0.5f);
204 }
205
206 /// @par
207 ///
208 /// See the #rcConfig documentation for more information on the configuration parameters.
209 /// 
210 /// @see rcAllocHeightfield, rcHeightfield 
211 bool rcCreateHeightfield(rcContext* /*ctx*/, rcHeightfield& hf, int width, int height,
212                                                  const float* bmin, const float* bmax,
213                                                  float cs, float ch)
214 {
215         // TODO: VC complains about unref formal variable, figure out a way to handle this better.
216 //      rcAssert(ctx);
217         
218         hf.width = width;
219         hf.height = height;
220         rcVcopy(hf.bmin, bmin);
221         rcVcopy(hf.bmax, bmax);
222         hf.cs = cs;
223         hf.ch = ch;
224         hf.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*)*hf.width*hf.height, RC_ALLOC_PERM);
225         if (!hf.spans)
226                 return false;
227         memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height);
228         return true;
229 }
230
231 static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm)
232 {
233         float e0[3], e1[3];
234         rcVsub(e0, v1, v0);
235         rcVsub(e1, v2, v0);
236         rcVcross(norm, e0, e1);
237         rcVnormalize(norm);
238 }
239
240 /// @par
241 ///
242 /// Only sets the aread id's for the walkable triangles.  Does not alter the
243 /// area id's for unwalkable triangles.
244 /// 
245 /// See the #rcConfig documentation for more information on the configuration parameters.
246 /// 
247 /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
248 void rcMarkWalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle,
249                                                          const float* verts, int /*nv*/,
250                                                          const int* tris, int nt,
251                                                          unsigned char* areas)
252 {
253         // TODO: VC complains about unref formal variable, figure out a way to handle this better.
254 //      rcAssert(ctx);
255         
256         const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
257
258         float norm[3];
259         
260         for (int i = 0; i < nt; ++i)
261         {
262                 const int* tri = &tris[i*3];
263                 calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
264                 // Check if the face is walkable.
265                 if (norm[1] > walkableThr)
266                         areas[i] = RC_WALKABLE_AREA;
267         }
268 }
269
270 /// @par
271 ///
272 /// Only sets the aread id's for the unwalkable triangles.  Does not alter the
273 /// area id's for walkable triangles.
274 /// 
275 /// See the #rcConfig documentation for more information on the configuration parameters.
276 /// 
277 /// @see rcHeightfield, rcClearUnwalkableTriangles, rcRasterizeTriangles
278 void rcClearUnwalkableTriangles(rcContext* /*ctx*/, const float walkableSlopeAngle,
279                                                                 const float* verts, int /*nv*/,
280                                                                 const int* tris, int nt,
281                                                                 unsigned char* areas)
282 {
283         // TODO: VC complains about unref formal variable, figure out a way to handle this better.
284 //      rcAssert(ctx);
285         
286         const float walkableThr = cosf(walkableSlopeAngle/180.0f*RC_PI);
287         
288         float norm[3];
289         
290         for (int i = 0; i < nt; ++i)
291         {
292                 const int* tri = &tris[i*3];
293                 calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
294                 // Check if the face is walkable.
295                 if (norm[1] <= walkableThr)
296                         areas[i] = RC_NULL_AREA;
297         }
298 }
299
300 int rcGetHeightFieldSpanCount(rcContext* /*ctx*/, rcHeightfield& hf)
301 {
302         // TODO: VC complains about unref formal variable, figure out a way to handle this better.
303 //      rcAssert(ctx);
304         
305         const int w = hf.width;
306         const int h = hf.height;
307         int spanCount = 0;
308         for (int y = 0; y < h; ++y)
309         {
310                 for (int x = 0; x < w; ++x)
311                 {
312                         for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next)
313                         {
314                                 if (s->area != RC_NULL_AREA)
315                                         spanCount++;
316                         }
317                 }
318         }
319         return spanCount;
320 }
321
322 /// @par
323 ///
324 /// This is just the beginning of the process of fully building a compact heightfield.
325 /// Various filters may be applied applied, then the distance field and regions built.
326 /// E.g: #rcBuildDistanceField and #rcBuildRegions
327 ///
328 /// See the #rcConfig documentation for more information on the configuration parameters.
329 ///
330 /// @see rcAllocCompactHeightfield, rcHeightfield, rcCompactHeightfield, rcConfig
331 bool rcBuildCompactHeightfield(rcContext* ctx, const int walkableHeight, const int walkableClimb,
332                                                            rcHeightfield& hf, rcCompactHeightfield& chf)
333 {
334         rcAssert(ctx);
335         
336         ctx->startTimer(RC_TIMER_BUILD_COMPACTHEIGHTFIELD);
337         
338         const int w = hf.width;
339         const int h = hf.height;
340         const int spanCount = rcGetHeightFieldSpanCount(ctx, hf);
341
342         // Fill in header.
343         chf.width = w;
344         chf.height = h;
345         chf.spanCount = spanCount;
346         chf.walkableHeight = walkableHeight;
347         chf.walkableClimb = walkableClimb;
348         chf.maxRegions = 0;
349         rcVcopy(chf.bmin, hf.bmin);
350         rcVcopy(chf.bmax, hf.bmax);
351         chf.bmax[1] += walkableHeight*hf.ch;
352         chf.cs = hf.cs;
353         chf.ch = hf.ch;
354         chf.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell)*w*h, RC_ALLOC_PERM);
355         if (!chf.cells)
356         {
357                 ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h);
358                 return false;
359         }
360         memset(chf.cells, 0, sizeof(rcCompactCell)*w*h);
361         chf.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan)*spanCount, RC_ALLOC_PERM);
362         if (!chf.spans)
363         {
364                 ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount);
365                 return false;
366         }
367         memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount);
368         chf.areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*spanCount, RC_ALLOC_PERM);
369         if (!chf.areas)
370         {
371                 ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount);
372                 return false;
373         }
374         memset(chf.areas, RC_NULL_AREA, sizeof(unsigned char)*spanCount);
375         
376         const int MAX_HEIGHT = 0xffff;
377         
378         // Fill in cells and spans.
379         int idx = 0;
380         for (int y = 0; y < h; ++y)
381         {
382                 for (int x = 0; x < w; ++x)
383                 {
384                         const rcSpan* s = hf.spans[x + y*w];
385                         // If there are no spans at this cell, just leave the data to index=0, count=0.
386                         if (!s) continue;
387                         rcCompactCell& c = chf.cells[x+y*w];
388                         c.index = idx;
389                         c.count = 0;
390                         while (s)
391                         {
392                                 if (s->area != RC_NULL_AREA)
393                                 {
394                                         const int bot = (int)s->smax;
395                                         const int top = s->next ? (int)s->next->smin : MAX_HEIGHT;
396                                         chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff);
397                                         chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff);
398                                         chf.areas[idx] = s->area;
399                                         idx++;
400                                         c.count++;
401                                 }
402                                 s = s->next;
403                         }
404                 }
405         }
406
407         // Find neighbour connections.
408         const int MAX_LAYERS = RC_NOT_CONNECTED-1;
409         int tooHighNeighbour = 0;
410         for (int y = 0; y < h; ++y)
411         {
412                 for (int x = 0; x < w; ++x)
413                 {
414                         const rcCompactCell& c = chf.cells[x+y*w];
415                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
416                         {
417                                 rcCompactSpan& s = chf.spans[i];
418                                 
419                                 for (int dir = 0; dir < 4; ++dir)
420                                 {
421                                         rcSetCon(s, dir, RC_NOT_CONNECTED);
422                                         const int nx = x + rcGetDirOffsetX(dir);
423                                         const int ny = y + rcGetDirOffsetY(dir);
424                                         // First check that the neighbour cell is in bounds.
425                                         if (nx < 0 || ny < 0 || nx >= w || ny >= h)
426                                                 continue;
427                                                 
428                                         // Iterate over all neighbour spans and check if any of the is
429                                         // accessible from current cell.
430                                         const rcCompactCell& nc = chf.cells[nx+ny*w];
431                                         for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k)
432                                         {
433                                                 const rcCompactSpan& ns = chf.spans[k];
434                                                 const int bot = rcMax(s.y, ns.y);
435                                                 const int top = rcMin(s.y+s.h, ns.y+ns.h);
436
437                                                 // Check that the gap between the spans is walkable,
438                                                 // and that the climb height between the gaps is not too high.
439                                                 if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb)
440                                                 {
441                                                         // Mark direction as walkable.
442                                                         const int idx = k - (int)nc.index;
443                                                         if (idx < 0 || idx > MAX_LAYERS)
444                                                         {
445                                                                 tooHighNeighbour = rcMax(tooHighNeighbour, idx);
446                                                                 continue;
447                                                         }
448                                                         rcSetCon(s, dir, idx);
449                                                         break;
450                                                 }
451                                         }
452                                         
453                                 }
454                         }
455                 }
456         }
457         
458         if (tooHighNeighbour > MAX_LAYERS)
459         {
460                 ctx->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)",
461                                  tooHighNeighbour, MAX_LAYERS);
462         }
463                 
464         ctx->stopTimer(RC_TIMER_BUILD_COMPACTHEIGHTFIELD);
465         
466         return true;
467 }
468
469 /*
470 static int getHeightfieldMemoryUsage(const rcHeightfield& hf)
471 {
472         int size = 0;
473         size += sizeof(hf);
474         size += hf.width * hf.height * sizeof(rcSpan*);
475         
476         rcSpanPool* pool = hf.pools;
477         while (pool)
478         {
479                 size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL;
480                 pool = pool->next;
481         }
482         return size;
483 }
484
485 static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf)
486 {
487         int size = 0;
488         size += sizeof(rcCompactHeightfield);
489         size += sizeof(rcCompactSpan) * chf.spanCount;
490         size += sizeof(rcCompactCell) * chf.width * chf.height;
491         return size;
492 }
493 */