Angle constraint doesn't work for certain angles

Question

I'm writing an angle joint for my 2d physics engine. It works, except for when the max angle is negative and the min angle is positive (when the angle is directly to the left).

As you can see, all of the other balls move within their tolerance angle, but the one directly to the left does not.

public class AngleJoint extends Joint {

    private float minAngle;
    private float maxAngle;

    public AngleJoint(
        final GameEntity a, 
        final GameEntity b, 
        final float midAngle, 
        final float tolerance
    ) {
        super(a, b);
        assert tolerance >= 0;
        minAngle = midAngle - tolerance;
        maxAngle = midAngle + tolerance;

        while (minAngle > Math.PI) {
            minAngle -= 2 * Math.PI;
        }
        while (minAngle < -Math.PI) {
            minAngle += 2 * Math.PI;
        }
        while (maxAngle > Math.PI) {
            maxAngle -= 2 * Math.PI;
        }
        while (maxAngle < -Math.PI) {
            maxAngle += 2 * Math.PI;
        }
        System.out.println(minAngle + ", " + maxAngle);
    }

    @Override
    public void update() {
        assert getA() != null && getB() != null;

        final CManifold m = new CManifold();
        m.a = getA();
        m.b = getB();

        final Vec2D aToB = getB().center().minus(getA().center());
        // angle from A to B
        final float angle = aToB.getTheta();

        if (angle >= minAngle && angle <= maxAngle) {
            // we don't need to do anything
            return;
        }

        final float distBtoA = aToB.length();

        final float closestAngleBound 
            = Math.abs(angle - maxAngle) < Math.abs(angle - minAngle) 
            ? maxAngle : minAngle;

        // where we should be
        final Vec2D solvedLocation 
            = getA().center().plus(
                new Vec2D((float) (
                    Math.cos(closestAngleBound) * distBtoA), 
                    (float) (Math.sin(closestAngleBound) * distBtoA)
                )
            );
        final Vec2D correction = solvedLocation.minus(getB().center());
        final float d = correction.length();

        m.setNormal(correction.divide(d));
        m.setPenetration(d);
        Collisions.fixCollision(m, false);
    }

}

This is where I create this particular scene.

final Vec2D centerV = new Vec2D(500, 700);

    center = createBall(centerV, 75);
    center.setMass(GameEntity.INFINITE_MASS);
    entities.add(center);

    final float vertices = 6;
    final float dist = 120;

    GameEntity first = null;
    GameEntity last = null;
    for (int i = 0; i < vertices; i++) {
        final float angle = (float) (2 * Math.PI / vertices * i);
        final Vec2D newCenter 
            = new Vec2D(
                (float) (centerV.x + Math.cos(angle) * dist), 
                (float) (centerV.y + Math.sin(angle) * dist)
            );
        final GameEntity vertex = createBall(newCenter, 10);
        entities.add(vertex);
        if (last != null) {
            // constraints.add(new DistanceJoint(last, vertex));
        } else {
            first = vertex;
        }
        constraints.add(new DistanceJoint(center, vertex));
        constraints.add(new AngleJoint(center, vertex, angle, .1f));
        last = vertex;
        if (i == vertices - 1 && first != null) {
            // constraints.add(new DistanceJoint(first, vertex));
        }
    }
}

How can I fix my update method so that the ball to the left behaves similarly to the other ones?

Answer 1

I fixed the problem by first implementing a check that Warren Dew suggested, but also changing a bit of my math. This is the finished joint class

public class AngleJoint extends Joint {

    // angles stored between -Pi and Pi
    private final float minAngle;
    private final float maxAngle;

    /**
     *
     * @param a
     * @param b
     * @param midAngle
     *            the angle in the range of -Pi to Pi.
     * @param tolerance
     *            the angle tolerance in both directions. 0 <= tolerance < Pi
     */
    public AngleJoint(final GameEntity a, final GameEntity b, final float midAngle, final float tolerance) {
        super(a, b);
        if (tolerance < 0 || tolerance >= AngleUtils.PI) {
            throw new IllegalArgumentException("Tolerance must be >= 0 and < Pi");
        }
        minAngle = AngleUtils.normalize(midAngle - tolerance);
        maxAngle = AngleUtils.normalize(midAngle + tolerance);
    }

    @Override
    public void update() {
        assert getA() != null && getB() != null;

        final CManifold m = new CManifold();
        m.a = getA();
        m.b = getB();

        final Vec2D aToB = getB().center().minus(getA().center());
        // angle from A to B
        final float angle = aToB.getTheta();

        if (angle >= minAngle && angle <= maxAngle) {
            // we don't need to do anything
            return;
        }
        // if we are in that dumb spot where maxAngle < min Angle (directly to the left) we need extra checks
        if (maxAngle < minAngle && (angle <= maxAngle && angle >= -AngleUtils.PI || angle >= minAngle && angle <= AngleUtils.PI)) {
            return;
        }

        final float distBtoA = aToB.length();

        final float closestAngleBound = AngleUtils.angleDifference(angle, maxAngle) < AngleUtils.angleDifference(angle, minAngle) ? maxAngle
                : minAngle;

        // where we should be
        final Vec2D solvedLocation = getA().center().plus(
                new Vec2D((float) (Math.cos(closestAngleBound) * distBtoA), (float) (Math.sin(closestAngleBound) * distBtoA)));
        final Vec2D correction = solvedLocation.minus(getB().center());
        final float d = correction.length();

        m.setNormal(correction.divide(d));
        m.setPenetration(d);
        Collisions.fixCollision(m, false);
    }

}