From e8d14375e597ddd11ae75afcfc846cfe41e82665 Mon Sep 17 00:00:00 2001 From: ikpil Date: Wed, 13 Sep 2023 23:33:16 +0900 Subject: [PATCH] refactor: float[16] -> RcMatrix4x4f --- src/DotRecast.Core/RcMatrix4x4f.cs | 104 +++++++++++++++--- src/DotRecast.Recast.Demo/Draw/DebugDraw.cs | 41 ++++--- src/DotRecast.Recast.Demo/Draw/GLU.cs | 44 +------- .../Draw/ModernOpenGLDraw.cs | 4 +- .../Tools/RcDynamicUpdateTool.cs | 6 +- 5 files changed, 113 insertions(+), 86 deletions(-) diff --git a/src/DotRecast.Core/RcMatrix4x4f.cs b/src/DotRecast.Core/RcMatrix4x4f.cs index 2a81478..26c2d00 100644 --- a/src/DotRecast.Core/RcMatrix4x4f.cs +++ b/src/DotRecast.Core/RcMatrix4x4f.cs @@ -1,4 +1,5 @@ using System; +using System.Numerics; namespace DotRecast.Core { @@ -12,22 +13,22 @@ namespace DotRecast.Core 0f, 0f, 0f, 1f ); - public float M11; - public float M12; - public float M13; - public float M14; - public float M21; - public float M22; - public float M23; - public float M24; - public float M31; - public float M32; - public float M33; - public float M34; - public float M41; - public float M42; - public float M43; - public float M44; + public float M11; // 0 + public float M12; // 1 + public float M13; // 2 + public float M14; // 3 + public float M21; // 4 + public float M22; // 5 + public float M23; // 6 + public float M24; // 7 + public float M31; // 8 + public float M32; // 9 + public float M33; // 10 + public float M34; // 11 + public float M41; // 12 + public float M42; // 13 + public float M43; // 14 + public float M44; // 15 public RcMatrix4x4f( float m11, float m12, float m13, float m14, @@ -56,6 +57,48 @@ namespace DotRecast.Core M44 = m44; } + public RcMatrix4x4f(float[] m) + { + M11 = m[0]; + M12 = m[1]; + M13 = m[2]; + M14 = m[3]; + M21 = m[4]; + M22 = m[5]; + M23 = m[6]; + M24 = m[7]; + M31 = m[8]; + M32 = m[9]; + M33 = m[10]; + M34 = m[11]; + M41 = m[12]; + M42 = m[13]; + M43 = m[14]; + M44 = m[15]; + } + + + public void CopyTo(float[] m) + { + m[0] = M11; + m[1] = M12; + m[2] = M13; + m[3] = M14; + m[4] = M21; + m[5] = M22; + m[6] = M23; + m[7] = M24; + m[8] = M31; + m[9] = M32; + m[10] = M33; + m[11] = M34; + m[12] = M41; + m[13] = M42; + m[14] = M43; + m[15] = M44; + } + + public static RcMatrix4x4f Identity => _identity; public readonly bool IsIdentity => @@ -105,7 +148,34 @@ namespace DotRecast.Core return dest; } - public static RcMatrix4x4f Rotate(float a, float x, float y, float z) + public static RcMatrix4x4f Mul(float[] left, float[] right) + { + float m00 = left[0] * right[0] + left[4] * right[1] + left[8] * right[2] + left[12] * right[3]; + float m01 = left[1] * right[0] + left[5] * right[1] + left[9] * right[2] + left[13] * right[3]; + float m02 = left[2] * right[0] + left[6] * right[1] + left[10] * right[2] + left[14] * right[3]; + float m03 = left[3] * right[0] + left[7] * right[1] + left[11] * right[2] + left[15] * right[3]; + float m10 = left[0] * right[4] + left[4] * right[5] + left[8] * right[6] + left[12] * right[7]; + float m11 = left[1] * right[4] + left[5] * right[5] + left[9] * right[6] + left[13] * right[7]; + float m12 = left[2] * right[4] + left[6] * right[5] + left[10] * right[6] + left[14] * right[7]; + float m13 = left[3] * right[4] + left[7] * right[5] + left[11] * right[6] + left[15] * right[7]; + float m20 = left[0] * right[8] + left[4] * right[9] + left[8] * right[10] + left[12] * right[11]; + float m21 = left[1] * right[8] + left[5] * right[9] + left[9] * right[10] + left[13] * right[11]; + float m22 = left[2] * right[8] + left[6] * right[9] + left[10] * right[10] + left[14] * right[11]; + float m23 = left[3] * right[8] + left[7] * right[9] + left[11] * right[10] + left[15] * right[11]; + float m30 = left[0] * right[12] + left[4] * right[13] + left[8] * right[14] + left[12] * right[15]; + float m31 = left[1] * right[12] + left[5] * right[13] + left[9] * right[14] + left[13] * right[15]; + float m32 = left[2] * right[12] + left[6] * right[13] + left[10] * right[14] + left[14] * right[15]; + float m33 = left[3] * right[12] + left[7] * right[13] + left[11] * right[14] + left[15] * right[15]; + + return new RcMatrix4x4f( + m00, m01, m02, m03, + m10, m11, m12, m13, + m20, m21, m22, m23, + m30, m31, m32, m33 + ); + } + + public static RcMatrix4x4f CreateFromRotate(float a, float x, float y, float z) { var matrix = new RcMatrix4x4f(); a = (float)(a * Math.PI / 180.0); // convert to radians diff --git a/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs b/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs index 7ffee95..28b7998 100644 --- a/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs +++ b/src/DotRecast.Recast.Demo/Draw/DebugDraw.cs @@ -19,6 +19,7 @@ freely, subject to the following restrictions: */ using System; +using System.Numerics; using Silk.NET.OpenGL; using DotRecast.Core; using DotRecast.Recast.Toolset.Builder; @@ -638,15 +639,17 @@ public class DebugDraw public float[] ViewMatrix(RcVec3f cameraPos, float[] cameraEulers) { - float[] rx = GLU.Build_4x4_rotation_matrix(cameraEulers[0], 1, 0, 0); - float[] ry = GLU.Build_4x4_rotation_matrix(cameraEulers[1], 0, 1, 0); - float[] r = GLU.Mul(rx, ry); - float[] t = new float[16]; - t[0] = t[5] = t[10] = t[15] = 1; - t[12] = -cameraPos.x; - t[13] = -cameraPos.y; - t[14] = -cameraPos.z; - _viewMatrix = GLU.Mul(r, t); + var rx = RcMatrix4x4f.CreateFromRotate(cameraEulers[0], 1, 0, 0); + var ry = RcMatrix4x4f.CreateFromRotate(cameraEulers[1], 0, 1, 0); + var r = RcMatrix4x4f.Mul(rx, ry); + + var t = new RcMatrix4x4f(); + t.M11 = t.M22 = t.M33 = t.M44 = 1; + t.M41 = -cameraPos.x; + t.M42 = -cameraPos.y; + t.M43 = -cameraPos.z; + var mul = RcMatrix4x4f.Mul(r, t); + mul.CopyTo(_viewMatrix); GetOpenGlDraw().ViewMatrix(_viewMatrix); UpdateFrustum(); return _viewMatrix; @@ -665,19 +668,13 @@ public class DebugDraw private void UpdateFrustum() { - float[] vpm = GLU.Mul(_projectionMatrix, _viewMatrix); - // left - NormalizePlane(vpm[0 + 3] + vpm[0 + 0], vpm[4 + 3] + vpm[4 + 0], vpm[8 + 3] + vpm[8 + 0], vpm[12 + 3] + vpm[12 + 0], ref frustumPlanes[0]); - // right - NormalizePlane(vpm[0 + 3] - vpm[0 + 0], vpm[4 + 3] - vpm[4 + 0], vpm[8 + 3] - vpm[8 + 0], vpm[12 + 3] - vpm[12 + 0], ref frustumPlanes[1]); - // top - NormalizePlane(vpm[0 + 3] - vpm[0 + 1], vpm[4 + 3] - vpm[4 + 1], vpm[8 + 3] - vpm[8 + 1], vpm[12 + 3] - vpm[12 + 1], ref frustumPlanes[2]); - // bottom - NormalizePlane(vpm[0 + 3] + vpm[0 + 1], vpm[4 + 3] + vpm[4 + 1], vpm[8 + 3] + vpm[8 + 1], vpm[12 + 3] + vpm[12 + 1], ref frustumPlanes[3]); - // near - NormalizePlane(vpm[0 + 3] + vpm[0 + 2], vpm[4 + 3] + vpm[4 + 2], vpm[8 + 3] + vpm[8 + 2], vpm[12 + 3] + vpm[12 + 2], ref frustumPlanes[4]); - // far - NormalizePlane(vpm[0 + 3] - vpm[0 + 2], vpm[4 + 3] - vpm[4 + 2], vpm[8 + 3] - vpm[8 + 2], vpm[12 + 3] - vpm[12 + 2], ref frustumPlanes[5]); + var vpm = RcMatrix4x4f.Mul(_projectionMatrix, _viewMatrix); + NormalizePlane(vpm.M14 + vpm.M11, vpm.M24 + vpm.M21, vpm.M34 + vpm.M31, vpm.M44 + vpm.M41, ref frustumPlanes[0]); // left + NormalizePlane(vpm.M14 - vpm.M11, vpm.M24 - vpm.M21, vpm.M34 - vpm.M31, vpm.M44 - vpm.M41, ref frustumPlanes[1]); // right + NormalizePlane(vpm.M14 - vpm.M12, vpm.M24 - vpm.M22, vpm.M34 - vpm.M32, vpm.M44 - vpm.M42, ref frustumPlanes[2]); // top + NormalizePlane(vpm.M14 + vpm.M12, vpm.M24 + vpm.M22, vpm.M34 + vpm.M32, vpm.M44 + vpm.M42, ref frustumPlanes[3]); // bottom + NormalizePlane(vpm.M14 + vpm.M13, vpm.M24 + vpm.M23, vpm.M34 + vpm.M33, vpm.M44 + vpm.M43, ref frustumPlanes[4]); // near + NormalizePlane(vpm.M14 - vpm.M13, vpm.M24 - vpm.M23, vpm.M34 - vpm.M33, vpm.M44 - vpm.M43, ref frustumPlanes[5]); // far } private void NormalizePlane(float px, float py, float pz, float pw, ref float[] plane) diff --git a/src/DotRecast.Recast.Demo/Draw/GLU.cs b/src/DotRecast.Recast.Demo/Draw/GLU.cs index a134afb..2c228e8 100644 --- a/src/DotRecast.Recast.Demo/Draw/GLU.cs +++ b/src/DotRecast.Recast.Demo/Draw/GLU.cs @@ -31,8 +31,7 @@ public static class GLU return projectionMatrix; } - public static void GlhPerspectivef2(float[] matrix, float fovyInDegrees, float aspectRatio, float znear, - float zfar) + public static void GlhPerspectivef2(float[] matrix, float fovyInDegrees, float aspectRatio, float znear, float zfar) { float ymax, xmax; ymax = (float)(znear * Math.Tan(fovyInDegrees * Math.PI / 360.0)); @@ -40,8 +39,7 @@ public static class GLU GlhFrustumf2(matrix, -xmax, xmax, -ymax, ymax, znear, zfar); } - private static void GlhFrustumf2(float[] matrix, float left, float right, float bottom, float top, float znear, - float zfar) + private static void GlhFrustumf2(float[] matrix, float left, float right, float bottom, float top, float znear, float zfar) { float temp, temp2, temp3, temp4; temp = 2.0f * znear; @@ -369,44 +367,6 @@ public static class GLU m[(c) * 4 + (r)] = v; } - public static float[] Build_4x4_rotation_matrix(float a, float x, float y, float z) - { - float[] matrix = new float[16]; - a = (float)(a * Math.PI / 180.0); // convert to radians - float s = (float)Math.Sin(a); - float c = (float)Math.Cos(a); - float t = 1.0f - c; - - float tx = t * x; - float ty = t * y; - float tz = t * z; - - float sz = s * z; - float sy = s * y; - float sx = s * x; - - matrix[0] = tx * x + c; - matrix[1] = tx * y + sz; - matrix[2] = tx * z - sy; - matrix[3] = 0; - - matrix[4] = tx * y - sz; - matrix[5] = ty * y + c; - matrix[6] = ty * z + sx; - matrix[7] = 0; - - matrix[8] = tx * z + sy; - matrix[9] = ty * z - sx; - matrix[10] = tz * z + c; - matrix[11] = 0; - - matrix[12] = 0; - matrix[13] = 0; - matrix[14] = 0; - matrix[15] = 1; - return matrix; - } - public static float[] Mul(float[] left, float[] right) { float m00 = left[0] * right[0] + left[4] * right[1] + left[8] * right[2] + left[12] * right[3]; diff --git a/src/DotRecast.Recast.Demo/Draw/ModernOpenGLDraw.cs b/src/DotRecast.Recast.Demo/Draw/ModernOpenGLDraw.cs index 23160dc..5721e90 100644 --- a/src/DotRecast.Recast.Demo/Draw/ModernOpenGLDraw.cs +++ b/src/DotRecast.Recast.Demo/Draw/ModernOpenGLDraw.cs @@ -317,12 +317,12 @@ public class ModernOpenGLDraw : IOpenGLDraw public void ProjectionMatrix(float[] projectionMatrix) { - this._projectionMatrix = projectionMatrix; + _projectionMatrix = projectionMatrix; } public void ViewMatrix(float[] viewMatrix) { - this._viewMatrix = viewMatrix; + _viewMatrix = viewMatrix; } public void Fog(float start, float end) diff --git a/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs b/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs index e14fe4c..ff4efd5 100644 --- a/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs +++ b/src/DotRecast.Recast.Toolset/Tools/RcDynamicUpdateTool.cs @@ -238,7 +238,7 @@ namespace DotRecast.Recast.Toolset.Tools SampleAreaModifications.SAMPLE_POLYAREA_TYPE_ROAD, walkableClimb); var roofUp = RcVec3f.Zero; RcVec3f roofExtent = RcVec3f.Of(4.5f, 4.5f, 8f); - var rx = RcMatrix4x4f.Rotate(45, forward.x, forward.y, forward.z); + var rx = RcMatrix4x4f.CreateFromRotate(45, forward.x, forward.y, forward.z); roofUp = MulMatrixVector(ref roofUp, rx, baseUp); RcVec3f roofCenter = RcVec3f.Of(p.x, p.y + 6, p.z); BoxCollider roof = new BoxCollider(roofCenter, Detour.Dynamic.Colliders.BoxCollider.GetHalfEdges(roofUp, forward, roofExtent), @@ -297,8 +297,8 @@ namespace DotRecast.Recast.Toolset.Tools private float[] TransformVertices(RcVec3f p, DemoInputGeomProvider geom, float ax) { - var rx = RcMatrix4x4f.Rotate((float)random.NextDouble() * ax, 1, 0, 0); - var ry = RcMatrix4x4f.Rotate((float)random.NextDouble() * 360, 0, 1, 0); + var rx = RcMatrix4x4f.CreateFromRotate((float)random.NextDouble() * ax, 1, 0, 0); + var ry = RcMatrix4x4f.CreateFromRotate((float)random.NextDouble() * 360, 0, 1, 0); var m = RcMatrix4x4f.Mul(rx, ry); float[] verts = new float[geom.vertices.Length]; RcVec3f v = new RcVec3f();