/// <summary> /// Get the fat AABB for a proxy. /// </summary> /// <param name="proxyId">The proxy id.</param> /// <param name="fatAABB">The fat AABB.</param> public void GetFatAABB(int proxyId, out JBBox fatAABB) { Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); fatAABB = _nodes[proxyId].AABB; }
private void InsertLeaf(int leaf) { ++_insertionCount; if (_root == NullNode) { _root = leaf; _nodes[_root].ParentOrNext = NullNode; return; } // Find the best sibling for this node JBBox leafAABB = _nodes[leaf].AABB; int sibling = _root; while (_nodes[sibling].IsLeaf() == false) { int child1 = _nodes[sibling].Child1; int child2 = _nodes[sibling].Child2; // Expand the node's AABB. //_nodes[sibling].AABB.Combine(ref leafAABB); JBBox.CreateMerged(ref _nodes[sibling].AABB, ref leafAABB, out _nodes[sibling].AABB); _nodes[sibling].LeafCount += 1; float siblingArea = _nodes[sibling].AABB.Perimeter; JBBox parentAABB = new JBBox(); //parentAABB.Combine(ref _nodes[sibling].AABB, ref leafAABB); JBBox.CreateMerged(ref _nodes[sibling].AABB, ref leafAABB, out _nodes[sibling].AABB); float parentArea = parentAABB.Perimeter; float cost1 = 2.0f * parentArea; float inheritanceCost = 2.0f * (parentArea - siblingArea); float cost2; if (_nodes[child1].IsLeaf()) { JBBox aabb = new JBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); JBBox.CreateMerged(ref leafAABB, ref _nodes[child1].AABB, out aabb); cost2 = aabb.Perimeter + inheritanceCost; } else { JBBox aabb = new JBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); JBBox.CreateMerged(ref leafAABB, ref _nodes[child1].AABB, out aabb); float oldArea = _nodes[child1].AABB.Perimeter; float newArea = aabb.Perimeter; cost2 = (newArea - oldArea) + inheritanceCost; } float cost3; if (_nodes[child2].IsLeaf()) { JBBox aabb = new JBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); JBBox.CreateMerged(ref leafAABB, ref _nodes[child2].AABB, out aabb); cost3 = aabb.Perimeter + inheritanceCost; } else { JBBox aabb = new JBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); JBBox.CreateMerged(ref leafAABB, ref _nodes[child2].AABB, out aabb); float oldArea = _nodes[child2].AABB.Perimeter; float newArea = aabb.Perimeter; cost3 = newArea - oldArea + inheritanceCost; } // Descend according to the minimum cost. if (cost1 < cost2 && cost1 < cost3) { break; } // Expand the node's AABB to account for the new leaf. //_nodes[sibling].AABB.Combine(ref leafAABB); JBBox.CreateMerged(ref leafAABB, ref _nodes[sibling].AABB, out _nodes[sibling].AABB); // Descend if (cost2 < cost3) { sibling = child1; } else { sibling = child2; } } // Create a new parent for the siblings. int oldParent = _nodes[sibling].ParentOrNext; int newParent = AllocateNode(); _nodes[newParent].ParentOrNext = oldParent; _nodes[newParent].UserData = default(T); //_nodes[newParent].AABB.Combine(ref leafAABB, ref _nodes[sibling].AABB); JBBox.CreateMerged(ref leafAABB, ref _nodes[sibling].AABB, out _nodes[newParent].AABB); _nodes[newParent].LeafCount = _nodes[sibling].LeafCount + 1; if (oldParent != NullNode) { // The sibling was not the root. if (_nodes[oldParent].Child1 == sibling) { _nodes[oldParent].Child1 = newParent; } else { _nodes[oldParent].Child2 = newParent; } _nodes[newParent].Child1 = sibling; _nodes[newParent].Child2 = leaf; _nodes[sibling].ParentOrNext = newParent; _nodes[leaf].ParentOrNext = newParent; } else { // The sibling was the root. _nodes[newParent].Child1 = sibling; _nodes[newParent].Child2 = leaf; _nodes[sibling].ParentOrNext = newParent; _nodes[leaf].ParentOrNext = newParent; _root = newParent; } }
/// <summary> /// Passes a axis aligned bounding box to the shape where collision /// could occour. /// </summary> /// <param name="box">The bounding box where collision could occur.</param> /// <returns>The upper index with which <see cref="SetCurrentShape"/> can be /// called.</returns> public abstract int Prepare(ref JBBox box);
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody) { if (rigidBody.Shape is Multishape) { Multishape ms = (rigidBody.Shape as Multishape); ms = ms.RequestWorkingClone(); JBBox transformedBoundingBox = softBody.BoundingBox; transformedBoundingBox.InverseTransform(ref rigidBody.position, ref rigidBody.orientation); int msLength = ms.Prepare(ref transformedBoundingBox); List <int> detected = potentialTriangleLists.GetNew(); softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox); foreach (int i in detected) { SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i); JVector point, normal; float penetration; bool result; for (int e = 0; e < msLength; e++) { ms.SetCurrentShape(e); result = XenoCollide.Detect(ms, t, ref rigidBody.orientation, ref JMatrix.InternalIdentity, ref rigidBody.position, ref JVector.InternalZero, out point, out normal, out penetration); if (result) { int minIndex = FindNearestTrianglePoint(softBody, i, ref point); RaiseCollisionDetected(rigidBody, softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration); } } } detected.Clear(); potentialTriangleLists.GiveBack(detected); ms.ReturnWorkingClone(); } else { List <int> detected = potentialTriangleLists.GetNew(); softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox); foreach (int i in detected) { SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i); JVector point, normal; float penetration; bool result; result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref JMatrix.InternalIdentity, ref rigidBody.position, ref JVector.InternalZero, out point, out normal, out penetration); if (result) { int minIndex = FindNearestTrianglePoint(softBody, i, ref point); RaiseCollisionDetected(rigidBody, softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration); } } detected.Clear(); potentialTriangleLists.GiveBack(detected); } }
private void DetectRigidRigid(RigidBody body1, RigidBody body2) { bool b1IsMulti = (body1.Shape is Multishape); bool b2IsMulti = (body2.Shape is Multishape); bool speculative = speculativeContacts || (body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts); JVector point, normal; float penetration; if (!b1IsMulti && !b2IsMulti) { if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { JVector point1, point2; FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { JVector hit1, hit2; if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { JVector delta = hit2 - hit1; if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared()) { penetration = delta * normal; if (penetration < 0.0f) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } } else if (b1IsMulti && b2IsMulti) { Multishape ms1 = (body1.Shape as Multishape); Multishape ms2 = (body2.Shape as Multishape); ms1 = ms1.RequestWorkingClone(); ms2 = ms2.RequestWorkingClone(); JBBox transformedBoundingBox = body2.boundingBox; transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation); int ms1Length = ms1.Prepare(ref transformedBoundingBox); transformedBoundingBox = body1.boundingBox; transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation); int ms2Length = ms2.Prepare(ref transformedBoundingBox); if (ms1Length == 0 || ms2Length == 0) { ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); return; } for (int i = 0; i < ms1Length; i++) { ms1.SetCurrentShape(i); for (int e = 0; e < ms2Length; e++) { ms2.SetCurrentShape(e); if (XenoCollide.Detect(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { JVector point1, point2; FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { JVector hit1, hit2; if (GJKCollide.ClosestPoints(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { JVector delta = hit2 - hit1; if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared()) { penetration = delta * normal; if (penetration < 0.0f) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } } } ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); } else { RigidBody b1, b2; if (body2.Shape is Multishape) { b1 = body2; b2 = body1; } else { b2 = body2; b1 = body1; } Multishape ms = (b1.Shape as Multishape); ms = ms.RequestWorkingClone(); JBBox transformedBoundingBox = b2.boundingBox; transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation); int msLength = ms.Prepare(ref transformedBoundingBox); if (msLength == 0) { ms.ReturnWorkingClone(); return; } for (int i = 0; i < msLength; i++) { ms.SetCurrentShape(i); if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out point, out normal, out penetration)) { JVector point1, point2; FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2); if (useTerrainNormal && ms is TerrainShape) { (ms as TerrainShape).CollisionNormal(out normal); JVector.Transform(ref normal, ref b1.orientation, out normal); } else if (useTriangleMeshNormal && ms is TriangleMeshShape) { (ms as TriangleMeshShape).CollisionNormal(out normal); JVector.Transform(ref normal, ref b1.orientation, out normal); } RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { JVector hit1, hit2; if (GJKCollide.ClosestPoints(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out hit1, out hit2, out normal)) { JVector delta = hit2 - hit1; if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared()) { penetration = delta * normal; if (penetration < 0.0f) { RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration); } } } } } ms.ReturnWorkingClone(); } }
public void BuildOctree() { triBoxes = new JBBox[tris.Length]; rootNodeBox = new JBBox(new JVector(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity), new JVector(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity)); for (int i = 0; i < tris.Length; i++) { var min = JVector.Min(positions[tris[i].I1], positions[tris[i].I2]); min = JVector.Min(positions[tris[i].I0], min); var max = JVector.Max(positions[tris[i].I1], positions[tris[i].I2]); max = JVector.Max(positions[tris[i].I0], max); min = JVector.Min(rootNodeBox.Min, min); max = JVector.Max(rootNodeBox.Max, max); triBoxes[i] = new JBBox(min, max); } var buildNodes = new List <BuildNode> { new BuildNode() }; buildNodes[0].box = rootNodeBox; var children = new JBBox[8]; for (int triNum = 0; triNum < tris.Length; triNum++) { int nodeIndex = 0; var box = rootNodeBox; while (box.Contains(triBoxes[triNum]) == JBBox.ContainmentType.Contains) { int childCon = -1; for (int i = 0; i < 8; ++i) { CreateAABox(box, (EChild)i, out children[i]); if (children[i].Contains(triBoxes[triNum]) == JBBox.ContainmentType.Contains) { childCon = i; break; } } if (childCon == -1) { buildNodes[nodeIndex].triIndices.Add(triNum); break; } else { int childIndex = -1; for (int index = 0; index < buildNodes[nodeIndex].nodeIndices.Count; ++index) { if (buildNodes[buildNodes[nodeIndex].nodeIndices[index]].childType == childCon) { childIndex = index; break; } } if (childIndex == -1) { var parentNode = buildNodes[nodeIndex]; var newNode = new BuildNode { childType = childCon, box = children[childCon] }; buildNodes.Add(newNode); nodeIndex = buildNodes.Count - 1; box = children[childCon]; parentNode.nodeIndices.Add(nodeIndex); } else { nodeIndex = buildNodes[nodeIndex].nodeIndices[childIndex]; box = children[childCon]; } } } } nodes = new Node[buildNodes.Count]; nodeStackPool = new ArrayResourcePool <ushort>(buildNodes.Count); for (int i = 0; i < nodes.Length; i++) { nodes[i].nodeIndices = new ushort[buildNodes[i].nodeIndices.Count]; for (int index = 0; index < nodes[i].nodeIndices.Length; ++index) { nodes[i].nodeIndices[index] = (ushort)buildNodes[i].nodeIndices[index]; } nodes[i].triIndices = new int[buildNodes[i].triIndices.Count]; buildNodes[i].triIndices.CopyTo(nodes[i].triIndices); nodes[i].box = buildNodes[i].box; } buildNodes.Clear(); }
private void DetectRigidRigid(RigidBody body1, RigidBody body2) { // CUSTOM: Added custom detection callbacks (primarily to accommodate actor movement on surfaces). var callback1 = body1.ShouldGenerateContact; var callback2 = body2.ShouldGenerateContact; bool b1IsMulti = (body1.Shape is Multishape); bool b2IsMulti = (body2.Shape is Multishape); bool speculative = speculativeContacts || (body1.AreSpeculativeContactsEnabled || body2.AreSpeculativeContactsEnabled); JVector point, normal; float penetration; if (!b1IsMulti && !b2IsMulti) { // CUSTOM: Added these callbacks (two rigid bodies). if ((callback1 != null && !callback1(body2, null)) || (callback2 != null && !callback2(body1, null))) { return; } if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { JVector point1, point2; FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { JVector hit1, hit2; if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { JVector delta = hit2 - hit1; if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared()) { penetration = delta * normal; if (penetration < 0.0f) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } } else if (b1IsMulti && b2IsMulti) { Multishape ms1 = (body1.Shape as Multishape); Multishape ms2 = (body2.Shape as Multishape); ms1 = ms1.RequestWorkingClone(); ms2 = ms2.RequestWorkingClone(); JBBox transformedBoundingBox = body2.boundingBox; transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation); int ms1Length = ms1.Prepare(ref transformedBoundingBox); transformedBoundingBox = body1.boundingBox; transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation); int ms2Length = ms2.Prepare(ref transformedBoundingBox); if (ms1Length == 0 || ms2Length == 0) { ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); return; } for (int i = 0; i < ms1Length; i++) { ms1.SetCurrentShape(i); for (int e = 0; e < ms2Length; e++) { ms2.SetCurrentShape(e); if (XenoCollide.Detect(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out point, out normal, out penetration)) { JVector point1, point2; FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2); RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { JVector hit1, hit2; if (GJKCollide.ClosestPoints(ms1, ms2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out hit1, out hit2, out normal)) { JVector delta = hit2 - hit1; if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared()) { penetration = delta * normal; if (penetration < 0.0f) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } } } ms1.ReturnWorkingClone(); ms2.ReturnWorkingClone(); } else { RigidBody b1, b2; if (body2.Shape is Multishape) { // CUSTOM: Swapped callbacks here as well. b1 = body2; b2 = body1; // A proper swap here (using a temporary variable) isn't necessary since, by this point, the other // callback (attached to the static body) is intentionally not used. callback2 = callback1; } else { b2 = body2; b1 = body1; } Multishape ms = (b1.Shape as Multishape); ms = ms.RequestWorkingClone(); JBBox transformedBoundingBox = b2.boundingBox; transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation); int msLength = ms.Prepare(ref transformedBoundingBox); if (msLength == 0) { ms.ReturnWorkingClone(); return; } for (int i = 0; i < msLength; i++) { ms.SetCurrentShape(i); // CUSTOM: Added this callback (to allow specific triangle collisions to be ignored). bool shouldCollideWith = ms is TriangleMeshShape tMesh && (callback2 == null || callback2(b1, tMesh.CurrentTriangle)); if (shouldCollideWith && XenoCollide.Detect(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out point, out normal, out penetration)) { JVector[] triangle = null; FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out var point1, out var point2); if (useTerrainNormal && ms is TerrainShape) { (ms as TerrainShape).CollisionNormal(out normal); JVector.Transform(ref normal, ref b1.orientation, out normal); } else if (useTriangleMeshNormal) { tMesh = ms as TriangleMeshShape; triangle = tMesh.CurrentTriangle; tMesh.CollisionNormal(out normal); JVector.Transform(ref normal, ref b1.orientation, out normal); } RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, triangle, penetration); } else if (speculative) { JVector hit1, hit2; if (GJKCollide.ClosestPoints(ms, b2.Shape, ref b1.orientation, ref b2.orientation, ref b1.position, ref b2.position, out hit1, out hit2, out normal)) { JVector delta = hit2 - hit1; if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared()) { penetration = delta * normal; if (penetration < 0.0f) { RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration); } } } } } ms.ReturnWorkingClone(); } }