private void InsertLeaf(int leaf) { ++_insertionCount; if (_root == NullNode) { _root = leaf; _nodes[_root].ParentOrNext = NullNode; return; } // Find the best sibling for this node TSBBox 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); TSBBox.CreateMerged(ref _nodes[sibling].AABB, ref leafAABB, out _nodes[sibling].AABB); _nodes[sibling].LeafCount += 1; FP siblingArea = _nodes[sibling].AABB.Perimeter; TSBBox parentAABB = new TSBBox(); //parentAABB.Combine(ref _nodes[sibling].AABB, ref leafAABB); TSBBox.CreateMerged(ref _nodes[sibling].AABB, ref leafAABB, out _nodes[sibling].AABB); FP parentArea = parentAABB.Perimeter; FP cost1 = (2 * FP.One) * parentArea; FP inheritanceCost = (2 * FP.One) * (parentArea - siblingArea); FP cost2; if (_nodes[child1].IsLeaf()) { TSBBox aabb = new TSBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); TSBBox.CreateMerged(ref leafAABB, ref _nodes[child1].AABB, out aabb); cost2 = aabb.Perimeter + inheritanceCost; } else { TSBBox aabb = new TSBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child1].AABB); TSBBox.CreateMerged(ref leafAABB, ref _nodes[child1].AABB, out aabb); FP oldArea = _nodes[child1].AABB.Perimeter; FP newArea = aabb.Perimeter; cost2 = (newArea - oldArea) + inheritanceCost; } FP cost3; if (_nodes[child2].IsLeaf()) { TSBBox aabb = new TSBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); TSBBox.CreateMerged(ref leafAABB, ref _nodes[child2].AABB, out aabb); cost3 = aabb.Perimeter + inheritanceCost; } else { TSBBox aabb = new TSBBox(); //aabb.Combine(ref leafAABB, ref _nodes[child2].AABB); TSBBox.CreateMerged(ref leafAABB, ref _nodes[child2].AABB, out aabb); FP oldArea = _nodes[child2].AABB.Perimeter; FP 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); TSBBox.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); TSBBox.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> /// Builds the octree. /// </summary> #region public void BuildOctree() public void BuildOctree() { // create tri and tri bounding box arrays triBoxes = new TSBBox[tris.Length]; // create an infinite size root box rootNodeBox = new TSBBox(new TSVector(FP.PositiveInfinity, FP.PositiveInfinity, FP.PositiveInfinity), new TSVector(FP.NegativeInfinity, FP.NegativeInfinity, FP.NegativeInfinity)); for (int i = 0; i < tris.Length; i++) { TSVector.Min(ref positions[tris[i].I1], ref positions[tris[i].I2], out triBoxes[i].min); TSVector.Min(ref positions[tris[i].I0], ref triBoxes[i].min, out triBoxes[i].min); TSVector.Max(ref positions[tris[i].I1], ref positions[tris[i].I2], out triBoxes[i].max); TSVector.Max(ref positions[tris[i].I0], ref triBoxes[i].max, out triBoxes[i].max); // get size of the root box TSVector.Min(ref rootNodeBox.min, ref triBoxes[i].min, out rootNodeBox.min); TSVector.Max(ref rootNodeBox.max, ref triBoxes[i].max, out rootNodeBox.max); } List <BuildNode> buildNodes = new List <BuildNode>(); buildNodes.Add(new BuildNode()); buildNodes[0].box = rootNodeBox; TSBBox[] children = new TSBBox[8]; for (int triNum = 0; triNum < tris.Length; triNum++) { int nodeIndex = 0; TSBBox box = rootNodeBox; while (box.Contains(ref triBoxes[triNum]) == TSBBox.ContainmentType.Contains) { int childCon = -1; for (int i = 0; i < 8; ++i) { CreateAABox(ref box, (EChild)i, out children[i]); if (children[i].Contains(ref triBoxes[triNum]) == TSBBox.ContainmentType.Contains) { // this box contains the tri, it can be the only one that does, // so we can stop our child search now and recurse into it childCon = i; break; } } // no child contains this tri completely, so it belong in this node if (childCon == -1) { buildNodes[nodeIndex].triIndices.Add(triNum); break; } else { // do we already have this child 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) { // nope create child BuildNode parentNode = buildNodes[nodeIndex]; BuildNode newNode = new BuildNode(); newNode.childType = childCon; newNode.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]; } } } } // now convert to the tighter Node from BuildNodes nodes = new Node[buildNodes.Count]; nodeStackPool = new ArrayResourcePool <ushort>(buildNodes.Count); //nodeStack = new UInt16[buildNodes.Count]; for (int i = 0; i < nodes.Length; i++) { nodes[i].nodeIndices = new UInt16[buildNodes[i].nodeIndices.Count]; for (int index = 0; index < nodes[i].nodeIndices.Length; ++index) { nodes[i].nodeIndices[index] = (UInt16)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(); buildNodes = null; }
/// <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 TSBBox fatAABB) { Debug.Assert(0 <= proxyId && proxyId < _nodeCapacity); fatAABB = _nodes[proxyId].AABB; }
/// <summary> /// Checks whether another bounding box is inside, outside or intersecting /// this box. /// </summary> /// <param name="box">The other bounding box to check.</param> /// <returns>The ContainmentType of the box.</returns> #region public ContainmentType Contains(JBBox box) public ContainmentType Contains(TSBBox box) { return(this.Contains(ref box)); }
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody) { if (rigidBody.Shape is Multishape) { Multishape ms = (rigidBody.Shape as Multishape); ms = ms.RequestWorkingClone(); TSBBox 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); TSVector point, normal; FP penetration; bool result; for (int e = 0; e < msLength; e++) { ms.SetCurrentShape(e); result = XenoCollide.Detect(ms, t, ref rigidBody.orientation, ref TSMatrix.InternalIdentity, ref rigidBody.position, ref TSVector.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); TSVector point, normal; FP penetration; bool result; result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref TSMatrix.InternalIdentity, ref rigidBody.position, ref TSVector.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); TSVector point, normal; FP penetration; CBFrame.Utils.Logger.Debug("line271 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity); 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)) { //normal = JVector.Up; //UnityEngine.Debug.Log("FINAL --- >>> normal: " + normal); TSVector 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) { TSVector 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)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration); } } } } //UnityEngine.Debug.Log("-----------------------: " + normal); } else if (b1IsMulti && b2IsMulti) { Multishape ms1 = (body1.Shape as Multishape); Multishape ms2 = (body2.Shape as Multishape); ms1 = ms1.RequestWorkingClone(); ms2 = ms2.RequestWorkingClone(); TSBBox 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)) { TSVector 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) { TSVector 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)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { 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(); TSBBox 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)) { TSVector 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); TSVector.Transform(ref normal, ref b1.orientation, out normal); } else if (useTriangleMeshNormal && ms is TriangleMeshShape) { (ms as TriangleMeshShape).CollisionNormal(out normal); TSVector.Transform(ref normal, ref b1.orientation, out normal); } RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration); } else if (speculative) { TSVector 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)) { TSVector delta = hit2 - hit1; if (delta.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude) { penetration = delta * normal; if (penetration < FP.Zero) { RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration); } } } } } ms.ReturnWorkingClone(); } CBFrame.Utils.Logger.Debug("line451 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity); }
/// <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 TSBBox box);