How is my Rigid Body Collision code inconsistent?

Question

I've been trying to replicate the system found here: http://www.myphysicslab.com/collision.html

So far this is my code (for the case of hitting an infinitely massive object, like an immovable wall):

public static Orientation collide( float bodyMass_1, float bodyVelocityX_1, float bodyVelocityY_1, float bodyAngleMomentum_1 , float pointXFromCenterX_1, 
float pointYFromCenterY_1, float momentOfInertia_1,
float edgeNormalX, float edgeNormalY, float elasticity )
{
    Orientation returning;

    float outAngleMomentum_1;

    float outVelocityX_1, outVelocityY_1;

    float relativeNormal;
    float deltaParameter;

    float pointVelocityX_1, pointVelocityY_1;

    pointVelocityX_1 = bodyVelocityX_1 - bodyAngleMomentum_1 * pointYFromCenterY_1;
    pointVelocityY_1 = bodyVelocityY_1 + bodyAngleMomentum_1 * pointXFromCenterX_1;


    System.out.println( edgeNormalX );
    System.out.println( edgeNormalY );

    relativeNormal = (float) Funct3D.dotProduct( pointVelocityX_1, pointVelocityY_1,edgeNormalX, edgeNormalY );


    if ( relativeNormal < 0 )
    {

        deltaParameter = (-(1 + elasticity) *( (float) Funct3D.dotProduct( pointVelocityX_1, pointVelocityY_1, edgeNormalX, edgeNormalY ) ))/ 
        ( 1/bodyMass_1 + Funct3D.sqr( (float) Funct3D.perpDotProduct(pointXFromCenterX_1,pointYFromCenterY_1,edgeNormalX,edgeNormalY)) / momentOfInertia_1 );

        outVelocityX_1 = bodyVelocityX_1 + deltaParameter * edgeNormalX / bodyMass_1;
        outVelocityY_1 = bodyVelocityY_1 + deltaParameter * edgeNormalY / bodyMass_1;

        outAngleMomentum_1 = bodyAngleMomentum_1 + (float)     Funct3D.perpDotProduct(pointXFromCenterX_1, pointYFromCenterY_1,     deltaParameter*edgeNormalX, deltaParameter*edgeNormalY ) / momentOfInertia_1;

        returning = new Orientation( outVelocityX_1, outVelocityY_1, outAngleMomentum_1 );
    }
    else
    {
        System.out.println( "NO COLLISION" );
        returning = new Orientation( bodyVelocityX_1, bodyVelocityY_1, bodyAngleMomentum_1 );
    }
    return returning;
}

The results of my code often result in the velocity not reflecting through the normal, or just an entire lack of conservation of momentum. Even when I have it just have it output the results immediately after execution, the results show that it isn't a matter of registering the collision several times, it occurs on its first run as well.

dotProduct returns the dot product of two vectors.

perpDotProduct returns the perp dot product of two vectors

If you need anything explained, or if you'd like to request the code scope to be widened any, please feel free to ask.

Answer 1

For this kind of task I would recommend to write a simple test, where you pass input parameters into collide method and expect Orientation with values that are known to be valid.

You can use JUnit framework or simple main method:

Orientation orientation = collide(a, b, c, d, ...);
assertEquals(KNOWN_X, orientation.getX());
assertEquals(KNOWN_Y, orientation.getX());
assertEquals(KNOWN_MOMENTUM, orientation.getMomentum());

With this code you can debug your method and check that values are correct at each step of your algorithm. Then you can check algorithm with new set of known input and output values.

Also consider cleaning up your code and move repetitive calculations to local variables (i.e. deltaParameter * edgeNormalX)