forked from mirror/DotRecast
372 lines
16 KiB
C#
372 lines
16 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Threading.Tasks;
|
|
using DotRecast.Core;
|
|
using DotRecast.Core.Numerics;
|
|
using System.Numerics;
|
|
using DotRecast.Detour.Dynamic;
|
|
using DotRecast.Detour.Dynamic.Colliders;
|
|
using DotRecast.Detour.Dynamic.Io;
|
|
using DotRecast.Recast.Toolset.Builder;
|
|
using DotRecast.Recast.Toolset.Geom;
|
|
using DotRecast.Recast.Toolset.Gizmos;
|
|
|
|
namespace DotRecast.Recast.Toolset.Tools
|
|
{
|
|
public class RcDynamicUpdateTool : IRcToolable
|
|
{
|
|
private DtDynamicNavMesh dynaMesh;
|
|
private readonly Dictionary<long, RcGizmo> colliderGizmos;
|
|
|
|
private readonly Random random;
|
|
private readonly DemoInputGeomProvider bridgeGeom;
|
|
private readonly DemoInputGeomProvider houseGeom;
|
|
private readonly DemoInputGeomProvider convexGeom;
|
|
|
|
public RcDynamicUpdateTool(Random rand, DemoInputGeomProvider bridgeGeom, DemoInputGeomProvider houseGeom, DemoInputGeomProvider convexGeom)
|
|
{
|
|
this.colliderGizmos = new Dictionary<long, RcGizmo>();
|
|
this.random = rand;
|
|
this.bridgeGeom = bridgeGeom;
|
|
this.houseGeom = houseGeom;
|
|
this.convexGeom = convexGeom;
|
|
}
|
|
|
|
public IEnumerable<RcGizmo> GetGizmos()
|
|
{
|
|
return colliderGizmos.Values;
|
|
}
|
|
|
|
public string GetName()
|
|
{
|
|
return "Dynamic Updates";
|
|
}
|
|
|
|
public DtDynamicNavMesh GetDynamicNavMesh()
|
|
{
|
|
return dynaMesh;
|
|
}
|
|
|
|
public void RemoveShape(Vector3 start, Vector3 dir)
|
|
{
|
|
foreach (var e in colliderGizmos)
|
|
{
|
|
if (Hit(start, dir, e.Value.Collider.Bounds()))
|
|
{
|
|
dynaMesh.RemoveCollider(e.Key);
|
|
colliderGizmos.Remove(e.Key);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private bool Hit(Vector3 point, Vector3 dir, float[] bounds)
|
|
{
|
|
float cx = 0.5f * (bounds[0] + bounds[3]);
|
|
float cy = 0.5f * (bounds[1] + bounds[4]);
|
|
float cz = 0.5f * (bounds[2] + bounds[5]);
|
|
float dx = 0.5f * (bounds[3] - bounds[0]);
|
|
float dy = 0.5f * (bounds[4] - bounds[1]);
|
|
float dz = 0.5f * (bounds[5] - bounds[2]);
|
|
float rSqr = dx * dx + dy * dy + dz * dz;
|
|
float mx = point.X - cx;
|
|
float my = point.Y - cy;
|
|
float mz = point.Z - cz;
|
|
float c = mx * mx + my * my + mz * mz - rSqr;
|
|
if (c <= 0.0f)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
float b = mx * dir.X + my * dir.Y + mz * dir.Z;
|
|
if (b > 0.0f)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
float disc = b * b - c;
|
|
return disc >= 0.0f;
|
|
}
|
|
|
|
|
|
public RcGizmo AddShape(RcDynamicColliderShape colliderShape, Vector3 p)
|
|
{
|
|
if (dynaMesh == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
RcGizmo colliderWithGizmo = null;
|
|
{
|
|
if (colliderShape == RcDynamicColliderShape.SPHERE)
|
|
{
|
|
colliderWithGizmo = SphereCollider(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
else if (colliderShape == RcDynamicColliderShape.CAPSULE)
|
|
{
|
|
colliderWithGizmo = CapsuleCollider(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
else if (colliderShape == RcDynamicColliderShape.BOX)
|
|
{
|
|
colliderWithGizmo = BoxCollider(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
else if (colliderShape == RcDynamicColliderShape.CYLINDER)
|
|
{
|
|
colliderWithGizmo = CylinderCollider(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
else if (colliderShape == RcDynamicColliderShape.COMPOSITE)
|
|
{
|
|
colliderWithGizmo = CompositeCollider(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
else if (colliderShape == RcDynamicColliderShape.TRIMESH_BRIDGE)
|
|
{
|
|
colliderWithGizmo = TrimeshBridge(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
else if (colliderShape == RcDynamicColliderShape.TRIMESH_HOUSE)
|
|
{
|
|
colliderWithGizmo = TrimeshHouse(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
else if (colliderShape == RcDynamicColliderShape.CONVEX)
|
|
{
|
|
colliderWithGizmo = ConvexTrimesh(p, dynaMesh.config.walkableClimb);
|
|
}
|
|
}
|
|
|
|
if (colliderWithGizmo != null)
|
|
{
|
|
long id = dynaMesh.AddCollider(colliderWithGizmo.Collider);
|
|
colliderGizmos.Add(id, colliderWithGizmo);
|
|
}
|
|
|
|
return colliderWithGizmo;
|
|
}
|
|
|
|
public DtDynamicNavMesh Load(string filename, IRcCompressor compressor)
|
|
{
|
|
using var fs = new FileStream(filename, FileMode.Open, FileAccess.Read, FileShare.Read);
|
|
using var br = new BinaryReader(fs);
|
|
DtVoxelFileReader reader = new DtVoxelFileReader(compressor);
|
|
DtVoxelFile voxelFile = reader.Read(br);
|
|
|
|
dynaMesh = new DtDynamicNavMesh(voxelFile);
|
|
dynaMesh.config.keepIntermediateResults = true;
|
|
|
|
colliderGizmos.Clear();
|
|
|
|
return dynaMesh;
|
|
}
|
|
|
|
public void Save(string filename, bool compression, IRcCompressor compressor)
|
|
{
|
|
DtVoxelFile voxelFile = DtVoxelFile.From(dynaMesh);
|
|
using var fs = new FileStream(filename, FileMode.CreateNew, FileAccess.Write);
|
|
using var bw = new BinaryWriter(fs);
|
|
DtVoxelFileWriter writer = new DtVoxelFileWriter(compressor);
|
|
writer.Write(bw, voxelFile, compression);
|
|
}
|
|
|
|
|
|
public RcGizmo SphereCollider(Vector3 p, float walkableClimb)
|
|
{
|
|
float radius = 1 + (float)random.NextDouble() * 10;
|
|
var collider = new DtSphereCollider(p, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb);
|
|
var gizmo = RcGizmoFactory.Sphere(p, radius);
|
|
|
|
return new RcGizmo(collider, gizmo);
|
|
}
|
|
|
|
public RcGizmo CapsuleCollider(Vector3 p, float walkableClimb)
|
|
{
|
|
float radius = 0.4f + (float)random.NextDouble() * 4f;
|
|
Vector3 a = new Vector3(
|
|
(1f - 2 * (float)random.NextDouble()),
|
|
0.01f + (float)random.NextDouble(),
|
|
(1f - 2 * (float)random.NextDouble())
|
|
);
|
|
a = Vector3.Normalize(a);
|
|
|
|
float len = 1f + (float)random.NextDouble() * 20f;
|
|
a.X *= len;
|
|
a.Y *= len;
|
|
a.Z *= len;
|
|
Vector3 start = new Vector3(p.X, p.Y, p.Z);
|
|
Vector3 end = new Vector3(p.X + a.X, p.Y + a.Y, p.Z + a.Z);
|
|
var collider = new DtCapsuleCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb);
|
|
var gizmo = RcGizmoFactory.Capsule(start, end, radius);
|
|
return new RcGizmo(collider, gizmo);
|
|
}
|
|
|
|
public RcGizmo BoxCollider(Vector3 p, float walkableClimb)
|
|
{
|
|
Vector3 extent = new Vector3(
|
|
0.5f + (float)random.NextDouble() * 6f,
|
|
0.5f + (float)random.NextDouble() * 6f,
|
|
0.5f + (float)random.NextDouble() * 6f
|
|
);
|
|
Vector3 forward = new Vector3((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()));
|
|
Vector3 up = new Vector3((1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()));
|
|
Vector3[] halfEdges = Detour.Dynamic.Colliders.DtBoxCollider.GetHalfEdges(up, forward, extent);
|
|
var collider = new DtBoxCollider(p, halfEdges, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb);
|
|
var gizmo = RcGizmoFactory.Box(p, halfEdges);
|
|
return new RcGizmo(collider, gizmo);
|
|
}
|
|
|
|
public RcGizmo CylinderCollider(Vector3 p, float walkableClimb)
|
|
{
|
|
float radius = 0.7f + (float)random.NextDouble() * 4f;
|
|
Vector3 a = new Vector3(1f - 2 * (float)random.NextDouble(), 0.01f + (float)random.NextDouble(), 1f - 2 * (float)random.NextDouble());
|
|
a = Vector3.Normalize(a);
|
|
float len = 2f + (float)random.NextDouble() * 20f;
|
|
a.X *= len;
|
|
a.Y *= len;
|
|
a.Z *= len;
|
|
Vector3 start = new Vector3(p.X, p.Y, p.Z);
|
|
Vector3 end = new Vector3(p.X + a.X, p.Y + a.Y, p.Z + a.Z);
|
|
var collider = new DtCylinderCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb);
|
|
var gizmo = RcGizmoFactory.Cylinder(start, end, radius);
|
|
|
|
return new RcGizmo(collider, gizmo);
|
|
}
|
|
|
|
public RcGizmo CompositeCollider(Vector3 p, float walkableClimb)
|
|
{
|
|
Vector3 baseExtent = new Vector3(5, 3, 8);
|
|
Vector3 baseCenter = new Vector3(p.X, p.Y + 3, p.Z);
|
|
Vector3 baseUp = new Vector3(0, 1, 0);
|
|
Vector3 forward = new Vector3((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()));
|
|
forward = Vector3.Normalize(forward);
|
|
|
|
Vector3 side = Vector3.Cross(forward, baseUp);
|
|
DtBoxCollider @base = new DtBoxCollider(baseCenter, Detour.Dynamic.Colliders.DtBoxCollider.GetHalfEdges(baseUp, forward, baseExtent),
|
|
SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb);
|
|
var roofUp = Vector3.Zero;
|
|
Vector3 roofExtent = new Vector3(4.5f, 4.5f, 8f);
|
|
var rx = RcMatrix4x4f.CreateFromRotate(45, forward.X, forward.Y, forward.Z);
|
|
roofUp = MulMatrixVector(ref roofUp, rx, baseUp);
|
|
Vector3 roofCenter = new Vector3(p.X, p.Y + 6, p.Z);
|
|
DtBoxCollider roof = new DtBoxCollider(roofCenter, Detour.Dynamic.Colliders.DtBoxCollider.GetHalfEdges(roofUp, forward, roofExtent),
|
|
SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb);
|
|
Vector3 trunkStart = new Vector3(
|
|
baseCenter.X - forward.X * 15 + side.X * 6,
|
|
p.Y,
|
|
baseCenter.Z - forward.Z * 15 + side.Z * 6
|
|
);
|
|
Vector3 trunkEnd = new Vector3(trunkStart.X, trunkStart.Y + 10, trunkStart.Z);
|
|
DtCapsuleCollider trunk = new DtCapsuleCollider(trunkStart, trunkEnd, 0.5f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD,
|
|
walkableClimb);
|
|
Vector3 crownCenter = new Vector3(
|
|
baseCenter.X - forward.X * 15 + side.X * 6, p.Y + 10,
|
|
baseCenter.Z - forward.Z * 15 + side.Z * 6
|
|
);
|
|
DtSphereCollider crown = new DtSphereCollider(crownCenter, 4f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GRASS,
|
|
walkableClimb);
|
|
DtCompositeCollider collider = new DtCompositeCollider(@base, roof, trunk, crown);
|
|
IRcGizmoMeshFilter baseGizmo = RcGizmoFactory.Box(baseCenter, Detour.Dynamic.Colliders.DtBoxCollider.GetHalfEdges(baseUp, forward, baseExtent));
|
|
IRcGizmoMeshFilter roofGizmo = RcGizmoFactory.Box(roofCenter, Detour.Dynamic.Colliders.DtBoxCollider.GetHalfEdges(roofUp, forward, roofExtent));
|
|
IRcGizmoMeshFilter trunkGizmo = RcGizmoFactory.Capsule(trunkStart, trunkEnd, 0.5f);
|
|
IRcGizmoMeshFilter crownGizmo = RcGizmoFactory.Sphere(crownCenter, 4f);
|
|
IRcGizmoMeshFilter gizmo = RcGizmoFactory.Composite(baseGizmo, roofGizmo, trunkGizmo, crownGizmo);
|
|
return new RcGizmo(collider, gizmo);
|
|
}
|
|
|
|
public RcGizmo TrimeshBridge(Vector3 p, float walkableClimb)
|
|
{
|
|
return TrimeshCollider(p, bridgeGeom, walkableClimb);
|
|
}
|
|
|
|
public RcGizmo TrimeshHouse(Vector3 p, float walkableClimb)
|
|
{
|
|
return TrimeshCollider(p, houseGeom, walkableClimb);
|
|
}
|
|
|
|
public RcGizmo ConvexTrimesh(Vector3 p, float walkableClimb)
|
|
{
|
|
float[] verts = TransformVertices(p, convexGeom, 360);
|
|
var collider = new DtConvexTrimeshCollider(verts, convexGeom.faces,
|
|
SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb * 10);
|
|
var gizmo = RcGizmoFactory.Trimesh(verts, convexGeom.faces);
|
|
return new RcGizmo(collider, gizmo);
|
|
}
|
|
|
|
private RcGizmo TrimeshCollider(Vector3 p, DemoInputGeomProvider geom, float walkableClimb)
|
|
{
|
|
float[] verts = TransformVertices(p, geom, 0);
|
|
var collider = new DtTrimeshCollider(verts, geom.faces, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD,
|
|
walkableClimb * 10);
|
|
var gizmo = RcGizmoFactory.Trimesh(verts, geom.faces);
|
|
|
|
return new RcGizmo(collider, gizmo);
|
|
}
|
|
|
|
private float[] TransformVertices(Vector3 p, DemoInputGeomProvider geom, float ax)
|
|
{
|
|
var rx = RcMatrix4x4f.CreateFromRotate((float)random.NextDouble() * ax, 1, 0, 0);
|
|
var ry = RcMatrix4x4f.CreateFromRotate((float)random.NextDouble() * 360, 0, 1, 0);
|
|
var m = RcMatrix4x4f.Mul(ref rx, ref ry);
|
|
float[] verts = new float[geom.vertices.Length];
|
|
Vector3 v = new Vector3();
|
|
Vector3 vr = new Vector3();
|
|
for (int i = 0; i < geom.vertices.Length; i += 3)
|
|
{
|
|
v.X = geom.vertices[i];
|
|
v.Y = geom.vertices[i + 1];
|
|
v.Z = geom.vertices[i + 2];
|
|
MulMatrixVector(ref vr, m, v);
|
|
vr.X += p.X;
|
|
vr.Y += p.Y - 0.1f;
|
|
vr.Z += p.Z;
|
|
verts[i] = vr.X;
|
|
verts[i + 1] = vr.Y;
|
|
verts[i + 2] = vr.Z;
|
|
}
|
|
|
|
return verts;
|
|
}
|
|
|
|
private static float[] MulMatrixVector(float[] resultvector, float[] matrix, float[] pvector)
|
|
{
|
|
resultvector[0] = matrix[0] * pvector[0] + matrix[4] * pvector[1] + matrix[8] * pvector[2];
|
|
resultvector[1] = matrix[1] * pvector[0] + matrix[5] * pvector[1] + matrix[9] * pvector[2];
|
|
resultvector[2] = matrix[2] * pvector[0] + matrix[6] * pvector[1] + matrix[10] * pvector[2];
|
|
return resultvector;
|
|
}
|
|
|
|
private static Vector3 MulMatrixVector(ref Vector3 resultvector, RcMatrix4x4f matrix, Vector3 pvector)
|
|
{
|
|
resultvector.X = matrix.M11 * pvector.X + matrix.M21 * pvector.Y + matrix.M31 * pvector.Z;
|
|
resultvector.Y = matrix.M12 * pvector.X + matrix.M22 * pvector.Y + matrix.M32 * pvector.Z;
|
|
resultvector.Z = matrix.M13 * pvector.X + matrix.M23 * pvector.Y + matrix.M33 * pvector.Z;
|
|
return resultvector;
|
|
}
|
|
|
|
public bool UpdateDynaMesh(TaskFactory executor)
|
|
{
|
|
if (dynaMesh == null)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
bool updated = dynaMesh.Update(executor).Result;
|
|
if (updated)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
public bool Raycast(Vector3 spos, Vector3 epos, out float hitPos, out Vector3 raycastHitPos)
|
|
{
|
|
Vector3 sp = new Vector3(spos.X, spos.Y + 1.3f, spos.Z);
|
|
Vector3 ep = new Vector3(epos.X, epos.Y + 1.3f, epos.Z);
|
|
|
|
bool hasHit = dynaMesh.VoxelQuery().Raycast(sp, ep, out hitPos);
|
|
raycastHitPos = hasHit
|
|
? new Vector3(sp.X + hitPos * (ep.X - sp.X), sp.Y + hitPos * (ep.Y - sp.Y), sp.Z + hitPos * (ep.Z - sp.Z))
|
|
: ep;
|
|
|
|
return hasHit;
|
|
}
|
|
}
|
|
} |