public InverseKinematicsGoal(
RigidBone sourceBone, Vector3 unposedSourcePosition,
Vector3 targetPosition) : this(sourceBone,
unposedSourcePosition, Quaternion.Zero,
targetPosition, Quaternion.Zero)
{
}
private void MaybeStartTracking(FrameUpdateParameters updateParameters, RigidBoneSystemInputs inputs, ControlVertexInfo[] previousFrameControlVertexInfos)
{
if (tracking == true)
{
//already tracking
return;
}
if (!stateTracker.NonMenuActive)
{
return;
}
bool triggerPressed = stateTracker.IsPressed(EVRButtonId.k_EButton_SteamVR_Trigger);
if (!triggerPressed)
{
return;
}
tracking = true;
TrackedDevicePose_t gamePose = updateParameters.GamePoses[stateTracker.DeviceIdx];
Matrix controllerTransform = gamePose.mDeviceToAbsoluteTracking.Convert();
DualQuaternion controllerTransformDq = DualQuaternion.FromMatrix(controllerTransform);
var worldSourcePosition = controllerTransformDq.Translation * 100;
var worldSourceOrientation = controllerTransformDq.Rotation;
sourceBone = parentInstance.MapPositionToBone(worldSourcePosition, previousFrameControlVertexInfos);
var inverseSourceBoneTotalTransform = sourceBone.GetChainedTransform(inputs).Invert();
boneRelativeSourcePosition = inverseSourceBoneTotalTransform.Transform(worldSourcePosition) - sourceBone.CenterPoint;
boneRelativeSourceOrientation = worldSourceOrientation.Chain(inverseSourceBoneTotalTransform.Rotation);
}
public RigidBone(Bone source, RigidBone parent)
{
Source = source;
Index = source.Index;
Parent = parent;
RotationOrder = source.RotationOrder;
Constraint = source.RotationConstraint;
}
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 InverseKinematicsGoal(
RigidBone sourceBone,
Vector3 unposedSourcePosition, Quaternion unposedSourceOrientation,
Vector3 targetPosition, Quaternion targetOrientation)
{
SourceBone = sourceBone;
UnposedSourcePosition = unposedSourcePosition;
UnposedSourceOrientation = unposedSourceOrientation;
TargetPosition = targetPosition;
TargetOrientation = targetOrientation;
}
/**
* Set only the twist component of a bone's rotation. The original swing is preserved.
*/
public static void SetTwistOnly(this RigidBone bone, RigidBoneSystemInputs inputs, Quaternion localRotation)
{
var orientedRotation = bone.OrientationSpace.TransformToOrientedSpace(localRotation);
TwistSwing twistSwing = TwistSwing.Decompose(bone.RotationOrder.TwistAxis, orientedRotation);
var originalTwistSwing = inputs.Rotations[bone.Index];
var twistWithOriginalSwing = new TwistSwing(
twistSwing.Twist,
originalTwistSwing.Swing);
bone.SetOrientedSpaceRotation(inputs, twistWithOriginalSwing, true);
}
public RigidBoneSystem(BoneSystem source)
{
this.source = source;
bones = new RigidBone[source.Bones.Count];
for (int boneIdx = 0; boneIdx < bones.Length; ++boneIdx)
{
var sourceBone = source.Bones[boneIdx];
bones[boneIdx] = new RigidBone(source.Bones[boneIdx], sourceBone.Parent != null ? bones[sourceBone.Parent.Index] : null);
}
bonesByName = bones.ToDictionary(bone => bone.Source.Name, bone => bone);
}
public void Synchronize(ChannelOutputs outputs)
{
while (outputs.Parent != null)
{
outputs = outputs.Parent;
}
for (int boneIdx = 0; boneIdx < bones.Length; ++boneIdx)
{
RigidBone bone = bones[boneIdx];
bone.Synchronize(outputs);
}
}
private IEnumerable <RigidBone> GetBoneChain(RigidBone sourceBone, bool hasOrientationGoal)
{
for (var bone = sourceBone; bone != null; bone = bone.Parent)
{
if (areOrientable[bone.Index] && hasOrientationGoal)
{
//omit orientable bones if there's a orientation goal since rotation for those bone is already set
continue;
}
yield return(bone);
}
}
public RigidTransform[] GetBoneTransforms(RigidBoneSystemInputs inputs)
{
RigidTransform[] boneTransforms = new RigidTransform[bones.Length];
RigidTransform rootTransform = RigidTransform.FromTranslation(inputs.RootTranslation);
for (int boneIdx = 0; boneIdx < bones.Length; ++boneIdx)
{
RigidBone bone = bones[boneIdx];
RigidBone parent = bone.Parent;
RigidTransform parentTransform = parent != null ? boneTransforms[parent.Source.Index] : rootTransform;
boneTransforms[boneIdx] = bone.GetChainedTransform(inputs, parentTransform);
}
return(boneTransforms);
}
private void ApplyPartialSolution(RigidBone bone, BonePartialSolution partialSolution, RigidTransform[] boneTransforms, Vector3 figureCenterOverride, RigidBoneSystemInputs inputs, float time)
{
var twistAxis = bone.RotationOrder.TwistAxis;
var originalRotationQ = inputs.Rotations[bone.Index].AsQuaternion(twistAxis);
var rotationDelta = QuaternionExtensions.FromRotationVector(time * partialSolution.angularVelocity);
var newRotationQ = originalRotationQ.Chain(rotationDelta);
var newRotation = TwistSwing.Decompose(twistAxis, newRotationQ);
inputs.Rotations[bone.Index] = bone.Constraint.Clamp(newRotation);
if (bone == boneSystem.RootBone)
{
var preTotalTransform = boneTransforms[bone.Index];
var postTotalTransform = bone.GetChainedTransform(inputs);
var unposedFigureCenterOverride = preTotalTransform.InverseTransform(figureCenterOverride);
var postFigureCenterOverride = postTotalTransform.Transform(unposedFigureCenterOverride);
var centerDisplacement = figureCenterOverride - postFigureCenterOverride;
inputs.RootTranslation += centerDisplacement;
}
}
private void ApplyCorrection(RigidBoneSystemInputs inputs, RigidTransform[] boneTransforms, RigidBone bone, ref Vector3 sourcePosition, Vector3 targetPosition, float weight)
{
var centerPosition = GetCenterPosition(boneTransforms, bone);
var rotationCorrection = QuaternionExtensions.RotateBetween(
sourcePosition - centerPosition,
targetPosition - centerPosition);
var boneTransform = boneTransforms[bone.Index];
var baseLocalRotation = bone.GetRotation(inputs);
var localRotationCorrection = Quaternion.Invert(boneTransform.Rotation) * rotationCorrection * boneTransform.Rotation;
var lerpedRotation = Quaternion.Lerp(
baseLocalRotation,
baseLocalRotation * localRotationCorrection,
weight);
bone.SetRotation(inputs, lerpedRotation, true);
var newBoneTransform = bone.GetChainedTransform(inputs, bone.Parent != null ? boneTransforms[bone.Parent.Index] : RigidTransform.Identity);
var newSourcePosition = newBoneTransform.Transform(boneTransform.InverseTransform(sourcePosition));
sourcePosition = newSourcePosition;
}
private Vector3 GetCenterPosition(RigidTransform[] boneTransforms, RigidBone bone)
{
return(boneTransforms[bone.Index].Transform(bone.CenterPoint));
}
