a.samsonkin
/
DotRecastNetSim
forked from bit/DotRecastNetSim
1
0
Fork 0
Code Pull Requests 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

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

@ -27,32 +27,6 @@ namespace DotRecast.Core
public const float EPS = 1e-4f; public const float EPS = 1e-4f;
private static readonly float EQUAL_THRESHOLD = Sqr(1.0f / 16384.0f); 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) public static float Sqr(float f)
{ {
return f * f; return f * f;
@ -91,37 +65,29 @@ namespace DotRecast.Core
} }
public static float VDistSqr(Vector3f v1, float[] v2, int i)
/// 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)
{ {
return new Vector3f( float dx = v2[i] - v1.x;
verts[v1 + 0] + (verts[v2 + 0] - verts[v1 + 0]) * t, float dy = v2[i + 1] - v1.y;
verts[v1 + 1] + (verts[v2 + 1] - verts[v1 + 1]) * t, float dz = v2[i + 2] - v1.z;
verts[v1 + 2] + (verts[v2 + 2] - verts[v1 + 2]) * t return dx * dx + dy * dy + dz * dz;
);
}
public static void VMin(ref Vector3f @out, float[] @in, int i)
{
@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]);
} }
public static float VDistSqr(Vector3f v1, Vector3f v2)
public static void VMax(ref Vector3f @out, float[] @in, int i)
{ {
@out.x = Math.Max(@out.x, @in[i]); float dx = v2.x - v1.x;
@out.y = Math.Max(@out.y, @in[i + 1]); float dy = v2.y - v1.y;
@out.z = Math.Max(@out.z, @in[i + 2]); 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. /// Returns the distance between two points.
/// @param[in] v1 A point. [(x, y, z)] /// @param[in] v1 A point. [(x, y, z)]

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

@ -217,6 +217,22 @@ namespace DotRecast.Core
z *= d; z *= d;
} }
} }
[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)] [MethodImpl(MethodImplOptions.AggressiveInlining)]
public static bool operator ==(Vector3f left, Vector3f right) public static bool operator ==(Vector3f left, Vector3f right)
@ -325,5 +341,21 @@ namespace DotRecast.Core
z = v1.z + (v2.z * 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 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); bmax.Set(data.verts, data.polys[i].verts[0] * 3);
for (int j = 1; j < data.polys[i].vertCount; j++) for (int j = 1; j < data.polys[i].vertCount; j++)
{ {
VMin(ref bmin, data.verts, data.polys[i].verts[j] * 3); bmin.Min(data.verts, data.polys[i].verts[j] * 3);
VMax(ref bmax, 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); 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) for (int j = 1; j < p.vertCount; ++j)
{ {
v = p.verts[j] * 3; v = p.verts[j] * 3;
VMin(ref bmin, tile.data.verts, v); bmin.Min(tile.data.verts, v);
VMax(ref bmax, tile.data.verts, v); bmax.Max(tile.data.verts, v);
} }
if (OverlapBounds(qmin, qmax, bmin, bmax)) 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); bmax.Set(option.detailVerts, dv);
for (int j = 1; j < ndv; j++) for (int j = 1; j < ndv; j++)
{ {
VMin(ref bmin, option.detailVerts, dv + j * 3); bmin.Min(option.detailVerts, dv + j * 3);
VMax(ref bmax, option.detailVerts, dv + j * 3); bmax.Max(option.detailVerts, dv + j * 3);
} }
// BV-tree uses cs for all dimensions // 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 va = imin * 3;
int vb = ((imin + 1) % nv) * 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); return Results.Success(closest);
@ -656,8 +656,8 @@ namespace DotRecast.Detour
for (int j = 1; j < p.vertCount; ++j) for (int j = 1; j < p.vertCount; ++j)
{ {
v = p.verts[j] * 3; v = p.verts[j] * 3;
VMin(ref bmin, tile.data.verts, v); bmin.Min(tile.data.verts, v);
VMax(ref bmax, tile.data.verts, v); bmax.Max(tile.data.verts, v);
} }
if (OverlapBounds(qmin, qmax, bmin, bmax)) if (OverlapBounds(qmin, qmax, bmin, bmax))
@ -1912,7 +1912,7 @@ namespace DotRecast.Detour
if (distSqr < bestDist) if (distSqr < bestDist)
{ {
// Update nearest distance. // Update nearest distance.
bestPos = VLerp(verts, vj, vi, tseg); bestPos = Vector3f.Lerp(verts, vj, vi, tseg);
bestDist = distSqr; bestDist = distSqr;
bestNode = curNode; bestNode = curNode;
} }
@ -2089,8 +2089,8 @@ namespace DotRecast.Detour
float s = 1.0f / 255.0f; float s = 1.0f / 255.0f;
float tmin = link.bmin * s; float tmin = link.bmin * s;
float tmax = link.bmax * s; float tmax = link.bmax * s;
left = VLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin); left = Vector3f.Lerp(fromTile.data.verts, v0 * 3, v1 * 3, tmin);
right = VLerp(fromTile.data.verts, v0 * 3, v1 * 3, tmax); 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 tmin = ints[k].tmin / 255.0f;
float tmax = ints[k].tmax / 255.0f; float tmax = ints[k].tmax / 255.0f;
var seg = new SegmentVert(); var seg = new SegmentVert();
seg.vmin = VLerp(tile.data.verts, vj, vi, tmin); seg.vmin = Vector3f.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = VLerp(tile.data.verts, vj, vi, tmax); seg.vmax = Vector3f.Lerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg); segmentVerts.Add(seg);
segmentRefs.Add(ints[k].refs); segmentRefs.Add(ints[k].refs);
} }
@ -3137,8 +3137,8 @@ namespace DotRecast.Detour
float tmin = imin / 255.0f; float tmin = imin / 255.0f;
float tmax = imax / 255.0f; float tmax = imax / 255.0f;
var seg = new SegmentVert(); var seg = new SegmentVert();
seg.vmin = VLerp(tile.data.verts, vj, vi, tmin); seg.vmin = Vector3f.Lerp(tile.data.verts, vj, vi, tmin);
seg.vmax = VLerp(tile.data.verts, vj, vi, tmax); seg.vmax = Vector3f.Lerp(tile.data.verts, vj, vi, tmax);
segmentVerts.Add(seg); segmentVerts.Add(seg);
segmentRefs.Add(0L); segmentRefs.Add(0L);
} }