public void TestCalculateRelaxationBiasAtZeroVelocity()
{
var rnd = new Random(0);
var relaxedRotation = RandomUtil.UnitQuaternion(rnd);
var currentRotation = RandomUtil.UnitQuaternion(rnd);
Assert.AreEqual(1, InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, Vector3.Zero));
}
private BonePartialSolution SolveSingleBone(
RigidBone bone,
Vector3 worldSource, Vector3 worldTarget, MassMoment[] massMoments, Vector3 figureCenterOverride,
RigidBoneSystemInputs inputs, RigidTransform[] boneTransforms)
{
if (bone.Parent == boneSystem.RootBone)
{
//skip the hip bone because it's the same as the root bone but with a different center
return(BonePartialSolution.Zero);
}
var center = bone != boneSystem.RootBone ? boneTransforms[bone.Index].Transform(bone.CenterPoint) : figureCenterOverride;
var parentTotalRotation = bone.Parent != null ? boneTransforms[bone.Parent.Index].Rotation : Quaternion.Identity;
var boneToWorldSpaceRotation = bone.OrientationSpace.Orientation.Chain(parentTotalRotation);
var worldToBoneSpaceRotation = Quaternion.Invert(boneToWorldSpaceRotation);
var boneSpaceSource = Vector3.Transform(worldSource - center, worldToBoneSpaceRotation);
var boneSpaceTarget = Vector3.Transform(worldTarget - center, worldToBoneSpaceRotation);
var force = boneSpaceTarget - boneSpaceSource;
var torque = Vector3.Cross(boneSpaceSource, force);
float mass = boneAttributes[bone.Index].MassIncludingDescendants;
Vector3 unnormalizedAxisOfRotation = Vector3.Cross(worldSource - center, worldTarget - center);
float unnormalizedAxisOfRotationLength = unnormalizedAxisOfRotation.Length();
if (MathUtil.IsZero(unnormalizedAxisOfRotationLength))
{
return(BonePartialSolution.Zero);
}
Vector3 axisOfRotation = unnormalizedAxisOfRotation / unnormalizedAxisOfRotationLength;
float momentOfInertia = massMoments[bone.Index].GetMomentOfInertia(axisOfRotation, center);
var angularVelocity = torque / momentOfInertia;
var twistAxis = bone.RotationOrder.TwistAxis;
var existingRotation = bone.GetOrientedSpaceRotation(inputs).AsQuaternion(twistAxis);
var relaxedRotation = bone.Constraint.Center.AsQuaternion(twistAxis);
float relaxationBias = InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, existingRotation, angularVelocity);
angularVelocity *= relaxationBias;
var linearVelocity = Vector3.Cross(angularVelocity, boneSpaceSource);
var rotation = QuaternionExtensions.RotateBetween(
Vector3.Normalize(boneSpaceSource),
Vector3.Normalize(boneSpaceTarget));
var radius = boneSpaceSource.Length();
var distance = rotation.AccurateAngle() * radius;
float time = distance == 0 ? 0 : distance / linearVelocity.Length();
DebugUtilities.AssertFinite(angularVelocity);
return(new BonePartialSolution {
angularVelocity = angularVelocity,
time = time
});
}
public void TestCalculateRelaxationBiasAtFullyRelaxed()
{
var rnd = new Random(0);
var relaxedRotation = RandomUtil.UnitQuaternion(rnd);
var currentRotation = relaxedRotation;
Vector3 velocity = RandomUtil.Vector3(rnd);
Assert.AreEqual(1, InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, velocity));
}
public void TestCalculateRelaxationBiasScaleInvariance()
{
var rnd = new Random(0);
var relaxedRotation = RandomUtil.UnitQuaternion(rnd);
var currentRotation = RandomUtil.UnitQuaternion(rnd);
var velocity = RandomUtil.Vector3(rnd);
var expected = InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, velocity);
Assert.AreEqual(expected, InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, 2 * velocity), Acc);
Assert.AreEqual(expected, InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, 0.5f * velocity), Acc);
}
public void TestCalculateRelaxationBiasDirectionality()
{
var rnd = new Random(1);
var relaxedRotation = RandomUtil.UnitQuaternion(rnd);
var currentRotation = RandomUtil.UnitQuaternion(rnd);
var towardsRelaxedVelocity = Quaternion.Invert(currentRotation).Chain(relaxedRotation).ToRotationVector();
Assert.AreEqual(1.5f, InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, towardsRelaxedVelocity), Acc);
var awayFromRelaxedVelocity = -towardsRelaxedVelocity;
Assert.AreEqual(0.5f, InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, awayFromRelaxedVelocity), Acc);
var orthogonalToRelaxedVelocity = Vector3.Cross(Vector3.UnitX, towardsRelaxedVelocity);
Assert.AreEqual(1f, InverseKinematicsUtilities.CalculateRelaxationBias(relaxedRotation, currentRotation, orthogonalToRelaxedVelocity), Acc);
}
