java debugging game-physics physics-engine

Objects running through each other

I am developing a 2-D physics engine in java using this tutorial: https://gamedevelopment.tutsplus.com/tutorials/how-to-create-a-custom-2d-physics-engine-the-basics-and-impulse-resolution--gamedev-6331

The problem is that sometimes when a box collides with a ball through a corner the box passes through it and sometimes when the ball is small, it gets trapped inside the box. I think there is some precision error in penetration calculation in the CollisionAABBCircle.java file. Here is are two gifs showing what is happening.

Here the smallest ball comes from top and gets trapped.

Here a box passes through the biggest ball.

Here is the code:

GUI.java

import javafx.animation.KeyFrame;
import javafx.animation.Timeline;
import javafx.application.Application;
import javafx.scene.Scene;
import javafx.scene.canvas.Canvas;
import javafx.scene.canvas.GraphicsContext;
import javafx.scene.layout.StackPane;
import javafx.scene.paint.Color;
import javafx.stage.Stage;
import javafx.util.Duration;
import physics.ImpulseScene;
import physics.Point;
import sprite.Ball;
import sprite.Box;

public class GUI extends Application {

    public static final int WIDTH = 800;
    public static final int HEIGHT = 500;

    public static GraphicsContext gc;

    @Override
    public void start(Stage stage) throws Exception {
        Canvas canvas = new Canvas(WIDTH, HEIGHT);
        gc = canvas.getGraphicsContext2D();
        stage.setScene(new Scene(new StackPane(canvas)));
        stage.setTitle("BOUNCE BOUNCE BOUNCE");
        stage.show();

        Ball ball1 = new Ball(10, new Point(20, 20));
        ball1.gc = gc;
        ball1.color = Color.BLACK;
        ball1.setMass(10);
        ball1.getVelocity().setXComponent(1);
        ball1.getVelocity().setYComponent(1);

        Ball ball2 = new Ball(20, new Point(100, 100));
        ball2.gc = gc;
        ball2.color = Color.BLACK;
        ball2.setMass(20);
        ball2.getVelocity().setXComponent(1);
        ball2.getVelocity().setYComponent(-2);

        Ball ball3 = new Ball(30, new Point(250, 250));
        ball3.gc = gc;
        ball3.color = Color.BLACK;
        ball3.setMass(30);
        ball3.getVelocity().setXComponent(3);
        ball3.getVelocity().setYComponent(-2);

        Ball ball4 = new Ball(40, new Point(400, 400));
        ball4.gc = gc;
        ball4.color = Color.BLACK;
        ball4.setMass(40);
        ball4.getVelocity().setXComponent(2);
        ball4.getVelocity().setYComponent(0);

        Ball ball5 = new Ball(50, new Point(100, 700));
        ball5.gc = gc;
        ball5.color = Color.BLACK;
        ball5.setMass(50);
        ball5.getVelocity().setXComponent(2);
        ball5.getVelocity().setYComponent(3);

        Ball ball6 = new Ball(60, new Point(400, 100));
        ball6.gc = gc;
        ball6.color = Color.BLACK;
        ball6.setMass(60);
        ball6.getVelocity().setXComponent(0);
        ball6.getVelocity().setYComponent(0);

        Ball ball7 = new Ball(70, new Point(340, 340));
        ball7.gc = gc;
        ball7.color = Color.BLACK;
        ball7.setMass(70);
        ball7.getVelocity().setXComponent(-1);
        ball7.getVelocity().setYComponent(2);

        Box box1 = new Box(new Point(80, 80), 50, 50);
        box1.gc = gc;
        box1.color = Color.BROWN;
        box1.setMass(10);
        box1.getVelocity().setXComponent(3);
        box1.getVelocity().setYComponent(2);

        Box box2 = new Box(new Point(180, 180), 60, 60);
        box2.gc = gc;
        box2.color = Color.BROWN;
        box2.setMass(10);
        box2.getVelocity().setXComponent(4);
        box2.getVelocity().setYComponent(-4);

        Box box3 = new Box(new Point(280, 280), 60, 60);
        box3.gc = gc;
        box3.color = Color.BROWN;
        box3.setMass(10);
        box3.getVelocity().setXComponent(4);
        box3.getVelocity().setYComponent(-4);

        Box box4 = new Box(new Point(380, 380), 60, 60);
        box4.gc = gc;
        box4.color = Color.BROWN;
        box4.setMass(10);
        box4.getVelocity().setXComponent(4);
        box4.getVelocity().setYComponent(-4);

        ImpulseScene scene = new ImpulseScene(10);
        scene.bodies.add(ball1);
        scene.bodies.add(ball2);
        scene.bodies.add(ball3);
        scene.bodies.add(ball4);
        scene.bodies.add(ball5);
        scene.bodies.add(ball6);
        scene.bodies.add(ball7);
        scene.bodies.add(box1);
        scene.bodies.add(box2);
        scene.bodies.add(box3);
        scene.bodies.add(box4);

        Bounds bounds = canvas.getBoundsInLocal();

        Timeline timeline = new Timeline(new KeyFrame(Duration.millis(16), e -> {
            gc.setFill(Color.WHITESMOKE);
            gc.fillRect(0, 0, WIDTH, HEIGHT);
            ball1.update(bounds);
            ball2.update(bounds);
            ball3.update(bounds);
            ball4.update(bounds);
            ball5.update(bounds);
            ball6.update(bounds);
            ball7.update(bounds);
            box1.update(bounds);
            box2.update(bounds);
            box3.update(bounds);
            box4.update(bounds);
            scene.step();
        }));
        timeline.setCycleCount(Timeline.INDEFINITE);
        timeline.play();
    }

    public static void main(String[] args) {
        launch(args);
    }
}

Ball.java

package sprite;

import javafx.geometry.Bounds;
import javafx.scene.canvas.GraphicsContext;
import javafx.scene.paint.Color;
import physics.Circle;
import physics.Point;
import physics.Vector;

public class Ball extends Circle {

    public static Color color;
    private double height;
    private double width;
    public GraphicsContext gc;

    public Ball(long radius, Point position) {
        super(radius, position);
        this.setHeight(2 * radius);
        this.setWidth(2 * radius);
    }

    public void update(Bounds bounds) {
        Point position = getPosition();
        Vector velocity = getVelocity();

        if (position.getX() + velocity.getXComponent() < getRadius()
                || position.getX() + velocity.getXComponent() > bounds.getMaxX() - getRadius()) {
            velocity.setXComponent(velocity.getXComponent() * (-1));
        }
        if (position.getY() + velocity.getYComponent() < getRadius()
                || position.getY() + velocity.getYComponent() > bounds.getMaxY() - getRadius()) {
            velocity.setYComponent(velocity.getYComponent() * (-1));
        }

        double px = position.getX() + velocity.getXComponent();
        double py = position.getY() + velocity.getYComponent();

        setPosition(new Point(px, py));

        //System.out.println(getPosition());
    }

    public void render() {
        if (gc != null && color != null) {
            //System.out.println("Rendering");
            gc.setFill(color);
            gc.fillOval(getPosition().getX()-getRadius(), getPosition().getY()
                    - getRadius(), 2*getRadius(), 2*getRadius());
        }
    }

    @Override
    public void setPosition(Point p) {
        super.setPosition(p);
        render();
    }

    private void setWidth(double l) {
        this.width = l;
    }

    private void setHeight(double l) {
        this.height = l;
    }

}

Box.java

package sprite;

import javax.swing.text.Position;

import javafx.geometry.Bounds;
import javafx.scene.canvas.GraphicsContext;
import javafx.scene.paint.Color;
import physics.AABB;
import physics.Point;
import physics.Vector;

public class Box extends AABB {

    private double height;
    private double width;
    private double halfHeight;
    private double halfWidth;

    public Color color;
    public GraphicsContext gc;

    public Box(Point min, Point max) {
        super(min, max);
        this.setHeight(getMin().getY() - getMax().getY());
        this.setWidth(getMax().getX() - getMin().getX());
    }

    public Box(Point position, double height, double width) {
        super(position, height, width);
        this.setHeight(getMin().getY() - getMax().getY());
        this.setWidth(getMax().getX() - getMin().getX());
    }

    public void update(Bounds bound) {

        Point position = getPosition();
        Vector velocity = getVelocity();

        double px = position.getX() - (this.halfWidth);
        double py = position.getY() - (this.halfHeight);

        if (px + velocity.getXComponent() < 0 || px + velocity.getXComponent() + width > bound.getMaxX()) {
            velocity.setXComponent(velocity.getXComponent() * (-1));
        }
        if (py + velocity.getYComponent() < 0 || py + velocity.getYComponent() + height > bound.getMaxY()) {
            velocity.setYComponent(velocity.getYComponent() * (-1));
        }

        px += velocity.getXComponent();
        py += velocity.getYComponent();

        getMin().setX(px);
        getMin().setY(py + height);
        getMax().setX(px + width);
        getMax().setY(py);

        updatePosition();

    }

    @Override
    public void setPosition(Point p) {
        super.setPosition(p);
        render();
    }

    private void render() {
        if (color!=null && gc!=null) {
            gc.setFill(color);
            gc.fillRect(getPosition().getX() - this.halfWidth, getPosition().getY() - this.halfHeight, this.width, this.height);
        }
    }

    public void setHeight(double height) {
        this.height = height;
        this.halfHeight = height / 2.0d;
    }

    public void setWidth(double width) {
        this.width = width;
        this.halfWidth = width / 2.0d;
    }

}

Circle.java

package physics;

public class Circle extends Shape {
    private long radius;

    public Circle(long radius, Point position) {
        this.radius = radius;
        this.setPosition(position);
        this.setVelocity(new Vector(new Point(0.0d, 0.0d), new Point(0, 0)));
        this.setMass(0);
        this.setRestitution(1);
        this.setAcceleration(new Vector(new Point(0, 0), new Point(0, 0)));
        this.setType(Type.Circle);
    }

    

    public long getRadius() {
        return this.radius;
    }

    public void setRadius(long radius) {
        this.radius = radius;
    }

}

AABB.java

package physics;

public class AABB extends Shape {
    private Point min;
    private Point max;

    public AABB(Point min, Point max) {
        this.min = min;
        this.max = max;
        this.setVelocity(new Vector(new Point(0, 0), new Point(0, 0)));
        this.setMass(0);
        this.setRestitution(1);
        this.setType(Type.AABB);
        updatePosition();
    }

    public AABB(Point position, double height, double width) throws IllegalArgumentException {
        double minX;
        double minY;
        double maxX;
        double maxY;

        minX = position.getX() - (width / 2.0d);
        minY = position.getY() + (height / 2.0d);

        maxX = position.getX() + (width / 2.0d);
        maxY = position.getY() - (height / 2.0d);

        if (minX < 0 || minY < height) {
            throw new IllegalArgumentException("Inappropriate position");
        }

        Point min = new Point(minX, minY);
        Point max = new Point(maxX, maxY);
        this.min = min;
        this.max = max;
        this.setVelocity(new Vector(new Point(0, 0), new Point(0, 0)));
        this.setMass(0);
        this.setRestitution(1);
        this.setType(Type.AABB);
        setPosition(position);
    }

    public void updatePosition() {
        setPosition(new Point((min.getX() + max.getX()) / 2, (min.getY() + max.getY()) / 2));
    }

    public Point getMin() {
        return this.min;
    }

    public Point getMax() {
        return this.max;
    }

}

Shape.java

package physics;

public class Shape {
    private Vector velocity;
    private Vector acceleration;
    private long mass;
    private double invMass;
    private float restitution;
    private Type type;
    private Point position;

    public enum Type {
        Circle, AABB, count
    }

    public Vector getAcceleration() {
        return this.acceleration;
    }

    public void setAcceleration(Vector acceleration) {
        this.acceleration = acceleration;
    }

    public long getMass() {
        return this.mass;
    }

    public void setMass(long mass) {
        this.mass = mass;
        this.setInvMass(mass);
    }

    public Vector getVelocity() {
        return this.velocity;
    }

    public void setVelocity(Vector velocity) {
        this.velocity = velocity;
    }

    public double getInvMass() {
        return this.invMass;
    }

    private void setInvMass(long mass) {
        if (mass == 0) {
            invMass = Long.MAX_VALUE;
            return;
        }
        this.invMass = 1.0d / (double) mass;
    }

    public float getRestitution() {
        return this.restitution;
    }

    public void setRestitution(float d) {
        this.restitution = d;
    }

    public Type getType() {
        return this.type;
    }

    public void setType(Type type) {
        this.type = type;
    }

    public Point getPosition() {
        return this.position;
    }

    public void setPosition(Point position) {
        this.position = position;
    }

}

Vector.java

package physics;

public class Vector {
    private Point p1;
    private Point p2;
    private double xComponent;
    private double yComponent;
    private double angle;
    private double magnitude;

    public Vector(Point p1, Point p2) {
        this.p1 = p1;
        this.p2 = p2;
        this.xComponent = this.p2.getX() - this.p1.getX();
        this.yComponent = this.p2.getY() - this.p1.getY();
        this.angle = Math.atan2(this.yComponent, this.xComponent);
        this.magnitude = Math.sqrt(this.xComponent * this.xComponent + this.yComponent * this.yComponent);
    }

    public Vector(Point p2) {
        Point p1 = new Point(0, 0);
        this.p1 = p1;
        this.p2 = p2;
        this.xComponent = this.p2.getX() - this.p1.getX();
        this.yComponent = this.p2.getY() - this.p1.getY();
        this.angle = Math.atan2(this.yComponent, this.xComponent);
        this.magnitude = Math.sqrt(this.xComponent * this.xComponent + this.yComponent * this.yComponent);
    }

    public Vector(double magnitude, Vector unitVector) {
        scaledProduct(magnitude, unitVector);
    }

    public Vector() {
    }

    private void scaledProduct(double magnitude, Vector unitVector) {
        Point point = new Point(magnitude * unitVector.getXComponent(), magnitude * unitVector.getYComponent());
        new Vector(point);
    }

    public static Vector scalarProduct(double magnitude, Vector unitVector) {
        Point point = new Point(magnitude * unitVector.getXComponent(), magnitude * unitVector.getYComponent());
        return new Vector(point);
    }

    public static double dotProduct(Vector v1, Vector v2) {
        return (v1.xComponent * v2.xComponent + v1.yComponent * v2.yComponent);
    }

    public static Vector sum(Vector v1, Vector v2) {
        return new Vector(new Point(v1.getXComponent() + v2.getXComponent(), v1.getYComponent() + v2.getYComponent()));
    }

    public static Vector difference(Vector from, Vector vector) {
        return new Vector(new Point(from.getXComponent() - vector.getXComponent(),
                from.getYComponent() - vector.getYComponent()));
    }

    public static Vector scalarDivision(Vector vector, double by) {
        return new Vector(new Point(vector.getXComponent() / by, vector.getYComponent() / by));
    }

    public static double angleBetween(Vector v1, Vector v2) {
        return Math.acos(Vector.dotProduct(v1, v2) / (v1.getMagnitude() * v2.getMagnitude()));
    }

    public double getXComponent() {
        return this.xComponent;
    }

    public void setXComponent(double d) {
        this.xComponent = d;
        update();
    }

    public double getYComponent() {
        return this.yComponent;
    }

    public void setYComponent(double d) {
        this.yComponent = d;
        update();
    }

    public double getAngle() {
        return this.angle;
    }

    public void setAngle(double angle) {
        this.angle = angle;
        update();
    }

    public double getMagnitude() {
        return this.magnitude;
    }

    public void setMagnitude(double length) {
        this.magnitude = length;
    }

    public void update() {
        this.angle = Math.atan2(this.yComponent, this.xComponent);
        this.magnitude = Math.sqrt(this.xComponent * this.xComponent + this.yComponent * this.yComponent);
    }

    @Override
    public boolean equals(Object v) {
        Vector vector = (Vector) v;
        return (ImpulseMath.equal(this.xComponent, vector.xComponent)
                && ImpulseMath.equal(this.yComponent, vector.yComponent));
    }

}

Point.java

package physics;

public class Point {
    private double x;
    private double y;

    public Point(double x, double y) {
        this.x = x;
        this.y = y;
    }

    public static double distance(Point p1, Point p2) {
        return Math.sqrt(
                (p1.getX() - p2.getX()) * (p1.getX() - p2.getX()) + (p1.getY() - p2.getY()) * (p1.getY() - p2.getY()));
    }

    public double getX() {
        return x;
    }

    public void setX(double x) {
        this.x = x;
    }

    public double getY() {
        return y;
    }

    public void setY(double y) {
        this.y = y;
    }
}

ImpulseScene.java

package physics;

import java.util.ArrayList;

public class ImpulseScene {
    public double dt;
    public int iterations;
    public ArrayList<Shape> bodies = new ArrayList<>();
    public ArrayList<Manifold> contacts = new ArrayList<>();

    public ImpulseScene(int iterations) {
        this.iterations = iterations;
    }

    public void step() {
        contacts.clear();
        for (int i = 0; i < bodies.size(); ++i) {
            Shape A = bodies.get(i);

            for (int j = i + 1; j < bodies.size(); ++j) {
                Shape B = bodies.get(j);

                if (A.getInvMass() == 0 && B.getInvMass() == 0) {
                    continue;
                }

                Manifold m = new Manifold(A, B);
                m.solve();

                if (m.contactCount > 0) {
                    contacts.add(m);
                }
            }

        }

        for (int i = 0; i < contacts.size(); ++i) {
            contacts.get(i).initialize();
        }

        for (int j = 0; j < iterations; ++j) {
            for (int i = 0; i < contacts.size(); ++i) {
                contacts.get(i).applyImpulse();
            }
        }

        for (int i = 0; i < contacts.size(); ++i) {
            contacts.get(i).positionalCorrection();
        }
    }

    public void clear() {
        contacts.clear();
        bodies.clear();
    }
}

Collision.java

package physics;

public class Collision {

    public static CollisionCallback dispatch[][] = { { CollisionCircleCircle.instance, CollisionCircleAABB.instance },
            { CollisionAABBCircle.instance, CollisionAABBAABB.instance } };

}

CollisionCallback.java

package physics;

public interface CollisionCallback {
    public void resolveCollision(Shape a, Shape b, Manifold manifold);
}

CollisionCircleAABB.java

package physics;

public class CollisionCircleAABB implements CollisionCallback {
    public static final CollisionCircleAABB instance = new CollisionCircleAABB();

    @Override
    public void resolveCollision(Shape a, Shape b, Manifold manifold) {
        CollisionAABBCircle.instance.resolveCollision(b, a, manifold);

        if (manifold.contactCount>0) {
            manifold.normal.setXComponent(-manifold.normal.getXComponent());
            manifold.normal.setYComponent(-manifold.normal.getYComponent());
        }
    }
}

CollisionAABBCircle.java

package physics;

public class CollisionAABBCircle implements CollisionCallback {
    public static final CollisionAABBCircle instance = new CollisionAABBCircle();

    @Override
    public void resolveCollision(Shape shape1, Shape shape2, Manifold manifold) {
        AABB a = (AABB) shape1;
        Circle c = (Circle) shape2;

        Vector normal = Vector.difference(new Vector(c.getPosition()), new Vector(a.getPosition()));

        Vector closest = new Vector(new Point(normal.getXComponent(), normal.getYComponent()));

        double xExtent = (a.getMax().getX() - a.getMin().getX()) / 2.0d;
        double yExtent = (a.getMin().getY() - a.getMax().getY()) / 2.0d;

        closest.setXComponent(ImpulseMath.clamp(-xExtent, xExtent, closest.getXComponent()));
        closest.setYComponent(ImpulseMath.clamp(-yExtent, yExtent, closest.getYComponent()));

        boolean inside = false;

        if (normal.equals(closest)) {
            inside = true;

            if (Math.abs(normal.getXComponent()) < Math.abs(normal.getYComponent())) {
                if (closest.getXComponent() > 0) {
                    closest.setXComponent(xExtent);
                } else {
                    closest.setXComponent(-xExtent);
                }
            } else {
                if (closest.getYComponent() > 0) {
                    closest.setYComponent(yExtent);
                } else {
                    closest.setYComponent(-yExtent);
                }
            }
        }

        Vector n = Vector.difference(normal, closest);
        double d = n.getMagnitude();
        double r = c.getRadius();

        if (d > r && !inside) {
            manifold.contactCount = 0;
            return;
        }

        manifold.contactCount = 1;

        if (inside) {
            manifold.normal.setXComponent((-normal.getXComponent()) / normal.getMagnitude());
            manifold.normal.setYComponent((-normal.getYComponent()) / normal.getMagnitude());
        } else {
            manifold.normal.setXComponent((normal.getXComponent()) / normal.getMagnitude());
            manifold.normal.setYComponent((normal.getYComponent()) / normal.getMagnitude());
        }

        manifold.penetration = r - d;
    }
}

Manifold.java

package physics;

public class Manifold {
    public Shape a;
    public Shape b;
    public double penetration;
    public final Vector normal = new Vector(new Point(0.0d, 0.0d));
    public int contactCount;
    public double e;

    public Manifold(Shape a, Shape b) {
        this.a = a;
        this.b = b;
        this.contactCount = 0;
    }

    public void solve() {
        int ia = a.getType().ordinal();
        int ib = b.getType().ordinal();

        Collision.dispatch[ia][ib].resolveCollision(a, b, this);
    }

    public void initialize() {
        e = Math.min(a.getRestitution(), b.getRestitution());
    }

    public void applyImpulse() {
        Vector relativeVelocity = Vector.difference(b.getVelocity(), a.getVelocity());
        double velocityAlongNormal = Vector.dotProduct(relativeVelocity, this.normal);

        if (velocityAlongNormal > 0) {
            return;
        }

        double restitution = Math.min(a.getRestitution(), b.getRestitution());

        double j = -(1 + restitution) * velocityAlongNormal;

        j /= (a.getInvMass() + b.getInvMass());

        Vector impulse = Vector.scalarProduct(j, this.normal);
        a.setVelocity(Vector.difference(a.getVelocity(), Vector.scalarProduct(a.getInvMass(), impulse)));
        b.setVelocity(Vector.sum(b.getVelocity(), Vector.scalarProduct(b.getInvMass(), impulse)));
    }

    public void positionalCorrection() {
        double correction = Math.max(this.penetration - ImpulseMath.PENETRATION_ALLOWANCE, 0.0d)
                / (a.getInvMass() + b.getInvMass()) * ImpulseMath.PENETRATION_CORRETION;
        Point posA = a.getPosition();
        Point posB = b.getPosition();
        Point newPosA = new Point(posA.getX() - (correction * a.getInvMass()),
                posA.getY() - (correction * a.getInvMass()));
        Point newPosB = new Point(posB.getX() + (correction * b.getInvMass()),
                posB.getY() + (correction * b.getInvMass()));
        a.setPosition(newPosA);
        b.setPosition(newPosB);
    }

}

I couldn't think of any reason this could be happening. Please help me out here, I am out of ideas.

Solution

Solved the issue. It turns out the code on the tutorial website was wrong. In the CollisionAABBCircle.java file in the last if condition, it should be n not normal. It solves the issue because as the ball is inside the box the normal should be the vector pointing from center of the ball to the nearest point on edge. After changing it the final code will be:

if (inside) {
        manifold.normal.setXComponent((-n.getXComponent()) / n.getMagnitude());
        manifold.normal.setYComponent((-n.getYComponent()) / n.getMagnitude());
} else {
        manifold.normal.setXComponent((normal.getXComponent()) / normal.getMagnitude());
        manifold.normal.setYComponent((normal.getYComponent()) / normal.getMagnitude());
}