Fix selection when adding a primitive while already in edge/face-select mode
[blender-staging.git] / extern / recastnavigation / Recast / Source / Recast.cpp
1 //
2 // Copyright (c) 2009 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 "Recast.h"
26 #include "RecastLog.h"
27 #include "RecastTimer.h"
28
29
30 void rcIntArray::resize(int n)
31 {
32         if (n > m_cap)
33         {
34                 if (!m_cap) m_cap = 8;
35                 while (m_cap < n) m_cap *= 2;
36                 int* newData = new int[m_cap];
37                 if (m_size && newData) memcpy(newData, m_data, m_size*sizeof(int));
38                 delete [] m_data;
39                 m_data = newData;
40         }
41         m_size = n;
42 }
43
44 void rcCalcBounds(const float* verts, int nv, float* bmin, float* bmax)
45 {
46         // Calculate bounding box.
47         vcopy(bmin, verts);
48         vcopy(bmax, verts);
49         for (int i = 1; i < nv; ++i)
50         {
51                 const float* v = &verts[i*3];
52                 vmin(bmin, v);
53                 vmax(bmax, v);
54         }
55 }
56
57 void rcCalcGridSize(const float* bmin, const float* bmax, float cs, int* w, int* h)
58 {
59         *w = (int)((bmax[0] - bmin[0])/cs+0.5f);
60         *h = (int)((bmax[2] - bmin[2])/cs+0.5f);
61 }
62
63 bool rcCreateHeightfield(rcHeightfield& hf, int width, int height,
64                                                  const float* bmin, const float* bmax,
65                                                  float cs, float ch)
66 {
67         hf.width = width;
68         hf.height = height;
69         hf.spans = new rcSpan*[hf.width*hf.height];
70         vcopy(hf.bmin, bmin);
71         vcopy(hf.bmax, bmax);
72         hf.cs = cs;
73         hf.ch = ch;
74         if (!hf.spans)
75                 return false;
76         memset(hf.spans, 0, sizeof(rcSpan*)*hf.width*hf.height);
77         return true;
78 }
79
80 static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm)
81 {
82         float e0[3], e1[3];
83         vsub(e0, v1, v0);
84         vsub(e1, v2, v0);
85         vcross(norm, e0, e1);
86         vnormalize(norm);
87 }
88
89 void rcMarkWalkableTriangles(const float walkableSlopeAngle,
90                                                          const float* verts, int nv,
91                                                          const int* tris, int nt,
92                                                          unsigned char* flags)
93 {
94         const float walkableThr = cosf(walkableSlopeAngle/180.0f*(float)M_PI);
95
96         float norm[3];
97         
98         for (int i = 0; i < nt; ++i)
99         {
100                 const int* tri = &tris[i*3];
101                 calcTriNormal(&verts[tri[0]*3], &verts[tri[1]*3], &verts[tri[2]*3], norm);
102                 // Check if the face is walkable.
103                 if (norm[1] > walkableThr)
104                         flags[i] |= RC_WALKABLE;
105         }
106 }
107
108 static int getSpanCount(unsigned char flags, rcHeightfield& hf)
109 {
110         const int w = hf.width;
111         const int h = hf.height;
112         int spanCount = 0;
113         for (int y = 0; y < h; ++y)
114         {
115                 for (int x = 0; x < w; ++x)
116                 {
117                         for (rcSpan* s = hf.spans[x + y*w]; s; s = s->next)
118                         {
119                                 if (s->flags == flags)
120                                         spanCount++;
121                         }
122                 }
123         }
124         return spanCount;
125 }
126
127 inline void setCon(rcCompactSpan& s, int dir, int i)
128 {
129         s.con &= ~(0xf << (dir*4));
130         s.con |= (i&0xf) << (dir*4);
131 }
132
133 bool rcBuildCompactHeightfield(const int walkableHeight, const int walkableClimb,
134                                                            unsigned char flags, rcHeightfield& hf,
135                                                            rcCompactHeightfield& chf)
136 {
137         rcTimeVal startTime = rcGetPerformanceTimer();
138         
139         const int w = hf.width;
140         const int h = hf.height;
141         const int spanCount = getSpanCount(flags, hf);
142
143         // Fill in header.
144         chf.width = w;
145         chf.height = h;
146         chf.spanCount = spanCount;
147         chf.walkableHeight = walkableHeight;
148         chf.walkableClimb = walkableClimb;
149         chf.maxRegions = 0;
150         vcopy(chf.bmin, hf.bmin);
151         vcopy(chf.bmax, hf.bmax);
152         chf.bmax[1] += walkableHeight*hf.ch;
153         chf.cs = hf.cs;
154         chf.ch = hf.ch;
155         chf.cells = new rcCompactCell[w*h];
156         if (!chf.cells)
157         {
158                 if (rcGetLog())
159                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", w*h);
160                 return false;
161         }
162         memset(chf.cells, 0, sizeof(rcCompactCell)*w*h);
163         chf.spans = new rcCompactSpan[spanCount];
164         if (!chf.spans)
165         {
166                 if (rcGetLog())
167                         rcGetLog()->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount);
168                 return false;
169         }
170         memset(chf.spans, 0, sizeof(rcCompactSpan)*spanCount);
171         
172         const int MAX_HEIGHT = 0xffff;
173         
174         // Fill in cells and spans.
175         int idx = 0;
176         for (int y = 0; y < h; ++y)
177         {
178                 for (int x = 0; x < w; ++x)
179                 {
180                         const rcSpan* s = hf.spans[x + y*w];
181                         // If there are no spans at this cell, just leave the data to index=0, count=0.
182                         if (!s) continue;
183                         rcCompactCell& c = chf.cells[x+y*w];
184                         c.index = idx;
185                         c.count = 0;
186                         while (s)
187                         {
188                                 if (s->flags == flags)
189                                 {
190                                         const int bot = (int)s->smax;
191                                         const int top = s->next ? (int)s->next->smin : MAX_HEIGHT;
192                                         chf.spans[idx].y = (unsigned short)rcClamp(bot, 0, 0xffff);
193                                         chf.spans[idx].h = (unsigned char)rcClamp(top - bot, 0, 0xff);
194                                         idx++;
195                                         c.count++;
196                                 }
197                                 s = s->next;
198                         }
199                 }
200         }
201
202         // Find neighbour connections.
203         for (int y = 0; y < h; ++y)
204         {
205                 for (int x = 0; x < w; ++x)
206                 {
207                         const rcCompactCell& c = chf.cells[x+y*w];
208                         for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
209                         {
210                                 rcCompactSpan& s = chf.spans[i];
211                                 for (int dir = 0; dir < 4; ++dir)
212                                 {
213                                         setCon(s, dir, 0xf);
214                                         const int nx = x + rcGetDirOffsetX(dir);
215                                         const int ny = y + rcGetDirOffsetY(dir);
216                                         // First check that the neighbour cell is in bounds.
217                                         if (nx < 0 || ny < 0 || nx >= w || ny >= h)
218                                                 continue;
219                                         // Iterate over all neighbour spans and check if any of the is
220                                         // accessible from current cell.
221                                         const rcCompactCell& nc = chf.cells[nx+ny*w];
222                                         for (int k = (int)nc.index, nk = (int)(nc.index+nc.count); k < nk; ++k)
223                                         {
224                                                 const rcCompactSpan& ns = chf.spans[k];
225                                                 const int bot = rcMax(s.y, ns.y);
226                                                 const int top = rcMin(s.y+s.h, ns.y+ns.h);
227
228                                                 // Check that the gap between the spans is walkable,
229                                                 // and that the climb height between the gaps is not too high.
230                                                 if ((top - bot) >= walkableHeight && rcAbs((int)ns.y - (int)s.y) <= walkableClimb)
231                                                 {
232                                                         // Mark direction as walkable.
233                                                         setCon(s, dir, k - (int)nc.index);
234                                                         break;
235                                                 }
236                                         }
237                                 }
238                         }
239                 }
240         }
241         
242         rcTimeVal endTime = rcGetPerformanceTimer();
243         
244         if (rcGetBuildTimes())
245                 rcGetBuildTimes()->buildCompact += rcGetDeltaTimeUsec(startTime, endTime);
246         
247         return true;
248 }
249
250 static int getHeightfieldMemoryUsage(const rcHeightfield& hf)
251 {
252         int size = 0;
253         size += sizeof(hf);
254         size += hf.width * hf.height * sizeof(rcSpan*);
255         
256         rcSpanPool* pool = hf.pools;
257         while (pool)
258         {
259                 size += (sizeof(rcSpanPool) - sizeof(rcSpan)) + sizeof(rcSpan)*RC_SPANS_PER_POOL;
260                 pool = pool->next;
261         }
262         return size;
263 }
264
265 static int getCompactHeightFieldMemoryusage(const rcCompactHeightfield& chf)
266 {
267         int size = 0;
268         size += sizeof(rcCompactHeightfield);
269         size += sizeof(rcCompactSpan) * chf.spanCount;
270         size += sizeof(rcCompactCell) * chf.width * chf.height;
271         return size;
272 }