static void GetQuantizedTransformations(
float4x4 leafToBody, Aabb bounds,
out float3 translation, out quaternion orientationQ, out float3 scale, out float3x3 shear,
float linearPrecision = k_DefaultLinearPrecision
)
{
translation = RoundToNearest(leafToBody.c3.xyz, linearPrecision);
var farthestPoint =
math.abs(math.lengthsq(bounds.Max) > math.lengthsq(bounds.Min) ? bounds.Max : bounds.Min);
// round scale using precision inversely proportional to mesh size along largest axis
// (i.e. amount to scale farthest point one unit of linear precision)
var scalePrecision = linearPrecision / math.max(math.cmax(farthestPoint), math.FLT_MIN_NORMAL);
scale = RoundToNearest(leafToBody.DecomposeScale(), scalePrecision);
if (math.determinant(leafToBody) < 0f)
{
scale.x *= -1f;
}
shear = new float3x3(
leafToBody.c0.xyz / math.max(math.abs(scale.x), math.FLT_MIN_NORMAL) * math.sign(scale.x),
leafToBody.c1.xyz / math.max(math.abs(scale.y), math.FLT_MIN_NORMAL) * math.sign(scale.y),
leafToBody.c2.xyz / math.max(math.abs(scale.z), math.FLT_MIN_NORMAL) * math.sign(scale.z)
);
var orientation = float3x3.LookRotationSafe(shear.c2, shear.c1);
// if shear is very nearly identity, hash it as identity
// TODO: quantize shear
shear = math.mul(shear, math.inverse(orientation));
if (!PhysicsShapeExtensions.HasShear(new float4x4(shear, 0f)))
{
shear = float3x3.identity;
}
// round orientation using precision inversely proportional to scaled mesh size
// (i.e. radians to rotate farthest scaled point one unit of linear precision)
var angularPrecision = math.min(linearPrecision / math.length(farthestPoint * scale), math.PI);
var axisPrecision = math.min(math.cos(angularPrecision), math.sin(angularPrecision));
orientation.c0 = math.normalize(RoundToNearest(orientation.c0, axisPrecision));
orientation.c1 = math.normalize(RoundToNearest(orientation.c1, axisPrecision));
orientation.c2 = math.normalize(RoundToNearest(orientation.c2, axisPrecision));
translation = Sanitize(translation);
orientationQ = new quaternion(Sanitize(orientation));
scale = Sanitize(scale);
shear = Sanitize(shear);
}
