120121122123124125126127128129130
public void invXform(Vector3f inVec, Vector3f out) { out.sub(inVec, origin); Matrix3f mat = Stack.alloc(basis); mat.transpose(); mat.transform(out); } public Quat4f getRotation(Quat4f out) { MatrixUtil.getRotation(basis, out);
279280281282283284285286287288289
Vector3f o = Stack.alloc(Vector3f.class); o.sub(t.origin, center); // compute inertia tensor in coordinate system of compound shape Matrix3f j = Stack.alloc(Matrix3f.class); j.transpose(t.basis); j.m00 *= i.x; j.m01 *= i.x; j.m02 *= i.x; j.m10 *= i.y;
241242243244245246247248249
result.m02 = mv.m02; result.m12 = mv.m12; result.m22 = mv.m22; result.invert(); result.transpose(); return result; } }