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 } }