I'm trying to get basic collision dynamics working

Question

I've simplified things down to cubes/a single cube colliding with an infinite-mass rectangle and the following code:

The problem is, the boxes tend to spin too much and get stuck together spinning and, if the binary search is included, just hit and spin a lot.

Thanks for any and all help.

/// <summary>
/// Projects an abstract 1D line "perpendicular" to the axis, 
/// stretching across the width of the model,
/// measured from that axis.
/// </summary>
/// <param name="Axis"></param>
/// <param name="Min"></param>
/// <param name="Max"></param>
protected virtual void ProjectToAxis(Vector2 Axis, IMotionData motionData, out double Min, out double Max)
{
    Double DotP = Axis.Dot(motionData.PositionGS + (this.Vertices[0].Position * this.Model.Scale).Rotate(motionData.RotationGS));

    Min = Max = DotP;

    for (int t = 1; t < this.Vertices.Count(); ++t)
    {
        DotP = Axis.Dot(motionData.PositionGS + (this.Vertices[t].Position * this.Model.Scale).Rotate(motionData.RotationGS));

        Min = Math.Min(DotP, Min);
        Max = Math.Max(DotP, Max);
    }
}


/// <summary>
/// Projects two imaginary lines even with each edge,
/// equal to the width of each object while looking at
/// that edge, then checks to see if they intersect.
/// </summary>
/// <param name="B1"></param>
/// <param name="B2"></param>
/// <returns></returns>
public static bool DetectCollision(Body B1, Body B2, Double elapsedSeconds)
{
    CollisionData collisionInfo = new CollisionData();
    double lowestDistance = double.MaxValue;
    double distance;

    Vector2 normalB1ToB2 = (B2.MotionHandler.PositionGS - B1.MotionHandler.PositionGS).Normalized;

    foreach (Edge edge in B1.Edges)
    {
        if (edge.Normal.RelativePosition.Dot(normalB1ToB2) >= 0.0)
        {
            double minA, minB, maxA, maxB;
            B1.ProjectToAxis(edge.Normal.RelativePosition, B1.MotionHandler.MotionDataGet, out minA, out maxA);
            B2.ProjectToAxis(edge.Normal.RelativePosition, B2.MotionHandler.MotionDataGet, out minB, out maxB);


            if (minA < minB)
                distance = minB - maxA;
            else
                distance = minA - maxB;

            if (distance > 0.0f)
                return false;
            else if (Math.Abs(distance) < lowestDistance)
            {
                lowestDistance = Math.Abs(distance);

                collisionInfo.Normal = edge.Normal.RelativePosition;
                collisionInfo.Edge = edge;
            }
        }
    }


    Vector2 normalB2ToB1 = -normalB1ToB2;


    foreach (Edge edge in B2.Edges)
    {
        if (edge.Normal.RelativePosition.Dot(normalB2ToB1) >= 0.0)
        {
            double minA, minB, maxA, maxB;
            B1.ProjectToAxis(edge.Normal.RelativePosition, B1.MotionHandler.MotionDataGet, out minA, out maxA);
            B2.ProjectToAxis(edge.Normal.RelativePosition, B2.MotionHandler.MotionDataGet, out minB, out maxB);

            if (minA < minB)
                distance = minB - maxA;
            else
                distance = minA - maxB;

            if (distance > 0.0f)
                return false;
            else if (Math.Abs(distance) < lowestDistance)
            {
                lowestDistance = Math.Abs(distance);

                collisionInfo.Normal = edge.Normal.RelativePosition;
                collisionInfo.Edge = edge;
            }
        }
    }


    collisionInfo.Depth = lowestDistance;


    /* Double lowHighSeconds = elapsedSeconds;
    Double highLowSeconds = 0.0;
    Double seconds;
    IMotionData md1;
    IMotionData md2;
    bool collision;
    do
    {
        md1 = B1.MotionHandler.MotionDataLastGet.Copy;
        md2 = B2.MotionHandler.MotionDataLastGet.Copy;

        collision = true;
        lowestDistance = Double.MaxValue;
        seconds = MathExtensions.MathExt.Lerp(highLowSeconds, lowHighSeconds, 0.5);

        B1.MotionHandler.Simulate(seconds, ref md1);
        B2.MotionHandler.Simulate(seconds, ref md2);


        normalB1ToB2 = (md2.PositionGS - md1.PositionGS).Normalized;

        foreach (Edge edge in B1.Edges)
        {
            if ((edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS).Dot(normalB1ToB2) >= 0.0)
            {
                double minA, minB, maxA, maxB;
                B1.ProjectToAxis((edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS), md1, out minA, out maxA);
                B2.ProjectToAxis((edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS), md2, out minB, out maxB);


                if (minA < minB)
                    distance = minB - maxA;
                else
                    distance = minA - maxB;

                if (distance > 0.0f)
                    collision = false;
                else if (Math.Abs(distance) < lowestDistance)
                {
                    lowestDistance = Math.Abs(distance);

                    collisionInfo.Normal = (edge.Normal.Position * B1.Model.Scale).Rotate(md1.RotationGS);
                    collisionInfo.Edge = edge;
                }
            }
        }


        normalB2ToB1 = -normalB1ToB2;


        foreach (Edge edge in B2.Edges)
        {
            if ((edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS).Dot(normalB2ToB1) >= 0.0)
            {
                double minA, minB, maxA, maxB;
                B2.ProjectToAxis((edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS), md2, out minA, out maxA);
                B1.ProjectToAxis((edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS), md1, out minB, out maxB);


                if (minA < minB)
                    distance = minB - maxA;
                else
                    distance = minA - maxB;

                if (distance > 0.0f)
                    collision = false;
                else if (Math.Abs(distance) < lowestDistance)
                {
                    lowestDistance = Math.Abs(distance);

                    collisionInfo.Normal = (edge.Normal.Position * B2.Model.Scale).Rotate(md2.RotationGS);
                    collisionInfo.Edge = edge;
                }
            }
        }

        collisionInfo.Depth = lowestDistance;

        if (!collision)
        {
            lowHighSeconds = seconds;
        }
        else
        {
            highLowSeconds = seconds;
        }
    } while (Math.Abs(highLowSeconds - lowHighSeconds) > 0.0001);

    B1.MotionHandler.MotionDataSet = md1;
    B2.MotionHandler.MotionDataSet = md2; */

    // bool flip = false;
    if (collisionInfo.Edge.Parent != B2.Model)
    {
        Body temp = B1;
        B1 = B2;
        B2 = temp;
    }


    //This is needed to make sure that the collision normal is pointing at B1
    int Sign = Math.Sign(
        collisionInfo.Normal.Dot(
            B1.MotionHandler.MotionDataGet.PositionGS + (B1.Center * B1.Model.Scale).Rotate(B1.MotionHandler.MotionDataGet.RotationGS) -
            B2.MotionHandler.MotionDataGet.PositionGS + (B2.Center * B2.Model.Scale).Rotate(B2.MotionHandler.MotionDataGet.RotationGS)
        )
    );

    //Remember that the line equation is N*( R - R0 ). We choose B2->Center 
    //as R0; the normal N is given by the collision normal

    if (Sign != 1)
        collisionInfo.Normal = -collisionInfo.Normal; //Revert the collision normal if it points away from B1


    double SmallestD = double.MaxValue; //Initialize the smallest distance to a high value
        //Measure the distance of the vertex from the line using the line equation
    for (int t = 0; t < B1.Vertices.Count(); ++t)
    {
        double Distance = collisionInfo.Normal.Dot(B1.Vertices[t].WorldPosition - B2.Center);

        // If the measured distance is smaller than the smallest distance reported 
        // so far, set the smallest distance and the collision vertex
        if (Distance < SmallestD)
        {
            SmallestD = Distance;
            collisionInfo.Vertex = B1.Vertices[t];
        }
    }


    if ((Body.CollisionType & CollisionType.Velocity) > 0)
    {
        Vector2 vab1 = B1.MotionHandler.VelocityGS - B2.MotionHandler.VelocityGS;

        Vector2 rap = (B1.MotionHandler.PositionGS - collisionInfo.Normal);
        Vector2 rbp = (B2.MotionHandler.PositionGS - collisionInfo.Normal);

        Double rap2 = (rap.Cross(collisionInfo.Normal));
        Double rbp2 = (rbp.Cross(collisionInfo.Normal));

        Vector2 one = (collisionInfo.Vertex.WorldPosition - B1.MotionHandler.PositionGS).GetPerpendicular;
        Vector2 two = (collisionInfo.Vertex.WorldPosition - B2.MotionHandler.PositionGS).GetPerpendicular;

        Double j = (-(1 + 0.0) * vab1.Dot(collisionInfo.Normal)) /
            ((collisionInfo.Normal.Dot(collisionInfo.Normal) * (B1.MotionHandler.InverseMassGS + B2.MotionHandler.InverseMassGS)) +
            (one.Dot(one) * B1.MotionHandler.InverseInertiaGS) + (two.Dot(two) * B2.MotionHandler.InverseInertiaGS));


        B1.MotionHandler.AddImpulse = new Force(
            collisionInfo.Normal,
            j /* ,
            one */
        );
        B2.MotionHandler.AddImpulse = new Force(
            collisionInfo.Normal,
            -(j) /* ,
            two */
        );


        NewtonianMotionData data1 = (NewtonianMotionData)B1.MotionHandler.MotionDataGet;
        NewtonianMotionData data2 = (NewtonianMotionData)B2.MotionHandler.MotionDataGet;

        data1.AngularVelocity += (one.Dot(j * collisionInfo.Normal)) * data1.inverseInertia;
        data2.AngularVelocity += (two.Dot(-j * collisionInfo.Normal)) * data2.inverseInertia;

        B1.MotionHandler.MotionDataSet = data1;
        B2.MotionHandler.MotionDataSet = data2;
    }

    return true;
}

Answer 1

You've got two problems.

1) There's something wrong with the code. You need to fix that.

2) You don't know how to figure out what "something" is.

Solving the first problem is gated on your solving the second problem. You need to learn how to debug a program you just wrote.

You've already tested it and gotten a result which you've identified as nonsensical. That's a good first step. Now break it down even farther. Pick a simple problem in this domain that you can solve yourself with pencil and paper; do so, and then watch your algorithm solve the same problem in the debugger, checking every step along the way. Listen to quiet nagging doubts. When there is anything that looks slightly off or unexpected, stop what you're doing and investigate the issue until you understand whether things are working correctly or not. Eventually you'll find a step where things aren't as they should be, and that's where the bug is.

Yes, this is tedious. When you've found the bug and fixed it, pause and reflect upon what caused you to write the bug in the first place, and figure out how to not write that kind of bug ever again.

UPDATE:

Re: your recent comments.

Apology accepted. Now calm down. You're never going to find this bug if you're this worked up. Your brain will not let you. Humans who are in a panicky, worked-up state lose the ability to reason. That's why fire doors open outwards; humans fleeing a burning building literally will not stop to think "I'm pushing on this door and its not opening, maybe I should try pulling". They just push harder. I suspect you are pushing harder.

Debugging requires rationality and careful attention to small details. If you're all worked up about this problem then that's going to go out the window and its just going to get worse. Take it from someone who has been there. We've all been there. It's a deeply frustrating thing to have caused a bug in your own program that you then cannot find.

The reason no one is helping you is because... well, let me list the set of preconditions that have to be met for me to help you with more than vague platitudes and suggestions of how to focus your debugging efforts:

1) I have to know something about simulation of 3d physics. I had a pretty decent grasp of the differential equations of simple of Newtonian mechanics in 1992, but I haven't used it since. And the equation of a damped driven spring is rather different than the equations of rigid body collisions. If I spent a couple weeks reviewing my notes I could get the math back, but that's not realistic. You need someone who is deeply conversant right now with 3d collision physics simulations.

2) I have to be able to read and understand your code, code which is hundreds of lines long, written by someone other than me, to solve a problem I'm not familiar with. Worse, a hundred lines of that code is commented out. Why? Is it relevant? Is the bug in there? Moreover, I need to be able to read and understand the code without running it in a debugger. Heck, I can't even compile that code. It depends on libraries that I don't have.

And even worse still, one of those libraries might contain the bug. For all I know, the bug is a typo in some code that calculates a normal somewhere that you haven't shown us. The code shown could be perfect.

3) I need to have the free time to work on someone else's hard problem; a problem that the person who wrote the code and understands the physics is making no headway on.

All of these are requirements; if any one of them is missing, the reader cannot effectively help you. You're asking people you don't know to help you find a black cat in a dark warehouse at midnight without a flashlight -- a cat that might not even be there. It's not surprising you're getting few takers. Of the 74 stack overflow users who have read your question, how many of them meet all three requirements? I meet none of them.

If you want help on this site then post an easier problem. Narrow the problem down to a problem that requires less special knowledge of physics and simulation algorithms and has only the relevant code, preferably code that can be compiled and run.