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

inline vector3f

This commit is contained in:
ikpil 2023-05-23 00:07:12 +09:00
parent febc66e791
commit 3f79c8294f
6 changed files with 65 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,32 +27,6 @@ namespace DotRecast.Core
public const float EPS = 1e-4f;
private static readonly float EQUAL_THRESHOLD = Sqr(1.0f / 16384.0f);
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;
}
public static float Sqr(float f)
{
return f * f;
@ -91,38 +65,30 @@ namespace DotRecast.Core
}
/// 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]
public static Vector3f VLerp(float[] verts, int v1, int v2, float t)
public static float VDistSqr(Vector3f v1, float[] v2, int i)
{
return new Vector3f(
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
);
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 void VMin(ref Vector3f @out, float[] @in, int i)
public static float VDistSqr(Vector3f v1, Vector3f v2)
{
@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]);
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 void VMax(ref Vector3f @out, float[] @in, int i)
public static float VDistSqr(float[] v, int i, int j)
{
@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]);
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)]

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

@ -218,6 +218,22 @@ namespace DotRecast.Core
}
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Min(float[] @in, int i)
{
x = Math.Min(x, @in[i]);
y = Math.Min(y, @in[i + 1]);
z = Math.Min(z, @in[i + 2]);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public void Max(float[] @in, int i)
{
x = Math.Max(x, @in[i]);
y = Math.Max(y, @in[i + 1]);
z = Math.Max(z, @in[i + 2]);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool operator ==(Vector3f left, Vector3f right)
{
@ -325,5 +341,21 @@ namespace DotRecast.Core
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 Vector3f Lerp(float[] verts, int v1, int v2, float t)
{
return new Vector3f(
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
);
}
}
}

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

@ -45,8 +45,8 @@ namespace DotRecast.Detour.Extras
bmax.Set(data.verts, data.polys[i].verts[0] * 3);
for (int j = 1; j < data.polys[i].vertCount; j++)
{
VMin(ref bmin, data.verts, data.polys[i].verts[j] * 3);
VMax(ref bmax, data.verts, data.polys[i].verts[j] * 3);
bmin.Min(data.verts, data.polys[i].verts[j] * 3);
bmax.Max(data.verts, data.polys[i].verts[j] * 3);
}
it.bmin[0] = Clamp((int)((bmin.x - data.header.bmin.x) * quantFactor), 0, 0x7fffffff);

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

@ -412,8 +412,8 @@ namespace DotRecast.Detour
for (int j = 1; j < p.vertCount; ++j)
{
v = p.verts[j] * 3;
VMin(ref bmin, tile.data.verts, v);
VMax(ref bmax, tile.data.verts, v);
bmin.Min(tile.data.verts, v);
bmax.Max(tile.data.verts, v);
}
if (OverlapBounds(qmin, qmax, bmin, bmax))

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

@ -166,8 +166,8 @@ namespace DotRecast.Detour
bmax.Set(option.detailVerts, dv);
for (int j = 1; j < ndv; j++)
{
VMin(ref bmin, option.detailVerts, dv + j * 3);
VMax(ref bmax, option.detailVerts, dv + j * 3);
bmin.Min(option.detailVerts, dv + j * 3);
bmax.Max(option.detailVerts, dv + j * 3);
}
// BV-tree uses cs for all dimensions

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

@ -499,7 +499,7 @@ namespace DotRecast.Detour
int va = imin * 3;
int vb = ((imin + 1) % nv) * 3;
closest = VLerp(verts, va, vb, edget[imin]);
closest = Vector3f.Lerp(verts, va, vb, edget[imin]);
}
return Results.Success(closest);
@ -656,8 +656,8 @@ namespace DotRecast.Detour
for (int j = 1; j < p.vertCount; ++j)
{
v = p.verts[j] * 3;
VMin(ref bmin, tile.data.verts, v);
VMax(ref bmax, tile.data.verts, v);
bmin.Min(tile.data.verts, v);
bmax.Max(tile.data.verts, v);
}
if (OverlapBounds(qmin, qmax, bmin, bmax))
@ -1912,7 +1912,7 @@ namespace DotRecast.Detour
if (distSqr < bestDist)
{
// Update nearest distance.
bestPos = VLerp(verts, vj, vi, tseg);
bestPos = Vector3f.Lerp(verts, vj, vi, tseg);
bestDist = distSqr;
bestNode = curNode;
}
@ -2089,8 +2089,8 @@ namespace DotRecast.Detour
float s = 1.0f / 255.0f;
float tmin = link.bmin * s;
float tmax = link.bmax * s;
left = VLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin);
right = VLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax);
left = Vector3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin);
right = Vector3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax);
}
}
@ -3123,8 +3123,8 @@ namespace DotRecast.Detour
float tmin = ints[k].tmin / 255.0f;
float tmax = ints[k].tmax / 255.0f;
var seg = new SegmentVert();
seg.vmin = VLerp(tile.data.verts, vj, vi, tmin);
seg.vmax = VLerp(tile.data.verts, vj, vi, tmax);
seg.vmin = Vector3f.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = Vector3f.Lerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg);
segmentRefs.Add(ints[k].refs);
}
@ -3137,8 +3137,8 @@ namespace DotRecast.Detour
float tmin = imin / 255.0f;
float tmax = imax / 255.0f;
var seg = new SegmentVert();
seg.vmin = VLerp(tile.data.verts, vj, vi, tmin);
seg.vmax = VLerp(tile.data.verts, vj, vi, tmax);
seg.vmin = Vector3f.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = Vector3f.Lerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg);
segmentRefs.Add(0L);
}