Search in sources :

Example 56 with Transform

use of spacegraph.space3d.phys.math.Transform 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);
}
Also used : Matrix3f(spacegraph.util.math.Matrix3f) Transform(spacegraph.space3d.phys.math.Transform) spacegraph.util.math.v3(spacegraph.util.math.v3)

Aggregations

Transform (spacegraph.space3d.phys.math.Transform)56 spacegraph.util.math.v3 (spacegraph.util.math.v3)41 Matrix3f (spacegraph.util.math.Matrix3f)11 Collidable (spacegraph.space3d.phys.Collidable)6 Body3D (spacegraph.space3d.phys.Body3D)3 CollisionShape (spacegraph.space3d.phys.shape.CollisionShape)3 Quat4f (spacegraph.util.math.Quat4f)3 CompoundShape (spacegraph.space3d.phys.shape.CompoundShape)2 ConvexShape (spacegraph.space3d.phys.shape.ConvexShape)2 Surface (spacegraph.space2d.Surface)1 SimpleSpatial (spacegraph.space3d.SimpleSpatial)1 ManifoldPoint (spacegraph.space3d.phys.collision.narrow.ManifoldPoint)1 VoronoiSimplexSolver (spacegraph.space3d.phys.collision.narrow.VoronoiSimplexSolver)1 ContactConstraint (spacegraph.space3d.phys.constraint.ContactConstraint)1 Point2PointConstraint (spacegraph.space3d.phys.constraint.Point2PointConstraint)1 SolverConstraint (spacegraph.space3d.phys.constraint.SolverConstraint)1 TypedConstraint (spacegraph.space3d.phys.constraint.TypedConstraint)1 Generic6DofConstraint (spacegraph.space3d.phys.constraint.generic.Generic6DofConstraint)1 CapsuleShape (spacegraph.space3d.phys.shape.CapsuleShape)1 ConcaveShape (spacegraph.space3d.phys.shape.ConcaveShape)1