A (very) simple 2d Java Phyics Simulator

physics.java

/* This is a work in progress, not done yet (doesn't do anything in fact!).*/

// I don't have a website, so it's just my screen name... feel free to refactor
// package name how you like. But credit me plz ;D
package shivshank.engine.physics;

import somepackage.that.doesnt.exist.yet.Vector2f;
import java.util.List;
import java.util.ArrayList;

/**
 * A 2D rigid body physics engine.
 * 
 * This simulator handles the physics (roughly) of various rigid bodies with the environment.
 * 
 * Note that engine <strong>does not</strong> handle physics between each rigid body.
 *     Each rigid body can only collide with the environment.
 * 
 * The engine does not support rotating stuffs, either, because math and laziness...
 *  (but hopefully I'll get there!)
 */
class PhysicsSimulator {

    public static class Geometer {
        public enum WINDING {
            CW, // Clockwise
            CCW, // Counter-Clockwise (anticlockwise)
            CL // Collinear (points lie on the same line)
        }
        
        /**
         * Get the winding order of points A, B, and C
         */
        public static WINDING getOrientation(Vector2f a, Vector2f b,
                                             Vector2f c) {
            // Calculate slope of AB, then BC
            // if m(AB) > m(BC: then CW, elif m == m CL, else CCW
            double result = (b.y - a.y)/(b.x - a.x) - (c.y - b.y)/(c.x - b.x);
            
            //
            if (result > 0) {
                return WINDING.CW;
            } else if (result == 0) {
                return WINDING.CL;
            } else {
                return WINDING.CCW;
            }
        }
        
        /*
         * Gets whether two lines, AB and CD, are crossing.
         * <p>
         * Returns false if two lines are coincident (ABCD is Collinear).
         */
        public static boolean intersects(Vector2f a, Vector2f b,
                                         Vector2f c, Vector2f d) {
            WINDING o1 = getOrientation(a, b, c);
            WINDING o2 = getOrientation(a, b, d);
            
            WINDING o3 = getOrientation(c, d, a);
            WINDING o4 = getOrientation(c, d, b);
            
            if (o1 != o2 && o3 != o4) {
                return true;
            }
            
            return false;
        }
        
        /**
         * Returns true if p is within rectangle defined by A and B
         * <p>
         * If point P is on the line defined by AB, this function can be used to
         *     check if point P is actually on the segment AB.
         */
        public static float inBounds(Vector2f a, Vector2f b, Vector2f p) {
            return p.x >= Math.min(a.x, b.x) && p.x <= Math.max(a.x, b.x)
                && p.y >= Math.min(a.y, b.y) && p.y <= Math.max(a.y, b.y);
        }
        
        public static float distance(Vector2f a, Vector2f b) {
            return new Vector2f(b).sub(a).length();
        }
        
        /**
         * Gets shortest distance from line AB to P.
         * <p>
         * Note that the closest point on AB might be the points A or B.
         */
        public static float distance(Vector2f a, Vector2f b, Vector2f p) {
            // x = (b - a) * 1/||b - a||
            // x represents the direction line AB is going in
            Vector2f x = new Vector2f(b).sub(a).normalize();
            
            // project p onto AB
            x.scale( x.dot(new Vector2f(p).sub(a)) );
            // x is now closest point to p on AB
            x.add(a);
            
            // check that this point is actually on the segment
            if (inBounds(a, b, x)) {
                return x.sub(p).length();
            }
            
            // if x is not on the segment, then see if A or B is closer to P
            // (make sure we don't change a or b)
            Vector2f temp = new Vector2f(a);
            return Math.min(temp.sub(p).length(), 
                            temp.assign(b).sub(p).length());
        }
        
        /**
         * Gets whether line AB intersects capsule CD with radius r.
         */
        public static boolean intersectsCapsule(Vector2f a, Vector2f b,
                                              Vector2f c, Vector2f d, float r) {
            // TODO: This isn't entirely correct... This is only testing if
            // the endpoints are inside the capsule
            
            // true if the distance from A to CD or B to CD is less than r
            return Math.min(distance(c, d, a), distance(c, d, b)) < r;
        }
        
        /*
            Should the Geometer have a function/method for AABBs?
        public static boolean intersectsSweptAABB(Vector2f a, Vector2f b,
            Vector2f start, Vector2f end, float height, float width
        */
    }
    
    /**
     * Represents the center of mass of a Rigid Body.
     * <p>
     * Simulates the forces acting on that Rigid Body.
     */
    public static abstract class RigidBody {
        private Vector2f velocity;
        private Vector2f pos;
        private Vector2f forces;
        private Vector2f impulses;
        private final float mass;
        
        /**
         * What this rigid body looked like before it was last stepped.
         */
        public RigidBody previous;
        
        public RigidBody(pos, mass) {
            this.pos = pos;
            this.mass = mass;
            
            forces = new Vector2f(0f, 0f);
            impulses = new Vector2f(0f, 0f);
            
            velocity = new Vector2f(0f, 0f);
        }

        public void assign(RigidBody b) {
            velocity.set(b.getVelocity());
            pos.set(b.getPos());
            forces.set(b.getForces());
            impulses.set(b.getImpulses());
        }
        
        public float getMass() {
            return mass;
        }
        
        public Vector2f getPos() {
            return pos;
        }
        
        public Vector2f getVelocity() {
            return velocity;
        }
        
        public Vector2f getForces() {
            return forces;
        }
        
        public Vector2f getImpulses() {
            return impulses;
        }
        
        public void setPos(float x, float y) {
            pos.set(x, y);
        }
        
        public void translate(float x, float y) {
            pos.translate(x, y);
        }
        
        public void setVelocity(float x, float y) {
            velocity.set(x, y);
        }
        
        public void clampSpeed(float m) {
            float speed = velocity.length();
             
            if (speed > m) {
                velocity.scale(m / speed);
            }
        }
        
        public void clearForces() {
            forces.set(0f, 0f);
        }
        
        public void clearImpulses() {
            impules.set(0f, 0f);
        }
        
        /**
         * Apply a force for this frame.
         * <p>
         * Causes acceleration over this frame, a = sum(forces).
         */
        public void applyForce(Vector2f f) {
            forces.add(f);
        }
        
        /**
         * Apply an impulse.
         * <p>
         * Causes a change in velocity inversely proportional to mass
         * (dv = j/m).
         */
        public void applyImpulse(Vector2f j) {
            impulses.add(j);
        }
        
        /**
         * Advances the position of this rigid body.
         */
        public void step(double deltaTime) {
            if (previous == null) {
                previous = new RigidBody(null, mass);
            }
            previous.assign(this);
            
            // TODO: Investigate the accuracy of this integration scheme
            // first attempt at velocity verlet integration of position:
            Vector3f tf = forces;
            
            // apply the instantaneous change in velocity resulting impulses
            velocity.add(impulses);
            clearImpulses();
            
            // x(t + dt) = x(t) + v(t)*dt + dt^2 * 1/2 * acc
            
            pos.add( // v(t)*dt;
                new Vector3f(vel).scale(deltaTime)
            ).add( // dt^2 * 1/2 * acc
                new Vector3f(tf).scale(deltaTime * deltaTime * 0.5)
            );
            
            // v(t + dt) = v(t) + dt/m * ( f(t) + f(t + dt) )
            // TODO: THERE'S AN ERROR HERE IN MY IMPLEMENTATION:
            // (I use f(t) * 2 instead of f(t) + f(t+dt))
            // I don't really understand how to calculate force/impulse/acc yet!
            velocity.add(
                new Vector3f(tf).scale(2f * ((float)deltaTime) / mass)
            );
            
            // tf is just an alias to forces; don't clear until end of frame
            clearForces();
        }
    }
    
    public static class Circle extends RigidBody {
        private float radius;
        
        public Circle(Vector2f pos, float mass, float radius) {
            super(pos, mass);
            
            this.radius = radius;
        }
        
        public float getRadius() {
            return radius;
        }
    }
    
    public static class AABB extends RigidBody {
        private float width;
        private float height;
        
        public AABB(Vector2f pos, float mass, float w, float h) {
            super(pos, mass);
            
            width = w;
            height = h;
        }
        
        public float getHeight() {
            return height;
        }
        
        public float getWidth() {
            return width;
        }
    }
    
    /**
     * Represents a Collision that happened/is happening.
     */
    // TODO
    public static class Contact {
        public final Vector2f normal;
        public final Vector2f point;
        public final Vector2f point;
        
        public Contact() {
        }
    }
    
    /**
     * A static, non-moving Polygon. It can be convex or concave.
     * <p>
     * See the getEdgeNormal function to see how normals are calculated. It is
     *     important that all normals point outward.
     */
    public static class Polygon {
        
        public List<Vector2f> points;
        
        public Polygon(Vector2f start) {
            points = new ArrayList<Vector2f>();
            points.add(start);
        }
        
        public void addSegment(Vector2f p) {
            points.add(p);
        }
        
        public void List<Vector2f> getPoints() {
            return points;
        }
        
        /**
         * Gets the edge formed by points i and i+1
         *
         * @param i the edge, on the inclusive range of [0, points.size() - 1]
         */
        public Vector2f[] getEdge(int i) {
            return new Vector2f[] {
                points[i],
                points[(i+1) % points.size()]
            };
        }
        
        public Vector2f getEdgeNormal(int i) {
            Vector2f[] line = getEdge(i);
            
            Vector2f normal = new Vector2f(line[1]).sub(line[0]);
        }
    }
    
    public static class CollisionDetector {
        public static void broadphase(PhysicsSimulator p, AABB rect,
                                      List<Polygon> relevant) {
            // TODO: Broad phase collision
            relevant.addAll(p.getEnvironment());
        }
        
        public static Contact test(RigidBody b, Polygon p) {
            // Maybe it's better for code complexity if each RB has methods
            // for collision detection? Although that couples each type of RB...
            if (b instanceof Circle) {
                test((Circle) b, p);
            } else if (b instanceof AABB) {
                test((AABB) b, p);
            } else {
                throw new UnsupportedOperationException("Not implemented yet");
            }
        }
        
        public static Contact test(Circle a, Circle b) {
            throw new UnsupportedOperationException("Not implemented yet");
        }
        
        public static Contact test(AABB a, AABB b) {
            throw new UnsupportedOperationException("Not implemented yet");
        }
        
        public static Contact test(AABB a, Circle b) {
            throw new UnsupportedOperationException("Not implemented yet");
        }
        
        public static Contact test(AABB a, Polygon p) {
            
        }
        
        public static Contact test(Circle a, Polygon p) {
            if (a.previous == null) {
                for(int i=0; i < p.getPoints().size(); i+=1) {
                    Vector2f[] edge = p.getEdge(i);
                    if (Geometer.distance(edge[0], edge[1], a.getPos()) < a.getRadius()) {
                        // distance from line to center of circle < radius
                        return true;
                    }
                }
            }
            
            // Swept Check
        }
    }
    
    public static class ContactSolver {
    
    }
    
    // TODO: add ForceVolumes; it will be cool!
    private List<RigidBody> bodies;
    private List<Polygon> environment;
    
    // TODO: Add a good system for looking up bodies (such as by ID)
    public PhysicsSimulator() {
        bodies = new ArrayList<RigidBody>();
        environment = new ArrayList<Polygon>();
    }
    
    public void addBody(RigidBody b) {
        bodies.add(b);
    }
    
    public void addPolygon(Polygon p) {
        environment.add(p);
    }
    
    public List<RigidBody> getBodies() {
        return bodies;
    }
    
    public List<Polygon> getEnvironment() {
        return environment;
    }
    
    /**
     * Advances the simulation by deltaTime.
     */
    public void step(double deltaTime) {
        // accumulate forces
        Vector2f gravity = new Vector2f(0f, 0f);
        for (RigidBody b : bodies) {
            b.applyForce( gravity.set(b.getMass() * -9.81f) );
        }
        
        ArrayList<Polygon> relevant = new ArrayList();
        
        // do collision detection
        for (RigidBody b : bodies) {
            CollisionDetector.broadphase(this, b, relevant);
            
            for (Polygon p : relevant) {
                Contact c = CollisionDetector.test(b, p);
                
                if (c != null) {
                    // do contact resolution
                }
                
                // NOW WHAT? repeat detection?
            }
        }
        
        // integrate positions
        for (RigidBody b : bodies) {
            b.step(deltaTime);
        }
    }
}