Skip to content

Commit

Permalink
rename RcVecUtils to RcVec
Browse files Browse the repository at this point in the history
  • Loading branch information
@ikpil
ikpil committed Jun 24, 2024
1 parent 72b7cfd commit e5d5867
Show file tree
Hide file tree
Showing 30 changed files with 158 additions and 153 deletions.
30 changes: 28 additions & 2 deletions src/DotRecast.Core/Numerics/RcVecUtils.cs → src/DotRecast.Core/Numerics/RcVec.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,10 @@

namespace DotRecast.Core.Numerics
{
public static class RcVecUtils
public static class RcVec
{
public const float EPSILON = 1e-6f;
private static readonly float EQUAL_THRESHOLD = RcMath.Sqr(1.0f / 16384.0f);

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static RcVec3f Create(Span<float> values, int n)
Expand Down Expand Up @@ -36,36 +37,56 @@ public static float Get(this RcVec3f v, int i)
}
}

/// Performs a 'sloppy' colocation check of the specified points.
/// @param[in] p0 A point. [(x, y, z)]
/// @param[in] p1 A point. [(x, y, z)]
/// @return True if the points are considered to be at the same location.
///
/// Basically, this function will return true if the specified points are
/// close enough to eachother to be considered colocated.
public static bool Equal(RcVec3f p0, RcVec3f p1)
{
float d = RcVec3f.DistanceSquared(p0, p1);
return d < EQUAL_THRESHOLD;
}


[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dot2(RcVec3f a, RcVec3f b)
{
return a.X * b.X + a.Z * b.Z;
}


[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float DistSq2(float[] verts, int p, int q)
{
float dx = verts[q + 0] - verts[p + 0];
float dy = verts[q + 2] - verts[p + 2];
return dx * dx + dy * dy;
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2(float[] verts, int p, int q)
{
return MathF.Sqrt(DistSq2(verts, p, q));
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float DistSq2(RcVec3f p, RcVec3f q)
{
float dx = q.X - p.X;
float dy = q.Z - p.Z;
return dx * dx + dy * dy;
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2(RcVec3f p, RcVec3f q)
{
return MathF.Sqrt(DistSq2(p, q));
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Cross2(float[] verts, int p1, int p2, int p3)
{
float u1 = verts[p2 + 0] - verts[p1 + 0];
Expand All @@ -75,6 +96,7 @@ public static float Cross2(float[] verts, int p1, int p2, int p3)
return u1 * v2 - v1 * u2;
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Cross2(RcVec3f p1, RcVec3f p2, RcVec3f p3)
{
float u1 = p2.X - p1.X;
Expand All @@ -83,7 +105,7 @@ public static float Cross2(RcVec3f p1, RcVec3f p2, RcVec3f p3)
float v2 = p3.Z - p1.Z;
return u1 * v2 - v1 * u2;
}

/// Derives the dot product of two vectors on the xz-plane. (@p u . @p v)
/// @param[in] u A vector [(x, y, z)]
/// @param[in] v A vector [(x, y, z)]
Expand Down Expand Up @@ -162,6 +184,10 @@ public static float Dist2D(RcVec3f v1, RcVec3f v2)
return (float)MathF.Sqrt(dx * dx + dz * dz);
}

/// Derives the square of the distance between the specified points on the xz-plane.
/// @param[in] v1 A point. [(x, y, z)]
/// @param[in] v2 A point. [(x, y, z)]
/// @return The square of the distance between the point on the xz-plane.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2DSqr(RcVec3f v1, RcVec3f v2)
{
Expand Down
10 changes: 5 additions & 5 deletions src/DotRecast.Detour.Crowd/DtCrowd.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -875,7 +875,7 @@ private void BuildNeighbours(IList<DtCrowdAgent> agents)
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
float updateThr = ag.option.collisionQueryRange * 0.25f;
if (RcVecUtils.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > RcMath.Sqr(updateThr)
if (RcVec.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > RcMath.Sqr(updateThr)
|| !ag.boundary.IsValid(_navQuery, _filters[ag.option.queryFilterType]))
{
ag.boundary.Update(ag.corridor.GetFirstPoly(), ag.npos, ag.option.collisionQueryRange, _navQuery,
Expand Down Expand Up @@ -1007,7 +1007,7 @@ private void TriggerOffMeshConnections(IList<DtCrowdAgent> agents)
anim.polyRef = refs[1];
anim.active = true;
anim.t = 0.0f;
anim.tmax = (RcVecUtils.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;
anim.tmax = (RcVec.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;

ag.state = DtCrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH;
ag.ncorners = 0;
Expand Down Expand Up @@ -1097,14 +1097,14 @@ private void CalculateSteering(IList<DtCrowdAgent> agents)
float dist = MathF.Sqrt(distSqr);
float weight = separationWeight * (1.0f - RcMath.Sqr(dist * invSeparationDist));

disp = RcVecUtils.Mad(disp, diff, weight / dist);
disp = RcVec.Mad(disp, diff, weight / dist);
w += 1.0f;
}

if (w > 0.0001f)
{
// Adjust desired velocity.
dvel = RcVecUtils.Mad(dvel, disp, 1.0f / w);
dvel = RcVec.Mad(dvel, disp, 1.0f / w);
// Clamp desired velocity to desired speed.
float speedSqr = dvel.LengthSquared();
float desiredSqr = RcMath.Sqr(ag.desiredSpeed);
Expand Down Expand Up @@ -1260,7 +1260,7 @@ private void HandleCollisions(IList<DtCrowdAgent> agents)
pen = (1.0f / dist) * (pen * 0.5f) * _config.collisionResolveFactor;
}

ag.disp = RcVecUtils.Mad(ag.disp, diff, pen);
ag.disp = RcVec.Mad(ag.disp, diff, pen);

w += 1.0f;
}
Expand Down
6 changes: 3 additions & 3 deletions src/DotRecast.Detour.Crowd/DtCrowdAgent.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ public void Integrate(float dt)

// Integrate
if (vel.Length() > 0.0001f)
npos = RcVecUtils.Mad(npos, vel, dt);
npos = RcVec.Mad(npos, vel, dt);
else
vel = RcVec3f.Zero;
}
Expand All @@ -112,7 +112,7 @@ public bool OverOffmeshConnection(float radius)
: false;
if (offMeshConnection)
{
float distSq = RcVecUtils.Dist2DSqr(npos, corners[ncorners - 1].pos);
float distSq = RcVec.Dist2DSqr(npos, corners[ncorners - 1].pos);
if (distSq < radius * radius)
return true;
}
Expand All @@ -127,7 +127,7 @@ public float GetDistanceToGoal(float range)

bool endOfPath = ((corners[ncorners - 1].flags & DtStraightPathFlags.DT_STRAIGHTPATH_END) != 0) ? true : false;
if (endOfPath)
return Math.Min(RcVecUtils.Dist2D(npos, corners[ncorners - 1].pos), range);
return Math.Min(RcVec.Dist2D(npos, corners[ncorners - 1].pos), range);

return range;
}
Expand Down
10 changes: 5 additions & 5 deletions src/DotRecast.Detour.Crowd/DtObstacleAvoidanceQuery.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,16 +186,16 @@ private bool IsectRaySeg(RcVec3f ap, RcVec3f u, RcVec3f bp, RcVec3f bq, ref floa
{
RcVec3f v = RcVec3f.Subtract(bq, bp);
RcVec3f w = RcVec3f.Subtract(ap, bp);
float d = RcVecUtils.Perp2D(u, v);
float d = RcVec.Perp2D(u, v);
if (MathF.Abs(d) < 1e-6f)
return false;

d = 1.0f / d;
t = RcVecUtils.Perp2D(v, w) * d;
t = RcVec.Perp2D(v, w) * d;
if (t < 0 || t > 1)
return false;

float s = RcVecUtils.Perp2D(u, w) * d;
float s = RcVec.Perp2D(u, w) * d;
if (s < 0 || s > 1)
return false;

Expand All @@ -216,8 +216,8 @@ private float ProcessSample(RcVec3f vcand, float cs, RcVec3f pos, float rad, RcV
float minPenalty, DtObstacleAvoidanceDebugData debug)
{
// penalty for straying away from the desired and current velocities
float vpen = m_params.weightDesVel * (RcVecUtils.Dist2D(vcand, dvel) * m_invVmax);
float vcpen = m_params.weightCurVel * (RcVecUtils.Dist2D(vcand, vel) * m_invVmax);
float vpen = m_params.weightDesVel * (RcVec.Dist2D(vcand, dvel) * m_invVmax);
float vcpen = m_params.weightCurVel * (RcVec.Dist2D(vcand, vel) * m_invVmax);

// find the threshold hit time to bail out based on the early out penalty
// (see how the penalty is calculated below to understand)
Expand Down
6 changes: 3 additions & 3 deletions src/DotRecast.Detour.Crowd/DtPathCorridor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ public int FindCorners(Span<DtStraightPath> corners, int maxCorners, DtNavMeshQu
while (0 < ncorners)
{
if ((corners[0].flags & DtStraightPathFlags.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0 ||
RcVecUtils.Dist2DSqr(corners[0].pos, m_pos) > RcMath.Sqr(MIN_TARGET_DIST))
RcVec.Dist2DSqr(corners[0].pos, m_pos) > RcMath.Sqr(MIN_TARGET_DIST))
{
break;
}
Expand Down Expand Up @@ -197,7 +197,7 @@ of the call to match the needs to the agent.
public void OptimizePathVisibility(RcVec3f next, float pathOptimizationRange, DtNavMeshQuery navquery, IDtQueryFilter filter)
{
// Clamp the ray to max distance.
float dist = RcVecUtils.Dist2D(m_pos, next);
float dist = RcVec.Dist2D(m_pos, next);

// If too close to the goal, do not try to optimize.
if (dist < 0.01f)
Expand All @@ -210,7 +210,7 @@ public void OptimizePathVisibility(RcVec3f next, float pathOptimizationRange, Dt

// Adjust ray length.
var delta = RcVec3f.Subtract(next, m_pos);
RcVec3f goal = RcVecUtils.Mad(m_pos, delta, pathOptimizationRange / dist);
RcVec3f goal = RcVec.Mad(m_pos, delta, pathOptimizationRange / dist);

var res = new List<long>();
var status = navquery.Raycast(m_path[0], m_pos, goal, filter, out var t, out var norm, ref res);
Expand Down
8 changes: 4 additions & 4 deletions src/DotRecast.Detour.Extras/BVTreeBuilder.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,12 +40,12 @@ private static int CreateBVTree(DtMeshData data, DtBVNode[] nodes, float quantFa
BVItem it = new BVItem();
items[i] = it;
it.i = i;
RcVec3f bmin = RcVecUtils.Create(data.verts, data.polys[i].verts[0] * 3);
RcVec3f bmax = RcVecUtils.Create(data.verts, data.polys[i].verts[0] * 3);
RcVec3f bmin = RcVec.Create(data.verts, data.polys[i].verts[0] * 3);
RcVec3f bmax = RcVec.Create(data.verts, data.polys[i].verts[0] * 3);
for (int j = 1; j < data.polys[i].vertCount; j++)
{
bmin = RcVec3f.Min(bmin, RcVecUtils.Create(data.verts, data.polys[i].verts[j] * 3));
bmax = RcVec3f.Max(bmax, RcVecUtils.Create(data.verts, data.polys[i].verts[j] * 3));
bmin = RcVec3f.Min(bmin, RcVec.Create(data.verts, data.polys[i].verts[j] * 3));
bmax = RcVec3f.Max(bmax, RcVec.Create(data.verts, data.polys[i].verts[j] * 3));
}

it.bmin[0] = Math.Clamp((int)((bmin.X - data.header.bmin.X) * quantFactor), 0, 0x7fffffff);
Expand Down
2 changes: 1 addition & 1 deletion src/DotRecast.Detour.Extras/Jumplink/AbstractGroundSampler.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ public abstract class AbstractGroundSampler : IGroundSampler
protected void SampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es, ComputeNavMeshHeight heightFunc)
{
float cs = acfg.cellSize;
float dist = MathF.Sqrt(RcVecUtils.Dist2DSqr(es.start.p, es.start.q));
float dist = MathF.Sqrt(RcVec.Dist2DSqr(es.start.p, es.start.q));
int ngsamples = Math.Max(2, (int)MathF.Ceiling(dist / cs));

SampleGroundSegment(heightFunc, es.start, ngsamples);
Expand Down
2 changes: 1 addition & 1 deletion src/DotRecast.Detour.Extras/Jumplink/JumpLinkBuilder.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ private List<JumpLink> BuildJumpLinks(JumpLinkBuilderConfig acfg, EdgeSampler es
GroundSegment end = es.end[js.groundSegment];
RcVec3f ep = end.gsamples[js.startSample].p;
RcVec3f eq = end.gsamples[js.startSample + js.samples - 1].p;
float d = Math.Min(RcVecUtils.Dist2DSqr(sp, sq), RcVecUtils.Dist2DSqr(ep, eq));
float d = Math.Min(RcVec.Dist2DSqr(sp, sq), RcVec.Dist2DSqr(ep, eq));
if (d >= 4 * acfg.agentRadius * acfg.agentRadius)
{
JumpLink link = new JumpLink();
Expand Down
2 changes: 1 addition & 1 deletion src/DotRecast.Detour.Extras/Jumplink/TrajectorySampler.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ public void Sample(JumpLinkBuilderConfig acfg, RcHeightfield heightfield, EdgeSa
private bool SampleTrajectory(JumpLinkBuilderConfig acfg, RcHeightfield solid, RcVec3f pa, RcVec3f pb, ITrajectory tra)
{
float cs = Math.Min(acfg.cellSize, acfg.cellHeight);
float d = RcVecUtils.Dist2D(pa, pb) + MathF.Abs(pa.Y - pb.Y);
float d = RcVec.Dist2D(pa, pb) + MathF.Abs(pa.Y - pb.Y);
int nsamples = Math.Max(2, (int)MathF.Ceiling(d / cs));
for (int i = 0; i < nsamples; ++i)
{
Expand Down
8 changes: 4 additions & 4 deletions src/DotRecast.Detour/DtConvexConvexIntersections.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,10 +54,10 @@ public static float[] Intersect(float[] p, float[] q)

do
{
a = RcVecUtils.Create(p, 3 * (ai % n));
b = RcVecUtils.Create(q, 3 * (bi % m));
a1 = RcVecUtils.Create(p, 3 * ((ai + n - 1) % n)); // prev a
b1 = RcVecUtils.Create(q, 3 * ((bi + m - 1) % m)); // prev b
a = RcVec.Create(p, 3 * (ai % n));
b = RcVec.Create(q, 3 * (bi % m));
a1 = RcVec.Create(p, 3 * ((ai + n - 1) % n)); // prev a
b1 = RcVec.Create(q, 3 * ((bi + m - 1) % m)); // prev b

RcVec3f A = RcVec3f.Subtract(a, a1);
RcVec3f B = RcVec3f.Subtract(b, b1);
Expand Down
12 changes: 6 additions & 6 deletions src/DotRecast.Detour/DtNavMesh.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -319,13 +319,13 @@ List<long> QueryPolygonsInTile(DtMeshTile tile, RcVec3f qmin, RcVec3f qmax)

// Calc polygon bounds.
int v = p.verts[0] * 3;
bmin = RcVecUtils.Create(tile.data.verts, v);
bmax = RcVecUtils.Create(tile.data.verts, v);
bmin = RcVec.Create(tile.data.verts, v);
bmax = RcVec.Create(tile.data.verts, v);
for (int j = 1; j < p.vertCount; ++j)
{
v = p.verts[j] * 3;
bmin = RcVec3f.Min(bmin, RcVecUtils.Create(tile.data.verts, v));
bmax = RcVec3f.Max(bmax, RcVecUtils.Create(tile.data.verts, v));
bmin = RcVec3f.Min(bmin, RcVec.Create(tile.data.verts, v));
bmax = RcVec3f.Max(bmax, RcVec.Create(tile.data.verts, v));
}

if (DtUtils.OverlapBounds(qmin, qmax, bmin, bmax))
Expand Down Expand Up @@ -1489,8 +1489,8 @@ public DtStatus GetOffMeshConnectionPolyEndPoints(long prevRef, long polyRef, re
}
}

startPos = RcVecUtils.Create(tile.data.verts, poly.verts[idx0] * 3);
endPos = RcVecUtils.Create(tile.data.verts, poly.verts[idx1] * 3);
startPos = RcVec.Create(tile.data.verts, poly.verts[idx0] * 3);
endPos = RcVec.Create(tile.data.verts, poly.verts[idx1] * 3);

return DtStatus.DT_SUCCESS;
}
Expand Down
14 changes: 7 additions & 7 deletions src/DotRecast.Detour/DtNavMeshBuilder.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,12 +164,12 @@ private static int CreateBVTree(DtNavMeshCreateParams option, DtBVNode[] nodes)
int vb = option.detailMeshes[i * 4 + 0];
int ndv = option.detailMeshes[i * 4 + 1];
int dv = vb * 3;
var bmin = RcVecUtils.Create(option.detailVerts, dv);
var bmax = RcVecUtils.Create(option.detailVerts, dv);
var bmin = RcVec.Create(option.detailVerts, dv);
var bmax = RcVec.Create(option.detailVerts, dv);
for (int j = 1; j < ndv; j++)
{
bmin = RcVec3f.Min(bmin, RcVecUtils.Create(option.detailVerts, dv + j * 3));
bmax = RcVec3f.Max(bmax, RcVecUtils.Create(option.detailVerts, dv + j * 3));
bmin = RcVec3f.Min(bmin, RcVec.Create(option.detailVerts, dv + j * 3));
bmax = RcVec3f.Max(bmax, RcVec.Create(option.detailVerts, dv + j * 3));
}

// BV-tree uses cs for all dimensions
Expand Down Expand Up @@ -322,8 +322,8 @@ public static DtMeshData CreateNavMeshData(DtNavMeshCreateParams option)

for (int i = 0; i < option.offMeshConCount; ++i)
{
var p0 = RcVecUtils.Create(option.offMeshConVerts, (i * 2 + 0) * 3);
var p1 = RcVecUtils.Create(option.offMeshConVerts, (i * 2 + 1) * 3);
var p0 = RcVec.Create(option.offMeshConVerts, (i * 2 + 0) * 3);
var p1 = RcVec.Create(option.offMeshConVerts, (i * 2 + 1) * 3);

offMeshConClass[i * 2 + 0] = ClassifyOffMeshPoint(p0, bmin, bmax);
offMeshConClass[i * 2 + 1] = ClassifyOffMeshPoint(p1, bmin, bmax);
Expand Down Expand Up @@ -624,7 +624,7 @@ public static DtMeshData CreateNavMeshData(DtNavMeshCreateParams option)
int endPts = i * 2 * 3;
for (int j = 0; j < 2; ++j)
{
con.pos[j] = RcVecUtils.Create(option.offMeshConVerts, endPts + (j * 3));
con.pos[j] = RcVec.Create(option.offMeshConVerts, endPts + (j * 3));
}

con.rad = option.offMeshConRad[i];
Expand Down
Loading

0 comments on commit e5d5867

Please sign in to comment.