Fixes using gameobject like chairs, also fixes indoor checks

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);

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]

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<byte> 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)
+                    if (tris[j] < poly.vertCount)
-                        v[j] = tile.verts[poly.verts[tile.detailTris[trisIndex + j]] * 3];
+                        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 &&
+                    if ((dtGetDetailTriEdgeFlags(tris[3], j) & (int)dtDetailTriEdgeFlags.DT_DETAIL_EDGE_BOUNDARY) == 0 &&
-                        (onlyBoundary || tile.detailTris[trisIndex + j] < tile.detailTris[trisIndex + k]))
+                        (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)

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;
 

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

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));
+    }
 }

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 iRotation = modelOwner.GetRotation().ToMatrix();
-            Matrix4x4.Invert(iRotation, out iInvRot);
+            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);
+            Vector3 p = iInvRot.Multiply(ray.Origin - iPos) * iInvScale;
-            Ray modRay = new Ray(p, Vector3.TransformNormal(ray.Direction, iInvRot));
+            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 pModel = iInvRot.Multiply(point - iPos) * iInvScale;
-            Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot);
+            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 pModel = iInvRot.Multiply(point - iPos) * iInvScale;
-            Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot);
+            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 iRotation = owner.GetRotation().ToMatrix();
-            Matrix4x4.Invert(iRotation, out iInvRot);
+            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;
 

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);
+            Vector3 p = iInvRot.Multiply(pRay.Origin - iPos) * iInvScale;
-            Ray modRay = new Ray(p, Vector3.TransformNormal(pRay.Direction, iInvRot));
+            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 pModel = iInvRot.Multiply(p - iPos) * iInvScale;
-            Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot);
+            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 pModel = iInvRot.Multiply(p - iPos) * iInvScale;
-            Vector3 zDirModel = Vector3.TransformNormal(new Vector3(0.0f, 0.0f, -1.0f), iInvRot);
+            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;

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,
+                    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));
+                        _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();