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 <ctype.h>
23 #include <string.h>
24 #include <algorithm>
25 #include "Recast.h"
26 #include "InputGeom.h"
27 #include "ChunkyTriMesh.h"
28 #include "MeshLoaderObj.h"
29 #include "DebugDraw.h"
30 #include "RecastDebugDraw.h"
31 #include "DetourNavMesh.h"
32 #include "Sample.h"
33
intersectSegmentTriangle(const float * sp,const float * sq,const float * a,const float * b,const float * c,float & t)34 static bool intersectSegmentTriangle(const float* sp, const float* sq,
35 const float* a, const float* b, const float* c,
36 float &t)
37 {
38 float v, w;
39 float ab[3], ac[3], qp[3], ap[3], norm[3], e[3];
40 rcVsub(ab, b, a);
41 rcVsub(ac, c, a);
42 rcVsub(qp, sp, sq);
43
44 // Compute triangle normal. Can be precalculated or cached if
45 // intersecting multiple segments against the same triangle
46 rcVcross(norm, ab, ac);
47
48 // Compute denominator d. If d <= 0, segment is parallel to or points
49 // away from triangle, so exit early
50 float d = rcVdot(qp, norm);
51 if (d <= 0.0f) return false;
52
53 // Compute intersection t value of pq with plane of triangle. A ray
54 // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay
55 // dividing by d until intersection has been found to pierce triangle
56 rcVsub(ap, sp, a);
57 t = rcVdot(ap, norm);
58 if (t < 0.0f) return false;
59 if (t > d) return false; // For segment; exclude this code line for a ray test
60
61 // Compute barycentric coordinate components and test if within bounds
62 rcVcross(e, qp, ap);
63 v = rcVdot(ac, e);
64 if (v < 0.0f || v > d) return false;
65 w = -rcVdot(ab, e);
66 if (w < 0.0f || v + w > d) return false;
67
68 // Segment/ray intersects triangle. Perform delayed division
69 t /= d;
70
71 return true;
72 }
73
parseRow(char * buf,char * bufEnd,char * row,int len)74 static char* parseRow(char* buf, char* bufEnd, char* row, int len)
75 {
76 bool start = true;
77 bool done = false;
78 int n = 0;
79 while (!done && buf < bufEnd)
80 {
81 char c = *buf;
82 buf++;
83 // multirow
84 switch (c)
85 {
86 case '\n':
87 if (start) break;
88 done = true;
89 break;
90 case '\r':
91 break;
92 case '\t':
93 case ' ':
94 if (start) break;
95 // else falls through
96 default:
97 start = false;
98 row[n++] = c;
99 if (n >= len-1)
100 done = true;
101 break;
102 }
103 }
104 row[n] = '\0';
105 return buf;
106 }
107
108
109
InputGeom()110 InputGeom::InputGeom() :
111 m_chunkyMesh(0),
112 m_mesh(0),
113 m_hasBuildSettings(false),
114 m_offMeshConCount(0),
115 m_volumeCount(0)
116 {
117 }
118
~InputGeom()119 InputGeom::~InputGeom()
120 {
121 delete m_chunkyMesh;
122 delete m_mesh;
123 }
124
loadMesh(rcContext * ctx,const std::string & filepath)125 bool InputGeom::loadMesh(rcContext* ctx, const std::string& filepath)
126 {
127 if (m_mesh)
128 {
129 delete m_chunkyMesh;
130 m_chunkyMesh = 0;
131 delete m_mesh;
132 m_mesh = 0;
133 }
134 m_offMeshConCount = 0;
135 m_volumeCount = 0;
136
137 m_mesh = new rcMeshLoaderObj;
138 if (!m_mesh)
139 {
140 ctx->log(RC_LOG_ERROR, "loadMesh: Out of memory 'm_mesh'.");
141 return false;
142 }
143 if (!m_mesh->load(filepath))
144 {
145 ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Could not load '%s'", filepath.c_str());
146 return false;
147 }
148
149 rcCalcBounds(m_mesh->getVerts(), m_mesh->getVertCount(), m_meshBMin, m_meshBMax);
150
151 m_chunkyMesh = new rcChunkyTriMesh;
152 if (!m_chunkyMesh)
153 {
154 ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Out of memory 'm_chunkyMesh'.");
155 return false;
156 }
157 if (!rcCreateChunkyTriMesh(m_mesh->getVerts(), m_mesh->getTris(), m_mesh->getTriCount(), 256, m_chunkyMesh))
158 {
159 ctx->log(RC_LOG_ERROR, "buildTiledNavigation: Failed to build chunky mesh.");
160 return false;
161 }
162
163 return true;
164 }
165
loadGeomSet(rcContext * ctx,const std::string & filepath)166 bool InputGeom::loadGeomSet(rcContext* ctx, const std::string& filepath)
167 {
168 char* buf = 0;
169 FILE* fp = fopen(filepath.c_str(), "rb");
170 if (!fp)
171 {
172 return false;
173 }
174 if (fseek(fp, 0, SEEK_END) != 0)
175 {
176 fclose(fp);
177 return false;
178 }
179
180 long bufSize = ftell(fp);
181 if (bufSize < 0)
182 {
183 fclose(fp);
184 return false;
185 }
186 if (fseek(fp, 0, SEEK_SET) != 0)
187 {
188 fclose(fp);
189 return false;
190 }
191 buf = new char[bufSize];
192 if (!buf)
193 {
194 fclose(fp);
195 return false;
196 }
197 size_t readLen = fread(buf, bufSize, 1, fp);
198 fclose(fp);
199 if (readLen != 1)
200 {
201 delete[] buf;
202 return false;
203 }
204
205 m_offMeshConCount = 0;
206 m_volumeCount = 0;
207 delete m_mesh;
208 m_mesh = 0;
209
210 char* src = buf;
211 char* srcEnd = buf + bufSize;
212 char row[512];
213 while (src < srcEnd)
214 {
215 // Parse one row
216 row[0] = '\0';
217 src = parseRow(src, srcEnd, row, sizeof(row)/sizeof(char));
218 if (row[0] == 'f')
219 {
220 // File name.
221 const char* name = row+1;
222 // Skip white spaces
223 while (*name && isspace(*name))
224 name++;
225 if (*name)
226 {
227 if (!loadMesh(ctx, name))
228 {
229 delete [] buf;
230 return false;
231 }
232 }
233 }
234 else if (row[0] == 'c')
235 {
236 // Off-mesh connection
237 if (m_offMeshConCount < MAX_OFFMESH_CONNECTIONS)
238 {
239 float* v = &m_offMeshConVerts[m_offMeshConCount*3*2];
240 int bidir, area = 0, flags = 0;
241 float rad;
242 sscanf(row+1, "%f %f %f %f %f %f %f %d %d %d",
243 &v[0], &v[1], &v[2], &v[3], &v[4], &v[5], &rad, &bidir, &area, &flags);
244 m_offMeshConRads[m_offMeshConCount] = rad;
245 m_offMeshConDirs[m_offMeshConCount] = (unsigned char)bidir;
246 m_offMeshConAreas[m_offMeshConCount] = (unsigned char)area;
247 m_offMeshConFlags[m_offMeshConCount] = (unsigned short)flags;
248 m_offMeshConCount++;
249 }
250 }
251 else if (row[0] == 'v')
252 {
253 // Convex volumes
254 if (m_volumeCount < MAX_VOLUMES)
255 {
256 ConvexVolume* vol = &m_volumes[m_volumeCount++];
257 sscanf(row+1, "%d %d %f %f", &vol->nverts, &vol->area, &vol->hmin, &vol->hmax);
258 for (int i = 0; i < vol->nverts; ++i)
259 {
260 row[0] = '\0';
261 src = parseRow(src, srcEnd, row, sizeof(row)/sizeof(char));
262 sscanf(row, "%f %f %f", &vol->verts[i*3+0], &vol->verts[i*3+1], &vol->verts[i*3+2]);
263 }
264 }
265 }
266 else if (row[0] == 's')
267 {
268 // Settings
269 m_hasBuildSettings = true;
270 sscanf(row + 1, "%f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f",
271 &m_buildSettings.cellSize,
272 &m_buildSettings.cellHeight,
273 &m_buildSettings.agentHeight,
274 &m_buildSettings.agentRadius,
275 &m_buildSettings.agentMaxClimb,
276 &m_buildSettings.agentMaxSlope,
277 &m_buildSettings.regionMinSize,
278 &m_buildSettings.regionMergeSize,
279 &m_buildSettings.edgeMaxLen,
280 &m_buildSettings.edgeMaxError,
281 &m_buildSettings.vertsPerPoly,
282 &m_buildSettings.detailSampleDist,
283 &m_buildSettings.detailSampleMaxError,
284 &m_buildSettings.partitionType,
285 &m_buildSettings.navMeshBMin[0],
286 &m_buildSettings.navMeshBMin[1],
287 &m_buildSettings.navMeshBMin[2],
288 &m_buildSettings.navMeshBMax[0],
289 &m_buildSettings.navMeshBMax[1],
290 &m_buildSettings.navMeshBMax[2],
291 &m_buildSettings.tileSize);
292 }
293 }
294
295 delete [] buf;
296
297 return true;
298 }
299
load(rcContext * ctx,const std::string & filepath)300 bool InputGeom::load(rcContext* ctx, const std::string& filepath)
301 {
302 size_t extensionPos = filepath.find_last_of('.');
303 if (extensionPos == std::string::npos)
304 return false;
305
306 std::string extension = filepath.substr(extensionPos);
307 std::transform(extension.begin(), extension.end(), extension.begin(), tolower);
308
309 if (extension == ".gset")
310 return loadGeomSet(ctx, filepath);
311 if (extension == ".obj")
312 return loadMesh(ctx, filepath);
313
314 return false;
315 }
316
saveGeomSet(const BuildSettings * settings)317 bool InputGeom::saveGeomSet(const BuildSettings* settings)
318 {
319 if (!m_mesh) return false;
320
321 // Change extension
322 std::string filepath = m_mesh->getFileName();
323 size_t extPos = filepath.find_last_of('.');
324 if (extPos != std::string::npos)
325 filepath = filepath.substr(0, extPos);
326
327 filepath += ".gset";
328
329 FILE* fp = fopen(filepath.c_str(), "w");
330 if (!fp) return false;
331
332 // Store mesh filename.
333 fprintf(fp, "f %s\n", m_mesh->getFileName().c_str());
334
335 // Store settings if any
336 if (settings)
337 {
338 fprintf(fp,
339 "s %f %f %f %f %f %f %f %f %f %f %f %f %f %d %f %f %f %f %f %f %f\n",
340 settings->cellSize,
341 settings->cellHeight,
342 settings->agentHeight,
343 settings->agentRadius,
344 settings->agentMaxClimb,
345 settings->agentMaxSlope,
346 settings->regionMinSize,
347 settings->regionMergeSize,
348 settings->edgeMaxLen,
349 settings->edgeMaxError,
350 settings->vertsPerPoly,
351 settings->detailSampleDist,
352 settings->detailSampleMaxError,
353 settings->partitionType,
354 settings->navMeshBMin[0],
355 settings->navMeshBMin[1],
356 settings->navMeshBMin[2],
357 settings->navMeshBMax[0],
358 settings->navMeshBMax[1],
359 settings->navMeshBMax[2],
360 settings->tileSize);
361 }
362
363 // Store off-mesh links.
364 for (int i = 0; i < m_offMeshConCount; ++i)
365 {
366 const float* v = &m_offMeshConVerts[i*3*2];
367 const float rad = m_offMeshConRads[i];
368 const int bidir = m_offMeshConDirs[i];
369 const int area = m_offMeshConAreas[i];
370 const int flags = m_offMeshConFlags[i];
371 fprintf(fp, "c %f %f %f %f %f %f %f %d %d %d\n",
372 v[0], v[1], v[2], v[3], v[4], v[5], rad, bidir, area, flags);
373 }
374
375 // Convex volumes
376 for (int i = 0; i < m_volumeCount; ++i)
377 {
378 ConvexVolume* vol = &m_volumes[i];
379 fprintf(fp, "v %d %d %f %f\n", vol->nverts, vol->area, vol->hmin, vol->hmax);
380 for (int j = 0; j < vol->nverts; ++j)
381 fprintf(fp, "%f %f %f\n", vol->verts[j*3+0], vol->verts[j*3+1], vol->verts[j*3+2]);
382 }
383
384 fclose(fp);
385
386 return true;
387 }
388
isectSegAABB(const float * sp,const float * sq,const float * amin,const float * amax,float & tmin,float & tmax)389 static bool isectSegAABB(const float* sp, const float* sq,
390 const float* amin, const float* amax,
391 float& tmin, float& tmax)
392 {
393 static const float EPS = 1e-6f;
394
395 float d[3];
396 d[0] = sq[0] - sp[0];
397 d[1] = sq[1] - sp[1];
398 d[2] = sq[2] - sp[2];
399 tmin = 0.0;
400 tmax = 1.0f;
401
402 for (int i = 0; i < 3; i++)
403 {
404 if (fabsf(d[i]) < EPS)
405 {
406 if (sp[i] < amin[i] || sp[i] > amax[i])
407 return false;
408 }
409 else
410 {
411 const float ood = 1.0f / d[i];
412 float t1 = (amin[i] - sp[i]) * ood;
413 float t2 = (amax[i] - sp[i]) * ood;
414 if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; }
415 if (t1 > tmin) tmin = t1;
416 if (t2 < tmax) tmax = t2;
417 if (tmin > tmax) return false;
418 }
419 }
420
421 return true;
422 }
423
424
raycastMesh(float * src,float * dst,float & tmin)425 bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
426 {
427 // Prune hit ray.
428 float btmin, btmax;
429 if (!isectSegAABB(src, dst, m_meshBMin, m_meshBMax, btmin, btmax))
430 return false;
431 float p[2], q[2];
432 p[0] = src[0] + (dst[0]-src[0])*btmin;
433 p[1] = src[2] + (dst[2]-src[2])*btmin;
434 q[0] = src[0] + (dst[0]-src[0])*btmax;
435 q[1] = src[2] + (dst[2]-src[2])*btmax;
436
437 int cid[512];
438 const int ncid = rcGetChunksOverlappingSegment(m_chunkyMesh, p, q, cid, 512);
439 if (!ncid)
440 return false;
441
442 tmin = 1.0f;
443 bool hit = false;
444 const float* verts = m_mesh->getVerts();
445
446 for (int i = 0; i < ncid; ++i)
447 {
448 const rcChunkyTriMeshNode& node = m_chunkyMesh->nodes[cid[i]];
449 const int* tris = &m_chunkyMesh->tris[node.i*3];
450 const int ntris = node.n;
451
452 for (int j = 0; j < ntris*3; j += 3)
453 {
454 float t = 1;
455 if (intersectSegmentTriangle(src, dst,
456 &verts[tris[j]*3],
457 &verts[tris[j+1]*3],
458 &verts[tris[j+2]*3], t))
459 {
460 if (t < tmin)
461 tmin = t;
462 hit = true;
463 }
464 }
465 }
466
467 return hit;
468 }
469
addOffMeshConnection(const float * spos,const float * epos,const float rad,unsigned char bidir,unsigned char area,unsigned short flags)470 void InputGeom::addOffMeshConnection(const float* spos, const float* epos, const float rad,
471 unsigned char bidir, unsigned char area, unsigned short flags)
472 {
473 if (m_offMeshConCount >= MAX_OFFMESH_CONNECTIONS) return;
474 float* v = &m_offMeshConVerts[m_offMeshConCount*3*2];
475 m_offMeshConRads[m_offMeshConCount] = rad;
476 m_offMeshConDirs[m_offMeshConCount] = bidir;
477 m_offMeshConAreas[m_offMeshConCount] = area;
478 m_offMeshConFlags[m_offMeshConCount] = flags;
479 m_offMeshConId[m_offMeshConCount] = 1000 + m_offMeshConCount;
480 rcVcopy(&v[0], spos);
481 rcVcopy(&v[3], epos);
482 m_offMeshConCount++;
483 }
484
deleteOffMeshConnection(int i)485 void InputGeom::deleteOffMeshConnection(int i)
486 {
487 m_offMeshConCount--;
488 float* src = &m_offMeshConVerts[m_offMeshConCount*3*2];
489 float* dst = &m_offMeshConVerts[i*3*2];
490 rcVcopy(&dst[0], &src[0]);
491 rcVcopy(&dst[3], &src[3]);
492 m_offMeshConRads[i] = m_offMeshConRads[m_offMeshConCount];
493 m_offMeshConDirs[i] = m_offMeshConDirs[m_offMeshConCount];
494 m_offMeshConAreas[i] = m_offMeshConAreas[m_offMeshConCount];
495 m_offMeshConFlags[i] = m_offMeshConFlags[m_offMeshConCount];
496 }
497
drawOffMeshConnections(duDebugDraw * dd,bool hilight)498 void InputGeom::drawOffMeshConnections(duDebugDraw* dd, bool hilight)
499 {
500 unsigned int conColor = duRGBA(192,0,128,192);
501 unsigned int baseColor = duRGBA(0,0,0,64);
502 dd->depthMask(false);
503
504 dd->begin(DU_DRAW_LINES, 2.0f);
505 for (int i = 0; i < m_offMeshConCount; ++i)
506 {
507 float* v = &m_offMeshConVerts[i*3*2];
508
509 dd->vertex(v[0],v[1],v[2], baseColor);
510 dd->vertex(v[0],v[1]+0.2f,v[2], baseColor);
511
512 dd->vertex(v[3],v[4],v[5], baseColor);
513 dd->vertex(v[3],v[4]+0.2f,v[5], baseColor);
514
515 duAppendCircle(dd, v[0],v[1]+0.1f,v[2], m_offMeshConRads[i], baseColor);
516 duAppendCircle(dd, v[3],v[4]+0.1f,v[5], m_offMeshConRads[i], baseColor);
517
518 if (hilight)
519 {
520 duAppendArc(dd, v[0],v[1],v[2], v[3],v[4],v[5], 0.25f,
521 (m_offMeshConDirs[i]&1) ? 0.6f : 0.0f, 0.6f, conColor);
522 }
523 }
524 dd->end();
525
526 dd->depthMask(true);
527 }
528
addConvexVolume(const float * verts,const int nverts,const float minh,const float maxh,unsigned char area)529 void InputGeom::addConvexVolume(const float* verts, const int nverts,
530 const float minh, const float maxh, unsigned char area)
531 {
532 if (m_volumeCount >= MAX_VOLUMES) return;
533 ConvexVolume* vol = &m_volumes[m_volumeCount++];
534 memset(vol, 0, sizeof(ConvexVolume));
535 memcpy(vol->verts, verts, sizeof(float)*3*nverts);
536 vol->hmin = minh;
537 vol->hmax = maxh;
538 vol->nverts = nverts;
539 vol->area = area;
540 }
541
deleteConvexVolume(int i)542 void InputGeom::deleteConvexVolume(int i)
543 {
544 m_volumeCount--;
545 m_volumes[i] = m_volumes[m_volumeCount];
546 }
547
drawConvexVolumes(struct duDebugDraw * dd,bool)548 void InputGeom::drawConvexVolumes(struct duDebugDraw* dd, bool /*hilight*/)
549 {
550 dd->depthMask(false);
551
552 dd->begin(DU_DRAW_TRIS);
553
554 for (int i = 0; i < m_volumeCount; ++i)
555 {
556 const ConvexVolume* vol = &m_volumes[i];
557 unsigned int col = duTransCol(dd->areaToCol(vol->area), 32);
558 for (int j = 0, k = vol->nverts-1; j < vol->nverts; k = j++)
559 {
560 const float* va = &vol->verts[k*3];
561 const float* vb = &vol->verts[j*3];
562
563 dd->vertex(vol->verts[0],vol->hmax,vol->verts[2], col);
564 dd->vertex(vb[0],vol->hmax,vb[2], col);
565 dd->vertex(va[0],vol->hmax,va[2], col);
566
567 dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
568 dd->vertex(va[0],vol->hmax,va[2], col);
569 dd->vertex(vb[0],vol->hmax,vb[2], col);
570
571 dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
572 dd->vertex(vb[0],vol->hmax,vb[2], col);
573 dd->vertex(vb[0],vol->hmin,vb[2], duDarkenCol(col));
574 }
575 }
576
577 dd->end();
578
579 dd->begin(DU_DRAW_LINES, 2.0f);
580 for (int i = 0; i < m_volumeCount; ++i)
581 {
582 const ConvexVolume* vol = &m_volumes[i];
583 unsigned int col = duTransCol(dd->areaToCol(vol->area), 220);
584 for (int j = 0, k = vol->nverts-1; j < vol->nverts; k = j++)
585 {
586 const float* va = &vol->verts[k*3];
587 const float* vb = &vol->verts[j*3];
588 dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
589 dd->vertex(vb[0],vol->hmin,vb[2], duDarkenCol(col));
590 dd->vertex(va[0],vol->hmax,va[2], col);
591 dd->vertex(vb[0],vol->hmax,vb[2], col);
592 dd->vertex(va[0],vol->hmin,va[2], duDarkenCol(col));
593 dd->vertex(va[0],vol->hmax,va[2], col);
594 }
595 }
596 dd->end();
597
598 dd->begin(DU_DRAW_POINTS, 3.0f);
599 for (int i = 0; i < m_volumeCount; ++i)
600 {
601 const ConvexVolume* vol = &m_volumes[i];
602 unsigned int col = duDarkenCol(duTransCol(dd->areaToCol(vol->area), 220));
603 for (int j = 0; j < vol->nverts; ++j)
604 {
605 dd->vertex(vol->verts[j*3+0],vol->verts[j*3+1]+0.1f,vol->verts[j*3+2], col);
606 dd->vertex(vol->verts[j*3+0],vol->hmin,vol->verts[j*3+2], col);
607 dd->vertex(vol->verts[j*3+0],vol->hmax,vol->verts[j*3+2], col);
608 }
609 }
610 dd->end();
611
612
613 dd->depthMask(true);
614 }
615