Sweep and Prune - new BroadPhase

转自:http://www.codeplex.com/FarseerPhysics/Thread/View.aspx?ThreadId=16522


I did a quick look through your most recent code and was kind of shocked to find out you still didn’t have a more efficient Broad Phase. So I decided to write one for your engine during my break. I tried to make it simple instead of optimized for ease of understanding.

public   class  SweepAndPrune
{
    delegate void CollisionCallback(Wrapper w1, Wrapper w2);
 
    class Wrapper
    {
        public Node xBegin;
        public Node xEnd;
        public Node yBegin;
        public Node yEnd;
        public Geometry geometry;
        public List<Geometry> colliders;
        public Wrapper(Geometry geometry)
        {
            this.geometry = geometry;
            this.colliders = new List<Geometry>();
            this.xBegin = new Node(this, true);
            this.xEnd = new Node(this, false);
            this.yBegin = new Node(this, true);
            this.yEnd = new Node(this, false);
        }
        public void Update()
        {
            colliders.Clear();
            xBegin.value = geometry.AABB.Min.X;
            xEnd.value = geometry.AABB.Max.X;
            yBegin.value = geometry.AABB.Min.Y;
            yEnd.value = geometry.AABB.Max.Y;
        }
    }
 
    class Node
    {
        public bool begin;
        public float value;
        public Wrapper wrapper;
        public Node(Wrapper wrapper, bool begin)
        {
            this.wrapper = wrapper;
            this.begin = begin;
        }
    }
 
    List<Wrapper> wrappers = new List<Wrapper>();
    List<Node> xList = new List<Node>();
    List<Node> yList = new List<Node>();
 
    public void AddGeometry(Geometry item)
    {
        Wrapper wrapper = new Wrapper(item);
        wrappers.Add(wrapper);
        xList.Add(wrapper.xBegin);
        xList.Add(wrapper.xEnd);
        yList.Add(wrapper.yBegin);
        yList.Add(wrapper.yEnd);
    }
    public void RemoveDisposed()
    {
        if (wrappers.RemoveAll(delegate(Wrapper w) { return w.geometry.Body.IsDisposed; }) > 0)
        {
            xList.RemoveAll(delegate(Node n) { return n.wrapper.geometry.Body.IsDisposed; });
            yList.RemoveAll(delegate(Node n) { return n.wrapper.geometry.Body.IsDisposed; });
        }
    }
    public void Run()
    {
        Update();
        RunAxis(xList, HandleFirstCollision);
        RunAxis(yList, HandleSecondCollision);
    }
    /// <summary>
    /// Updates the nodes and sorts them.
    /// </summary>
    void Update()
    {
        foreach (Wrapper wrapper in wrappers)
        {
            wrapper.Update();
        }
        xList.Sort(delegate(Node l, Node r) { return l.value.CompareTo(r.value); });
        yList.Sort(delegate(Node l, Node r) { return l.value.CompareTo(r.value); });
    }
    /// <summary>
    /// Runs the collision detection on a axis
    /// </summary>
    void RunAxis(List<Node> list, CollisionCallback callback)
    {
        LinkedList<Wrapper> proximityList = new LinkedList<Wrapper>();
        foreach (Node node in list)
        {
            if (node.begin)
            {
                foreach (Wrapper wrapper in proximityList)
                {
                    callback(node.wrapper, wrapper);
                }
                proximityList.AddLast(node.wrapper);
            }
            else
            {
                proximityList.Remove(node.wrapper);
            }
        }
    }
    /// <summary>
    /// when there is a collsion along the first axis
    /// and if there is no early fail conditions then they are
    /// added to each others colliders list
    /// </summary>
    void HandleFirstCollision(Wrapper w1, Wrapper w2)
    {
        //if(early fail conditions) {return;}
        w1.colliders.Add(w2.geometry);
        w2.colliders.Add(w1.geometry);
    }
    /// <summary>
    /// when there is a collision along the second axis then 
    /// it checks to see if there was a collision along the first. 
    /// if there is then the 2 geometries bounding boxes are colliding.
    /// </summary>
    void HandleSecondCollision(Wrapper w1, Wrapper w2)
    {
        if (w1.colliders.Contains(w2.geometry))
        {
            //this is a confirmed broadphase collision
            //so add a new arbiter or something for 
            //w1.geometry and w2.geometry
        }
    }
}