From d05595e44c449847f5ba50b456cf836837cef6a8 Mon Sep 17 00:00:00 2001 From: hondacrx Date: Wed, 13 Jul 2022 15:02:06 -0400 Subject: [PATCH] Fixes using gameobject like chairs, also fixes indoor checks --- Source/Framework/GameMath/Box.cs | 6 +- .../RecastDetour/Detour/DetourCommon.cs | 17 ++++ .../RecastDetour/Detour/DetourNavMesh.cs | 23 ++--- .../RecastDetour/Detour/DetourNavMeshQuery.cs | 2 +- Source/Framework/Util/Extensions.cs | 16 +++- Source/Framework/Util/MathFunctions.cs | 92 ++++++++++++++++++- .../Game/Collision/Models/GameObjectModel.cs | 30 +++--- Source/Game/Collision/Models/ModelInstance.cs | 20 ++-- Source/Game/Entities/GameObject/GameObject.cs | 10 +- 9 files changed, 168 insertions(+), 48 deletions(-) diff --git a/Source/Framework/GameMath/Box.cs b/Source/Framework/GameMath/Box.cs index 6315f747c..89b9457bd 100644 --- a/Source/Framework/GameMath/Box.cs +++ b/Source/Framework/GameMath/Box.cs @@ -29,6 +29,8 @@ namespace Framework.GameMath public Box(Vector3 min, Vector3 max) { + _center = (max + min) * 0.5f; + Vector3 bounds = new Vector3(max.X - min.X, max.Y - min.Y, max.Z - min.Z); _edgeVector[0] = new Vector3(bounds.X, 0, 0); _edgeVector[1] = new Vector3(0, bounds.Y, 0); @@ -67,12 +69,12 @@ namespace Framework.GameMath Vector3 v = _edgeVector[1]; Vector3 w = _edgeVector[0]; - Matrix4x4 M = new(u.X, v.X, w.X, 0.0f, u.Y, v.Y, w.Y, 0.0f, u.Z, v.Z, w.Z, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + Matrix4x4 M = new(u.X, v.X, w.X, 0.0f, u.Y, v.Y, w.Y, 0.0f, u.Z, v.Z, w.Z, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); // M^-1 * (point - _corner[0]) = point in unit cube's object space // compute the inverse of M Matrix4x4.Invert(M, out M); - Vector3 osPoint = Vector3.TransformNormal(point - Corner(0), M); + Vector3 osPoint = M.Multiply(point - Corner(0)); return (osPoint.X >= 0) && (osPoint.Y >= 0) && (osPoint.Z >= 0) && (osPoint.X <= 1) && (osPoint.Y <= 1) && (osPoint.Z <= 1); diff --git a/Source/Framework/RecastDetour/Detour/DetourCommon.cs b/Source/Framework/RecastDetour/Detour/DetourCommon.cs index 8bcdafdac..5041d6148 100644 --- a/Source/Framework/RecastDetour/Detour/DetourCommon.cs +++ b/Source/Framework/RecastDetour/Detour/DetourCommon.cs @@ -1,4 +1,5 @@ using System; +using System.Numerics; public static partial class Detour { @@ -180,6 +181,22 @@ public static partial class Detour return dx * dx + dz * dz; } + public static float dtDistancePtSegSqr2D(float[] pt, int ptStart, Vector3 p, Vector3 q, ref float t) + { + float pqx = q.GetAt(0) - p.GetAt(0); + float pqz = q.GetAt(2) - p.GetAt(2); + float dx = pt[ptStart + 0] - p.GetAt(0); + float dz = pt[ptStart + 2] - p.GetAt(2); + float d = pqx * pqx + pqz * pqz; + t = pqx * dx + pqz * dz; + if (d > 0) t /= d; + if (t < 0) t = 0; + else if (t > 1) t = 1; + dx = p.GetAt(0) + t * pqx - pt[ptStart + 0]; + dz = p.GetAt(2) + t * pqz - pt[ptStart + 2]; + return dx * dx + dz * dz; + } + /// Derives the centroid of a convex polygon. /// @param[out] tc The centroid of the polgyon. [(x, y, z)] /// @param[in] idx The polygon indices. [(vertIndex) * @p nidx] diff --git a/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs b/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs index 75025567a..2cb80e33c 100644 --- a/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs +++ b/Source/Framework/RecastDetour/Detour/DetourNavMesh.cs @@ -1,6 +1,8 @@ using System; using System.Diagnostics; using System.Linq; +using System.Numerics; + /** @typedef dtPolyRef @par @@ -735,31 +737,30 @@ public static partial class Detour float tmin = 0; int pmin = 0; int pmax = 0; - float[] v = new float[0]; + Vector3[] v = new Vector3[3]; for (int i = 0; i < pd.triCount; i++) { - int trisIndex = (int)((pd.triBase + i) * 4); + Span tris = tile.detailTris.AsSpan().Slice((int)(pd.triBase + i) * 4); const int ANY_BOUNDARY_EDGE = ((int)dtDetailTriEdgeFlags.DT_DETAIL_EDGE_BOUNDARY << 0) | ((int)dtDetailTriEdgeFlags.DT_DETAIL_EDGE_BOUNDARY << 2) | ((int)dtDetailTriEdgeFlags.DT_DETAIL_EDGE_BOUNDARY << 4); - if (onlyBoundary && (tile.detailTris[trisIndex + 3] & ANY_BOUNDARY_EDGE) == 0) + if (onlyBoundary && (tris[3] & ANY_BOUNDARY_EDGE) == 0) continue; - v = new float[3]; for (int j = 0; j < 3; ++j) { - if (tile.detailTris[trisIndex + j] < poly.vertCount) - v[j] = tile.verts[poly.verts[tile.detailTris[trisIndex + j]] * 3]; + if (tris[j] < poly.vertCount) + v[j] = new(tile.verts.AsSpan(poly.verts[tris[j]] * 3)); else - v[j] = tile.detailVerts[(pd.vertBase + (tile.detailTris[trisIndex + j] - poly.vertCount)) * 3]; + v[j] = new(tile.detailVerts.AsSpan((int)(pd.vertBase + (tris[j] - poly.vertCount)) * 3)); } for (int k = 0, j = 2; k < 3; j = k++) { - if ((dtGetDetailTriEdgeFlags(tile.detailTris[trisIndex + 3], j) & (int)dtDetailTriEdgeFlags.DT_DETAIL_EDGE_BOUNDARY) == 0 && - (onlyBoundary || tile.detailTris[trisIndex + j] < tile.detailTris[trisIndex + k])) + if ((dtGetDetailTriEdgeFlags(tris[3], j) & (int)dtDetailTriEdgeFlags.DT_DETAIL_EDGE_BOUNDARY) == 0 && + (onlyBoundary || tris[j] < tris[k])) { // Only looking at boundary edges and this is internal, or // this is an inner edge that we will see again or have already seen. @@ -767,7 +768,7 @@ public static partial class Detour } float t = 0; - float d = dtDistancePtSegSqr2D(pos, 0, v, j, v, k, ref t); + float d = dtDistancePtSegSqr2D(pos, 0, v[j], v[k], ref t); if (d < dmin) { dmin = d; @@ -778,7 +779,7 @@ public static partial class Detour } } - dtVlerp(closest, 0, v, pmin, v, pmax, tmin); + dtVlerp(closest, 0, new float[] { v[pmin].X, v[pmin].Y, v[pmin].Z }, 0, new float[] { v[pmax].X, v[pmax].Y, v[pmax].Z }, 0, tmin); } public bool getPolyHeight(dtMeshTile tile, dtPoly poly, float[] pos, float height) diff --git a/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs b/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs index 919a9cc82..143b8ddf5 100644 --- a/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs +++ b/Source/Framework/RecastDetour/Detour/DetourNavMeshQuery.cs @@ -1911,7 +1911,7 @@ public static partial class Detour for (int i = 0; i < pathSize; ++i) { - float[] left = new float[3];//, right[3]; + float[] left = new float[3]; float[] right = new float[3]; byte fromType = 0, toType = 0; diff --git a/Source/Framework/Util/Extensions.cs b/Source/Framework/Util/Extensions.cs index 6be005f92..45b9332a6 100644 --- a/Source/Framework/Util/Extensions.cs +++ b/Source/Framework/Util/Extensions.cs @@ -264,7 +264,7 @@ namespace System // cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz // -sy cy*sx cx*cy - var matrix = Matrix4x4.CreateFromQuaternion(quaternion); + var matrix = quaternion.ToMatrix(); if (matrix.M31 < 1.0) { if (matrix.M31 > -1.0) @@ -292,7 +292,19 @@ namespace System public static Matrix4x4 fromEulerAnglesZYX(float fYAngle, float fPAngle, float fRAngle) { - return Matrix4x4.CreateFromYawPitchRoll(fYAngle, fPAngle, fRAngle); + float fCos = MathF.Cos(fYAngle); + float fSin = MathF.Sin(fYAngle); + Matrix4x4 kZMat = new(fCos, -fSin, 0.0f, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); + + fCos = MathF.Cos(fPAngle); + fSin = MathF.Sin(fPAngle); + Matrix4x4 kYMat = new(fCos, 0.0f, fSin, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, -fSin, 0.0f, fCos, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); + + fCos = MathF.Cos(fRAngle); + fSin = MathF.Sin(fRAngle); + Matrix4x4 kXMat = new(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); + + return kZMat * (kYMat * kXMat); } #region Strings diff --git a/Source/Framework/Util/MathFunctions.cs b/Source/Framework/Util/MathFunctions.cs index bd3191da6..bc6ac7302 100644 --- a/Source/Framework/Util/MathFunctions.cs +++ b/Source/Framework/Util/MathFunctions.cs @@ -292,13 +292,101 @@ public static class MathFunctions if (!box.isFinite()) return box; - box._center = new(rotation.M11 * box._center.GetAt(0) + rotation.M12 * box._center.GetAt(1) + rotation.M13 * box._center.GetAt(2) + translation.GetAt(0), + Box outBox = box; + + outBox._center = new(rotation.M11 * box._center.GetAt(0) + rotation.M12 * box._center.GetAt(1) + rotation.M13 * box._center.GetAt(2) + translation.GetAt(0), rotation.M21 * box._center.GetAt(0) + rotation.M22 * box._center.GetAt(1) + rotation.M23 * box._center.GetAt(2) + translation.GetAt(1), rotation.M31 * box._center.GetAt(0) + rotation.M32 * box._center.GetAt(1) + rotation.M33 * box._center.GetAt(2) + translation.GetAt(2)); for (int i = 0; i < 3; ++i) - box._edgeVector[i] = Vector3.TransformNormal(box._edgeVector[i], rotation); + outBox._edgeVector[i] = rotation.Multiply(box._edgeVector[i]); + + outBox._area = box._area; + outBox._volume = box._volume; return box; } + public static Matrix4x4 Inverse(this Matrix4x4 elt) + { + Matrix4x4 kInverse; + elt.Inverse(out kInverse); + return kInverse; + } + public static bool Inverse(this Matrix4x4 elt, out Matrix4x4 rkInverse) + { + // Invert a 3x3 using cofactors. This is about 8 times faster than + // the Numerical Recipes code which uses Gaussian elimination. + rkInverse = new(); + rkInverse.M11 = elt.M22 * elt.M33 - + elt.M23 * elt.M32; + rkInverse.M12 = elt.M13 * elt.M32 - + elt.M12 * elt.M33; + rkInverse.M13 = elt.M12 * elt.M23 - + elt.M13 * elt.M22; + rkInverse.M21 = elt.M23 * elt.M31 - + elt.M21 * elt.M33; + rkInverse.M22 = elt.M11 * elt.M33 - + elt.M13 * elt.M31; + rkInverse.M23 = elt.M13 * elt.M21 - + elt.M11 * elt.M23; + rkInverse.M31 = elt.M21 * elt.M32 - + elt.M22 * elt.M31; + rkInverse.M32 = elt.M12 * elt.M31 - + elt.M11 * elt.M32; + rkInverse.M33 = elt.M11 * elt.M22 - + elt.M12 * elt.M21; + + float fDet = + elt.M11 * rkInverse.M11 + + elt.M12 * rkInverse.M21 + + elt.M13 * rkInverse.M31; + + if (Math.Abs(fDet) <= float.Epsilon) + return false; + + float fInvDet = 1.0f / fDet; + + rkInverse.M11 *= fInvDet; + rkInverse.M12 *= fInvDet; + rkInverse.M13 *= fInvDet; + rkInverse.M21 *= fInvDet; + rkInverse.M22 *= fInvDet; + rkInverse.M23 *= fInvDet; + rkInverse.M31 *= fInvDet; + rkInverse.M32 *= fInvDet; + rkInverse.M33 *= fInvDet; + + return true; + } + + public static Matrix4x4 ToMatrix(this Quaternion _q) + { + // Implementation from Watt and Watt, pg 362 + // See also http://www.flipcode.com/documents/matrfaq.html#Q54 + Quaternion q = _q; + q *= 1.0f / MathF.Sqrt((q.X * q.X) + (q.Y * q.Y) + (q.Z * q.Z) + (q.W * q.W)); + + float xx = 2.0f * q.X * q.X; + float xy = 2.0f * q.X * q.Y; + float xz = 2.0f * q.X * q.Z; + float xw = 2.0f * q.X * q.W; + + float yy = 2.0f * q.Y * q.Y; + float yz = 2.0f * q.Y * q.Z; + float yw = 2.0f * q.Y * q.W; + + float zz = 2.0f * q.Z * q.Z; + float zw = 2.0f * q.Z * q.W; + + return new Matrix4x4(1.0f - yy - zz, xy - zw, xz + yw, 0.0f, + xy + zw, 1.0f - xx - zz, yz - xw, 0.0f, + xz - yw, yz + xw, 1.0f - xx - yy, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); + } + + public static Vector3 Multiply(this Matrix4x4 elt, Vector3 v) + { + return new(elt.M11 * v.GetAt(0) + elt.M12 * v.GetAt(1) + elt.M13 * v.GetAt(2), + elt.M21 * v.GetAt(0) + elt.M22 * v.GetAt(1) + elt.M23 * v.GetAt(2), + elt.M31 * v.GetAt(0) + elt.M32 * v.GetAt(1) + elt.M33 * v.GetAt(2)); + } } diff --git a/Source/Game/Collision/Models/GameObjectModel.cs b/Source/Game/Collision/Models/GameObjectModel.cs index c916db03f..1aafb0be4 100644 --- a/Source/Game/Collision/Models/GameObjectModel.cs +++ b/Source/Game/Collision/Models/GameObjectModel.cs @@ -65,13 +65,13 @@ namespace Game.Collision iScale = modelOwner.GetScale(); iInvScale = 1.0f / iScale; - Matrix4x4 iRotation = Matrix4x4.CreateFromQuaternion(modelOwner.GetRotation()); - Matrix4x4.Invert(iRotation, out iInvRot); + Matrix4x4 iRotation = modelOwner.GetRotation().ToMatrix(); + iRotation.Inverse(out iInvRot); // transform bounding box: mdl_box = new AxisAlignedBox(mdl_box.Lo * iScale, mdl_box.Hi * iScale); AxisAlignedBox rotated_bounds = new(); for (int i = 0; i < 8; ++i) - rotated_bounds.merge(Vector3.TransformNormal(mdl_box.corner(i), iRotation)); + rotated_bounds.merge(iRotation.Multiply(mdl_box.corner(i))); iBound = rotated_bounds + iPos; owner = modelOwner; @@ -101,8 +101,8 @@ namespace Game.Collision return false; // child bounds are defined in object space: - Vector3 p = Vector3.TransformNormal((ray.Origin - iPos) * iInvScale, iInvRot); - Ray modRay = new Ray(p, Vector3.TransformNormal(ray.Direction, iInvRot)); + Vector3 p = iInvRot.Multiply(ray.Origin - iPos) * iInvScale; + Ray modRay = new Ray(p, iInvRot.Multiply(ray.Direction)); float distance = maxDist * iInvScale; bool hit = iModel.IntersectRay(modRay, ref distance, stopAtFirstHit, ignoreFlags); if (hit) @@ -125,13 +125,13 @@ namespace Game.Collision return; // child bounds are defined in object space: - Vector3 pModel = Vector3.TransformNormal((point - iPos) * iInvScale, iInvRot); - Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot); + Vector3 pModel = iInvRot.Multiply(point - iPos) * iInvScale; + Vector3 zDirModel = iInvRot.Multiply(new Vector3(0.0f, 0.0f, -1.0f)); float zDist; if (iModel.IntersectPoint(pModel, zDirModel, out zDist, info)) { Vector3 modelGround = pModel + zDist * zDirModel; - float world_Z = (Vector3.TransformNormal(modelGround, iInvRot) * iScale + iPos).Z; + float world_Z = (iInvRot.Multiply(modelGround) * iScale + iPos).Z; if (info.ground_Z < world_Z) { info.ground_Z = world_Z; @@ -152,13 +152,13 @@ namespace Game.Collision return false; // child bounds are defined in object space: - Vector3 pModel = Vector3.TransformNormal((point - iPos) * iInvScale, iInvRot); - Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot); + Vector3 pModel = iInvRot.Multiply(point - iPos) * iInvScale; + Vector3 zDirModel = iInvRot.Multiply(new Vector3(0.0f, 0.0f, -1.0f)); float zDist; if (iModel.GetLocationInfo(pModel, zDirModel, out zDist, info)) { Vector3 modelGround = pModel + zDist * zDirModel; - float world_Z = (Vector3.TransformNormal(modelGround, iInvRot) * iScale + iPos).Z; + float world_Z = (iInvRot.Multiply(modelGround) * iScale + iPos).Z; if (info.ground_Z < world_Z) { info.ground_Z = world_Z; @@ -172,7 +172,7 @@ namespace Game.Collision public bool GetLiquidLevel(Vector3 point, LocationInfo info, ref float liqHeight) { // child bounds are defined in object space: - Vector3 pModel = Vector3.TransformNormal((point - iPos) * iInvScale, iInvRot); + Vector3 pModel = iInvRot.Multiply(point - iPos) * iInvScale; //Vector3 zDirModel = iInvRot * Vector3(0.f, 0.f, -1.f); float zDist; if (info.hitModel.GetLiquidLevel(pModel, out zDist)) @@ -204,13 +204,13 @@ namespace Game.Collision iPos = owner.GetPosition(); - Matrix4x4 iRotation = Matrix4x4.CreateFromQuaternion(owner.GetRotation()); - Matrix4x4.Invert(iRotation, out iInvRot); + Matrix4x4 iRotation = owner.GetRotation().ToMatrix(); + iRotation.Inverse(out iInvRot); // transform bounding box: mdl_box = new AxisAlignedBox(mdl_box.Lo * iScale, mdl_box.Hi * iScale); AxisAlignedBox rotated_bounds = new(); for (int i = 0; i < 8; ++i) - rotated_bounds.merge(Vector3.TransformNormal(mdl_box.corner(i), iRotation)); + rotated_bounds.merge(iRotation.Multiply(mdl_box.corner(i))); iBound = rotated_bounds + iPos; diff --git a/Source/Game/Collision/Models/ModelInstance.cs b/Source/Game/Collision/Models/ModelInstance.cs index ca753ffb4..fc89ee473 100644 --- a/Source/Game/Collision/Models/ModelInstance.cs +++ b/Source/Game/Collision/Models/ModelInstance.cs @@ -108,7 +108,7 @@ namespace Game.Collision iModel = model; - Matrix4x4.Invert(Extensions.fromEulerAnglesZYX(MathFunctions.PI * spawn.iRot.Y / 180.0f, MathFunctions.PI * spawn.iRot.X / 180.0f, MathFunctions.PI * spawn.iRot.Z / 180.0f), out iInvRot); + Extensions.fromEulerAnglesZYX(MathFunctions.PI * spawn.iRot.Y / 180.0f, MathFunctions.PI * spawn.iRot.X / 180.0f, MathFunctions.PI * spawn.iRot.Z / 180.0f).Inverse(out iInvRot); iInvScale = 1.0f / iScale; } @@ -123,8 +123,8 @@ namespace Game.Collision return false; // child bounds are defined in object space: - Vector3 p = Vector3.TransformNormal((pRay.Origin - iPos) * iInvScale, iInvRot); - Ray modRay = new Ray(p, Vector3.TransformNormal(pRay.Direction, iInvRot)); + Vector3 p = iInvRot.Multiply(pRay.Origin - iPos) * iInvScale; + Ray modRay = new Ray(p, iInvRot.Multiply(pRay.Direction)); float distance = pMaxDist * iInvScale; bool hit = iModel.IntersectRay(modRay, ref distance, pStopAtFirstHit, ignoreFlags); if (hit) @@ -146,8 +146,8 @@ namespace Game.Collision if (!iBound.contains(p)) return; // child bounds are defined in object space: - Vector3 pModel = Vector3.TransformNormal((p - iPos) * iInvScale, iInvRot); - Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot); + Vector3 pModel = iInvRot.Multiply(p - iPos) * iInvScale; + Vector3 zDirModel = iInvRot.Multiply(new Vector3(0.0f, 0.0f, -1.0f)); float zDist; if (iModel.IntersectPoint(pModel, zDirModel, out zDist, info)) { @@ -155,7 +155,7 @@ namespace Game.Collision // Transform back to world space. Note that: // Mat * vec == vec * Mat.transpose() // and for rotation matrices: Mat.inverse() == Mat.transpose() - float world_Z = ((Vector3.TransformNormal(modelGround, iInvRot)) * iScale + iPos).Z; + float world_Z = (iInvRot.Multiply(modelGround) * iScale + iPos).Z; if (info.ground_Z < world_Z) { info.ground_Z = world_Z; @@ -167,7 +167,7 @@ namespace Game.Collision public bool GetLiquidLevel(Vector3 p, LocationInfo info, ref float liqHeight) { // child bounds are defined in object space: - Vector3 pModel = Vector3.TransformNormal((p - iPos) * iInvScale, iInvRot); + Vector3 pModel = iInvRot.Multiply(p - iPos) * iInvScale; //Vector3 zDirModel = iInvRot * Vector3(0.f, 0.f, -1.f); float zDist; if (info.hitModel.GetLiquidLevel(pModel, out zDist)) @@ -191,8 +191,8 @@ namespace Game.Collision if (!iBound.contains(p)) return false; // child bounds are defined in object space: - Vector3 pModel = Vector3.TransformNormal((p - iPos) * iInvScale, iInvRot); - Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot); + Vector3 pModel = iInvRot.Multiply(p - iPos) * iInvScale; + Vector3 zDirModel = iInvRot.Multiply(new Vector3(0.0f, 0.0f, -1.0f)); float zDist; if (iModel.GetLocationInfo(pModel, zDirModel, out zDist, info)) { @@ -200,7 +200,7 @@ namespace Game.Collision // Transform back to world space. Note that: // Mat * vec == vec * Mat.transpose() // and for rotation matrices: Mat.inverse() == Mat.transpose() - float world_Z = (Vector3.TransformNormal(modelGround, iInvRot) * iScale + iPos).Z; + float world_Z = (iInvRot.Multiply(modelGround * iScale) + iPos).Z; if (info.ground_Z < world_Z) // hm...could it be handled automatically with zDist at intersection? { info.ground_Z = world_Z; diff --git a/Source/Game/Entities/GameObject/GameObject.cs b/Source/Game/Entities/GameObject/GameObject.cs index 2c8fe8d2e..8d1f14758 100644 --- a/Source/Game/Entities/GameObject/GameObject.cs +++ b/Source/Game/Entities/GameObject/GameObject.cs @@ -2446,10 +2446,10 @@ namespace Game.Entities float maxY = displayInfo.GeoBoxMax.Y * scale + radius; float maxZ = displayInfo.GeoBoxMax.Z * scale + radius; - Quaternion worldRotation = GetLocalRotation(); + Quaternion worldRotation = GetWorldRotation(); //Todo Test this. Needs checked. - var worldSpaceBox = MathFunctions.toWorldSpace(Matrix4x4.CreateFromQuaternion(worldRotation), new Vector3(GetPositionX(), GetPositionY(), GetPositionZ()), new Box(new Vector3(minX, minY, minZ), new Vector3(maxX, maxY, maxZ))); + var worldSpaceBox = MathFunctions.toWorldSpace(worldRotation.ToMatrix(), new Vector3(GetPositionX(), GetPositionY(), GetPositionZ()), new Box(new Vector3(minX, minY, minZ), new Vector3(maxX, maxY, maxZ))); return worldSpaceBox.Contains(new Vector3(pos.GetPositionX(), pos.GetPositionY(), pos.GetPositionZ())); } @@ -3743,15 +3743,15 @@ namespace Game.Entities TransportAnimationRecord newAnimation = _animationInfo.GetNextAnimNode(newProgress); if (oldAnimation != null && newAnimation != null) { - Matrix4x4 pathRotation = Matrix4x4.CreateFromQuaternion(new Quaternion(_owner.m_gameObjectData.ParentRotation.GetValue().X, _owner.m_gameObjectData.ParentRotation.GetValue().Y, - _owner.m_gameObjectData.ParentRotation.GetValue().Z, _owner.m_gameObjectData.ParentRotation.GetValue().W)); + Matrix4x4 pathRotation = new Quaternion(_owner.m_gameObjectData.ParentRotation.GetValue().X, _owner.m_gameObjectData.ParentRotation.GetValue().Y, + _owner.m_gameObjectData.ParentRotation.GetValue().Z, _owner.m_gameObjectData.ParentRotation.GetValue().W).ToMatrix(); Vector3 prev = new(oldAnimation.Pos.X, oldAnimation.Pos.Y, oldAnimation.Pos.Z); Vector3 next = new(newAnimation.Pos.X, newAnimation.Pos.Y, newAnimation.Pos.Z); float animProgress = (float)(newProgress - oldAnimation.TimeIndex) / (float)(newAnimation.TimeIndex - oldAnimation.TimeIndex); - Vector3 dst = Vector3.TransformNormal(Vector3.Lerp(prev, next, animProgress), pathRotation);//todo check this + Vector3 dst = pathRotation.Multiply(Vector3.Lerp(prev, next, animProgress)); dst += _owner.GetStationaryPosition();