Commits

committed 014efb7

Implement packed linear constraints and solver.

• Participants
• Parent commits cf53903

File ferox-physics/src/main/java/com/ferox/physics/dynamics/LinearConstraintPool.java

`+package com.ferox.physics.dynamics;`
`+`
`+import java.util.Arrays;`
`+`
`+import com.ferox.math.Const;`
`+import com.ferox.math.Matrix3;`
`+import com.ferox.math.Vector3;`
`+import com.ferox.math.entreri.Matrix3Property;`
`+import com.ferox.math.entreri.Vector3Property;`
`+import com.lhkbob.entreri.property.DoubleProperty;`
`+`
`+public class LinearConstraintPool {`
`+    private int count; // current number of valid constraints in the pool`
`+    `
`+    /*`
`+     * Packed primitive structures for a linear constraint`
`+     */`
`+    private double[] constraintForceMixes = new double[0]; // produces soft constraints`
`+    private double[] solutions = new double[0]; // current solution of the diff eq. to solve`
`+    `
`+    private double[] jacobianDiagInverses = new double[0]; // inverse of jacobian's diagonal`
`+    `
`+    private double[] directions = new double[0]; // (Vector3) constraint direction`
`+    private double[] torqueAs = new double[0]; // (Vector3) angular torque on A about direction`
`+    private double[] torqueBs = new double[0]; // (Vector3) angular torque on B about direction`
`+    `
`+    private double[] linearDirAs = new double[0]; // (Vector3) constraint direction scaled by A's inverse mass`
`+    private double[] linearDirBs = new double[0]; // (Vector3) constraint direction scaled by B's inverse mass`
`+    private double[] angleDirAs = new double[0]; // (Vector3) angular torque transformed by A's inertia tensor inverse`
`+    private double[] angleDirBs = new double[0]; // (Vector3) angular torque transformed by B's inertia tensor inverse`
`+    `
`+    private double[] appliedImpulses = new double[0]; // intermediate soln. during iterative solving`
`+    private double[] upperLimits = new double[0]; // upper limit on any change in impulse`
`+    private double[] lowerLimits = new double[0]; // lower limit on any change in impulse`
`+    `
`+    // this int[] stores indexes back into this pool of constraints`
`+    private int[] dynamicLimits = new int[0]; // dynamically limit impulse by impulse of other constraint`
`+    private double[] dynamicScaleFactors = new double[0]; // scale factor applied to dynamic limits`
`+ `
`+    // these two int[] store indexes into the RigidBody table in the system`
`+    private int[] bodyAs = new int[0];`
`+    private int[] bodyBs = new int[0];`
`+    `
`+    // accessed by values inside bodyAs and bodyBs`
`+    private Matrix3Property inertiaTensorInverses;`
`+    private DoubleProperty inverseMasses;`
`+    private Vector3Property deltaLinearImpulses;`
`+    private Vector3Property deltaAngularImpulses;`
`+    `
`+    private final Vector3 direction = new Vector3();`
`+    private final Vector3 torqueA = new Vector3();`
`+    private final Vector3 torqueB = new Vector3();`
`+    private final Matrix3 tensor = new Matrix3();`
`+    `
`+    // FIXME we might be able to avoid this if we have the manifolds remember`
`+    // the indices of the constraints they produce and as part of the final`
`+    // update phase, they read back the total applied impulse`
`+//    private Object impulseListener;`
`+    `
`+    public LinearConstraintPool() {`
`+        count = 0;`
`+        setCapacity(10);`
`+    }`
`+    `
`+    public void setInertiaTensorInverseProperty(Matrix3Property prop) {`
`+        inertiaTensorInverses = prop;`
`+    }`
`+    `
`+    public void setInverseMassProperty(DoubleProperty prop) {`
`+        inverseMasses = prop;`
`+    }`
`+    `
`+    public void setDeltaLinearImpulseProperty(Vector3Property prop) {`
`+        deltaLinearImpulses = prop;`
`+    }`
`+    `
`+    public void setDeltaAngularImpulseProperty(Vector3Property prop) {`
`+        deltaAngularImpulses = prop;`
`+    }`
`+    `
`+    public void setCapacity(int newCapacity) {`
`+        constraintForceMixes = Arrays.copyOf(constraintForceMixes, newCapacity);`
`+        solutions = Arrays.copyOf(solutions, newCapacity);`
`+        jacobianDiagInverses = Arrays.copyOf(jacobianDiagInverses, newCapacity);`
`+        directions = Arrays.copyOf(directions, 3 * newCapacity);`
`+        torqueAs = Arrays.copyOf(torqueAs, 3 * newCapacity);`
`+        torqueBs = Arrays.copyOf(torqueBs, 3 * newCapacity);`
`+        linearDirAs = Arrays.copyOf(linearDirAs, 3 * newCapacity);`
`+        linearDirBs = Arrays.copyOf(linearDirBs, 3 * newCapacity);`
`+        angleDirAs = Arrays.copyOf(angleDirAs, 3 * newCapacity);`
`+        angleDirBs = Arrays.copyOf(angleDirBs, 3 * newCapacity);`
`+        appliedImpulses = Arrays.copyOf(appliedImpulses, newCapacity);`
`+        upperLimits = Arrays.copyOf(upperLimits, newCapacity);`
`+        lowerLimits = Arrays.copyOf(lowerLimits, newCapacity);`
`+        dynamicScaleFactors = Arrays.copyOf(dynamicScaleFactors, newCapacity);`
`+        dynamicLimits = Arrays.copyOf(dynamicLimits, newCapacity);`
`+        bodyAs = Arrays.copyOf(bodyAs, newCapacity);`
`+        bodyBs = Arrays.copyOf(bodyBs, newCapacity);`
`+        // FIXME allocate listeners too, unless we're not using them here`
`+        `
`+        count = Math.min(newCapacity, count);`
`+    }`
`+    `
`+    public void clear() {`
`+        count = 0;`
`+    }`
`+`
`+    public int addConstraint(int bodyA, int bodyB) {`
`+        int i = count++;`
`+        int veci = i * 3;`
`+        if (i >= bodyAs.length) {`
`+            // increase capacity (since capacity starts at 10, we know i != 0)`
`+            setCapacity(i * 2);`
`+        }`
`+        `
`+        bodyAs[i] = bodyA;`
`+        bodyBs[i] = bodyB;`
`+        `
`+        // set vectors to 0`
`+        // FIXME if 0'ing these values are too expensive, we can combine this`
`+        // method with setConstraintAxis so we just overwrite with new data`
`+        directions[veci] = 0; directions[veci + 1] = 0; directions[veci + 2] = 0;`
`+        torqueAs[veci] = 0; torqueAs[veci + 1] = 0; torqueAs[veci + 2] = 0;`
`+        torqueBs[veci] = 0; torqueBs[veci + 1] = 0; torqueBs[veci + 2] = 0;`
`+        linearDirAs[veci] = 0; linearDirAs[veci + 1] = 0; linearDirAs[veci + 2] = 0;`
`+        linearDirBs[veci] = 0; linearDirBs[veci + 1] = 0; linearDirBs[veci + 2] = 0;`
`+        angleDirAs[veci] = 0; angleDirAs[veci + 1] = 0; angleDirAs[veci + 2] = 0;`
`+        angleDirBs[veci] = 0; angleDirBs[veci + 1] = 0; angleDirBs[veci + 2] = 0;`
`+`
`+        // zero out other values, too`
`+        jacobianDiagInverses[i] = 0;`
`+        solutions[i] = 0;`
`+        constraintForceMixes[i] = 0;`
`+        `
`+        lowerLimits[i] = -Double.MAX_VALUE;`
`+        upperLimits[i] = Double.MAX_VALUE;`
`+        dynamicScaleFactors[i] = 0;`
`+        dynamicLimits[i] = -1;`
`+        `
`+        appliedImpulses[i] = 0;`
`+        `
`+        return i;`
`+    }`
`+    `
`+    public void addDeltaImpulse(int i, double deltaImpulse) {`
`+        int veci = i * 3;`
`+        appliedImpulses[i] += deltaImpulse;`
`+        `
`+        int ba = bodyAs[i];`
`+        if (ba >= 0) {`
`+            deltaLinearImpulses.get(ba, direction);`
`+            direction.x += deltaImpulse * linearDirAs[veci];`
`+            direction.y += deltaImpulse * linearDirAs[veci + 1];`
`+            direction.z += deltaImpulse * linearDirAs[veci + 2];`
`+            deltaLinearImpulses.set(direction, ba);`
`+            `
`+            deltaAngularImpulses.get(ba, direction);`
`+            direction.x += deltaImpulse * angleDirAs[veci];`
`+            direction.y += deltaImpulse * angleDirAs[veci + 1];`
`+            direction.z += deltaImpulse * angleDirAs[veci + 2];`
`+            deltaAngularImpulses.set(direction, ba);`
`+        }`
`+        `
`+        int bb = bodyBs[i];`
`+        if (bb >= 0) {`
`+            deltaLinearImpulses.get(bb, direction);`
`+            direction.x += deltaImpulse * linearDirBs[veci];`
`+            direction.y += deltaImpulse * linearDirBs[veci + 1];`
`+            direction.z += deltaImpulse * linearDirBs[veci + 2];`
`+            deltaLinearImpulses.set(direction, bb);`
`+            `
`+            deltaAngularImpulses.get(bb, direction);`
`+            direction.x += deltaImpulse * angleDirBs[veci];`
`+            direction.y += deltaImpulse * angleDirBs[veci + 1];`
`+            direction.z += deltaImpulse * angleDirBs[veci + 2];`
`+            deltaAngularImpulses.set(direction, bb);`
`+        }`
`+    }`
`+    `
`+    public void setAppliedImpulse(int i, double impulse) {`
`+        appliedImpulses[i] = impulse;`
`+    }`
`+    `
`+    public void setConstraintAxis(int i, @Const Vector3 direction, @Const Vector3 torqueA, @Const Vector3 torqueB) {`
`+        int veci = i * 3;`
`+        `
`+        direction.get(directions, veci);`
`+        torqueA.get(torqueAs, veci);`
`+        torqueB.get(torqueBs, veci);`
`+        `
`+        int ba = bodyAs[i];`
`+        if (ba >= 0) {`
`+            double imA = inverseMasses.get(ba);`
`+            linearDirAs[veci] = direction.x * imA;`
`+            linearDirAs[veci + 1] = direction.y * imA;`
`+            linearDirAs[veci + 2] = direction.z * imA;`
`+            `
`+            this.torqueA.mul(inertiaTensorInverses.get(ba, tensor), torqueA);`
`+            angleDirAs[veci] = this.torqueA.x;`
`+            angleDirAs[veci + 1] = this.torqueA.y;`
`+            angleDirAs[veci + 2] = this.torqueA.z;`
`+        }`
`+        `
`+        int bb = bodyBs[i];`
`+        if (bb >= 0) {`
`+            double imB = inverseMasses.get(bb);`
`+            linearDirBs[veci] = direction.x * imB;`
`+            linearDirBs[veci + 1] = direction.y * imB;`
`+            linearDirBs[veci + 2] = direction.z * imB;`
`+            `
`+            this.torqueB.mul(inertiaTensorInverses.get(bb, tensor), torqueB);`
`+            angleDirBs[veci] = this.torqueB.x;`
`+            angleDirBs[veci + 1] = this.torqueB.y;`
`+            angleDirBs[veci + 2] = this.torqueB.z;`
`+        }`
`+    }`
`+    `
`+    public void setSolution(int i, double solution, double cfm) {`
`+        solutions[i] = solution;`
`+        constraintForceMixes[i] = cfm;`
`+    }`
`+    `
`+    public void setJacobianDiagonalInverse(int i, double jacobian) {`
`+        jacobianDiagInverses[i] = jacobian;`
`+    }`
`+    `
`+    public void setStaticLimits(int i, double lower, double upper) {`
`+        if (lower > upper)`
`+            throw new IllegalArgumentException("Lower limit (" + lower + ") must be less than upper limit (" + upper + ")");`
`+        lowerLimits[i] = lower;`
`+        upperLimits[i] = upper;`
`+        dynamicLimits[i] = -1; // clear dynamic limit`
`+    }`
`+    `
`+    public void setDynamicLimits(int i, int linkedConstraint, double scale) {`
`+        if (linkedConstraint < 0 || linkedConstraint > count || scale <= 0)`
`+            throw new IllegalArgumentException("Constraint index (" + linkedConstraint + ") is invalid, or scale (" + scale + ") is not positive");`
`+        dynamicLimits[i] = linkedConstraint;`
`+        dynamicScaleFactors[i] = scale;`
`+    }`
`+    `
`+    public @Const Vector3 getConstraintDirection(int i) {`
`+        return direction.set(directions, i * 3);`
`+    }`
`+    `
`+    public @Const Vector3 getTorqueA(int i) {`
`+        return torqueA.set(torqueAs, i * 3);`
`+    }`
`+    `
`+    public @Const Vector3 getTorqueB(int i) {`
`+        return torqueB.set(torqueBs, i * 3);`
`+    }`
`+    `
`+    public double getJacobianDiagonalInverse(int i) {`
`+        return jacobianDiagInverses[i];`
`+    }`
`+    `
`+    public double getLowerImpulseLimit(int i) {`
`+        int dynamic = dynamicLimits[i];`
`+        if (dynamic >= 0) {`
`+            return -dynamicScaleFactors[i] * appliedImpulses[dynamic];`
`+        } else {`
`+            return lowerLimits[i];`
`+        }`
`+    }`
`+    `
`+    public double getUpperImpulseLimit(int i) {`
`+        int dynamic = dynamicLimits[i];`
`+        if (dynamic >= 0) {`
`+            return dynamicScaleFactors[i] * appliedImpulses[dynamic];`
`+        } else {`
`+            return upperLimits[i];`
`+        }`
`+    }`
`+    `
`+    public double getSolution(int i) {`
`+        return solutions[i];`
`+    }`
`+    `
`+    public double getConstraintForceMix(int i) {`
`+        return constraintForceMixes[i];`
`+    }`
`+    `
`+    public double getAppliedImpulse(int i) {`
`+        return appliedImpulses[i];`
`+    }`
`+    `
`+    public int getBodyAIndex(int i) {`
`+        return bodyAs[i];`
`+    }`
`+    `
`+    public int getBodyBIndex(int i) {`
`+        return bodyBs[i];`
`+    }`
`+    `
`+    public int getConstraintCount() {`
`+        return count;`
`+    }`
`+}`

File ferox-physics/src/main/java/com/ferox/physics/dynamics/LinearConstraintSolver.java

`+package com.ferox.physics.dynamics;`
`+`
`+import java.util.Random;`
`+`
`+import com.ferox.math.Vector3;`
`+import com.ferox.math.entreri.Vector3Property;`
`+`
`+public class LinearConstraintSolver {`
`+    private final Random shuffler;`
`+    `
`+    private boolean shuffleConstraints;`
`+    private boolean shuffleEachIteration;`
`+    private int numIterations;`
`+    `
`+    // solver body access`
`+    private Vector3Property deltaLinearImpulse;`
`+    private Vector3Property deltaAngularImpulse;`
`+    `
`+    private final Vector3 linear = new Vector3();`
`+    private final Vector3 angular = new Vector3();`
`+    `
`+    public LinearConstraintSolver() {`
`+        shuffler = new Random();`
`+        setShuffleConstraints(true);`
`+        setShuffleEveryIteration(true);`
`+        setIterationCount(10);`
`+    }`
`+    `
`+    public void setDeltaLinearImpulseProperty(Vector3Property prop) {`
`+        deltaLinearImpulse = prop;`
`+    }`
`+    `
`+    public void setDeltaAngularImpulseProperty(Vector3Property prop) {`
`+        deltaAngularImpulse = prop;`
`+    }`
`+    `
`+    public void setShuffleConstraints(boolean shuffle) {`
`+        shuffleConstraints = shuffle;`
`+    }`
`+    `
`+    public void setShuffleEveryIteration(boolean shuffle) {`
`+        shuffleEachIteration = shuffle;`
`+    }`
`+    `
`+    public void setIterationCount(int numIters) {`
`+        if (numIters <= 0)`
`+            throw new IllegalArgumentException("Iteration count must be at least 1, not " + numIters);`
`+        numIterations = numIters;`
`+    }`
`+    `
`+    public boolean getShuffleConstraints() {`
`+        return shuffleConstraints;`
`+    }`
`+    `
`+    public boolean getShuffleEveryIteration() {`
`+        return shuffleEachIteration;`
`+    }`
`+    `
`+    public int getIterationCount() {`
`+        return numIterations;`
`+    }`
`+`
`+    public void solve(LinearConstraintPool... groups) {`
`+        if (shuffleConstraints) {`
`+            if (shuffleEachIteration) {`
`+                solveShuffle(groups);`
`+            } else {`
`+                solveShuffleOnce(groups);`
`+            }`
`+        } else {`
`+            solveNoShuffling(groups);`
`+        }`
`+    }`
`+    `
`+    private void solveNoShuffling(LinearConstraintPool[] groups) {`
`+        // with no shuffling we don't need an array of indices`
`+        int count;`
`+        LinearConstraintPool group;`
`+        for (int i = 0; i < numIterations; i++) {`
`+            for (int j = 0; j < groups.length; j++) {`
`+                group = groups[j];`
`+                count = group.getConstraintCount();`
`+                for (int k = 0; k < count; k++) {`
`+                    solveSingleConstraint(group, k);`
`+                }`
`+            }`
`+        }`
`+    }`
`+    `
`+    private void solveShuffleOnce(LinearConstraintPool[] groups) {`
`+        // since we're shuffling, we have to allocate these arrays`
`+        int[][] indices = createIndices(groups);`
`+        // shuffle one time at the very start`
`+        for (int i = 0; i < groups.length; i++) {`
`+            shuffle(indices[i]);`
`+        }`
`+        `
`+        int[] shuffled;`
`+        LinearConstraintPool group;`
`+        for (int i = 0; i < numIterations; i++) {`
`+            for (int j = 0; j < groups.length; j++) {`
`+                shuffled = indices[j];`
`+                group = groups[j];`
`+                for (int k = 0; k < shuffled.length; k++) {`
`+                    solveSingleConstraint(group, k);`
`+                }`
`+            }`
`+        }`
`+    }`
`+    `
`+    private void solveShuffle(LinearConstraintPool[] groups) {`
`+        // since we're shuffling, we have to allocate these arrays`
`+        int[][] indices = createIndices(groups);`
`+        `
`+        int[] shuffled;`
`+        LinearConstraintPool group;`
`+        for (int i = 0; i < numIterations; i++) {`
`+            for (int j = 0; j < groups.length; j++) {`
`+                shuffled = indices[j];`
`+                group = groups[j];`
`+                `
`+                // shuffle the indices every iteration`
`+                shuffle(shuffled);`
`+                `
`+                for (int k = 0; k < shuffled.length; k++) {`
`+                    solveSingleConstraint(group, k);`
`+                }`
`+            }`
`+        }`
`+    }`
`+    `
`+    private void solveSingleConstraint(LinearConstraintPool group, int constraint) {`
`+        double jacobian = group.getJacobianDiagonalInverse(constraint);`
`+        double deltaImpulse = group.getSolution(constraint);`
`+        `
`+        int ba = group.getBodyAIndex(constraint);`
`+        if (ba >= 0) {`
`+            deltaLinearImpulse.get(ba, linear);`
`+            deltaAngularImpulse.get(ba, angular);`
`+            `
`+            deltaImpulse += -jacobian * (group.getConstraintDirection(constraint).dot(linear) + group.getTorqueA(constraint).dot(angular));`
`+        }`
`+        `
`+        int bb = group.getBodyBIndex(constraint);`
`+        if (bb >= 0) {`
`+            deltaLinearImpulse.get(bb, linear);`
`+            deltaAngularImpulse.get(bb, angular);`
`+            `
`+            deltaImpulse += jacobian * (group.getConstraintDirection(constraint).dot(linear) + group.getTorqueB(constraint).dot(angular));`
`+        }`
`+        `
`+        double applied = group.getAppliedImpulse(constraint);`
`+        double totalImpulse = applied + deltaImpulse;`
`+        double lower = group.getLowerImpulseLimit(constraint);`
`+        double upper = group.getUpperImpulseLimit(constraint);`
`+        if (totalImpulse < lower) {`
`+            deltaImpulse = lower - applied;`
`+        } else if (totalImpulse > upper) {`
`+            deltaImpulse = upper - applied;`
`+        }`
`+        `
`+        group.addDeltaImpulse(constraint, deltaImpulse);`
`+    }`
`+    `
`+    private int[][] createIndices(LinearConstraintPool[] groups) {`
`+        int[][] indices = new int[groups.length][];`
`+        `
`+        for (int i = 0; i < indices.length; i++) {`
`+            int[] forGroup = new int[groups[i].getConstraintCount()];`
`+            for (int j = 0; j < forGroup.length; j++) {`
`+                forGroup[j] = j;`
`+            }`
`+            `
`+            indices[i] = forGroup;`
`+        }`
`+        `
`+        return indices;`
`+    }`
`+    `
`+    private void shuffle(int[] indices) {`
`+        for (int i = indices.length; i >= 1; i--) {`
`+            int swap = shuffler.nextInt(i);`
`+            int temp = indices[i - 1];`
`+            indices[i - 1] = indices[swap];`
`+            indices[swap] = temp;`
`+        }`
`+    }`
`+}`