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

refactor: RcVecUtils.SafeNormalize

This commit is contained in:
ikpil 2023-10-24 23:46:45 +09:00
parent ef8ef94826
commit 04fa38bb96
15 changed files with 60 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -23,7 +23,6 @@ namespace DotRecast.Core.Numerics
{
public struct RcVec3f
{
public const float EPSILON = 1e-6f;
public float X;
public float Y;
@ -144,29 +143,10 @@ namespace DotRecast.Core.Numerics
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public override int GetHashCode()
{
int hash = X.GetHashCode();
hash = RcHashCodes.CombineHashCodes(hash, Y.GetHashCode());
hash = RcHashCodes.CombineHashCodes(hash, Z.GetHashCode());
return hash;
return HashCode.Combine(X, Y, Z);
}
/// Normalizes the vector if the length is greater than zero.
/// If the magnitude is zero, the vector is unchanged.
/// @param[in,out] v The vector to normalize. [(x, y, z)]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void SafeNormalize()
{
float sqMag = RcMath.Sqr(X) + RcMath.Sqr(Y) + RcMath.Sqr(Z);
if (sqMag > EPSILON)
{
float inverseMag = 1.0f / (float)Math.Sqrt(sqMag);
X *= inverseMag;
Y *= inverseMag;
Z *= inverseMag;
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Min(float[] @in, int i)
{
@ -368,7 +348,7 @@ namespace DotRecast.Core.Numerics
{
float dx = v2.X - v1.X;
float dz = v2.Z - v1.Z;
return (float)Math.Sqrt(dx * dx + dz * dz);
return (float)MathF.Sqrt(dx * dx + dz * dz);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
@ -457,18 +437,13 @@ namespace DotRecast.Core.Numerics
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static RcVec3f Normalize(RcVec3f v)
{
float d = (float)(1.0f / Math.Sqrt(RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z)));
float d = 1.0f / MathF.Sqrt(RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z));
if (d != 0)
{
return new RcVec3f(
v.X *= d,
v.Y *= d,
v.Z *= d
);
}
return v;
}
}
}

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

@ -5,6 +5,8 @@ namespace DotRecast.Core.Numerics
{
public static class RcVecUtils
{
public const float EPSILON = 1e-6f;
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static float Get(this RcVec2f v, int i)
{
@ -120,5 +122,25 @@ namespace DotRecast.Core.Numerics
v1[1] * vector2.Y +
v1[2] * vector2.Z;
}
/// Normalizes the vector if the length is greater than zero.
/// If the magnitude is zero, the vector is unchanged.
/// @param[in,out] v The vector to normalize. [(x, y, z)]
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static RcVec3f SafeNormalize(RcVec3f v)
{
float sqMag = RcMath.Sqr(v.X) + RcMath.Sqr(v.Y) + RcMath.Sqr(v.Z);
if (sqMag > EPSILON)
{
float inverseMag = 1.0f / MathF.Sqrt(sqMag);
return new RcVec3f(
v.X *= inverseMag,
v.Y *= inverseMag,
v.Z *= inverseMag
);
}
return v;
}
}
}

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

@ -1091,7 +1091,7 @@ namespace DotRecast.Detour.Crowd
continue;
}
float dist = (float)Math.Sqrt(distSqr);
float dist = MathF.Sqrt(distSqr);
float weight = separationWeight * (1.0f - RcMath.Sqr(dist * invSeparationDist));
disp = RcVec3f.Mad(disp, diff, weight / dist);
@ -1233,7 +1233,7 @@ namespace DotRecast.Detour.Crowd
continue;
}
dist = (float)Math.Sqrt(dist);
dist = MathF.Sqrt(dist);
float pen = (ag.option.radius + nei.option.radius) - dist;
if (dist < 0.0001f)
{

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

@ -175,7 +175,7 @@ namespace DotRecast.Detour.Crowd
return false; // no intersection.
a = 1.0f / a;
float rd = (float)Math.Sqrt(d);
float rd = MathF.Sqrt(d);
tmin = (b - rd) * a;
tmax = (b + rd) * a;
@ -364,7 +364,7 @@ namespace DotRecast.Detour.Crowd
// vector normalization that ignores the y-component.
void DtNormalize2D(float[] v)
{
float d = (float)Math.Sqrt(v[0] * v[0] + v[2] * v[2]);
float d = MathF.Sqrt(v[0] * v[0] + v[2] * v[2]);
if (d == 0)
return;
d = 1.0f / d;

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

@ -11,7 +11,7 @@ namespace DotRecast.Detour.Extras.Jumplink
protected void SampleGround(JumpLinkBuilderConfig acfg, EdgeSampler es, ComputeNavMeshHeight heightFunc)
{
float cs = acfg.cellSize;
float dist = (float)Math.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q));
float dist = MathF.Sqrt(RcVec3f.Dist2DSqr(es.start.p, es.start.q));
int ngsamples = Math.Max(2, (int)Math.Ceiling(dist / cs));
SampleGroundSegment(heightFunc, es.start, ngsamples);

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

@ -3302,7 +3302,7 @@ namespace DotRecast.Detour
hitNormal = RcVec3f.Normalize(new RcVec3f(tangent.Z, 0, -tangent.X));
}
hitDist = (float)Math.Sqrt(radiusSqr);
hitDist = MathF.Sqrt(radiusSqr);
return status;
}

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

@ -193,7 +193,7 @@ namespace DotRecast.Detour
acc += dacc;
}
float v = (float)Math.Sqrt(t);
float v = MathF.Sqrt(t);
float a = 1 - v;
float b = (1 - u) * v;

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

@ -335,7 +335,7 @@ public class DebugDraw
float dx = x1 - x0;
float dy = y1 - y0;
float dz = z1 - z0;
float len = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
float len = MathF.Sqrt(dx * dx + dy * dy + dz * dz);
RcVec3f prev = new RcVec3f();
EvalArc(x0, y0, z0, dx, dy, dz, len * h, PAD, ref prev);
for (int i = 1; i <= NUM_ARC_PTS; ++i)
@ -677,7 +677,7 @@ public class DebugDraw
private void NormalizePlane(float px, float py, float pz, float pw, ref float[] plane)
{
float length = (float)Math.Sqrt(px * px + py * py + pz * pz);
float length = MathF.Sqrt(px * px + py * py + pz * pz);
if (length != 0)
{
length = 1f / length;

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

@ -424,7 +424,7 @@ public class TestNavmeshSampleTool : ISampleTool
dd.DepthMask(false);
float dx = m_epos.X - m_spos.X;
float dz = m_epos.Z - m_spos.Z;
float dist = (float)Math.Sqrt(dx * dx + dz * dz);
float dist = MathF.Sqrt(dx * dx + dz * dz);
dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f);
dd.DepthMask(true);
}
@ -566,7 +566,7 @@ public class TestNavmeshSampleTool : ISampleTool
dd.DepthMask(false);
float dx = m_epos.X - m_spos.X;
float dz = m_epos.Z - m_spos.Z;
float dist = (float)Math.Sqrt(dx * dx + dz * dz);
float dist = MathF.Sqrt(dx * dx + dz * dz);
dd.DebugDrawCircle(m_spos.X, m_spos.Y + agentHeight / 2, m_spos.Z, dist, DuRGBA(64, 16, 0, 220), 2.0f);
dd.DepthMask(true);
}

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

@ -101,7 +101,7 @@ namespace DotRecast.Recast.Toolset.Geom
normals[i] = e0.Y * e1.Z - e0.Z * e1.Y;
normals[i + 1] = e0.Z * e1.X - e0.X * e1.Z;
normals[i + 2] = e0.X * e1.Y - e0.Y * e1.X;
float d = (float)Math.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
float d = MathF.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
if (d > 0)
{
d = 1.0f / d;

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

@ -74,7 +74,7 @@ namespace DotRecast.Recast.Toolset.Tools
// Find movement delta.
RcVec3f delta = RcVec3f.Subtract(steerPos, iterPos);
float len = (float)Math.Sqrt(RcVec3f.Dot(delta, delta));
float len = MathF.Sqrt(RcVec3f.Dot(delta, delta));
// If the steer target is end of path or off-mesh link, do not move past the location.
if ((endOfPath || offMeshConnection) && len < STEP_SIZE)
{
@ -329,7 +329,7 @@ namespace DotRecast.Recast.Toolset.Tools
float dx = epos.X - spos.X;
float dz = epos.Z - spos.Z;
float dist = (float)Math.Sqrt(dx * dx + dz * dz);
float dist = MathF.Sqrt(dx * dx + dz * dz);
List<long> tempResultRefs = new List<long>();
List<long> tempParentRefs = new List<long>();
@ -417,7 +417,7 @@ namespace DotRecast.Recast.Toolset.Tools
float dx = epos.X - spos.X;
float dz = epos.Z - spos.Z;
float dist = (float)Math.Sqrt(dx * dx + dz * dz);
float dist = MathF.Sqrt(dx * dx + dz * dz);
IDtPolygonByCircleConstraint constraint = constrainByCircle
? DtStrictDtPolygonByCircleConstraint.Shared

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

@ -163,7 +163,7 @@ namespace DotRecast.Recast.Geom
normals[i] = e0.Y * e1.Z - e0.Z * e1.Y;
normals[i + 1] = e0.Z * e1.X - e0.X * e1.Z;
normals[i + 2] = e0.X * e1.Y - e0.Y * e1.X;
float d = (float)Math.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
float d = MathF.Sqrt(normals[i] * normals[i] + normals[i + 1] * normals[i + 1] + normals[i + 2] * normals[i + 2]);
if (d > 0)
{
d = 1.0f / d;

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

@ -760,12 +760,12 @@ namespace DotRecast.Recast
// From A to B on the x/z plane
RcVec3f prevSegmentDir = RcVec3f.Subtract(vertB, vertA);
prevSegmentDir.Y = 0; // Squash onto x/z plane
prevSegmentDir.SafeNormalize();
prevSegmentDir = RcVecUtils.SafeNormalize(prevSegmentDir);
// From B to C on the x/z plane
RcVec3f currSegmentDir = RcVec3f.Subtract(vertC, vertB);
currSegmentDir.Y = 0; // Squash onto x/z plane
currSegmentDir.SafeNormalize();
currSegmentDir = RcVecUtils.SafeNormalize(currSegmentDir);
// The y component of the cross product of the two normalized segment directions.
// The X and Z components of the cross product are both zero because the two
@ -792,7 +792,7 @@ namespace DotRecast.Recast
bool bevel = cornerMiterSqMag * MITER_LIMIT * MITER_LIMIT < 1.0f;
// Scale the corner miter so it's proportional to how much the corner should be offset compared to the edges.
if (cornerMiterSqMag > RcVec3f.EPSILON)
if (cornerMiterSqMag > RcVecUtils.EPSILON)
{
float scale = 1.0f / cornerMiterSqMag;
cornerMiterX *= scale;

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

@ -253,7 +253,7 @@ namespace DotRecast.Recast
return null;
}
float discrSqrt = (float)Math.Sqrt(discr);
float discrSqrt = MathF.Sqrt(discr);
float tmin = -b - discrSqrt;
float tmax = -b + discrSqrt;
@ -346,7 +346,7 @@ namespace DotRecast.Recast
float discr = b * b - c;
if (discr > EPSILON)
{
float discrSqrt = (float)Math.Sqrt(discr);
float discrSqrt = MathF.Sqrt(discr);
float t1 = -b - discrSqrt;
float t2 = -b + discrSqrt;
if (t1 <= 1 && t2 >= 0)
@ -454,7 +454,7 @@ namespace DotRecast.Recast
return null; // No real roots; no intersection
}
float discSqrt = (float)Math.Sqrt(discr);
float discSqrt = MathF.Sqrt(discr);
float t1 = (-b - discSqrt) / a;
float t2 = (-b + discSqrt) / a;

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

@ -60,7 +60,7 @@ namespace DotRecast.Recast
private static float Vdist2(float[] verts, int p, int q)
{
return (float)Math.Sqrt(VdistSq2(verts, p, q));
return MathF.Sqrt(VdistSq2(verts, p, q));
}
private static float VdistSq2(float[] p, float[] q)
@ -88,17 +88,17 @@ namespace DotRecast.Recast
private static float Vdist2(float[] p, float[] q)
{
return (float)Math.Sqrt(VdistSq2(p, q));
return MathF.Sqrt(VdistSq2(p, q));
}
private static float Vdist2(RcVec3f p, RcVec3f q)
{
return (float)Math.Sqrt(VdistSq2(p, q));
return MathF.Sqrt(VdistSq2(p, q));
}
private static float Vdist2(float[] p, RcVec3f q)
{
return (float)Math.Sqrt(VdistSq2(p, q));
return MathF.Sqrt(VdistSq2(p, q));
}
@ -119,12 +119,12 @@ namespace DotRecast.Recast
private static float Vdist2(float[] p, float[] verts, int q)
{
return (float)Math.Sqrt(VdistSq2(p, verts, q));
return MathF.Sqrt(VdistSq2(p, verts, q));
}
private static float Vdist2(RcVec3f p, float[] verts, int q)
{
return (float)Math.Sqrt(VdistSq2(p, verts, q));
return MathF.Sqrt(VdistSq2(p, verts, q));
}
@ -743,7 +743,7 @@ namespace DotRecast.Recast
minDist = Math.Min(minDist, maxEdgeDist);
}
return (float)Math.Sqrt(minDist);
return MathF.Sqrt(minDist);
}
private static void TriangulateHull(int nverts, float[] verts, int nhull, int[] hull, int nin, List<int> tris)
@ -890,7 +890,7 @@ namespace DotRecast.Recast
float dx = @in[vi + 0] - @in[vj + 0];
float dy = @in[vi + 1] - @in[vj + 1];
float dz = @in[vi + 2] - @in[vj + 2];
float d = (float)Math.Sqrt(dx * dx + dz * dz);
float d = MathF.Sqrt(dx * dx + dz * dz);
int nn = 1 + (int)Math.Floor(d / sampleDist);
if (nn >= MAX_VERTS_PER_EDGE)
{