diff --git a/src/DotRecast.Core/RcMatrix4x4f.cs b/src/DotRecast.Core/RcMatrix4x4f.cs index 5d66bb5..7dbdcf5 100644 --- a/src/DotRecast.Core/RcMatrix4x4f.cs +++ b/src/DotRecast.Core/RcMatrix4x4f.cs @@ -1,22 +1,106 @@ namespace DotRecast.Core { - public struct RcMatrix4x4 + public struct RcMatrix4x4f { - public float m11; - public float m12; - public float m13; - public float m14; - public float m21; - public float m22; - public float m23; - public float m24; - public float m31; - public float m32; - public float m33; - public float m34; - public float m41; - public float m42; - public float m43; - public float m44; + private static readonly RcMatrix4x4f _identity = new RcMatrix4x4f + ( + 1f, 0f, 0f, 0f, + 0f, 1f, 0f, 0f, + 0f, 0f, 1f, 0f, + 0f, 0f, 0f, 1f + ); + + public float M11; + public float M12; + public float M13; + public float M14; + public float M21; + public float M22; + public float M23; + public float M24; + public float M31; + public float M32; + public float M33; + public float M34; + public float M41; + public float M42; + public float M43; + public float M44; + + public RcMatrix4x4f( + float m11, float m12, float m13, float m14, + float m21, float m22, float m23, float m24, + float m31, float m32, float m33, float m34, + float m41, float m42, float m43, float m44) + { + M11 = m11; + M12 = m12; + M13 = m13; + M14 = m14; + + M21 = m21; + M22 = m22; + M23 = m23; + M24 = m24; + + M31 = m31; + M32 = m32; + M33 = m33; + M34 = m34; + + M41 = m41; + M42 = m42; + M43 = m43; + M44 = m44; + } + + public static RcMatrix4x4f Identity => _identity; + + public readonly bool IsIdentity => + M11.Equals(1f) && M22.Equals(1f) && M33.Equals(1f) && M44.Equals(1f) && + M12 == 0f && M13 == 0f && M14 == 0f && + M21 == 0f && M23 == 0f && M24 == 0f && + M31 == 0f && M32 == 0f && M34 == 0f && + M41 == 0f && M42 == 0f && M43 == 0f; + + public static RcMatrix4x4f Mul(RcMatrix4x4f left, RcMatrix4x4f right) + { + float m11 = left.M11 * right.M11 + left.M21 * right.M12 + left.M31 * right.M13 + left.M41 * right.M14; + float m12 = left.M12 * right.M11 + left.M22 * right.M12 + left.M32 * right.M13 + left.M42 * right.M14; + float m13 = left.M13 * right.M11 + left.M23 * right.M12 + left.M33 * right.M13 + left.M43 * right.M14; + float m14 = left.M14 * right.M11 + left.M24 * right.M12 + left.M34 * right.M13 + left.M44 * right.M14; + float m21 = left.M11 * right.M21 + left.M21 * right.M22 + left.M31 * right.M23 + left.M41 * right.M24; + float m22 = left.M12 * right.M21 + left.M22 * right.M22 + left.M32 * right.M23 + left.M42 * right.M24; + float m23 = left.M13 * right.M21 + left.M23 * right.M22 + left.M33 * right.M23 + left.M43 * right.M24; + float m24 = left.M14 * right.M21 + left.M24 * right.M22 + left.M34 * right.M23 + left.M44 * right.M24; + float m31 = left.M11 * right.M31 + left.M21 * right.M32 + left.M31 * right.M33 + left.M41 * right.M34; + float m32 = left.M12 * right.M31 + left.M22 * right.M32 + left.M32 * right.M33 + left.M42 * right.M34; + float m33 = left.M13 * right.M31 + left.M23 * right.M32 + left.M33 * right.M33 + left.M43 * right.M34; + float m34 = left.M14 * right.M31 + left.M24 * right.M32 + left.M34 * right.M33 + left.M44 * right.M34; + float m41 = left.M11 * right.M41 + left.M21 * right.M42 + left.M31 * right.M43 + left.M41 * right.M44; + float m42 = left.M12 * right.M41 + left.M22 * right.M42 + left.M32 * right.M43 + left.M42 * right.M44; + float m43 = left.M13 * right.M41 + left.M23 * right.M42 + left.M33 * right.M43 + left.M43 * right.M44; + float m44 = left.M14 * right.M41 + left.M24 * right.M42 + left.M34 * right.M43 + left.M44 * right.M44; + + RcMatrix4x4f dest = new RcMatrix4x4f(); + dest.M11 = m11; + dest.M12 = m12; + dest.M13 = m13; + dest.M14 = m14; + dest.M21 = m21; + dest.M22 = m22; + dest.M23 = m23; + dest.M24 = m24; + dest.M31 = m31; + dest.M32 = m32; + dest.M33 = m33; + dest.M34 = m34; + dest.M41 = m41; + dest.M42 = m42; + dest.M43 = m43; + dest.M44 = m44; + + return dest; + } } } \ No newline at end of file diff --git a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateSampleTool.cs b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateSampleTool.cs index 865ebc8..44ad4a8 100644 --- a/src/DotRecast.Recast.Demo/Tools/DynamicUpdateSampleTool.cs +++ b/src/DotRecast.Recast.Demo/Tools/DynamicUpdateSampleTool.cs @@ -83,10 +83,7 @@ public class DynamicUpdateSampleTool : ISampleTool private DynamicNavMesh dynaMesh; private readonly TaskFactory executor; private readonly Dictionary colliderGizmos = new(); - private readonly Random random = Random.Shared; - private readonly DemoInputGeomProvider bridgeGeom; - private readonly DemoInputGeomProvider houseGeom; - private readonly DemoInputGeomProvider convexGeom; + private bool sposSet; private bool eposSet; private RcVec3f spos; @@ -96,11 +93,11 @@ public class DynamicUpdateSampleTool : ISampleTool public DynamicUpdateSampleTool() { - _tool = new(); + var bridgeGeom = DemoObjImporter.Load(Loader.ToBytes("bridge.obj")); + var houseGeom = DemoObjImporter.Load(Loader.ToBytes("house.obj")); + var convexGeom = DemoObjImporter.Load(Loader.ToBytes("convex.obj")); + _tool = new(Random.Shared, bridgeGeom, houseGeom, convexGeom); executor = Task.Factory; - bridgeGeom = DemoObjImporter.Load(Loader.ToBytes("bridge.obj")); - houseGeom = DemoObjImporter.Load(Loader.ToBytes("house.obj")); - convexGeom = DemoObjImporter.Load(Loader.ToBytes("convex.obj")); } public void Layout() @@ -371,35 +368,35 @@ public class DynamicUpdateSampleTool : ISampleTool { if (colliderShape == DynamicColliderShape.SPHERE) { - colliderWithGizmo = SphereCollider(p); + colliderWithGizmo = _tool.SphereCollider(p, dynaMesh.config.walkableClimb); } else if (colliderShape == DynamicColliderShape.CAPSULE) { - colliderWithGizmo = CapsuleCollider(p); + colliderWithGizmo = _tool.CapsuleCollider(p, dynaMesh.config.walkableClimb); } else if (colliderShape == DynamicColliderShape.BOX) { - colliderWithGizmo = BoxCollider(p); + colliderWithGizmo = _tool.BoxCollider(p, dynaMesh.config.walkableClimb); } else if (colliderShape == DynamicColliderShape.CYLINDER) { - colliderWithGizmo = CylinderCollider(p); + colliderWithGizmo = _tool.CylinderCollider(p, dynaMesh.config.walkableClimb); } else if (colliderShape == DynamicColliderShape.COMPOSITE) { - colliderWithGizmo = CompositeCollider(p); + colliderWithGizmo = _tool.CompositeCollider(p, dynaMesh.config.walkableClimb); } else if (colliderShape == DynamicColliderShape.TRIMESH_BRIDGE) { - colliderWithGizmo = TrimeshBridge(p); + colliderWithGizmo = _tool.TrimeshBridge(p, dynaMesh.config.walkableClimb); } else if (colliderShape == DynamicColliderShape.TRIMESH_HOUSE) { - colliderWithGizmo = TrimeshHouse(p); + colliderWithGizmo = _tool.TrimeshHouse(p, dynaMesh.config.walkableClimb); } else if (colliderShape == DynamicColliderShape.CONVEX) { - colliderWithGizmo = ConvexTrimesh(p); + colliderWithGizmo = _tool.ConvexTrimesh(p, dynaMesh.config.walkableClimb); } } @@ -440,177 +437,6 @@ public class DynamicUpdateSampleTool : ISampleTool } } - private RcGizmo SphereCollider(RcVec3f p) - { - float radius = 1 + (float)random.NextDouble() * 10; - var collider = new SphereCollider(p, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb); - var gizmo = GizmoFactory.Sphere(p, radius); - - return new RcGizmo(collider, gizmo); - } - - private RcGizmo CapsuleCollider(RcVec3f p) - { - float radius = 0.4f + (float)random.NextDouble() * 4f; - RcVec3f a = RcVec3f.Of( - (1f - 2 * (float)random.NextDouble()), - 0.01f + (float)random.NextDouble(), - (1f - 2 * (float)random.NextDouble()) - ); - a.Normalize(); - float len = 1f + (float)random.NextDouble() * 20f; - a.x *= len; - a.y *= len; - a.z *= len; - RcVec3f start = RcVec3f.Of(p.x, p.y, p.z); - RcVec3f end = RcVec3f.Of(p.x + a.x, p.y + a.y, p.z + a.z); - var collider = new CapsuleCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb); - var gizmo = GizmoFactory.Capsule(start, end, radius); - return new RcGizmo(collider, gizmo); - } - - private RcGizmo BoxCollider(RcVec3f p) - { - RcVec3f extent = RcVec3f.Of( - 0.5f + (float)random.NextDouble() * 6f, - 0.5f + (float)random.NextDouble() * 6f, - 0.5f + (float)random.NextDouble() * 6f - ); - RcVec3f forward = RcVec3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble())); - RcVec3f up = RcVec3f.Of((1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble())); - RcVec3f[] halfEdges = Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(up, forward, extent); - var collider = new BoxCollider(p, halfEdges, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb); - var gizmo = GizmoFactory.Box(p, halfEdges); - return new RcGizmo(collider, gizmo); - } - - private RcGizmo CylinderCollider(RcVec3f p) - { - float radius = 0.7f + (float)random.NextDouble() * 4f; - RcVec3f a = RcVec3f.Of(1f - 2 * (float)random.NextDouble(), 0.01f + (float)random.NextDouble(), 1f - 2 * (float)random.NextDouble()); - a.Normalize(); - float len = 2f + (float)random.NextDouble() * 20f; - a[0] *= len; - a[1] *= len; - a[2] *= len; - RcVec3f start = RcVec3f.Of(p.x, p.y, p.z); - RcVec3f end = RcVec3f.Of(p.x + a.x, p.y + a.y, p.z + a.z); - var collider = new CylinderCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb); - var gizmo = GizmoFactory.Cylinder(start, end, radius); - - return new RcGizmo(collider, gizmo); - } - - private RcGizmo CompositeCollider(RcVec3f p) - { - RcVec3f baseExtent = RcVec3f.Of(5, 3, 8); - RcVec3f baseCenter = RcVec3f.Of(p.x, p.y + 3, p.z); - RcVec3f baseUp = RcVec3f.Of(0, 1, 0); - RcVec3f forward = RcVec3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble())); - forward.Normalize(); - RcVec3f side = RcVec3f.Cross(forward, baseUp); - BoxCollider @base = new BoxCollider(baseCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(baseUp, forward, baseExtent), - SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb); - var roofUp = RcVec3f.Zero; - RcVec3f roofExtent = RcVec3f.Of(4.5f, 4.5f, 8f); - float[] rx = GLU.Build_4x4_rotation_matrix(45, forward.x, forward.y, forward.z); - roofUp = MulMatrixVector(ref roofUp, rx, baseUp); - RcVec3f roofCenter = RcVec3f.Of(p.x, p.y + 6, p.z); - BoxCollider roof = new BoxCollider(roofCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(roofUp, forward, roofExtent), - SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb); - RcVec3f trunkStart = RcVec3f.Of( - baseCenter.x - forward.x * 15 + side.x * 6, - p.y, - baseCenter.z - forward.z * 15 + side.z * 6 - ); - RcVec3f trunkEnd = RcVec3f.Of(trunkStart.x, trunkStart.y + 10, trunkStart.z); - CapsuleCollider trunk = new CapsuleCollider(trunkStart, trunkEnd, 0.5f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, - dynaMesh.config.walkableClimb); - RcVec3f crownCenter = RcVec3f.Of( - baseCenter.x - forward.x * 15 + side.x * 6, p.y + 10, - baseCenter.z - forward.z * 15 + side.z * 6 - ); - SphereCollider crown = new SphereCollider(crownCenter, 4f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GRASS, - dynaMesh.config.walkableClimb); - CompositeCollider collider = new CompositeCollider(@base, roof, trunk, crown); - IRcGizmoMeshFilter baseGizmo = GizmoFactory.Box(baseCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(baseUp, forward, baseExtent)); - IRcGizmoMeshFilter roofGizmo = GizmoFactory.Box(roofCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(roofUp, forward, roofExtent)); - IRcGizmoMeshFilter trunkGizmo = GizmoFactory.Capsule(trunkStart, trunkEnd, 0.5f); - IRcGizmoMeshFilter crownGizmo = GizmoFactory.Sphere(crownCenter, 4f); - IRcGizmoMeshFilter gizmo = GizmoFactory.Composite(baseGizmo, roofGizmo, trunkGizmo, crownGizmo); - return new RcGizmo(collider, gizmo); - } - - private RcGizmo TrimeshBridge(RcVec3f p) - { - return TrimeshCollider(p, bridgeGeom); - } - - private RcGizmo TrimeshHouse(RcVec3f p) - { - return TrimeshCollider(p, houseGeom); - } - - private RcGizmo ConvexTrimesh(RcVec3f p) - { - float[] verts = TransformVertices(p, convexGeom, 360); - var collider = new ConvexTrimeshCollider(verts, convexGeom.faces, - SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb * 10); - var gizmo = GizmoFactory.Trimesh(verts, convexGeom.faces); - return new RcGizmo(collider, gizmo); - } - - private RcGizmo TrimeshCollider(RcVec3f p, DemoInputGeomProvider geom) - { - float[] verts = TransformVertices(p, geom, 0); - var collider = new TrimeshCollider(verts, geom.faces, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, - dynaMesh.config.walkableClimb * 10); - var gizmo = GizmoFactory.Trimesh(verts, geom.faces); - - return new RcGizmo(collider, gizmo); - } - - private float[] TransformVertices(RcVec3f p, DemoInputGeomProvider geom, float ax) - { - float[] rx = GLU.Build_4x4_rotation_matrix((float)random.NextDouble() * ax, 1, 0, 0); - float[] ry = GLU.Build_4x4_rotation_matrix((float)random.NextDouble() * 360, 0, 1, 0); - float[] m = GLU.Mul(rx, ry); - float[] verts = new float[geom.vertices.Length]; - RcVec3f v = new RcVec3f(); - RcVec3f vr = new RcVec3f(); - 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 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 RcVec3f MulMatrixVector(ref RcVec3f resultvector, float[] matrix, RcVec3f pvector) - { - resultvector.x = matrix[0] * pvector.x + matrix[4] * pvector.y + matrix[8] * pvector.z; - resultvector.y = matrix[1] * pvector.x + matrix[5] * pvector.y + matrix[9] * pvector.z; - resultvector.z = matrix[2] * pvector.x + matrix[6] * pvector.y + matrix[10] * pvector.z; - return resultvector; - } - public void HandleClickRay(RcVec3f start, RcVec3f dir, bool shift) { diff --git a/src/DotRecast.Recast.Demo/UI/ImFilePicker.cs b/src/DotRecast.Recast.Demo/UI/ImFilePicker.cs index 1e4ce78..9ab2547 100644 --- a/src/DotRecast.Recast.Demo/UI/ImFilePicker.cs +++ b/src/DotRecast.Recast.Demo/UI/ImFilePicker.cs @@ -1,9 +1,10 @@ using System; -using System.Numerics; + using System.Collections.Generic; using System.Collections.Immutable; using System.Drawing; using System.IO; +using System.Numerics; using ImGuiNET; using Serilog; diff --git a/src/DotRecast.Recast.Demo/UI/RcCanvas.cs b/src/DotRecast.Recast.Demo/UI/RcCanvas.cs index c2ba9e0..2e6c756 100644 --- a/src/DotRecast.Recast.Demo/UI/RcCanvas.cs +++ b/src/DotRecast.Recast.Demo/UI/RcCanvas.cs @@ -16,7 +16,7 @@ freely, subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -using System.Numerics; + using ImGuiNET; using Serilog; using Serilog.Core; diff --git a/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs index a3abf8e..bda8caa 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs @@ -1,11 +1,30 @@ -using System.IO; +using System; +using System.IO; + using DotRecast.Core; +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 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.random = rand; + this.bridgeGeom = bridgeGeom; + this.houseGeom = houseGeom; + this.convexGeom = convexGeom; + } + public string GetName() { return "Dynamic Updates"; @@ -28,5 +47,216 @@ namespace DotRecast.Recast.Toolset.Tools VoxelFileWriter writer = new VoxelFileWriter(compressor); writer.Write(bw, voxelFile, compression); } + + + public RcGizmo SphereCollider(RcVec3f p, float walkableClimb) + { + float radius = 1 + (float)random.NextDouble() * 10; + var collider = new SphereCollider(p, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb); + var gizmo = GizmoFactory.Sphere(p, radius); + + return new RcGizmo(collider, gizmo); + } + + public RcGizmo CapsuleCollider(RcVec3f p, float walkableClimb) + { + float radius = 0.4f + (float)random.NextDouble() * 4f; + RcVec3f a = RcVec3f.Of( + (1f - 2 * (float)random.NextDouble()), + 0.01f + (float)random.NextDouble(), + (1f - 2 * (float)random.NextDouble()) + ); + a.Normalize(); + float len = 1f + (float)random.NextDouble() * 20f; + a.x *= len; + a.y *= len; + a.z *= len; + RcVec3f start = RcVec3f.Of(p.x, p.y, p.z); + RcVec3f end = RcVec3f.Of(p.x + a.x, p.y + a.y, p.z + a.z); + var collider = new CapsuleCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb); + var gizmo = GizmoFactory.Capsule(start, end, radius); + return new RcGizmo(collider, gizmo); + } + + public RcGizmo BoxCollider(RcVec3f p, float walkableClimb) + { + RcVec3f extent = RcVec3f.Of( + 0.5f + (float)random.NextDouble() * 6f, + 0.5f + (float)random.NextDouble() * 6f, + 0.5f + (float)random.NextDouble() * 6f + ); + RcVec3f forward = RcVec3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble())); + RcVec3f up = RcVec3f.Of((1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble())); + RcVec3f[] halfEdges = Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(up, forward, extent); + var collider = new BoxCollider(p, halfEdges, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb); + var gizmo = GizmoFactory.Box(p, halfEdges); + return new RcGizmo(collider, gizmo); + } + + public RcGizmo CylinderCollider(RcVec3f p, float walkableClimb) + { + float radius = 0.7f + (float)random.NextDouble() * 4f; + RcVec3f a = RcVec3f.Of(1f - 2 * (float)random.NextDouble(), 0.01f + (float)random.NextDouble(), 1f - 2 * (float)random.NextDouble()); + a.Normalize(); + float len = 2f + (float)random.NextDouble() * 20f; + a[0] *= len; + a[1] *= len; + a[2] *= len; + RcVec3f start = RcVec3f.Of(p.x, p.y, p.z); + RcVec3f end = RcVec3f.Of(p.x + a.x, p.y + a.y, p.z + a.z); + var collider = new CylinderCollider(start, end, radius, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, walkableClimb); + var gizmo = GizmoFactory.Cylinder(start, end, radius); + + return new RcGizmo(collider, gizmo); + } + + public RcGizmo CompositeCollider(RcVec3f p, float walkableClimb) + { + RcVec3f baseExtent = RcVec3f.Of(5, 3, 8); + RcVec3f baseCenter = RcVec3f.Of(p.x, p.y + 3, p.z); + RcVec3f baseUp = RcVec3f.Of(0, 1, 0); + RcVec3f forward = RcVec3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble())); + forward.Normalize(); + RcVec3f side = RcVec3f.Cross(forward, baseUp); + BoxCollider @base = new BoxCollider(baseCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(baseUp, forward, baseExtent), + SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb); + var roofUp = RcVec3f.Zero; + RcVec3f roofExtent = RcVec3f.Of(4.5f, 4.5f, 8f); + var rx = Matrix(45, forward.x, forward.y, forward.z); + roofUp = MulMatrixVector(ref roofUp, rx, baseUp); + RcVec3f roofCenter = RcVec3f.Of(p.x, p.y + 6, p.z); + BoxCollider roof = new BoxCollider(roofCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(roofUp, forward, roofExtent), + SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb); + RcVec3f trunkStart = RcVec3f.Of( + baseCenter.x - forward.x * 15 + side.x * 6, + p.y, + baseCenter.z - forward.z * 15 + side.z * 6 + ); + RcVec3f trunkEnd = RcVec3f.Of(trunkStart.x, trunkStart.y + 10, trunkStart.z); + CapsuleCollider trunk = new CapsuleCollider(trunkStart, trunkEnd, 0.5f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, + walkableClimb); + RcVec3f crownCenter = RcVec3f.Of( + baseCenter.x - forward.x * 15 + side.x * 6, p.y + 10, + baseCenter.z - forward.z * 15 + side.z * 6 + ); + SphereCollider crown = new SphereCollider(crownCenter, 4f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GRASS, + walkableClimb); + CompositeCollider collider = new CompositeCollider(@base, roof, trunk, crown); + IRcGizmoMeshFilter baseGizmo = GizmoFactory.Box(baseCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(baseUp, forward, baseExtent)); + IRcGizmoMeshFilter roofGizmo = GizmoFactory.Box(roofCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(roofUp, forward, roofExtent)); + IRcGizmoMeshFilter trunkGizmo = GizmoFactory.Capsule(trunkStart, trunkEnd, 0.5f); + IRcGizmoMeshFilter crownGizmo = GizmoFactory.Sphere(crownCenter, 4f); + IRcGizmoMeshFilter gizmo = GizmoFactory.Composite(baseGizmo, roofGizmo, trunkGizmo, crownGizmo); + return new RcGizmo(collider, gizmo); + } + + public RcGizmo TrimeshBridge(RcVec3f p, float walkableClimb) + { + return TrimeshCollider(p, bridgeGeom, walkableClimb); + } + + public RcGizmo TrimeshHouse(RcVec3f p, float walkableClimb) + { + return TrimeshCollider(p, houseGeom, walkableClimb); + } + + public RcGizmo ConvexTrimesh(RcVec3f p, float walkableClimb) + { + float[] verts = TransformVertices(p, convexGeom, 360); + var collider = new ConvexTrimeshCollider(verts, convexGeom.faces, + SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb * 10); + var gizmo = GizmoFactory.Trimesh(verts, convexGeom.faces); + return new RcGizmo(collider, gizmo); + } + + private RcGizmo TrimeshCollider(RcVec3f p, DemoInputGeomProvider geom, float walkableClimb) + { + float[] verts = TransformVertices(p, geom, 0); + var collider = new TrimeshCollider(verts, geom.faces, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, + walkableClimb * 10); + var gizmo = GizmoFactory.Trimesh(verts, geom.faces); + + return new RcGizmo(collider, gizmo); + } + + private float[] TransformVertices(RcVec3f p, DemoInputGeomProvider geom, float ax) + { + var rx = Matrix((float)random.NextDouble() * ax, 1, 0, 0); + var ry = Matrix((float)random.NextDouble() * 360, 0, 1, 0); + var m = RcMatrix4x4f.Mul(rx, ry); + float[] verts = new float[geom.vertices.Length]; + RcVec3f v = new RcVec3f(); + RcVec3f vr = new RcVec3f(); + 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 RcVec3f MulMatrixVector(ref RcVec3f resultvector, RcMatrix4x4f matrix, RcVec3f 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 static RcMatrix4x4f Matrix(float a, float x, float y, float z) + { + var matrix = new RcMatrix4x4f(); + a = (float)(a * Math.PI / 180.0); // convert to radians + float s = (float)Math.Sin(a); + float c = (float)Math.Cos(a); + float t = 1.0f - c; + + float tx = t * x; + float ty = t * y; + float tz = t * z; + + float sz = s * z; + float sy = s * y; + float sx = s * x; + + matrix.M11 = tx * x + c; + matrix.M12 = tx * y + sz; + matrix.M13 = tx * z - sy; + matrix.M14 = 0; + + matrix.M21 = tx * y - sz; + matrix.M22 = ty * y + c; + matrix.M23 = ty * z + sx; + matrix.M24 = 0; + + matrix.M31 = tx * z + sy; + matrix.M32 = ty * z - sx; + matrix.M33 = tz * z + c; + matrix.M34 = 0; + + matrix.M41 = 0; + matrix.M42 = 0; + matrix.M43 = 0; + matrix.M44 = 1; + + return matrix; + } } } \ No newline at end of file