Collision Resolution: Objects "swap" places

Question

I am working on making my own simple collision resolution class so I can learn a bit about how it works, while also improving my entity-component model framework. For those of you unfamiliar with a entity-component model, read this, it's a super fascinating way to build games very quickly and focus on code reuseability. I have a algorithm which I got from this tutorial that worked pretty well in my stand-alone tests but, when I implemented them into my framework, it can do some wonky things.

The resolution will work 9/10 but, there are cases where it does bugs, like when the objects "swap" places. You can see the bug in my video here -> http://www.youtube.com/watch?v=7tGDylY52Wc

Here is the `CollisionHandler class which deals with the pairing and resolution. This is a service, which is a non-entity way of managing entities, components, or anything you need in a state.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using EntityEngineV4.Collision.Shapes;
using EntityEngineV4.Engine;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace EntityEngineV4.Collision
{
    public class CollisionHandler : Service
    {
        private List<Collision> _collideables;
        private HashSet<Pair> _pairs;
        private HashSet<Manifold> _manifolds;

        public CollisionHandler(EntityState stateref) : base(stateref)
        {
            _collideables = new List<Collision>();
            _pairs = new HashSet<Pair>();
            _manifolds = new HashSet<Manifold>();
        }

        public override void Update(GameTime gt)
        {
            BroadPhase();
            foreach (var manifold in _manifolds)
            {
                manifold.A.OnCollision(manifold.B);
                manifold.B.OnCollision(manifold.A);

                //Attempt to resolve collisions
                if (CanObjectsResolve(manifold.A, manifold.B) || CanObjectsResolve(manifold.B, manifold.A))
                {
                    ResolveCollision(manifold);
                    PositionalCorrection(manifold);
                }

            }
        }

        public override void Draw(SpriteBatch sb)
        {
            _manifolds.Clear();
        }

        public void AddCollision(Collision c)
        {
            //Check if the Collision is already in the list.
            if (Enumerable.Contains(_collideables, c)) return;
            _collideables.Add(c);

            //Generate our pairs
            GeneratePairs();
        }

        public void GeneratePairs()
        {
            if (_collideables.Count() <= 1) return;

            _pairs.Clear();

            foreach (var a in _collideables)
            {
                foreach (var b in _collideables)
                {
                    if (a.Equals(b)) continue;
                    if (CanObjectsPair(a, b))
                    {
                        var p = new Pair(a,b);
                        _pairs.Add(p);
                    }
                }
            }
        }

        public void BroadPhase()
        {
            //Do a basic SAT test
            foreach (var pair in _pairs)
            {
                Vector2 normal = pair.A.Position - pair.B.Position;

                //Calculate half widths
                float aExtent = pair.A.BoundingRect.Width / 2f;
                float bExtent = pair.B.BoundingRect.Width / 2f;

                //Calculate the overlap. 
                float xExtent = aExtent + bExtent - Math.Abs(normal.X);

                //If the overlap is greater than 0
                if (xExtent > 0)
                {
                    //Calculate half widths
                    aExtent = pair.A.BoundingRect.Height / 2f;
                    bExtent = pair.B.BoundingRect.Height / 2f;

                    //Calculate overlap
                    float yExtent = aExtent + bExtent - Math.Abs(normal.Y);

                    if (yExtent > 0)
                    {
                        //Do our real test now.
                        Manifold m = CheckCollision(pair.A.Shape, pair.B.Shape);
                        if (m.AreColliding)
                            _manifolds.Add(m);
                    }
                }
            }
        }

        //Static methods

        /// <summary>
        /// Compares the masks and checks to see if they should be allowed to form a pair.
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns>Whether or not the the two objects should be paired</returns>
        public static bool CanObjectsPair(Collision a, Collision b)
        {
            return a.GroupMask.HasMatchingBit(b.GroupMask) || //Compare the group masks.
                   a.GroupMask.HasMatchingBit(b.PairMask) || //Compare the pair masks to the group masks.
                    a.PairMask.HasMatchingBit(b.GroupMask);
        }

        public static bool CanObjectsResolve(Collision resolver, Collision other)
        {
            return resolver.ResolutionGroupMask.HasMatchingBit(other.ResolutionGroupMask) || //Compare the group masks.
                    resolver.ResolutionPairMask.HasMatchingBit(other.ResolutionGroupMask);
        }

        public static void ResolveCollision(Manifold m)
        {
            Vector2 relVelocity = m.B.Velocity - m.A.Velocity;
            //Finds out if the objects are moving towards each other.
            //We only need to resolve collisions that are moving towards, not away.
            float velAlongNormal = PhysicsMath.DotProduct(relVelocity, m.Normal);
            if (velAlongNormal > 0)
                return;
            float e = Math.Min(m.A.Restitution, m.B.Restitution);

            float j = -(1 + e) * velAlongNormal;
            j /= m.A.InvertedMass + m.B.InvertedMass;

            Vector2 impulse = j * m.Normal;
            if (CanObjectsResolve(m.A, m.B))
                m.A.Velocity -= m.A.InvertedMass * impulse;
            if(CanObjectsResolve(m.B, m.A))
                m.B.Velocity += m.B.InvertedMass * impulse;
        }

        public static void PositionalCorrection(Manifold m)
        {
            const float percent = 0.2f;
            const float slop = 0.01f;
            Vector2 correction = Math.Max(m.PenetrationDepth - slop, 0.0f) / (m.A.InvertedMass + m.B.InvertedMass) * percent * m.Normal;
            m.A.Position -= m.A.InvertedMass * correction;
            m.B.Position += m.B.InvertedMass * correction;
        }

        /// <summary>
        /// Compares bounding boxes using Seperating Axis Thereom. 
        /// </summary>
        public static Manifold AABBvsAABB(AABB a, AABB b)
        {
            //Start packing the manifold
            Manifold m = new Manifold(a.Collision, b.Collision);
            m.Normal = a.Position - b.Position;

            //Calculate half widths
            float aExtent = a.Width / 2f;
            float bExtent = b.Width / 2f;

            //Calculate the overlap. 
            float xExtent = aExtent + bExtent - Math.Abs(m.Normal.X);

            //If the overlap is greater than 0
            if (xExtent > 0)
            {
                //Calculate half widths
                aExtent = a.Height / 2f;
                bExtent = b.Height / 2f;

                //Calculate overlap
                float yExtent = aExtent + bExtent - Math.Abs(m.Normal.Y);

                if (yExtent > 0)
                {
                                        //Find which axis has the biggest penetration ;D
                    Vector2 fixnormal;
                    if (xExtent > yExtent){
                        if(m.Normal.X < 0)
                            fixnormal = -Vector2.UnitX;
                        else
                            fixnormal = Vector2.UnitX;

                        m.Normal = PhysicsMath.GetNormal(a.Position, b.Position) * fixnormal.X;
                        m.PenetrationDepth = xExtent;   
                    }
                    else {
                        if(m.Normal.Y < 0)
                             fixnormal = -Vector2.UnitY;
                        else
                            fixnormal= Vector2.UnitY;
                        m.Normal = PhysicsMath.GetNormal(a.Position, b.Position) * fixnormal.Y;
                        m.PenetrationDepth = yExtent;
                    }
                    m.AreColliding = true;
                    return m;
                }
            }
            m.AreColliding = false;
            return m;
        }

        //Collision resolver methods

        public static Manifold CheckCollision(Shape a, Shape b)
        {
            return collide((dynamic) a, (dynamic) b);
        }

        private static Manifold collide(AABB a, AABB b)
        {
            return AABBvsAABB(a, b);
        }

    }
}

Next is the Collision component. This holds some bitmasks which tell it what groups it is a part of(GroupMask), what groups it is allowed to pair with with out being a part of that group (PairMask), and who it should resolve collisions with, (ResolutionGroupMask) and finally, who it should resolve collisions with one-way (ResolutionPairMask). It also holds values related to detection and resolution, such as Shape, mass, and restitution.

Just as a quick side note, under the comment "Dependencies" there is a field for a Body and a Physics, both of these objects are components which are supplied to track the location of the collideable.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using EntityEngineV4.Collision.Shapes;
using EntityEngineV4.Components;
using EntityEngineV4.Engine;
using Microsoft.Xna.Framework;

namespace EntityEngineV4.Collision
{
    public class Collision : Component
    {
        //Delegates and events
        public delegate void EventHandler(Collision c);
        public event EventHandler CollideEvent;

        /// <summary>
        /// The group mask is the bit mask used to determine which groups the component is a part of.
        /// The CollisionHandler will pair all components with the same mask.
        /// </summary>
        /// <value>
        /// The group mask.
        /// </value>
        public Bitmask GroupMask { get; protected set; }

        /// <summary>
        /// The pair mask is the bit mask used to determine which groups the component will pair with.
        /// The CollisionHandler will only pair components whose group mask matches the pair mask.
        /// </summary>
        /// <value>
        /// The pair mask.
        /// </value>
        public Bitmask PairMask { get; protected set; }

        /// <summary>
        /// The resolution mask is the bit mask which will determine which groups will physically collide with each other
        /// </summary>
        public Bitmask ResolutionGroupMask { get; protected set; }

        /// <summary>
        /// The resolution mask is the bit mask which will determine which pairs will physically collide with each other
        /// </summary>
        public Bitmask ResolutionPairMask { get; protected set; }

        //Collision Related Values

        //// <summary>
        /// Backing field for Mass.
        /// </summary>
        private float _mass = 1f;
        /// <summary>
        /// The mass of the object.
        /// </summary>
        /// <value>
        /// The mass.
        /// </value>
        public float Mass
        {
            get { return _mass; }
            set
            {
                if (value < 0) throw new Exception("Mass cannot be less than zero!");
                _mass = value;

                if (Math.Abs(value - 0) < .00001f)
                    InvertedMass = 0;
                else
                    InvertedMass = 1 / _mass;
            }
        }

        /// <summary>
        /// Gets one divided by mass (1/mass).
        /// </summary>
        /// <value>
        /// The inverted mass.
        /// </value>
        public float InvertedMass { get; private set; }

        /// <summary>
        /// Bounciness of this object
        /// </summary>
        public float Restitution = 0f;

        public Shape Shape;

        //Dependencies
        private CollisionHandler _collisionHandler;
        private Body _collisionBody;
        private Physics _collisionPhysics;


        //Properties

        public Rectangle BoundingRect
        {
            get { return _collisionBody.BoundingRect; }
            set { _collisionBody.BoundingRect = value; }
        }

        public Vector2 Position
        {
            get { return _collisionBody.Position; }
            set { _collisionBody.Position = value; }
        }

        public Vector2 Bounds
        {
            get { return _collisionBody.Bounds; }
            set { _collisionBody.Bounds = value; }
        }

        public Vector2 Velocity
        {
            get { return _collisionPhysics.Velocity; }
            set { _collisionPhysics.Velocity = value; }
        }

        public Collision(Entity parent, string name, Shape shape, Body collisionBody) : base(parent, name)
        {
            _collisionBody = collisionBody;
            _collisionHandler = parent.StateRef.GetService<CollisionHandler>();

            _collisionPhysics = new Physics(Parent, name + ".Physics", _collisionBody);

            Shape = shape;
            Shape.Collision = this;

            GroupMask = new Bitmask();
            PairMask = new Bitmask();
            ResolutionGroupMask = new Bitmask();
            ResolutionPairMask = new Bitmask();
        }

        public Collision(Entity parent, string name, Shape shape, Body collisionBody, Physics collisionPhysics)
            : base(parent, name)
        {
            _collisionBody = collisionBody;
            _collisionHandler = parent.StateRef.GetService<CollisionHandler>();

            _collisionPhysics = collisionPhysics;

            Shape = shape;
            Shape.Collision = this;

            GroupMask = new Bitmask();
            PairMask = new Bitmask();
            ResolutionGroupMask = new Bitmask();
            ResolutionPairMask = new Bitmask();
        }

        public void OnCollision(Collision c)
        {
            if (CollideEvent != null)
                CollideEvent(c);
        }

        public void AddToHandler()
        {
            _collisionHandler.AddCollision(this);
        }
    }
}

Lastly, I felt this was pretty self-explanatory but, I feel I should include it in case of an error. This is the PhysicsMath class which just holds some simple methods for calculating dot products, normals, and distance.

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace EntityEngineV4.Engine
{
    public static class PhysicsMath
    {
        public static float DotProduct(Vector2 a, Vector2 b)
        {
            return a.X * b.X + a.Y * b.Y;
        }

        public static float Distance(Vector2 a, Vector2 b)
        {
            return (float)Math.Sqrt(((int)(a.X - b.X) ^ 2 + (int)(a.Y - b.Y) ^ 2));
        }

        public static Vector2 GetNormal(Vector2 a, Vector2 b)
        {
            Vector2 ret = b - a;
            ret.Normalize();
            return ret;
        }
    }
}

Any help is greatly appreciated.

Store the complete state of both objects in question each frame to debug this until the bug appears. Then use this frame data as input and debug this frame as often as you want to without the need of waiting until it happens, since you can just replay the recorded data. Use your debugger and step through the code. — Maik Semder
– Maik Semder, Commented Jun 11, 2013 at 8:23

gareththegeek · Accepted Answer · 2013-06-12 20:19:18Z

I think the problem is that in one frame the objects move fast enough to pass through each other. Behold my ascii art:

Frame 1:

a is heading towards b

+-------+ +-------+
|   A   | |   B   |
+-------+ +-------+

Frame 2:

    +----+--+----+
    |   B|  |A   |
    +----+--+----+

At this point the collision response moves them away from each other but the centre of A is now to the right of B so they will swap places.

I think you need to use a volume sweeping technique to determine the position of the two objects at the time of collision. The time of collision is somewhere between frame 1 and frame 2 e.g. 1.3 frames...

Edit

In 2D where you only have to prevent the objects from crossing over you can probably do something like this:

At the beginning of the frame (t0), object A is at Position A0 and object B is at position B0. At the end of the frame (t1), A is at A1 and B is at B1. Assuming there is a collision then at time t (where t0 < t < t1) A and B are at the same position i.e. At == Bt.

Given constant velocities for A and B of vA and vB we have:

A0 + vA * t = B0 + vB * t
A0 - B0 = vB * t - vA * t
A0 - B0 = t * (vB - vA)

(A0 - B0) / (vB - vA) = t

Assuming I haven't had a maths fail, this should give you the time in the frame at which collision occured.

Edit 2

Ok, so the maths above is a bit simplistic as it does not include the area of the colliders. I dug out a bit of code from my old 3D collision detection library but I can't remember where I got it from or how it works. The simple explaination is that it forms simultaneous equations for the two objects motion during the frame and attempts to solve for time. There are potentially two collision times, when the objects first moved into collision and when they left collision. You obviously want the lower of the two solutions.

This code is for 3D swept volume involving spheres so you will need to adapt it:

protected bool SphereSweepTest(
        Vector3 a0, // A's starting position
        Vector3 a1, // A's finishing position
        Vector3 b0, // B's starting position
        Vector3 b1, // B's finishing position
        float rab, // Sum of the two sphere's radii
        out float t) // Earliest time of collision
    {
        // Form and solve a quadratic equation to determine if the 
        // two spheres will cross paths and if so when in normalised time
        // Normalised time means that 0.0 is the start of this frame
        // and 1.0 is the end.  Obviously a collision must occur between
        // 0.0 and 1.0 otherwise the collision is outside this frame, the
        // spheres haven't collided, they are simply on a collision course.

        Vector3 va = a1 - a0;
        Vector3 vb = b1 - b0;

        Vector3 AB = b0 - a0;
        Vector3 vab = vb - va;

        //float ABdot = Vector3.Dot(AB, AB);
        float ABdot = AB.X * AB.X + AB.Y * AB.Y + AB.Z * AB.Z;
        float rabrab = rab * rab;

        //float a = Vector3.Dot(vab, vab);
        float a = vab.X * vab.X + vab.Y * vab.Y + vab.Z * vab.Z;
        float b = 2.0f * (vab.X * AB.X + vab.Y * AB.Y + vab.Z * AB.Z);
        float c = ABdot - rabrab;

        float q = b * b - 4.0f * a * c;
        if (q > 0.0f)
        {
            float sq = (float)Math.Sqrt(q);
            float d = 1.0f / (2.0f * a);
            float u0 = (-b + sq) * d;
            float u1 = (-b - sq) * d;

            // Start and end are outside current time frame
            if ((u0 < 0.0f && u1 < 0.0f) || (u0 > 1.0f && u1 > 1.0f))
            {
                t = -1.0f;
                return false;
            }

            // Make sure start time is less than end time
            if (u0 > u1)
            {
                float ut = u0;
                u0 = u1; u1 = ut;
            }

            // Interpolate time
            t = u0 + u1 - u0;

            // Make sure time is inside current frame - clamp to 1
            // No need to clamp to 0 since the objects cannot begin 
            // the frame in collision
            if (t > 1.0f) t = 1.0f;

            return true;
        }

        t = -1.0f;
        return false;
    }

Hmm, I'm planning on adding linear interpolation into the mix, would linear interpolation mess with this? Could you give me any links you might have on volume sweeping? Maybe even a code example? — redcodefinal
– redcodefinal, Commented Jun 7, 2013 at 16:34
Hmm, ok makes sense. I'll see what I can whip up in code tonight. — redcodefinal
– redcodefinal, Commented Jun 10, 2013 at 14:27
while checking the math I noticed that we infer that A and B are at the same position at the time of collision. I'm not sure how this is possible seeing that the closest A could be to B is determined by the bounds of the two objects. could you please elaborate on why this is true? — redcodefinal
– redcodefinal, Commented Jun 10, 2013 at 19:21
You're right, I over-simplified things. This is why I was referring to swept volume in my original answer. This boils down to representing each square as a circle and creating a shape which contains all positions of the circle during the frame (a kind of rectangle with two round ends). You can then check if this shape intersects the other squares and find the time of intersection. I can't find the maths for it now but I think I have them at home. I will try and dig them out later. — gareththegeek
– gareththegeek, Commented Jun 11, 2013 at 9:34

Stack Exchange Network

Collision Resolution: Objects "swap" places

1 Answer 1

You must log in to answer this question.

Linked

Hot Network Questions