Rotation won't work in Java physics engine

Question

I am making a java rigid body physics engine, and it has gone great so far, until I tried to implement rotation. I don't know where the problem is coming from. I have methods calculating the moment of inertia of convex polygons and circles using formulas from these websites:

http://lab.polygonal.de/?p=57

http://en.wikipedia.org/wiki/List_of_moments_of_inertia

This is the code for the polygon moment of inertia:

public float momentOfInertia() {
    Vector C = centerOfMass().subtract(position);   //center of mass
    Line[] sides = sides();                         //sides of the polygon
    float moi = 0;                                  //moment of inertia
    for(int i = 0; i < sides.length; i++) {
        Line l = sides[i];                          //current side of polygon being looped through
        Vector p1 = C;                              //points 1, 2, and 3 are the points of the triangle
        Vector p2 = l.point1;
        Vector p3 = l.point2;
        Vector Cp = p1.add(p2).add(p3).divide(3);   //center of mass of the triangle, or C'
        float d = new Line(C, Cp).length();         //distance between center of mass
        Vector bv = p2.subtract(p1);                //vector for side b of triangle
        float b = bv.magnitude();                   //scalar for length of side b
        Vector u = bv.divide(b);                    //unit vector for side b
        Vector cv = p3.subtract(p1);                //vector for side c of triangle, only used to calculate variables a and h
        float a = cv.dot(u);                        //length of a in triangle
        Vector av = u.multiply(a);                  //vector for a in triangle
        Vector hv = cv.subtract(av);                //vector for height of triangle, or h in diagram
        float h = hv.magnitude();                   //length of height of triangle, or h in diagram
        float I = ((b*b*b*h)-(b*b*h*a)+(b*h*a*a)+(b*h*h*h))/36;     //calculate moment of inertia of individual triangle
        float M = (b*h)/2;                          //mass or area of triangle
        moi += I+M*d*d;                             //equation in sigma series of website
    }
    return moi;
}

And this is for the circle:

public float momentOfInertia() {
    return (float) Math.pow(radius, 2)*area()/2;
}

I know for a fact that the area functions work fine, I have checked them. I just don't know how to check if the moment of inertia equations are wrong.

For collision detection, I used the separating axis theorem to check for any combination of two polygons and circles, where it can find out whether they are colliding, the normal velocity of the collision, and the contact point of the collision. These methods all work beautifully.

I might also like to say how positions are organized. Every body has a position and a shape, either a polygon or a circle. Each shape has a position, and polygons have individual vertices. So if I want to find the absolute position of a vertex of a polygon-shaped body, I need to add the positions of the body, the polygon, and the vertex itself. The center of mass equation is in absolute position according to the shape, with no account for the body. The center of mass and moment of inertia methods are in the Shape class.

For every body, the constants are being updated according to the force and torque in the body's update method where dt is delta time. I also rotate the polygon based on the difference in rotation, because the vertices are ever changing.

public void update(float dt) {
    if(mass != 0) {
        momentum = momentum.add(force.multiply(dt));
        velocity = momentum.divide(mass);
        position = position.add(velocity.multiply(dt));
        angularMomentum += torque*dt;
        angularVelocity = angularMomentum/momentOfInertia;
        angle += angularVelocity*dt;
        shape.rotate(angularVelocity*dt);
    }
}

Finally, I also have a CollisionResolver class which fixes the collision of two colliding bodies, involving applying the normal force and friction. Here is the class's only method which does all of this:

public static void resolveCollision(Body a, Body b, float dt) {
    //calculate normal vector
    Vector norm = CollisionDetector.normal(a, b);
    Vector normb = norm.multiply(-1);
    //undo overlap between bodies
    float ratio1 = a.mass/(a.mass+b.mass);
    float ratio2 = b.mass/(b.mass+a.mass);
    a.position = a.position.add(norm.multiply(ratio1));
    b.position = b.position.add(normb.multiply(ratio2));
    //calculate contact point of collision and other values needed for rotation
    Vector cp = CollisionDetector.contactPoint(a, b, norm);
    Vector c = a.shape.centerOfMass().add(a.position);
    Vector cb = b.shape.centerOfMass().add(b.position);
    Vector d = cp.subtract(c);
    Vector db = cp.subtract(cb);
    //create the normal force vector from the velocity
    Vector u = norm.unit();
    Vector ub = u.multiply(-1);
    Vector F = new Vector(0, 0);
    boolean doA = a.mass != 0;
    if(doA) {
        F = a.force;
    }else {
        F = b.force;
    }
    Vector n = new Vector(0, 0);
    Vector nb = new Vector(0, 0);
    if(doA) {
        Vector Fyp = u.multiply(F.dot(u));
        n = Fyp.multiply(-1);
        nb = Fyp;
    }else{
        Vector Fypb = ub.multiply(F.dot(ub));
        n = Fypb;
        nb = Fypb.multiply(-1);
    }
    //calculate normal force for body A
    float r = a.restitution;
    Vector v1 = a.velocity;
    Vector vy1p = u.multiply(u.dot(v1));
    Vector vx1p = v1.subtract(vy1p);
    Vector vy2p = vy1p.multiply(-r);
    Vector v2 = vy2p.add(vx1p);
    //calculate normal force for body B
    float rb = b.restitution;
    Vector v1b = b.velocity;
    Vector vy1pb = ub.multiply(ub.dot(v1b));
    Vector vx1pb = v1b.subtract(vy1pb);
    Vector vy2pb = vy1pb.multiply(-rb);
    Vector v2b = vy2pb.add(vx1pb);
    //calculate friction for body A
    float mk = (a.friction+b.friction)/2;
    Vector v = a.velocity;
    Vector vyp = u.multiply(v.dot(u));
    Vector vxp = v.subtract(vyp);
    float fk = -n.multiply(mk).magnitude();
    Vector fkv = vxp.unit().multiply(fk);                               //friction force
    Vector vr = vxp.subtract(d.multiply(a.angularVelocity));
    Vector fkvr = vr.unit().multiply(fk);                               //friction torque - indicated by r for rotation
    //calculate friction for body B
    Vector vb = b.velocity;
    Vector vypb = ub.multiply(vb.dot(ub));
    Vector vxpb = vb.subtract(vypb);
    float fkb = -nb.multiply(mk).magnitude();
    Vector fkvb = vxpb.unit().multiply(fkb);                            //friction force
    Vector vrb = vxpb.subtract(db.multiply(b.angularVelocity));
    Vector fkvrb = vrb.unit().multiply(fkb);                            //friction torque - indicated by r for rotation
    //move bodies based on calculations
    a.momentum = v2.multiply(a.mass).add(fkv.multiply(dt));
    if(a.mass != 0) {
        a.velocity = a.momentum.divide(a.mass);
        a.position = a.position.add(a.velocity.multiply(dt));
    }
    b.momentum = v2b.multiply(b.mass).add(fkvb.multiply(dt));
    if(b.mass != 0) {
        b.velocity = b.momentum.divide(b.mass);
        b.position = b.position.add(b.velocity.multiply(dt));
    }
    //apply torque to bodies
    float t = (d.cross(fkvr)+d.cross(n));
    float tb = (db.cross(fkvrb)+db.cross(nb));
    if(a.mass != 0) {
        a.angularMomentum = t*dt;
        a.angularVelocity = a.angularMomentum/a.momentOfInertia;
        a.angle += a.angularVelocity*dt;
        a.shape.rotate(a.angularVelocity*dt);
    }
    if(b.mass != 0) {
        b.angularMomentum = tb*dt;
        b.angularVelocity = b.angularMomentum/b.momentOfInertia;
        b.angle += b.angularVelocity*dt;
        b.shape.rotate(b.angularVelocity*dt);
    }
}

As for the actual problem, both the circles and polygons rotate very slowly and often in wrong directions. I know I am throwing a lot out there, but this problem has been bugging me for a while, and I would appreciate any help I can get.

Thanks.

Answer 1

This answer addresses the "I just don't know how to check if the moment of inertia equations are wrong." part of the question.

There are several possible approaches, some of which you may have already tried, and they can be used in combination:

1. Unit testing Take your moment of inertia code and apply it to problems with known solutions from a tutorial or textbook.

2. Dimensional analysis I would recommend this anyway for any scientific or engineering program. You may have deleted comments for compactness of posted code, but they are important. Annotate each variable that represents a physical quantity with its units. Check that every expression you evaluate has the right units, based on its inputs, for its result variable. For example, in the classic equation F=ma in SI units: F is in Newtons, equivalent to kg.m/(s^2), m is in kg, a is in m/(s^2), so it all balances. Be careful with transitions between physics world coordinates and screen coordinates.

3. Program simplification Try working first with only one instance of one very simple shape for which you can do all the calculations by hand. Since some of your problems do not relate to rotation, a circle may be a good first choice because of its symmetry. Debug that, comparing intermediate results to equivalent results from paper-and-pencil (and calculator). Gradually add more instances of the same shape, then debug a single instance of the next shape...

4. Deliberate error Given that you suspect your inertia calculations, try setting arbitrary values slightly different from your calculations, and see what differences they make in the display. Are the effects similar to the problems you are seeing? If so, keep it as a hypothesis.

As a more general note, programs that do iterative simulation can be very vulnerable to accumulated floating point error. Unless you have a real need to save space, and have done enough analysis of the numerical stability of your code to be sure float is OK, I strongly recommend using double instead. This is probably not your current problem, but is something that could become an issue later.