From aeeda3814df41e79f753f32bdbfb0c77e7bade08 Mon Sep 17 00:00:00 2001 From: AndresTraks Date: Sat, 21 Oct 2017 17:32:38 +0300 Subject: [PATCH] Fix capsule shape drawing --- .../DemoFramework/Graphics/MeshFactory.cs | 319 +++++++++--------- 1 file changed, 160 insertions(+), 159 deletions(-) diff --git a/demos/Generic/DemoFramework/Graphics/MeshFactory.cs b/demos/Generic/DemoFramework/Graphics/MeshFactory.cs index 1c7fadd..a40c5bb 100644 --- a/demos/Generic/DemoFramework/Graphics/MeshFactory.cs +++ b/demos/Generic/DemoFramework/Graphics/MeshFactory.cs @@ -1,8 +1,8 @@ using System; using System.Collections.Generic; +using System.IO; using System.Runtime.InteropServices; using BulletSharp; -using BulletSharp.SoftBody; namespace DemoFramework { @@ -82,11 +82,18 @@ public static Vector3[] CreateShape(CollisionShape shape, out uint[] indices) return CreateSphere(shape as SphereShape, out indices); case BroadphaseNativeType.StaticPlaneShape: return CreateStaticPlane(shape as StaticPlaneShape, out indices); + case BroadphaseNativeType.TerrainShape: + return CreateHeightFieldTerrainShape(shape as HeightfieldTerrainShape, out indices); case BroadphaseNativeType.TriangleMeshShape: indices = null; return CreateTriangleMesh((shape as TriangleMeshShape).MeshInterface); - default: - throw new NotImplementedException(); + case BroadphaseNativeType.UniformScalingShape: + indices = null; + return CreateUniformScalingShape(shape as UniformScalingShape, out indices); + case BroadphaseNativeType.ScaledTriangleMeshShape: + indices = null; + return CreateScaledBvhTriangleMeshShape(shape as ScaledBvhTriangleMeshShape, out indices); + } if (shape is PolyhedralConvexShape) { @@ -95,13 +102,38 @@ public static Vector3[] CreateShape(CollisionShape shape, out uint[] indices) throw new NotImplementedException(); } + public static Vector3[] CreateScaledBvhTriangleMeshShape(ScaledBvhTriangleMeshShape shape, out uint[] indices) + { + Vector3[] vert = CreateShape(shape.ChildShape, out indices); + var scale = shape.LocalScaling; + + return ScaleShape(vert, ref scale); + } + + public static Vector3[] CreateUniformScalingShape(UniformScalingShape shape, out uint[] indices) + { + Vector3[] vert = CreateShape(shape.ChildShape, out indices); + var scale = new Vector3(shape.UniformScalingFactor); + + return ScaleShape(vert, ref scale); + } + + public static Vector3[] ScaleShape(Vector3[] shape, ref Vector3 scaling) + { + for (int i = 0; i < shape.Length; i++) + { + shape[i] *= scaling; + } + return shape; + } + public static ushort[] CompactIndexBuffer(uint[] indices) { if (indices.Length > 65535) { - return null; + throw new ArgumentOutOfRangeException(nameof(indices)); } - ushort[] ib = new ushort[indices.Length]; + var ib = new ushort[indices.Length]; for (int i = 0; i < ib.Length; i++) { ib[i] = (ushort)indices[i]; @@ -120,18 +152,18 @@ public static Vector3[] CreateBox(BoxShape shape) { for (int i = 1; i != -3; i -= 2) { - normal = GetVectorByAxis(0, i, 0, j); - vertices[v++] = GetVectorByAxis(i, i, i, j) * size; + normal = RotateYAxisUp(0, i, 0, j); + vertices[v++] = RotateYAxisUp(i, i, i, j) * size; vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(1, i, -1, j) * size; + vertices[v++] = RotateYAxisUp(1, i, -1, j) * size; vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(-1, i, 1, j) * size; + vertices[v++] = RotateYAxisUp(-1, i, 1, j) * size; vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(-i, i, -i, j) * size; + vertices[v++] = RotateYAxisUp(-i, i, -i, j) * size; vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(-1, i, 1, j) * size; + vertices[v++] = RotateYAxisUp(-1, i, 1, j) * size; vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(1, i, -1, j) * size; + vertices[v++] = RotateYAxisUp(1, i, -1, j) * size; vertices[v++] = normal; } } @@ -159,75 +191,78 @@ public static Vector3[] CreateCapsule(CapsuleShape shape, out uint[] indices) float radius = shape.Radius; float cylinderHalfHeight = shape.HalfHeight; - int slices = (int)(radius * 10.0f); - int stacks = (int)(radius * 10.0f); - slices = (slices > 16) ? 16 : (slices < 3) ? 3 : slices; - stacks = (stacks > 16) ? 16 : (stacks < 3) ? 3 : stacks; - - float hAngleStep = (float)Math.PI * 2 / slices; - float vAngleStep = (float)Math.PI / stacks; + int slices = (int)(radius * 5.0f); + int stacks = (int)(radius * 5.0f); + slices = 2 * MinMax(slices, 3, 16); + stacks = 2 * MinMax(stacks, 2, 16) + 1; int vertexCount = 2 + slices * (stacks - 1); int indexCount = 6 * slices * (stacks - 1); - Vector3[] vertices = new Vector3[vertexCount * 2]; + var vertices = new Vector3[vertexCount * 2]; indices = new uint[indexCount]; - int i = 0, v = 0; - // Vertices // Top and bottom - vertices[v++] = GetVectorByAxis(0, -cylinderHalfHeight - radius, 0, up); - vertices[v++] = GetVectorByAxis(-Vector3.UnitY, up); - vertices[v++] = GetVectorByAxis(0, cylinderHalfHeight + radius, 0, up); - vertices[v++] = GetVectorByAxis(Vector3.UnitY, up); + const int topVertexIndex = 0; + const int bottomVertexIndex = 1; + float apex = cylinderHalfHeight + radius; + + vertices[0] = RotateYAxisUp(0, -apex, 0, up); + vertices[1] = RotateYAxisUp(-Vector3.UnitY, up); + vertices[2] = RotateYAxisUp(0, apex, 0, up); + vertices[3] = RotateYAxisUp(Vector3.UnitY, up); // Stacks - int j, k; - float angle = 0; + int v = 4; + float hAngle = 0; float vAngle = -(float)Math.PI / 2; - Vector3 vTemp; - Vector3 cylinderOffset = GetVectorByAxis(0, -cylinderHalfHeight, 0, up); - for (j = 0; j < stacks - 1; j++) + float hAngleStep = (float)Math.PI * 2 / slices; + float vAngleStep = (float)Math.PI / stacks; + Vector3 cylinderOffset = RotateYAxisUp(0, -cylinderHalfHeight, 0, up); + for (int j = 0; j < stacks - 1; j++) { float prevAngle = vAngle; vAngle += vAngleStep; - if (vAngle > 0 && prevAngle < 0) + if (vAngle > 0 && prevAngle <= 0) { - cylinderOffset = GetVectorByAxis(0, cylinderHalfHeight, 0, up); + cylinderOffset = -cylinderOffset; } - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { - angle += hAngleStep; + hAngle += hAngleStep; - vTemp = GetVectorByAxis((float)Math.Cos(vAngle) * (float)Math.Sin(angle), + Vector3 sphereVertex = RotateYAxisUp( + (float)Math.Cos(vAngle) * (float)Math.Sin(hAngle), (float)Math.Sin(vAngle), - (float)Math.Cos(vAngle) * (float)Math.Cos(angle), up); - vertices[v++] = vTemp * radius + cylinderOffset; - vertices[v++] = Vector3.Normalize(vTemp); + (float)Math.Cos(vAngle) * (float)Math.Cos(hAngle), + up); + vertices[v++] = sphereVertex * radius + cylinderOffset; + vertices[v++] = Vector3.Normalize(sphereVertex); } } // Indices // Top cap + int i = 0; uint index = 2; - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i++] = index++; - indices[i++] = 0; + indices[i++] = topVertexIndex; indices[i++] = index; } indices[i - 1] = 2; // Stacks int sliceDiff = slices * 3; - for (j = 0; j < stacks - 2; j++) + for (int j = 0; j < stacks - 2; j++) { - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 2]; indices[i + 1] = index++; @@ -235,7 +270,7 @@ public static Vector3[] CreateCapsule(CapsuleShape shape, out uint[] indices) i += 3; } - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 1]; indices[i + 1] = indices[i - sliceDiff]; @@ -247,10 +282,10 @@ public static Vector3[] CreateCapsule(CapsuleShape shape, out uint[] indices) // Bottom cap index--; - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i++] = index--; - indices[i++] = 1; + indices[i++] = bottomVertexIndex; indices[i++] = index; } indices[i - 1] = indices[i - sliceDiff]; @@ -258,9 +293,9 @@ public static Vector3[] CreateCapsule(CapsuleShape shape, out uint[] indices) return vertices; } - public static Vector3 GetVectorByAxis(Vector3 vector, int axis) + private static Vector3 RotateYAxisUp(Vector3 vector, int upAxis) { - switch (axis) + switch (upAxis) { case 0: return new Vector3(vector.Y, vector.Z, vector.X); @@ -271,9 +306,9 @@ public static Vector3 GetVectorByAxis(Vector3 vector, int axis) } } - public static Vector3 GetVectorByAxis(float x, float y, float z, int axis) + private static Vector3 RotateYAxisUp(float x, float y, float z, int upAxis) { - switch (axis) + switch (upAxis) { case 0: return new Vector3(y, z, x); @@ -296,7 +331,7 @@ public static Vector3[] CreateCone(ConeShape shape, out uint[] indices) const int vertexCount = 2 + 6 * numSteps; const int indexCount = (4 * numSteps + 2) * 3; - Vector3[] vertices = new Vector3[vertexCount * 2]; + var vertices = new Vector3[vertexCount * 2]; indices = new uint[indexCount]; int i = 0, v = 0; @@ -304,14 +339,14 @@ public static Vector3[] CreateCone(ConeShape shape, out uint[] indices) uint baseIndex; Vector3 normal; - // Draw the base - normal = GetVectorByAxis(-Vector3.UnitY, up); + // Base + normal = RotateYAxisUp(-Vector3.UnitY, up); baseIndex = index; - vertices[v++] = GetVectorByAxis(0, -halfHeight, 0, up); + vertices[v++] = RotateYAxisUp(0, -halfHeight, 0, up); vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(0, -halfHeight, radius, up); + vertices[v++] = RotateYAxisUp(0, -halfHeight, radius, up); vertices[v++] = normal; index += 2; @@ -320,7 +355,7 @@ public static Vector3[] CreateCone(ConeShape shape, out uint[] indices) float x = radius * (float)Math.Sin(j * angleStep); float z = radius * (float)Math.Cos(j * angleStep); - vertices[v++] = GetVectorByAxis(x, -halfHeight, z, up); + vertices[v++] = RotateYAxisUp(x, -halfHeight, z, up); vertices[v++] = normal; indices[i++] = baseIndex; @@ -333,14 +368,15 @@ public static Vector3[] CreateCone(ConeShape shape, out uint[] indices) indices[i++] = index - 1; - normal = GetVectorByAxis(0, 0, radius, up); + // Side + normal = RotateYAxisUp(0, 0, radius, up); normal.Normalize(); baseIndex = index; - vertices[v++] = GetVectorByAxis(0, halfHeight, 0, up); + vertices[v++] = RotateYAxisUp(0, halfHeight, 0, up); vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(0, -halfHeight, radius, up); + vertices[v++] = RotateYAxisUp(0, -halfHeight, radius, up); vertices[v++] = normal; index += 2; @@ -349,13 +385,13 @@ public static Vector3[] CreateCone(ConeShape shape, out uint[] indices) float x = radius * (float)Math.Sin(j * angleStep); float z = radius * (float)Math.Cos(j * angleStep); - normal = GetVectorByAxis(x, 0, z, up); + normal = RotateYAxisUp(x, 0, z, up); normal.Normalize(); - vertices[v++] = GetVectorByAxis(0, halfHeight, 0, up); + vertices[v++] = RotateYAxisUp(0, halfHeight, 0, up); vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(x, -halfHeight, z, up); + vertices[v++] = RotateYAxisUp(x, -halfHeight, z, up); vertices[v++] = normal; indices[i++] = index - 2; @@ -388,7 +424,7 @@ public static Vector3[] CreateCylinder(CylinderShape shape, out uint[] indices) const int vertexCount = 2 + 6 * numSteps; const int indexCount = (4 * numSteps + 2) * 3; - Vector3[] vertices = new Vector3[vertexCount * 2]; + var vertices = new Vector3[vertexCount * 2]; indices = new uint[indexCount]; int i = 0, v = 0; @@ -399,13 +435,13 @@ public static Vector3[] CreateCylinder(CylinderShape shape, out uint[] indices) // Draw two sides for (int side = 1; side != -3; side -= 2) { - normal = GetVectorByAxis(side * Vector3.UnitY, up); + normal = RotateYAxisUp(side * Vector3.UnitY, up); baseIndex = index; - vertices[v++] = GetVectorByAxis(0, side * halfHeight, 0, up); + vertices[v++] = RotateYAxisUp(0, side * halfHeight, 0, up); vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(0, side * halfHeight, radius, up); + vertices[v++] = RotateYAxisUp(0, side * halfHeight, radius, up); vertices[v++] = normal; index += 2; @@ -414,7 +450,7 @@ public static Vector3[] CreateCylinder(CylinderShape shape, out uint[] indices) float x = radius * (float)Math.Sin(j * angleStep); float z = radius * (float)Math.Cos(j * angleStep); - vertices[v++] = GetVectorByAxis(x, side * halfHeight, z, up); + vertices[v++] = RotateYAxisUp(x, side * halfHeight, z, up); vertices[v++] = normal; indices[i++] = baseIndex; @@ -444,14 +480,14 @@ public static Vector3[] CreateCylinder(CylinderShape shape, out uint[] indices) } - normal = GetVectorByAxis(0, 0, radius, up); + normal = RotateYAxisUp(0, 0, radius, up); normal.Normalize(); baseIndex = index; - vertices[v++] = GetVectorByAxis(0, halfHeight, radius, up); + vertices[v++] = RotateYAxisUp(0, halfHeight, radius, up); vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(0, -halfHeight, radius, up); + vertices[v++] = RotateYAxisUp(0, -halfHeight, radius, up); vertices[v++] = normal; index += 2; @@ -460,13 +496,13 @@ public static Vector3[] CreateCylinder(CylinderShape shape, out uint[] indices) float x = radius * (float)Math.Sin(j * angleStep); float z = radius * (float)Math.Cos(j * angleStep); - normal = GetVectorByAxis(x, 0, z, up); + normal = RotateYAxisUp(x, 0, z, up); normal.Normalize(); - vertices[v++] = GetVectorByAxis(x, halfHeight, z, up); + vertices[v++] = RotateYAxisUp(x, halfHeight, z, up); vertices[v++] = normal; - vertices[v++] = GetVectorByAxis(x, -halfHeight, z, up); + vertices[v++] = RotateYAxisUp(x, -halfHeight, z, up); vertices[v++] = normal; indices[i++] = index - 2; @@ -489,17 +525,17 @@ public static Vector3[] CreateCylinder(CylinderShape shape, out uint[] indices) public static Vector3[] CreateConvexHull(ConvexHullShape shape) { - ShapeHull hull = new ShapeHull(shape); + var hull = new ShapeHull(shape); hull.BuildHull(shape.Margin); int vertexCount = hull.NumIndices; UIntArray indices = hull.Indices; Vector3Array points = hull.Vertices; - Vector3[] vertices = new Vector3[vertexCount * 2]; + var vertices = new Vector3[vertexCount * 2]; - int v = 0, i; - for (i = 0; i < vertexCount; i += 3) + int v = 0; + for (int i = 0; i < vertexCount; i += 3) { Vector3 v0 = points[(int)indices[i]]; Vector3 v1 = points[(int)indices[i + 1]]; @@ -523,22 +559,21 @@ public static Vector3[] CreateConvexHull(ConvexHullShape shape) public static Vector3[] CreateMultiSphere(MultiSphereShape shape, out uint[] indices) { - List allVertices = new List(); - List allIndices = new List(); + var allVertices = new List(); + var allIndices = new List(); int vertexCount = 0; int indexCount = 0; - int i; - for (i = 0; i < shape.SphereCount; i++) + for (int i = 0; i < shape.SphereCount; i++) { uint[] sphereIndices; Vector3[] sphereVertices = CreateSphere(shape.GetSphereRadius(i), out sphereIndices); // Adjust sphere position Vector3 position = shape.GetSpherePosition(i); - for (int j = 0; j < sphereVertices.Length / 2; j++) + for (int j = 0; j < sphereVertices.Length; j += 2) { - sphereVertices[j * 2] += position; + sphereVertices[j] += position; } // Adjust indices @@ -614,69 +649,69 @@ public static Vector3[] CreateSphere(SphereShape shape, out uint[] indices) return CreateSphere(shape.Radius, out indices); } - static Vector3[] CreateSphere(float radius, out uint[] indices) + private static Vector3[] CreateSphere(float radius, out uint[] indices) { int slices = (int)(radius * 10.0f); int stacks = (int)(radius * 10.0f); - slices = (slices > 16) ? 16 : (slices < 3) ? 3 : slices; - stacks = (stacks > 16) ? 16 : (stacks < 2) ? 2 : stacks; - - float hAngleStep = (float)Math.PI * 2 / slices; - float vAngleStep = (float)Math.PI / stacks; + slices = MinMax(slices, 6, 16); + stacks = MinMax(stacks, 6, 16); int vertexCount = 2 + slices * (stacks - 1); int indexCount = 6 * slices * (stacks - 1); - Vector3[] vertices = new Vector3[vertexCount * 2]; + var vertices = new Vector3[vertexCount * 2]; indices = new uint[indexCount]; - int i = 0, v = 0; - // Vertices // Top and bottom + int v = 0; + const int topVertexIndex = 0; + const int bottomVertexIndex = 1; + vertices[v++] = new Vector3(0, -radius, 0); vertices[v++] = -Vector3.UnitY; vertices[v++] = new Vector3(0, radius, 0); vertices[v++] = Vector3.UnitY; // Stacks - int j, k; - float angle = 0; float vAngle = -(float)Math.PI / 2; - Vector3 vTemp; - for (j = 0; j < stacks - 1; j++) + float horAngleStep = (float)Math.PI * 2 / slices; + float vertAngleStep = (float)Math.PI / stacks; + for (int j = 0; j < stacks - 1; j++) { - vAngle += vAngleStep; + vAngle += vertAngleStep; - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { - angle += hAngleStep; + float angle = k * horAngleStep; - vTemp = new Vector3((float)Math.Cos(vAngle) * (float)Math.Sin(angle), (float)Math.Sin(vAngle), (float)Math.Cos(vAngle) * (float)Math.Cos(angle)); - vertices[v++] = vTemp * radius; - vertices[v++] = Vector3.Normalize(vTemp); + var vertex = new Vector3( + (float)Math.Cos(vAngle) * (float)Math.Sin(angle), + (float)Math.Sin(vAngle), + (float)Math.Cos(vAngle) * (float)Math.Cos(angle)); + vertices[v++] = vertex * radius; + vertices[v++] = Vector3.Normalize(vertex); } } - // Indices // Top cap + int i = 0; ushort index = 2; - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i++] = index++; - indices[i++] = 0; + indices[i++] = topVertexIndex; indices[i++] = index; } indices[i - 1] = 2; // Stacks - //for (j = 0; j < 1; j++) int sliceDiff = slices * 3; - for (j = 0; j < stacks - 2; j++) + for (int j = 0; j < stacks - 2; j++) { - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 2]; indices[i + 1] = index++; @@ -684,7 +719,7 @@ static Vector3[] CreateSphere(float radius, out uint[] indices) i += 3; } - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i] = indices[i - sliceDiff + 1]; indices[i + 1] = indices[i - sliceDiff]; @@ -696,10 +731,10 @@ static Vector3[] CreateSphere(float radius, out uint[] indices) // Bottom cap index--; - for (k = 0; k < slices; k++) + for (int k = 0; k < slices; k++) { indices[i++] = index--; - indices[i++] = 1; + indices[i++] = bottomVertexIndex; indices[i++] = index; } indices[i - 1] = indices[i - sliceDiff]; @@ -707,7 +742,12 @@ static Vector3[] CreateSphere(float radius, out uint[] indices) return vertices; } - static void PlaneSpace1(Vector3 n, out Vector3 p, out Vector3 q) + private static int MinMax(int value, int min, int max) + { + return Math.Min(Math.Max(value, min), max); + } + + private static void PlaneSpace1(Vector3 n, out Vector3 p, out Vector3 q) { if (Math.Abs(n[2]) > (Math.Sqrt(2) / 2)) { @@ -751,7 +791,7 @@ public static Vector3[] CreateStaticPlane(StaticPlaneShape shape, out uint[] ind }; } - static Vector3[] CreateTriangleMesh(StridingMeshInterface meshInterface) + private static Vector3[] CreateTriangleMesh(StridingMeshInterface meshInterface) { BulletSharp.DataStream vertexBuffer, indexBuffer; int numVerts, numFaces; @@ -792,6 +832,13 @@ static Vector3[] CreateTriangleMesh(StridingMeshInterface meshInterface) return vertices; } + private static Vector3[] CreateHeightFieldTerrainShape(HeightfieldTerrainShape heightfieldTerrainShape, out uint[] indices) + { + // HeightfieldTerrainShape does not expose its data + indices = null; + return new Vector3[1]; + } + /* public void UpdateSoftBody(SoftBody softBody, ShapeData shapeData) { @@ -905,51 +952,5 @@ public void UpdateSoftBody(SoftBody softBody, ShapeData shapeData) } } */ - /* - public void RenderSoftBodyTextured(SoftBody softBody) - { - if (!(softBody.UserObject is Array)) - return; - - object[] userObjArr = softBody.UserObject as object[]; - FloatArray vertexBuffer = userObjArr[0] as FloatArray; - IntArray indexBuffer = userObjArr[1] as IntArray; - - int vertexCount = (vertexBuffer.Count / 8); - - if (vertexCount > 0) - { - int faceCount = indexBuffer.Count / 2; - - bool index32 = vertexCount > 65536; - - Mesh mesh = new Mesh(device, faceCount, vertexCount, - MeshFlags.SystemMemory | (index32 ? MeshFlags.Use32Bit : 0), - VertexFormat.Position | VertexFormat.Normal | VertexFormat.Texture1); - - SlimDX.DataStream indices = mesh.LockIndexBuffer(LockFlags.Discard); - if (index32) - { - foreach (int i in indexBuffer) - indices.Write(i); - } - else - { - foreach (int i in indexBuffer) - indices.Write((ushort)i); - } - mesh.UnlockIndexBuffer(); - - SlimDX.DataStream verts = mesh.LockVertexBuffer(LockFlags.Discard); - foreach (float f in vertexBuffer) - verts.Write(f); - mesh.UnlockVertexBuffer(); - - mesh.ComputeNormals(); - mesh.DrawSubset(0); - mesh.Dispose(); - } - } - */ } }