private void Solve(AnimationStream stream) { AnimationHumanStream humanStream = stream.AsHuman(); Vector3 bodyPosition = humanStream.bodyPosition; Vector3 bodyPositionDelta = Vector3.zero; float sumWeight = 0; for (int goalIter = 0; goalIter < 4; goalIter++) { float weight = humanStream.GetGoalWeightPosition((AvatarIKGoal)goalIter); weight = Mathf.Clamp01(weight); bodyPositionDelta += (humanStream.GetGoalPosition((AvatarIKGoal)goalIter) - humanStream.GetGoalPositionFromPose((AvatarIKGoal)goalIter)) * weight; sumWeight += weight; } if (sumWeight > 1) { bodyPositionDelta /= sumWeight; } bodyPosition += bodyPositionDelta; humanStream.bodyPosition = bodyPosition; if (bodyEffector.body.IsValid(stream)) { bodyEffector.body.SetPosition(stream, bodyPosition); } humanStream.SolveIK(); }
void StoreOutput(AnimationStream stream) { m_CurrentBufferIndex = (m_CurrentBufferIndex + 1) % k_OutputBufferCount; { var index = m_CurrentBufferIndex; for (var i = 0; i < m_Bones.Length; i++) { m_OutputPositions[index] = m_Bones[i].GetLocalPosition(stream); m_OutputRotations[index] = m_Bones[i].GetLocalRotation(stream); m_OutputScales[index] = m_Bones[i].GetLocalScale(stream); index += k_OutputBufferCount; } } if (stream.isHumanStream) { var humanStream = stream.AsHuman(); for (var i = 0; i < k_NumIkHandles; i++) { var index = i * k_OutputBufferCount + m_CurrentBufferIndex; m_OutputIkPositions[index] = humanStream.GetGoalLocalPosition((AvatarIKGoal)i); m_OutputIkRotations[index] = humanStream.GetGoalLocalRotation((AvatarIKGoal)i); } } m_StoredOutputCount++; }
private void SetBodyEffector(AnimationStream stream, ref BodyEffectorHandle handle) { if (handle.body.IsValid(stream)) { AnimationHumanStream humanStream = stream.AsHuman(); humanStream.bodyRotation = handle.body.GetRotation(stream); } }
private void SetHintEffector(AnimationStream stream, AvatarIKHint goal, ref HintEffectorHandle handle) { if (handle.hint.IsValid(stream) && handle.weight.IsValid(stream)) { AnimationHumanStream humanStream = stream.AsHuman(); humanStream.SetHintPosition(goal, handle.hint.GetPosition(stream)); humanStream.SetHintWeightPosition(goal, handle.weight.GetFloat(stream)); } }
private void SetEffector(AnimationStream stream, AvatarIKGoal goal, ref EffectorHandle handle) { if (handle.effector.IsValid(stream) && handle.positionWeight.IsValid(stream) && handle.rotationWeight.IsValid(stream)) { AnimationHumanStream humanStream = stream.AsHuman(); humanStream.SetGoalPosition(goal, handle.effector.GetPosition(stream)); humanStream.SetGoalRotation(goal, handle.effector.GetRotation(stream)); humanStream.SetGoalWeightPosition(goal, handle.positionWeight.GetFloat(stream)); humanStream.SetGoalWeightRotation(goal, handle.rotationWeight.GetFloat(stream)); } }
private void Solve(AnimationStream stream) { AnimationHumanStream humanStream = stream.AsHuman(); bodyPosition = humanStream.bodyPosition; Vector3 bodyPositionDelta = SolvePull(stream); bodyPosition += bodyPositionDelta; humanStream.bodyPosition = bodyPosition; humanStream.SolveIK(); }
private void SetLookAtEffector(AnimationStream stream, ref LookEffectorHandle handle) { if (handle.lookAt.IsValid(stream)) { AnimationHumanStream humanStream = stream.AsHuman(); humanStream.SetLookAtPosition(handle.lookAt.GetPosition(stream)); humanStream.SetLookAtEyesWeight(handle.eyesWeight.GetFloat(stream)); humanStream.SetLookAtHeadWeight(handle.headWeight.GetFloat(stream)); humanStream.SetLookAtBodyWeight(handle.bodyWeight.GetFloat(stream)); humanStream.SetLookAtClampWeight(handle.clampWeight.GetFloat(stream)); } }
public void ProcessAnimation(AnimationStream stream) { float weight; if (m_UseAnimatorProperty) { weight = m_AnimatorWeight.GetFloat(stream); weight += m_AnimatorWeightOffset.GetFloat(stream); weight = Mathf.Clamp01(weight); //m_WeightHandle.SetFloat(stream, weight); } else { weight = m_WeightHandle.GetFloat(stream); } weight = 1f; Vector3 effectorPosition; Quaternion effectorRotation; if (m_UseStreamEffector) { effectorPosition = m_EffectorStreamHandle.GetPosition(stream); effectorRotation = m_EffectorStreamHandle.GetRotation(stream); } else { effectorPosition = m_EffectorSceneHandle.GetPosition(stream); effectorRotation = m_EffectorSceneHandle.GetRotation(stream); } effectorRotation *= Quaternion.Euler(m_TargetOffset); if (m_IkType == IkType.Generic) { SolveTwoBoneIK(stream, m_StartHandle, m_MidHandle, m_EndHandle, effectorPosition, effectorRotation, weight, weight); } else if (m_IkType == IkType.Humanoid) { if (stream.isHumanStream) { var humanStream = stream.AsHuman(); humanStream.SetGoalPosition(m_HumanLimb, effectorPosition); humanStream.SetGoalRotation(m_HumanLimb, effectorRotation); humanStream.SetGoalWeightPosition(m_HumanLimb, weight); humanStream.SetGoalWeightRotation(m_HumanLimb, weight); humanStream.SolveIK(); } } }
public void ProcessAnimation(AnimationStream output) { if (output.isHumanStream) { var humanOutput = output.AsHuman(); ApplyLimbData(humanOutput, leftFoot, AvatarIKGoal.LeftFoot, AvatarIKHint.LeftKnee); ApplyLimbData(humanOutput, rightFoot, AvatarIKGoal.RightFoot, AvatarIKHint.RightKnee); ApplyLimbData(humanOutput, leftHand, AvatarIKGoal.LeftHand, AvatarIKHint.LeftElbow); ApplyLimbData(humanOutput, rightHand, AvatarIKGoal.RightHand, AvatarIKHint.RightElbow); ApplyLookData(humanOutput, look); humanOutput.SolveIK(); } }
private void Solve(AnimationStream stream) { AnimationHumanStream humanStream = stream.AsHuman(); Vector3 bodyPosition = humanStream.bodyPosition; Vector3 bodyPositionDelta = SolvePull(stream); bodyPosition += bodyPositionDelta; humanStream.bodyPosition = bodyPosition; if (bodyEffector.body.IsValid(stream)) { bodyEffector.body.SetPosition(stream, bodyPosition); } humanStream.SolveIK(); }
private void PrepareSolvePull(AnimationStream stream, NativeArray <LimbPart> limbParts) { AnimationHumanStream humanStream = stream.AsHuman(); Vector3 bodyPosition = humanStream.bodyPosition; for (int goalIter = 0; goalIter < 4; goalIter++) { var effector = GetEffectorHandle((AvatarIKGoal)goalIter); var limbHandle = GetIKLimbHandle((AvatarIKGoal)goalIter); Vector3 top = limbHandle.top.GetPosition(stream); limbParts[goalIter] = new LimbPart { localPosition = top - bodyPosition, goalPosition = humanStream.GetGoalPosition((AvatarIKGoal)goalIter), goalWeight = humanStream.GetGoalWeightPosition((AvatarIKGoal)goalIter), goalPullWeight = effector.pullWeight.GetFloat(stream), maximumExtension = limbHandle.maximumExtension, stiffness = stiffness }; } }
private Vector3 SolvePull(AnimationStream stream) { AnimationHumanStream humanStream = stream.AsHuman(); Vector3 originalBodyPosition = humanStream.bodyPosition; Vector3 bodyPosition = originalBodyPosition; NativeArray <LimbPart> limbParts = new NativeArray <LimbPart>(4, Allocator.Temp); PrepareSolvePull(stream, limbParts); for (int iter = 0; iter < maxPullIteration; iter++) { Vector3 deltaPosition = Vector3.zero; for (int goalIter = 0; goalIter < 4; goalIter++) { Vector3 top = bodyPosition + limbParts[goalIter].localPosition; Vector3 localForce = limbParts[goalIter].goalPosition - top; float restLenght = limbParts[goalIter].maximumExtension; float currentLenght = localForce.magnitude; localForce.Normalize(); var force = Mathf.Max(limbParts[goalIter].stiffness * (currentLenght - restLenght), 0.0f); deltaPosition += (localForce * force * limbParts[goalIter].goalPullWeight * limbParts[goalIter].goalWeight); } deltaPosition /= (maxPullIteration - iter); bodyPosition += deltaPosition; } limbParts.Dispose(); return(bodyPosition - originalBodyPosition); }
public void ProcessAnimation(AnimationStream stream) { Vector3 rootPosition; Quaternion rootRotation; root.GetGlobalTR(stream, out rootPosition, out rootRotation); var rootTx = new AffineTransform(rootPosition, rootRotation); var mirroredTransforms = new NativeArray <AffineTransform> (mirroringTransforms.Length, Allocator.Temp); // 追加トランスフォームのミラーリング計算 if (mirror) { for (int i = 0; i < mirroringTransforms.Length; i++) { if (!mirroringTransforms[i].source.IsValid(stream)) { continue; } if (!mirroringTransforms[i].driven.IsValid(stream)) { continue; } Vector3 position; Quaternion rotation; mirroringTransforms[i].source.GetGlobalTR(stream, out position, out rotation); var drivenTx = new AffineTransform(position, rotation); drivenTx = rootTx.Inverse() * drivenTx; drivenTx = AnimationStreamMirrorExtensions.Mirrored(drivenTx); drivenTx = rootTx * drivenTx; mirroredTransforms[i] = drivenTx; } } // Humanoid ミラーリング if (stream.isHumanStream) { AnimationHumanStream humanStream = stream.AsHuman(); if (mirror) { humanStream.MirrorPose(); } humanStream.SolveIK(); } // 追加トランスフォームのミラーリング適用 if (mirror) { for (int i = 0; i < mirroringTransforms.Length; i++) { if (!mirroringTransforms[i].source.IsValid(stream)) { continue; } if (!mirroringTransforms[i].driven.IsValid(stream)) { continue; } mirroringTransforms[i].driven.SetGlobalTR(stream, mirroredTransforms[i].position, mirroredTransforms[i].rotation, false); } } // 追加トランスフォームのミラーリング拘束 if (mirror) { for (int i = 0; i < mirroringConstrants.Length; i++) { if (!mirroringConstrants[i].source.IsValid(stream)) { continue; } if (!mirroringConstrants[i].driven.IsValid(stream)) { continue; } Vector3 position; Quaternion rotation; mirroringConstrants[i].source.GetGlobalTR(stream, out position, out rotation); mirroringConstrants[i].driven.SetGlobalTR(stream, position, rotation, false); } } }
public void ProcessAnimation(AnimationStream stream) { float weight; if (m_UseAnimatorProperty) { weight = m_AnimatorWeight.GetFloat(stream); weight += m_AnimatorWeightOffset.GetFloat(stream); weight = Mathf.Clamp01(weight); m_WeightHandle.SetFloat(stream, weight); } else { weight = m_WeightHandle.GetFloat(stream); } weight = 1f; Vector3 effectorPosition; Quaternion effectorRotation; if (m_UseStreamEffector) { effectorPosition = m_EffectorStreamHandle.GetPosition(stream); effectorRotation = m_EffectorStreamHandle.GetRotation(stream); } else { effectorPosition = m_EffectorSceneHandle.GetPosition(stream); effectorRotation = m_EffectorSceneHandle.GetRotation(stream); } effectorRotation *= Quaternion.Euler(m_TargetOffset); // Changes to the stream seems to kill the foot ik data in the stream. // TODO: (sunek) Get rid of this workaround once fixed in release if (stream.isHumanStream) { var humanStream = stream.AsHuman(); humanStream.SetGoalPosition(AvatarIKGoal.LeftFoot, humanStream.GetGoalPosition(AvatarIKGoal.LeftFoot)); humanStream.SetGoalRotation(AvatarIKGoal.LeftFoot, humanStream.GetGoalRotation(AvatarIKGoal.LeftFoot)); humanStream.SetGoalPosition(AvatarIKGoal.RightFoot, humanStream.GetGoalPosition(AvatarIKGoal.RightFoot)); humanStream.SetGoalRotation(AvatarIKGoal.RightFoot, humanStream.GetGoalRotation(AvatarIKGoal.RightFoot)); } if (m_IkType == IkType.Generic) { AnimJobUtilities.SolveTwoBoneIK(stream, m_StartHandle, m_MidHandle, m_EndHandle, effectorPosition, effectorRotation, weight, weight); } else if (m_IkType == IkType.Humanoid) { if (stream.isHumanStream) { var humanStream = stream.AsHuman(); humanStream.SetGoalPosition(m_HumanLimb, effectorPosition); humanStream.SetGoalRotation(m_HumanLimb, effectorRotation); humanStream.SetGoalWeightPosition(m_HumanLimb, weight); humanStream.SetGoalWeightRotation(m_HumanLimb, weight); humanStream.SolveIK(); } } }
private void SyncIKFromPose() { var selectedTransform = Selection.transforms; var stream = new AnimationStream(); if (m_Animator.OpenAnimationStream(ref stream)) { AnimationHumanStream humanStream = stream.AsHuman(); // don't sync if transform is currently selected if (!Array.Exists(selectedTransform, tr => tr == m_LeftFootEffector.transform)) { m_LeftFootEffector.transform.position = humanStream.GetGoalPositionFromPose(AvatarIKGoal.LeftFoot); m_LeftFootEffector.transform.rotation = humanStream.GetGoalRotationFromPose(AvatarIKGoal.LeftFoot); } if (!Array.Exists(selectedTransform, tr => tr == m_RightFootEffector.transform)) { m_RightFootEffector.transform.position = humanStream.GetGoalPositionFromPose(AvatarIKGoal.RightFoot); m_RightFootEffector.transform.rotation = humanStream.GetGoalRotationFromPose(AvatarIKGoal.RightFoot); } if (!Array.Exists(selectedTransform, tr => tr == m_LeftHandEffector.transform)) { m_LeftHandEffector.transform.position = humanStream.GetGoalPositionFromPose(AvatarIKGoal.LeftHand); m_LeftHandEffector.transform.rotation = humanStream.GetGoalRotationFromPose(AvatarIKGoal.LeftHand); } if (!Array.Exists(selectedTransform, tr => tr == m_RightHandEffector.transform)) { m_RightHandEffector.transform.position = humanStream.GetGoalPositionFromPose(AvatarIKGoal.RightHand); m_RightHandEffector.transform.rotation = humanStream.GetGoalRotationFromPose(AvatarIKGoal.RightHand); } if (!Array.Exists(selectedTransform, tr => tr == m_LeftKneeHintEffector.transform)) { m_LeftKneeHintEffector.transform.position = humanStream.GetHintPosition(AvatarIKHint.LeftKnee); } if (!Array.Exists(selectedTransform, tr => tr == m_RightKneeHintEffector.transform)) { m_RightKneeHintEffector.transform.position = humanStream.GetHintPosition(AvatarIKHint.RightKnee); } if (!Array.Exists(selectedTransform, tr => tr == m_LeftElbowHintEffector.transform)) { m_LeftElbowHintEffector.transform.position = humanStream.GetHintPosition(AvatarIKHint.LeftElbow); } if (!Array.Exists(selectedTransform, tr => tr == m_RightElbowHintEffector.transform)) { m_RightElbowHintEffector.transform.position = humanStream.GetHintPosition(AvatarIKHint.RightElbow); } if (!Array.Exists(selectedTransform, tr => tr == m_BodyRotationEffector.transform)) { m_BodyRotationEffector.transform.position = humanStream.bodyPosition; m_BodyRotationEffector.transform.rotation = humanStream.bodyRotation; } m_Animator.CloseAnimationStream(ref stream); } }
public void ProcessAnimation(AnimationStream stream) { if (math.abs(bankAmount) < 0.001f) { return; } var bankPosition = new Vector3( settings.position.x * bankAmount * 0.01f, settings.position.y * bankAmount * 0.01f, settings.position.z * bankAmount * 0.01f); var weightedBankRotation = Quaternion.Euler(new Vector3( settings.rotation.x * bankAmount * (1 - settings.spineMultiplier), settings.rotation.y * bankAmount * (1 - settings.spineMultiplier), settings.rotation.z * bankAmount) * (1 - settings.spineMultiplier)); var bankRotation = Quaternion.Euler(new Vector3( settings.rotation.x * bankAmount, settings.rotation.y * bankAmount, settings.rotation.z * bankAmount)); var footPosition = new Vector3( settings.position.x * bankAmount * 0.01f * settings.footMultiplier, settings.position.y * bankAmount * 0.01f * settings.footMultiplier, settings.position.z * bankAmount * 0.01f * settings.footMultiplier); //TODO: A multiplier here?? var footRotation = Quaternion.Euler(new Vector3( settings.rotation.x * bankAmount * settings.footMultiplier, settings.rotation.y * bankAmount * settings.footMultiplier, settings.rotation.z * bankAmount * settings.footMultiplier)); // Humanoid if (stream.isHumanStream) { var humanStream = stream.AsHuman(); var position = humanStream.bodyLocalPosition; humanStream.bodyLocalRotation = weightedBankRotation * humanStream.bodyLocalRotation; humanStream.bodyLocalPosition = bankRotation * position + bankPosition; var numHandles = m_HeadLeftRightMuscles.Length; var multiplier = bankAmount * 0.075f * settings.headMultiplier / numHandles; for (var i = 0; i < numHandles; i++) { var headLeftRight = humanStream.GetMuscle(m_HeadLeftRightMuscles[i]); humanStream.SetMuscle(m_HeadLeftRightMuscles[i], headLeftRight + multiplier); } numHandles = m_SpineLeftRightMuscles.Length; multiplier = bankAmount * 0.075f * settings.spineMultiplier / numHandles; for (var i = 0; i < numHandles; i++) { var spineLeftRight = humanStream.GetMuscle(m_SpineLeftRightMuscles[i]); humanStream.SetMuscle(m_SpineLeftRightMuscles[i], spineLeftRight + multiplier); } humanStream.SetGoalLocalPosition(AvatarIKGoal.LeftFoot, humanStream.GetGoalLocalPosition(AvatarIKGoal.LeftFoot) + footPosition); humanStream.SetGoalRotation(AvatarIKGoal.LeftFoot, humanStream.GetGoalRotation(AvatarIKGoal.LeftFoot) * footRotation); humanStream.SetGoalLocalPosition(AvatarIKGoal.RightFoot, humanStream.GetGoalLocalPosition(AvatarIKGoal.RightFoot) + footPosition); humanStream.SetGoalRotation(AvatarIKGoal.RightFoot, humanStream.GetGoalRotation(AvatarIKGoal.RightFoot) * footRotation); } // Generic else // TODO: Flesh this path out or consider loosing it { m_SkeletonHandle.SetLocalPosition(stream, m_SkeletonHandle.GetLocalPosition(stream) + bankPosition); m_SkeletonHandle.SetLocalRotation(stream, m_SkeletonHandle.GetLocalRotation(stream) * bankRotation); } }
public void ProcessAnimation(AnimationStream stream) { var invDeltaTime = 1.0f / stream.deltaTime; // Store from pose if (doTransition) { var row = 0; var prevIndexOffset = (m_CurrentBufferIndex + 1) % k_OutputBufferCount; for (var i = 0; i < m_Bones.Length; i++) { var lastOutputBufferIndex = row + m_CurrentBufferIndex; var prevOutputBufferIndex = row + prevIndexOffset; // Position { var currentPos = (float3)m_Bones[i].GetLocalPosition(stream); var vLast = m_OutputPositions[lastOutputBufferIndex]; var vPrev = m_OutputPositions[prevOutputBufferIndex]; var vDelta = vLast - currentPos; var delta = math.length(vDelta); var dir = math.normalize(vDelta); var vDeltaPrev = vPrev - currentPos; var deltaPrev = math.dot(vDeltaPrev, dir); var vel = (delta - deltaPrev) * invDeltaTime; // if(i == k_debugBone) // Debug.Log("start delta:" + delta + " vel:" + vel); // If delta is negative we invert properties to keep delta positive if (delta < 0) { delta = -delta; vel = -vel; dir = -dir; // // if(i == k_debugBone) // Debug.Log(" ... negated to delta:" + delta + " vel:" + vel); } // if(i == k_debugBone && vel > 0) // Debug.Log(" ... vel positive so its clamped to 0"); vel = vel < 0 ? vel : 0; m_PosDirArray[i] = dir; m_PosDeltaArray[i] = delta; m_PosDeltaVelArray[i] = vel; } // Rotation { var currentRot = m_Bones[i].GetLocalRotation(stream); var qLast = m_OutputRotations[lastOutputBufferIndex]; var qPrev = m_OutputRotations[prevOutputBufferIndex]; var qInvCurrentRot = math.inverse(currentRot); var qDeltaRot = math.mul(qLast, qInvCurrentRot); var qDeltaPrevRot = math.mul(qPrev, qInvCurrentRot); float3 rotAxis; float delta; GetAxisAngle(qDeltaRot, out rotAxis, out delta); // TODO (mogensh) use this !! var prevRot = TwistAroundAxis(qDeltaPrevRot, rotAxis); float3 rotAxis2; float rot2; GetAxisAngle(qDeltaPrevRot, out rotAxis2, out rot2); var vel = (delta - rot2) * invDeltaTime; m_RotAxisArray[i] = rotAxis; m_RotDeltaArray[i] = delta; m_RotDeltaVelArray[i] = vel; } // Scale m_StarteScaleArray[i] = m_OutputScales[lastOutputBufferIndex]; row += k_OutputBufferCount; } for (var i = 0; i < k_NumIkHandles; i++) { var index = i * k_OutputBufferCount + m_CurrentBufferIndex; m_FromIkPositions[i] = m_OutputIkPositions[index]; m_FromIkRotations[i] = m_OutputIkRotations[index]; } doTransition = false; } // Decrement delta transitionTimeRemaining -= stream.deltaTime; // TODO: Find meaningful way of decrementing this if (transitionTimeRemaining <= 0f) { inTransition = false; } if (inTransition) { var transitionTime = transitionDuration - transitionTimeRemaining; var factor = transitionTimeRemaining / transitionDuration; for (var i = 0; i < m_Bones.Length; i++) { // Position { var startDelta = m_PosDeltaArray[i]; var deltaVel = m_PosDeltaVelArray[i]; // var delta = startDelta + deltaVel * transitionTime; var delta = Approach(startDelta, deltaVel, transitionDuration, transitionTime); // if(i == k_debugBone) // Debug.Log("startDelta:" + startDelta + " deltaVel:" + deltaVel + " time:" + transitionTime + " delta:" + delta); var dir = m_PosDirArray[i]; var vDelta = delta * dir; var streamPos = (float3)m_Bones[i].GetLocalPosition(stream); var pos = streamPos + vDelta; m_Bones[i].SetLocalPosition(stream, pos); } // Rotation { var startDelta = m_RotDeltaArray[i]; var deltaVel = m_RotDeltaVelArray[i]; // var delta = startDelta + deltaVel * transitionTime; var delta = Approach(startDelta, deltaVel, transitionDuration, transitionTime); var rotAxis = m_RotAxisArray[i]; var qDeltaRot = quaternion.AxisAngle(rotAxis, delta); var qStreamRot = m_Bones[i].GetLocalRotation(stream); var qRot = qDeltaRot * qStreamRot; m_Bones[i].SetLocalRotation(stream, qStreamRot); } // Scale m_Bones[i].SetLocalScale(stream, m_Bones[i].GetLocalScale(stream)); } if (stream.isHumanStream) { var humanStream = stream.AsHuman(); for (var i = 0; i < k_NumIkHandles; i++) { var position = math.lerp(humanStream.GetGoalLocalPosition((AvatarIKGoal)i), m_FromIkPositions[i], factor); humanStream.SetGoalLocalPosition((AvatarIKGoal)i, position); var rotation = math.slerp(humanStream.GetGoalLocalRotation((AvatarIKGoal)i), m_FromIkRotations[i], factor); humanStream.SetGoalLocalRotation((AvatarIKGoal)i, rotation); } } } // Store output values StoreOutput(stream); }
public void ProcessAnimation(AnimationStream stream) { ikWeight = Mathf.Clamp01(ikWeight + (1 - settings.enterStateEaseIn)); if (stream.isHumanStream) { var human = stream.AsHuman(); var leftToePos = leftToe.GetPosition(stream); var rightToePos = rightToe.GetPosition(stream); var leftIkOffset = ikOffset.x * ikWeight; var rightIkOffset = ikOffset.y * ikWeight; leftToePos += new Vector3(0f, leftIkOffset, 0f); rightToePos += new Vector3(0f, rightIkOffset, 0f); var leftAnklePos = human.GetGoalPosition(AvatarIKGoal.LeftFoot); var rightAnklePos = human.GetGoalPosition(AvatarIKGoal.RightFoot); var leftAnkleRot = human.GetGoalRotation(AvatarIKGoal.LeftFoot); var rightAnkleRot = human.GetGoalRotation(AvatarIKGoal.RightFoot); var leftAnkleIkPos = new Vector3(leftAnklePos.x, leftAnklePos.y + leftIkOffset, leftAnklePos.z); var rightAnkleIkPos = new Vector3(rightAnklePos.x, rightAnklePos.y + rightIkOffset, rightAnklePos.z); var hipHeightOffset = (leftIkOffset + rightIkOffset) * 0.5f; var forwardBackBias = (leftIkOffset - rightIkOffset) * settings.weightShiftHorizontal; // TODO: (sunek) Rework weight shift to move towards actual lower foot? hipHeightOffset += Mathf.Abs(leftIkOffset - rightIkOffset) * settings.weightShiftVertical; var standAngle = Quaternion.AngleAxis(settings.weightShiftAngle, Vector3.up) * Vector3.forward; human.bodyLocalPosition += new Vector3(standAngle.x * forwardBackBias, hipHeightOffset, standAngle.z * forwardBackBias); // Figure out the normal rotation var leftNormalRot = Quaternion.LookRotation(Vector3.forward, normalLeftFoot); var rightNormalRot = Quaternion.LookRotation(Vector3.forward, normalRightFoot); // Clamp normal rotation var leftAngle = Quaternion.Angle(Quaternion.identity, leftNormalRot); var rightAngle = Quaternion.Angle(Quaternion.identity, rightNormalRot); if (leftAngle > settings.maxFootRotationOffset && settings.maxFootRotationOffset > 0f) { var fraction = settings.maxFootRotationOffset / leftAngle; leftNormalRot = Quaternion.Lerp(Quaternion.identity, leftNormalRot, fraction); } if (rightAngle > settings.maxFootRotationOffset && settings.maxFootRotationOffset > 0f) { var fraction = settings.maxFootRotationOffset / rightAngle; rightNormalRot = Quaternion.Lerp(Quaternion.identity, rightNormalRot, fraction); } // Apply rotation to ankle var leftToesMatrix = Matrix4x4.TRS(leftToePos, Quaternion.identity, Vector3.one); var rightToesMatrix = Matrix4x4.TRS(rightToePos, Quaternion.identity, Vector3.one); var leftToesNormalDeltaMatrix = Matrix4x4.TRS(leftToePos, leftNormalRot, Vector3.one) * leftToesMatrix.inverse; var rightToesNormalDeltaMatrix = Matrix4x4.TRS(rightToePos, rightNormalRot, Vector3.one) * rightToesMatrix.inverse; var leftAnkleMatrix = Matrix4x4.TRS(leftAnkleIkPos, leftAnkleRot, Vector3.one) * leftToesMatrix.inverse; var rightAnkleMatrix = Matrix4x4.TRS(rightAnkleIkPos, rightAnkleRot, Vector3.one) * rightToesMatrix.inverse; leftAnkleMatrix = leftToesNormalDeltaMatrix * leftAnkleMatrix * leftToesMatrix; rightAnkleMatrix = rightToesNormalDeltaMatrix * rightAnkleMatrix * rightToesMatrix; leftAnkleIkPos = leftAnkleMatrix.GetColumn(3); rightAnkleIkPos = rightAnkleMatrix.GetColumn(3); leftAnkleRot = Quaternion.Lerp(leftAnkleRot, leftAnkleMatrix.rotation, ikWeight); rightAnkleRot = Quaternion.Lerp(rightAnkleRot, rightAnkleMatrix.rotation, ikWeight); // Update ik position // TODO: (sunek) Consider combating leg overstretch var leftPosition = Vector3.Lerp(leftAnklePos, leftAnkleIkPos, ikWeight); var rightPosition = Vector3.Lerp(rightAnklePos, rightAnkleIkPos, ikWeight); human.SetGoalPosition(AvatarIKGoal.LeftFoot, leftPosition); human.SetGoalPosition(AvatarIKGoal.RightFoot, rightPosition); human.SetGoalRotation(AvatarIKGoal.LeftFoot, leftAnkleRot); human.SetGoalRotation(AvatarIKGoal.RightFoot, rightAnkleRot); human.SetGoalWeightPosition(AvatarIKGoal.LeftFoot, 1f); human.SetGoalWeightPosition(AvatarIKGoal.RightFoot, 1f); human.SetGoalWeightRotation(AvatarIKGoal.LeftFoot, 1f); human.SetGoalWeightRotation(AvatarIKGoal.RightFoot, 1f); } }