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 "Recast.h" |
20 | #include "RecastAlloc.h" |
21 | #include "RecastAssert.h" |
22 | |
23 | #include <math.h> |
24 | #include <string.h> |
25 | #include <stdio.h> |
26 | #include <stdarg.h> |
27 | |
28 | namespace |
29 | { |
30 | /// Allocates and constructs an object of the given type, returning a pointer. |
31 | /// @param[in] allocLifetime Allocation lifetime hint |
32 | template<typename T> |
33 | T* rcNew(const rcAllocHint allocLifetime) |
34 | { |
35 | T* ptr = (T*)rcAlloc(sizeof(T), allocLifetime); |
36 | ::new(rcNewTag(), (void*)ptr) T(); |
37 | return ptr; |
38 | } |
39 | |
40 | /// Destroys and frees an object allocated with rcNew. |
41 | /// @param[in] ptr The object pointer to delete. |
42 | template<typename T> |
43 | void rcDelete(T* ptr) |
44 | { |
45 | if (ptr) |
46 | { |
47 | ptr->~T(); |
48 | rcFree((void*)ptr); |
49 | } |
50 | } |
51 | } // anonymous namespace |
52 | |
53 | float rcSqrt(float x) |
54 | { |
55 | return sqrtf(x); |
56 | } |
57 | |
58 | void rcContext::log(const rcLogCategory category, const char* format, ...) |
59 | { |
60 | if (!m_logEnabled) |
61 | { |
62 | return; |
63 | } |
64 | static const int MSG_SIZE = 512; |
65 | char msg[MSG_SIZE]; |
66 | va_list argList; |
67 | va_start(argList, format); |
68 | int len = vsnprintf(msg, MSG_SIZE, format, argList); |
69 | if (len >= MSG_SIZE) |
70 | { |
71 | len = MSG_SIZE - 1; |
72 | msg[MSG_SIZE - 1] = '\0'; |
73 | |
74 | const char* errorMessage = "Log message was truncated" ; |
75 | doLog(RC_LOG_ERROR, errorMessage, (int)strlen(errorMessage)); |
76 | } |
77 | va_end(argList); |
78 | doLog(category, msg, len); |
79 | } |
80 | |
81 | void rcContext::doResetLog() |
82 | { |
83 | // Defined out of line to fix the weak v-tables warning |
84 | } |
85 | |
86 | rcHeightfield* rcAllocHeightfield() |
87 | { |
88 | return rcNew<rcHeightfield>(RC_ALLOC_PERM); |
89 | } |
90 | |
91 | void rcFreeHeightField(rcHeightfield* heightfield) |
92 | { |
93 | rcDelete(heightfield); |
94 | } |
95 | |
96 | rcHeightfield::rcHeightfield() |
97 | : width() |
98 | , height() |
99 | , bmin() |
100 | , bmax() |
101 | , cs() |
102 | , ch() |
103 | , spans() |
104 | , pools() |
105 | , freelist() |
106 | { |
107 | } |
108 | |
109 | rcHeightfield::~rcHeightfield() |
110 | { |
111 | // Delete span array. |
112 | rcFree(spans); |
113 | // Delete span pools. |
114 | while (pools) |
115 | { |
116 | rcSpanPool* next = pools->next; |
117 | rcFree(pools); |
118 | pools = next; |
119 | } |
120 | } |
121 | |
122 | rcCompactHeightfield* rcAllocCompactHeightfield() |
123 | { |
124 | return rcNew<rcCompactHeightfield>(RC_ALLOC_PERM); |
125 | } |
126 | |
127 | void rcFreeCompactHeightfield(rcCompactHeightfield* compactHeightfield) |
128 | { |
129 | rcDelete(compactHeightfield); |
130 | } |
131 | |
132 | rcCompactHeightfield::rcCompactHeightfield() |
133 | : width() |
134 | , height() |
135 | , spanCount() |
136 | , walkableHeight() |
137 | , walkableClimb() |
138 | , borderSize() |
139 | , maxDistance() |
140 | , maxRegions() |
141 | , bmin() |
142 | , bmax() |
143 | , cs() |
144 | , ch() |
145 | , cells() |
146 | , spans() |
147 | , dist() |
148 | , areas() |
149 | { |
150 | } |
151 | |
152 | rcCompactHeightfield::~rcCompactHeightfield() |
153 | { |
154 | rcFree(cells); |
155 | rcFree(spans); |
156 | rcFree(dist); |
157 | rcFree(areas); |
158 | } |
159 | |
160 | rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet() |
161 | { |
162 | return rcNew<rcHeightfieldLayerSet>(RC_ALLOC_PERM); |
163 | } |
164 | |
165 | void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* layerSet) |
166 | { |
167 | rcDelete(layerSet); |
168 | } |
169 | |
170 | rcHeightfieldLayerSet::rcHeightfieldLayerSet() |
171 | : layers() |
172 | , nlayers() |
173 | { |
174 | } |
175 | |
176 | rcHeightfieldLayerSet::~rcHeightfieldLayerSet() |
177 | { |
178 | for (int i = 0; i < nlayers; ++i) |
179 | { |
180 | rcFree(layers[i].heights); |
181 | rcFree(layers[i].areas); |
182 | rcFree(layers[i].cons); |
183 | } |
184 | rcFree(layers); |
185 | } |
186 | |
187 | |
188 | rcContourSet* rcAllocContourSet() |
189 | { |
190 | return rcNew<rcContourSet>(RC_ALLOC_PERM); |
191 | } |
192 | |
193 | void rcFreeContourSet(rcContourSet* contourSet) |
194 | { |
195 | rcDelete(contourSet); |
196 | } |
197 | |
198 | rcContourSet::rcContourSet() |
199 | : conts() |
200 | , nconts() |
201 | , bmin() |
202 | , bmax() |
203 | , cs() |
204 | , ch() |
205 | , width() |
206 | , height() |
207 | , borderSize() |
208 | , maxError() |
209 | { |
210 | } |
211 | |
212 | rcContourSet::~rcContourSet() |
213 | { |
214 | for (int i = 0; i < nconts; ++i) |
215 | { |
216 | rcFree(conts[i].verts); |
217 | rcFree(conts[i].rverts); |
218 | } |
219 | rcFree(conts); |
220 | } |
221 | |
222 | rcPolyMesh* rcAllocPolyMesh() |
223 | { |
224 | return rcNew<rcPolyMesh>(RC_ALLOC_PERM); |
225 | } |
226 | |
227 | void rcFreePolyMesh(rcPolyMesh* polyMesh) |
228 | { |
229 | rcDelete(polyMesh); |
230 | } |
231 | |
232 | rcPolyMesh::rcPolyMesh() |
233 | : verts() |
234 | , polys() |
235 | , regs() |
236 | , flags() |
237 | , areas() |
238 | , nverts() |
239 | , npolys() |
240 | , maxpolys() |
241 | , nvp() |
242 | , bmin() |
243 | , bmax() |
244 | , cs() |
245 | , ch() |
246 | , borderSize() |
247 | , maxEdgeError() |
248 | { |
249 | } |
250 | |
251 | rcPolyMesh::~rcPolyMesh() |
252 | { |
253 | rcFree(verts); |
254 | rcFree(polys); |
255 | rcFree(regs); |
256 | rcFree(flags); |
257 | rcFree(areas); |
258 | } |
259 | |
260 | rcPolyMeshDetail* rcAllocPolyMeshDetail() |
261 | { |
262 | return rcNew<rcPolyMeshDetail>(RC_ALLOC_PERM); |
263 | } |
264 | |
265 | void rcFreePolyMeshDetail(rcPolyMeshDetail* detailMesh) |
266 | { |
267 | if (detailMesh == NULL) |
268 | { |
269 | return; |
270 | } |
271 | rcFree(detailMesh->meshes); |
272 | rcFree(detailMesh->verts); |
273 | rcFree(detailMesh->tris); |
274 | rcFree(detailMesh); |
275 | } |
276 | |
277 | rcPolyMeshDetail::rcPolyMeshDetail() |
278 | : meshes() |
279 | , verts() |
280 | , tris() |
281 | , nmeshes() |
282 | , nverts() |
283 | , ntris() |
284 | { |
285 | } |
286 | |
287 | void rcCalcBounds(const float* verts, int numVerts, float* minBounds, float* maxBounds) |
288 | { |
289 | // Calculate bounding box. |
290 | rcVcopy(minBounds, verts); |
291 | rcVcopy(maxBounds, verts); |
292 | for (int i = 1; i < numVerts; ++i) |
293 | { |
294 | const float* v = &verts[i * 3]; |
295 | rcVmin(minBounds, v); |
296 | rcVmax(maxBounds, v); |
297 | } |
298 | } |
299 | |
300 | void rcCalcGridSize(const float* minBounds, const float* maxBounds, const float cellSize, int* sizeX, int* sizeZ) |
301 | { |
302 | *sizeX = (int)((maxBounds[0] - minBounds[0]) / cellSize + 0.5f); |
303 | *sizeZ = (int)((maxBounds[2] - minBounds[2]) / cellSize + 0.5f); |
304 | } |
305 | |
306 | bool rcCreateHeightfield(rcContext* context, rcHeightfield& heightfield, int sizeX, int sizeZ, |
307 | const float* minBounds, const float* maxBounds, |
308 | float cellSize, float cellHeight) |
309 | { |
310 | rcIgnoreUnused(context); |
311 | |
312 | heightfield.width = sizeX; |
313 | heightfield.height = sizeZ; |
314 | rcVcopy(heightfield.bmin, minBounds); |
315 | rcVcopy(heightfield.bmax, maxBounds); |
316 | heightfield.cs = cellSize; |
317 | heightfield.ch = cellHeight; |
318 | heightfield.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*) * heightfield.width * heightfield.height, RC_ALLOC_PERM); |
319 | if (!heightfield.spans) |
320 | { |
321 | return false; |
322 | } |
323 | memset(heightfield.spans, 0, sizeof(rcSpan*) * heightfield.width * heightfield.height); |
324 | return true; |
325 | } |
326 | |
327 | static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* faceNormal) |
328 | { |
329 | float e0[3], e1[3]; |
330 | rcVsub(e0, v1, v0); |
331 | rcVsub(e1, v2, v0); |
332 | rcVcross(faceNormal, e0, e1); |
333 | rcVnormalize(faceNormal); |
334 | } |
335 | |
336 | void rcMarkWalkableTriangles(rcContext* context, const float walkableSlopeAngle, |
337 | const float* verts, const int numVerts, |
338 | const int* tris, const int numTris, |
339 | unsigned char* triAreaIDs) |
340 | { |
341 | rcIgnoreUnused(context); |
342 | rcIgnoreUnused(numVerts); |
343 | |
344 | const float walkableThr = cosf(walkableSlopeAngle / 180.0f * RC_PI); |
345 | |
346 | float norm[3]; |
347 | |
348 | for (int i = 0; i < numTris; ++i) |
349 | { |
350 | const int* tri = &tris[i * 3]; |
351 | calcTriNormal(&verts[tri[0] * 3], &verts[tri[1] * 3], &verts[tri[2] * 3], norm); |
352 | // Check if the face is walkable. |
353 | if (norm[1] > walkableThr) |
354 | { |
355 | triAreaIDs[i] = RC_WALKABLE_AREA; |
356 | } |
357 | } |
358 | } |
359 | |
360 | void rcClearUnwalkableTriangles(rcContext* context, const float walkableSlopeAngle, |
361 | const float* verts, int numVerts, |
362 | const int* tris, int numTris, |
363 | unsigned char* triAreaIDs) |
364 | { |
365 | rcIgnoreUnused(context); |
366 | rcIgnoreUnused(numVerts); |
367 | |
368 | // The minimum Y value for a face normal of a triangle with a walkable slope. |
369 | const float walkableLimitY = cosf(walkableSlopeAngle / 180.0f * RC_PI); |
370 | |
371 | float faceNormal[3]; |
372 | for (int i = 0; i < numTris; ++i) |
373 | { |
374 | const int* tri = &tris[i * 3]; |
375 | calcTriNormal(&verts[tri[0] * 3], &verts[tri[1] * 3], &verts[tri[2] * 3], faceNormal); |
376 | // Check if the face is walkable. |
377 | if (faceNormal[1] <= walkableLimitY) |
378 | { |
379 | triAreaIDs[i] = RC_NULL_AREA; |
380 | } |
381 | } |
382 | } |
383 | |
384 | int rcGetHeightFieldSpanCount(rcContext* context, const rcHeightfield& heightfield) |
385 | { |
386 | rcIgnoreUnused(context); |
387 | |
388 | const int numCols = heightfield.width * heightfield.height; |
389 | int spanCount = 0; |
390 | for (int columnIndex = 0; columnIndex < numCols; ++columnIndex) |
391 | { |
392 | for (rcSpan* span = heightfield.spans[columnIndex]; span != NULL; span = span->next) |
393 | { |
394 | if (span->area != RC_NULL_AREA) |
395 | { |
396 | spanCount++; |
397 | } |
398 | } |
399 | } |
400 | return spanCount; |
401 | } |
402 | |
403 | bool rcBuildCompactHeightfield(rcContext* context, const int walkableHeight, const int walkableClimb, |
404 | const rcHeightfield& heightfield, rcCompactHeightfield& compactHeightfield) |
405 | { |
406 | rcAssert(context); |
407 | |
408 | rcScopedTimer timer(context, RC_TIMER_BUILD_COMPACTHEIGHTFIELD); |
409 | |
410 | const int xSize = heightfield.width; |
411 | const int zSize = heightfield.height; |
412 | const int spanCount = rcGetHeightFieldSpanCount(context, heightfield); |
413 | |
414 | // Fill in header. |
415 | compactHeightfield.width = xSize; |
416 | compactHeightfield.height = zSize; |
417 | compactHeightfield.spanCount = spanCount; |
418 | compactHeightfield.walkableHeight = walkableHeight; |
419 | compactHeightfield.walkableClimb = walkableClimb; |
420 | compactHeightfield.maxRegions = 0; |
421 | rcVcopy(compactHeightfield.bmin, heightfield.bmin); |
422 | rcVcopy(compactHeightfield.bmax, heightfield.bmax); |
423 | compactHeightfield.bmax[1] += walkableHeight * heightfield.ch; |
424 | compactHeightfield.cs = heightfield.cs; |
425 | compactHeightfield.ch = heightfield.ch; |
426 | compactHeightfield.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell) * xSize * zSize, RC_ALLOC_PERM); |
427 | if (!compactHeightfield.cells) |
428 | { |
429 | context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)" , xSize * zSize); |
430 | return false; |
431 | } |
432 | memset(compactHeightfield.cells, 0, sizeof(rcCompactCell) * xSize * zSize); |
433 | compactHeightfield.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan) * spanCount, RC_ALLOC_PERM); |
434 | if (!compactHeightfield.spans) |
435 | { |
436 | context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)" , spanCount); |
437 | return false; |
438 | } |
439 | memset(compactHeightfield.spans, 0, sizeof(rcCompactSpan) * spanCount); |
440 | compactHeightfield.areas = (unsigned char*)rcAlloc(sizeof(unsigned char) * spanCount, RC_ALLOC_PERM); |
441 | if (!compactHeightfield.areas) |
442 | { |
443 | context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)" , spanCount); |
444 | return false; |
445 | } |
446 | memset(compactHeightfield.areas, RC_NULL_AREA, sizeof(unsigned char) * spanCount); |
447 | |
448 | const int MAX_HEIGHT = 0xffff; |
449 | |
450 | // Fill in cells and spans. |
451 | int currentCellIndex = 0; |
452 | const int numColumns = xSize * zSize; |
453 | for (int columnIndex = 0; columnIndex < numColumns; ++columnIndex) |
454 | { |
455 | const rcSpan* span = heightfield.spans[columnIndex]; |
456 | |
457 | // If there are no spans at this cell, just leave the data to index=0, count=0. |
458 | if (span == NULL) |
459 | { |
460 | continue; |
461 | } |
462 | |
463 | rcCompactCell& cell = compactHeightfield.cells[columnIndex]; |
464 | cell.index = currentCellIndex; |
465 | cell.count = 0; |
466 | |
467 | for (; span != NULL; span = span->next) |
468 | { |
469 | if (span->area != RC_NULL_AREA) |
470 | { |
471 | const int bot = (int)span->smax; |
472 | const int top = span->next ? (int)span->next->smin : MAX_HEIGHT; |
473 | compactHeightfield.spans[currentCellIndex].y = (unsigned short)rcClamp(bot, 0, 0xffff); |
474 | compactHeightfield.spans[currentCellIndex].h = (unsigned char)rcClamp(top - bot, 0, 0xff); |
475 | compactHeightfield.areas[currentCellIndex] = span->area; |
476 | currentCellIndex++; |
477 | cell.count++; |
478 | } |
479 | } |
480 | } |
481 | |
482 | // Find neighbour connections. |
483 | const int MAX_LAYERS = RC_NOT_CONNECTED - 1; |
484 | int maxLayerIndex = 0; |
485 | const int zStride = xSize; // for readability |
486 | for (int z = 0; z < zSize; ++z) |
487 | { |
488 | for (int x = 0; x < xSize; ++x) |
489 | { |
490 | const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride]; |
491 | for (int i = (int)cell.index, ni = (int)(cell.index + cell.count); i < ni; ++i) |
492 | { |
493 | rcCompactSpan& span = compactHeightfield.spans[i]; |
494 | |
495 | for (int dir = 0; dir < 4; ++dir) |
496 | { |
497 | rcSetCon(span, dir, RC_NOT_CONNECTED); |
498 | const int neighborX = x + rcGetDirOffsetX(dir); |
499 | const int neighborZ = z + rcGetDirOffsetY(dir); |
500 | // First check that the neighbour cell is in bounds. |
501 | if (neighborX < 0 || neighborZ < 0 || neighborX >= xSize || neighborZ >= zSize) |
502 | { |
503 | continue; |
504 | } |
505 | |
506 | // Iterate over all neighbour spans and check if any of the is |
507 | // accessible from current cell. |
508 | const rcCompactCell& neighborCell = compactHeightfield.cells[neighborX + neighborZ * zStride]; |
509 | for (int k = (int)neighborCell.index, nk = (int)(neighborCell.index + neighborCell.count); k < nk; ++k) |
510 | { |
511 | const rcCompactSpan& neighborSpan = compactHeightfield.spans[k]; |
512 | const int bot = rcMax(span.y, neighborSpan.y); |
513 | const int top = rcMin(span.y + span.h, neighborSpan.y + neighborSpan.h); |
514 | |
515 | // Check that the gap between the spans is walkable, |
516 | // and that the climb height between the gaps is not too high. |
517 | if ((top - bot) >= walkableHeight && rcAbs((int)neighborSpan.y - (int)span.y) <= walkableClimb) |
518 | { |
519 | // Mark direction as walkable. |
520 | const int layerIndex = k - (int)neighborCell.index; |
521 | if (layerIndex < 0 || layerIndex > MAX_LAYERS) |
522 | { |
523 | maxLayerIndex = rcMax(maxLayerIndex, layerIndex); |
524 | continue; |
525 | } |
526 | rcSetCon(span, dir, layerIndex); |
527 | break; |
528 | } |
529 | } |
530 | } |
531 | } |
532 | } |
533 | } |
534 | |
535 | if (maxLayerIndex > MAX_LAYERS) |
536 | { |
537 | context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)" , |
538 | maxLayerIndex, MAX_LAYERS); |
539 | } |
540 | |
541 | return true; |
542 | } |
543 | |