Skip to content
Permalink
Browse files

[Upstream] Robust nearest point queries (#53)

* Make closestHeightPointTriangle use no epsilon (except for checking
denominator)
* When point is outside poly, directly find closest height from boundary
detail edges instead of first interpolating and then finding height from
detail tris.
* When point is inside poly, if all closestHeightPointTriangle queries
failed, then find height from detail edges. This should only happen if
the point is right on top of an internal detail tri edge.
  • Loading branch information...
ppiastucki committed Mar 29, 2019
1 parent d194c90 commit 3c532068d79fe0306fedf035e50216008c306cdf
@@ -326,6 +326,10 @@ public static boolean overlapBounds(float[] amin, float[] amax, float[] bmin, fl

// Compute scaled barycentric coordinates
float denom = v0[0] * v1[2] - v0[2] * v1[0];
if (Math.abs(denom) < EPS) {
return Optional.empty();
}

float u = v1[2] * v2[0] - v1[0] * v2[2];
float v = v0[0] * v2[2] - v0[2] * v2[0];

@@ -334,12 +338,9 @@ public static boolean overlapBounds(float[] amin, float[] amax, float[] bmin, fl
u = -u;
v = -v;
}
// The (sloppy) epsilon is needed to allow to get height of points which
// are interpolated along the edges of the triangles.
float epsilon = -1e-4f * denom;

// If point lies inside the triangle, return interpolated ycoord.
if (u >= epsilon && v >= epsilon && (u + v) <= denom - epsilon) {
if (u >= 0.0f && v >= 0.0f && (u + v) <= denom) {
float h = a[1] + (v0[1] * u + v1[1] * v) / denom;
return Optional.of(h);
}
@@ -1,6 +1,6 @@
/*
Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
Recast4J Copyright (c) 2015 Piotr Piastucki piotr@jtilia.org
recast4J copyright (c) 2015-2019 Piotr Piastucki piotr@jtilia.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
@@ -20,21 +20,26 @@ Recast4J Copyright (c) 2015 Piotr Piastucki piotr@jtilia.org

public class MeshData {

/** The tile header. */
public MeshHeader header;
/** The tile vertices. [Size: MeshHeader::vertCount] */
public float[] verts;
/** The tile polygons. [Size: MeshHeader::polyCount] */
public Poly[] polys;
/** The tile's detail sub-meshes. [Size: MeshHeader::detailMeshCount] */
public PolyDetail[] detailMeshes;
/** The detail mesh's unique vertices. [(x, y, z) * MeshHeader::detailVertCount] */
public float[] detailVerts;
/** The detail mesh's triangles. [(vertA, vertB, vertC) * MeshHeader::detailTriCount] */
public int[] detailTris;
/** The tile bounding volume nodes. [Size: MeshHeader::bvNodeCount] (Will be null if bounding volumes are disabled.) */
public BVNode[] bvTree;
/** The tile off-mesh connections. [Size: MeshHeader::offMeshConCount] */
public OffMeshConnection[] offMeshCons;

/** The tile header. */
public MeshHeader header;
/** The tile vertices. [Size: MeshHeader::vertCount] */
public float[] verts;
/** The tile polygons. [Size: MeshHeader::polyCount] */
public Poly[] polys;
/** The tile's detail sub-meshes. [Size: MeshHeader::detailMeshCount] */
public PolyDetail[] detailMeshes;
/** The detail mesh's unique vertices. [(x, y, z) * MeshHeader::detailVertCount] */
public float[] detailVerts;
/**
* The detail mesh's triangles. [(vertA, vertB, vertC) * MeshHeader::detailTriCount]
* See DetailTriEdgeFlags and NavMesh::getDetailTriEdgeFlags.
*/
public int[] detailTris;
/**
* The tile bounding volume nodes. [Size: MeshHeader::bvNodeCount] (Will be null if bounding volumes are disabled.)
*/
public BVNode[] bvTree;
/** The tile off-mesh connections. [Size: MeshHeader::offMeshConCount] */
public OffMeshConnection[] offMeshCons;

}
@@ -20,15 +20,15 @@ recast4j Copyright (c) 2015-2019 Piotr Piastucki piotr@jtilia.org

import static org.recast4j.detour.DetourCommon.clamp;
import static org.recast4j.detour.DetourCommon.closestHeightPointTriangle;
import static org.recast4j.detour.DetourCommon.distancePtPolyEdgesSqr;
import static org.recast4j.detour.DetourCommon.distancePtSegSqr2D;
import static org.recast4j.detour.DetourCommon.nextPow2;
import static org.recast4j.detour.DetourCommon.oppositeTile;
import static org.recast4j.detour.DetourCommon.overlapBounds;
import static org.recast4j.detour.DetourCommon.overlapQuantBounds;
import static org.recast4j.detour.DetourCommon.pointInPolygon;
import static org.recast4j.detour.DetourCommon.sqr;
import static org.recast4j.detour.DetourCommon.vAdd;
import static org.recast4j.detour.DetourCommon.vCopy;
import static org.recast4j.detour.DetourCommon.vDist;
import static org.recast4j.detour.DetourCommon.vLenSqr;
import static org.recast4j.detour.DetourCommon.vLerp;
import static org.recast4j.detour.DetourCommon.vMax;
@@ -44,6 +44,7 @@ recast4j Copyright (c) 2015-2019 Piotr Piastucki piotr@jtilia.org
static int DT_SALT_BITS = 16;
static int DT_TILE_BITS = 28;
static int DT_POLY_BITS = 20;
public static final int DT_DETAIL_EDGE_BOUNDARY = 0x01;

/// A flag that indicates that an entity links to an external entity.
/// (E.g. A polygon edge is a portal that links to another polygon.)
@@ -998,77 +999,132 @@ void baseOffMeshLinks(MeshTile tile) {
* @param pos
* @return
*/
ClosestPointOnPolyResult closestPointOnPoly(long ref, float[] pos) {
Tupple2<MeshTile, Poly> tileAndPoly = getTileAndPolyByRefUnsafe(ref);
MeshTile tile = tileAndPoly.first;
Poly poly = tileAndPoly.second;
// Off-mesh connections don't have detail polygons.
float[] closestPointOnDetailEdges(MeshTile tile, Poly poly, float[] pos, boolean onlyBoundary) {
int ANY_BOUNDARY_EDGE = (DT_DETAIL_EDGE_BOUNDARY << 0) | (DT_DETAIL_EDGE_BOUNDARY << 2)
| (DT_DETAIL_EDGE_BOUNDARY << 4);
int ip = poly.index;
PolyDetail pd = tile.data.detailMeshes[ip];

float dmin = Float.MAX_VALUE;
float tmin = 0;
float[] pmin = null;
float[] pmax = null;

for (int i = 0; i < pd.triCount; i++) {
int ti = (pd.triBase + i) * 4;
int[] tris = tile.data.detailTris;
if (onlyBoundary && (tris[ti + 3] & ANY_BOUNDARY_EDGE) == 0) {
continue;
}

float[][] v = new float[3][];
for (int j = 0; j < 3; ++j) {
if (tris[ti + j] < poly.vertCount) {
int index = poly.verts[tris[ti + j]] * 3;
v[j] = new float[] { tile.data.verts[index], tile.data.verts[index + 1],
tile.data.verts[index + 2] };
} else {
int index = (pd.vertBase + (tris[ti + j] - poly.vertCount)) * 3;
v[j] = new float[] { tile.data.detailVerts[index], tile.data.detailVerts[index + 1],
tile.data.detailVerts[index + 2] };
}
}

for (int k = 0, j = 2; k < 3; j = k++) {
if ((getDetailTriEdgeFlags(tris[3], j) & DT_DETAIL_EDGE_BOUNDARY) == 0
&& (onlyBoundary || tris[j] < tris[k])) {
// Only looking at boundary edges and this is internal, or
// this is an inner edge that we will see again or have already seen.
continue;
}

Tupple2<Float, Float> dt = distancePtSegSqr2D(pos, v[j], v[k]);
float d = dt.first;
float t = dt.second;
if (d < dmin) {
dmin = d;
tmin = t;
pmin = v[j];
pmax = v[k];
}
}
}

return vLerp(pmin, pmax, tmin);
}

Optional<Float> getPolyHeight(MeshTile tile, Poly poly, float[] pos) {
// Off-mesh connections do not have detail polys and getting height
// over them does not make sense.
if (poly.getType() == Poly.DT_POLYTYPE_OFFMESH_CONNECTION) {
int v0 = poly.verts[0] * 3;
int v1 = poly.verts[1] * 3;
float d0 = vDist(pos, tile.data.verts, v0);
float d1 = vDist(pos, tile.data.verts, v1);
float u = d0 / (d0 + d1);
float[] closest = vLerp(tile.data.verts, v0, v1, u);
return new ClosestPointOnPolyResult(false, closest);
return Optional.empty();
}

// Clamp point to be inside the polygon.
int ip = poly.index;
PolyDetail pd = tile.data.detailMeshes[ip];

float[] verts = new float[m_maxVertPerPoly * 3];
float[] edged = new float[m_maxVertPerPoly];
float[] edget = new float[m_maxVertPerPoly];
int nv = poly.vertCount;
for (int i = 0; i < nv; ++i) {
System.arraycopy(tile.data.verts, poly.verts[i] * 3, verts, i * 3, 3);
}

boolean posOverPoly = false;
float[] closest = new float[3];
vCopy(closest, pos);
if (!distancePtPolyEdgesSqr(pos, verts, nv, edged, edget)) {
// Point is outside the polygon, dtClamp to nearest edge.
float dmin = edged[0];
int imin = 0;
for (int i = 1; i < nv; ++i) {
if (edged[i] < dmin) {
dmin = edged[i];
imin = i;
}
}
int va = imin * 3;
int vb = ((imin + 1) % nv) * 3;
closest = vLerp(verts, va, vb, edget[imin]);
posOverPoly = false;
} else {
posOverPoly = true;
if (!pointInPolygon(pos, verts, nv)) {
return Optional.empty();
}

// Find height at the location.
int ip = poly.index;
if (tile.data.detailMeshes != null && tile.data.detailMeshes.length > ip) {
PolyDetail pd = tile.data.detailMeshes[ip];
for (int j = 0; j < pd.triCount; ++j) {
int t = (pd.triBase + j) * 4;
float[][] v = new float[3][];
for (int k = 0; k < 3; ++k) {
if (tile.data.detailTris[t + k] < poly.vertCount) {
int index = poly.verts[tile.data.detailTris[t + k]] * 3;
v[k] = new float[] { tile.data.verts[index], tile.data.verts[index + 1],
tile.data.verts[index + 2] };
} else {
int index = (pd.vertBase + (tile.data.detailTris[t + k] - poly.vertCount)) * 3;
v[k] = new float[] { tile.data.detailVerts[index], tile.data.detailVerts[index + 1],
tile.data.detailVerts[index + 2] };
}
}
Optional<Float> heightResult = closestHeightPointTriangle(closest, v[0], v[1], v[2]);
if (heightResult.isPresent()) {
closest[1] = heightResult.get();
break;
for (int j = 0; j < pd.triCount; ++j) {
int t = (pd.triBase + j) * 4;
float[][] v = new float[3][];
for (int k = 0; k < 3; ++k) {
if (tile.data.detailTris[t + k] < poly.vertCount) {
int index = poly.verts[tile.data.detailTris[t + k]] * 3;
v[k] = new float[] { tile.data.verts[index], tile.data.verts[index + 1],
tile.data.verts[index + 2] };
} else {
int index = (pd.vertBase + (tile.data.detailTris[t + k] - poly.vertCount)) * 3;
v[k] = new float[] { tile.data.detailVerts[index], tile.data.detailVerts[index + 1],
tile.data.detailVerts[index + 2] };
}
}
Optional<Float> h = closestHeightPointTriangle(pos, v[0], v[1], v[2]);
if (h.isPresent()) {
return h;
}
}

// If all triangle checks failed above (can happen with degenerate triangles
// or larger floating point values) the point is on an edge, so just select
// closest. This should almost never happen so the extra iteration here is
// ok.
float[] closest = closestPointOnDetailEdges(tile, poly, pos, false);
return Optional.of(closest[1]);
}

ClosestPointOnPolyResult closestPointOnPoly(long ref, float[] pos) {
Tupple2<MeshTile, Poly> tileAndPoly = getTileAndPolyByRefUnsafe(ref);
MeshTile tile = tileAndPoly.first;
Poly poly = tileAndPoly.second;
float[] closest = new float[3];
vCopy(closest, pos);
Optional<Float> h = getPolyHeight(tile, poly, pos);
if (h.isPresent()) {
closest[1] = h.get();
return new ClosestPointOnPolyResult(true, closest);
}
return new ClosestPointOnPolyResult(posOverPoly, closest);

// Off-mesh connections don't have detail polygons.
if (poly.getType() == Poly.DT_POLYTYPE_OFFMESH_CONNECTION) {
int i = poly.verts[0] * 3;
float[] v0 = new float[] { tile.data.verts[i], tile.data.verts[i + 1], tile.data.verts[i + 2] };
i = poly.verts[1] * 3;
float[] v1 = new float[] { tile.data.verts[i], tile.data.verts[i + 1], tile.data.verts[i + 2] };
Tupple2<Float, Float> dt = distancePtSegSqr2D(pos, v0, v1);
return new ClosestPointOnPolyResult(false, vLerp(v0, v1, dt.second));
}
// Outside poly that is not an offmesh connection.
return new ClosestPointOnPolyResult(false, closestPointOnDetailEdges(tile, poly, pos, true));
}

FindNearestPolyResult findNearestPolyInTile(MeshTile tile, float[] center, float[] extents) {
@@ -1376,4 +1432,18 @@ public Status setPolyArea(long ref, char area) {

return Result.success(poly.getArea());
}

/**
* Get flags for edge in detail triangle.
*
* @param triFlags
* The flags for the triangle (last component of detail vertices above).
* @param edgeIndex
* The index of the first vertex of the edge. For instance, if 0,
* @return flags for edge AB.
*/
public static int getDetailTriEdgeFlags(int triFlags, int edgeIndex) {
return (triFlags >> (edgeIndex * 2)) & 0x3;
}

}
Oops, something went wrong.

0 comments on commit 3c53206

Please sign in to comment.
You can’t perform that action at this time.