svn merge ^/trunk/blender -r40644:40720
[blender-staging.git] / extern / recastnavigation / Recast / Source / RecastRasterization.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 #define _USE_MATH_DEFINES
20 #include <math.h>
21 #include <stdio.h>
22 #include "Recast.h"
23 #include "RecastAlloc.h"
24 #include "RecastAssert.h"
25
26 inline bool overlapBounds(const float* amin, const float* amax, const float* bmin, const float* bmax)
27 {
28         bool overlap = true;
29         overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
30         overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
31         overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
32         return overlap;
33 }
34
35 inline bool overlapInterval(unsigned short amin, unsigned short amax,
36                                                         unsigned short bmin, unsigned short bmax)
37 {
38         if (amax < bmin) return false;
39         if (amin > bmax) return false;
40         return true;
41 }
42
43
44 static rcSpan* allocSpan(rcHeightfield& hf)
45 {
46         // If running out of memory, allocate new page and update the freelist.
47         if (!hf.freelist || !hf.freelist->next)
48         {
49                 // Create new page.
50                 // Allocate memory for the new pool.
51                 rcSpanPool* pool = (rcSpanPool*)rcAlloc(sizeof(rcSpanPool), RC_ALLOC_PERM);
52                 if (!pool) return 0;
53                 pool->next = 0;
54                 // Add the pool into the list of pools.
55                 pool->next = hf.pools;
56                 hf.pools = pool;
57                 // Add new items to the free list.
58                 rcSpan* freelist = hf.freelist;
59                 rcSpan* head = &pool->items[0];
60                 rcSpan* it = &pool->items[RC_SPANS_PER_POOL];
61                 do
62                 {
63                         --it;
64                         it->next = freelist;
65                         freelist = it;
66                 }
67                 while (it != head);
68                 hf.freelist = it;
69         }
70         
71         // Pop item from in front of the free list.
72         rcSpan* it = hf.freelist;
73         hf.freelist = hf.freelist->next;
74         return it;
75 }
76
77 static void freeSpan(rcHeightfield& hf, rcSpan* ptr)
78 {
79         if (!ptr) return;
80         // Add the node in front of the free list.
81         ptr->next = hf.freelist;
82         hf.freelist = ptr;
83 }
84
85 static void addSpan(rcHeightfield& hf, const int x, const int y,
86                                         const unsigned short smin, const unsigned short smax,
87                                         const unsigned char area, const int flagMergeThr)
88 {
89         
90         int idx = x + y*hf.width;
91         
92         rcSpan* s = allocSpan(hf);
93         s->smin = smin;
94         s->smax = smax;
95         s->area = area;
96         s->next = 0;
97         
98         // Empty cell, add he first span.
99         if (!hf.spans[idx])
100         {
101                 hf.spans[idx] = s;
102                 return;
103         }
104         rcSpan* prev = 0;
105         rcSpan* cur = hf.spans[idx];
106         
107         // Insert and merge spans.
108         while (cur)
109         {
110                 if (cur->smin > s->smax)
111                 {
112                         // Current span is further than the new span, break.
113                         break;
114                 }
115                 else if (cur->smax < s->smin)
116                 {
117                         // Current span is before the new span advance.
118                         prev = cur;
119                         cur = cur->next;
120                 }
121                 else
122                 {
123                         // Merge spans.
124                         if (cur->smin < s->smin)
125                                 s->smin = cur->smin;
126                         if (cur->smax > s->smax)
127                                 s->smax = cur->smax;
128                         
129                         // Merge flags.
130                         if (rcAbs((int)s->smax - (int)cur->smax) <= flagMergeThr)
131                                 s->area = rcMax(s->area, cur->area);
132                         
133                         // Remove current span.
134                         rcSpan* next = cur->next;
135                         freeSpan(hf, cur);
136                         if (prev)
137                                 prev->next = next;
138                         else
139                                 hf.spans[idx] = next;
140                         cur = next;
141                 }
142         }
143         
144         // Insert new span.
145         if (prev)
146         {
147                 s->next = prev->next;
148                 prev->next = s;
149         }
150         else
151         {
152                 s->next = hf.spans[idx];
153                 hf.spans[idx] = s;
154         }
155 }
156
157 /// @par
158 ///
159 /// The span addition can be set to favor flags. If the span is merged to
160 /// another span and the new @p smax is within @p flagMergeThr units
161 /// from the existing span, the span flags are merged.
162 ///
163 /// @see rcHeightfield, rcSpan.
164 void rcAddSpan(rcContext* /*ctx*/, rcHeightfield& hf, const int x, const int y,
165                            const unsigned short smin, const unsigned short smax,
166                            const unsigned char area, const int flagMergeThr)
167 {
168 //      rcAssert(ctx);
169         addSpan(hf, x,y, smin, smax, area, flagMergeThr);
170 }
171
172 static int clipPoly(const float* in, int n, float* out, float pnx, float pnz, float pd)
173 {
174         float d[12];
175         for (int i = 0; i < n; ++i)
176                 d[i] = pnx*in[i*3+0] + pnz*in[i*3+2] + pd;
177         
178         int m = 0;
179         for (int i = 0, j = n-1; i < n; j=i, ++i)
180         {
181                 bool ina = d[j] >= 0;
182                 bool inb = d[i] >= 0;
183                 if (ina != inb)
184                 {
185                         float s = d[j] / (d[j] - d[i]);
186                         out[m*3+0] = in[j*3+0] + (in[i*3+0] - in[j*3+0])*s;
187                         out[m*3+1] = in[j*3+1] + (in[i*3+1] - in[j*3+1])*s;
188                         out[m*3+2] = in[j*3+2] + (in[i*3+2] - in[j*3+2])*s;
189                         m++;
190                 }
191                 if (inb)
192                 {
193                         out[m*3+0] = in[i*3+0];
194                         out[m*3+1] = in[i*3+1];
195                         out[m*3+2] = in[i*3+2];
196                         m++;
197                 }
198         }
199         return m;
200 }
201
202 static void rasterizeTri(const float* v0, const float* v1, const float* v2,
203                                                  const unsigned char area, rcHeightfield& hf,
204                                                  const float* bmin, const float* bmax,
205                                                  const float cs, const float ics, const float ich,
206                                                  const int flagMergeThr)
207 {
208         const int w = hf.width;
209         const int h = hf.height;
210         float tmin[3], tmax[3];
211         const float by = bmax[1] - bmin[1];
212         
213         // Calculate the bounding box of the triangle.
214         rcVcopy(tmin, v0);
215         rcVcopy(tmax, v0);
216         rcVmin(tmin, v1);
217         rcVmin(tmin, v2);
218         rcVmax(tmax, v1);
219         rcVmax(tmax, v2);
220         
221         // If the triangle does not touch the bbox of the heightfield, skip the triagle.
222         if (!overlapBounds(bmin, bmax, tmin, tmax))
223                 return;
224         
225         // Calculate the footpring of the triangle on the grid.
226         int x0 = (int)((tmin[0] - bmin[0])*ics);
227         int y0 = (int)((tmin[2] - bmin[2])*ics);
228         int x1 = (int)((tmax[0] - bmin[0])*ics);
229         int y1 = (int)((tmax[2] - bmin[2])*ics);
230         x0 = rcClamp(x0, 0, w-1);
231         y0 = rcClamp(y0, 0, h-1);
232         x1 = rcClamp(x1, 0, w-1);
233         y1 = rcClamp(y1, 0, h-1);
234         
235         // Clip the triangle into all grid cells it touches.
236         float in[7*3], out[7*3], inrow[7*3];
237         
238         for (int y = y0; y <= y1; ++y)
239         {
240                 // Clip polygon to row.
241                 rcVcopy(&in[0], v0);
242                 rcVcopy(&in[1*3], v1);
243                 rcVcopy(&in[2*3], v2);
244                 int nvrow = 3;
245                 const float cz = bmin[2] + y*cs;
246                 nvrow = clipPoly(in, nvrow, out, 0, 1, -cz);
247                 if (nvrow < 3) continue;
248                 nvrow = clipPoly(out, nvrow, inrow, 0, -1, cz+cs);
249                 if (nvrow < 3) continue;
250                 
251                 for (int x = x0; x <= x1; ++x)
252                 {
253                         // Clip polygon to column.
254                         int nv = nvrow;
255                         const float cx = bmin[0] + x*cs;
256                         nv = clipPoly(inrow, nv, out, 1, 0, -cx);
257                         if (nv < 3) continue;
258                         nv = clipPoly(out, nv, in, -1, 0, cx+cs);
259                         if (nv < 3) continue;
260                         
261                         // Calculate min and max of the span.
262                         float smin = in[1], smax = in[1];
263                         for (int i = 1; i < nv; ++i)
264                         {
265                                 smin = rcMin(smin, in[i*3+1]);
266                                 smax = rcMax(smax, in[i*3+1]);
267                         }
268                         smin -= bmin[1];
269                         smax -= bmin[1];
270                         // Skip the span if it is outside the heightfield bbox
271                         if (smax < 0.0f) continue;
272                         if (smin > by) continue;
273                         // Clamp the span to the heightfield bbox.
274                         if (smin < 0.0f) smin = 0;
275                         if (smax > by) smax = by;
276                         
277                         // Snap the span to the heightfield height grid.
278                         unsigned short ismin = (unsigned short)rcClamp((int)floorf(smin * ich), 0, RC_SPAN_MAX_HEIGHT);
279                         unsigned short ismax = (unsigned short)rcClamp((int)ceilf(smax * ich), (int)ismin+1, RC_SPAN_MAX_HEIGHT);
280                         
281                         addSpan(hf, x, y, ismin, ismax, area, flagMergeThr);
282                 }
283         }
284 }
285
286 /// @par
287 ///
288 /// No spans will be added if the triangle does not overlap the heightfield grid.
289 ///
290 /// @see rcHeightfield
291 void rcRasterizeTriangle(rcContext* ctx, const float* v0, const float* v1, const float* v2,
292                                                  const unsigned char area, rcHeightfield& solid,
293                                                  const int flagMergeThr)
294 {
295         rcAssert(ctx);
296
297         ctx->startTimer(RC_TIMER_RASTERIZE_TRIANGLES);
298
299         const float ics = 1.0f/solid.cs;
300         const float ich = 1.0f/solid.ch;
301         rasterizeTri(v0, v1, v2, area, solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr);
302
303         ctx->stopTimer(RC_TIMER_RASTERIZE_TRIANGLES);
304 }
305
306 /// @par
307 ///
308 /// Spans will only be added for triangles that overlap the heightfield grid.
309 ///
310 /// @see rcHeightfield
311 void rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
312                                                   const int* tris, const unsigned char* areas, const int nt,
313                                                   rcHeightfield& solid, const int flagMergeThr)
314 {
315         rcAssert(ctx);
316
317         ctx->startTimer(RC_TIMER_RASTERIZE_TRIANGLES);
318         
319         const float ics = 1.0f/solid.cs;
320         const float ich = 1.0f/solid.ch;
321         // Rasterize triangles.
322         for (int i = 0; i < nt; ++i)
323         {
324                 const float* v0 = &verts[tris[i*3+0]*3];
325                 const float* v1 = &verts[tris[i*3+1]*3];
326                 const float* v2 = &verts[tris[i*3+2]*3];
327                 // Rasterize.
328                 rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr);
329         }
330         
331         ctx->stopTimer(RC_TIMER_RASTERIZE_TRIANGLES);
332 }
333
334 /// @par
335 ///
336 /// Spans will only be added for triangles that overlap the heightfield grid.
337 ///
338 /// @see rcHeightfield
339 void rcRasterizeTriangles(rcContext* ctx, const float* verts, const int /*nv*/,
340                                                   const unsigned short* tris, const unsigned char* areas, const int nt,
341                                                   rcHeightfield& solid, const int flagMergeThr)
342 {
343         rcAssert(ctx);
344
345         ctx->startTimer(RC_TIMER_RASTERIZE_TRIANGLES);
346         
347         const float ics = 1.0f/solid.cs;
348         const float ich = 1.0f/solid.ch;
349         // Rasterize triangles.
350         for (int i = 0; i < nt; ++i)
351         {
352                 const float* v0 = &verts[tris[i*3+0]*3];
353                 const float* v1 = &verts[tris[i*3+1]*3];
354                 const float* v2 = &verts[tris[i*3+2]*3];
355                 // Rasterize.
356                 rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr);
357         }
358         
359         ctx->stopTimer(RC_TIMER_RASTERIZE_TRIANGLES);
360 }
361
362 /// @par
363 ///
364 /// Spans will only be added for triangles that overlap the heightfield grid.
365 ///
366 /// @see rcHeightfield
367 void rcRasterizeTriangles(rcContext* ctx, const float* verts, const unsigned char* areas, const int nt,
368                                                   rcHeightfield& solid, const int flagMergeThr)
369 {
370         rcAssert(ctx);
371         
372         ctx->startTimer(RC_TIMER_RASTERIZE_TRIANGLES);
373         
374         const float ics = 1.0f/solid.cs;
375         const float ich = 1.0f/solid.ch;
376         // Rasterize triangles.
377         for (int i = 0; i < nt; ++i)
378         {
379                 const float* v0 = &verts[(i*3+0)*3];
380                 const float* v1 = &verts[(i*3+1)*3];
381                 const float* v2 = &verts[(i*3+2)*3];
382                 // Rasterize.
383                 rasterizeTri(v0, v1, v2, areas[i], solid, solid.bmin, solid.bmax, solid.cs, ics, ich, flagMergeThr);
384         }
385         
386         ctx->stopTimer(RC_TIMER_RASTERIZE_TRIANGLES);
387 }