bit
/
DotRecastNetSim
forked from mirror/DotRecast
40
0
Fork 1
Code Pull Requests Releases Activity

change float[3] -> struct vector3f

This commit is contained in:
ikpil 2023-03-29 00:03:33 +09:00
parent e6ea55e1a3
commit 5f6a1ac070
64 changed files with 683 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
src/DotRecast.Core/RecastMath.cs
View File

@ -37,11 +37,11 @@ namespace DotRecast.Core
public static float[] vCross(float[] v1, float[] v2)
{
float[] dest = new float[3];
Vector3f dest = new Vector3f();
dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
dest[1] = v1[2] * v2[0] - v1[0] * v2[2];
dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
return dest;
return dest.ToArray();
}
public static float vDot(float[] v1, float[] v2)
@ -101,11 +101,11 @@ namespace DotRecast.Core
/// @param[in] s The amount to scale @p v2 by before adding to @p v1.
public static float[] vMad(float[] v1, float[] v2, float s)
{
float[] dest = new float[3];
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;
return dest.ToArray();
}
/// Performs a linear interpolation between two vectors. (@p v1 toward @p
@ -116,47 +116,57 @@ namespace DotRecast.Core
/// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0]
public static float[] vLerp(float[] verts, int v1, int v2, float t)
{
float[] dest = new float[3];
Vector3f dest = new Vector3f();
dest[0] = verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t;
dest[1] = verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t;
dest[2] = verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t;
return dest;
return dest.ToArray();
}
public static float[] vLerp(float[] v1, float[] v2, float t)
{
float[] dest = new float[3];
Vector3f dest = new Vector3f();
dest[0] = v1[0] + (v2[0] - v1[0]) * t;
dest[1] = v1[1] + (v2[1] - v1[1]) * t;
dest[2] = v1[2] + (v2[2] - v1[2]) * t;
return dest;
return dest.ToArray();
}
public static float[] vSub(VectorPtr v1, VectorPtr v2)
{
float[] dest = new float[3];
Vector3f dest = new Vector3f();
dest[0] = v1.get(0) - v2.get(0);
dest[1] = v1.get(1) - v2.get(1);
dest[2] = v1.get(2) - v2.get(2);
return dest;
return dest.ToArray();
}
public static float[] vSub(float[] v1, float[] v2)
{
float[] dest = new float[3];
Vector3f dest = new Vector3f();
dest[0] = v1[0] - v2[0];
dest[1] = v1[1] - v2[1];
dest[2] = v1[2] - v2[2];
return dest;
return dest.ToArray();
}
public static float[] vSub(Vector3f v1, Vector3f v2)
{
Vector3f dest = new Vector3f();
dest[0] = v1[0] - v2[0];
dest[1] = v1[1] - v2[1];
dest[2] = v1[2] - v2[2];
return dest.ToArray();
}
public static float[] vAdd(float[] v1, float[] v2)
{
float[] dest = new float[3];
Vector3f dest = new Vector3f();
dest[0] = v1[0] + v2[0];
dest[1] = v1[1] + v2[1];
dest[2] = v1[2] + v2[2];
return dest;
return dest.ToArray();
}
public static float[] vCopy(float[] @in)
@ -175,6 +185,14 @@ namespace DotRecast.Core
@out[2] = c;
}
public static void vSet(ref Vector3f @out, float a, float b, float c)
{
@out.x = a;
@out.y = b;
@out.z = c;
}
public static void vCopy(float[] @out, float[] @in)
{
@out[0] = @in[0];
@ -182,6 +200,20 @@ namespace DotRecast.Core
@out[2] = @in[2];
}
public static void vCopy(ref Vector3f @out, float[] @in)
{
@out.x = @in[0];
@out.y = @in[1];
@out.z = @in[2];
}
public static void vCopy(ref Vector3f @out, Vector3f @in)
{
@out.x = @in[0];
@out.y = @in[1];
@out.z = @in[2];
}
public static void vCopy(float[] @out, float[] @in, int i)
{
@out[0] = @in[i];
@ -189,6 +221,13 @@ namespace DotRecast.Core
@out[2] = @in[i + 2];
}
public static void vCopy(ref Vector3f @out, float[] @in, int i)
{
@out.x = @in[i];
@out.y = @in[i + 1];
@out.z = @in[i + 2];
}
public static void vMin(float[] @out, float[] @in, int i)
{
@out[0] = Math.Min(@out[0], @in[i]);
@ -196,6 +235,14 @@ namespace DotRecast.Core
@out[2] = Math.Min(@out[2], @in[i + 2]);
}
public static void vMin(ref Vector3f @out, float[] @in, int i)
{
@out.x = Math.Min(@out.x, @in[i]);
@out.y = Math.Min(@out.y, @in[i + 1]);
@out.z = Math.Min(@out.z, @in[i + 2]);
}
public static void vMax(float[] @out, float[] @in, int i)
{
@out[0] = Math.Max(@out[0], @in[i]);
@ -203,6 +250,14 @@ namespace DotRecast.Core
@out[2] = Math.Max(@out[2], @in[i + 2]);
}
public static void vMax(ref Vector3f @out, float[] @in, int i)
{
@out.x = Math.Max(@out.x, @in[i]);
@out.y = Math.Max(@out.y, @in[i + 1]);
@out.z = Math.Max(@out.z, @in[i + 2]);
}
/// Returns the distance between two points.
/// @param[in] v1 A point. [(x, y, z)]
/// @param[in] v2 A point. [(x, y, z)]
@ -215,6 +270,24 @@ namespace DotRecast.Core
return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
}
public static float vDist(Vector3f v1, float[] v2)
{
float dx = v2[0] - v1[0];
float dy = v2[1] - v1[1];
float dz = v2[2] - v1[2];
return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
}
public static float vDist(Vector3f v1, Vector3f v2)
{
float dx = v2[0] - v1[0];
float dy = v2[1] - v1[1];
float dz = v2[2] - v1[2];
return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
}
/// Returns the distance between two points.
/// @param[in] v1 A point. [(x, y, z)]
/// @param[in] v2 A point. [(x, y, z)]
@ -227,6 +300,15 @@ namespace DotRecast.Core
return dx * dx + dy * dy + dz * dz;
}
public static float vDistSqr(Vector3f v1, Vector3f v2)
{
float dx = v2[0] - v1[0];
float dy = v2[1] - v1[1];
float dz = v2[2] - v1[2];
return dx * dx + dy * dy + dz * dz;
}
/// Derives the square of the scalar length of the vector. (len * len)
/// @param[in] v The vector. [(x, y, z)]
/// @return The square of the scalar length of the vector.
@ -290,6 +372,17 @@ namespace DotRecast.Core
}
}
public static void vNormalize(ref Vector3f v)
{
float d = (float)(1.0f / Math.Sqrt(sqr(v[0]) + sqr(v[1]) + sqr(v[2])));
if (d != 0)
{
v.x *= d;
v.y *= d;
v.z *= d;
}
}
/// Performs a 'sloppy' colocation check of the specified points.
/// @param[in] p0 A point. [(x, y, z)]
@ -365,6 +458,16 @@ namespace DotRecast.Core
return acx * abz - abx * acz;
}
public static float triArea2D(Vector3f a, Vector3f 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.
/// @param[in] amin Minimum bounds of box A. [(x, y, z)]
/// @param[in] amax Maximum bounds of box A. [(x, y, z)]
@ -521,7 +624,7 @@ namespace DotRecast.Core
return ((amin + eps) > bmax || (amax - eps) < bmin) ? false : true;
}
static float eps = 1e-4f;
const float eps = 1e-4f;
/// @par
///
@ -765,6 +868,23 @@ namespace DotRecast.Core
return Tuple.Create(s, t);
}
public static Tuple<float, float>? intersectSegSeg2D(Vector3f ap, Vector3f aq, Vector3f bp, Vector3f bq)
{
float[] u = vSub(aq, ap);
float[] v = vSub(bq, bp);
float[] w = vSub(ap, bp);
float d = vperpXZ(u, v);
if (Math.Abs(d) < 1e-6f)
{
return null;
}
float s = vperpXZ(v, w) / d;
float t = vperpXZ(u, w) / d;
return Tuple.Create(s, t);
}
public static float[] vScale(float[] @in, float scale)
{
float[] @out = new float[3];

10
src/DotRecast.Core/Vector3f.cs
View File

@ -26,6 +26,16 @@ namespace DotRecast.Core
public float y;
public float z;
public static Vector3f Of(float[] f)
{
return new Vector3f()
{
x = f[0],
y = f[1],
z = f[2],
};
}
public Vector3f(float x, float y, float z)
{
this.x = x;

12
src/DotRecast.Detour.Crowd/Crowd.cs
View File

@ -147,7 +147,7 @@ namespace DotRecast.Detour.Crowd
private readonly ObstacleAvoidanceQuery.ObstacleAvoidanceParams[] m_obstacleQueryParams = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams[DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS];
private readonly ObstacleAvoidanceQuery m_obstacleQuery;
private ProximityGrid m_grid;
private readonly float[] m_ext = new float[3];
private readonly Vector3f m_ext = new Vector3f();
private readonly QueryFilter[] m_filters = new QueryFilter[DT_CROWD_MAX_QUERY_FILTER_TYPE];
private NavMeshQuery navQuery;
private NavMesh navMesh;
@ -453,7 +453,7 @@ namespace DotRecast.Detour.Crowd
bool replan = false;
// First check that the current location is valid.
float[] agentPos = new float[3];
Vector3f agentPos = new Vector3f();
long agentRef = ag.corridor.getFirstPoly();
vCopy(agentPos, ag.npos);
if (!navQuery.isValidPolyRef(agentRef, m_filters[ag.option.queryFilterType]))
@ -605,7 +605,7 @@ namespace DotRecast.Detour.Crowd
}
List<long> reqPath = pathFound.result;
float[] reqPos = new float[3];
Vector3f reqPos = new Vector3f();
if (pathFound.succeeded() && reqPath.Count > 0)
{
// In progress or succeed.
@ -1047,7 +1047,7 @@ namespace DotRecast.Detour.Crowd
continue;
}
float[] dvel = new float[3];
Vector3f dvel = new Vector3f();
if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY)
{
@ -1082,7 +1082,7 @@ namespace DotRecast.Detour.Crowd
float separationWeight = ag.option.separationWeight;
float w = 0;
float[] disp = new float[3];
Vector3f disp = new Vector3f();
for (int j = 0; j < ag.neis.Count; ++j)
{
@ -1156,7 +1156,7 @@ namespace DotRecast.Detour.Crowd
for (int j = 0; j < ag.boundary.getSegmentCount(); ++j)
{
float[] s = ag.boundary.getSegment(j);
float[] s3 = new float[3];
Vector3f s3 = new Vector3f();
Array.Copy(s, 3, s3, 0, 3);
if (triArea2D(ag.npos, s, s3) < 0.0f)
{

16
src/DotRecast.Detour.Crowd/CrowdAgent.cs
View File

@ -77,23 +77,23 @@ namespace DotRecast.Detour.Crowd
/// The desired speed.
public float desiredSpeed;
public float[] npos = new float[3];
public Vector3f npos = new Vector3f();
/// < The current agent position. [(x, y, z)]
public float[] disp = new float[3];
public Vector3f disp = new Vector3f();
/// < A temporary value used to accumulate agent displacement during iterative
/// collision resolution. [(x, y, z)]
public float[] dvel = new float[3];
public Vector3f dvel = new Vector3f();
/// < The desired velocity of the agent. Based on the current path, calculated
/// from
/// scratch each frame. [(x, y, z)]
public float[] nvel = new float[3];
public Vector3f nvel = new Vector3f();
/// < The desired velocity adjusted by obstacle avoidance, calculated from scratch each
/// frame. [(x, y, z)]
public float[] vel = new float[3];
public Vector3f vel = new Vector3f();
/// < The actual velocity of the agent. The change from nvel -> vel is
/// constrained by max acceleration. [(x, y, z)]
@ -109,7 +109,7 @@ namespace DotRecast.Detour.Crowd
public long targetRef;
/// < Target polyref of the movement request.
public float[] targetPos = new float[3];
public Vector3f targetPos = new Vector3f();
/// < Target position of the movement request (or velocity in case of
/// DT_CROWDAGENT_TARGET_VELOCITY).
@ -184,7 +184,7 @@ namespace DotRecast.Detour.Crowd
public float[] calcSmoothSteerDirection()
{
float[] dir = new float[3];
Vector3f dir = new Vector3f();
if (0 < corners.Count)
{
int ip0 = 0;
@ -214,7 +214,7 @@ namespace DotRecast.Detour.Crowd
public float[] calcStraightSteerDirection()
{
float[] dir = new float[3];
Vector3f dir = new Vector3f();
if (0 < corners.Count)
{
dir = vSub(corners[0].getPos(), npos);

6
src/DotRecast.Detour.Crowd/CrowdAgentAnimation.cs
View File

@ -23,9 +23,9 @@ namespace DotRecast.Detour.Crowd
public class CrowdAgentAnimation
{
public bool active;
public float[] initPos = new float[3];
public float[] startPos = new float[3];
public float[] endPos = new float[3];
public Vector3f initPos = new Vector3f();
public Vector3f startPos = new Vector3f();
public Vector3f endPos = new Vector3f();
public long polyRef;
public float t, tmax;
}

2
src/DotRecast.Detour.Crowd/LocalBoundary.cs
View File

@ -38,7 +38,7 @@ namespace DotRecast.Detour.Crowd
public float d;
}
float[] m_center = new float[3];
Vector3f m_center = new Vector3f();
List<Segment> m_segs = new List<Segment>();
List<long> m_polys = new List<long>();

32
src/DotRecast.Detour.Crowd/ObstacleAvoidanceQuery.cs
View File

@ -36,31 +36,31 @@ namespace DotRecast.Detour.Crowd
public class ObstacleCircle
{
/** Position of the obstacle */
public readonly float[] p = new float[3];
public readonly Vector3f p = new Vector3f();
/** Velocity of the obstacle */
public readonly float[] vel = new float[3];
public readonly Vector3f vel = new Vector3f();
/** Velocity of the obstacle */
public readonly float[] dvel = new float[3];
public readonly Vector3f dvel = new Vector3f();
/** Radius of the obstacle */
public float rad;
/** Use for side selection during sampling. */
public readonly float[] dp = new float[3];
public readonly Vector3f dp = new Vector3f();
/** Use for side selection during sampling. */
public readonly float[] np = new float[3];
public readonly Vector3f np = new Vector3f();
}
public class ObstacleSegment
{
/** End points of the obstacle segment */
public readonly float[] p = new float[3];
public readonly Vector3f p = new Vector3f();
/** End points of the obstacle segment */
public readonly float[] q = new float[3];
public readonly Vector3f q = new Vector3f();
public bool touch;
}
@ -205,7 +205,7 @@ namespace DotRecast.Detour.Crowd
float[] pb = cir.p;
float[] orig = { 0f, 0f, 0f };
float[] dv = new float[3];
Vector3f dv = new Vector3f();
vCopy(cir.dp, vSub(pb, pa));
vNormalize(cir.dp);
dv = vSub(cir.dvel, dvel);
@ -346,7 +346,7 @@ namespace DotRecast.Detour.Crowd
{
// Special case when the agent is very close to the segment.
float[] sdir = vSub(seg.q, seg.p);
float[] snorm = new float[3];
Vector3f snorm = new Vector3f();
snorm[0] = -sdir[2];
snorm[2] = sdir[0];
// If the velocity is pointing towards the segment, no collision.
@ -399,7 +399,7 @@ namespace DotRecast.Detour.Crowd
m_vmax = vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : float.MaxValue;
float[] nvel = new float[3];
Vector3f nvel = new Vector3f();
vSet(nvel, 0f, 0f, 0f);
if (debug != null)
@ -417,7 +417,7 @@ namespace DotRecast.Detour.Crowd
{
for (int x = 0; x < m_params.gridSize; ++x)
{
float[] vcand = new float[3];
Vector3f vcand = new Vector3f();
vSet(vcand, cvx + x * cs - half, 0f, cvz + y * cs - half);
if (sqr(vcand[0]) + sqr(vcand[2]) > sqr(vmax + cs / 2))
@ -450,7 +450,7 @@ namespace DotRecast.Detour.Crowd
// vector normalization that ignores the y-component.
float[] dtRotate2D(float[] v, float ang)
{
float[] dest = new float[3];
Vector3f dest = new Vector3f();
float c = (float)Math.Cos(ang);
float s = (float)Math.Sin(ang);
dest[0] = v[0] * c - v[2] * s;
@ -470,7 +470,7 @@ namespace DotRecast.Detour.Crowd
m_vmax = vmax;
m_invVmax = vmax > 0 ? 1.0f / vmax : float.MaxValue;
float[] nvel = new float[3];
Vector3f nvel = new Vector3f();
vSet(nvel, 0f, 0f, 0f);
if (debug != null)
@ -537,18 +537,18 @@ namespace DotRecast.Detour.Crowd
// Start sampling.
float cr = vmax * (1.0f - m_params.velBias);
float[] res = new float[3];
Vector3f res = new Vector3f();
vSet(res, dvel[0] * m_params.velBias, 0, dvel[2] * m_params.velBias);
int ns = 0;
for (int k = 0; k < depth; ++k)
{
float minPenalty = float.MaxValue;
float[] bvel = new float[3];
Vector3f bvel = new Vector3f();
vSet(bvel, 0, 0, 0);
for (int i = 0; i < npat; ++i)
{
float[] vcand = new float[3];
Vector3f vcand = new Vector3f();
vSet(vcand, res[0] + pat[i * 2 + 0] * cr, 0f, res[2] + pat[i * 2 + 1] * cr);
if (sqr(vcand[0]) + sqr(vcand[2]) > sqr(vmax + 0.001f))
continue;

4
src/DotRecast.Detour.Crowd/PathCorridor.cs
View File

@ -64,8 +64,8 @@ namespace DotRecast.Detour.Crowd
*/
public class PathCorridor
{
private readonly float[] m_pos = new float[3];
private readonly float[] m_target = new float[3];
private readonly Vector3f m_pos = new Vector3f();
private readonly Vector3f m_target = new Vector3f();
private List<long> m_path;
protected List<long> mergeCorridorStartMoved(List<long> path, List<long> visited)

4
src/DotRecast.Detour.Crowd/PathQuery.cs
View File

@ -23,9 +23,9 @@ namespace DotRecast.Detour.Crowd
public class PathQuery
{
/// Path find start and end location.
public float[] startPos = new float[3];
public Vector3f startPos = new Vector3f();
public float[] endPos = new float[3];
public Vector3f endPos = new Vector3f();
public long startRef;
public long endRef;
public QueryFilter filter;

4
src/DotRecast.Detour.Crowd/Tracking/CrowdAgentDebugInfo.cs
View File

@ -23,8 +23,8 @@ namespace DotRecast.Detour.Crowd.Tracking
public class CrowdAgentDebugInfo
{
public CrowdAgent agent;
public float[] optStart = new float[3];
public float[] optEnd = new float[3];
public Vector3f optStart = new Vector3f();
public Vector3f optEnd = new Vector3f();
public ObstacleAvoidanceDebugData vod;
}
}

2
src/DotRecast.Detour.Crowd/Tracking/ObstacleAvoidanceDebugData.cs
View File

@ -102,7 +102,7 @@ namespace DotRecast.Detour.Crowd.Tracking
public float[] getSampleVelocity(int i)
{
float[] vel = new float[3];
Vector3f vel = new Vector3f();
vel[0] = m_vel[i * 3];
vel[1] = m_vel[i * 3 + 1];
vel[2] = m_vel[i * 3 + 2];

2
src/DotRecast.Detour.Dynamic/Io/VoxelFile.cs
View File

@ -52,7 +52,7 @@ namespace DotRecast.Detour.Dynamic.Io
public bool useTiles;
public int tileSizeX;
public int tileSizeZ;
public float[] rotation = new float[3];
public Vector3f rotation = new Vector3f();
public float[] bounds = new float[6];
public readonly List<VoxelTile> tiles = new List<VoxelTile>();

4
src/DotRecast.Detour.Dynamic/Io/VoxelFileReader.cs
View File

@ -101,11 +101,11 @@ namespace DotRecast.Detour.Dynamic.Io
int width = buf.getInt();
int depth = buf.getInt();
int borderSize = buf.getInt();
float[] boundsMin = new float[3];
Vector3f boundsMin = new Vector3f();
boundsMin[0] = buf.getFloat();
boundsMin[1] = buf.getFloat();
boundsMin[2] = buf.getFloat();
float[] boundsMax = new float[3];
Vector3f boundsMax = new Vector3f();
boundsMax[0] = buf.getFloat();
boundsMax[1] = buf.getFloat();
boundsMax[2] = buf.getFloat();

4
src/DotRecast.Detour.Extras/BVTreeBuilder.cs
View File

@ -38,8 +38,8 @@ namespace DotRecast.Detour.Extras
NavMeshBuilder.BVItem it = new NavMeshBuilder.BVItem();
items[i] = it;
it.i = i;
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
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++)

4
src/DotRecast.Detour.Extras/Jumplink/Edge.cs
View File

@ -2,7 +2,7 @@ namespace DotRecast.Detour.Extras.Jumplink
{
public class Edge
{
public readonly float[] sp = new float[3];
public readonly float[] sq = new float[3];
public readonly Vector3f sp = new Vector3f();
public readonly Vector3f sq = new Vector3f();
}
}

6
src/DotRecast.Detour.Extras/Jumplink/EdgeSampler.cs
View File

@ -9,9 +9,9 @@ namespace DotRecast.Detour.Extras.Jumplink
public readonly List<GroundSegment> end = new List<GroundSegment>();
public readonly Trajectory trajectory;
public readonly float[] ax = new float[3];
public readonly float[] ay = new float[3];
public readonly float[] az = new float[3];
public readonly Vector3f ax = new Vector3f();
public readonly Vector3f ay = new Vector3f();
public readonly Vector3f az = new Vector3f();
public EdgeSampler(Edge edge, Trajectory trajectory)
{

4
src/DotRecast.Detour.Extras/Jumplink/EdgeSamplerFactory.cs
View File

@ -28,7 +28,7 @@ namespace DotRecast.Detour.Extras.Jumplink
{
EdgeSampler es = new EdgeSampler(edge, new JumpTrajectory(acfg.jumpHeight));
es.start.height = acfg.agentClimb * 2;
float[] offset = new float[3];
Vector3f offset = new Vector3f();
trans2d(offset, es.az, es.ay, new float[] { acfg.startDistance, -acfg.agentClimb });
vadd(es.start.p, edge.sp, offset);
vadd(es.start.q, edge.sq, offset);
@ -56,7 +56,7 @@ namespace DotRecast.Detour.Extras.Jumplink
{
EdgeSampler es = new EdgeSampler(edge, new ClimbTrajectory());
es.start.height = acfg.agentClimb * 2;
float[] offset = new float[3];
Vector3f offset = new Vector3f();
trans2d(offset, es.az, es.ay, new float[] { acfg.startDistance, -acfg.agentClimb });
vadd(es.start.p, edge.sp, offset);
vadd(es.start.q, edge.sq, offset);

2
src/DotRecast.Detour.Extras/Jumplink/GroundSample.cs
View File

@ -2,7 +2,7 @@ namespace DotRecast.Detour.Extras.Jumplink
{
public class GroundSample
{
public readonly float[] p = new float[3];
public readonly Vector3f p = new Vector3f();
public bool validTrajectory;
public bool validHeight;
}

4
src/DotRecast.Detour.Extras/Jumplink/GroundSegment.cs
View File

@ -2,8 +2,8 @@ namespace DotRecast.Detour.Extras.Jumplink
{
public class GroundSegment
{
public readonly float[] p = new float[3];
public readonly float[] q = new float[3];
public readonly Vector3f p = new Vector3f();
public readonly Vector3f q = new Vector3f();
public GroundSample[] gsamples;
public float height;
}

8
src/DotRecast.Detour.TileCache/TileCache.cs
View File

@ -455,8 +455,8 @@ namespace DotRecast.Detour.TileCache
foreach (long i in tiles)
{
CompressedTile tile = m_tiles[decodeTileIdTile(i)];
float[] tbmin = new float[3];
float[] tbmax = new float[3];
Vector3f tbmin = new Vector3f();
Vector3f tbmax = new Vector3f();
calcTightTileBounds(tile.header, tbmin, tbmax);
if (overlapBounds(bmin, bmax, tbmin, tbmax))
{
@ -499,8 +499,8 @@ namespace DotRecast.Detour.TileCache
if (req.action == ObstacleRequestAction.REQUEST_ADD)
{
// Find touched tiles.
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
getObstacleBounds(ob, bmin, bmax);
ob.touched = queryTiles(bmin, bmax);
// Add tiles to update list.

4
src/DotRecast.Detour.TileCache/TileCacheBuilder.cs
View File

@ -1859,8 +1859,8 @@ namespace DotRecast.Detour.TileCache
public void markCylinderArea(TileCacheLayer layer, float[] orig, float cs, float ch, float[] pos, float radius,
float height, int areaId)
{
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
bmin[0] = pos[0] - radius;
bmin[1] = pos[1];
bmin[2] = pos[2] - radius;

4
src/DotRecast.Detour.TileCache/TileCacheLayerHeader.cs
View File

@ -35,8 +35,8 @@ namespace DotRecast.Detour.TileCache
/// < Data version
public int tx, ty, tlayer;
public float[] bmin = new float[3];
public float[] bmax = new float[3];
public Vector3f bmin = new Vector3f();
public Vector3f bmax = new Vector3f();
public int hmin, hmax;
/// < Height min/max range

10
src/DotRecast.Detour.TileCache/TileCacheObstacle.cs
View File

@ -33,12 +33,12 @@ namespace DotRecast.Detour.TileCache
public readonly int index;
public TileCacheObstacleType type;
public readonly float[] pos = new float[3];
public readonly float[] bmin = new float[3];
public readonly float[] bmax = new float[3];
public readonly Vector3f pos = new Vector3f();
public readonly Vector3f bmin = new Vector3f();
public readonly Vector3f bmax = new Vector3f();
public float radius, height;
public readonly float[] center = new float[3];
public readonly float[] extents = new float[3];
public readonly Vector3f center = new Vector3f();
public readonly Vector3f extents = new Vector3f();
public readonly float[] rotAux = new float[2]; // { cos(0.5f*angle)*sin(-0.5f*angle); cos(0.5f*angle)*cos(0.5f*angle) - 0.5 }
public List<long> touched = new List<long>();
public readonly List<long> pending = new List<long>();

2
src/DotRecast.Detour.TileCache/TileCacheParams.cs
View File

@ -22,7 +22,7 @@ namespace DotRecast.Detour.TileCache
{
public class TileCacheParams
{
public readonly float[] orig = new float[3];
public readonly Vector3f orig = new Vector3f();
public float cs, ch;
public int width, height;
public float walkableHeight;

75
src/DotRecast.Detour/ConvexConvexIntersection.cs
View File

@ -17,6 +17,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
namespace DotRecast.Detour
{
@ -50,10 +51,10 @@ namespace DotRecast.Detour
float[] inters = new float[Math.Max(m, n) * 3 * 3];
int ii = 0;
/* Initialize variables. */
float[] a = new float[3];
float[] b = new float[3];
float[] a1 = new float[3];
float[] b1 = new float[3];
Vector3f a = new Vector3f();
Vector3f b = new Vector3f();
Vector3f a1 = new Vector3f();
Vector3f b1 = new Vector3f();
int aa = 0;
int ba = 0;
@ -62,15 +63,15 @@ namespace DotRecast.Detour
InFlag f = InFlag.Unknown;
bool FirstPoint = true;
float[] ip = new float[3];
float[] iq = new float[3];
Vector3f ip = new Vector3f();
Vector3f iq = new Vector3f();
do
{
vCopy(a, p, 3 * (ai % n));
vCopy(b, q, 3 * (bi % m));
vCopy(a1, p, 3 * ((ai + n - 1) % n)); // prev a
vCopy(b1, q, 3 * ((bi + m - 1) % m)); // prev b
vCopy(ref a, p, 3 * (ai % n));
vCopy(ref b, q, 3 * (bi % m));
vCopy(ref a1, p, 3 * ((ai + n - 1) % n)); // prev a
vCopy(ref b1, q, 3 * ((bi + m - 1) % m)); // prev b
float[] A = vSub(a, a1);
float[] B = vSub(b, b1);
@ -84,7 +85,7 @@ namespace DotRecast.Detour
}
bool parallel = cross == 0f;
Intersection code = parallel ? parallelInt(a1, a, b1, b, ip, iq) : segSegInt(a1, a, b1, b, ip, iq);
Intersection code = parallel ? parallelInt(a1, a, b1, b, ref ip, ref iq) : segSegInt(a1, a, b1, b, ref ip, ref iq);
if (code == Intersection.Single)
{
@ -210,6 +211,28 @@ namespace DotRecast.Detour
return ii + 3;
}
private static int addVertex(float[] inters, int ii, Vector3f p)
{
if (ii > 0)
{
if (inters[ii - 3] == p[0] && inters[ii - 2] == p[1] && inters[ii - 1] == p[2])
{
return ii;
}
if (inters[0] == p[0] && inters[1] == p[1] && inters[2] == p[2])
{
return ii;
}
}
inters[ii] = p[0];
inters[ii + 1] = p[1];
inters[ii + 2] = p[2];
return ii + 3;
}
private static InFlag inOut(InFlag inflag, float aHB, float bHA)
{
if (aHB > 0)
@ -224,7 +247,7 @@ namespace DotRecast.Detour
return inflag;
}
private static Intersection segSegInt(float[] a, float[] b, float[] c, float[] d, float[] p, float[] q)
private static Intersection segSegInt(Vector3f a, Vector3f b, Vector3f c, Vector3f d, ref Vector3f p, ref Vector3f q)
{
var isec = intersectSegSeg2D(a, b, c, d);
if (null != isec)
@ -243,54 +266,54 @@ namespace DotRecast.Detour
return Intersection.None;
}
private static Intersection parallelInt(float[] a, float[] b, float[] c, float[] d, float[] p, float[] q)
private static Intersection parallelInt(Vector3f a, Vector3f b, Vector3f c, Vector3f d, ref Vector3f p, ref Vector3f q)
{
if (between(a, b, c) && between(a, b, d))
{
vCopy(p, c);
vCopy(q, d);
vCopy(ref p, c);
vCopy(ref q, d);
return Intersection.Overlap;
}
if (between(c, d, a) && between(c, d, b))
{
vCopy(p, a);
vCopy(q, b);
vCopy(ref p, a);
vCopy(ref q, b);
return Intersection.Overlap;
}
if (between(a, b, c) && between(c, d, b))
{
vCopy(p, c);
vCopy(q, b);
vCopy(ref p, c);
vCopy(ref q, b);
return Intersection.Overlap;
}
if (between(a, b, c) && between(c, d, a))
{
vCopy(p, c);
vCopy(q, a);
vCopy(ref p, c);
vCopy(ref q, a);
return Intersection.Overlap;
}
if (between(a, b, d) && between(c, d, b))
{
vCopy(p, d);
vCopy(q, b);
vCopy(ref p, d);
vCopy(ref q, b);
return Intersection.Overlap;
}
if (between(a, b, d) && between(c, d, a))
{
vCopy(p, d);
vCopy(q, a);
vCopy(ref p, d);
vCopy(ref q, a);
return Intersection.Overlap;
}
return Intersection.None;
}
private static bool between(float[] a, float[] b, float[] c)
private static bool between(Vector3f a, Vector3f b, Vector3f c)
{
if (Math.Abs(a[0] - b[0]) > Math.Abs(a[2] - b[2]))
{

16
src/DotRecast.Detour/LegacyNavMeshQuery.cs
View File

@ -60,7 +60,7 @@ namespace DotRecast.Detour
m_openList.clear();
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, startPos);
vCopy(ref startNode.pos, startPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = vDist(startPos, endPos) * H_SCALE;
@ -148,7 +148,7 @@ namespace DotRecast.Detour
neighbourTile);
if (!midpod.failed())
{
neighbourNode.pos = midpod.result;
neighbourNode.pos = Vector3f.Of(midpod.result);
}
}
@ -160,9 +160,9 @@ namespace DotRecast.Detour
if (neighbourRef == endRef)
{
// Cost
float curCost = filter.getCost(bestNode.pos, neighbourNode.pos, parentRef, parentTile, parentPoly,
float curCost = filter.getCost(bestNode.pos.ToArray(), neighbourNode.pos.ToArray(), parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly);
float endCost = filter.getCost(neighbourNode.pos, endPos, bestRef, bestTile, bestPoly, neighbourRef,
float endCost = filter.getCost(neighbourNode.pos.ToArray(), endPos, bestRef, bestTile, bestPoly, neighbourRef,
neighbourTile, neighbourPoly, 0L, null, null);
cost = bestNode.cost + curCost + endCost;
@ -171,7 +171,7 @@ namespace DotRecast.Detour
else
{
// Cost
float curCost = filter.getCost(bestNode.pos, neighbourNode.pos, parentRef, parentTile, parentPoly,
float curCost = filter.getCost(bestNode.pos.ToArray(), neighbourNode.pos.ToArray(), parentRef, parentTile, parentPoly,
bestRef, bestTile, bestPoly, neighbourRef, neighbourTile, neighbourPoly);
cost = bestNode.cost + curCost;
heuristic = vDist(neighbourNode.pos, endPos) * H_SCALE;
@ -365,7 +365,7 @@ namespace DotRecast.Detour
neighbourTile);
if (!midpod.failed())
{
neighbourNode.pos = midpod.result;
neighbourNode.pos = Vector3f.Of(midpod.result);
}
}
@ -669,7 +669,7 @@ namespace DotRecast.Detour
m_openList.push(startNode);
float radiusSqr = sqr(maxRadius);
float[] hitPos = new float[3];
Vector3f hitPos = new Vector3f();
VectorPtr bestvj = null;
VectorPtr bestvi = null;
while (!m_openList.isEmpty())
@ -842,7 +842,7 @@ namespace DotRecast.Detour
}
// Calc hit normal.
float[] hitNormal = new float[3];
Vector3f hitNormal = new Vector3f();
if (bestvi != null && bestvj != null)
{
float[] tangent = vSub(bestvi, bestvj);

6
src/DotRecast.Detour/MeshHeader.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
/** Provides high level information related to a dtMeshTile object. */
@ -95,10 +97,10 @@ namespace DotRecast.Detour
public float walkableClimb;
/** The minimum bounds of the tile's AABB. [(x, y, z)] */
public readonly float[] bmin = new float[3];
public Vector3f bmin = new Vector3f();
/** The maximum bounds of the tile's AABB. [(x, y, z)] */
public readonly float[] bmax = new float[3];
public Vector3f bmax = new Vector3f();
/** The bounding volume quantization factor. */
public float bvQuantFactor;

28
src/DotRecast.Detour/NavMesh.cs
View File

@ -55,7 +55,7 @@ namespace DotRecast.Detour
private readonly NavMeshParams m_params;
/// < Current initialization params. TODO: do not store this info twice.
private readonly float[] m_orig;
private readonly Vector3f m_orig;
/// < Origin of the tile (0,0)
// float m_orig[3]; ///< Origin of the tile (0,0)
@ -324,7 +324,7 @@ namespace DotRecast.Detour
private static NavMeshParams getNavMeshParams(MeshData data)
{
NavMeshParams option = new NavMeshParams();
vCopy(option.orig, data.header.bmin);
vCopy(ref option.orig, data.header.bmin);
option.tileWidth = data.header.bmax[0] - data.header.bmin[0];
option.tileHeight = data.header.bmax[2] - data.header.bmin[2];
option.maxTiles = 1;
@ -341,8 +341,8 @@ namespace DotRecast.Detour
if (tile.data.bvTree != null)
{
int nodeIndex = 0;
float[] tbmin = tile.data.header.bmin;
float[] tbmax = tile.data.header.bmax;
var tbmin = tile.data.header.bmin;
var tbmax = tile.data.header.bmax;
float qfac = tile.data.header.bvQuantFactor;
// Calculate quantized box
int[] bmin = new int[3];
@ -391,8 +391,8 @@ namespace DotRecast.Detour
}
else
{
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
long @base = getPolyRefBase(tile);
for (int i = 0; i < tile.data.header.polyCount; ++i)
{
@ -405,13 +405,13 @@ namespace DotRecast.Detour
// Calc polygon bounds.
int v = p.verts[0] * 3;
vCopy(bmin, tile.data.verts, v);
vCopy(bmax, tile.data.verts, v);
vCopy(ref bmin, tile.data.verts, v);
vCopy(ref bmax, tile.data.verts, v);
for (int j = 1; j < p.vertCount; ++j)
{
v = p.verts[j] * 3;
vMin(bmin, tile.data.verts, v);
vMax(bmax, tile.data.verts, v);
vMin(ref bmin, tile.data.verts, v);
vMax(ref bmax, tile.data.verts, v);
}
if (overlapBounds(qmin, qmax, bmin, bmax))
@ -836,7 +836,7 @@ namespace DotRecast.Detour
float[] ext = new float[] { targetCon.rad, target.data.header.walkableClimb, targetCon.rad };
// Find polygon to connect to.
float[] p = new float[3];
Vector3f p = new Vector3f();
p[0] = targetCon.pos[3];
p[1] = targetCon.pos[4];
p[2] = targetCon.pos[5];
@ -1309,7 +1309,7 @@ namespace DotRecast.Detour
Tuple<MeshTile, Poly> tileAndPoly = getTileAndPolyByRefUnsafe(refs);
MeshTile tile = tileAndPoly.Item1;
Poly poly = tileAndPoly.Item2;
float[] closest = new float[3];
Vector3f closest = new Vector3f();
vCopy(closest, pos);
float? h = getPolyHeight(tile, poly, pos);
if (null != h)
@ -1555,8 +1555,8 @@ namespace DotRecast.Detour
}
}
float[] startPos = new float[3];
float[] endPos = new float[3];
Vector3f startPos = new Vector3f();
Vector3f endPos = new Vector3f();
vCopy(startPos, tile.data.verts, poly.verts[idx0] * 3);
vCopy(endPos, tile.data.verts, poly.verts[idx1] * 3);
return Results.success(Tuple.Create(startPos, endPos));

16
src/DotRecast.Detour/NavMeshBuilder.cs
View File

@ -190,15 +190,15 @@ namespace DotRecast.Detour
{
int vb = option.detailMeshes[i * 4 + 0];
int ndv = option.detailMeshes[i * 4 + 1];
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
int dv = vb * 3;
vCopy(bmin, option.detailVerts, dv);
vCopy(bmax, option.detailVerts, dv);
vCopy(ref bmin, option.detailVerts, dv);
vCopy(ref bmax, option.detailVerts, dv);
for (int j = 1; j < ndv; j++)
{
vMin(bmin, option.detailVerts, dv + j * 3);
vMax(bmax, option.detailVerts, dv + j * 3);
vMin(ref bmin, option.detailVerts, dv + j * 3);
vMax(ref bmax, option.detailVerts, dv + j * 3);
}
// BV-tree uses cs for all dimensions
@ -341,8 +341,8 @@ namespace DotRecast.Detour
hmin -= option.walkableClimb;
hmax += option.walkableClimb;
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
vCopy(bmin, option.bmin);
vCopy(bmax, option.bmax);
bmin[1] = hmin;

4
src/DotRecast.Detour/NavMeshParams.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
/**
@ -29,7 +31,7 @@ namespace DotRecast.Detour
public class NavMeshParams
{
/** The world space origin of the navigation mesh's tile space. [(x, y, z)] */
public readonly float[] orig = new float[3];
public Vector3f orig = new Vector3f();
/** The width of each tile. (Along the x-axis.) */
public float tileWidth;

24
src/DotRecast.Detour/NavMeshQuery.cs
View File

@ -263,7 +263,7 @@ namespace DotRecast.Detour
m_openList.clear();
Node startNode = m_nodePool.getNode(startRef);
vCopy(startNode.pos, centerPos);
vCopy(ref startNode.pos, centerPos);
startNode.pidx = 0;
startNode.cost = 0;
startNode.total = 0;
@ -382,7 +382,7 @@ namespace DotRecast.Detour
// Cost
if (neighbourNode.flags == 0)
{
neighbourNode.pos = vLerp(va, vb, 0.5f);
neighbourNode.pos = Vector3f.Of(vLerp(va, vb, 0.5f));
}
float total = bestNode.total + vDist(bestNode.pos, neighbourNode.pos);
@ -650,8 +650,8 @@ namespace DotRecast.Detour
}
else
{
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
long @base = m_nav.getPolyRefBase(tile);
for (int i = 0; i < tile.data.header.polyCount; ++i)
{
@ -1843,7 +1843,7 @@ namespace DotRecast.Detour
LinkedList<Node> stack = new LinkedList<Node>();
stack.AddLast(startNode);
float[] bestPos = new float[3];
Vector3f bestPos = new Vector3f();
float bestDist = float.MaxValue;
Node bestNode = null;
vCopy(bestPos, startPos);
@ -2045,8 +2045,8 @@ namespace DotRecast.Detour
protected Result<PortalResult> getPortalPoints(long from, Poly fromPoly, MeshTile fromTile, long to, Poly toPoly,
MeshTile toTile, int fromType, int toType)
{
float[] left = new float[3];
float[] right = new float[3];
Vector3f left = new Vector3f();
Vector3f right = new Vector3f();
// Find the link that points to the 'to' polygon.
Link link = null;
for (int i = fromTile.polyLinks[fromPoly.index]; i != NavMesh.DT_NULL_LINK; i = fromTile.links[i].next)
@ -2132,7 +2132,7 @@ namespace DotRecast.Detour
float[] left = ppoints.result.left;
float[] right = ppoints.result.right;
float[] mid = new float[3];
Vector3f mid = new Vector3f();
mid[0] = (left[0] + right[0]) * 0.5f;
mid[1] = (left[1] + right[1]) * 0.5f;
mid[2] = (left[2] + right[2]) * 0.5f;
@ -2230,9 +2230,9 @@ namespace DotRecast.Detour
float[] verts = new float[m_nav.getMaxVertsPerPoly() * 3 + 3];
float[] curPos = new float[3], lastPos = new float[3];
Vector3f curPos = new Vector3f(), lastPos = new Vector3f();
vCopy(curPos, startPos);
vCopy(ref curPos, startPos);
float[] dir = vSub(endPos, startPos);
MeshTile prevTile, tile, nextTile;
@ -3230,7 +3230,7 @@ namespace DotRecast.Detour
m_openList.push(startNode);
float radiusSqr = sqr(maxRadius);
float[] hitPos = new float[3];
Vector3f hitPos = new Vector3f();
VectorPtr bestvj = null;
VectorPtr bestvi = null;
while (!m_openList.isEmpty())
@ -3403,7 +3403,7 @@ namespace DotRecast.Detour
}
// Calc hit normal.
float[] hitNormal = new float[3];
Vector3f hitNormal = new Vector3f();
if (bestvi != null && bestvj != null)
{
float[] tangent = vSub(bestvi, bestvj);

3
src/DotRecast.Detour/Node.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour
{
@ -33,7 +34,7 @@ namespace DotRecast.Detour
public readonly int index;
/** Position of the node. */
public float[] pos = new float[3];
public Vector3f pos = new Vector3f();
/** Cost of reaching the given node. */
public float cost;

6
src/DotRecast.Detour/QueryData.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Detour
{
public class QueryData
@ -26,8 +28,8 @@ namespace DotRecast.Detour
public Node lastBestNode;
public float lastBestNodeCost;
public long startRef, endRef;
public float[] startPos = new float[3];
public float[] endPos = new float[3];
public Vector3f startPos = new Vector3f();
public Vector3f endPos = new Vector3f();
public QueryFilter filter;
public int options;
public float raycastLimitSqr;

3
src/DotRecast.Detour/RaycastHit.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Detour
{
@ -31,7 +32,7 @@ namespace DotRecast.Detour
public float t;
/** hitNormal The normal of the nearest wall hit. [(x, y, z)] */
public readonly float[] hitNormal = new float[3];
public readonly Vector3f hitNormal = new Vector3f();
/** Visited polygons. */
public readonly List<long> path = new List<long>();

10
src/DotRecast.Recast.Demo/Draw/DebugDraw.cs
View File

@ -253,12 +253,12 @@ public class DebugDraw
float dy = y1 - y0;
float dz = z1 - z0;
float len = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
float[] prev = new float[3];
Vector3f prev = new Vector3f();
evalArc(x0, y0, z0, dx, dy, dz, len * h, PAD, prev);
for (int i = 1; i <= NUM_ARC_PTS; ++i)
{
float u = PAD + i * ARC_PTS_SCALE;
float[] pt = new float[3];
Vector3f pt = new Vector3f();
evalArc(x0, y0, z0, dx, dy, dz, len * h, u, pt);
vertex(prev[0], prev[1], prev[2], col);
vertex(pt[0], pt[1], pt[2], col);
@ -270,7 +270,7 @@ public class DebugDraw
// End arrows
if (as0 > 0.001f)
{
float[] p = new float[3], q = new float[3];
Vector3f p = new Vector3f(), q = new float[3];
evalArc(x0, y0, z0, dx, dy, dz, len * h, PAD, p);
evalArc(x0, y0, z0, dx, dy, dz, len * h, PAD + 0.05f, q);
appendArrowHead(p, q, as0, col);
@ -278,7 +278,7 @@ public class DebugDraw
if (as1 > 0.001f)
{
float[] p = new float[3], q = new float[3];
Vector3f p = new Vector3f(), q = new float[3];
evalArc(x0, y0, z0, dx, dy, dz, len * h, 1 - PAD, p);
evalArc(x0, y0, z0, dx, dy, dz, len * h, 1 - (PAD + 0.05f), q);
appendArrowHead(p, q, as1, col);
@ -392,7 +392,7 @@ public class DebugDraw
return;
}
float[] ax = new float[3], ay = { 0, 1, 0 }, az = new float[3];
Vector3f ax = new Vector3f(), ay = { 0, 1, 0 }, az = new float[3];
vsub(az, q, p);
vnormalize(az);
vcross(ax, ay, az);

2
src/DotRecast.Recast.Demo/Draw/RecastDebugDraw.cs
View File

@ -577,7 +577,7 @@ public class RecastDebugDraw : DebugDraw
private float[] getContourCenter(Contour cont, float[] orig, float cs, float ch)
{
float[] center = new float[3];
Vector3f center = new Vector3f();
center[0] = 0;
center[1] = 0;
center[2] = 0;

2
src/DotRecast.Recast.Demo/Geom/DemoInputGeomProvider.cs
View File

@ -100,7 +100,7 @@ public class DemoInputGeomProvider : InputGeomProvider
int v0 = faces[i] * 3;
int v1 = faces[i + 1] * 3;
int v2 = faces[i + 2] * 3;
float[] e0 = new float[3], e1 = new float[3];
Vector3f e0 = new Vector3f(), e1 = new float[3];
for (int j = 0; j < 3; ++j)
{
e0[j] = vertices[v1 + j] - vertices[v0 + j];

2
src/DotRecast.Recast.Demo/Geom/Intersections.cs
View File

@ -82,7 +82,7 @@ public class Intersections
{
float EPS = 1e-6f;
float[] d = new float[3];
Vector3f d = new Vector3f();
d[0] = sq[0] - sp[0];
d[1] = sq[1] - sp[1];
d[2] = sq[2] - sp[2];

10
src/DotRecast.Recast.Demo/RecastDemo.cs
View File

@ -91,13 +91,13 @@ public class RecastDemo
private float scrollZoom;
private readonly float[] origMousePos = new float[2];
private readonly float[] origCameraEulers = new float[2];
private readonly float[] origCameraPos = new float[3];
private readonly Vector3f origCameraPos = new Vector3f();
private readonly float[] cameraEulers = { 45, -45 };
private readonly float[] cameraPos = { 0, 0, 0 };
private readonly float[] rayStart = new float[3];
private readonly float[] rayEnd = new float[3];
private readonly Vector3f rayStart = new Vector3f();
private readonly Vector3f rayEnd = new Vector3f();
private float[] projectionMatrix = new float[16];
private float[] modelviewMatrix = new float[16];
@ -112,7 +112,7 @@ public class RecastDemo
private int[] viewport;
private bool markerPositionSet;
private readonly float[] markerPosition = new float[3];
private readonly Vector3f markerPosition = new Vector3f();
private ToolsView toolsUI;
private RcSettingsView settingsUI;
private long prevFrameTime;
@ -656,7 +656,7 @@ public class RecastDemo
}
else
{
float[] pos = new float[3];
Vector3f pos = new Vector3f();
pos[0] = rayStart[0] + (rayEnd[0] - rayStart[0]) * hitTime;
pos[1] = rayStart[1] + (rayEnd[1] - rayStart[1]) * hitTime;
pos[2] = rayStart[2] + (rayEnd[2] - rayStart[2]) * hitTime;

2
src/DotRecast.Recast.Demo/Tools/CrowdProfilingTool.cs
View File

@ -420,7 +420,7 @@ public class CrowdProfilingTool
private class AgentData
{
public readonly AgentType type;
public readonly float[] home = new float[3];
public readonly Vector3f home = new Vector3f();
public AgentData(AgentType type, float[] home)
{

4
src/DotRecast.Recast.Demo/Tools/CrowdTool.cs
View File

@ -265,7 +265,7 @@ public class CrowdTool : Tool
foreach (CrowdAgent ag in crowd.getActiveAgents())
{
float[] bmin = new float[3], bmax = new float[3];
Vector3f bmin = new Vector3f(), bmax = new float[3];
getAgentBounds(ag, bmin, bmax);
float[] isect = Intersections.intersectSegmentAABB(s, p, bmin, bmax);
if (null != isect)
@ -432,7 +432,7 @@ public class CrowdTool : Tool
float[] pos = ag.npos;
dd.begin(LINES, 3.0f);
float[] prev = new float[3];
Vector3f prev = new Vector3f();
float preva = 1;
vCopy(prev, pos);
for (int j = 0; j < AGENT_MAX_TRAIL - 1; ++j)

4
src/DotRecast.Recast.Demo/Tools/DynamicUpdateTool.cs
View File

@ -340,8 +340,8 @@ public class DynamicUpdateTool : Tool
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];
float[] v = new float[3];
float[] vr = new float[3];
Vector3f v = new Vector3f();
Vector3f vr = new Vector3f();
for (int i = 0; i < geom.vertices.Length; i += 3)
{
v[0] = geom.vertices[i];

4
src/DotRecast.Recast.Demo/Tools/Gizmos/CapsuleGizmo.cs
View File

@ -35,7 +35,7 @@ public class CapsuleGizmo : ColliderGizmo
float halfLength = 0.5f * vLen(axis);
vertices = new float[spVertices.Length];
gradient = new float[spVertices.Length / 3];
float[] v = new float[3];
Vector3f v = new Vector3f();
for (int i = 0; i < spVertices.Length; i += 3)
{
float offset = (i >= spVertices.Length / 2) ? -halfLength : halfLength;
@ -61,7 +61,7 @@ public class CapsuleGizmo : ColliderGizmo
side = new float[] { 0, 0, 1 };
}
float[] forward = new float[3];
Vector3f forward = new Vector3f();
cross(forward, side, axis);
cross(side, axis, forward);
normalize(side);

4
src/DotRecast.Recast.Demo/Tools/Gizmos/CylinderGizmo.cs
View File

@ -35,7 +35,7 @@ public class CylinderGizmo : ColliderGizmo
vertices = generateCylindricalVertices();
float halfLength = 0.5f * vLen(axis);
gradient = new float[vertices.Length / 3];
float[] v = new float[3];
Vector3f v = new Vector3f();
for (int i = 0; i < vertices.Length; i += 3)
{
float offset = (i >= vertices.Length / 2) ? -halfLength : halfLength;
@ -68,7 +68,7 @@ public class CylinderGizmo : ColliderGizmo
side = new float[] { 0, 0, 1 };
}
float[] forward = new float[3];
Vector3f forward = new Vector3f();
cross(forward, side, axis);
cross(side, axis, forward);
normalize(side);

4
src/DotRecast.Recast.Demo/Tools/Gizmos/GizmoHelper.cs
View File

@ -175,8 +175,8 @@ public class GizmoHelper
public static int getColorByNormal(float[] vertices, int v0, int v1, int v2)
{
float[] e0 = new float[3], e1 = new float[3];
float[] normal = new float[3];
Vector3f e0 = new Vector3f(), e1 = new float[3];
Vector3f normal = new Vector3f();
for (int j = 0; j < 3; ++j)
{
e0[j] = vertices[v1 + j] - vertices[v0 + j];

2
src/DotRecast.Recast.Demo/Tools/JumpLinkBuilderTool.cs
View File

@ -431,7 +431,7 @@ public class JumpLinkBuilderTool : Tool
{
int area = SampleAreaModifications.SAMPLE_POLYAREA_TYPE_JUMP_AUTO;
int flags = SampleAreaModifications.SAMPLE_POLYFLAGS_JUMP;
float[] prev = new float[3];
Vector3f prev = new Vector3f();
for (int i = 0; i < link.startSamples.Length; i++)
{
float[] p = link.startSamples[i].p;

4
src/DotRecast.Recast.Demo/Tools/TestNavmeshTool.cs
View File

@ -949,7 +949,7 @@ public class TestNavmeshTool : Tool
private float[] getPolyCenter(NavMesh navMesh, long refs)
{
float[] center = new float[3];
Vector3f center = new Vector3f();
center[0] = 0;
center[1] = 0;
center[2] = 0;
@ -993,7 +993,7 @@ public class TestNavmeshTool : Tool
if (m_polys != null)
{
// In case of partial path, make sure the end point is clamped to the last polygon.
float[] epos = new float[3];
Vector3f epos = new Vector3f();
vCopy(epos, m_epos);
if (m_polys[m_polys.Count - 1] != m_endRef)
{

6
src/DotRecast.Recast/CompactHeightfield.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Recast
{
/** A compact, static heightfield representing unobstructed space. */
@ -48,10 +50,10 @@ namespace DotRecast.Recast
public int maxRegions;
/** The minimum bounds in world space. [(x, y, z)] */
public float[] bmin = new float[3];
public Vector3f bmin = new Vector3f();
/** The maximum bounds in world space. [(x, y, z)] */
public float[] bmax = new float[3];
public Vector3f bmax = new Vector3f();
/** The size of each cell. (On the xz-plane.) */
public float cs;

5
src/DotRecast.Recast/ContourSet.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Recast
{
@ -29,10 +30,10 @@ namespace DotRecast.Recast
public List<Contour> conts = new List<Contour>();
/** The minimum bounds in world space. [(x, y, z)] */
public float[] bmin = new float[3];
public Vector3f bmin = new Vector3f();
/** The maximum bounds in world space. [(x, y, z)] */
public float[] bmax = new float[3];
public Vector3f bmax = new Vector3f();
/** The size of each cell. (On the xz-plane.) */
public float cs;

3
src/DotRecast.Recast/Geom/SimpleInputGeomProvider.cs
View File

@ -21,6 +21,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using System.Collections.Immutable;
using DotRecast.Core;
namespace DotRecast.Recast.Geom
{
@ -114,7 +115,7 @@ namespace DotRecast.Recast.Geom
int v0 = faces[i] * 3;
int v1 = faces[i + 1] * 3;
int v2 = faces[i + 2] * 3;
float[] e0 = new float[3], e1 = new float[3];
Vector3f e0 = new Vector3f(), e1 = new Vector3f();
for (int j = 0; j < 3; ++j)
{
e0[j] = vertices[v1 + j] - vertices[v0 + j];

6
src/DotRecast.Recast/HeightfieldLayerSet.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Recast
{
/// Represents a set of heightfield layers.
@ -29,10 +31,10 @@ namespace DotRecast.Recast
/// @see rcHeightfieldLayerSet
public class HeightfieldLayer
{
public readonly float[] bmin = new float[3];
public Vector3f bmin = new Vector3f();
/// < The minimum bounds in world space. [(x, y, z)]
public readonly float[] bmax = new float[3];
public Vector3f bmax = new Vector3f();
/// < The maximum bounds in world space. [(x, y, z)]
public float cs;

6
src/DotRecast.Recast/PolyMesh.cs
View File

@ -17,6 +17,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Recast
{
/** Represents a polygon mesh suitable for use in building a navigation mesh. */
@ -50,10 +52,10 @@ namespace DotRecast.Recast
public int[] flags;
/** The minimum bounds in world space. [(x, y, z)] */
public readonly float[] bmin = new float[3];
public Vector3f bmin = new Vector3f();
/** The maximum bounds in world space. [(x, y, z)] */
public readonly float[] bmax = new float[3];
public Vector3f bmax = new Vector3f();
/** The size of each cell. (On the xz-plane.) */
public float cs;

39
src/DotRecast.Recast/Recast.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
namespace DotRecast.Recast
{
@ -50,6 +51,17 @@ namespace DotRecast.Recast
return new int[] { (int)((bmax[0] - bmin[0]) / cs + 0.5f), (int)((bmax[2] - bmin[2]) / cs + 0.5f) };
}
public static int[] calcGridSize(Vector3f bmin, float[] bmax, float cs)
{
return new int[] { (int)((bmax[0] - bmin[0]) / cs + 0.5f), (int)((bmax[2] - bmin[2]) / cs + 0.5f) };
}
public static int[] calcGridSize(Vector3f bmin, Vector3f bmax, float cs)
{
return new int[] { (int)((bmax[0] - bmin[0]) / cs + 0.5f), (int)((bmax[2] - bmin[2]) / cs + 0.5f) };
}
public static int[] calcTileCount(float[] bmin, float[] bmax, float cs, int tileSizeX, int tileSizeZ)
{
int[] gwd = Recast.calcGridSize(bmin, bmax, cs);
@ -72,11 +84,11 @@ namespace DotRecast.Recast
{
int[] areas = new int[nt];
float walkableThr = (float)Math.Cos(walkableSlopeAngle / 180.0f * Math.PI);
float[] norm = new float[3];
Vector3f norm = new Vector3f();
for (int i = 0; i < nt; ++i)
{
int tri = i * 3;
calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], norm);
calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], ref norm);
// Check if the face is walkable.
if (norm[1] > walkableThr)
areas[i] = areaMod.apply(areas[i]);
@ -87,14 +99,25 @@ namespace DotRecast.Recast
static void calcTriNormal(float[] verts, int v0, int v1, int v2, float[] norm)
{
float[] e0 = new float[3];
float[] e1 = new float[3];
RecastVectors.sub(e0, verts, v1 * 3, v0 * 3);
RecastVectors.sub(e1, verts, v2 * 3, v0 * 3);
Vector3f e0 = new Vector3f();
Vector3f e1 = new Vector3f();
RecastVectors.sub(ref e0, verts, v1 * 3, v0 * 3);
RecastVectors.sub(ref e1, verts, v2 * 3, v0 * 3);
RecastVectors.cross(norm, e0, e1);
RecastVectors.normalize(norm);
}
static void calcTriNormal(float[] verts, int v0, int v1, int v2, ref Vector3f norm)
{
Vector3f e0 = new Vector3f();
Vector3f e1 = new Vector3f();
RecastVectors.sub(ref e0, verts, v1 * 3, v0 * 3);
RecastVectors.sub(ref e1, verts, v2 * 3, v0 * 3);
RecastVectors.cross(ref norm, e0, e1);
RecastVectors.normalize(ref norm);
}
/// @par
///
/// Only sets the area id's for the unwalkable triangles. Does not alter the
@ -108,12 +131,12 @@ namespace DotRecast.Recast
{
float walkableThr = (float)Math.Cos(walkableSlopeAngle / 180.0f * Math.PI);
float[] norm = new float[3];
Vector3f norm = new Vector3f();
for (int i = 0; i < nt; ++i)
{
int tri = i * 3;
calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], norm);
calcTriNormal(verts, tris[tri], tris[tri + 1], tris[tri + 2], ref norm);
// Check if the face is walkable.
if (norm[1] <= walkableThr)
areas[i] = RC_NULL_AREA;

21
src/DotRecast.Recast/RecastArea.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
namespace DotRecast.Recast
{
@ -331,7 +332,7 @@ namespace DotRecast.Recast
ctx.stopTimer("MARK_BOX_AREA");
}
static bool pointInPoly(float[] verts, float[] p)
static bool pointInPoly(float[] verts, Vector3f p)
{
bool c = false;
int i, j;
@ -361,14 +362,14 @@ namespace DotRecast.Recast
{
ctx.startTimer("MARK_CONVEXPOLY_AREA");
float[] bmin = new float[3];
float[] bmax = new float[3];
RecastVectors.copy(bmin, verts, 0);
RecastVectors.copy(bmax, verts, 0);
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
RecastVectors.copy(ref bmin, verts, 0);
RecastVectors.copy(ref bmax, verts, 0);
for (int i = 3; i < verts.Length; i += 3)
{
RecastVectors.min(bmin, verts, i);
RecastVectors.max(bmax, verts, i);
RecastVectors.min(ref bmin, verts, i);
RecastVectors.max(ref bmax, verts, i);
}
bmin[1] = hmin;
@ -412,7 +413,7 @@ namespace DotRecast.Recast
continue;
if (s.y >= miny && s.y <= maxy)
{
float[] p = new float[3];
Vector3f p = new Vector3f();
p[0] = chf.bmin[0] + (x + 0.5f) * chf.cs;
p[1] = 0;
p[2] = chf.bmin[2] + (z + 0.5f) * chf.cs;
@ -517,8 +518,8 @@ namespace DotRecast.Recast
{
ctx.startTimer("MARK_CYLINDER_AREA");
float[] bmin = new float[3];
float[] bmax = new float[3];
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
bmin[0] = pos[0] - r;
bmin[1] = pos[1];
bmin[2] = pos[2] - r;

10
src/DotRecast.Recast/RecastBuilderConfig.cs
View File

@ -18,6 +18,8 @@ freely, subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
using DotRecast.Core;
namespace DotRecast.Recast
{
using static RecastVectors;
@ -36,10 +38,10 @@ namespace DotRecast.Recast
public readonly int height;
/** The minimum bounds of the field's AABB. [(x, y, z)] [Units: wu] **/
public readonly float[] bmin = new float[3];
public readonly Vector3f bmin = new Vector3f();
/** The maximum bounds of the field's AABB. [(x, y, z)] [Units: wu] **/
public readonly float[] bmax = new float[3];
public readonly Vector3f bmax = new Vector3f();
public RecastBuilderConfig(RecastConfig cfg, float[] bmin, float[] bmax) : this(cfg, bmin, bmax, 0, 0)
{
@ -50,8 +52,8 @@ namespace DotRecast.Recast
this.tileX = tileX;
this.tileZ = tileZ;
this.cfg = cfg;
copy(this.bmin, bmin);
copy(this.bmax, bmax);
copy(ref this.bmin, bmin);
copy(ref this.bmax, bmax);
if (cfg.useTiles)
{
float tsx = cfg.tileSizeX * cfg.cs;

4
src/DotRecast.Recast/RecastCompact.cs
View File

@ -56,8 +56,8 @@ namespace DotRecast.Recast
chf.walkableHeight = walkableHeight;
chf.walkableClimb = walkableClimb;
chf.maxRegions = 0;
copy(chf.bmin, hf.bmin);
copy(chf.bmax, hf.bmax);
copy(ref chf.bmin, hf.bmin);
copy(ref chf.bmax, hf.bmax);
chf.bmax[1] += walkableHeight * hf.ch;
chf.cs = hf.cs;
chf.ch = hf.ch;

4
src/DotRecast.Recast/RecastContour.cs
View File

@ -781,8 +781,8 @@ namespace DotRecast.Recast
ContourSet cset = new ContourSet();
ctx.startTimer("CONTOURS");
RecastVectors.copy(cset.bmin, chf.bmin, 0);
RecastVectors.copy(cset.bmax, chf.bmax, 0);
RecastVectors.copy(ref cset.bmin, chf.bmin, 0);
RecastVectors.copy(ref cset.bmax, chf.bmax, 0);
if (borderSize > 0)
{
// If the heightfield was build with bordersize, remove the offset.

13
src/DotRecast.Recast/RecastLayers.cs
View File

@ -20,6 +20,7 @@ freely, subject to the following restrictions:
using System;
using System.Collections.Generic;
using DotRecast.Core;
namespace DotRecast.Recast
{
@ -427,10 +428,10 @@ namespace DotRecast.Recast
int lh = h - borderSize * 2;
// Build contracted bbox for layers.
float[] bmin = new float[3];
float[] bmax = new float[3];
copy(bmin, chf.bmin);
copy(bmax, chf.bmax);
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
copy(ref bmin, chf.bmin);
copy(ref bmax, chf.bmax);
bmin[0] += borderSize * chf.cs;
bmin[2] += borderSize * chf.cs;
bmax[0] -= borderSize * chf.cs;
@ -474,8 +475,8 @@ namespace DotRecast.Recast
layer.ch = chf.ch;
// Adjust the bbox to fit the heightfield.
copy(layer.bmin, bmin);
copy(layer.bmax, bmax);
copy(ref layer.bmin, bmin);
copy(ref layer.bmax, bmax);
layer.bmin[1] = bmin[1] + hmin * chf.ch;
layer.bmax[1] = bmin[1] + hmax * chf.ch;
layer.hmin = hmin;

16
src/DotRecast.Recast/RecastMesh.cs
View File

@ -976,8 +976,8 @@ namespace DotRecast.Recast
{
ctx.startTimer("POLYMESH");
PolyMesh mesh = new PolyMesh();
RecastVectors.copy(mesh.bmin, cset.bmin, 0);
RecastVectors.copy(mesh.bmax, cset.bmax, 0);
RecastVectors.copy(ref mesh.bmin, cset.bmin, 0);
RecastVectors.copy(ref mesh.bmax, cset.bmax, 0);
mesh.cs = cset.cs;
mesh.ch = cset.ch;
mesh.borderSize = cset.borderSize;
@ -1231,16 +1231,16 @@ namespace DotRecast.Recast
mesh.nvp = meshes[0].nvp;
mesh.cs = meshes[0].cs;
mesh.ch = meshes[0].ch;
RecastVectors.copy(mesh.bmin, meshes[0].bmin, 0);
RecastVectors.copy(mesh.bmax, meshes[0].bmax, 0);
RecastVectors.copy(ref mesh.bmin, meshes[0].bmin, 0);
RecastVectors.copy(ref mesh.bmax, meshes[0].bmax, 0);
int maxVerts = 0;
int maxPolys = 0;
int maxVertsPerMesh = 0;
for (int i = 0; i < nmeshes; ++i)
{
RecastVectors.min(mesh.bmin, meshes[i].bmin, 0);
RecastVectors.max(mesh.bmax, meshes[i].bmax, 0);
RecastVectors.min(ref mesh.bmin, meshes[i].bmin, 0);
RecastVectors.max(ref mesh.bmax, meshes[i].bmax, 0);
maxVertsPerMesh = Math.Max(maxVertsPerMesh, meshes[i].nverts);
maxVerts += meshes[i].nverts;
maxPolys += meshes[i].npolys;
@ -1361,8 +1361,8 @@ namespace DotRecast.Recast
dst.npolys = src.npolys;
dst.maxpolys = src.npolys;
dst.nvp = src.nvp;
RecastVectors.copy(dst.bmin, src.bmin, 0);
RecastVectors.copy(dst.bmax, src.bmax, 0);
RecastVectors.copy(ref dst.bmin, src.bmin, 0);
RecastVectors.copy(ref dst.bmax, src.bmax, 0);
dst.cs = src.cs;
dst.ch = src.ch;
dst.borderSize = src.borderSize;

118
src/DotRecast.Recast/RecastMeshDetail.cs
View File

@ -52,6 +52,12 @@ namespace DotRecast.Recast
return a[0] * b[0] + a[2] * b[2];
}
private static float vdot2(Vector3f a, Vector3f b)
{
return a[0] * b[0] + a[2] * b[2];
}
private static float vdistSq2(float[] verts, int p, int q)
{
float dx = verts[q + 0] - verts[p + 0];
@ -71,11 +77,39 @@ namespace DotRecast.Recast
return dx * dx + dy * dy;
}
private static float vdistSq2(float[] p, Vector3f q)
{
float dx = q[0] - p[0];
float dy = q[2] - p[2];
return dx * dx + dy * dy;
}
private static float vdistSq2(Vector3f p, Vector3f q)
{
float dx = q[0] - p[0];
float dy = q[2] - p[2];
return dx * dx + dy * dy;
}
private static float vdist2(float[] p, float[] q)
{
return (float)Math.Sqrt(vdistSq2(p, q));
}
private static float vdist2(Vector3f p, Vector3f q)
{
return (float)Math.Sqrt(vdistSq2(p, q));
}
private static float vdist2(float[] p, Vector3f q)
{
return (float)Math.Sqrt(vdistSq2(p, q));
}
private static float vdistSq2(float[] p, float[] verts, int q)
{
float dx = verts[q + 0] - p[0];
@ -83,11 +117,25 @@ namespace DotRecast.Recast
return dx * dx + dy * dy;
}
private static float vdistSq2(Vector3f p, float[] verts, int q)
{
float dx = verts[q + 0] - p[0];
float dy = verts[q + 2] - p[2];
return dx * dx + dy * dy;
}
private static float vdist2(float[] p, float[] verts, int q)
{
return (float)Math.Sqrt(vdistSq2(p, verts, q));
}
private static float vdist2(Vector3f p, float[] verts, int q)
{
return (float)Math.Sqrt(vdistSq2(p, verts, q));
}
private static float vcross2(float[] verts, int p1, int p2, int p3)
{
float u1 = verts[p2 + 0] - verts[p1 + 0];
@ -106,15 +154,25 @@ namespace DotRecast.Recast
return u1 * v2 - v1 * u2;
}
private static bool circumCircle(float[] verts, int p1, int p2, int p3, float[] c, AtomicFloat r)
private static float vcross2(Vector3f p1, Vector3f p2, Vector3f p3)
{
float u1 = p2[0] - p1[0];
float v1 = p2[2] - p1[2];
float u2 = p3[0] - p1[0];
float v2 = p3[2] - p1[2];
return u1 * v2 - v1 * u2;
}
private static bool circumCircle(float[] verts, int p1, int p2, int p3, ref Vector3f c, AtomicFloat r)
{
float EPS = 1e-6f;
// Calculate the circle relative to p1, to avoid some precision issues.
float[] v1 = new float[3];
float[] v2 = new float[3];
float[] v3 = new float[3];
RecastVectors.sub(v2, verts, p2, p1);
RecastVectors.sub(v3, verts, p3, p1);
Vector3f v1 = new Vector3f();
Vector3f v2 = new Vector3f();
Vector3f v3 = new Vector3f();
RecastVectors.sub(ref v2, verts, p2, p1);
RecastVectors.sub(ref v3, verts, p3, p1);
float cp = vcross2(v1, v2, v3);
if (Math.Abs(cp) > EPS)
@ -126,23 +184,23 @@ namespace DotRecast.Recast
c[1] = 0;
c[2] = (v1Sq * (v3[0] - v2[0]) + v2Sq * (v1[0] - v3[0]) + v3Sq * (v2[0] - v1[0])) / (2 * cp);
r.Exchange(vdist2(c, v1));
RecastVectors.add(c, c, verts, p1);
RecastVectors.add(ref c, c, verts, p1);
return true;
}
RecastVectors.copy(c, verts, p1);
RecastVectors.copy(ref c, verts, p1);
r.Exchange(0f);
return false;
}
private static float distPtTri(float[] p, float[] verts, int a, int b, int c)
{
float[] v0 = new float[3];
float[] v1 = new float[3];
float[] v2 = new float[3];
RecastVectors.sub(v0, verts, c, a);
RecastVectors.sub(v1, verts, b, a);
RecastVectors.sub(v2, p, verts, a);
Vector3f v0 = new Vector3f();
Vector3f v1 = new Vector3f();
Vector3f v2 = new Vector3f();
RecastVectors.sub(ref v0, verts, c, a);
RecastVectors.sub(ref v1, verts, b, a);
RecastVectors.sub(ref v2, p, verts, a);
float dot00 = vdot2(v0, v0);
float dot01 = vdot2(v0, v1);
@ -460,7 +518,7 @@ namespace DotRecast.Recast
// Find best point on left of edge.
int pt = npts;
float[] c = new float[3];
Vector3f c = new Vector3f();
AtomicFloat r = new AtomicFloat(-1f);
for (int u = 0; u < npts; ++u)
{
@ -475,7 +533,7 @@ namespace DotRecast.Recast
{
// The circle is not updated yet, do it now.
pt = u;
circumCircle(pts, s * 3, t * 3, u * 3, c, r);
circumCircle(pts, s * 3, t * 3, u * 3, ref c, r);
continue;
}
@ -490,7 +548,7 @@ namespace DotRecast.Recast
{
// Inside safe circumcircle, update circle.
pt = u;
circumCircle(pts, s * 3, t * 3, u * 3, c, r);
circumCircle(pts, s * 3, t * 3, u * 3, ref c, r);
}
else
{
@ -508,7 +566,7 @@ namespace DotRecast.Recast
// Edge is valid.
pt = u;
circumCircle(pts, s * 3, t * 3, u * 3, c, r);
circumCircle(pts, s * 3, t * 3, u * 3, ref c, r);
}
}
}
@ -927,14 +985,14 @@ namespace DotRecast.Recast
if (sampleDist > 0)
{
// Create sample locations in a grid.
float[] bmin = new float[3];
float[] bmax = new float[3];
RecastVectors.copy(bmin, @in, 0);
RecastVectors.copy(bmax, @in, 0);
Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f();
RecastVectors.copy(ref bmin, @in, 0);
RecastVectors.copy(ref bmax, @in, 0);
for (int i = 1; i < nin; ++i)
{
RecastVectors.min(bmin, @in, i * 3);
RecastVectors.max(bmax, @in, i * 3);
RecastVectors.min(ref bmin, @in, i * 3);
RecastVectors.max(ref bmax, @in, i * 3);
}
int x0 = (int)Math.Floor(bmin[0] / sampleDist);
@ -946,12 +1004,12 @@ namespace DotRecast.Recast
{
for (int x = x0; x < x1; ++x)
{
float[] pt = new float[3];
Vector3f pt = new Vector3f();
pt[0] = x * sampleDist;
pt[1] = (bmax[1] + bmin[1]) * 0.5f;
pt[2] = z * sampleDist;
// Make sure the samples are not too close to the edges.
if (distToPoly(nin, @in, pt) > -sampleDist / 2)
if (distToPoly(nin, @in, pt.ToArray()) > -sampleDist / 2)
{
continue;
}
@ -975,7 +1033,7 @@ namespace DotRecast.Recast
}
// Find sample with most error.
float[] bestpt = new float[3];
Vector3f bestpt = new Vector3f();
float bestd = 0;
int besti = -1;
for (int i = 0; i < nsamples; ++i)
@ -986,13 +1044,13 @@ namespace DotRecast.Recast
continue; // skip added.
}
float[] pt = new float[3];
Vector3f pt = new Vector3f();
// The sample location is jittered to get rid of some bad triangulations
// which are cause by symmetrical data from the grid structure.
pt[0] = samples[s + 0] * sampleDist + getJitterX(i) * cs * 0.1f;
pt[1] = samples[s + 1] * chf.ch;
pt[2] = samples[s + 2] * sampleDist + getJitterY(i) * cs * 0.1f;
float d = distToTriMesh(pt, verts, nverts, tris, tris.Count / 4);
float d = distToTriMesh(pt.ToArray(), verts, nverts, tris, tris.Count / 4);
if (d < 0)
{
continue; // did not hit the mesh.
@ -1360,7 +1418,7 @@ namespace DotRecast.Recast
int nvp = mesh.nvp;
float cs = mesh.cs;
float ch = mesh.ch;
float[] orig = mesh.bmin;
Vector3f orig = mesh.bmin;
int borderSize = mesh.borderSize;
int heightSearchRadius = (int)Math.Max(1, Math.Ceiling(mesh.maxEdgeError));

27
src/DotRecast.Recast/RecastRasterization.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
using static DotRecast.Core.RecastMath;
using static DotRecast.Recast.RecastConstants;
@ -49,6 +50,16 @@ namespace DotRecast.Recast
return overlap;
}
private static bool overlapBounds(float[] amin, float[] amax, Vector3f bmin, Vector3f bmax)
{
bool overlap = true;
overlap = (amin[0] > bmax[0] || amax[0] < bmin[0]) ? false : overlap;
overlap = (amin[1] > bmax[1] || amax[1] < bmin[1]) ? false : overlap;
overlap = (amin[2] > bmax[2] || amax[2] < bmin[2]) ? false : overlap;
return overlap;
}
/**
* Adds a span to the heightfield. If the new span overlaps existing spans, it will merge the new span with the
* existing ones. The span addition can be set to favor flags. If the span is merged to another span and the new
@ -247,17 +258,17 @@ namespace DotRecast.Recast
private static void rasterizeTri(float[] verts, int v0, int v1, int v2, int area, Heightfield hf, float[] hfBBMin,
float[] hfBBMax, float cellSize, float inverseCellSize, float inverseCellHeight, int flagMergeThreshold)
{
float[] tmin = new float[3];
float[] tmax = new float[3];
Vector3f tmin = new Vector3f();
Vector3f tmax = new Vector3f();
float by = hfBBMax[1] - hfBBMin[1];
// Calculate the bounding box of the triangle.
RecastVectors.copy(tmin, verts, v0 * 3);
RecastVectors.copy(tmax, verts, v0 * 3);
RecastVectors.min(tmin, verts, v1 * 3);
RecastVectors.min(tmin, verts, v2 * 3);
RecastVectors.max(tmax, verts, v1 * 3);
RecastVectors.max(tmax, verts, v2 * 3);
RecastVectors.copy(ref tmin, verts, v0 * 3);
RecastVectors.copy(ref tmax, verts, v0 * 3);
RecastVectors.min(ref tmin, verts, v1 * 3);
RecastVectors.min(ref tmin, verts, v2 * 3);
RecastVectors.max(ref tmax, verts, v1 * 3);
RecastVectors.max(ref tmax, verts, v2 * 3);
// If the triangle does not touch the bbox of the heightfield, skip the triagle.
if (!overlapBounds(hfBBMin, hfBBMax, tmin, tmax))

139
src/DotRecast.Recast/RecastVectors.cs
View File

@ -19,6 +19,7 @@ freely, subject to the following restrictions:
*/
using System;
using DotRecast.Core;
namespace DotRecast.Recast
{
@ -31,6 +32,22 @@ namespace DotRecast.Recast
a[2] = Math.Min(a[2], b[i + 2]);
}
public static void min(ref Vector3f a, float[] b, int i)
{
a[0] = Math.Min(a[0], b[i + 0]);
a[1] = Math.Min(a[1], b[i + 1]);
a[2] = Math.Min(a[2], b[i + 2]);
}
public static void min(ref Vector3f a, Vector3f b, int i)
{
a[0] = Math.Min(a[0], b[i + 0]);
a[1] = Math.Min(a[1], b[i + 1]);
a[2] = Math.Min(a[2], b[i + 2]);
}
public static void max(float[] a, float[] b, int i)
{
a[0] = Math.Max(a[0], b[i + 0]);
@ -38,16 +55,55 @@ namespace DotRecast.Recast
a[2] = Math.Max(a[2], b[i + 2]);
}
public static void max(ref Vector3f a, float[] b, int i)
{
a[0] = Math.Max(a[0], b[i + 0]);
a[1] = Math.Max(a[1], b[i + 1]);
a[2] = Math.Max(a[2], b[i + 2]);
}
public static void max(ref Vector3f a, Vector3f b, int i)
{
a[0] = Math.Max(a[0], b[i + 0]);
a[1] = Math.Max(a[1], b[i + 1]);
a[2] = Math.Max(a[2], b[i + 2]);
}
public static void copy(float[] @out, float[] @in, int i)
{
copy(@out, 0, @in, i);
}
public static void copy(ref Vector3f @out, float[] @in, int i)
{
copy(ref @out, 0, @in, i);
}
public static void copy(ref Vector3f @out, Vector3f @in, int i)
{
copy(ref @out, 0, @in, i);
}
public static void copy(float[] @out, float[] @in)
{
copy(@out, 0, @in, 0);
}
public static void copy(ref Vector3f @out, float[] @in)
{
copy(ref @out, 0, @in, 0);
}
public static void copy(ref Vector3f @out, Vector3f @in)
{
@out = @in;
}
public static void copy(float[] @out, int n, float[] @in, int m)
{
@out[n] = @in[m];
@ -55,6 +111,29 @@ namespace DotRecast.Recast
@out[n + 2] = @in[m + 2];
}
public static void copy(float[] @out, int n, Vector3f @in, int m)
{
@out[n] = @in[m];
@out[n + 1] = @in[m + 1];
@out[n + 2] = @in[m + 2];
}
public static void copy(ref Vector3f @out, int n, float[] @in, int m)
{
@out[n] = @in[m];
@out[n + 1] = @in[m + 1];
@out[n + 2] = @in[m + 2];
}
public static void copy(ref Vector3f @out, int n, Vector3f @in, int m)
{
@out[n] = @in[m];
@out[n + 1] = @in[m + 1];
@out[n + 2] = @in[m + 2];
}
public static void add(float[] e0, float[] a, float[] verts, int i)
{
e0[0] = a[0] + verts[i];
@ -62,6 +141,20 @@ namespace DotRecast.Recast
e0[2] = a[2] + verts[i + 2];
}
public static void add(ref Vector3f e0, float[] a, float[] verts, int i)
{
e0[0] = a[0] + verts[i];
e0[1] = a[1] + verts[i + 1];
e0[2] = a[2] + verts[i + 2];
}
public static void add(ref Vector3f e0, Vector3f a, float[] verts, int i)
{
e0[0] = a[0] + verts[i];
e0[1] = a[1] + verts[i + 1];
e0[2] = a[2] + verts[i + 2];
}
public static void sub(float[] e0, float[] verts, int i, int j)
{
e0[0] = verts[i] - verts[j];
@ -69,6 +162,14 @@ namespace DotRecast.Recast
e0[2] = verts[i + 2] - verts[j + 2];
}
public static void sub(ref Vector3f e0, float[] verts, int i, int j)
{
e0[0] = verts[i] - verts[j];
e0[1] = verts[i + 1] - verts[j + 1];
e0[2] = verts[i + 2] - verts[j + 2];
}
public static void sub(float[] e0, float[] i, float[] verts, int j)
{
e0[0] = i[0] - verts[j];
@ -76,6 +177,14 @@ namespace DotRecast.Recast
e0[2] = i[2] - verts[j + 2];
}
public static void sub(ref Vector3f e0, float[] i, float[] verts, int j)
{
e0[0] = i[0] - verts[j];
e0[1] = i[1] - verts[j + 1];
e0[2] = i[2] - verts[j + 2];
}
public static void cross(float[] dest, float[] v1, float[] v2)
{
dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
@ -83,6 +192,28 @@ namespace DotRecast.Recast
dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
public static void cross(float[] dest, Vector3f v1, Vector3f v2)
{
dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
dest[1] = v1[2] * v2[0] - v1[0] * v2[2];
dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
public static void cross(ref Vector3f dest, float[] v1, float[] v2)
{
dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
dest[1] = v1[2] * v2[0] - v1[0] * v2[2];
dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
public static void cross(ref Vector3f dest, Vector3f v1, Vector3f v2)
{
dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
dest[1] = v1[2] * v2[0] - v1[0] * v2[2];
dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
}
public static void normalize(float[] v)
{
float d = (float)(1.0f / Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
@ -91,6 +222,14 @@ namespace DotRecast.Recast
v[2] *= d;
}
public static void normalize(ref Vector3f v)
{
float d = (float)(1.0f / Math.Sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
v[0] *= d;
v[1] *= d;
v[2] *= d;
}
public static float dot(float[] v1, float[] v2)
{
return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];

2
src/DotRecast.Recast/RecastVoxelization.cs
View File

@ -29,7 +29,7 @@ namespace DotRecast.Recast
RecastConfig cfg = builderCfg.cfg;
// Allocate voxel heightfield where we rasterize our input data to.
Heightfield solid = new Heightfield(builderCfg.width, builderCfg.height, builderCfg.bmin, builderCfg.bmax, cfg.cs, cfg.ch, cfg.borderSize);
Heightfield solid = new Heightfield(builderCfg.width, builderCfg.height, builderCfg.bmin.ToArray(), builderCfg.bmax.ToArray(), cfg.cs, cfg.ch, cfg.borderSize);
// Allocate array that can hold triangle area types.
// If you have multiple meshes you need to process, allocate

2
test/DotRecast.Detour.Crowd.Test/AbstractCrowdTest.cs
View File

@ -117,7 +117,7 @@ public class AbstractCrowdTest
{
for (int j = 0; j < size; j++)
{
float[] pos = new float[3];
Vector3f pos = new Vector3f();
pos[0] = startPos[0] + i * distance;
pos[1] = startPos[1];
pos[2] = startPos[2] + j * distance;