public void Insert(Rigidbody rb)
{
if (m_children != null)
{
for (int i = 0; i < 4; ++i)
{
if (Intersections.DoIntersectAABBAABB(rb.ColliderData.GlobalAABB, m_children[i].f_aabb))
{
m_children[i].Insert(rb);
}
}
}
else if (f_depth < MAX_DEPTH && f_elements.Count == MAX_ELEMENTS)
{
// This means I have to split the QT
m_children = new QuadTree[4];
m_children[0] = QuadTree.New(f_aabb.BottomLeftAABB, f_depth + 1);
m_children[1] = QuadTree.New(f_aabb.BottomRightAABB, f_depth + 1);
m_children[2] = QuadTree.New(f_aabb.TopLeftAABB, f_depth + 1);
m_children[3] = QuadTree.New(f_aabb.TopRightAABB, f_depth + 1);
for (int i = 0; i < f_elements.Count; ++i)
{
Insert(f_elements[i]);
}
Insert(rb);
f_elements.Clear();
s_elementCount -= MAX_ELEMENTS;
}
else
{
if (f_elements.Count == 0)
{
--s_emptyNodes;
}
f_elements.Add(rb);
++s_elementCount;
}
}
public void Remove(Rigidbody rb, AABB oldAABB)
{
if (m_children == null)
{
f_elements.Remove(rb);
if (f_elements.Count == 0)
{
++s_emptyNodes;
}
--s_elementCount;
}
else
{
for (int i = 0; i < 4; ++i)
{
if (Intersections.DoIntersectAABBAABB(oldAABB, m_children[i].f_aabb))
{
m_children[i].Remove(rb, oldAABB);
}
}
/* Check if all children are empty now and therefore delete them again */
{
for (int i = 0; i < 4; ++i)
{
if (!(m_children[i].f_elements.Count == 0 && m_children[i].m_children == null))
{
return;
}
}
for (int i = 0; i < 4; ++i)
{
Release(m_children[i]);
}
m_children = null;
}
}
}
public void Move(Rigidbody rigidbody, AABB oldAABB, AABB newAABB)
{
if (m_children == null)
{
bool intersectOld = (Intersections.DoIntersectAABBAABB(oldAABB, f_aabb));
bool intersectNew = (Intersections.DoIntersectAABBAABB(newAABB, f_aabb));
if (intersectOld && !intersectNew)
{
Remove(rigidbody, oldAABB);
}
else if (!intersectOld && intersectNew)
{
Insert(rigidbody);
}
}
else
{
for (int i = 0; i < 4; ++i)
{
bool intersectOld = (Intersections.DoIntersectAABBAABB(oldAABB, m_children[i].f_aabb));
bool intersectNew = (Intersections.DoIntersectAABBAABB(newAABB, m_children[i].f_aabb));
if (intersectOld && !intersectNew)
{
m_children[i].Remove(rigidbody, oldAABB);
}
else if (!intersectOld && intersectNew)
{
m_children[i].Insert(rigidbody);
}
else if (intersectOld && intersectNew /*&& !(oldAABB.ContainsCompletely(m_children[i].f_aabb) && newAABB.ContainsCompletely(m_children[i].f_aabb))*/)
{
m_children[i].Move(rigidbody, oldAABB, newAABB);
}
}
}
}
/// <summary>
/// Returns an Enumerable with intersecting rbs
/// Normally, do not use this
/// </summary>
public IEnumerable <Rigidbody> IntersectAll(AABB aabb)
{
if (m_children == null)
{
foreach (Rigidbody innerRB in f_elements)
{
yield return(innerRB);
}
}
else
{
for (int i = 0; i < 4; ++i)
{
if (Intersections.DoIntersectAABBAABB(aabb, m_children[i].f_aabb))
{
foreach (Rigidbody innerRB in m_children[i].IntersectAll(aabb))
{
yield return(innerRB);
}
}
}
}
}
protected void NarrowPhase(Rigidbody rb1, Rigidbody rb2)
{
#if DEBUG_COLLISIONS
f_checkStates[CalcRBID(rb1, rb2)] = CheckState.NarrowCheck;
++m_narrowChecks;
#endif
// If none of them are triggers, that means that the collision shall be resolved
// Well, they cannot be resolved here already; the data has to be remembered until the end of the resolutions
if (!rb1.ColliderData.IsTrigger && !rb2.ColliderData.IsTrigger)
{
// TODO; Implement the other collisions
if (rb1.ColliderData.Collider2D is AABB)
{
if (rb2.ColliderData.Collider2D is AABB)
{
// this should not be possible is this example
}
else
{
// AABB - Circle
}
}
else
{
if (rb2.ColliderData.Collider2D is AABB)
{
// Circle - AABB
}
else
{
// Circle - Circle
CollisionDataRB cd = Intersections.CollisionCircleCircle((Circle)rb1.ColliderData.Collider2D, (Circle)rb2.ColliderData.Collider2D, rb1, rb2);
if (cd.DidCollide)
{
//UnityEngine.Debug.Log(Name + ": " + (rb1.ID / 4) + " - " + (rb2.ID / 4));
m_collisionsThisFrame[CalcRBID(rb1, rb2)] = cd;
m_collisionsLastFrame.Remove(CalcRBID(rb1, rb2));
#if DEBUG_COLLISIONS
f_checkStates[CalcRBID(rb1, rb2)] = CheckState.Collision;
++m_collisions;
#endif
//TESTING
// this is so bad, that I only keep it here, to not think it would be nice
// As stated above, THIS SHOULD NOT HAPPEN HERE!!!!!
// Also, this now triggers a collision although the collision is considered as not happening
Velocity v1 = rb1.DynamicData.Velocity;
Velocity v2 = rb2.DynamicData.Velocity;
//if (Velocity.Angle(v1, v2) < new Angle(90)) {
if (!m_collisionsLastFrame.ContainsKey(CalcRBID(rb1, rb2)))
{
Mass m1 = rb1.ObjectData.Mass;
Mass m2 = rb2.ObjectData.Mass;
rb1.DynamicData = new DynamicData((v1 * (m1 - m2) + (2 * m2 * v2)) / (m1 + m2), Acceleration.zero);
rb2.DynamicData = new DynamicData((v2 * (m2 - m1) + (2 * m1 * v1)) / (m2 + m1), Acceleration.zero);
}
//TESTING END
}
}
}
// pass collision to the event
}
else // this means not both are colliders, so I only fire a trigger event on both (if touching)
// In a trigger, we only care, whether they are touching or not
// TODO I can already call the trigger functions here
{
if (rb1.ColliderData.Collider2D is AABB)
{
if (rb2.ColliderData.Collider2D is AABB)
{
// this should not be possible in this example
if (Intersections.DoIntersectAABBAABB((AABB)rb1.ColliderData.Collider2D, (AABB)rb2.ColliderData.Collider2D, rb1.GameObject.Transform, rb2.GameObject.Transform))
{
// call triggers for 1 and send 2
// call triggers for 2 and send 1
m_triggersThisFrame.Add(CalcRBID(rb1, rb2));
m_triggersLastFrame.Remove(CalcRBID(rb1, rb2));
#if DEBUG_COLLISIONS
f_checkStates[CalcRBID(rb1, rb2)] = CheckState.Trigger;
++m_triggers;
#endif
}
}
else
{
// AABB - Circle
if (Intersections.DoIntersectAABBCircle((AABB)rb1.ColliderData.Collider2D, (Circle)rb2.ColliderData.Collider2D, rb1.GameObject.Transform, rb2.GameObject.Transform))
{
// call triggers for 1 and send 2
// call triggers for 2 and send 1
m_triggersThisFrame.Add(CalcRBID(rb1, rb2));
m_triggersLastFrame.Remove(CalcRBID(rb1, rb2));
#if DEBUG_COLLISIONS
f_checkStates[CalcRBID(rb1, rb2)] = CheckState.Trigger;
++m_triggers;
#endif
}
}
}
else
{
if (rb2.ColliderData.Collider2D is AABB)
{
// Circle - AABB
if (Intersections.DoIntersectAABBCircle((AABB)rb2.ColliderData.Collider2D, (Circle)rb1.ColliderData.Collider2D, rb2.GameObject.Transform, rb1.GameObject.Transform))
{
// call triggers for 1 and send 2
// call triggers for 2 and send 1
m_triggersThisFrame.Add(CalcRBID(rb1, rb2));
m_triggersLastFrame.Remove(CalcRBID(rb1, rb2));
#if DEBUG_COLLISIONS
f_checkStates[CalcRBID(rb1, rb2)] = CheckState.Trigger;
++m_triggers;
#endif
}
}
else
{
// Circle - Circle
if (Intersections.DoIntersectCircleCircle((Circle)rb1.ColliderData.Collider2D, (Circle)rb2.ColliderData.Collider2D, rb1.GameObject.Transform, rb2.GameObject.Transform))
{
// call triggers for 1 and send 2
// call triggers for 2 and send 1
m_triggersThisFrame.Add(CalcRBID(rb1, rb2));
m_triggersLastFrame.Remove(CalcRBID(rb1, rb2));
#if DEBUG_COLLISIONS
f_checkStates[CalcRBID(rb1, rb2)] = CheckState.Trigger;
++m_triggers;
#endif
}
}
}
// TODO only check if they are touching + calling trigger functions after that if any
// pass collider to the event
}
}
