您最多选择25个主题
主题必须以中文或者字母或数字开头，可以包含连字符 (-)，并且长度不得超过35个字符
263 行
12 KiB
263 行
12 KiB
using System;
|
|
using UnityEngine;
|
|
|
|
namespace Unity.Animation
|
|
{
|
|
using Burst;
|
|
using Mathematics;
|
|
using DataFlowGraph;
|
|
using Profiling;
|
|
using Entities;
|
|
|
|
public class StandIkNode
|
|
: NodeDefinition<StandIkNode.Data, StandIkNode.SimPorts, StandIkNode.KernelData, StandIkNode.KernelDefs, StandIkNode.Kernel>
|
|
, IMsgHandler<BlobAssetReference<RigDefinition>>
|
|
, IMsgHandler<StandIkNode.StandIkData>
|
|
{
|
|
public struct SimPorts : ISimulationPortDefinition
|
|
{
|
|
public MessageInput<StandIkNode, BlobAssetReference<RigDefinition>> RigDefinition;
|
|
public MessageInput<StandIkNode, StandIkData> StandIkSetup;
|
|
}
|
|
|
|
const float k_Epsilon = 0.000001F;
|
|
const float k_Rad2Deg = 360 / (math.PI * 2);
|
|
static readonly ProfilerMarker k_ProfileMarker = new ProfilerMarker("Animation.StandIkNode");
|
|
|
|
public struct KernelDefs : IKernelPortDefinition
|
|
{
|
|
public DataInput<StandIkNode, Buffer<float>> Input;
|
|
public DataOutput<StandIkNode, Buffer<float>> Output;
|
|
public DataInput<StandIkNode, float> Weight;
|
|
}
|
|
|
|
public struct Data : INodeData
|
|
{
|
|
}
|
|
|
|
public struct StandIkData
|
|
{
|
|
public Settings Settings;
|
|
public int LeftToeIdx;
|
|
public int RightToeIdx;
|
|
public int LeftFootIkIdx;
|
|
public int RightFootIkIdx;
|
|
public int HipsIdx;
|
|
public float2 ikOffset;
|
|
public float3 normalLeftFoot;
|
|
public float3 normalRightFoot;
|
|
public float Weight;
|
|
}
|
|
|
|
[Serializable]
|
|
public struct Settings
|
|
{
|
|
[Tooltip("Place over toe bone when in it's stand idle pose. " +
|
|
"Note that the Foot IK should not be enabled while adjusting!")]
|
|
public float3 leftToeStandPos;
|
|
|
|
[Tooltip("Place over toe bone when in it's stand idle pose. " +
|
|
"Note that the Foot IK should not be enabled while adjusting!")]
|
|
public float3 rightToeStandPos;
|
|
[Range(0, 1)]
|
|
public int debugIdlePos;
|
|
|
|
[Space(10)]
|
|
[Range(0, 1)]
|
|
public int enabled;
|
|
|
|
[Space(10)]
|
|
[Range(0f, 1f)]
|
|
public float emitRayOffset;
|
|
[Range(0f, 20f)]
|
|
public float maxRayDistance;
|
|
[Range(0, 1)]
|
|
public int debugRayCast;
|
|
|
|
[Space(10)]
|
|
[Range(0f, 1f)]
|
|
public float maxStepSize;
|
|
[Range(-90f, 90f)]
|
|
public float weightShiftAngle;
|
|
[Range(-1f, 1f)]
|
|
public float weightShiftHorizontal;
|
|
[Range(-1f, 1f)]
|
|
public float weightShiftVertical;
|
|
[Range(5f, 50f)]
|
|
public float maxFootRotationOffset;
|
|
|
|
[Space(10)]
|
|
[Range(0f, 1f)]
|
|
public float enterStateEaseIn;
|
|
}
|
|
|
|
public struct KernelData : IKernelData
|
|
{
|
|
public BlobAssetReference<RigDefinition> RigDefinition;
|
|
public ProfilerMarker ProfilerMarker;
|
|
public StandIkData Data;
|
|
public float3 VectorForward;
|
|
public float3 VectorOne;
|
|
public quaternion OffsetRotation;
|
|
}
|
|
|
|
[BurstCompile]
|
|
public struct Kernel : IGraphKernel<KernelData, KernelDefs>
|
|
{
|
|
public void Execute(RenderContext ctx, KernelData data, ref KernelDefs ports)
|
|
{
|
|
data.ProfilerMarker.Begin();
|
|
var output = ctx.Resolve(ref ports.Output);
|
|
output.CopyFrom(ctx.Resolve(in ports.Input));
|
|
|
|
var weightValue = ctx.Resolve(ports.Weight);
|
|
if (weightValue > 0f)
|
|
{
|
|
var stream = AnimationStreamProvider.Create(data.RigDefinition, output);
|
|
if (stream.IsNull)
|
|
{
|
|
data.ProfilerMarker.End();
|
|
return;
|
|
}
|
|
|
|
// TODO: (sunek) Get a real number! Requires replicating and incrementing the IK weight in update
|
|
// ikWeight = Mathf.Clamp01(ikWeight + (1 - settings.enterStateEaseIn));
|
|
|
|
var ikWeight = data.Data.Weight;
|
|
var settings = data.Data.Settings;
|
|
|
|
var leftToePos = stream.GetLocalToRigTranslation(data.Data.LeftToeIdx);
|
|
var rightToePos = stream.GetLocalToRigTranslation(data.Data.RightToeIdx);
|
|
|
|
var leftIkOffset = data.Data.ikOffset.x * ikWeight;
|
|
var rightIkOffset = data.Data.ikOffset.y * ikWeight;
|
|
|
|
leftToePos += new float3(0f, leftIkOffset, 0f);
|
|
rightToePos += new float3(0f, rightIkOffset, 0f);
|
|
|
|
var leftAnklePos = stream.GetLocalToRigTranslation(data.Data.LeftFootIkIdx);
|
|
var rightAnklePos = stream.GetLocalToRigTranslation(data.Data.RightFootIkIdx);
|
|
var leftAnkleRot = stream.GetLocalToRigRotation(data.Data.LeftFootIkIdx);
|
|
var rightAnkleRot = stream.GetLocalToRigRotation(data.Data.RightFootIkIdx);
|
|
|
|
var leftAnkleIkPos = new float3(leftAnklePos.x, leftAnklePos.y + leftIkOffset, leftAnklePos.z);
|
|
var rightAnkleIkPos = new float3(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 = math.mul(quaternion.AxisAngle(Vector3.up, math.radians(settings.weightShiftAngle)), data.VectorForward);
|
|
|
|
var hipsPosition = stream.GetLocalToRigTranslation(data.Data.HipsIdx);
|
|
hipsPosition += new float3(standAngle.x * forwardBackBias, hipHeightOffset, standAngle.z * forwardBackBias);
|
|
stream.SetLocalToRigTranslation(data.Data.HipsIdx, hipsPosition);
|
|
|
|
// Figure out the normal rotation
|
|
var leftNormalRot = quaternion.identity;
|
|
var rightNormalRot = quaternion.identity;
|
|
|
|
if (!data.Data.normalLeftFoot.Equals(float3.zero))
|
|
{
|
|
leftNormalRot = quaternion.LookRotationSafe(data.Data.normalLeftFoot, data.VectorForward);
|
|
rightNormalRot = quaternion.LookRotationSafe(data.Data.normalRightFoot, data.VectorForward);
|
|
leftNormalRot = math.mul(leftNormalRot, data.OffsetRotation);
|
|
rightNormalRot = math.mul(rightNormalRot, data.OffsetRotation);
|
|
}
|
|
|
|
// Clamp normal rotation
|
|
var leftAngle = Angle(quaternion.identity, leftNormalRot);
|
|
var rightAngle = Angle(quaternion.identity, rightNormalRot);
|
|
|
|
if (leftAngle > settings.maxFootRotationOffset && settings.maxFootRotationOffset > 0f)
|
|
{
|
|
var fraction = settings.maxFootRotationOffset / leftAngle;
|
|
leftNormalRot = math.nlerp(Quaternion.identity, leftNormalRot, fraction);
|
|
}
|
|
|
|
if (rightAngle > settings.maxFootRotationOffset && settings.maxFootRotationOffset > 0f)
|
|
{
|
|
var fraction = settings.maxFootRotationOffset / rightAngle;
|
|
rightNormalRot = math.nlerp(Quaternion.identity, rightNormalRot, fraction);
|
|
}
|
|
|
|
// Apply rotation to ankle
|
|
var leftToesMatrix = Matrix4x4.TRS(leftToePos, quaternion.identity, data.VectorOne);
|
|
var rightToesMatrix = Matrix4x4.TRS(rightToePos, quaternion.identity, data.VectorOne);
|
|
|
|
leftNormalRot = math.normalize(leftNormalRot);
|
|
rightNormalRot = math.normalize(rightNormalRot);
|
|
|
|
leftAnkleRot = math.normalize(leftAnkleRot);
|
|
rightAnkleRot = math.normalize(rightAnkleRot);
|
|
|
|
var leftToesNormalDeltaMatrix = Matrix4x4.TRS(leftToePos, leftNormalRot, data.VectorOne) * leftToesMatrix.inverse;
|
|
var rightToesNormalDeltaMatrix = Matrix4x4.TRS(rightToePos, rightNormalRot, data.VectorOne) * rightToesMatrix.inverse;
|
|
|
|
var leftAnkleMatrix = Matrix4x4.TRS(leftAnkleIkPos, leftAnkleRot, data.VectorOne) * leftToesMatrix.inverse;
|
|
var rightAnkleMatrix = Matrix4x4.TRS(rightAnkleIkPos, rightAnkleRot, data.VectorOne) * rightToesMatrix.inverse;
|
|
|
|
leftAnkleMatrix = leftToesNormalDeltaMatrix * leftAnkleMatrix * leftToesMatrix;
|
|
rightAnkleMatrix = rightToesNormalDeltaMatrix * rightAnkleMatrix * rightToesMatrix;
|
|
|
|
// Todo:Find a better way to do this?
|
|
var leftColumn = leftAnkleMatrix.GetColumn(3);
|
|
leftAnkleIkPos = new float3(leftColumn[0], leftColumn[1], leftColumn[2]); // TODO: (sunek) Is there a slicing syntax instead?
|
|
var rightColumn = rightAnkleMatrix.GetColumn(3);
|
|
rightAnkleIkPos = new float3(rightColumn[0], rightColumn[1], rightColumn[2]);
|
|
|
|
leftAnkleRot = math.nlerp(leftAnkleRot, leftAnkleMatrix.rotation, ikWeight);
|
|
rightAnkleRot = math.nlerp(rightAnkleRot, rightAnkleMatrix.rotation, ikWeight);
|
|
|
|
// Update ik position
|
|
// TODO: (sunek) Consider combating leg overstretch
|
|
var leftPosition = math.lerp(leftAnklePos, leftAnkleIkPos, ikWeight);
|
|
var rightPosition = math.lerp(rightAnklePos, rightAnkleIkPos, ikWeight);
|
|
|
|
stream.SetLocalToRigTranslation(data.Data.LeftFootIkIdx, leftPosition);
|
|
stream.SetLocalToRigTranslation(data.Data.RightFootIkIdx, rightPosition);
|
|
|
|
stream.SetLocalToRigRotation(data.Data.LeftFootIkIdx, leftAnkleRot);
|
|
stream.SetLocalToRigRotation(data.Data.RightFootIkIdx, rightAnkleRot);
|
|
}
|
|
|
|
data.ProfilerMarker.End();
|
|
}
|
|
|
|
private static float Angle(quaternion a, quaternion b)
|
|
{
|
|
// TODO: Return in radians (and convert the user param from deg to rad)?
|
|
float dot = math.dot(a, b);
|
|
return IsEqualUsingDot(dot) ? 0.0f : math.acos(math.min(math.abs(dot), 1.0F)) * 2.0F * k_Rad2Deg;
|
|
}
|
|
|
|
private static bool IsEqualUsingDot(float dot)
|
|
{
|
|
// Returns false in the presence of NaN values.
|
|
return dot > 1.0f - k_Epsilon;
|
|
}
|
|
}
|
|
|
|
public override void Init(InitContext ctx)
|
|
{
|
|
ref var kData = ref GetKernelData(ctx.Handle);
|
|
kData.ProfilerMarker = k_ProfileMarker;
|
|
kData.VectorForward = new float3(0f, 0f, 1f);
|
|
kData.VectorOne = new float3(1f, 1f, 1f);
|
|
// kData.OffsetRotation = quaternion.Euler(math.radians(-90f), 0f, math.radians(180f));
|
|
kData.OffsetRotation = new quaternion(0f, 0.7f, 0.7f, 0f);
|
|
}
|
|
|
|
public void HandleMessage(in MessageContext ctx, in BlobAssetReference<RigDefinition> rigBindings)
|
|
{
|
|
GetKernelData(ctx.Handle).RigDefinition = rigBindings;
|
|
Set.SetBufferSize(ctx.Handle, (OutputPortID)KernelPorts.Output, Buffer<float>.SizeRequest(rigBindings.Value.Bindings.CurveCount));
|
|
}
|
|
|
|
public void HandleMessage(in MessageContext ctx, in StandIkData data)
|
|
{
|
|
GetKernelData(ctx.Handle).Data = data;
|
|
}
|
|
}
|
|
}
|