forked from bit/DotRecastNetSim
change float[] -> vector3f
This commit is contained in:
ikpil
parent
7c653af669
commit
00c3cd57f3
|
|
@ -19,7 +19,6 @@ freely, subject to the following restrictions:
|
|||
*/
|
||||
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace DotRecast.Core
|
||||
{
|
||||
|
|
@ -36,6 +35,14 @@ namespace DotRecast.Core
|
|||
return dx * dx + dy * dy + dz * dz;
|
||||
}
|
||||
|
||||
public static float vDistSqr(Vector3f v1, float[] v2, int i)
|
||||
{
|
||||
float dx = v2[i] - v1[0];
|
||||
float dy = v2[i + 1] - v1[1];
|
||||
float dz = v2[i + 2] - v1[2];
|
||||
return dx * dx + dy * dy + dz * dz;
|
||||
}
|
||||
|
||||
public static float vDistSqr(Vector3f v1, Vector3f v2, int i)
|
||||
{
|
||||
float dx = v2[i] - v1[0];
|
||||
|
|
@ -126,6 +133,16 @@ namespace DotRecast.Core
|
|||
dest[2] = v1[2] + v2[2] * s;
|
||||
return dest;
|
||||
}
|
||||
|
||||
public static Vector3f vMad(float[] v1, Vector3f v2, float s)
|
||||
{
|
||||
Vector3f dest = new Vector3f();
|
||||
dest[0] = v1[0] + v2[0] * s;
|
||||
dest[1] = v1[1] + v2[1] * s;
|
||||
dest[2] = v1[2] + v2[2] * s;
|
||||
return dest;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// Performs a linear interpolation between two vectors. (@p v1 toward @p
|
||||
|
|
@ -595,6 +612,16 @@ namespace DotRecast.Core
|
|||
float acz = c[2] - a[2];
|
||||
return acx * abz - abx * acz;
|
||||
}
|
||||
|
||||
public static float triArea2D(Vector3f a, float[] b, Vector3f c)
|
||||
{
|
||||
float abx = b[0] - a[0];
|
||||
float abz = b[2] - a[2];
|
||||
float acx = c[0] - a[0];
|
||||
float acz = c[2] - a[2];
|
||||
return acx * abz - abx * acz;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// Determines if two axis-aligned bounding boxes overlap.
|
||||
|
|
|
|||
|
|
@ -85,5 +85,17 @@ namespace DotRecast.Core
|
|||
{
|
||||
return new float[] { x, y, z };
|
||||
}
|
||||
|
||||
public static bool operator ==(Vector3f left, Vector3f right)
|
||||
{
|
||||
return (left.x == right.x)
|
||||
&& (left.y == right.y)
|
||||
&& (left.z == right.z);
|
||||
}
|
||||
|
||||
public static bool operator !=(Vector3f left, Vector3f right)
|
||||
{
|
||||
return !(left == right);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -25,16 +25,16 @@ namespace DotRecast.Detour.Dynamic.Colliders
|
|||
public class BoxCollider : AbstractCollider
|
||||
{
|
||||
private readonly Vector3f center;
|
||||
private readonly float[][] halfEdges;
|
||||
private readonly Vector3f[] halfEdges;
|
||||
|
||||
public BoxCollider(Vector3f center, float[][] halfEdges, int area, float flagMergeThreshold) :
|
||||
public BoxCollider(Vector3f center, Vector3f[] halfEdges, int area, float flagMergeThreshold) :
|
||||
base(area, flagMergeThreshold, bounds(center, halfEdges))
|
||||
{
|
||||
this.center = center;
|
||||
this.halfEdges = halfEdges;
|
||||
}
|
||||
|
||||
private static float[] bounds(Vector3f center, float[][] halfEdges)
|
||||
private static float[] bounds(Vector3f center, Vector3f[] halfEdges)
|
||||
{
|
||||
float[] bounds = new float[]
|
||||
{
|
||||
|
|
@ -62,11 +62,11 @@ namespace DotRecast.Detour.Dynamic.Colliders
|
|||
|
||||
public void rasterize(Heightfield hf, Telemetry telemetry)
|
||||
{
|
||||
RecastFilledVolumeRasterization.rasterizeBox(hf, center, halfEdges, area, (int)Math.Floor(flagMergeThreshold / hf.ch),
|
||||
telemetry);
|
||||
RecastFilledVolumeRasterization.rasterizeBox(
|
||||
hf, center, halfEdges, area, (int)Math.Floor(flagMergeThreshold / hf.ch), telemetry);
|
||||
}
|
||||
|
||||
public static Vector3f[] getHalfEdges(Vector3f up, Vector3f forward, float[] extent)
|
||||
public static Vector3f[] getHalfEdges(Vector3f up, Vector3f forward, Vector3f extent)
|
||||
{
|
||||
Vector3f[] halfEdges =
|
||||
{
|
||||
|
|
|
|||
|
|
@ -47,12 +47,12 @@ namespace DotRecast.Detour.Dynamic
|
|||
*
|
||||
* @return Optional with hit parameter (t) or empty if no hit found
|
||||
*/
|
||||
public float? raycast(float[] start, float[] end)
|
||||
public float? raycast(Vector3f start, Vector3f end)
|
||||
{
|
||||
return traverseTiles(start, end);
|
||||
}
|
||||
|
||||
private float? traverseTiles(float[] start, float[] end)
|
||||
private float? traverseTiles(Vector3f start, Vector3f end)
|
||||
{
|
||||
float relStartX = start[0] - origin[0];
|
||||
float relStartZ = start[2] - origin[2];
|
||||
|
|
@ -107,7 +107,7 @@ namespace DotRecast.Detour.Dynamic
|
|||
return null;
|
||||
}
|
||||
|
||||
private float? traversHeightfield(int x, int z, float[] start, float[] end, float tMin, float tMax)
|
||||
private float? traversHeightfield(int x, int z, Vector3f start, Vector3f end, float tMin, float tMax)
|
||||
{
|
||||
Heightfield hf = heightfieldProvider.Invoke(x, z);
|
||||
if (null != hf)
|
||||
|
|
|
|||
|
|
@ -399,7 +399,7 @@ public class CrowdTool : Tool
|
|||
float gridy = -float.MaxValue;
|
||||
foreach (CrowdAgent ag in crowd.getActiveAgents())
|
||||
{
|
||||
float[] pos = ag.corridor.getPos();
|
||||
Vector3f pos = ag.corridor.getPos();
|
||||
gridy = Math.Max(gridy, pos[1]);
|
||||
}
|
||||
|
||||
|
|
@ -430,12 +430,12 @@ public class CrowdTool : Tool
|
|||
foreach (CrowdAgent ag in crowd.getActiveAgents())
|
||||
{
|
||||
AgentTrail trail = m_trails[ag.idx];
|
||||
float[] pos = ag.npos;
|
||||
Vector3f pos = ag.npos;
|
||||
|
||||
dd.begin(LINES, 3.0f);
|
||||
Vector3f prev = new Vector3f();
|
||||
float preva = 1;
|
||||
vCopy(prev, pos);
|
||||
vCopy(ref prev, pos);
|
||||
for (int j = 0; j < AGENT_MAX_TRAIL - 1; ++j)
|
||||
{
|
||||
int idx = (trail.htrail + AGENT_MAX_TRAIL - j) % AGENT_MAX_TRAIL;
|
||||
|
|
@ -444,7 +444,7 @@ public class CrowdTool : Tool
|
|||
dd.vertex(prev[0], prev[1] + 0.1f, prev[2], duRGBA(0, 0, 0, (int)(128 * preva)));
|
||||
dd.vertex(trail.trail[v], trail.trail[v + 1] + 0.1f, trail.trail[v + 2], duRGBA(0, 0, 0, (int)(128 * a)));
|
||||
preva = a;
|
||||
vCopy(prev, trail.trail, v);
|
||||
vCopy(ref prev, trail.trail, v);
|
||||
}
|
||||
|
||||
dd.end();
|
||||
|
|
@ -457,7 +457,7 @@ public class CrowdTool : Tool
|
|||
continue;
|
||||
|
||||
float radius = ag.option.radius;
|
||||
float[] pos = ag.npos;
|
||||
Vector3f pos = ag.npos;
|
||||
|
||||
if (toolParams.m_showCorners)
|
||||
{
|
||||
|
|
@ -466,8 +466,8 @@ public class CrowdTool : Tool
|
|||
dd.begin(LINES, 2.0f);
|
||||
for (int j = 0; j < ag.corners.Count; ++j)
|
||||
{
|
||||
float[] va = j == 0 ? pos : ag.corners[j - 1].getPos();
|
||||
float[] vb = ag.corners[j].getPos();
|
||||
Vector3f va = j == 0 ? pos : ag.corners[j - 1].getPos();
|
||||
Vector3f vb = ag.corners[j].getPos();
|
||||
dd.vertex(va[0], va[1] + radius, va[2], duRGBA(128, 0, 0, 192));
|
||||
dd.vertex(vb[0], vb[1] + radius, vb[2], duRGBA(128, 0, 0, 192));
|
||||
}
|
||||
|
|
@ -475,7 +475,7 @@ public class CrowdTool : Tool
|
|||
if ((ag.corners[ag.corners.Count - 1].getFlags()
|
||||
& NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0)
|
||||
{
|
||||
float[] v = ag.corners[ag.corners.Count - 1].getPos();
|
||||
Vector3f v = ag.corners[ag.corners.Count - 1].getPos();
|
||||
dd.vertex(v[0], v[1], v[2], duRGBA(192, 0, 0, 192));
|
||||
dd.vertex(v[0], v[1] + radius * 2, v[2], duRGBA(192, 0, 0, 192));
|
||||
}
|
||||
|
|
@ -509,10 +509,9 @@ public class CrowdTool : Tool
|
|||
|
||||
if (toolParams.m_showCollisionSegments)
|
||||
{
|
||||
float[] center = ag.boundary.getCenter();
|
||||
Vector3f center = ag.boundary.getCenter();
|
||||
dd.debugDrawCross(center[0], center[1] + radius, center[2], 0.2f, duRGBA(192, 0, 128, 255), 2.0f);
|
||||
dd.debugDrawCircle(center[0], center[1] + radius, center[2], ag.option.collisionQueryRange,
|
||||
duRGBA(192, 0, 128, 128), 2.0f);
|
||||
dd.debugDrawCircle(center[0], center[1] + radius, center[2], ag.option.collisionQueryRange, duRGBA(192, 0, 128, 128), 2.0f);
|
||||
|
||||
dd.begin(LINES, 3.0f);
|
||||
for (int j = 0; j < ag.boundary.getSegmentCount(); ++j)
|
||||
|
|
@ -524,7 +523,7 @@ public class CrowdTool : Tool
|
|||
if (triArea2D(pos, s0, s3) < 0.0f)
|
||||
col = duDarkenCol(col);
|
||||
|
||||
dd.appendArrow(s[0], s[1] + 0.2f, s[2], s[3], s[4] + 0.2f, s[5], 0.0f, 0.3f, col);
|
||||
dd.appendArrow(s[0][0], s[0][1] + 0.2f, s[0][2], s[1][0], s[1][2] + 0.2f, s[1][2], 0.0f, 0.3f, col);
|
||||
}
|
||||
|
||||
dd.end();
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@ using System.Collections.Generic;
|
|||
using System.Collections.Immutable;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Numerics;
|
||||
using System.Runtime.Serialization;
|
||||
using System.Threading.Tasks;
|
||||
using DotRecast.Core;
|
||||
|
|
@ -116,10 +117,10 @@ public class DynamicUpdateTool : Tool
|
|||
private readonly DemoInputGeomProvider convexGeom;
|
||||
private bool sposSet;
|
||||
private bool eposSet;
|
||||
private float[] spos;
|
||||
private float[] epos;
|
||||
private Vector3f spos;
|
||||
private Vector3f epos;
|
||||
private bool raycastHit;
|
||||
private float[] raycastHitPos;
|
||||
private Vector3f raycastHitPos;
|
||||
|
||||
public DynamicUpdateTool()
|
||||
{
|
||||
|
|
@ -191,25 +192,25 @@ public class DynamicUpdateTool : Tool
|
|||
if (shift)
|
||||
{
|
||||
sposSet = true;
|
||||
spos = ArrayUtils.CopyOf(p, p.Length);
|
||||
spos = p;
|
||||
}
|
||||
else
|
||||
{
|
||||
eposSet = true;
|
||||
epos = ArrayUtils.CopyOf(p, p.Length);
|
||||
epos = p;
|
||||
}
|
||||
|
||||
if (sposSet && eposSet && dynaMesh != null)
|
||||
{
|
||||
float[] sp = { spos[0], spos[1] + 1.3f, spos[2] };
|
||||
float[] ep = { epos[0], epos[1] + 1.3f, epos[2] };
|
||||
Vector3f sp = Vector3f.Of(spos[0], spos[1] + 1.3f, spos[2]);
|
||||
Vector3f ep = Vector3f.Of(epos[0], epos[1] + 1.3f, epos[2]);
|
||||
long t1 = Stopwatch.GetTimestamp();
|
||||
float? hitPos = dynaMesh.voxelQuery().raycast(sp, ep);
|
||||
long t2 = Stopwatch.GetTimestamp();
|
||||
raycastTime = (t2 - t1) / TimeSpan.TicksPerMillisecond;
|
||||
raycastHit = hitPos.HasValue;
|
||||
raycastHitPos = hitPos.HasValue
|
||||
? new float[] { sp[0] + hitPos.Value * (ep[0] - sp[0]), sp[1] + hitPos.Value * (ep[1] - sp[1]), sp[2] + hitPos.Value * (ep[2] - sp[2]) }
|
||||
? Vector3f.Of(sp[0] + hitPos.Value * (ep[0] - sp[0]), sp[1] + hitPos.Value * (ep[1] - sp[1]), sp[2] + hitPos.Value * (ep[2] - sp[2]))
|
||||
: ep;
|
||||
}
|
||||
}
|
||||
|
|
@ -244,17 +245,16 @@ public class DynamicUpdateTool : Tool
|
|||
|
||||
private Tuple<Collider, ColliderGizmo> boxCollider(Vector3f p)
|
||||
{
|
||||
float[] extent = new float[]
|
||||
{
|
||||
0.5f + (float)random.NextDouble() * 6f, 0.5f + (float)random.NextDouble() * 6f,
|
||||
Vector3f extent = Vector3f.Of(
|
||||
0.5f + (float)random.NextDouble() * 6f,
|
||||
0.5f + (float)random.NextDouble() * 6f,
|
||||
0.5f + (float)random.NextDouble() * 6f
|
||||
};
|
||||
float[] forward = new float[] { (1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()) };
|
||||
float[] up = new float[] { (1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()) };
|
||||
float[][] halfEdges = BoxCollider.getHalfEdges(up, forward, extent);
|
||||
);
|
||||
Vector3f forward = Vector3f.Of((1f - 2 * (float)random.NextDouble()), 0, (1f - 2 * (float)random.NextDouble()));
|
||||
Vector3f up = Vector3f.Of((1f - 2 * (float)random.NextDouble()), 0.01f + (float)random.NextDouble(), (1f - 2 * (float)random.NextDouble()));
|
||||
Vector3f[] halfEdges = BoxCollider.getHalfEdges(up, forward, extent);
|
||||
return Tuple.Create<Collider, ColliderGizmo>(
|
||||
new BoxCollider(p, halfEdges, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb),
|
||||
GizmoFactory.box(p, halfEdges));
|
||||
new BoxCollider(p, halfEdges, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_WATER, dynaMesh.config.walkableClimb), GizmoFactory.box(p, halfEdges));
|
||||
}
|
||||
|
||||
private Tuple<Collider, ColliderGizmo> cylinderCollider(Vector3f p)
|
||||
|
|
@ -282,25 +282,25 @@ public class DynamicUpdateTool : Tool
|
|||
Vector3f side = vCross(forward, baseUp);
|
||||
BoxCollider @base = new BoxCollider(baseCenter, BoxCollider.getHalfEdges(baseUp, forward, baseExtent),
|
||||
SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb);
|
||||
float[] roofExtent = new float[] { 4.5f, 4.5f, 8f };
|
||||
var roofUp = Vector3f.Zero;
|
||||
Vector3f roofExtent = Vector3f.Of(4.5f, 4.5f, 8f);
|
||||
float[] rx = GLU.build_4x4_rotation_matrix(45, forward[0], forward[1], forward[2]);
|
||||
float[] roofUp = mulMatrixVector(new float[3], rx, baseUp);
|
||||
float[] roofCenter = new float[] { p[0], p[1] + 6, p[2] };
|
||||
roofUp = mulMatrixVector(ref roofUp, rx, baseUp);
|
||||
Vector3f roofCenter = Vector3f.Of(p[0], p[1] + 6, p[2]);
|
||||
BoxCollider roof = new BoxCollider(roofCenter, BoxCollider.getHalfEdges(roofUp, forward, roofExtent),
|
||||
SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, dynaMesh.config.walkableClimb);
|
||||
float[] trunkStart = new float[]
|
||||
{
|
||||
baseCenter[0] - forward[0] * 15 + side[0] * 6, p[1],
|
||||
Vector3f trunkStart = Vector3f.Of(
|
||||
baseCenter[0] - forward[0] * 15 + side[0] * 6,
|
||||
p[1],
|
||||
baseCenter[2] - forward[2] * 15 + side[2] * 6
|
||||
};
|
||||
float[] trunkEnd = new float[] { trunkStart[0], trunkStart[1] + 10, trunkStart[2] };
|
||||
);
|
||||
Vector3f trunkEnd = Vector3f.Of(trunkStart[0], trunkStart[1] + 10, trunkStart[2]);
|
||||
CapsuleCollider trunk = new CapsuleCollider(trunkStart, trunkEnd, 0.5f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD,
|
||||
dynaMesh.config.walkableClimb);
|
||||
float[] crownCenter = new float[]
|
||||
{
|
||||
Vector3f crownCenter = Vector3f.Of(
|
||||
baseCenter[0] - forward[0] * 15 + side[0] * 6, p[1] + 10,
|
||||
baseCenter[2] - forward[2] * 15 + side[2] * 6
|
||||
};
|
||||
);
|
||||
SphereCollider crown = new SphereCollider(crownCenter, 4f, SampleAreaModifications.SAMPLE_POLYAREA_TYPE_GRASS,
|
||||
dynaMesh.config.walkableClimb);
|
||||
CompositeCollider collider = new CompositeCollider(@base, roof, trunk, crown);
|
||||
|
|
@ -317,12 +317,12 @@ public class DynamicUpdateTool : Tool
|
|||
return trimeshCollider(p, bridgeGeom);
|
||||
}
|
||||
|
||||
private Tuple<Collider, ColliderGizmo> trimeshHouse(float[] p)
|
||||
private Tuple<Collider, ColliderGizmo> trimeshHouse(Vector3f p)
|
||||
{
|
||||
return trimeshCollider(p, houseGeom);
|
||||
}
|
||||
|
||||
private Tuple<Collider, ColliderGizmo> convexTrimesh(float[] p)
|
||||
private Tuple<Collider, ColliderGizmo> convexTrimesh(Vector3f p)
|
||||
{
|
||||
float[] verts = transformVertices(p, convexGeom, 360);
|
||||
ConvexTrimeshCollider collider = new ConvexTrimeshCollider(verts, convexGeom.faces,
|
||||
|
|
@ -351,7 +351,7 @@ public class DynamicUpdateTool : Tool
|
|||
v[0] = geom.vertices[i];
|
||||
v[1] = geom.vertices[i + 1];
|
||||
v[2] = geom.vertices[i + 2];
|
||||
mulMatrixVector(vr, m, v);
|
||||
mulMatrixVector(ref vr, m, v);
|
||||
vr[0] += p[0];
|
||||
vr[1] += p[1] - 0.1f;
|
||||
vr[2] += p[2];
|
||||
|
|
@ -370,8 +370,8 @@ public class DynamicUpdateTool : Tool
|
|||
resultvector[2] = matrix[2] * pvector[0] + matrix[6] * pvector[1] + matrix[10] * pvector[2];
|
||||
return resultvector;
|
||||
}
|
||||
|
||||
private float[] mulMatrixVector(ref Vector3f resultvector, float[] matrix, float[] pvector)
|
||||
|
||||
private Vector3f mulMatrixVector(ref Vector3f resultvector, float[] matrix, Vector3f 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];
|
||||
|
|
@ -454,7 +454,7 @@ public class DynamicUpdateTool : Tool
|
|||
}
|
||||
|
||||
dd.depthMask(false);
|
||||
if (raycastHitPos != null)
|
||||
if (raycastHitPos != Vector3f.Zero)
|
||||
{
|
||||
int spathCol = raycastHit ? duRGBA(128, 32, 16, 220) : duRGBA(64, 128, 240, 220);
|
||||
dd.begin(LINES, 2.0f);
|
||||
|
|
@ -467,7 +467,7 @@ public class DynamicUpdateTool : Tool
|
|||
}
|
||||
}
|
||||
|
||||
private void drawAgent(RecastDebugDraw dd, float[] pos, int col)
|
||||
private void drawAgent(RecastDebugDraw dd, Vector3f pos, int col)
|
||||
{
|
||||
float r = sample.getSettingsUI().getAgentRadius();
|
||||
float h = sample.getSettingsUI().getAgentHeight();
|
||||
|
|
|
|||
|
|
@ -25,15 +25,15 @@ public class BoxGizmo : ColliderGizmo
|
|||
};
|
||||
|
||||
private readonly float[] vertices = new float[8 * 3];
|
||||
private readonly float[] center;
|
||||
private readonly Vector3f center;
|
||||
private readonly Vector3f[] halfEdges;
|
||||
|
||||
public BoxGizmo(float[] center, float[] extent, Vector3f forward, Vector3f up) :
|
||||
public BoxGizmo(Vector3f center, Vector3f extent, Vector3f forward, Vector3f up) :
|
||||
this(center, BoxCollider.getHalfEdges(up, forward, extent))
|
||||
{
|
||||
}
|
||||
|
||||
public BoxGizmo(float[] center, Vector3f[] halfEdges)
|
||||
public BoxGizmo(Vector3f center, Vector3f[] halfEdges)
|
||||
{
|
||||
this.center = center;
|
||||
this.halfEdges = halfEdges;
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
using DotRecast.Recast.Demo.Draw;
|
||||
using DotRecast.Core;
|
||||
using DotRecast.Recast.Demo.Draw;
|
||||
using static DotRecast.Recast.RecastVectors;
|
||||
using static DotRecast.Core.RecastMath;
|
||||
using static DotRecast.Recast.Demo.Tools.Gizmos.GizmoHelper;
|
||||
|
|
@ -13,25 +14,25 @@ public class CylinderGizmo : ColliderGizmo
|
|||
private readonly float[] center;
|
||||
private readonly float[] gradient;
|
||||
|
||||
public CylinderGizmo(float[] start, float[] end, float radius)
|
||||
public CylinderGizmo(Vector3f start, Vector3f end, float radius)
|
||||
{
|
||||
center = new float[]
|
||||
{
|
||||
0.5f * (start[0] + end[0]), 0.5f * (start[1] + end[1]),
|
||||
0.5f * (start[2] + end[2])
|
||||
};
|
||||
float[] axis = new float[] { end[0] - start[0], end[1] - start[1], end[2] - start[2] };
|
||||
float[][] normals = new float[3][];
|
||||
normals[1] = new float[] { end[0] - start[0], end[1] - start[1], end[2] - start[2] };
|
||||
normalize(normals[1]);
|
||||
Vector3f axis = Vector3f.Of(end[0] - start[0], end[1] - start[1], end[2] - start[2]);
|
||||
Vector3f[] normals = new Vector3f[3];
|
||||
normals[1] = Vector3f.Of(end[0] - start[0], end[1] - start[1], end[2] - start[2]);
|
||||
normalize(ref normals[1]);
|
||||
normals[0] = getSideVector(axis);
|
||||
normals[2] = new float[3];
|
||||
cross(normals[2], normals[0], normals[1]);
|
||||
normalize(normals[2]);
|
||||
normals[2] = Vector3f.Zero;
|
||||
cross(ref normals[2], normals[0], normals[1]);
|
||||
normalize(ref normals[2]);
|
||||
triangles = generateCylindricalTriangles();
|
||||
float[] trX = new float[] { normals[0][0], normals[1][0], normals[2][0] };
|
||||
float[] trY = new float[] { normals[0][1], normals[1][1], normals[2][1] };
|
||||
float[] trZ = new float[] { normals[0][2], normals[1][2], normals[2][2] };
|
||||
Vector3f trX = Vector3f.Of(normals[0][0], normals[1][0], normals[2][0]);
|
||||
Vector3f trY = Vector3f.Of(normals[0][1], normals[1][1], normals[2][1]);
|
||||
Vector3f trZ = Vector3f.Of(normals[0][2], normals[1][2], normals[2][2]);
|
||||
vertices = generateCylindricalVertices();
|
||||
float halfLength = 0.5f * vLen(axis);
|
||||
gradient = new float[vertices.Length / 3];
|
||||
|
|
@ -54,24 +55,24 @@ public class CylinderGizmo : ColliderGizmo
|
|||
v[0] = vertices[i] - center[0];
|
||||
v[1] = vertices[i + 1] - center[1];
|
||||
v[2] = vertices[i + 2] - center[2];
|
||||
normalize(v);
|
||||
normalize(ref v);
|
||||
gradient[i / 3] = clamp(0.57735026f * (v[0] + v[1] + v[2]), -1, 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private float[] getSideVector(float[] axis)
|
||||
private Vector3f getSideVector(Vector3f axis)
|
||||
{
|
||||
float[] side = { 1, 0, 0 };
|
||||
Vector3f side = Vector3f.Of(1, 0, 0);
|
||||
if (axis[0] > 0.8)
|
||||
{
|
||||
side = new float[] { 0, 0, 1 };
|
||||
side = Vector3f.Of(0, 0, 1);
|
||||
}
|
||||
|
||||
Vector3f forward = new Vector3f();
|
||||
cross(forward, side, axis);
|
||||
cross(side, axis, forward);
|
||||
normalize(side);
|
||||
cross(ref forward, side, axis);
|
||||
cross(ref side, axis, forward);
|
||||
normalize(ref side);
|
||||
return side;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ namespace DotRecast.Recast.Demo.Tools.Gizmos;
|
|||
|
||||
public static class GizmoFactory
|
||||
{
|
||||
public static ColliderGizmo box(float[] center, float[][] halfEdges)
|
||||
public static ColliderGizmo box(Vector3f center, Vector3f[] halfEdges)
|
||||
{
|
||||
return new BoxGizmo(center, halfEdges);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
using System;
|
||||
using DotRecast.Core;
|
||||
using DotRecast.Recast.Demo.Draw;
|
||||
using static DotRecast.Core.RecastMath;
|
||||
|
||||
|
|
@ -175,7 +176,8 @@ public class GizmoHelper
|
|||
|
||||
public static int getColorByNormal(float[] vertices, int v0, int v1, int v2)
|
||||
{
|
||||
Vector3f e0 = new Vector3f(), e1 = new float[3];
|
||||
Vector3f e0 = new Vector3f();
|
||||
Vector3f e1 = new Vector3f();
|
||||
Vector3f normal = new Vector3f();
|
||||
for (int j = 0; j < 3; ++j)
|
||||
{
|
||||
|
|
@ -186,7 +188,7 @@ public class GizmoHelper
|
|||
normal[0] = e0[1] * e1[2] - e0[2] * e1[1];
|
||||
normal[1] = e0[2] * e1[0] - e0[0] * e1[2];
|
||||
normal[2] = e0[0] * e1[1] - e0[1] * e1[0];
|
||||
RecastVectors.normalize(normal);
|
||||
RecastVectors.normalize(ref normal);
|
||||
float c = clamp(0.57735026f * (normal[0] + normal[1] + normal[2]), -1, 1);
|
||||
int col = DebugDraw.duLerpCol(DebugDraw.duRGBA(32, 32, 0, 160), DebugDraw.duRGBA(220, 220, 0, 160),
|
||||
(int)(127 * (1 + c)));
|
||||
|
|
|
|||
|
|
@ -236,7 +236,7 @@ public class JumpLinkBuilderTool : Tool
|
|||
{
|
||||
GroundSample s = link.start.gsamples[i];
|
||||
float u = i / (float)(link.start.gsamples.Length - 1);
|
||||
float[] spt = vLerp(link.start.p, link.start.q, u);
|
||||
Vector3f spt = vLerp(link.start.p, link.start.q, u);
|
||||
int col = duRGBA(48, 16, 16, 255); // duRGBA(255,(s->flags & 4)?255:0,0,255);
|
||||
float off = 0.1f;
|
||||
if (!s.validHeight)
|
||||
|
|
@ -256,7 +256,7 @@ public class JumpLinkBuilderTool : Tool
|
|||
{
|
||||
GroundSample s = link.start.gsamples[i];
|
||||
float u = i / (float)(link.start.gsamples.Length - 1);
|
||||
float[] spt = vLerp(link.start.p, link.start.q, u);
|
||||
Vector3f spt = vLerp(link.start.p, link.start.q, u);
|
||||
int col = duRGBA(255, 255, 255, 255);
|
||||
float off = 0;
|
||||
if (s.validHeight)
|
||||
|
|
@ -276,7 +276,7 @@ public class JumpLinkBuilderTool : Tool
|
|||
{
|
||||
GroundSample s = end.gsamples[i];
|
||||
float u = i / (float)(end.gsamples.Length - 1);
|
||||
float[] spt = vLerp(end.p, end.q, u);
|
||||
Vector3f spt = vLerp(end.p, end.q, u);
|
||||
int col = duRGBA(48, 16, 16, 255); // duRGBA(255,(s->flags & 4)?255:0,0,255);
|
||||
float off = 0.1f;
|
||||
if (!s.validHeight)
|
||||
|
|
@ -295,7 +295,7 @@ public class JumpLinkBuilderTool : Tool
|
|||
{
|
||||
GroundSample s = end.gsamples[i];
|
||||
float u = i / (float)(end.gsamples.Length - 1);
|
||||
float[] spt = vLerp(end.p, end.q, u);
|
||||
Vector3f spt = vLerp(end.p, end.q, u);
|
||||
int col = duRGBA(255, 255, 255, 255);
|
||||
float off = 0;
|
||||
if (s.validHeight)
|
||||
|
|
@ -316,7 +316,7 @@ public class JumpLinkBuilderTool : Tool
|
|||
dd.depthMask(true);
|
||||
}
|
||||
|
||||
private void drawTrajectory(RecastDebugDraw dd, JumpLink link, float[] pa, float[] pb, Trajectory tra, int cola)
|
||||
private void drawTrajectory(RecastDebugDraw dd, JumpLink link, Vector3f pa, Vector3f pb, Trajectory tra, int cola)
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -434,8 +434,8 @@ public class JumpLinkBuilderTool : Tool
|
|||
Vector3f prev = new Vector3f();
|
||||
for (int i = 0; i < link.startSamples.Length; i++)
|
||||
{
|
||||
float[] p = link.startSamples[i].p;
|
||||
float[] q = link.endSamples[i].p;
|
||||
Vector3f p = link.startSamples[i].p;
|
||||
Vector3f q = link.endSamples[i].p;
|
||||
if (i == 0 || vDist2D(prev, p) > agentRadius)
|
||||
{
|
||||
geom.addOffMeshConnection(p, q, agentRadius, false, area, flags);
|
||||
|
|
|
|||
|
|
@ -730,13 +730,13 @@ public class TestNavmeshTool : Tool
|
|||
{
|
||||
dd.debugDrawNavMeshPoly(m_navMesh, m_startRef, startCol);
|
||||
dd.depthMask(false);
|
||||
if (m_spos != null)
|
||||
if (m_spos != Vector3f.Zero)
|
||||
{
|
||||
dd.debugDrawCircle(m_spos[0], m_spos[1] + agentHeight / 2, m_spos[2], m_distanceToWall,
|
||||
duRGBA(64, 16, 0, 220), 2.0f);
|
||||
}
|
||||
|
||||
if (m_hitPos != null)
|
||||
if (m_hitPos != Vector3f.Zero)
|
||||
{
|
||||
dd.begin(LINES, 3.0f);
|
||||
dd.vertex(m_hitPos[0], m_hitPos[1] + 0.02f, m_hitPos[2], duRGBA(0, 0, 0, 192));
|
||||
|
|
@ -757,8 +757,8 @@ public class TestNavmeshTool : Tool
|
|||
if (m_parent[i] != 0)
|
||||
{
|
||||
dd.depthMask(false);
|
||||
float[] p0 = getPolyCenter(m_navMesh, m_parent[i]);
|
||||
float[] p1 = getPolyCenter(m_navMesh, m_polys[i]);
|
||||
Vector3f p0 = getPolyCenter(m_navMesh, m_parent[i]);
|
||||
Vector3f p1 = getPolyCenter(m_navMesh, m_polys[i]);
|
||||
dd.debugDrawArc(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], 0.25f, 0.0f, 0.4f,
|
||||
duRGBA(0, 0, 0, 128), 2.0f);
|
||||
dd.depthMask(true);
|
||||
|
|
@ -790,8 +790,8 @@ public class TestNavmeshTool : Tool
|
|||
if (m_parent[i] != 0)
|
||||
{
|
||||
dd.depthMask(false);
|
||||
float[] p0 = getPolyCenter(m_navMesh, m_parent[i]);
|
||||
float[] p1 = getPolyCenter(m_navMesh, m_polys[i]);
|
||||
Vector3f p0 = getPolyCenter(m_navMesh, m_parent[i]);
|
||||
Vector3f p1 = getPolyCenter(m_navMesh, m_polys[i]);
|
||||
dd.debugDrawArc(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], 0.25f, 0.0f, 0.4f,
|
||||
duRGBA(0, 0, 0, 128), 2.0f);
|
||||
dd.depthMask(true);
|
||||
|
|
@ -827,8 +827,8 @@ public class TestNavmeshTool : Tool
|
|||
if (m_parent[i] != 0)
|
||||
{
|
||||
dd.depthMask(false);
|
||||
float[] p0 = getPolyCenter(m_navMesh, m_parent[i]);
|
||||
float[] p1 = getPolyCenter(m_navMesh, m_polys[i]);
|
||||
Vector3f p0 = getPolyCenter(m_navMesh, m_parent[i]);
|
||||
Vector3f p1 = getPolyCenter(m_navMesh, m_polys[i]);
|
||||
dd.debugDrawArc(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], 0.25f, 0.0f, 0.4f,
|
||||
duRGBA(0, 0, 0, 128), 2.0f);
|
||||
dd.depthMask(true);
|
||||
|
|
@ -846,7 +846,7 @@ public class TestNavmeshTool : Tool
|
|||
for (int j = 0; j < wallSegments.getSegmentVerts().Count; ++j)
|
||||
{
|
||||
float[] s = wallSegments.getSegmentVerts()[j];
|
||||
float[] s3 = new float[] { s[3], s[4], s[5] };
|
||||
Vector3f s3 = Vector3f.Of(s[3], s[4], s[5]);
|
||||
// Skip too distant segments.
|
||||
Tuple<float, float> distSqr = distancePtSegSqr2D(m_spos, s, 0, 3);
|
||||
if (distSqr.Item1 > RecastMath.sqr(m_neighbourhoodRadius))
|
||||
|
|
@ -854,11 +854,11 @@ public class TestNavmeshTool : Tool
|
|||
continue;
|
||||
}
|
||||
|
||||
float[] delta = vSub(s3, s);
|
||||
float[] p0 = vMad(s, delta, 0.5f);
|
||||
float[] norm = new float[] { delta[2], 0, -delta[0] };
|
||||
vNormalize(norm);
|
||||
float[] p1 = vMad(p0, norm, agentRadius * 0.5f);
|
||||
Vector3f delta = vSub(s3, s);
|
||||
Vector3f p0 = vMad(s, delta, 0.5f);
|
||||
Vector3f norm = Vector3f.Of(delta[2], 0, -delta[0]);
|
||||
vNormalize(ref norm);
|
||||
Vector3f p1 = vMad(p0, norm, agentRadius * 0.5f);
|
||||
// Skip backfacing segments.
|
||||
if (wallSegments.getSegmentRefs()[j] != 0)
|
||||
{
|
||||
|
|
@ -903,7 +903,7 @@ public class TestNavmeshTool : Tool
|
|||
dd.depthMask(false);
|
||||
dd.begin(POINTS, 4.0f);
|
||||
int col = duRGBA(64, 16, 0, 220);
|
||||
foreach (float[] point in randomPoints)
|
||||
foreach (Vector3f point in randomPoints)
|
||||
{
|
||||
dd.vertex(point[0], point[1] + 0.1f, point[2], col);
|
||||
}
|
||||
|
|
@ -945,12 +945,10 @@ public class TestNavmeshTool : Tool
|
|||
dd.depthMask(true);
|
||||
}
|
||||
|
||||
private float[] getPolyCenter(NavMesh navMesh, long refs)
|
||||
private Vector3f getPolyCenter(NavMesh navMesh, long refs)
|
||||
{
|
||||
Vector3f center = new Vector3f();
|
||||
center[0] = 0;
|
||||
center[1] = 0;
|
||||
center[2] = 0;
|
||||
Vector3f center = Vector3f.Zero;
|
||||
|
||||
Result<Tuple<MeshTile, Poly>> tileAndPoly = navMesh.getTileAndPolyByRef(refs);
|
||||
if (tileAndPoly.succeeded())
|
||||
{
|
||||
|
|
@ -992,7 +990,7 @@ public class TestNavmeshTool : Tool
|
|||
{
|
||||
// In case of partial path, make sure the end point is clamped to the last polygon.
|
||||
Vector3f epos = new Vector3f();
|
||||
vCopy(epos, m_epos);
|
||||
vCopy(ref epos, m_epos);
|
||||
if (m_polys[m_polys.Count - 1] != m_endRef)
|
||||
{
|
||||
Result<ClosestPointOnPolyResult> result = m_navQuery
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@ freely, subject to the following restrictions:
|
|||
|
||||
using System;
|
||||
using DotRecast.Core;
|
||||
|
||||
using static DotRecast.Core.RecastMath;
|
||||
using static DotRecast.Recast.RecastConstants;
|
||||
using static DotRecast.Recast.RecastVectors;
|
||||
|
|
@ -75,19 +74,19 @@ namespace DotRecast.Recast
|
|||
ctx.stopTimer("RASTERIZE_CYLINDER");
|
||||
}
|
||||
|
||||
public static void rasterizeBox(Heightfield hf, Vector3f center, float[][] halfEdges, int area, int flagMergeThr,
|
||||
public static void rasterizeBox(Heightfield hf, Vector3f center, Vector3f[] halfEdges, int area, int flagMergeThr,
|
||||
Telemetry ctx)
|
||||
{
|
||||
ctx.startTimer("RASTERIZE_BOX");
|
||||
float[][] normals =
|
||||
Vector3f[] normals =
|
||||
{
|
||||
new[] { halfEdges[0][0], halfEdges[0][1], halfEdges[0][2] },
|
||||
new[] { halfEdges[1][0], halfEdges[1][1], halfEdges[1][2] },
|
||||
new[] { halfEdges[2][0], halfEdges[2][1], halfEdges[2][2] }
|
||||
Vector3f.Of(halfEdges[0][0], halfEdges[0][1], halfEdges[0][2]),
|
||||
Vector3f.Of(halfEdges[1][0], halfEdges[1][1], halfEdges[1][2]),
|
||||
Vector3f.Of(halfEdges[2][0], halfEdges[2][1], halfEdges[2][2]),
|
||||
};
|
||||
normalize(normals[0]);
|
||||
normalize(normals[1]);
|
||||
normalize(normals[2]);
|
||||
normalize(ref normals[0]);
|
||||
normalize(ref normals[1]);
|
||||
normalize(ref normals[2]);
|
||||
|
||||
float[] vertices = new float[8 * 3];
|
||||
float[] bounds = new float[]
|
||||
|
|
|
|||
|
|
@ -54,8 +54,8 @@ public class VoxelQueryTest
|
|||
});
|
||||
|
||||
VoxelQuery query = new VoxelQuery(ORIGIN, TILE_WIDTH, TILE_DEPTH, hfProvider.Object);
|
||||
float[] start = { 120, 10, 365 };
|
||||
float[] end = { 320, 10, 57 };
|
||||
Vector3f start = Vector3f.Of(120, 10, 365);
|
||||
Vector3f end = Vector3f.Of(320, 10, 57);
|
||||
|
||||
// When
|
||||
query.raycast(start, end);
|
||||
|
|
@ -70,8 +70,8 @@ public class VoxelQueryTest
|
|||
{
|
||||
DynamicNavMesh mesh = createDynaMesh();
|
||||
VoxelQuery query = mesh.voxelQuery();
|
||||
float[] start = { 7.4f, 0.5f, -64.8f };
|
||||
float[] end = { 31.2f, 0.5f, -75.3f };
|
||||
Vector3f start = Vector3f.Of(7.4f, 0.5f, -64.8f);
|
||||
Vector3f end = Vector3f.Of(31.2f, 0.5f, -75.3f);
|
||||
float? hit = query.raycast(start, end);
|
||||
Assert.That(hit, Is.Null);
|
||||
}
|
||||
|
|
@ -81,8 +81,8 @@ public class VoxelQueryTest
|
|||
{
|
||||
DynamicNavMesh mesh = createDynaMesh();
|
||||
VoxelQuery query = mesh.voxelQuery();
|
||||
float[] start = { 32.3f, 0.5f, 47.9f };
|
||||
float[] end = { -31.2f, 0.5f, -29.8f };
|
||||
Vector3f start = Vector3f.Of(32.3f, 0.5f, 47.9f);
|
||||
Vector3f end = Vector3f.Of(-31.2f, 0.5f, -29.8f);
|
||||
float? hit = query.raycast(start, end);
|
||||
Assert.That(hit, Is.Not.Null);
|
||||
Assert.That(hit.Value, Is.EqualTo(0.5263836f).Within(1e-7f));
|
||||
|
|
|
|||
Loading…
Reference in New Issue