bit
/
DotRecastNetSim
forked from mirror/DotRecast
40
0
Fork 1
Code Pull Requests Releases Activity

added DetourCommon

This commit is contained in:
ikpil 2023-06-01 23:44:02 +09:00
parent e87d1f9635
commit c535c76d18
1 changed files with 400 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

400
src/DotRecast.Detour/DetourCommon.cs Normal file
View File

@ -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;
}
}
}