Added physics object and some basic constraints

Author: GiveJavaAChance

src/polyray/physics/Particle.java

@@ -0,0 +1,34 @@
+package polyray.physics;
+
+import polyray.Vector3d;
+
+public class Particle extends PhysicsObject {
+
+    public final double mass;
+    private final double invMass;
+
+    public Particle(double mass, float friction, float restitution) {
+        super(friction, restitution);
+        this.mass = mass;
+        this.invMass = 1.0d / mass;
+    }
+
+    public Particle(double mass, Vector3d pos, float friction, float restitution) {
+        super(pos, friction, restitution);
+        this.mass = mass;
+        this.invMass = 1.0d / mass;
+    }
+
+    public void applyForce(Vector3d f) {
+        this.acc.x += f.x * invMass;
+        this.acc.y += f.y * invMass;
+        this.acc.z += f.z * invMass;
+    }
+
+    public void applyForce(double fx, double fy, double fz) {
+        this.acc.x += fx * invMass;
+        this.acc.y += fy * invMass;
+        this.acc.z += fz * invMass;
+    }
+
+}

src/polyray/physics/ParticleLine.java

@@ -0,0 +1,68 @@
+package polyray.physics;
+
+import polyray.Vector3d;
+
+public class ParticleLine {
+
+    public final Particle a, b;
+    public final double length;
+    private final double stiffness;
+    private final double ratioA, ratioB;
+
+    public ParticleLine(Particle a, Particle b, double length, double stiffness) {
+        this.a = a;
+        this.b = b;
+        this.length = length;
+        this.stiffness = stiffness * 0.5d;
+        double massSum = a.mass + b.mass;
+        this.ratioA = a.mass / massSum;
+        this.ratioB = b.mass / massSum;
+    }
+
+    public void applyRotationalForce(Vector3d axis, double force) {
+        Vector3d midpoint = new Vector3d(
+                a.pos.x * ratioA + b.pos.x * ratioB,
+                a.pos.y * ratioA + b.pos.y * ratioB,
+                a.pos.z * ratioA + b.pos.z * ratioB
+        );
+        
+        // This does not account for particle mass but is enough for now
+        Vector3d ra = new Vector3d(a.pos.x - midpoint.x, a.pos.y - midpoint.y, a.pos.z - midpoint.z);
+        Vector3d rb = new Vector3d(b.pos.x - midpoint.x, b.pos.y - midpoint.y, b.pos.z - midpoint.z);
+
+        Vector3d torqueA = Vector3d.mul(Vector3d.cross(ra, axis), force);
+        Vector3d torqueB = Vector3d.mul(Vector3d.cross(rb, axis), force);
+
+        a.applyForce(torqueA);
+        b.applyForce(torqueB);
+    }
+
+    public void update() {
+        double dx = b.pos.x - a.pos.x;
+        double dy = b.pos.y - a.pos.y;
+        double dz = b.pos.z - a.pos.z;
+        double dist = Math.sqrt(dx * dx + dy * dy + dz * dz);
+        double diff = (dist - length) * stiffness;
+        double mul = diff / dist;
+        dx *= mul;
+        dy *= mul;
+        dz *= mul;
+        double sum = a.mass + b.mass;
+        double ratio1 = a.mass / sum;
+        double ratio2 = b.mass / sum;
+        a.pos.x += dx * ratio1;
+        a.pos.y += dy * ratio1;
+        a.pos.z += dz * ratio1;
+        b.pos.x -= dx * ratio2;
+        b.pos.y -= dy * ratio2;
+        b.pos.z -= dz * ratio2;
+    }
+
+    public static final ParticleLine[] connectAllLines(Particle p, Particle[] particles, double stiffness) {
+        ParticleLine[] lines = new ParticleLine[particles.length];
+        for (int i = 0; i < particles.length; i++) {
+            lines[i] = new ParticleLine(p, particles[i], Vector3d.length(Vector3d.sub(p.pos, particles[i].pos)), stiffness);
+        }
+        return lines;
+    }
+}

src/polyray/physics/ParticleLineN.java

@@ -0,0 +1,93 @@
+package polyray.physics;
+
+import polyray.Vector3d;
+
+public class ParticleLineN {
+
+    private final Particle[] particles;
+    private final Vector3d[] pointVectors;
+    private final double invAmt;
+    private final double[] segmentLengths;
+    public final Vector3d lineOrigin = new Vector3d();
+    public final Vector3d lineVector = new Vector3d();
+
+    public ParticleLineN(double[] segmentLengths, Particle[] particles) {
+        this.segmentLengths = segmentLengths;
+        this.particles = particles;
+        this.pointVectors = new Vector3d[particles.length];
+        this.invAmt = 1.0d / particles.length;
+    }
+
+    public void update() {
+        lineOrigin.x = 0.0d;
+        lineOrigin.y = 0.0d;
+        lineOrigin.z = 0.0d;
+        for (Particle p : particles) {
+            lineOrigin.x += p.pos.x;
+            lineOrigin.y += p.pos.y;
+            lineOrigin.z += p.pos.z;
+        }
+        lineOrigin.x *= invAmt;
+        lineOrigin.y *= invAmt;
+        lineOrigin.z *= invAmt;
+
+        lineVector.x = 0.0d;
+        lineVector.y = 0.0d;
+        lineVector.z = 0.0d;
+        int i = 0;
+        for (Particle p : particles) {
+            Vector3d dir = Vector3d.sub(lineOrigin, p.pos);
+            pointVectors[i++] = dir;
+            if (Vector3d.dot(lineVector, dir) < 0.0d) {
+                lineVector.x -= dir.x;
+                lineVector.y -= dir.y;
+                lineVector.z -= dir.z;
+            } else {
+                lineVector.x += dir.x;
+                lineVector.y += dir.y;
+                lineVector.z += dir.z;
+            }
+        }
+        double mul = 1.0d / Vector3d.length(lineVector);
+        lineVector.x *= mul;
+        lineVector.y *= mul;
+        lineVector.z *= mul;
+        
+        i = 0;
+        for (Particle p : particles) {
+            Vector3d pointVector = pointVectors[i++];
+            Vector3d lineNormalBitangent = Vector3d.cross(lineVector, pointVector);
+            Vector3d lineNormal = Vector3d.normalize(Vector3d.cross(lineVector, lineNormalBitangent));
+            double dist = Vector3d.dot(pointVector, lineNormal);
+            if (Math.abs(dist) < 0.0001d) {
+                continue;
+            }
+            Vector3d correction = Vector3d.mul(lineNormal, dist);
+            p.pos.x += correction.x;
+            p.pos.y += correction.y;
+            p.pos.z += correction.z;
+        }
+        for (int j = 0; j < 10; j++) {
+            Particle a = particles[0];
+            for (i = 1; i < particles.length; i++) {
+                Particle b = particles[i];
+                applyLine(lineVector, a, b, (Vector3d.dot(Vector3d.sub(b.pos, a.pos), lineVector) - segmentLengths[i - 1]) * 0.5d);
+                a = b;
+            }
+        }
+    }
+
+    private void applyLine(Vector3d dir, Particle a, Particle b, double diff) {
+        double sum = a.mass + b.mass;
+        double ratio1 = a.mass / sum;
+        double ratio2 = b.mass / sum;
+        Vector3d ca = Vector3d.mul(dir, diff * ratio1);
+        Vector3d cb = Vector3d.mul(dir, -diff * ratio2);
+        a.pos.x += ca.x;
+        a.pos.y += ca.y;
+        a.pos.z += ca.z;
+        b.pos.x += cb.x;
+        b.pos.y += cb.y;
+        b.pos.z += cb.z;
+    }
+}

src/polyray/physics/ParticlePlane.java

@@ -0,0 +1,67 @@
+package polyray.physics;
+
+import polyray.Vector3d;
+
+public class ParticlePlane {
+
+    private final Particle[] particles;
+    private final Vector3d[] pointVectors;
+    private final double invAmt;
+    public final Vector3d planeOrigin = new Vector3d();
+    public final Vector3d planeNormal = new Vector3d();
+
+    public ParticlePlane(Particle[] particles) {
+        this.particles = particles;
+        this.pointVectors = new Vector3d[particles.length];
+        this.invAmt = 1.0d / particles.length;
+    }
+
+    public void update() {
+        planeOrigin.x = 0.0d;
+        planeOrigin.y = 0.0d;
+        planeOrigin.z = 0.0d;
+        for (Particle p : particles) {
+            planeOrigin.x += p.pos.x;
+            planeOrigin.y += p.pos.y;
+            planeOrigin.z += p.pos.z;
+        }
+        planeOrigin.x *= invAmt;
+        planeOrigin.y *= invAmt;
+        planeOrigin.z *= invAmt;
+
+        int i = 0;
+        planeNormal.x = 0.0d;
+        planeNormal.y = 0.0d;
+        planeNormal.z = 0.0d;
+        Vector3d prevPointVector = Vector3d.sub(particles[particles.length - 1].pos, planeOrigin);
+        for (Particle p : particles) {
+            Vector3d pointVector = Vector3d.sub(p.pos, planeOrigin);
+            pointVectors[i++] = pointVector;
+            Vector3d normal = Vector3d.cross(pointVector, prevPointVector);
+            double weight = Vector3d.length(normal);
+            if (Vector3d.dot(normal, planeNormal) < 0.0d) {
+                planeNormal.x -= normal.x * weight;
+                planeNormal.y -= normal.y * weight;
+                planeNormal.z -= normal.z * weight;
+            } else {
+                planeNormal.x += normal.x * weight;
+                planeNormal.y += normal.y * weight;
+                planeNormal.z += normal.z * weight;
+            }
+            prevPointVector = pointVector;
+        }
+        double mul = 1.0d / Vector3d.length(planeNormal);
+        planeNormal.x *= mul;
+        planeNormal.y *= mul;
+        planeNormal.z *= mul;
+
+        i = 0;
+        for (Particle p : particles) {
+            double height = Vector3d.dot(pointVectors[i++], planeNormal);
+            Vector3d correction = Vector3d.mul(planeNormal, -height);
+            p.pos.x += correction.x;
+            p.pos.y += correction.y;
+            p.pos.z += correction.z;
+        }
+    }
+}

src/polyray/physics/ParticleSphere.java

@@ -0,0 +1,43 @@
+package polyray.physics;
+
+import polyray.Vector3d;
+
+public class ParticleSphere {
+
+    private final Particle[] particles;
+    private final double invAmt;
+    private final double radius;
+    public final Vector3d sphereOrigin = new Vector3d();
+
+    public ParticleSphere(double radius, Particle... particles) {
+        this.radius = radius;
+        this.particles = particles;
+        this.invAmt = 1.0d / particles.length;
+    }
+
+    public void update() {
+        sphereOrigin.x = 0.0d;
+        sphereOrigin.y = 0.0d;
+        sphereOrigin.z = 0.0d;
+        for (Particle p : particles) {
+            sphereOrigin.x += p.pos.x;
+            sphereOrigin.y += p.pos.y;
+            sphereOrigin.z += p.pos.z;
+        }
+        sphereOrigin.x *= invAmt;
+        sphereOrigin.y *= invAmt;
+        sphereOrigin.z *= invAmt;
+
+        for (Particle p : particles) {
+            Vector3d pointVector = Vector3d.sub(p.pos, sphereOrigin);
+            double len = Vector3d.length(pointVector);
+            double mul = (radius - len) / len;
+            pointVector.x *= mul;
+            pointVector.y *= mul;
+            pointVector.z *= mul;
+            p.pos.x += pointVector.x;
+            p.pos.y += pointVector.y;
+            p.pos.z += pointVector.z;
+        }
+    }
+}

src/polyray/physics/PhysicsObject.java

@@ -0,0 +1,57 @@
+package polyray.physics;
+
+import polyray.Vector3d;
+import polyray.Vector3f;
+
+public class PhysicsObject {
+
+    public final Vector3d pos = new Vector3d();
+    public final Vector3d prevPos = new Vector3d();
+    public final Vector3d acc = new Vector3d();
+    public final float friction, restitution;
+
+    public PhysicsObject(float friction, float restitution) {
+        this.friction = friction;
+        this.restitution = restitution;
+    }
+
+    public PhysicsObject(Vector3d pos, float friction, float restitution) {
+        this.pos.x = pos.x;
+        this.pos.y = pos.y;
+        this.pos.z = pos.z;
+        this.prevPos.x = pos.x;
+        this.prevPos.y = pos.y;
+        this.prevPos.z = pos.z;
+        this.friction = friction;
+        this.restitution = restitution;
+    }
+
+    public void accelerate(Vector3d a) {
+        acc.x += a.x;
+        acc.y += a.y;
+        acc.z += a.z;
+    }
+
+    public void update(double dt) {
+        double tx = pos.x;
+        double ty = pos.y;
+        double tz = pos.z;
+        double ddt = dt * dt;
+        pos.x += pos.x - prevPos.x + acc.x * ddt;
+        pos.y += pos.y - prevPos.y + acc.y * ddt;
+        pos.z += pos.z - prevPos.z + acc.z * ddt;
+        prevPos.x = tx;
+        prevPos.y = ty;
+        prevPos.z = tz;
+        acc.x = 0.0d;
+        acc.y = 0.0d;
+        acc.z = 0.0d;
+    }
+
+    protected Vector3f bounce(Vector3f v, Vector3f n) {
+        float height = -Vector3f.dot(v, n);
+        Vector3f normVel = Vector3f.mul(n, height);
+        Vector3f perp = Vector3f.mul(Vector3f.add(v, normVel), 1.0f - friction);
+        return Vector3f.add(perp, Vector3f.mul(normVel, restitution));
+    }
+}