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

Vector3f inline

This commit is contained in:
ikpil 2023-05-24 00:06:28 +09:00
parent 3f79c8294f
commit 425e1887f6
17 changed files with 113 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
src/DotRecast.Core/RcMath.cs
View File

@ -37,7 +37,7 @@ namespace DotRecast.Core
float totd = 0;
for (int i = 0; i < npath - 1; ++i)
{
totd += (float)Math.Sqrt(VDistSqr(path, i * 3, (i + 1) * 3));
totd += (float)Math.Sqrt(Vector3f.DistSqr(path, i * 3, (i + 1) * 3));
}
return totd;
@ -65,41 +65,6 @@ namespace DotRecast.Core
}
public static float VDistSqr(Vector3f v1, float[] v2, int i)
{
float dx = v2[i] - v1.x;
float dy = v2[i + 1] - v1.y;
float dz = v2[i + 2] - v1.z;
return dx * dx + dy * dy + dz * dz;
}
public static float VDistSqr(Vector3f v1, Vector3f v2)
{
float dx = v2.x - v1.x;
float dy = v2.y - v1.y;
float dz = v2.z - v1.z;
return dx * dx + dy * dy + dz * dz;
}
public static float VDistSqr(float[] v, int i, int j)
{
float dx = v[i] - v[j];
float dy = v[i + 1] - v[j + 1];
float dz = v[i + 2] - v[j + 2];
return 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)]
/// @return The distance between the two points.
public static float VDistSqr(float[] v1, float[] 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)
@ -111,49 +76,7 @@ namespace DotRecast.Core
}
/// Derives 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 distance between the point on the xz-plane.
///
/// The vectors are projected onto the xz-plane, so the y-values are
/// ignored.
public static float VDist2D(float[] v1, float[] v2)
{
float dx = v2[0] - v1[0];
float dz = v2[2] - v1[2];
return (float)Math.Sqrt(dx * dx + dz * dz);
}
public static float VDist2D(Vector3f v1, Vector3f v2)
{
float dx = v2.x - v1.x;
float dz = v2.z - v1.z;
return (float)Math.Sqrt(dx * dx + dz * dz);
}
public static float VDist2DSqr(float[] v1, float[] v2)
{
float dx = v2[0] - v1[0];
float dz = v2[2] - v1[2];
return dx * dx + dz * dz;
}
public static float VDist2DSqr(Vector3f v1, Vector3f v2)
{
float dx = v2.x - v1.x;
float dz = v2.z - v1.z;
return dx * dx + dz * dz;
}
public static float VDist2DSqr(Vector3f p, float[] verts, int i)
{
float dx = verts[i] - p.x;
float dz = verts[i + 2] - p.z;
return dx * dx + dz * dz;
}
/// Normalizes the vector.
/// @param[in,out] v The vector to normalize. [(x, y, z)]
@ -183,7 +106,7 @@ namespace DotRecast.Core
public static bool VEqual(Vector3f p0, Vector3f p1, float thresholdSqr)
{
float d = VDistSqr(p0, p1);
float d = Vector3f.DistSqr(p0, p1);
return d < thresholdSqr;
}

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

@ -357,5 +357,94 @@ namespace DotRecast.Core
verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t
);
}
/// Returns the distance between two points.
/// @param[in] v1 A point. [(x, y, z)]
/// @param[in] v2 A point. [(x, y, z)]
/// @return The distance between the two points.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float DistSqr(Vector3f v1, float[] v2, int i)
{
float dx = v2[i] - v1.x;
float dy = v2[i + 1] - v1.y;
float dz = v2[i + 2] - v1.z;
return dx * dx + dy * dy + dz * dz;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float DistSqr(Vector3f v1, Vector3f v2)
{
float dx = v2.x - v1.x;
float dy = v2.y - v1.y;
float dz = v2.z - v1.z;
return dx * dx + dy * dy + dz * dz;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float DistSqr(float[] v, int i, int j)
{
float dx = v[i] - v[j];
float dy = v[i + 1] - v[j + 1];
float dz = v[i + 2] - v[j + 2];
return dx * dx + dy * dy + dz * dz;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float DistSqr(float[] v1, float[] 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 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 distance between the point on the xz-plane.
///
/// The vectors are projected onto the xz-plane, so the y-values are
/// ignored.
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2D(float[] v1, float[] v2)
{
float dx = v2[0] - v1[0];
float dz = v2[2] - v1[2];
return (float)Math.Sqrt(dx * dx + dz * dz);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2D(Vector3f v1, Vector3f v2)
{
float dx = v2.x - v1.x;
float dz = v2.z - v1.z;
return (float)Math.Sqrt(dx * dx + dz * dz);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2DSqr(float[] v1, float[] v2)
{
float dx = v2[0] - v1[0];
float dz = v2[2] - v1[2];
return dx * dx + dz * dz;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2DSqr(Vector3f v1, Vector3f v2)
{
float dx = v2.x - v1.x;
float dz = v2.z - v1.z;
return dx * dx + dz * dz;
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Dist2DSqr(Vector3f p, float[] verts, int i)
{
float dx = verts[i] - p.x;
float dz = verts[i + 2] - p.z;
return dx * dx + dz * dz;
}
}
}

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

@ -890,7 +890,7 @@ namespace DotRecast.Detour.Crowd
// Update the collision boundary after certain distance has been passed or
// if it has become invalid.
float updateThr = ag.option.collisionQueryRange * 0.25f;
if (VDist2DSqr(ag.npos, ag.boundary.GetCenter()) > Sqr(updateThr)
if (Vector3f.Dist2DSqr(ag.npos, ag.boundary.GetCenter()) > Sqr(updateThr)
|| !ag.boundary.IsValid(_navQuery, _filters[ag.option.queryFilterType]))
{
ag.boundary.Update(ag.corridor.GetFirstPoly(), ag.npos, ag.option.collisionQueryRange, _navQuery,
@ -1018,7 +1018,7 @@ namespace DotRecast.Detour.Crowd
anim.polyRef = refs[1];
anim.active = true;
anim.t = 0.0f;
anim.tmax = (VDist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;
anim.tmax = (Vector3f.Dist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f;
ag.state = CrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH;
ag.corners.Clear();

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

@ -139,7 +139,7 @@ namespace DotRecast.Detour.Crowd
: false;
if (offMeshConnection)
{
float distSq = VDist2DSqr(npos, corners[corners.Count - 1].GetPos());
float distSq = Vector3f.Dist2DSqr(npos, corners[corners.Count - 1].GetPos());
if (distSq < radius * radius)
return true;
}
@ -154,7 +154,7 @@ namespace DotRecast.Detour.Crowd
bool endOfPath = ((corners[corners.Count - 1].GetFlags() & NavMeshQuery.DT_STRAIGHTPATH_END) != 0) ? true : false;
if (endOfPath)
return Math.Min(VDist2D(npos, corners[corners.Count - 1].GetPos()), range);
return Math.Min(Vector3f.Dist2D(npos, corners[corners.Count - 1].GetPos()), range);
return range;
}

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

@ -209,8 +209,8 @@ namespace DotRecast.Detour.Crowd
float minPenalty, ObstacleAvoidanceDebugData debug)
{
// penalty for straying away from the desired and current velocities
float vpen = m_params.weightDesVel * (VDist2D(vcand, dvel) * m_invVmax);
float vcpen = m_params.weightCurVel * (VDist2D(vcand, vel) * m_invVmax);
float vpen = m_params.weightDesVel * (Vector3f.Dist2D(vcand, dvel) * m_invVmax);
float vcpen = m_params.weightCurVel * (Vector3f.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 understnad)

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

@ -246,7 +246,7 @@ namespace DotRecast.Detour.Crowd
foreach (StraightPathItem spi in path)
{
if ((spi.GetFlags() & NavMeshQuery.DT_STRAIGHTPATH_OFFMESH_CONNECTION) != 0
|| VDist2DSqr(spi.GetPos(), m_pos) > MIN_TARGET_DIST)
|| Vector3f.Dist2DSqr(spi.GetPos(), m_pos) > MIN_TARGET_DIST)
{
break;
}
@ -302,7 +302,7 @@ namespace DotRecast.Detour.Crowd
public void OptimizePathVisibility(Vector3f next, float pathOptimizationRange, NavMeshQuery navquery, IQueryFilter filter)
{
// Clamp the ray to max distance.
float dist = VDist2D(m_pos, next);
float dist = Vector3f.Dist2D(m_pos, next);
// If too close to the goal, do not try to optimize.
if (dist < 0.01f)

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

@ -11,7 +11,7 @@ namespace DotRecast.Detour.Extras.Jumplink
Func<Vector3f, float, Tuple<bool, float>> heightFunc)
{
float cs = acfg.cellSize;
float dist = (float)Math.Sqrt(VDist2DSqr(es.start.p, es.start.q));
float dist = (float)Math.Sqrt(Vector3f.Dist2DSqr(es.start.p, es.start.q));
int ngsamples = Math.Max(2, (int)Math.Ceiling(dist / cs));
SampleGroundSegment(heightFunc, es.start, ngsamples);
foreach (GroundSegment end in es.end)

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

@ -59,7 +59,7 @@ namespace DotRecast.Detour.Extras.Jumplink
GroundSegment end = es.end[js.groundSegment];
Vector3f ep = end.gsamples[js.startSample].p;
Vector3f eq = end.gsamples[js.startSample + js.samples - 1].p;
float d = Math.Min(VDist2DSqr(sp, sq), VDist2DSqr(ep, eq));
float d = Math.Min(Vector3f.Dist2DSqr(sp, sq), Vector3f.Dist2DSqr(ep, eq));
if (d >= 4 * acfg.agentRadius * acfg.agentRadius)
{
JumpLink link = new JumpLink();

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

@ -35,7 +35,7 @@ namespace DotRecast.Detour.Extras.Jumplink
private bool SampleTrajectory(JumpLinkBuilderConfig acfg, Heightfield solid, Vector3f pa, Vector3f pb, Trajectory tra)
{
float cs = Math.Min(acfg.cellSize, acfg.cellHeight);
float d = VDist2D(pa, pb) + Math.Abs(pa.y - pb.y);
float d = Vector3f.Dist2D(pa, pb) + Math.Abs(pa.y - pb.y);
int nsamples = Math.Max(2, (int)Math.Ceiling(d / cs));
for (int i = 0; i < nsamples; ++i)
{

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

@ -317,7 +317,7 @@ namespace DotRecast.Detour
bool tryLOS = false;
if ((m_query.options & DT_FINDPATH_ANY_ANGLE) != 0)
{
if ((parentRef != 0) && (VDistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
if ((parentRef != 0) && (Vector3f.DistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
{
tryLOS = true;
}

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

@ -853,7 +853,7 @@ namespace DotRecast.Detour
if ((options & DT_FINDPATH_ANY_ANGLE) != 0)
{
if ((parentRef != 0) && (raycastLimitSqr >= float.MaxValue
|| VDistSqr(parentNode.pos, bestNode.pos) < raycastLimitSqr))
|| Vector3f.DistSqr(parentNode.pos, bestNode.pos) < raycastLimitSqr))
{
tryLOS = true;
}
@ -1179,7 +1179,7 @@ namespace DotRecast.Detour
if ((m_query.options & DT_FINDPATH_ANY_ANGLE) != 0)
{
if ((parentRef != 0) && (m_query.raycastLimitSqr >= float.MaxValue
|| VDistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
|| Vector3f.DistSqr(parentNode.pos, bestNode.pos) < m_query.raycastLimitSqr))
{
tryLOS = true;
}

2
src/DotRecast.Detour/StrictPolygonByCircleConstraint.cs
View File

@ -19,7 +19,7 @@ namespace DotRecast.Detour
int outsideVertex = -1;
for (int pv = 0; pv < verts.Length; pv += 3)
{
if (VDist2DSqr(center, verts, pv) > radiusSqr)
if (Vector3f.Dist2DSqr(center, verts, pv) > radiusSqr)
{
outsideVertex = pv;
break;

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

@ -80,7 +80,7 @@ public class ConvexVolumeTool : Tool
// Create
// If clicked on that last pt, create the shape.
if (pts.Count > 0 && RcMath.VDistSqr(p, Vector3f.Of(pts[pts.Count - 3], pts[pts.Count - 2], pts[pts.Count - 1])) < 0.2f * 0.2f)
if (pts.Count > 0 && Vector3f.DistSqr(p, Vector3f.Of(pts[pts.Count - 3], pts[pts.Count - 2], pts[pts.Count - 1])) < 0.2f * 0.2f)
{
if (hull.Count > 2)
{

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

@ -189,7 +189,7 @@ public class CrowdProfilingTool
bool valid = true;
foreach (FindRandomPointResult zone in zones)
{
if (RcMath.VDistSqr(zone.GetRandomPt(), result.result.GetRandomPt()) < zoneSeparation)
if (Vector3f.DistSqr(zone.GetRandomPt(), result.result.GetRandomPt()) < zoneSeparation)
{
valid = false;
break;
@ -313,7 +313,7 @@ public class CrowdProfilingTool
List<FindRandomPointResult> potentialTargets = new();
foreach (FindRandomPointResult zone in zones)
{
if (RcMath.VDistSqr(zone.GetRandomPt(), ag.npos) > zoneRadius * zoneRadius)
if (Vector3f.DistSqr(zone.GetRandomPt(), ag.npos) > zoneRadius * zoneRadius)
{
potentialTargets.Add(zone);
}

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

@ -436,7 +436,7 @@ public class JumpLinkBuilderTool : Tool
{
Vector3f p = link.startSamples[i].p;
Vector3f q = link.endSamples[i].p;
if (i == 0 || VDist2D(prev, p) > agentRadius)
if (i == 0 || Vector3f.Dist2D(prev, p) > agentRadius)
{
geom.AddOffMeshConnection(p, q, agentRadius, false, area, flags);
prev = p;

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

@ -57,7 +57,7 @@ public class OffMeshConnectionTool : Tool
DemoOffMeshConnection nearestConnection = null;
foreach (DemoOffMeshConnection offMeshCon in geom.GetOffMeshConnections())
{
float d = Math.Min(RcMath.VDistSqr(p, offMeshCon.verts, 0), RcMath.VDistSqr(p, offMeshCon.verts, 3));
float d = Math.Min(Vector3f.DistSqr(p, offMeshCon.verts, 0), Vector3f.DistSqr(p, offMeshCon.verts, 3));
if (d < nearestDist && Math.Sqrt(d) < sample.GetSettingsUI().GetAgentRadius())
{
nearestDist = d;

2
test/DotRecast.Detour.Test/RandomPointTest.cs
View File

@ -99,7 +99,7 @@ public class RandomPointTest : AbstractDetourTest
Result<FindRandomPointResult> result = query.FindRandomPointWithinCircle(point.GetRandomRef(), point.GetRandomPt(),
radius, filter, f);
Assert.That(result.Failed(), Is.False);
float distance = VDist2D(point.GetRandomPt(), result.result.GetRandomPt());
float distance = Vector3f.Dist2D(point.GetRandomPt(), result.result.GetRandomPt());
Assert.That(distance <= radius, Is.True);
point = result.result;
}