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