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);
}
Aggregations