diff --git a/src/DotRecast.Detour/DetourCommon.cs b/src/DotRecast.Detour/DetourCommon.cs new file mode 100644 index 0000000..be2c922 --- /dev/null +++ b/src/DotRecast.Detour/DetourCommon.cs @@ -0,0 +1,400 @@ +using System; +using DotRecast.Core; + +namespace DotRecast.Detour +{ + public static class DtCommon + { + private static readonly float EQUAL_THRESHOLD = RcMath.Sqr(1.0f / 16384.0f); + + /// Performs a 'sloppy' colocation check of the specified points. + /// @param[in] p0 A point. [(x, y, z)] + /// @param[in] p1 A point. [(x, y, z)] + /// @return True if the points are considered to be at the same location. + /// + /// Basically, this function will return true if the specified points are + /// close enough to eachother to be considered colocated. + public static bool VEqual(Vector3f p0, Vector3f p1) + { + return VEqual(p0, p1, EQUAL_THRESHOLD); + } + + public static bool VEqual(Vector3f p0, Vector3f p1, float thresholdSqr) + { + float d = Vector3f.DistSqr(p0, p1); + return d < thresholdSqr; + } + + /// Determines if two axis-aligned bounding boxes overlap. + /// @param[in] amin Minimum bounds of box A. [(x, y, z)] + /// @param[in] amax Maximum bounds of box A. [(x, y, z)] + /// @param[in] bmin Minimum bounds of box B. [(x, y, z)] + /// @param[in] bmax Maximum bounds of box B. [(x, y, z)] + /// @return True if the two AABB's overlap. + /// @see dtOverlapBounds + public static bool OverlapQuantBounds(int[] amin, int[] amax, int[] bmin, int[] bmax) + { + bool overlap = true; + overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap; + overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap; + overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap; + return overlap; + } + + /// Determines if two axis-aligned bounding boxes overlap. + /// @param[in] amin Minimum bounds of box A. [(x, y, z)] + /// @param[in] amax Maximum bounds of box A. [(x, y, z)] + /// @param[in] bmin Minimum bounds of box B. [(x, y, z)] + /// @param[in] bmax Maximum bounds of box B. [(x, y, z)] + /// @return True if the two AABB's overlap. + /// @see dtOverlapQuantBounds + public static bool OverlapBounds(Vector3f amin, Vector3f amax, Vector3f bmin, Vector3f bmax) + { + bool overlap = true; + overlap = (amin.x > bmax.x || amax.x < bmin.x) ? false : overlap; + overlap = (amin.y > bmax.y || amax.y < bmin.y) ? false : overlap; + overlap = (amin.z > bmax.z || amax.z < bmin.z) ? false : overlap; + return overlap; + } + + public static bool OverlapRange(float amin, float amax, float bmin, float bmax, float eps) + { + return ((amin + eps) > bmax || (amax - eps) < bmin) ? false : true; + } + + /// @par + /// + /// All vertices are projected onto the xz-plane, so the y-values are ignored. + public static bool OverlapPolyPoly2D(float[] polya, int npolya, float[] polyb, int npolyb) + { + const float eps = 1e-4f; + for (int i = 0, j = npolya - 1; i < npolya; j = i++) + { + int va = j * 3; + int vb = i * 3; + + Vector3f n = Vector3f.Of(polya[vb + 2] - polya[va + 2], 0, -(polya[vb + 0] - polya[va + 0])); + + Vector2f aminmax = ProjectPoly(n, polya, npolya); + Vector2f bminmax = ProjectPoly(n, polyb, npolyb); + if (!OverlapRange(aminmax.x, aminmax.y, bminmax.x, bminmax.y, eps)) + { + // Found separating axis + return false; + } + } + + for (int i = 0, j = npolyb - 1; i < npolyb; j = i++) + { + int va = j * 3; + int vb = i * 3; + + Vector3f n = Vector3f.Of(polyb[vb + 2] - polyb[va + 2], 0, -(polyb[vb + 0] - polyb[va + 0])); + + Vector2f aminmax = ProjectPoly(n, polya, npolya); + Vector2f bminmax = ProjectPoly(n, polyb, npolyb); + if (!OverlapRange(aminmax.x, aminmax.y, bminmax.x, bminmax.y, eps)) + { + // Found separating axis + return false; + } + } + + return true; + } + + + /// @} + /// @name Computational geometry helper functions. + /// @{ + /// Derives the signed xz-plane area of the triangle ABC, or the + /// relationship of line AB to point C. + /// @param[in] a Vertex A. [(x, y, z)] + /// @param[in] b Vertex B. [(x, y, z)] + /// @param[in] c Vertex C. [(x, y, z)] + /// @return The signed xz-plane area of the triangle. + public static float TriArea2D(float[] verts, int a, int b, int c) + { + float abx = verts[b] - verts[a]; + float abz = verts[b + 2] - verts[a + 2]; + float acx = verts[c] - verts[a]; + float acz = verts[c + 2] - verts[a + 2]; + return acx * abz - abx * acz; + } + + public static float TriArea2D(Vector3f a, Vector3f b, Vector3f c) + { + float abx = b.x - a.x; + float abz = b.z - a.z; + float acx = c.x - a.x; + float acz = c.z - a.z; + return acx * abz - abx * acz; + } + + // Returns a random point in a convex polygon. + // Adapted from Graphics Gems article. + public static Vector3f RandomPointInConvexPoly(float[] pts, int npts, float[] areas, float s, float t) + { + // Calc triangle araes + float areasum = 0.0f; + for (int i = 2; i < npts; i++) + { + areas[i] = TriArea2D(pts, 0, (i - 1) * 3, i * 3); + areasum += Math.Max(0.001f, areas[i]); + } + + // Find sub triangle weighted by area. + float thr = s * areasum; + float acc = 0.0f; + float u = 1.0f; + int tri = npts - 1; + for (int i = 2; i < npts; i++) + { + float dacc = areas[i]; + if (thr >= acc && thr < (acc + dacc)) + { + u = (thr - acc) / dacc; + tri = i; + break; + } + + acc += dacc; + } + + float v = (float)Math.Sqrt(t); + + float a = 1 - v; + float b = (1 - u) * v; + float c = u * v; + int pa = 0; + int pb = (tri - 1) * 3; + int pc = tri * 3; + + return new Vector3f() + { + x = a * pts[pa] + b * pts[pb] + c * pts[pc], + y = a * pts[pa + 1] + b * pts[pb + 1] + c * pts[pc + 1], + z = a * pts[pa + 2] + b * pts[pb + 2] + c * pts[pc + 2] + }; + } + + public static float? ClosestHeightPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c) + { + const float EPS = 1e-6f; + + Vector3f v0 = c.Subtract(a); + Vector3f v1 = b.Subtract(a); + Vector3f v2 = p.Subtract(a); + + // Compute scaled barycentric coordinates + float denom = v0.x * v1.z - v0.z * v1.x; + if (Math.Abs(denom) < EPS) + { + return null; + } + + float u = v1.z * v2.x - v1.x * v2.z; + float v = v0.x * v2.z - v0.z * v2.x; + + if (denom < 0) + { + denom = -denom; + u = -u; + v = -v; + } + + // If point lies inside the triangle, return interpolated ycoord. + if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) + { + float h = a.y + (v0.y * u + v1.y * v) / denom; + return h; + } + + return null; + } + + public static Vector2f ProjectPoly(Vector3f axis, float[] poly, int npoly) + { + float rmin, rmax; + rmin = rmax = axis.Dot2D(poly, 0); + for (int i = 1; i < npoly; ++i) + { + float d = axis.Dot2D(poly, i * 3); + rmin = Math.Min(rmin, d); + rmax = Math.Max(rmax, d); + } + + return new Vector2f + { + x = rmin, + y = rmax, + }; + } + + /// @par + /// + /// All points are projected onto the xz-plane, so the y-values are ignored. + public static bool PointInPolygon(Vector3f pt, float[] verts, int nverts) + { + // TODO: Replace pnpoly with triArea2D tests? + int i, j; + bool c = false; + for (i = 0, j = nverts - 1; i < nverts; j = i++) + { + int vi = i * 3; + int vj = j * 3; + if (((verts[vi + 2] > pt.z) != (verts[vj + 2] > pt.z)) && (pt.x < (verts[vj + 0] - verts[vi + 0]) + * (pt.z - verts[vi + 2]) / (verts[vj + 2] - verts[vi + 2]) + verts[vi + 0])) + { + c = !c; + } + } + + return c; + } + + public static bool DistancePtPolyEdgesSqr(Vector3f pt, float[] verts, 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++) + { + int vi = i * 3; + int vj = j * 3; + if (((verts[vi + 2] > pt.z) != (verts[vj + 2] > pt.z)) && + (pt.x < (verts[vj + 0] - verts[vi + 0]) * (pt.z - verts[vi + 2]) / (verts[vj + 2] - verts[vi + 2]) + verts[vi + 0])) + { + c = !c; + } + + ed[j] = DistancePtSegSqr2D(pt, verts, vj, vi, out et[j]); + } + + return c; + } + + public static float DistancePtSegSqr2D(Vector3f pt, float[] verts, int p, int q, out float t) + { + var vp = Vector3f.Of(verts, p); + var vq = Vector3f.Of(verts, q); + return DistancePtSegSqr2D(pt, vp, vq, out t); + } + + public static float DistancePtSegSqr2D(Vector3f pt, Vector3f p, Vector3f q, out float t) + { + float pqx = q.x - p.x; + float pqz = q.z - p.z; + float dx = pt.x - p.x; + float dz = pt.z - p.z; + 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.x + t * pqx - pt.x; + dz = p.z + t * pqz - pt.z; + return dx * dx + dz * dz; + } + + public static IntersectResult IntersectSegmentPoly2D(Vector3f p0, Vector3f p1, float[] verts, int nverts) + { + IntersectResult result = new IntersectResult(); + float EPS = 0.000001f; + var dir = p1.Subtract(p0); + + var p0v = p0; + for (int i = 0, j = nverts - 1; i < nverts; j = i++) + { + Vector3f vpj = Vector3f.Of(verts, j * 3); + Vector3f vpi = Vector3f.Of(verts, i * 3); + var edge = vpi.Subtract(vpj); + var diff = p0v.Subtract(vpj); + float n = Vector3f.Perp2D(edge, diff); + float d = Vector3f.Perp2D(dir, edge); + if (Math.Abs(d) < EPS) + { + // S is nearly parallel to this edge + if (n < 0) + { + return result; + } + else + { + continue; + } + } + + float t = n / d; + if (d < 0) + { + // segment S is entering across this edge + if (t > result.tmin) + { + result.tmin = t; + result.segMin = j; + // S enters after leaving polygon + if (result.tmin > result.tmax) + { + return result; + } + } + } + else + { + // segment S is leaving across this edge + if (t < result.tmax) + { + result.tmax = t; + result.segMax = j; + // S leaves before entering polygon + if (result.tmax < result.tmin) + { + return result; + } + } + } + } + + result.intersects = true; + return result; + } + + public static int OppositeTile(int side) + { + return (side + 4) & 0x7; + } + + + public static bool IntersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq, out float s, out float t) + { + s = 0; + t = 0; + + Vector3f u = aq.Subtract(ap); + Vector3f v = bq.Subtract(bp); + Vector3f w = ap.Subtract(bp); + float d = Vector3f.PerpXZ(u, v); + if (Math.Abs(d) < 1e-6f) + { + return false; + } + + s = Vector3f.PerpXZ(v, w) / d; + t = Vector3f.PerpXZ(u, w) / d; + + return true; + } + } +} \ No newline at end of file