public void Clone() { ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron( new[] { new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 2, 0), new Vector3(0, 0, 3), new Vector3(1, 5, 0), new Vector3(0, 1, 7), }); ConvexPolyhedron clone = convexPolyhedron.Clone() as ConvexPolyhedron; Assert.IsNotNull(clone); for (int i = 0; i < clone.Vertices.Count; i++) { Assert.AreEqual(convexPolyhedron.Vertices[i], clone.Vertices[i]); } Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity), clone.GetAabb(Pose.Identity)); Assert.AreEqual(convexPolyhedron.InnerPoint, clone.InnerPoint); Assert.AreEqual(convexPolyhedron.GetSupportPoint(new Vector3(1, 1, 1)), clone.GetSupportPoint(new Vector3(1, 1, 1))); Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum); Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum); }
public void SerializationBinary() { var a = new ConvexPolyhedron( new[] { new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 2, 0), new Vector3(0, 0, 3), new Vector3(1, 5, 0), new Vector3(0, 1, 7), }); // Serialize object. var stream = new MemoryStream(); var formatter = new BinaryFormatter(); formatter.Serialize(stream, a); // Deserialize object. stream.Position = 0; var deserializer = new BinaryFormatter(); var b = (ConvexPolyhedron)deserializer.Deserialize(stream); for (int i = 0; i < b.Vertices.Count; i++) { Assert.AreEqual(a.Vertices[i], b.Vertices[i]); } Assert.AreEqual(a.GetAabb(Pose.Identity), b.GetAabb(Pose.Identity)); Assert.AreEqual(a.InnerPoint, b.InnerPoint); Assert.AreEqual(a.GetSupportPoint(new Vector3(1, 1, 1)), b.GetSupportPoint(new Vector3(1, 1, 1))); }
public void Clone() { ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron( new[] { new Vector3F(0, 0, 0), new Vector3F(1, 0, 0), new Vector3F(0, 2, 0), new Vector3F(0, 0, 3), new Vector3F(1, 5, 0), new Vector3F(0, 1, 7), }); ConvexPolyhedron clone = convexPolyhedron.Clone() as ConvexPolyhedron; Assert.IsNotNull(clone); for (int i = 0; i < clone.Vertices.Count; i++) Assert.AreEqual(convexPolyhedron.Vertices[i], clone.Vertices[i]); Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity), clone.GetAabb(Pose.Identity)); Assert.AreEqual(convexPolyhedron.InnerPoint, clone.InnerPoint); Assert.AreEqual(convexPolyhedron.GetSupportPoint(new Vector3F(1,1,1)), clone.GetSupportPoint(new Vector3F(1, 1, 1))); Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum); Assert.AreEqual(convexPolyhedron.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum); }
public void EmptyConvexPolyhedron() { ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(Enumerable.Empty<Vector3F>()); Assert.AreEqual(0, convexPolyhedron.Vertices.Count); Assert.AreEqual(Vector3F.Zero, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(), convexPolyhedron.GetAabb(Pose.Identity)); }
public void EmptyConvexPolyhedron() { ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(Enumerable.Empty <Vector3>()); Assert.AreEqual(0, convexPolyhedron.Vertices.Count); Assert.AreEqual(Vector3.Zero, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(), convexPolyhedron.GetAabb(Pose.Identity)); }
public void OnePoint() { Vector3F point = new Vector3F(1, 0, 0); ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point }); Assert.AreEqual(1, convexPolyhedron.Vertices.Count); Assert.AreEqual(point, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(point, point), convexPolyhedron.GetAabb(Pose.Identity)); }
private void CreateCompositeShape() { // Convert islands into CompositeShape with convex children. _decomposition = new CompositeShape(); if (_islands == null) { return; } foreach (var island in _islands) { if (island.Vertices.Length <= 0) { continue; } // ReSharper disable EmptyGeneralCatchClause try { // ----- Get convex hull mesh. DcelMesh convexHullMesh; if (island.ConvexHullBuilder == null) { // Create convex hull from scratch. // Get all vertices of all island triangles. var points = island.Triangles.SelectMany(t => t.Vertices); // Create convex hull. convexHullMesh = GeometryHelper.CreateConvexHull(points, VertexLimit, SkinWidth); } else { // Use existing convex hull. convexHullMesh = island.ConvexHullBuilder.Mesh; if (convexHullMesh.Vertices.Count > VertexLimit || SkinWidth != 0) { convexHullMesh.ModifyConvex(VertexLimit, SkinWidth); } } // ----- Add a ConvexPolyhedron to CompositeShape. if (convexHullMesh.Vertices.Count > 0) { var convexHullPoints = convexHullMesh.Vertices.Select(v => v.Position); var convexPolyhedron = new ConvexPolyhedron(convexHullPoints); var geometricObject = new GeometricObject(convexPolyhedron); _decomposition.Children.Add(geometricObject); } } catch { // Could not generate convex hull. Ignore object. } // ReSharper restore EmptyGeneralCatchClause } }
public void OnePoint() { Vector3 point = new Vector3(1, 0, 0); ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point }); Assert.AreEqual(1, convexPolyhedron.Vertices.Count); Assert.AreEqual(point, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(point, point), convexPolyhedron.GetAabb(Pose.Identity)); }
public void RandomConvexPolyhedron() { // Use a fixed seed. RandomHelper.Random = new Random(12345); // Try polyhedra with 0, 1, 2, ... points. for (int numberOfPoints = 0; numberOfPoints < 100; numberOfPoints++) { List <Vector3> points = new List <Vector3>(numberOfPoints); // Create random polyhedra. for (int i = 0; i < numberOfPoints; i++) { points.Add( new Vector3( RandomHelper.Random.NextFloat(-10, 10), RandomHelper.Random.NextFloat(-20, 20), RandomHelper.Random.NextFloat(-100, 100))); } //var convexHull = GeometryHelper.CreateConvexHull(points); ConvexPolyhedron convex = new ConvexPolyhedron(points); // Sample primary directions Vector3 right = new Vector3(2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right); Vector3 left = new Vector3(-2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left); Vector3 up = new Vector3(0, 2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up); Vector3 down = new Vector3(0, -2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down); Vector3 back = new Vector3(0, 0, 2); AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back); Vector3 front = new Vector3(0, 0, -2); AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front); // Sample random directions for (int i = 0; i < 10; i++) { Vector3 direction = RandomHelper.Random.NextVector3(-1, 1); if (direction.IsNumericallyZero) { continue; } Vector3 supportPoint = convex.GetSupportPoint(direction); Vector3 reference = GetSupportPoint(direction, points); // The support points can be different, e.g. if a an edge of face is normal to the // direction. When projected onto the direction both support points must be at equal // distance. AssertSupportPointsAreEquivalent(reference, supportPoint, direction); } } }
public void TwoPoints() { Vector3 point0 = new Vector3(1, 0, 0); Vector3 point1 = new Vector3(10, 0, 0); ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1 }); Assert.AreEqual(2, convexPolyhedron.Vertices.Count); Assert.AreEqual((point0 + point1) / 2, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(point0, point1), convexPolyhedron.GetAabb(Pose.Identity)); }
public void ThreePoints() { Vector3 point0 = new Vector3(1, 1, 1); Vector3 point1 = new Vector3(2, 1, 1); Vector3 point2 = new Vector3(1, 2, 1); ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1, point2 }); Assert.AreEqual(3, convexPolyhedron.Vertices.Count); Assert.AreEqual((point0 + point1 + point2) / 3, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(new Vector3(1, 1, 1), new Vector3(2, 2, 1)), convexPolyhedron.GetAabb(Pose.Identity)); }
/// <summary> /// Get points that are inside a polyhedron. /// </summary> /// <param name="p">The polyhedron.</param> /// <returns>The indices of the points.</returns> public List <int> GetInnerPoints(ConvexPolyhedron p, bool smartUpdate, bool influenceRegion2, Vector3?seedPoint = null) { List <int> points = new List <int>(); if (seedPoint != null) { if (false) { for (int k = 0; k < lastActivePoints.Count; ++k) { if (p.IsPointInside(Points[lastActivePoints[k]].RightHandedPosition, influenceRegion2)) { points.Add(lastActivePoints[k]); } } } else { for (int k = 0; k < Points.Count; ++k) { if (p.IsPointInside(Points[k].RightHandedPosition, influenceRegion2)) { points.Add(k); } } } } else { Vector3 _seedPoint = (Vector3)seedPoint; Vector3Int seedKey = GetKey(new Vector3(_seedPoint.x, _seedPoint.z, _seedPoint.y)); for (int i = -2; i <= 2; ++i) { for (int j = -2; j <= 2; ++j) { for (int l = -2; l <= 2; ++l) { var voxelPoints = GetVoxelForEditing(seedKey + new Vector3Int(i, j, l)); for (int k = 0; k < voxelPoints.Count; ++k) { if (p.IsPointInside(Points[voxelPoints[k]].RightHandedPosition.normalized, influenceRegion2)) { points.Add(voxelPoints[k]); } } } } } } return(points); }
private List <int> GetInnerPoints(ConvexPolyhedron p, List <int> innerPoints) { List <int> points = new List <int>(); for (int k = 0; k < innerPoints.Count; ++k) { if (p.IsPointInside(PointCloud[innerPoints[k]].Position, influenceRegion2)) { points.Add(innerPoints[k]); } } return(points); }
ShapeData CreateConvexHullShape(ConvexHullShape shape) { ConvexPolyhedron poly = shape.ConvexPolyhedron; if (poly != null) { throw new NotImplementedException(); } ShapeHull hull = new ShapeHull(shape); hull.BuildHull(shape.Margin); int indexCount = hull.NumIndices; UIntArray indices = hull.Indices; Vector3Array points = hull.Vertices; ShapeData shapeData = new ShapeData(); shapeData.VertexCount = indexCount; Vector3[] vertices = new Vector3[indexCount * 2]; int v = 0, i; for (i = 0; i < indexCount; i += 3) { Vector3 v0 = points[(int)indices[i]]; Vector3 v1 = points[(int)indices[i + 1]]; Vector3 v2 = points[(int)indices[i + 2]]; Vector3 v01 = v0 - v1; Vector3 v02 = v0 - v2; Vector3 normal = Vector3.Cross(v01, v02); normal.Normalize(); vertices[v++] = v0; vertices[v++] = normal; vertices[v++] = v1; vertices[v++] = normal; vertices[v++] = v2; vertices[v++] = normal; } shapeData.SetVertexBuffer(device, vertices); return(shapeData); }
public void GetSupportPoint() { ConvexPolyhedron emptyConvexPolyhedron = new ConvexPolyhedron(Enumerable.Empty<Vector3F>()); Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(1, 0, 0))); Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(0, 1, 0))); Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(0, 0, 1))); Assert.AreEqual(new Vector3F(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3F(1, 1, 1))); Vector3F p0 = new Vector3F(2, 0, 0); Vector3F p1 = new Vector3F(-1, -1, -2); Vector3F p2 = new Vector3F(0, 2, -3); Assert.IsTrue(Vector3F.AreNumericallyEqual(p0, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(1, 0, 0)))); Assert.IsTrue(Vector3F.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(0, 1, 0)))); Assert.IsTrue(Vector3F.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(0, 0, -1)))); Assert.IsTrue(Vector3F.AreNumericallyEqual(p1, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3F(-1, 0, 1)))); }
/// <summary> /// Find the most appropriate active point for the given edge. /// </summary> /// <param name="e_ij">Edge</param> /// <param name="edges">Set of existing edges</param> /// <param name="triangles">List of existing triangles</param> /// <returns>null if there is no active point, the index of the point</returns> private int?FindActivePoint(Edge e_ij, Dictionary <Edge, Edge> edges, List <Triangle> triangles, HashSet <Edge> fixedEdges, HashSet <int> fixedVertices) { findAPCounter++; int ap1, ap2; List <int> innerPoints; ConvexPolyhedron p = BuildPolyhedron(e_ij, edges, out ap1, out ap2, out innerPoints); List <Vector3> allowedPoints = new List <Vector3> { PointCloud[e_ij.vertex1].Position, PointCloud[e_ij.vertex2].Position }; if (ap1 != -1 && !fixedVertices.Contains(ap1)) { allowedPoints.Add(PointCloud[ap1].Position); } if (ap2 != -1 && !fixedVertices.Contains(ap2)) { allowedPoints.Add(PointCloud[ap2].Position); } //var points = VoxelSet.GetInnerPoints(p, (PointCloud[e_ij.vertex1].Position + PointCloud[e_ij.vertex2].Position) / 2); var points = GetInnerPoints(p, innerPoints); float sum = float.PositiveInfinity; int? activePoint = null; HashSet <Triangle> tr = new HashSet <Triangle>(); foreach (var m in points) { tr.UnionWith(PointCloud[m].triangles); } foreach (var m in points) { float currentSum = CalculateEnergy(e_ij.vertex1, e_ij.vertex2, e_ij.vertex3, m); if (PointCloud[m].status == PointStatus.ACTIVE && currentSum < sum && (!checkLengths || Distance(m, e_ij.vertex1) <= maxEdgeLength && Distance(m, e_ij.vertex2) <= maxEdgeLength) && GeomIntegrity(e_ij, m, tr, allowedPoints.ToArray(), fixedEdges, fixedVertices)) // if do not intersect other triangles { activePoint = m; sum = currentSum; } } return(activePoint); }
public void SimpleTetrahedron() { List <Vector3> points = new List <Vector3> { new Vector3(0, 0, 0), new Vector3(0, 1, 0), new Vector3(1, 0, 0), new Vector3(0, 0, 1), }; ConvexPolyhedron convex = new ConvexPolyhedron(points); // Sample primary directions Vector3 right = new Vector3(2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right); Vector3 left = new Vector3(-2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left); Vector3 up = new Vector3(0, 2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up); Vector3 down = new Vector3(0, -2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down); Vector3 back = new Vector3(0, 0, 2); AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back); Vector3 front = new Vector3(0, 0, -2); AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front); // Sample random directions for (int i = 0; i < 10; i++) { Vector3 direction = RandomHelper.Random.NextVector3(-1, 1); Vector3 supportPoint = convex.GetSupportPoint(direction); Vector3 reference = GetSupportPoint(direction, points); // The support points can be different, e.g. if a an edge of face is normal to the // direction. When projected onto the direction both support points must be at equal // distance. AssertSupportPointsAreEquivalent(reference, supportPoint, direction); } }
// Creates a convex shape for the given MeshContent. private void LoadConvex(MeshContent mesh, out Pose pose, out Shape shape) { // Apply node's transformation to all vertices. pose = Pose.Identity; Matrix transform = mesh.AbsoluteTransform; for (int i = 0; i < mesh.Positions.Count; i++) { mesh.Positions[i] = Vector3.Transform(mesh.Positions[i], transform); } // Convert the vertices from Microsoft.Xna.Framework.Vector3 to // DigitalRune.Mathematics.Algebra.Vector3F. IEnumerable <Vector3F> vertices = mesh.Positions.Select(pos => (Vector3F)pos); // Return a ConvexPolyhedron (convex hull) consisting of the mesh vertices. shape = new ConvexPolyhedron(vertices); }
public void GetSupportPoint() { ConvexPolyhedron emptyConvexPolyhedron = new ConvexPolyhedron(Enumerable.Empty <Vector3>()); Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(1, 0, 0))); Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(0, 1, 0))); Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(0, 0, 1))); Assert.AreEqual(new Vector3(0, 0, 0), emptyConvexPolyhedron.GetSupportPoint(new Vector3(1, 1, 1))); Vector3 p0 = new Vector3(2, 0, 0); Vector3 p1 = new Vector3(-1, -1, -2); Vector3 p2 = new Vector3(0, 2, -3); Assert.IsTrue(Vector3.AreNumericallyEqual(p0, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(1, 0, 0)))); Assert.IsTrue(Vector3.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(0, 1, 0)))); Assert.IsTrue(Vector3.AreNumericallyEqual(p2, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(0, 0, -1)))); Assert.IsTrue(Vector3.AreNumericallyEqual(p1, new ConvexPolyhedron(new[] { p0, p1, p2 }).GetSupportPoint(new Vector3(-1, 0, 1)))); }
/// <summary> /// Get points that are inside a polyhedron. /// </summary> /// <param name="p">The polyhedron.</param> /// <returns>The indices of the points.</returns> public int[] GetInnerPoints(ConvexPolyhedron p, Vector3?seedPoint = null) { List <int> points = new List <int>(); if (seedPoint != null) { for (int k = 0; k < Points.Count; ++k) { if (p.IsPointInside(Points[k].RightHandedPosition)) { points.Add(k); } } } else { Vector3 _seedPoint = (Vector3)seedPoint; Vector3Int seedKey = GetKey(new Vector3(_seedPoint.x, _seedPoint.z, _seedPoint.y)); for (int i = -2; i <= 2; ++i) { for (int j = -2; j <= 2; ++j) { for (int l = -2; l <= 2; ++l) { var voxelPoints = GetVoxelForEditing(seedKey + new Vector3Int(i, j, l)); for (int k = 0; k < voxelPoints.Count; ++k) { if (p.IsPointInside(Points[voxelPoints[k]].RightHandedPosition)) { points.Add(voxelPoints[k]); } } } } } } return(points.ToArray()); }
/// <summary> /// Build an influence region of the given edge. /// </summary> /// <param name="edge">The edge, which influence region to be foind.</param> /// <param name="edges">The set of existing edges.</param> /// <returns>Polyhedron object that is the influence region.</returns> private ConvexPolyhedron BuildPolyhedron(Edge edge, Dictionary <Edge, Edge> edges, out int ap1, out int ap2, out List <int> innerPoints) { if (influenceRegion2) { return(BuildPolyhedron2(edge, edges, out ap1, out ap2, out innerPoints)); } buildPCounter++; ap1 = -1; ap2 = -1; int i = edge.vertex1; int j = edge.vertex2; int k = edge.vertex3; // check if the i and j vertices are chosen correctly if (Vector3.Dot(Vector3.Cross(PointCloud[k].Position - PointCloud[i].Position, PointCloud[k].Position - PointCloud[j].Position), edge.normal) < 0) { i = edge.vertex2; j = edge.vertex1; } // positions of i, j and k Vector3 ip = PointCloud[i].Position; Vector3 jp = PointCloud[j].Position; Vector3 kp = PointCloud[k].Position; // the size of the polyhedron float s = (float)Math.Max(PointCloud[i].UniformityDegree, PointCloud[j].UniformityDegree) * (PointCloud[i].MinEdge + PointCloud[j].MinEdge) / 2; // bary center of the triangle ijk Vector3 p = (ip + jp + kp) / 3; // center of the edge ij Vector3 p_m = (ip + jp) / 2; // normal of the triangle (polygon) ijk Vector3 N = edge.normal; ConvexPolyhedron polyhedron = new ConvexPolyhedron(); Vector3 n1 = Vector3.Cross(edge.normal, jp - ip).normalized; polyhedron.AddFace(n1, ip); // test polyhedron.AddFace(N, p_m + 2 * s * N); // top face polyhedron.AddFace(-N, p_m - 2 * s * N); // bottom face - polyhedron.AddFace(-Vector3.Cross(N, ip - p).normalized, ip); // face containing pi - polyhedron.AddFace(Vector3.Cross(N, jp - p).normalized, jp); // face containing pj Vector3 N5 = Vector3.Cross(jp - ip, N).normalized; polyhedron.AddFace(N5, p_m + s * N5); // here it checks whether there are some edges inside the influnce region // that are connected to i or j var points = VoxelSet.GetInnerPoints(polyhedron, smartUpdate, influenceRegion2, p_m); Vector3?pLeft = null, pRight = null; //float minAngleLeft = 181f, minAngleRight = 181f; float minAngleLeft = 90 + regionAngle, minAngleRight = 90 + regionAngle; foreach (var point in points) { if (point == i || point == j) { continue; } Edge left = new Edge(point, i, Vector3.back); Edge right = new Edge(point, j, Vector3.back); if (edges.ContainsKey(left)) { float angleLeft = Vector3.Angle(jp - ip, PointCloud[point].Position - ip); if (angleLeft < minAngleLeft) { minAngleLeft = angleLeft; pLeft = PointCloud[point].Position; ap1 = point; } } if (edges.ContainsKey(right)) { float angleRight = Vector3.Angle(ip - jp, PointCloud[point].Position - jp); if (angleRight < minAngleRight) { minAngleRight = angleRight; pRight = PointCloud[point].Position; ap2 = point; } } } // additional faces that we need to hold geometry integrity if (pLeft != null) { polyhedron.AddFace(-Vector3.Cross(N, (Vector3)pLeft - ip).normalized, ip); } if (pRight != null) { polyhedron.AddFace(Vector3.Cross(N, (Vector3)pRight - jp).normalized, jp); } innerPoints = points; return(polyhedron); }
public void SerializationBinary() { var a = new ConvexPolyhedron( new[] { new Vector3F(0, 0, 0), new Vector3F(1, 0, 0), new Vector3F(0, 2, 0), new Vector3F(0, 0, 3), new Vector3F(1, 5, 0), new Vector3F(0, 1, 7), }); // Serialize object. var stream = new MemoryStream(); var formatter = new BinaryFormatter(); formatter.Serialize(stream, a); // Deserialize object. stream.Position = 0; var deserializer = new BinaryFormatter(); var b = (ConvexPolyhedron)deserializer.Deserialize(stream); for (int i = 0; i < b.Vertices.Count; i++) Assert.AreEqual(a.Vertices[i], b.Vertices[i]); Assert.AreEqual(a.GetAabb(Pose.Identity), b.GetAabb(Pose.Identity)); Assert.AreEqual(a.InnerPoint, b.InnerPoint); Assert.AreEqual(a.GetSupportPoint(new Vector3F(1, 1, 1)), b.GetSupportPoint(new Vector3F(1, 1, 1))); }
// Creates a lot of random objects. private void CreateRandomObjects() { var random = new Random(); var isFirstHeightField = true; int currentShape = 0; int numberOfObjects = 0; while (true) { numberOfObjects++; if (numberOfObjects > ObjectsPerType) { currentShape++; numberOfObjects = 0; } Shape shape; switch (currentShape) { case 0: // Box shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3); break; case 1: // Capsule shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize); break; case 2: // Cone shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize); break; case 3: // Cylinder shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize); break; case 4: // Sphere shape = new SphereShape(ObjectSize); break; case 5: // Convex hull of several points. ConvexHullOfPoints hull = new ConvexHullOfPoints(); hull.Points.Add(new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize)); hull.Points.Add(new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize)); hull.Points.Add(new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize)); hull.Points.Add(new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize)); hull.Points.Add(new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)); shape = hull; break; case 6: // A composite shape: two boxes that form a "T" shape. var composite = new CompositeShape(); composite.Children.Add( new GeometricObject( new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize), new Pose(new Vector3(0, 0, 0)))); composite.Children.Add( new GeometricObject( new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize), new Pose(new Vector3(0, 2 * ObjectSize, 0)))); shape = composite; break; case 7: shape = new CircleShape(ObjectSize); break; case 8: { var compBvh = new CompositeShape(); compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Matrix.Identity))); compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-MathHelper.ToRadians(15))))); compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity))); compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.CreateRotationX(0.3f)))); compBvh.Partition = new AabbTree<int>(); shape = compBvh; break; } case 9: CompositeShape comp = new CompositeShape(); comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3(0, 0.5f * ObjectSize, 0), Quaternion.Identity))); comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3(0.3f * ObjectSize, 0.7f * ObjectSize, 0), Quaternion.CreateRotationZ(-MathHelper.ToRadians(45))))); comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3(0, 1.15f * ObjectSize, 0), Quaternion.Identity))); shape = comp; break; case 10: shape = new ConvexHullOfPoints(new[] { new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize), new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize), new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize), new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize), new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize) }); break; case 11: ConvexHullOfShapes shapeHull = new ConvexHullOfShapes(); shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3(0, 2 * ObjectSize, 0), Matrix.Identity))); shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity)); shape = shapeHull; break; case 12: shape = Shape.Empty; break; case 13: var numberOfSamplesX = 10; var numberOfSamplesZ = 10; var samples = new float[numberOfSamplesX * numberOfSamplesZ]; for (int z = 0; z < numberOfSamplesZ; z++) for (int x = 0; x < numberOfSamplesX; x++) samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6); HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ); shape = heightField; break; //case 14: //shape = new LineShape(new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, -0.3f).Normalized); //break; case 15: shape = new LineSegmentShape( new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3(0.1f, 0.2f, -0.3f)); break; case 16: shape = new MinkowskiDifferenceShape { ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)), ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)) }; break; case 17: shape = new MinkowskiSumShape { ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)), ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)), }; break; case 18: shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2); break; case 19: shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3); break; case 20: shape = new PointShape(0.1f, 0.3f, 0.2f); break; case 21: shape = new RayShape(new Vector3(0.2f, 0, -0.12f), new Vector3(1, 2, 3).Normalized, ObjectSize * 2); break; case 22: shape = new RayShape(new Vector3(0.2f, 0, -0.12f), new Vector3(1, 2, 3).Normalized, ObjectSize * 2) { StopsAtFirstHit = true }; break; case 23: shape = new RectangleShape(ObjectSize, ObjectSize * 2); break; case 24: shape = new TransformedShape( new GeometricObject( new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), new Pose(new Vector3(0.1f, 1, -0.2f)))); break; case 25: shape = new TriangleShape( new Vector3(ObjectSize, 0, 0), new Vector3(0, ObjectSize, 0), new Vector3(ObjectSize, ObjectSize, ObjectSize)); break; //case 26: // { // // Create a composite object from which we get the mesh. // CompositeShape compBvh = new CompositeShape(); // compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Matrix.Identity))); // compBvh.Children.Add( // new GeometricObject( // new BoxShape(0.8f, 0.5f, 0.5f), // new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-(float)MathHelper.ToRadians(15))))); // compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity))); // compBvh.Children.Add( // new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.CreateRotationX(0.3f)))); // TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) }; // meshBvhShape.Partition = new AabbTree<int>(); // shape = meshBvhShape; // break; // } //case 27: // { // // Create a composite object from which we get the mesh. // CompositeShape compBvh = new CompositeShape(); // compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3(0, 0.5f, 0), Quaternion.Identity))); // compBvh.Children.Add( // new GeometricObject( // new BoxShape(0.8f, 0.5f, 0.5f), // new Pose(new Vector3(0.5f, 0.7f, 0), Quaternion.CreateRotationZ(-(float)MathHelper.ToRadians(15))))); // compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Quaternion.Identity))); // compBvh.Children.Add( // new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Quaternion.CreateRotationX(0.3f)))); // TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) }; // meshBvhShape.Partition = new AabbTree<int>(); // shape = meshBvhShape; // break; // } case 28: shape = new ConvexPolyhedron(new[] { new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize), new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize), new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize), new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize), new Vector3(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize) }); break; case 29: return; default: currentShape++; continue; } // Create an object with the random shape, pose, color and velocity. Pose randomPose = new Pose( random.NextVector3(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2), random.NextQuaternion()); var newObject = new MovingGeometricObject { Pose = randomPose, Shape = shape, LinearVelocity = random.NextQuaternion().Rotate(new Vector3(MaxLinearVelocity, 0, 0)), AngularVelocity = random.NextQuaternion().Rotate(Vector3.Forward) * RandomHelper.Random.NextFloat(0, MaxAngularVelocity), }; if (RandomHelper.Random.NextBool()) newObject.LinearVelocity = Vector3.Zero; if (RandomHelper.Random.NextBool()) newObject.AngularVelocity = Vector3.Zero; if (shape is LineShape || shape is HeightField) { // Do not move lines or the height field. newObject.LinearVelocity = Vector3.Zero; newObject.AngularVelocity = Vector3.Zero; } // Create only 1 heightField! if (shape is HeightField) { if (isFirstHeightField) { isFirstHeightField = true; newObject.Pose = new Pose(new Vector3(-BoxSize, -BoxSize, -BoxSize)); } else { currentShape++; numberOfObjects = 0; continue; } } // Add collision object to collision domain. _domain.CollisionObjects.Add(new CollisionObject(newObject)); //co.Type = CollisionObjectType.Trigger; //co.Name = "Object" + shape.GetType().Name + "_" + i; } }
public ConstraintVehicleObject(IServiceLocator services) { Name = "Vehicle"; _services = services; _inputService = services.GetInstance <IInputService>(); _simulation = services.GetInstance <Simulation>(); // Load models for rendering. var contentManager = services.GetInstance <ContentManager>(); _vehicleModelNode = contentManager.Load <ModelNode>("Car/Car").Clone(); _wheelModelNodes = new ModelNode[4]; _wheelModelNodes[0] = contentManager.Load <ModelNode>("Car/Wheel").Clone(); _wheelModelNodes[1] = _wheelModelNodes[0].Clone(); _wheelModelNodes[2] = _wheelModelNodes[0].Clone(); _wheelModelNodes[3] = _wheelModelNodes[0].Clone(); // Add wheels under the car model node. _vehicleModelNode.Children.Add(_wheelModelNodes[0]); _vehicleModelNode.Children.Add(_wheelModelNodes[1]); _vehicleModelNode.Children.Add(_wheelModelNodes[2]); _vehicleModelNode.Children.Add(_wheelModelNodes[3]); // ----- Create the chassis of the car. // The Vehicle needs a rigid body that represents the chassis. This can be any shape (e.g. // a simple BoxShape). In this example we will build a convex polyhedron from the car model. // 1. Extract the vertices from the car model. // The car model has ~10,000 vertices. It consists of a MeshNode for the glass // parts and a MeshNode "Car" for the chassis. var meshNode = _vehicleModelNode.GetDescendants() .OfType <MeshNode>() .First(mn => mn.Name == "Car"); var mesh = MeshHelper.ToTriangleMesh(meshNode.Mesh); // Apply the transformation of the mesh node. mesh.Transform(meshNode.PoseWorld * Matrix.CreateScale(meshNode.ScaleWorld)); // 2. (Optional) Create simplified convex hull from mesh. // We could also skip this step and directly create a convex polyhedron from the mesh using // var chassisShape = new ConvexPolyhedron(mesh.Vertices); // However, the convex polyhedron would still have 500-600 vertices. // We can reduce the number of vertices by using the GeometryHelper. // Create a convex hull for mesh with max. 64 vertices. Additional, shrink the hull by 4 cm. var convexHull = GeometryHelper.CreateConvexHull(mesh.Vertices, 64, -0.04f); // 3. Create convex polyhedron shape using the vertices of the convex hull. var chassisShape = new ConvexPolyhedron(convexHull.Vertices.Select(v => v.Position)); // (Note: Building convex hulls and convex polyhedra are time-consuming. To save loading time // we should build the shape in the XNA content pipeline. See other DigitalRune Physics // Samples.) // The mass properties of the car. We use a mass of 800 kg. var mass = MassFrame.FromShapeAndMass(chassisShape, Vector3.One, 800, 0.1f, 1); // Trick: We artificially modify the center of mass of the rigid body. Lowering the center // of mass makes the car more stable against rolling in tight curves. // We could also modify mass.Inertia for other effects. var pose = mass.Pose; pose.Position.Y -= 0.5f; // Lower the center of mass. pose.Position.Z = -0.5f; // The center should be below the driver. // (Note: The car model is not exactly centered.) mass.Pose = pose; // Material for the chassis. var material = new UniformMaterial { Restitution = 0.1f, StaticFriction = 0.2f, DynamicFriction = 0.2f }; var chassis = new RigidBody(chassisShape, mass, material) { Pose = new Pose(new Vector3(0, 2, 0)), // Start position UserData = "NoDraw", // (Remove this line to render the collision model.) }; // ----- Create the vehicle. Vehicle = new ConstraintVehicle(_simulation, chassis); // Add 4 wheels. Vehicle.Wheels.Add(new ConstraintWheel { Offset = new Vector3(-0.9f, 0.6f, -2.0f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 2 }); // Front left Vehicle.Wheels.Add(new ConstraintWheel { Offset = new Vector3(0.9f, 0.6f, -2.0f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 2 }); // Front right Vehicle.Wheels.Add(new ConstraintWheel { Offset = new Vector3(-0.9f, 0.6f, 0.98f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 1.8f }); // Back left Vehicle.Wheels.Add(new ConstraintWheel { Offset = new Vector3(0.9f, 0.6f, 0.98f), Radius = 0.36f, SuspensionRestLength = 0.55f, MinSuspensionLength = 0.25f, Friction = 1.8f }); // Back right // Vehicles are disabled per default. This way we can create the vehicle and the simulation // objects are only added when needed. Vehicle.Enabled = false; }
public void SimpleTetrahedron() { List<Vector3F> points = new List<Vector3F> { new Vector3F(0, 0, 0), new Vector3F(0, 1, 0), new Vector3F(1, 0, 0), new Vector3F(0, 0, 1), }; ConvexPolyhedron convex = new ConvexPolyhedron(points); // Sample primary directions Vector3F right = new Vector3F(2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right); Vector3F left = new Vector3F(-2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left); Vector3F up = new Vector3F(0, 2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up); Vector3F down = new Vector3F(0, -2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down); Vector3F back = new Vector3F(0, 0, 2); AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back); Vector3F front = new Vector3F(0, 0, -2); AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front); // Sample random directions for (int i = 0; i < 10; i++) { Vector3F direction = RandomHelper.Random.NextVector3F(-1, 1); Vector3F supportPoint = convex.GetSupportPoint(direction); Vector3F reference = GetSupportPoint(direction, points); // The support points can be different, e.g. if a an edge of face is normal to the // direction. When projected onto the direction both support points must be at equal // distance. AssertSupportPointsAreEquivalent(reference, supportPoint, direction); } }
public ShapesSample(Microsoft.Xna.Framework.Game game) : base(game) { // Add basic force effects. Simulation.ForceEffects.Add(new Gravity()); Simulation.ForceEffects.Add(new Damping()); // Add a ground plane. RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0)) { Name = "GroundPlane", // Names are not required but helpful for debugging. MotionType = MotionType.Static, }; Simulation.RigidBodies.Add(groundPlane); // ----- Add a sphere. Shape sphere = new SphereShape(0.5f); Simulation.RigidBodies.Add(new RigidBody(sphere)); // ----- Add a box. BoxShape box = new BoxShape(0.5f, 0.9f, 0.7f); Simulation.RigidBodies.Add(new RigidBody(box)); // ----- Add a capsule. CapsuleShape capsule = new CapsuleShape(0.4f, 1.2f); Simulation.RigidBodies.Add(new RigidBody(capsule)); // ----- Add a cone. ConeShape cone = new ConeShape(0.5f, 1f); Simulation.RigidBodies.Add(new RigidBody(cone)); // ----- Add a cylinder. CylinderShape cylinder = new CylinderShape(0.3f, 1f); Simulation.RigidBodies.Add(new RigidBody(cylinder)); // ----- Add a convex hull of random points. ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints(); for (int i = 0; i < 20; i++) { convexHullOfPoints.Points.Add(RandomHelper.Random.NextVector3F(-0.5f, 0.5f)); } Simulation.RigidBodies.Add(new RigidBody(convexHullOfPoints)); // ----- Add a convex polyhedron. // (A ConvexPolyhedron is similar to the ConvexHullOfPoints. The difference is that // the points in a ConvexHullOfPoints can be changed at runtime. A ConvexPolyhedron // cannot be changed at runtime, but it is faster.) List <Vector3F> points = new List <Vector3F>(); for (int i = 0; i < 20; i++) { points.Add(RandomHelper.Random.NextVector3F(-0.7f, 0.7f)); } ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(points); Simulation.RigidBodies.Add(new RigidBody(convexPolyhedron)); // ----- Add a composite shape (a table that consists of 5 boxes). CompositeShape composite = new CompositeShape(); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, -0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, -0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, 0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, 0.5f)))); composite.Children.Add(new GeometricObject(new BoxShape(1.8f, 0.1f, 1.1f), new Pose(new Vector3F(0, 0.8f, 0)))); Simulation.RigidBodies.Add(new RigidBody(composite)); // ----- Add a convex hull of multiple shapes. ConvexHullOfShapes convexHullOfShapes = new ConvexHullOfShapes(); convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(-0.4f, 0, 0)))); convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(+0.4f, 0, 0)))); Simulation.RigidBodies.Add(new RigidBody(convexHullOfShapes)); // ----- Add the Minkowski sum of two shapes. // (The Minkowski sum is a mathematical operation that combines two shapes. // Here a circle is combined with a sphere. The result is a wheel.) MinkowskiSumShape minkowskiSum = new MinkowskiSumShape(); minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.2f), Pose.Identity); minkowskiSum.ObjectB = new GeometricObject(new CircleShape(0.5f), Pose.Identity); Simulation.RigidBodies.Add(new RigidBody(minkowskiSum)); // Create another Minkowski sum. (Here a small sphere is added to a box to create a // box with rounded corners.) minkowskiSum = new MinkowskiSumShape(); minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.1f), Pose.Identity); minkowskiSum.ObjectB = new GeometricObject(new BoxShape(0.2f, 0.5f, 0.8f), Pose.Identity); Simulation.RigidBodies.Add(new RigidBody(minkowskiSum)); // ----- Add a triangle mesh. // A triangle mesh could be loaded from a file or built from an XNA model. // Here we first create a composite shape and convert the shape into a triangle // mesh. (Any Shape in DigitalRune.Geometry can be converted to a triangle mesh.) CompositeShape dumbbell = new CompositeShape(); dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(0.6f, 0.0f, 0.0f)))); dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(-0.6f, 0.0f, 0.0f)))); dumbbell.Children.Add(new GeometricObject(new CylinderShape(0.1f, 0.6f), new Pose(Matrix33F.CreateRotationZ(ConstantsF.PiOver2)))); TriangleMeshShape triangleMeshShape = new TriangleMeshShape(dumbbell.GetMesh(0.01f, 2)); // Optional: We can enable "contact welding". When this flag is enabled, the triangle shape // precomputes additional internal information for the mesh. The collision detection will // be able to compute better contacts (e.g. better normal vectors at triangle edges). // Pro: Collision detection can compute better contact information. // Con: Contact welding information needs a bit of memory. And the collision detection is // a bit slower. triangleMeshShape.EnableContactWelding = true; // Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition // adds an additional memory overhead, but it improves collision detection speed tremendously!) triangleMeshShape.Partition = new AabbTree <int>(); Simulation.RigidBodies.Add(new RigidBody(triangleMeshShape)); // ----- Set random positions/orientations. // (Start with the second object. The first object is the ground plane which should // not be changed.) for (int i = 1; i < Simulation.RigidBodies.Count; i++) { RigidBody body = Simulation.RigidBodies[i]; Vector3F position = RandomHelper.Random.NextVector3F(-3, 3); position.Y = 3; // Position the objects 3m above ground. QuaternionF orientation = RandomHelper.Random.NextQuaternionF(); body.Pose = new Pose(position, orientation); } }
public void ThreePoints() { Vector3F point0 = new Vector3F(1, 1, 1); Vector3F point1 = new Vector3F(2, 1, 1); Vector3F point2 = new Vector3F(1, 2, 1); ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1, point2 }); Assert.AreEqual(3, convexPolyhedron.Vertices.Count); Assert.AreEqual((point0 + point1 + point2) / 3, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(new Vector3F(1, 1, 1), new Vector3F(2, 2, 1)), convexPolyhedron.GetAabb(Pose.Identity)); }
public void TwoPoints() { Vector3F point0 = new Vector3F(1, 0, 0); Vector3F point1 = new Vector3F(10, 0, 0); ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(new[] { point0, point1 }); Assert.AreEqual(2, convexPolyhedron.Vertices.Count); Assert.AreEqual((point0 + point1) / 2, convexPolyhedron.InnerPoint); Assert.AreEqual(new Aabb(point0, point1), convexPolyhedron.GetAabb(Pose.Identity)); }
public void RandomConvexPolyhedron() { // Use a fixed seed. RandomHelper.Random = new Random(12345); // Try polyhedra with 0, 1, 2, ... points. for (int numberOfPoints = 0; numberOfPoints < 100; numberOfPoints++) { List<Vector3F> points = new List<Vector3F>(numberOfPoints); // Create random polyhedra. for (int i = 0; i < numberOfPoints; i++) points.Add( new Vector3F( RandomHelper.Random.NextFloat(-10, 10), RandomHelper.Random.NextFloat(-20, 20), RandomHelper.Random.NextFloat(-100, 100))); //var convexHull = GeometryHelper.CreateConvexHull(points); ConvexPolyhedron convex = new ConvexPolyhedron(points); // Sample primary directions Vector3F right = new Vector3F(2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(right, points), convex.GetSupportPoint(right), right); Vector3F left = new Vector3F(-2, 0, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(left, points), convex.GetSupportPoint(left), left); Vector3F up = new Vector3F(0, 2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(up, points), convex.GetSupportPoint(up), up); Vector3F down = new Vector3F(0, -2, 0); AssertSupportPointsAreEquivalent(GetSupportPoint(down, points), convex.GetSupportPoint(down), down); Vector3F back = new Vector3F(0, 0, 2); AssertSupportPointsAreEquivalent(GetSupportPoint(back, points), convex.GetSupportPoint(back), back); Vector3F front = new Vector3F(0, 0, -2); AssertSupportPointsAreEquivalent(GetSupportPoint(front, points), convex.GetSupportPoint(front), front); // Sample random directions for (int i = 0; i < 10; i++) { Vector3F direction = RandomHelper.Random.NextVector3F(-1, 1); if (direction.IsNumericallyZero) continue; Vector3F supportPoint = convex.GetSupportPoint(direction); Vector3F reference = GetSupportPoint(direction, points); // The support points can be different, e.g. if a an edge of face is normal to the // direction. When projected onto the direction both support points must be at equal // distance. AssertSupportPointsAreEquivalent(reference, supportPoint, direction); } } }
Mesh CreateConvexHullShape(ConvexHullShape shape) { ConvexPolyhedron poly = shape.ConvexPolyhedron; if (poly != null) { throw new NotImplementedException(); } ShapeHull hull = new ShapeHull(shape); hull.BuildHull(shape.Margin); UIntArray hullIndices = hull.Indices; Vector3Array points = hull.Vertices; int vertexCount = hull.NumIndices; int faceCount = hull.NumTriangles; bool index32 = vertexCount > 65536; Mesh mesh = new Mesh(device, faceCount, vertexCount, MeshFlags.SystemMemory | (index32 ? MeshFlags.Use32Bit : 0), VertexFormat.Position | VertexFormat.Normal); SlimDX.DataStream indices = mesh.LockIndexBuffer(LockFlags.Discard); int i; if (index32) { for (i = 0; i < vertexCount; i++) { indices.Write(i); } } else { for (i = 0; i < vertexCount; i++) { indices.Write((short)i); } } mesh.UnlockIndexBuffer(); SlimDX.DataStream verts = mesh.LockVertexBuffer(LockFlags.Discard); Vector3 scale = Vector3.Multiply(shape.LocalScaling, 1.0f + shape.Margin); for (i = 0; i < vertexCount; i += 3) { verts.Write(Vector3.Modulate(points[(int)hullIndices[i]], scale)); verts.Position += 12; verts.Write(Vector3.Modulate(points[(int)hullIndices[i + 1]], scale)); verts.Position += 12; verts.Write(Vector3.Modulate(points[(int)hullIndices[i + 2]], scale)); verts.Position += 12; } mesh.UnlockVertexBuffer(); mesh.ComputeNormals(); shapes.Add(shape, mesh); return(mesh); }
/// <summary> /// Build open influence region with 3 faces for a given edge. /// </summary> /// <param name="edge">The given edge.</param> /// <param name="edges">All existing edges.</param> /// <param name="ap1">Allowed point 1</param> /// <param name="ap2">Allowed point 2</param>s /// <param name="innerPoints">Points that approximately lie inside the influence region (polyhedron).</param> /// <returns>The polyhedron.</returns> private ConvexPolyhedron BuildPolyhedron2(Edge edge, Dictionary <Edge, Edge> edges, out int ap1, out int ap2, out List <int> innerPoints) { buildPCounter++; ap1 = -1; ap2 = -1; int i = edge.vertex1; int j = edge.vertex2; int k = edge.vertex3; // check if the i and j vertices are chosen correctly if (Vector3.Dot(Vector3.Cross(PointCloud[k].Position - PointCloud[i].Position, PointCloud[k].Position - PointCloud[j].Position), edge.normal) < 0) { i = edge.vertex2; j = edge.vertex1; } // positions of i, j and k Vector3 ip = PointCloud[i].Position; Vector3 jp = PointCloud[j].Position; Vector3 kp = PointCloud[k].Position; Vector3 n1 = Vector3.Cross(edge.normal, jp - ip).normalized; Vector3 n3 = (Vector3.Cross(n1 + Mathf.Tan(Mathf.Deg2Rad * regionAngle) * (ip - jp).normalized, edge.normal)).normalized; Vector3 n2 = Vector3.Cross(edge.normal, n1 + Mathf.Tan(Mathf.Deg2Rad * regionAngle) * (jp - ip).normalized).normalized; float s = sMult * (float)Math.Max(PointCloud[i].UniformityDegree, PointCloud[j].UniformityDegree) * (PointCloud[i].MinEdge + PointCloud[j].MinEdge) / 2; ConvexPolyhedron polyhedron = new ConvexPolyhedron(); polyhedron.AddFace(n1, ip); polyhedron.AddFace(n2, ip); polyhedron.AddFace(n3, jp); polyhedron.AddFace(-n1, s * -n1 + (ip + jp) / 2); polyhedron.AddFace(edge.normal, ip + s * edge.normal); // top face polyhedron.AddFace(-edge.normal, ip - s * edge.normal); // bottom face - innerPoints = VoxelSet.GetInnerPoints(polyhedron, false, influenceRegion2, (ip + jp) / 2); Vector3?pLeft = null, pRight = null; float minAngleLeft = 181f, minAngleRight = 181f; foreach (var point in innerPoints) { if (point == i || point == j) { continue; } Edge left = new Edge(point, i, Vector3.back); Edge right = new Edge(point, j, Vector3.back); if (edges.ContainsKey(left)) { float angleLeft = Vector3.Angle(ip - jp, PointCloud[point].Position - ip); if (angleLeft < minAngleLeft) { minAngleLeft = angleLeft; pLeft = PointCloud[point].Position; ap1 = point; } } if (edges.ContainsKey(right)) { float angleRight = Vector3.Angle(jp - ip, PointCloud[point].Position - jp); if (angleRight < minAngleRight) { minAngleRight = angleRight; pRight = PointCloud[point].Position; ap2 = point; } } } ConvexPolyhedron p = new ConvexPolyhedron(); p.AddFace(n1, ip); if (pLeft == null) { p.AddFace(n2, ip); } else { p.AddFace(Vector3.Cross((Vector3)pLeft - ip, edge.normal).normalized, ip); } if (pRight == null) { p.AddFace(n3, jp); } else { p.AddFace(-Vector3.Cross((Vector3)pRight - jp, edge.normal).normalized, jp); } polyhedron.AddFace(-n1, s * -n1 + (ip + jp) / 2); return(p); }