Examples with Matrix3f spacegraph.util.math.Matrix3f used on opensource projects

Example 26 with Matrix3f

use of spacegraph.util.math.Matrix3f in project narchy by automenta.

the class CompoundShape method calculatePrincipalAxisTransform.

/**
 * Computes the exact moment of inertia and the transform from the coordinate
 * system defined by the principal axes of the moment of inertia and the center
 * of mass to the current coordinate system. "masses" points to an array
 * of masses of the children. The resulting transform "principal" has to be
 * applied inversely to all children transforms in order for the local coordinate
 * system of the compound shape to be centered at the center of mass and to coincide
 * with the principal axes. This also necessitates a correction of the world transform
 * of the collision object by the principal transform.
 */
public void calculatePrincipalAxisTransform(float[] masses, Transform principal, v3 inertia) {
    int n = children.size();
    float totalMass = 0;
    v3 center = new v3();
    center.set(0, 0, 0);
    for (int k = 0; k < n; k++) {
        // return array[index];
        center.scaleAdd(masses[k], children.get(k).transform, center);
        totalMass += masses[k];
    }
    center.scale(1f / totalMass);
    principal.set(center);
    Matrix3f tensor = new Matrix3f();
    tensor.setZero();
    for (int k = 0; k < n; k++) {
        v3 i = new v3();
        // return array[index];
        children.get(k).childShape.calculateLocalInertia(masses[k], i);
        // return array[index];
        Transform t = children.get(k).transform;
        v3 o = new v3();
        o.sub(t, center);
        // compute inertia tensor in coordinate system of compound shape
        Matrix3f j = new Matrix3f();
        j.transpose(t.basis);
        j.m00 *= i.x;
        j.m01 *= i.x;
        j.m02 *= i.x;
        j.m10 *= i.y;
        j.m11 *= i.y;
        j.m12 *= i.y;
        j.m20 *= i.z;
        j.m21 *= i.z;
        j.m22 *= i.z;
        j.mul(t.basis, j);
        // add inertia tensor
        tensor.add(j);
        // compute inertia tensor of pointmass at o
        float o2 = o.lengthSquared();
        j.setRow(0, o2, 0, 0);
        j.setRow(1, 0, o2, 0);
        j.setRow(2, 0, 0, o2);
        j.m00 += o.x * -o.x;
        j.m01 += o.y * -o.x;
        j.m02 += o.z * -o.x;
        j.m10 += o.x * -o.y;
        j.m11 += o.y * -o.y;
        j.m12 += o.z * -o.y;
        j.m20 += o.x * -o.z;
        j.m21 += o.y * -o.z;
        j.m22 += o.z * -o.z;
        // add inertia tensor of pointmass
        tensor.m00 += masses[k] * j.m00;
        tensor.m01 += masses[k] * j.m01;
        tensor.m02 += masses[k] * j.m02;
        tensor.m10 += masses[k] * j.m10;
        tensor.m11 += masses[k] * j.m11;
        tensor.m12 += masses[k] * j.m12;
        tensor.m20 += masses[k] * j.m20;
        tensor.m21 += masses[k] * j.m21;
        tensor.m22 += masses[k] * j.m22;
    }
    MatrixUtil.diagonalize(tensor, principal.basis, 0.00001f, 20);
    inertia.set(tensor.m00, tensor.m11, tensor.m22);
}