a.samsonkin
/
DotRecastNetSim
forked from bit/DotRecastNetSim
1
0
Fork 0
Code Pull Requests Activity

refactor: RcVec3f.Mad, IsFinite, Lerp

This commit is contained in:
ikpil 2023-10-26 00:05:00 +09:00
parent 18a195404f
commit 9a62fbf9f4
10 changed files with 91 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
src/DotRecast.Core/Numerics/RcVec3f.cs
View File

@ -263,62 +263,6 @@ namespace DotRecast.Core.Numerics
(vector1.Z * vector2.Z);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float PerpXZ(RcVec3f a, RcVec3f b)
{
return (a.X * b.Z) - (a.Z * b.X);
}
/// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s))
/// @param[out] dest The result vector. [(x, y, z)]
/// @param[in] v1 The base vector. [(x, y, z)]
/// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)]
/// @param[in] s The amount to scale @p v2 by before adding to @p v1.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static RcVec3f Mad(RcVec3f v1, RcVec3f v2, float s)
{
return new RcVec3f()
{
X = v1.X + (v2.X * s),
Y = v1.Y + (v2.Y * s),
Z = v1.Z + (v2.Z * s),
};
}
/// Performs a linear interpolation between two vectors. (@p v1 toward @p
/// v2)
/// @param[out] dest The result vector. [(x, y, x)]
/// @param[in] v1 The starting vector.
/// @param[in] v2 The destination vector.
/// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static RcVec3f Lerp(float[] verts, int v1, int v2, float t)
{
return new RcVec3f(
verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t,
verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t,
verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t
);
}
/// Checks that the specified vector's components are all finite.
/// @param[in] v A point. [(x, y, z)]
/// @return True if all of the point's components are finite, i.e. not NaN
/// or any of the infinities.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool IsFinite(RcVec3f v)
{
return float.IsFinite(v.X) && float.IsFinite(v.Y) && float.IsFinite(v.Z);
}
/// Checks that the specified vector's 2D components are finite.
/// @param[in] v A point. [(x, y, z)]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool IsFinite2D(RcVec3f v)
{
return float.IsFinite(v.X) && float.IsFinite(v.Z);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public readonly void CopyTo(float[] array)
{

56
src/DotRecast.Core/Numerics/RcVecUtils.cs
View File

@ -219,5 +219,61 @@ namespace DotRecast.Core.Numerics
{
return u.Z * v.X - u.X * v.Z;
}
/// Checks that the specified vector's components are all finite.
/// @param[in] v A point. [(x, y, z)]
/// @return True if all of the point's components are finite, i.e. not NaN
/// or any of the infinities.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool IsFinite(RcVec3f v)
{
return float.IsFinite(v.X) && float.IsFinite(v.Y) && float.IsFinite(v.Z);
}
/// Checks that the specified vector's 2D components are finite.
/// @param[in] v A point. [(x, y, z)]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool IsFinite2D(RcVec3f v)
{
return float.IsFinite(v.X) && float.IsFinite(v.Z);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float PerpXZ(RcVec3f a, RcVec3f b)
{
return (a.X * b.Z) - (a.Z * b.X);
}
/// Performs a linear interpolation between two vectors. (@p v1 toward @p
/// v2)
/// @param[out] dest The result vector. [(x, y, x)]
/// @param[in] v1 The starting vector.
/// @param[in] v2 The destination vector.
/// @param[in] t The interpolation factor. [Limits: 0 <= value <= 1.0]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static RcVec3f Lerp(float[] verts, int v1, int v2, float t)
{
return new RcVec3f(
verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t,
verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t,
verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t
);
}
/// Performs a scaled vector addition. (@p v1 + (@p v2 * @p s))
/// @param[out] dest The result vector. [(x, y, z)]
/// @param[in] v1 The base vector. [(x, y, z)]
/// @param[in] v2 The vector to scale and add to @p v1. [(x, y, z)]
/// @param[in] s The amount to scale @p v2 by before adding to @p v1.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static RcVec3f Mad(RcVec3f v1, RcVec3f v2, float s)
{
return new RcVec3f()
{
X = v1.X + (v2.X * s),
Y = v1.Y + (v2.Y * s),
Z = v1.Z + (v2.Z * s),
};
}
}
}

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

@ -1094,14 +1094,14 @@ namespace DotRecast.Detour.Crowd
float dist = MathF.Sqrt(distSqr);
float weight = separationWeight * (1.0f - RcMath.Sqr(dist * invSeparationDist));
disp = RcVec3f.Mad(disp, diff, weight / dist);
disp = RcVecUtils.Mad(disp, diff, weight / dist);
w += 1.0f;
}
if (w > 0.0001f)
{
// Adjust desired velocity.
dvel = RcVec3f.Mad(dvel, disp, 1.0f / w);
dvel = RcVecUtils.Mad(dvel, disp, 1.0f / w);
// Clamp desired velocity to desired speed.
float speedSqr = dvel.LengthSquared();
float desiredSqr = RcMath.Sqr(ag.desiredSpeed);
@ -1254,7 +1254,7 @@ namespace DotRecast.Detour.Crowd
pen = (1.0f / dist) * (pen * 0.5f) * _config.collisionResolveFactor;
}
ag.disp = RcVec3f.Mad(ag.disp, diff, pen);
ag.disp = RcVecUtils.Mad(ag.disp, diff, pen);
w += 1.0f;
}

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

@ -121,7 +121,7 @@ namespace DotRecast.Detour.Crowd
// Integrate
if (vel.Length() > 0.0001f)
npos = RcVec3f.Mad(npos, vel, dt);
npos = RcVecUtils.Mad(npos, vel, dt);
else
vel = RcVec3f.Zero;
}

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

@ -193,7 +193,7 @@ namespace DotRecast.Detour.Crowd
// Adjust ray length.
var delta = RcVec3f.Subtract(next, m_pos);
RcVec3f goal = RcVec3f.Mad(m_pos, delta, pathOptimizationRange / dist);
RcVec3f goal = RcVecUtils.Mad(m_pos, delta, pathOptimizationRange / dist);
var status = navquery.Raycast(m_path[0], m_pos, goal, filter, 0, 0, out var rayHit);
if (status.Succeeded())

48
src/DotRecast.Detour/DtNavMeshQuery.cs
View File

@ -205,7 +205,7 @@ namespace DotRecast.Detour
randomPt = centerPos;
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || maxRadius < 0
if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || maxRadius < 0
|| !float.IsFinite(maxRadius) || null == filter || null == frand)
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
@ -406,7 +406,7 @@ namespace DotRecast.Detour
closest = pos;
posOverPoly = false;
if (!m_nav.IsValidPolyRef(refs) || !RcVec3f.IsFinite(pos))
if (!m_nav.IsValidPolyRef(refs) || !RcVecUtils.IsFinite(pos))
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
@ -441,7 +441,7 @@ namespace DotRecast.Detour
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
if (tile == null || !RcVec3f.IsFinite(pos))
if (tile == null || !RcVecUtils.IsFinite(pos))
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
@ -476,7 +476,7 @@ namespace DotRecast.Detour
int va = imin * 3;
int vb = ((imin + 1) % nv) * 3;
closest = RcVec3f.Lerp(verts, va, vb, edget[imin]);
closest = RcVecUtils.Lerp(verts, va, vb, edget[imin]);
}
return DtStatus.DT_SUCCSESS;
@ -502,7 +502,7 @@ namespace DotRecast.Detour
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
if (!RcVec3f.IsFinite2D(pos))
if (!RcVecUtils.IsFinite2D(pos))
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
@ -673,7 +673,7 @@ namespace DotRecast.Detour
*/
public DtStatus QueryPolygons(RcVec3f center, RcVec3f halfExtents, IDtQueryFilter filter, IDtPolyQuery query)
{
if (!RcVec3f.IsFinite(center) || !RcVec3f.IsFinite(halfExtents) || null == filter)
if (!RcVecUtils.IsFinite(center) || !RcVecUtils.IsFinite(halfExtents) || null == filter)
{
return DtStatus.DT_INVALID_PARAM;
}
@ -694,7 +694,7 @@ namespace DotRecast.Detour
*/
public IList<DtMeshTile> QueryTiles(RcVec3f center, RcVec3f halfExtents)
{
if (!RcVec3f.IsFinite(center) || !RcVec3f.IsFinite(halfExtents))
if (!RcVecUtils.IsFinite(center) || !RcVecUtils.IsFinite(halfExtents))
{
return RcImmutableArray<DtMeshTile>.Empty;
}
@ -744,7 +744,7 @@ namespace DotRecast.Detour
path.Clear();
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) || null == filter)
if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos) || null == filter)
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
@ -1020,7 +1020,7 @@ namespace DotRecast.Detour
m_query.raycastLimitSqr = RcMath.Sqr(raycastLimit);
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) || null == filter)
if (!m_nav.IsValidPolyRef(startRef) || !m_nav.IsValidPolyRef(endRef) || !RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos) || null == filter)
{
return DtStatus.DT_INVALID_PARAM;
}
@ -1514,7 +1514,7 @@ namespace DotRecast.Detour
ref List<DtStraightPath> straightPath,
int maxStraightPath, int options)
{
if (!RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos) || null == straightPath
if (!RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos) || null == straightPath
|| null == path || 0 == path.Count || path[0] == 0 || maxStraightPath <= 0)
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
@ -1779,8 +1779,8 @@ namespace DotRecast.Detour
visited.Clear();
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(startPos)
|| !RcVec3f.IsFinite(endPos) || null == filter)
if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(startPos)
|| !RcVecUtils.IsFinite(endPos) || null == filter)
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
}
@ -1883,7 +1883,7 @@ namespace DotRecast.Detour
if (distSqr < bestDist)
{
// Update nearest distance.
bestPos = RcVec3f.Lerp(verts, vj, vi, tseg);
bestPos = RcVecUtils.Lerp(verts, vj, vi, tseg);
bestDist = distSqr;
bestNode = curNode;
}
@ -2062,8 +2062,8 @@ namespace DotRecast.Detour
float s = 1.0f / 255.0f;
float tmin = link.bmin * s;
float tmax = link.bmax * s;
left = RcVec3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin);
right = RcVec3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax);
left = RcVecUtils.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin);
right = RcVecUtils.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax);
}
}
@ -2165,7 +2165,7 @@ namespace DotRecast.Detour
hit = null;
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(startPos) || !RcVec3f.IsFinite(endPos)
if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(startPos) || !RcVecUtils.IsFinite(endPos)
|| null == filter || (prevRef != 0 && !m_nav.IsValidPolyRef(prevRef)))
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
@ -2340,7 +2340,7 @@ namespace DotRecast.Detour
// compute the intersection point at the furthest end of the polygon
// and correct the height (since the raycast moves in 2d)
lastPos = curPos;
curPos = RcVec3f.Mad(startPos, dir, hit.t);
curPos = RcVecUtils.Mad(startPos, dir, hit.t);
var e1 = verts[segMax];
var e2 = verts[(segMax + 1) % nv];
var eDir = RcVec3f.Subtract(e2, e1);
@ -2434,7 +2434,7 @@ namespace DotRecast.Detour
}
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || radius < 0
if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || radius < 0
|| !float.IsFinite(radius) || null == filter)
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;
@ -2777,7 +2777,7 @@ namespace DotRecast.Detour
ref List<long> resultRef, ref List<long> resultParent)
{
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || radius < 0
if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || radius < 0
|| !float.IsFinite(radius) || null == filter
|| null == resultRef || null == resultParent)
{
@ -3055,8 +3055,8 @@ namespace DotRecast.Detour
float tmin = ints[k].tmin / 255.0f;
float tmax = ints[k].tmax / 255.0f;
var seg = new RcSegmentVert();
seg.vmin = RcVec3f.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = RcVec3f.Lerp(tile.data.verts, vj, vi, tmax);
seg.vmin = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg);
segmentRefs.Add(ints[k].refs);
}
@ -3069,8 +3069,8 @@ namespace DotRecast.Detour
float tmin = imin / 255.0f;
float tmax = imax / 255.0f;
var seg = new RcSegmentVert();
seg.vmin = RcVec3f.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = RcVec3f.Lerp(tile.data.verts, vj, vi, tmax);
seg.vmin = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = RcVecUtils.Lerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg);
segmentRefs.Add(0L);
}
@ -3109,7 +3109,7 @@ namespace DotRecast.Detour
hitNormal = RcVec3f.Zero;
// Validate input
if (!m_nav.IsValidPolyRef(startRef) || !RcVec3f.IsFinite(centerPos) || maxRadius < 0
if (!m_nav.IsValidPolyRef(startRef) || !RcVecUtils.IsFinite(centerPos) || maxRadius < 0
|| !float.IsFinite(maxRadius) || null == filter)
{
return DtStatus.DT_FAILURE | DtStatus.DT_INVALID_PARAM;

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

@ -425,14 +425,14 @@ namespace DotRecast.Detour
RcVec3f u = RcVec3f.Subtract(aq, ap);
RcVec3f v = RcVec3f.Subtract(bq, bp);
RcVec3f w = RcVec3f.Subtract(ap, bp);
float d = RcVec3f.PerpXZ(u, v);
float d = RcVecUtils.PerpXZ(u, v);
if (MathF.Abs(d) < 1e-6f)
{
return false;
}
s = RcVec3f.PerpXZ(v, w) / d;
t = RcVec3f.PerpXZ(u, w) / d;
s = RcVecUtils.PerpXZ(v, w) / d;
t = RcVecUtils.PerpXZ(u, w) / d;
return true;
}

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

@ -508,10 +508,10 @@ public class TestNavmeshSampleTool : ISampleTool
}
RcVec3f delta = RcVec3f.Subtract(s3, s.vmin);
RcVec3f p0 = RcVec3f.Mad(s.vmin, delta, 0.5f);
RcVec3f p0 = RcVecUtils.Mad(s.vmin, delta, 0.5f);
RcVec3f norm = new RcVec3f(delta.Z, 0, -delta.X);
norm = RcVec3f.Normalize(norm);
RcVec3f p1 = RcVec3f.Mad(p0, norm, agentRadius * 0.5f);
RcVec3f p1 = RcVecUtils.Mad(p0, norm, agentRadius * 0.5f);
// Skip backfacing segments.
if (segmentRefs[j] != 0)
{

2
src/DotRecast.Recast.Toolset/Tools/RcTestNavMeshTool.cs
View File

@ -85,7 +85,7 @@ namespace DotRecast.Recast.Toolset.Tools
len = STEP_SIZE / len;
}
RcVec3f moveTgt = RcVec3f.Mad(iterPos, delta, len);
RcVec3f moveTgt = RcVecUtils.Mad(iterPos, delta, len);
// Move
navQuery.MoveAlongSurface(polys[0], iterPos, moveTgt, filter, out var result, ref visited);

7
test/DotRecast.Core.Test/Vector3Tests.cs
View File

@ -172,13 +172,6 @@ public class Vector3Tests
// IsFinite
}
[Test]
[Repeat(100000)]
public void TestVectorPerp2D()
{
// Perp2D
}
// [Test]
// [Repeat(100000)]
// public void TestVectorLerp()