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();
}
public static void SwapMuscles(this AnimationHumanStream stream, MuscleHandle a, MuscleHandle b)
{
var t = stream.GetMuscle(a);
stream.SetMuscle(a, stream.GetMuscle(b));
stream.SetMuscle(b, t);
}
private void ApplyLookData(AnimationHumanStream stream, IKLookData.JobData data)
{
stream.SetLookAtPosition(data.position);
stream.SetLookAtBodyWeight(data.bodyWeight);
stream.SetLookAtHeadWeight(data.headWeight);
stream.SetLookAtEyesWeight(data.eyesWeight);
stream.SetLookAtClampWeight(data.weightClamp);
}
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));
}
}
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);
}
}
}
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);
}
}
private void ApplyLimbData(AnimationHumanStream stream, IKLimbData.JobData data, AvatarIKGoal goal, AvatarIKHint hint)
{
if (data.positionWeight > 0)
{
if (data.positionWeight >= 1)
{
stream.SetGoalPosition(goal, data.position);
stream.SetGoalWeightPosition(goal, 1f);
}
else
{
var position = Vector3.Lerp(stream.GetGoalPosition(goal), data.position, data.positionWeight);
var positionWeight = Mathf.Lerp(stream.GetGoalWeightPosition(goal), 1f, data.positionWeight);
stream.SetGoalPosition(goal, position);
stream.SetGoalWeightPosition(goal, positionWeight);
}
}
else
{
// For some reason, IK will be broken if same values are not reapplied before SolveIK is called
stream.SetGoalPosition(goal, stream.GetGoalPosition(goal));
stream.SetGoalWeightPosition(goal, stream.GetGoalWeightPosition(goal));
}
if (data.rotationWeight > 0)
{
if (data.rotationWeight >= 1)
{
stream.SetGoalRotation(goal, data.rotation);
stream.SetGoalWeightRotation(goal, 1f);
}
else
{
var rotation = Quaternion.Slerp(stream.GetGoalRotation(goal), data.rotation, data.rotationWeight);
var rotationWeight = Mathf.Lerp(stream.GetGoalWeightRotation(goal), 1f, data.rotationWeight);
stream.SetGoalRotation(goal, rotation);
stream.SetGoalWeightRotation(goal, rotationWeight);
}
}
else
{
// Same as above
stream.SetGoalRotation(goal, stream.GetGoalRotation(goal));
stream.SetGoalWeightRotation(goal, stream.GetGoalWeightRotation(goal));
}
if (data.hintWeight > 0)
{
if (data.hintWeight >= 1)
{
stream.SetHintPosition(hint, data.hintPosition);
stream.SetHintWeightPosition(hint, 1f);
}
else
{
var hintPosition = Vector3.Lerp(stream.GetHintPosition(hint), data.hintPosition, data.hintWeight);
var hintWeight = Mathf.Lerp(stream.GetHintWeightPosition(hint), 1f, data.hintWeight);
stream.SetHintPosition(hint, hintPosition);
stream.SetHintWeightPosition(hint, hintWeight);
}
}
else
{
// Same as above
stream.SetHintPosition(hint, stream.GetHintPosition(hint));
stream.SetHintWeightPosition(hint, stream.GetHintWeightPosition(hint));
}
}
public static void MirrorPose(this AnimationHumanStream humanStream)
{
// mirror body
for (int i = 0; i < (int)BodyDof.LastBodyDof; i++)
{
humanStream.MultMuscle(new MuscleHandle((BodyDof)i), BodyDoFMirror[i]);
}
// mirror head
for (int i = 0; i < (int)HeadDof.LastHeadDof; i++)
{
humanStream.MultMuscle(new MuscleHandle((HeadDof)i), HeadDoFMirror[i]);
}
// swap arms
for (int i = 0; i < (int)ArmDof.LastArmDof; i++)
{
humanStream.SwapMuscles(
new MuscleHandle(HumanPartDof.LeftArm, (ArmDof)i),
new MuscleHandle(HumanPartDof.RightArm, (ArmDof)i));
}
// swap legs
for (int i = 0; i < (int)LegDof.LastLegDof; i++)
{
humanStream.SwapMuscles(
new MuscleHandle(HumanPartDof.LeftLeg, (LegDof)i),
new MuscleHandle(HumanPartDof.RightLeg, (LegDof)i));
}
// swap fingers
for (int i = 0; i < (int)FingerDof.LastFingerDof; i++)
{
humanStream.SwapMuscles(
new MuscleHandle(HumanPartDof.LeftThumb, (FingerDof)i),
new MuscleHandle(HumanPartDof.RightThumb, (FingerDof)i));
humanStream.SwapMuscles(
new MuscleHandle(HumanPartDof.LeftIndex, (FingerDof)i),
new MuscleHandle(HumanPartDof.RightIndex, (FingerDof)i));
humanStream.SwapMuscles(
new MuscleHandle(HumanPartDof.LeftMiddle, (FingerDof)i),
new MuscleHandle(HumanPartDof.RightMiddle, (FingerDof)i));
humanStream.SwapMuscles(
new MuscleHandle(HumanPartDof.LeftRing, (FingerDof)i),
new MuscleHandle(HumanPartDof.RightRing, (FingerDof)i));
humanStream.SwapMuscles(
new MuscleHandle(HumanPartDof.LeftLittle, (FingerDof)i),
new MuscleHandle(HumanPartDof.RightLittle, (FingerDof)i));
}
humanStream.bodyLocalPosition = Mirrored(humanStream.bodyLocalPosition);
// mirror rotation (invert Y axis angle)
humanStream.bodyLocalRotation = Mirrored(humanStream.bodyLocalRotation);
// swap ik
Vector3[] goalPositions = new Vector3[4];
Quaternion[] goalRotations = new Quaternion[4];
float[] goalWeightPositons = new float[4];
float[] goalWeightRotations = new float[4];
Vector3[] hintPositions = new Vector3[4];
float[] hintWeightPositions = new float[4];
for (int i = 0; i < 4; i++)
{
goalPositions[i] = humanStream.GetGoalLocalPosition(AvatarIKGoal.LeftFoot + i);
goalRotations[i] = humanStream.GetGoalLocalRotation(AvatarIKGoal.LeftFoot + i);
goalWeightPositons[i] = humanStream.GetGoalWeightPosition(AvatarIKGoal.LeftFoot + i);
goalWeightRotations[i] = humanStream.GetGoalWeightRotation(AvatarIKGoal.LeftFoot + i);
hintPositions[i] = humanStream.GetHintPosition(AvatarIKHint.LeftKnee + i);
hintWeightPositions[i] = humanStream.GetHintWeightPosition(AvatarIKHint.LeftKnee + i);
}
for (int i = 0; i < 4; i++)
{
int j = (i + 1) % 2 + (i / 2) * 2; // make [1, 0, 3, 2]
humanStream.SetGoalLocalPosition(AvatarIKGoal.LeftFoot + i, Mirrored(goalPositions[j]));
humanStream.SetGoalLocalRotation(AvatarIKGoal.LeftFoot + i, Mirrored(goalRotations[j]));
humanStream.SetGoalWeightPosition(AvatarIKGoal.LeftFoot + i, goalWeightPositons[j]);
humanStream.SetGoalWeightRotation(AvatarIKGoal.LeftFoot + i, goalWeightRotations[j]);
humanStream.SetHintPosition(AvatarIKHint.LeftKnee + i, hintPositions[j]);
humanStream.SetHintWeightPosition(AvatarIKHint.LeftKnee + i, hintWeightPositions[j]);
}
}
public static void MultMuscle(this AnimationHumanStream stream, MuscleHandle h, float f)
{
stream.SetMuscle(h, f * stream.GetMuscle(h));
}
