diff options
Diffstat (limited to 'deps/recastnavigation/Detour/DetourCommon.cpp')
-rw-r--r-- | deps/recastnavigation/Detour/DetourCommon.cpp | 329 |
1 files changed, 0 insertions, 329 deletions
diff --git a/deps/recastnavigation/Detour/DetourCommon.cpp b/deps/recastnavigation/Detour/DetourCommon.cpp deleted file mode 100644 index c0b973e4a7..0000000000 --- a/deps/recastnavigation/Detour/DetourCommon.cpp +++ /dev/null @@ -1,329 +0,0 @@ -// -// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org -// -// This software is provided 'as-is', without any express or implied -// warranty. In no event will the authors be held liable for any damages -// arising from the use of this software. -// Permission is granted to anyone to use this software for any purpose, -// including commercial applications, and to alter it and redistribute it -// freely, subject to the following restrictions: -// 1. The origin of this software must not be misrepresented; you must not -// claim that you wrote the original software. If you use this software -// in a product, an acknowledgment in the product documentation would be -// appreciated but is not required. -// 2. Altered source versions must be plainly marked as such, and must not be -// misrepresented as being the original software. -// 3. This notice may not be removed or altered from any source distribution. -// - -#include <math.h> -#include "DetourCommon.h" - -////////////////////////////////////////////////////////////////////////////////////////// - -float dtSqrt(float x) -{ - return sqrtf(x); -} - -void dtClosestPtPointTriangle(float* closest, const float* p, - const float* a, const float* b, const float* c) -{ - // Check if P in vertex region outside A - float ab[3], ac[3], ap[3]; - dtVsub(ab, b, a); - dtVsub(ac, c, a); - dtVsub(ap, p, a); - float d1 = dtVdot(ab, ap); - float d2 = dtVdot(ac, ap); - if (d1 <= 0.0f && d2 <= 0.0f) - { - // barycentric coordinates (1,0,0) - dtVcopy(closest, a); - return; - } - - // Check if P in vertex region outside B - float bp[3]; - dtVsub(bp, p, b); - float d3 = dtVdot(ab, bp); - float d4 = dtVdot(ac, bp); - if (d3 >= 0.0f && d4 <= d3) - { - // barycentric coordinates (0,1,0) - dtVcopy(closest, b); - return; - } - - // Check if P in edge region of AB, if so return projection of P onto AB - float vc = d1*d4 - d3*d2; - if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) - { - // barycentric coordinates (1-v,v,0) - float v = d1 / (d1 - d3); - closest[0] = a[0] + v * ab[0]; - closest[1] = a[1] + v * ab[1]; - closest[2] = a[2] + v * ab[2]; - return; - } - - // Check if P in vertex region outside C - float cp[3]; - dtVsub(cp, p, c); - float d5 = dtVdot(ab, cp); - float d6 = dtVdot(ac, cp); - if (d6 >= 0.0f && d5 <= d6) - { - // barycentric coordinates (0,0,1) - dtVcopy(closest, c); - return; - } - - // Check if P in edge region of AC, if so return projection of P onto AC - float vb = d5*d2 - d1*d6; - if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) - { - // barycentric coordinates (1-w,0,w) - float w = d2 / (d2 - d6); - closest[0] = a[0] + w * ac[0]; - closest[1] = a[1] + w * ac[1]; - closest[2] = a[2] + w * ac[2]; - return; - } - - // Check if P in edge region of BC, if so return projection of P onto BC - float va = d3*d6 - d5*d4; - if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) - { - // barycentric coordinates (0,1-w,w) - float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - closest[0] = b[0] + w * (c[0] - b[0]); - closest[1] = b[1] + w * (c[1] - b[1]); - closest[2] = b[2] + w * (c[2] - b[2]); - return; - } - - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - float denom = 1.0f / (va + vb + vc); - float v = vb * denom; - float w = vc * denom; - closest[0] = a[0] + ab[0] * v + ac[0] * w; - closest[1] = a[1] + ab[1] * v + ac[1] * w; - closest[2] = a[2] + ab[2] * v + ac[2] * w; -} - -bool dtIntersectSegmentPoly2D(const float* p0, const float* p1, - const float* verts, int nverts, - float& tmin, float& tmax, - int& segMin, int& segMax) -{ - static const float EPS = 0.00000001f; - - tmin = 0; - tmax = 1; - segMin = -1; - segMax = -1; - - float dir[3]; - dtVsub(dir, p1, p0); - - for (int i = 0, j = nverts-1; i < nverts; j=i++) - { - float edge[3], diff[3]; - dtVsub(edge, &verts[i*3], &verts[j*3]); - dtVsub(diff, p0, &verts[j*3]); - const float n = dtVperp2D(edge, diff); - const float d = dtVperp2D(dir, edge); - if (fabsf(d) < EPS) - { - // S is nearly parallel to this edge - if (n < 0) - return false; - else - continue; - } - const float t = n / d; - if (d < 0) - { - // segment S is entering across this edge - if (t > tmin) - { - tmin = t; - segMin = j; - // S enters after leaving polygon - if (tmin > tmax) - return false; - } - } - else - { - // segment S is leaving across this edge - if (t < tmax) - { - tmax = t; - segMax = j; - // S leaves before entering polygon - if (tmax < tmin) - return false; - } - } - } - - return true; -} - -float dtDistancePtSegSqr2D(const float* pt, const float* p, const float* q, float& t) -{ - float pqx = q[0] - p[0]; - float pqz = q[2] - p[2]; - float dx = pt[0] - p[0]; - float dz = pt[2] - p[2]; - float d = pqx*pqx + pqz*pqz; - t = pqx*dx + pqz*dz; - if (d > 0) t /= d; - if (t < 0) t = 0; - else if (t > 1) t = 1; - dx = p[0] + t*pqx - pt[0]; - dz = p[2] + t*pqz - pt[2]; - return dx*dx + dz*dz; -} - -void dtCalcPolyCenter(float* tc, const unsigned short* idx, int nidx, const float* verts) -{ - tc[0] = 0.0f; - tc[1] = 0.0f; - tc[2] = 0.0f; - for (int j = 0; j < nidx; ++j) - { - const float* v = &verts[idx[j]*3]; - tc[0] += v[0]; - tc[1] += v[1]; - tc[2] += v[2]; - } - const float s = 1.0f / nidx; - tc[0] *= s; - tc[1] *= s; - tc[2] *= s; -} - -bool dtClosestHeightPointTriangle(const float* p, const float* a, const float* b, const float* c, float& h) -{ - float v0[3], v1[3], v2[3]; - dtVsub(v0, c,a); - dtVsub(v1, b,a); - dtVsub(v2, p,a); - - const float dot00 = dtVdot2D(v0, v0); - const float dot01 = dtVdot2D(v0, v1); - const float dot02 = dtVdot2D(v0, v2); - const float dot11 = dtVdot2D(v1, v1); - const float dot12 = dtVdot2D(v1, v2); - - // Compute barycentric coordinates - const float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01); - const float u = (dot11 * dot02 - dot01 * dot12) * invDenom; - const float v = (dot00 * dot12 - dot01 * dot02) * invDenom; - - // The (sloppy) epsilon is needed to allow to get height of points which - // are interpolated along the edges of the triangles. - static const float EPS = 1e-4f; - - // If point lies inside the triangle, return interpolated ycoord. - if (u >= -EPS && v >= -EPS && (u+v) <= 1+EPS) - { - h = a[1] + v0[1]*u + v1[1]*v; - return true; - } - - return false; -} - -bool dtPointInPolygon(const float* pt, const float* verts, const int nverts) -{ - // TODO: Replace pnpoly with triArea2D tests? - int i, j; - bool c = false; - for (i = 0, j = nverts-1; i < nverts; j = i++) - { - const float* vi = &verts[i*3]; - const float* vj = &verts[j*3]; - if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && - (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) - c = !c; - } - return c; -} - -bool dtDistancePtPolyEdgesSqr(const float* pt, const float* verts, const int nverts, - float* ed, float* et) -{ - // TODO: Replace pnpoly with triArea2D tests? - int i, j; - bool c = false; - for (i = 0, j = nverts-1; i < nverts; j = i++) - { - const float* vi = &verts[i*3]; - const float* vj = &verts[j*3]; - if (((vi[2] > pt[2]) != (vj[2] > pt[2])) && - (pt[0] < (vj[0]-vi[0]) * (pt[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) ) - c = !c; - ed[j] = dtDistancePtSegSqr2D(pt, vj, vi, et[j]); - } - return c; -} - -static void projectPoly(const float* axis, const float* poly, const int npoly, - float& rmin, float& rmax) -{ - rmin = rmax = dtVdot2D(axis, &poly[0]); - for (int i = 1; i < npoly; ++i) - { - const float d = dtVdot2D(axis, &poly[i*3]); - rmin = dtMin(rmin, d); - rmax = dtMax(rmax, d); - } -} - -inline bool overlapRange(const float amin, const float amax, - const float bmin, const float bmax, - const float eps) -{ - return ((amin+eps) > bmax || (amax-eps) < bmin) ? false : true; -} - -bool dtOverlapPolyPoly2D(const float* polya, const int npolya, - const float* polyb, const int npolyb) -{ - const float eps = 1e-4f; - - for (int i = 0, j = npolya-1; i < npolya; j=i++) - { - const float* va = &polya[j*3]; - const float* vb = &polya[i*3]; - const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) }; - float amin,amax,bmin,bmax; - projectPoly(n, polya, npolya, amin,amax); - projectPoly(n, polyb, npolyb, bmin,bmax); - if (!overlapRange(amin,amax, bmin,bmax, eps)) - { - // Found separating axis - return false; - } - } - for (int i = 0, j = npolyb-1; i < npolyb; j=i++) - { - const float* va = &polyb[j*3]; - const float* vb = &polyb[i*3]; - const float n[3] = { vb[2]-va[2], 0, -(vb[0]-va[0]) }; - float amin,amax,bmin,bmax; - projectPoly(n, polya, npolya, amin,amax); - projectPoly(n, polyb, npolyb, bmin,bmax); - if (!overlapRange(amin,amax, bmin,bmax, eps)) - { - // Found separating axis - return false; - } - } - return true; -} - |