diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableGizmo.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableGizmo.cs index 5ad69df87d..3378ab080c 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableGizmo.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/CollidableGizmo.cs @@ -112,7 +112,7 @@ private void PrepareModels() var bepuShapeCacheSys = _services.GetOrCreate(); var graphicsDevice = _services.GetSafeServiceAs().GraphicsDevice; - if (_component.Collider is MeshCollider meshCollider && (meshCollider.Model.Meshes.Count == 0 || meshCollider.Model == null!/*May be null in editor*/)) + if (_component.Collider is MeshCollider meshCollider && (meshCollider.Model == null!/*May be null in editor*/ || meshCollider.Model.Meshes.Count == 0)) { // It looks like meshes take some time before being filled in by the editor ... ? // Schedule it for later diff --git a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/ShapeCacheSystem.cs b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/ShapeCacheSystem.cs index 64832cef95..3ab8bec98e 100644 --- a/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/ShapeCacheSystem.cs +++ b/sources/engine/Stride.BepuPhysics/Stride.BepuPhysics/Systems/ShapeCacheSystem.cs @@ -139,6 +139,8 @@ private static void ExtractHull(DecomposedHulls hullDesc, out VertexPosition3[] internal static unsafe BepuUtilities.Memory.Buffer ExtractBepuMesh(Model model, IServiceRegistry services, BufferPool pool) { + var nodeTransforms = ExtractNodeTransforms(model); + int totalIndices = 0; foreach (var meshData in model.Meshes) { @@ -148,13 +150,24 @@ internal static unsafe BepuUtilities.Memory.Buffer ExtractBepuMesh(Mod pool.Take(totalIndices / 3, out var triangles); var bepuTriangles = triangles.As(); var spanLeft = new Span(bepuTriangles.Memory, bepuTriangles.Length); - foreach (var mesh in model.Meshes) + foreach (var meshData in model.Meshes) { - mesh.Draw.IndexBuffer.AsReadable(services, out var indexHelper, out int indexCount); - mesh.Draw.VertexBuffers[0].AsReadable(services, out var vertexHelper, out int vertexCount); + meshData.Draw.IndexBuffer.AsReadable(services, out var indexHelper, out int indexCount); + meshData.Draw.VertexBuffers[0].AsReadable(services, out var vertexHelper, out int vertexCount); var copyJob = new VertexBufferHelper.CopyAsTriangleList { IndexBufferHelper = indexHelper }; - vertexHelper.Read(spanLeft[..indexCount], copyJob); + var vertSlice = spanLeft[..indexCount]; + vertexHelper.Read(vertSlice, copyJob); + + if (nodeTransforms != null) + { + for (int i = 0; i < vertSlice.Length; i++) + { + Matrix posMatrix = Matrix.Translation(vertSlice[i]); + Matrix.Multiply(ref posMatrix, ref nodeTransforms[meshData.NodeIndex], out var finalMatrix); + vertSlice[i] = finalMatrix.TranslationVector; + } + } spanLeft = spanLeft[indexCount..]; } @@ -164,30 +177,87 @@ internal static unsafe BepuUtilities.Memory.Buffer ExtractBepuMesh(Mod private static void ExtractMeshBuffers(Model model, IServiceRegistry services, out VertexPosition3[] vertices, out int[] indices) { - int totalVertices = 0, totalIndices = 0; + var nodeTransforms = ExtractNodeTransforms(model); + + int totalVerts = 0, totalIndices = 0; foreach (var meshData in model.Meshes) { - totalVertices += meshData.Draw.VertexBuffers[0].Count; + totalVerts += meshData.Draw.VertexBuffers[0].Count; totalIndices += meshData.Draw.IndexBuffer.Count; } - vertices = new VertexPosition3[totalVertices]; - indices = new int[totalIndices]; + var combinedVerts = new VertexPosition3[totalVerts]; + var combinedIndices = new int[totalIndices]; + var verticesLeft = MemoryMarshal.Cast(combinedVerts.AsSpan()); + var indicesLeft = combinedIndices.AsSpan(); - var verticesLeft = MemoryMarshal.Cast(vertices.AsSpan()); - var indicesLeft = indices.AsSpan(); - - foreach (var mesh in model.Meshes) + int indexOffset = 0; + foreach (var meshData in model.Meshes) { - mesh.Draw.IndexBuffer.AsReadable(services, out var indexHelper, out int indexCount); - mesh.Draw.VertexBuffers[0].AsReadable(services, out var vertexHelper, out int vertexCount); + meshData.Draw.VertexBuffers[0].AsReadable(services, out var vertexHelper, out var vertexCount); + meshData.Draw.IndexBuffer.AsReadable(services, out var indexHelper, out var indexCount); + + var vertSlice = verticesLeft[..vertexCount]; + vertexHelper.Copy(vertSlice); + + if (nodeTransforms != null) + { + for (int i = 0; i < vertSlice.Length; i++) + { + Matrix posMatrix = Matrix.Translation(vertSlice[i]); + Matrix.Multiply(ref posMatrix, ref nodeTransforms[meshData.NodeIndex], out var finalMatrix); + vertSlice[i] = finalMatrix.TranslationVector; + } + } - vertexHelper.Copy(verticesLeft[..vertexCount]); - indexHelper.CopyTo(indicesLeft[..indexCount]); + var indicesForSlice = indicesLeft[..indexCount]; + indexHelper.CopyTo(indicesForSlice); + for (int i = 0; i < indicesForSlice.Length; i++) + indicesForSlice[i] += indexOffset; + indexOffset += vertexCount; verticesLeft = verticesLeft[vertexCount..]; indicesLeft = indicesLeft[indexCount..]; } + + vertices = combinedVerts; + indices = combinedIndices; + } + + private static Matrix[]? ExtractNodeTransforms(Model model) + { + Matrix[]? nodeTransforms = null; + if (model.Skeleton == null) + return nodeTransforms; + + var nodesLength = model.Skeleton.Nodes.Length; + nodeTransforms = new Matrix[nodesLength]; + nodeTransforms[0] = Matrix.Identity; + for (var i = 0; i < nodesLength; i++) + { + var node = model.Skeleton.Nodes[i]; + Matrix.Transformation(ref node.Transform.Scale, ref node.Transform.Rotation, ref node.Transform.Position, out var localMatrix); + + Matrix worldMatrix; + if (node.ParentIndex != -1) + { + if (node.ParentIndex >= i) + throw new InvalidOperationException("Skeleton nodes are not sorted"); + var nodeTransform = nodeTransforms[node.ParentIndex]; + Matrix.Multiply(ref localMatrix, ref nodeTransform, out worldMatrix); + } + else + { + worldMatrix = localMatrix; + } + + if (i != 0) + { + nodeTransforms[i] = worldMatrix; + } + } + + return nodeTransforms; } /// diff --git a/sources/engine/Stride.Physics/Shapes/StaticMeshColliderShape.cs b/sources/engine/Stride.Physics/Shapes/StaticMeshColliderShape.cs index de8da31a45..a700193d1d 100644 --- a/sources/engine/Stride.Physics/Shapes/StaticMeshColliderShape.cs +++ b/sources/engine/Stride.Physics/Shapes/StaticMeshColliderShape.cs @@ -158,28 +158,34 @@ static SharedMeshData BuildAndShareMeshes(Model model, IServiceRegistry services var combinedIndices = new int[totalIndices]; var verticesLeft = combinedVerts.AsSpan(); var indicesLeft = combinedIndices.AsSpan(); - + + int indexOffset = 0; foreach (var meshData in model.Meshes) { meshData.Draw.VertexBuffers[0].AsReadable(services, out var vertexHelper, out var vertexCount); meshData.Draw.IndexBuffer.AsReadable(services, out var indexHelper, out var indexCount); - var sliceForTheseVertices = verticesLeft[..vertexCount]; - vertexHelper.Copy(sliceForTheseVertices); - indexHelper.CopyTo(indicesLeft[..indexCount]); - - verticesLeft = verticesLeft[vertexCount..]; - indicesLeft = indicesLeft[indexCount..]; + var vertSlice = verticesLeft[..vertexCount]; + vertexHelper.Copy(vertSlice); if (nodeTransforms != null) { - for (int i = 0; i < sliceForTheseVertices.Length; i++) + for (int i = 0; i < vertSlice.Length; i++) { - Matrix posMatrix = Matrix.Translation(sliceForTheseVertices[i]); + Matrix posMatrix = Matrix.Translation(vertSlice[i]); Matrix.Multiply(ref posMatrix, ref nodeTransforms[meshData.NodeIndex], out var finalMatrix); - sliceForTheseVertices[i] = finalMatrix.TranslationVector; + vertSlice[i] = finalMatrix.TranslationVector; } } + + var indicesForSlice = indicesLeft[..indexCount]; + indexHelper.CopyTo(indicesForSlice); + for (int i = 0; i < indicesForSlice.Length; i++) + indicesForSlice[i] += indexOffset; + indexOffset += vertexCount; + + verticesLeft = verticesLeft[vertexCount..]; + indicesLeft = indicesLeft[indexCount..]; } if (string.IsNullOrWhiteSpace(modelUrl))