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);
