static float distPtTri(const float* p, const float* a, const float* b, const float* c) { float v0[3], v1[3], v2[3]; rcVsub(v0, c,a); rcVsub(v1, b,a); rcVsub(v2, p,a); const float dot00 = vdot2(v0, v0); const float dot01 = vdot2(v0, v1); const float dot02 = vdot2(v0, v2); const float dot11 = vdot2(v1, v1); const float dot12 = vdot2(v1, v2); // Compute barycentric coordinates const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); const float u = (dot11 * dot02 - dot01 * dot12) * invDenom; float v = (dot00 * dot12 - dot01 * dot02) * invDenom; // If point lies inside the triangle, return interpolated y-coord. static const float EPS = 1e-4f; if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) { const float y = a[1] + v0[1]*u + v1[1]*v; return fabsf(y-p[1]); } return FLT_MAX; }
static float distPtTri(const dtCoordinates& p, const dtCoordinates& a, const dtCoordinates& b, const dtCoordinates& c) { dtCoordinates v0, v1, v2; rcVsub(v0, c,a); rcVsub(v1, b,a); rcVsub(v2, p,a); const float dot00 = vdot2(v0, v0); const float dot01 = vdot2(v0, v1); const float dot02 = vdot2(v0, v2); const float dot11 = vdot2(v1, v1); const float dot12 = vdot2(v1, v2); // Compute barycentric coordinates const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); const float u = (dot11 * dot02 - dot01 * dot12) * invDenom; float v = (dot00 * dot12 - dot01 * dot02) * invDenom; // If point lies inside the triangle, return interpolated y-coord. static const float EPS = 1e-4f; if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) { const float y = a.Y() + v0.Y()*u + v1.Y()*v; return fabsf(y-p.Y()); } return FLT_MAX; }
static bool circumCircle(const float* p1, const float* p2, const float* p3, float* c, float& r) { static const float EPS = 1e-6f; // Calculate the circle relative to p1, to avoid some precision issues. const float v1[3] = {0,0,0}; float v2[3], v3[3]; rcVsub(v2, p2,p1); rcVsub(v3, p3,p1); const float cp = vcross2(v1, v2, v3); if (fabsf(cp) > EPS) { const float v1Sq = vdot2(v1,v1); const float v2Sq = vdot2(v2,v2); const float v3Sq = vdot2(v3,v3); c[0] = (v1Sq*(v2[2]-v3[2]) + v2Sq*(v3[2]-v1[2]) + v3Sq*(v1[2]-v2[2])) / (2*cp); c[1] = 0; c[2] = (v1Sq*(v3[0]-v2[0]) + v2Sq*(v1[0]-v3[0]) + v3Sq*(v2[0]-v1[0])) / (2*cp); r = vdist2(c, v1); rcVadd(c, c, p1); return true; } rcVcopy(c, p1); r = 0; return false; }
static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* norm) { float e0[3], e1[3]; rcVsub(e0, v1, v0); rcVsub(e1, v2, v0); rcVcross(norm, e0, e1); rcVnormalize(norm); }
static void calcTriNormal(const dtCoordinates& v0, const dtCoordinates& v1, const dtCoordinates& v2, dtCoordinates& norm) { dtCoordinates e0, e1; rcVsub(e0, v1, v0); rcVsub(e1, v2, v0); rcVcross(norm, e0, e1); rcVnormalize(norm); }
bool InputGeom::raycastMesh(float* src, float* dst, float& tmin) { float dir[3]; rcVsub(dir, dst, src); int nt = m_mesh->getTriCount(); const float* verts = m_mesh->getVerts(); const float* normals = m_mesh->getNormals(); const int* tris = m_mesh->getTris(); tmin = 1.0f; bool hit = false; for (int i = 0; i < nt*3; i += 3) { const float* n = &normals[i]; if (rcVdot(dir, n) > 0) continue; float t = 1; if (intersectSegmentTriangle(src, dst, &verts[tris[i]*3], &verts[tris[i+1]*3], &verts[tris[i+2]*3], t)) { if (t < tmin) tmin = t; hit = true; } } return hit; }
void TestCase::handleRender() { glLineWidth(2.0f); glBegin(GL_LINES); for (Test* iter = m_tests; iter; iter = iter->next) { float dir[3]; rcVsub(dir, iter->epos, iter->spos); rcVnormalize(dir); glColor4ub(128,25,0,192); glVertex3f(iter->spos[0],iter->spos[1]-0.3f,iter->spos[2]); glVertex3f(iter->spos[0],iter->spos[1]+0.3f,iter->spos[2]); glVertex3f(iter->spos[0],iter->spos[1]+0.3f,iter->spos[2]); glVertex3f(iter->spos[0]+dir[0]*0.3f,iter->spos[1]+0.3f+dir[1]*0.3f,iter->spos[2]+dir[2]*0.3f); glColor4ub(51,102,0,129); glVertex3f(iter->epos[0],iter->epos[1]-0.3f,iter->epos[2]); glVertex3f(iter->epos[0],iter->epos[1]+0.3f,iter->epos[2]); if (iter->expand) glColor4ub(255,192,0,255); else glColor4ub(0,0,0,64); for (int i = 0; i < iter->nstraight-1; ++i) { glVertex3f(iter->straight[i*3+0],iter->straight[i*3+1]+0.3f,iter->straight[i*3+2]); glVertex3f(iter->straight[(i+1)*3+0],iter->straight[(i+1)*3+1]+0.3f,iter->straight[(i+1)*3+2]); } } glEnd(); glLineWidth(1.0f); }
bool rcIsOverlapBounds2D( const dtCoordinates& vertexPoint, const dtCoordinates& bmin, const dtCoordinates& bmax ) { const dtCoordinates extend( 0.4f, 0.0f, 0.4f ); dtCoordinates amin, amax; rcVsub( amin, vertexPoint, extend ); rcVadd( amax, vertexPoint, extend ); bool overlap = true; overlap = (amin.X() > bmax.X() || amax.X() < bmin.X()) ? false : overlap; overlap = (amin.Z() > bmax.Z() || amax.Z() < bmin.Z()) ? false : overlap; return overlap; }
static bool intersectSegmentTriangle(const float* sp, const float* sq, const float* a, const float* b, const float* c, float &t) { float v, w; float ab[3], ac[3], qp[3], ap[3], norm[3], e[3]; rcVsub(ab, b, a); rcVsub(ac, c, a); rcVsub(qp, sp, sq); // Compute triangle normal. Can be precalculated or cached if // intersecting multiple segments against the same triangle rcVcross(norm, ab, ac); // Compute denominator d. If d <= 0, segment is parallel to or points // away from triangle, so exit early float d = rcVdot(qp, norm); if (d <= 0.0f) return false; // Compute intersection t value of pq with plane of triangle. A ray // intersects iff 0 <= t. Segment intersects iff 0 <= t <= 1. Delay // dividing by d until intersection has been found to pierce triangle rcVsub(ap, sp, a); t = rcVdot(ap, norm); if (t < 0.0f) return false; if (t > d) return false; // For segment; exclude this code line for a ray test // Compute barycentric coordinate components and test if within bounds rcVcross(e, qp, ap); v = rcVdot(ac, e); if (v < 0.0f || v > d) return false; w = -rcVdot(ab, e); if (w < 0.0f || v + w > d) return false; // Segment/ray intersects triangle. Perform delayed division t /= d; return true; }
static bool barDistSqPointToTri(const float* p, const float* a, const float* b, const float* c) { float v0[3], v1[3], v2[3]; rcVsub(v0, c,a); rcVsub(v1, b,a); rcVsub(v2, p,a); const float dot00 = vdot2(v0, v0); const float dot01 = vdot2(v0, v1); const float dot02 = vdot2(v0, v2); const float dot11 = vdot2(v1, v1); const float dot12 = vdot2(v1, v2); // Compute barycentric coordinates float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); float u = (dot11 * dot02 - dot01 * dot12) * invDenom; float v = (dot00 * dot12 - dot01 * dot02) * invDenom; float ud = u<0.f ? -u : (u>1.f ? u-1.f : 0.f); float vd = v<0.f ? -v : (v>1.f ? v-1.f : 0.f); return ud * ud + vd * vd; }
bool InputGeom::raycastMesh(float* src, float* dst, float& tmin) { float dir[3]; rcVsub(dir, dst, src); // Prune hit ray. float btmin, btmax; if (!isectSegAABB(src, dst, &m_meshBMin[0], &m_meshBMax[0], btmin, btmax)) return false; float p[2], q[2]; p[0] = src[0] + (dst[0]-src[0])*btmin; p[1] = src[2] + (dst[2]-src[2])*btmin; q[0] = src[0] + (dst[0]-src[0])*btmax; q[1] = src[2] + (dst[2]-src[2])*btmax; int cid[512]; const int ncid = rcGetChunksOverlappingSegment(m_chunkyMesh.get(), p, q, cid, 512); if (!ncid) return false; tmin = 1.0f; bool hit = false; const float* verts = m_loader->getVerts(); for (int i = 0; i < ncid; ++i) { const rcChunkyTriMeshNode& node = m_chunkyMesh->nodes[cid[i]]; const int* tris = &m_chunkyMesh->tris[node.i*3]; const int ntris = node.n; for (int j = 0; j < ntris*3; j += 3) { float t = 1; if (intersectSegmentTriangle( src, dst, &verts[tris[j]*3], &verts[tris[j+1]*3], &verts[tris[j+2]*3], t)) { if (t < tmin) tmin = t; hit = true; } } } return hit; }
static bool isectSegAABB(const float* sp, const float* sq, const float* amin, const float* amax, float& tmin, float& tmax) { static const float EPS = 1e-6f; float d[3]; rcVsub(d, sq, sp); tmin = 0; // set to -FLT_MAX to get first hit on line tmax = FLT_MAX; // set to max distance ray can travel (for segment) // For all three slabs for (int i = 0; i < 3; i++) { if (fabsf(d[i]) < EPS) { // Ray is parallel to slab. No hit if origin not within slab if (sp[i] < amin[i] || sp[i] > amax[i]) return false; } else { // Compute intersection t value of ray with near and far plane of slab const float ood = 1.0f / d[i]; float t1 = (amin[i] - sp[i]) * ood; float t2 = (amax[i] - sp[i]) * ood; // Make t1 be intersection with near plane, t2 with far plane if (t1 > t2) rcSwap(t1, t2); // Compute the intersection of slab intersections intervals if (t1 > tmin) tmin = t1; if (t2 < tmax) tmax = t2; // Exit with no collision as soon as slab intersection becomes empty if (tmin > tmax) return false; } } return true; }
void GameWorld::recalc() { if (!m_navMesh) return; if (m_sposSet) m_startRef = m_navMesh->findNearestPoly(m_spos, m_polyPickExt, &m_filter, 0); else m_startRef = 0; if (m_eposSet) m_endRef = m_navMesh->findNearestPoly(m_epos, m_polyPickExt, &m_filter, 0); else m_endRef = 0; #if 0 //if (m_toolMode == TOOLMODE_PATHFIND_ITER) { m_pathIterNum = 0; if (m_sposSet && m_eposSet && m_startRef && m_endRef) { #ifdef DUMP_REQS printf("pi %f %f %f %f %f %f 0x%x 0x%x\n", m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2], m_filter.includeFlags, m_filter.excludeFlags); rcGetLog()->log(RC_LOG_PROGRESS, "pi %f %f %f %f %f %f 0x%x 0x%x\n", m_spos[0],m_spos[1],m_spos[2], m_epos[0],m_epos[1],m_epos[2], m_filter.includeFlags, m_filter.excludeFlags ); #endif m_npolys = m_navMesh->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, MAX_POLYS); m_nsmoothPath = 0; if (m_npolys) { // Iterate over the path to find smooth path on the detail mesh surface. const dtPolyRef* polys = m_polys; int npolys = m_npolys; float iterPos[3], targetPos[3]; m_navMesh->closestPointOnPolyBoundary(m_startRef, m_spos, iterPos); m_navMesh->closestPointOnPolyBoundary(polys[npolys-1], m_epos, targetPos); static const float STEP_SIZE = 0.5f; static const float SLOP = 0.01f; m_nsmoothPath = 0; rcVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos); m_nsmoothPath++; // Move towards target a small advancement at a time until target reached or // when ran out of memory to store the path. while (npolys && m_nsmoothPath < MAX_SMOOTH) { // Find location to steer towards. float steerPos[3]; unsigned char steerPosFlag; dtPolyRef steerPosRef; if (!getSteerTarget(m_navMesh, iterPos, targetPos, SLOP, polys, npolys, steerPos, steerPosFlag, steerPosRef)) break; bool endOfPath = (steerPosFlag & DT_STRAIGHTPATH_END) ? true : false; bool offMeshConnection = (steerPosFlag & DT_STRAIGHTPATH_OFFMESH_CONNECTION) ? true : false; // Find movement delta. float delta[3], len; rcVsub(delta, steerPos, iterPos); len = sqrtf(rcVdot(delta,delta)); // If the steer target is end of path or off-mesh link, do not move past the location. if ((endOfPath || offMeshConnection) && len < STEP_SIZE) len = 1; else len = STEP_SIZE / len; float moveTgt[3]; rcVmad(moveTgt, iterPos, delta, len); // Move float result[3]; int n = m_navMesh->moveAlongPathCorridor(iterPos, moveTgt, result, polys, npolys); float h = 0; m_navMesh->getPolyHeight(polys[n], result, &h); result[1] = h; // Shrink path corridor if advanced. if (n) { polys += n; npolys -= n; } // Update position. rcVcopy(iterPos, result); // Handle end of path and off-mesh links when close enough. if (endOfPath && inRange(iterPos, steerPos, SLOP, 1.0f)) { // Reached end of path. rcVcopy(iterPos, targetPos); if (m_nsmoothPath < MAX_SMOOTH) { rcVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos); m_nsmoothPath++; } break; } else if (offMeshConnection && inRange(iterPos, steerPos, SLOP, 1.0f)) { // Reached off-mesh connection. float startPos[3], endPos[3]; // Advance the path up to and over the off-mesh connection. dtPolyRef prevRef = 0, polyRef = polys[0]; while (npolys && polyRef != steerPosRef) { prevRef = polyRef; polyRef = polys[0]; polys++; npolys--; } // Handle the connection. if (m_navMesh->getOffMeshConnectionPolyEndPoints(prevRef, polyRef, startPos, endPos)) { if (m_nsmoothPath < MAX_SMOOTH) { rcVcopy(&m_smoothPath[m_nsmoothPath*3], startPos); m_nsmoothPath++; // Hack to make the dotted path not visible during off-mesh connection. if (m_nsmoothPath & 1) { rcVcopy(&m_smoothPath[m_nsmoothPath*3], startPos); m_nsmoothPath++; } } // Move position at the other side of the off-mesh link. rcVcopy(iterPos, endPos); float h; m_navMesh->getPolyHeight(polys[0], iterPos, &h); iterPos[1] = h; } } // Store results. if (m_nsmoothPath < MAX_SMOOTH) { rcVcopy(&m_smoothPath[m_nsmoothPath*3], iterPos); m_nsmoothPath++; } } } } else { m_npolys = 0; m_nsmoothPath = 0; } } else if (m_toolMode == TOOLMODE_PATHFIND_STRAIGHT)
bool TestCase::handleRenderOverlay(double* proj, double* model, int* view) { GLdouble x, y, z; char text[64]; int n = 0; static const float LABEL_DIST = 1.0f; for (Test* iter = m_tests; iter; iter = iter->next) { float pt[3], dir[3]; if (iter->nstraight) { rcVcopy(pt, &iter->straight[3]); if (rcVdist(pt, iter->spos) > LABEL_DIST) { rcVsub(dir, pt, iter->spos); rcVnormalize(dir); rcVmad(pt, iter->spos, dir, LABEL_DIST); } pt[1]+=0.5f; } else { rcVsub(dir, iter->epos, iter->spos); rcVnormalize(dir); rcVmad(pt, iter->spos, dir, LABEL_DIST); pt[1]+=0.5f; } if (gluProject((GLdouble)pt[0], (GLdouble)pt[1], (GLdouble)pt[2], model, proj, view, &x, &y, &z)) { snprintf(text, 64, "Path %d\n", n); unsigned int col = imguiRGBA(0,0,0,128); if (iter->expand) col = imguiRGBA(255,192,0,220); imguiDrawText((int)x, (int)(y-25), IMGUI_ALIGN_CENTER, text, col); } n++; } static int resScroll = 0; bool mouseOverMenu = imguiBeginScrollArea("Test Results", 10, view[3] - 10 - 350, 200, 350, &resScroll); // mouseOverMenu = true; n = 0; for (Test* iter = m_tests; iter; iter = iter->next) { snprintf(text, 64, "Path %d\n", n); if (imguiCollapse(text, iter->expand)) iter->expand = !iter->expand; if (iter->expand) { snprintf(text, 64, "Poly: %.4f ms\n", (float)iter->findNearestPolyTime/1000.0f); imguiValue(text); snprintf(text, 64, "Path: %.4f ms\n", (float)iter->findPathTime/1000.0f); imguiValue(text); snprintf(text, 64, "Straight: %.4f ms\n", (float)iter->findStraightPathTime/1000.0f); imguiValue(text); } rcTimeVal total = iter->findNearestPolyTime + iter->findPathTime + iter->findStraightPathTime; snprintf(text, 64, "Total: %.4f ms\n", (float)total/1000.0f); imguiValue(text); // imguiDrawText(10, 700-n*20, IMGUI_ALIGN_LEFT, text, imguiRGBA(255,255,255,220)); n++; } imguiEndScrollArea(); return mouseOverMenu; }