Skip to content
Permalink
Browse files

Add A* Pathfinding importer

  • Loading branch information...
ppiastucki
ppiastucki committed Aug 6, 2017
1 parent de4a7fe commit 23781a2a5e59bde81d6048ddc23c930c1dd00aea
Showing with 838 additions and 18 deletions.
  1. +3 −0 .gitignore
  2. +34 −0 detour-extras/pom.xml
  3. +66 −0 detour-extras/src/main/java/org/recast4j/detour/extras/PolyUtils.java
  4. +16 −0 detour-extras/src/main/java/org/recast4j/detour/extras/Vector3f.java
  5. +47 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/BVTreeBuilder.java
  6. +20 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/BinaryReader.java
  7. +29 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/GraphConnectionReader.java
  8. +49 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/GraphMeshData.java
  9. +120 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/GraphMeshDataReader.java
  10. +20 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/GraphMeta.java
  11. +19 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/GraphMetaReader.java
  12. +52 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/LinkBuilder.java
  13. +49 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/Meta.java
  14. +28 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/MetaReader.java
  15. +21 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/NodeIndexReader.java
  16. +21 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/NodeLink2.java
  17. +35 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/NodeLink2Reader.java
  18. +50 −0 detour-extras/src/main/java/org/recast4j/detour/extras/unity/astar/OffMeshLinkCreator.java
  19. +82 −0 ...ur-extras/src/main/java/org/recast4j/detour/extras/unity/astar/UnityAStarPathfindingImporter.java
  20. +55 −0 ...xtras/src/test/java/org/recast4j/detour/extras/unity/astar/UnityAStarPathfindingImporterTest.java
  21. BIN detour-extras/src/test/resources/graph.zip
  22. +2 −2 detour/src/main/java/org/recast4j/detour/DetourCommon.java
  23. +3 −6 detour/src/main/java/org/recast4j/detour/NavMesh.java
  24. +6 −6 detour/src/main/java/org/recast4j/detour/NavMeshBuilder.java
  25. +2 −2 detour/src/main/java/org/recast4j/detour/Poly.java
  26. +9 −2 pom.xml
@@ -0,0 +1,3 @@
.settings
.metadata
.idea
@@ -0,0 +1,34 @@
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
<modelVersion>4.0.0</modelVersion>
<parent>
<groupId>org.recast4j</groupId>
<artifactId>parent</artifactId>
<version>1.0.6-SNAPSHOT</version>
</parent>
<artifactId>detour-extras</artifactId>
<dependencies>
<dependency>
<groupId>org.recast4j</groupId>
<artifactId>detour</artifactId>
<version>1.0.6-SNAPSHOT</version>
</dependency>
<dependency>
<groupId>org.recast4j</groupId>
<artifactId>detour</artifactId>
<type>test-jar</type>
<version>1.0.6-SNAPSHOT</version>
<scope>test</scope>
</dependency>
<dependency>
<groupId>junit</groupId>
<artifactId>junit</artifactId>
<scope>test</scope>
</dependency>
<dependency>
<groupId>com.google.code.gson</groupId>
<artifactId>gson</artifactId>
<version>2.8.1</version>
</dependency>
</dependencies>
</project>
@@ -0,0 +1,66 @@
package org.recast4j.detour.extras;

import org.recast4j.detour.MeshData;
import org.recast4j.detour.Poly;

public class PolyUtils {

/**
* Find edge shared by 2 polygons within the same tile
*/
public static int findEdge(Poly node, Poly neighbour, MeshData tile, MeshData neighbourTile) {
// Compare indices first assuming there are no duplicate vertices
for (int i = 0; i < node.vertCount; i++) {
int j = (i + 1) % node.vertCount;
for (int k = 0; k < neighbour.vertCount; k++) {
int l = (k + 1) % neighbour.vertCount;
if ((node.verts[i] == neighbour.verts[l] && node.verts[j] == neighbour.verts[k])
|| (node.verts[i] == neighbour.verts[k] && node.verts[j] == neighbour.verts[l])) {
return i;
}
}
}
// Fall back to comparing actual positions in case of duplicate vertices
for (int i = 0; i < node.vertCount; i++) {
int j = (i + 1) % node.vertCount;
for (int k = 0; k < neighbour.vertCount; k++) {
int l = (k + 1) % neighbour.vertCount;
if ((samePosition(tile.verts, node.verts[i], neighbourTile.verts, neighbour.verts[l])
&& samePosition(tile.verts, node.verts[j], neighbourTile.verts, neighbour.verts[k]))
|| (samePosition(tile.verts, node.verts[i], neighbourTile.verts, neighbour.verts[k])
&& samePosition(tile.verts, node.verts[j], neighbourTile.verts, neighbour.verts[l]))) {
return i;
}
}
}
return -1;
}

private static boolean samePosition(float[] verts, int v, float[] verts2, int v2) {
for (int i = 0; i < 3; i++) {
if (verts[3 * v + i] != verts2[3 * v2 + 1]) {
return false;
}
}
return true;
}

/**
* Find edge closest to the given coordinate
*/
public static int findEdge(Poly node, MeshData tile, float value, int comp) {
float error = Float.MAX_VALUE;
int edge = 0;
for (int i = 0; i < node.vertCount; i++) {
int j = (i + 1) % node.vertCount;
float v1 = tile.verts[3 * node.verts[i] + comp] - value;
float v2 = tile.verts[3 * node.verts[j] + comp] - value;
float d = v1 * v1 + v2 * v2;
if (d < error) {
error = d;
edge = i;
}
}
return edge;
}
}
@@ -0,0 +1,16 @@
package org.recast4j.detour.extras;

public class Vector3f {

public float x, y, z;

public Vector3f() {
}

public Vector3f(float x, float y, float z) {
this.x = x;
this.y = y;
this.z = z;
}

}
@@ -0,0 +1,47 @@
package org.recast4j.detour.extras.unity.astar;

import static org.recast4j.detour.DetourCommon.clamp;
import static org.recast4j.detour.DetourCommon.vCopy;
import static org.recast4j.detour.DetourCommon.vMax;
import static org.recast4j.detour.DetourCommon.vMin;

import org.recast4j.detour.BVNode;
import org.recast4j.detour.MeshData;
import org.recast4j.detour.NavMeshBuilder;
import org.recast4j.detour.NavMeshBuilder.BVItem;

public class BVTreeBuilder {

void build(GraphMeshData graphData, GraphMeta meta) {
for (MeshData d : graphData.tiles) {
d.bvTree = new BVNode[d.header.polyCount * 2];
d.header.bvNodeCount = createBVTree(d, d.bvTree, meta.cellSize);
}
}

private static int createBVTree(MeshData data, BVNode[] nodes, float cs) {
float quantFactor = 1 / cs;
BVItem[] items = new BVItem[data.header.polyCount];
for (int i = 0; i < data.header.polyCount; i++) {
BVItem it = new BVItem();
items[i] = it;
it.i = i;
float[] bmin = new float[3];
float[] bmax = new float[3];
vCopy(bmin, data.verts, data.polys[i].verts[0] * 3);
vCopy(bmax, data.verts, data.polys[i].verts[0] * 3);
for (int j = 1; j < data.polys[i].vertCount; j++) {
vMin(bmin, data.verts, data.polys[i].verts[j] * 3);
vMax(bmax, data.verts, data.polys[i].verts[j] * 3);
}
it.bmin[0] = clamp((int) ((bmin[0] - data.header.bmin[0]) * quantFactor), 0, 0xffff);
it.bmin[1] = clamp((int) ((bmin[1] - data.header.bmin[1]) * quantFactor), 0, 0xffff);
it.bmin[2] = clamp((int) ((bmin[2] - data.header.bmin[2]) * quantFactor), 0, 0xffff);
it.bmax[0] = clamp((int) ((bmax[0] - data.header.bmin[0]) * quantFactor), 0, 0xffff);
it.bmax[1] = clamp((int) ((bmax[1] - data.header.bmin[1]) * quantFactor), 0, 0xffff);
it.bmax[2] = clamp((int) ((bmax[2] - data.header.bmin[2]) * quantFactor), 0, 0xffff);
}
return NavMeshBuilder.subdivide(items, data.header.polyCount, 0, data.header.polyCount, 0, nodes);
}

}
@@ -0,0 +1,20 @@
package org.recast4j.detour.extras.unity.astar;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.zip.ZipEntry;
import java.util.zip.ZipFile;

import org.recast4j.detour.io.IOUtils;

abstract class BinaryReader {

protected ByteBuffer toByteBuffer(ZipFile file, String filename) throws IOException {
ZipEntry graphReferences = file.getEntry(filename);
ByteBuffer buffer = IOUtils.toByteBuffer(file.getInputStream(graphReferences));
buffer.order(ByteOrder.LITTLE_ENDIAN);
return buffer;
}

}
@@ -0,0 +1,29 @@
package org.recast4j.detour.extras.unity.astar;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.List;
import java.util.zip.ZipFile;

class GraphConnectionReader extends BinaryReader {

@SuppressWarnings("unused")
List<int[]> read(ZipFile file, String filename, GraphMeta meta, int[] indexToNode) throws IOException {
List<int[]> connections = new ArrayList<>();
ByteBuffer buffer = toByteBuffer(file, filename);
while (buffer.remaining() > 0) {
int count = buffer.getInt();
int [] nodeConnections = new int[count];
connections.add(nodeConnections);
for (int i = 0; i < count; i++) {
int nodeIndex = buffer.getInt();
nodeConnections[i] = indexToNode[nodeIndex];
// XXX: Is there anything we can do with the cost?
int cost = buffer.getInt();
}
}
return connections;
}

}
@@ -0,0 +1,49 @@
package org.recast4j.detour.extras.unity.astar;

import org.recast4j.detour.MeshData;
import org.recast4j.detour.Poly;

class GraphMeshData {

final int tileXCount;
final int tileZCount;

final MeshData[] tiles;

GraphMeshData(int tileXCount, int tileZCount, MeshData[] tiles) {
this.tileXCount = tileXCount;
this.tileZCount = tileZCount;
this.tiles = tiles;
}

int countNodes() {
int polyCount = 0;
for (MeshData t : tiles) {
polyCount += t.header.polyCount;
}
return polyCount;
}

public Poly getNode(int node) {
int index = 0;
for (MeshData t : tiles) {
if (node - index >= 0 && node - index < t.header.polyCount) {
return t.polys[node - index];
}
index += t.header.polyCount;
}
return null;
}

public MeshData getTile(int node) {
int index = 0;
for (MeshData t : tiles) {
if (node - index >= 0 && node - index < t.header.polyCount) {
return t;
}
index += t.header.polyCount;
}
return null;
}

}
@@ -0,0 +1,120 @@
package org.recast4j.detour.extras.unity.astar;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.zip.ZipFile;

import org.recast4j.detour.MeshData;
import org.recast4j.detour.MeshHeader;
import org.recast4j.detour.Poly;
import org.recast4j.detour.PolyDetail;

class GraphMeshDataReader extends BinaryReader {

static final float INT_PRECISION_FACTOR = 1000f;

@SuppressWarnings("unused")
GraphMeshData read(ZipFile file, String filename, GraphMeta meta, int maxVertPerPoly) throws IOException {
ByteBuffer buffer = toByteBuffer(file, filename);
int tileXCount = buffer.getInt();
if (tileXCount < 0)
return null;
int tileZCount = buffer.getInt();
MeshData[] tiles = new MeshData[tileXCount * tileZCount];
for (int z = 0; z < tileZCount; z++) {
for (int x = 0; x < tileXCount; x++) {
int tileIndex = x + z * tileXCount;
int tx = buffer.getInt();
int tz = buffer.getInt();
if (tx != x || tz != z) {
throw new IllegalArgumentException("Inconsistent tile positions");
}

tiles[tileIndex] = new MeshData();
int width = buffer.getInt();
int depth = buffer.getInt();

int trisCount = buffer.getInt();
int[] tris = new int[trisCount];
for (int i = 0; i < tris.length; i++) {
tris[i] = buffer.getInt();
}

int vertsCount = buffer.getInt();
float[] verts = new float[3 * vertsCount];
for (int i = 0; i < verts.length; i++) {
verts[i] = buffer.getInt() / INT_PRECISION_FACTOR;
}

int[] vertsInGraphSpace = new int[3 * buffer.getInt()];
for (int i = 0; i < vertsInGraphSpace.length; i++) {
vertsInGraphSpace[i] = buffer.getInt();
}

int nodeCount = buffer.getInt();
Poly[] nodes = new Poly[nodeCount];
PolyDetail[] detailNodes = new PolyDetail[nodeCount];
float[] detailVerts = new float[0];
int[] detailTris = new int[4 * nodeCount];
int vertMask = getVertMask(vertsCount);
for (int i = 0; i < nodes.length; i++) {
nodes[i] = new Poly(i, maxVertPerPoly);
nodes[i].vertCount = 3;
// XXX: What can we do with the penalty?
int penalty = buffer.getInt();
nodes[i].flags = buffer.getInt();
nodes[i].verts[0] = buffer.getInt() & vertMask;
nodes[i].verts[1] = buffer.getInt() & vertMask;
nodes[i].verts[2] = buffer.getInt() & vertMask;
// XXX: Detail mesh is not needed by recast4j, but RecastDemo will crash without it
detailNodes[i] = new PolyDetail();
detailNodes[i].vertBase = 0;
detailNodes[i].vertCount = 0;
detailNodes[i].triBase = i;
detailNodes[i].triCount = 1;
detailTris[4 * i] = 0;
detailTris[4 * i + 1] = 1;
detailTris[4 * i + 2] = 2;
// Bit for each edge that belongs to poly boundary, basically all edges marked as boundary as it is a triangle
detailTris[4 * i + 3] = (1 << 4) | (1 << 2) | 1;
}

tiles[tileIndex].verts = verts;
tiles[tileIndex].polys = nodes;
tiles[tileIndex].detailMeshes = detailNodes;
tiles[tileIndex].detailVerts = detailVerts;
tiles[tileIndex].detailTris = detailTris;
MeshHeader header = new MeshHeader();
header.magic = MeshHeader.DT_NAVMESH_MAGIC;
header.version = MeshHeader.DT_NAVMESH_VERSION;
header.x = x;
header.y = z;
header.polyCount = nodeCount;
header.vertCount = vertsCount;
header.detailMeshCount = nodeCount;
header.detailTriCount = nodeCount;
header.maxLinkCount = nodeCount * 3 * 2; // XXX: Needed by Recast, not needed by recast4j
header.bmin[0] = meta.forcedBoundsCenter.x + meta.forcedBoundsSize.x * ((float) x / tileXCount - 0.5f);
header.bmin[1] = -0.5f * meta.forcedBoundsSize.y + meta.forcedBoundsCenter.y;
header.bmin[2] = meta.forcedBoundsCenter.z + meta.forcedBoundsSize.z * ((float) z / tileZCount - 0.5f);
header.bmax[0] = meta.forcedBoundsCenter.x + meta.forcedBoundsSize.x * ((x + 1f) / tileXCount - 0.5f);
header.bmax[1] = 0.5f * meta.forcedBoundsSize.y + meta.forcedBoundsCenter.y;
header.bmax[2] = meta.forcedBoundsCenter.z + meta.forcedBoundsSize.z * ((z + 1f) / tileZCount - 0.5f);
header.bvQuantFactor = 1.0f / meta.cellSize;
header.offMeshBase = nodeCount;
tiles[tileIndex].header = header;
}
}
return new GraphMeshData(tileXCount, tileZCount, tiles);
}

private int getVertMask(int vertsCount) {
int vertMask = Integer.highestOneBit(vertsCount);
if (vertMask != vertsCount) {
vertMask *= 2;
}
vertMask--;
return vertMask;
}

}
Oops, something went wrong.

0 comments on commit 23781a2

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