public void DoIteration(SkeletonInputs inputs, Bone sourceBone, Vector2 unposedSource, Vector2 target)
{
var chain = FabrIkChain.Make(inputs, sourceBone, unposedSource, target);
chain.DoForwardPass();
chain.DoBackwardPass();
chain.ApplyToInputs(inputs);
}
/**
* Given a point that has already had bone total-transform applied to it, retransform it as the rotation of this bone was adjusted by a delta.
*/
public Vector2 RetransformPoint(SkeletonInputs inputs, float rotationDelta, Vector2 point)
{
Matrix3x2 parentTransform = GetChainedTransform(Parent, inputs);
Vector2 transformedCenter = Matrix3x2.TransformPoint(parentTransform, Center);
var retransform = Matrix3x2.Rotation(rotationDelta, transformedCenter);
return(Matrix3x2.TransformPoint(retransform, point));
}
/**
* Returns the gradient of a transformed point with respect to the rotation parameter.
*/
public Vector2 GetGradientOfTransformedPointWithRespectToRotation(SkeletonInputs inputs, Vector2 point)
{
Matrix3x2 parentTransform = GetChainedTransform(Parent, inputs);
Vector2 transformedCenter = Matrix3x2.TransformPoint(parentTransform, Center);
Vector2 centeredPoint = point - transformedCenter;
return(new Vector2(-centeredPoint.Y, centeredPoint.X));
}
private void AdjustBone(SkeletonInputs inputs, Bone bone, Vector2 source, Vector2 target, float weight)
{
var transform = bone.GetChainedTransform(inputs);
var center = Matrix3x2.TransformPoint(transform, bone.Center);
float rotationDelta = Vector2Utils.AngleBetween(source - center, target - center);
bone.IncrementRotation(inputs, rotationDelta * weight);
}
public void DoIteration(SkeletonInputs inputs, Bone sourceBone, Vector2 unposedSource, Vector2 target)
{
Vector2 posedSource = Matrix3x2.TransformPoint(sourceBone.GetChainedTransform(inputs), unposedSource);
List <Vector2> posedCenters = bones
.Select(bone => Matrix3x2.TransformPoint(bone.GetChainedTransform(inputs), bone.Center))
.ToList();
List <Vector2> boneVectors = posedCenters
.Zip(posedCenters.Skip(1).Concat(new [] { posedSource }), (center, end) => end - center)
.ToList();
List <float> weights = Enumerable.Range(0, bones.Count).Select(i => 1f / (bones.Count - i)).ToList();
Vector <float> gradient = Vector <float> .Build.Dense(bones.Count);
Matrix <float> hessian = Matrix <float> .Build.Dense(bones.Count, bones.Count);
for (int i = 0; i < bones.Count; ++i)
{
Vector2 bi = boneVectors[i];
Vector2 temp = target - posedSource + bi;
gradient[i] = weights[i] * (bi.Y * temp.X - bi.X * temp.Y);
hessian[i, i] = weights[i] * weights[i] * Vector2.Dot(bi, temp);
for (int j = 0; j < bones.Count; ++j)
{
Vector2 bj = boneVectors[j];
if (i != j)
{
hessian[i, j] = weights[i] * weights[j] * Vector2.Dot(bi, bj);
}
}
}
//var step = -hessian.Inverse().Multiply(gradient);
var step = -hessian.PseudoInverse().Multiply(gradient);
//var step = -0.5f * gradient;
/*
* // Ensure step is approaching a minimum and not a maximum
* for (int i = 0; i < bones.Count; ++i) {
* step[i] = -Math.Abs(step[i]) * Math.Sign(gradient[i]);
* }
*/
for (int i = 0; i < bones.Count; ++i)
{
float localRotationDelta = step[i] - ((i > 0) ? step[i - 1] : 0);
inputs.IncrementRotation(i, weights[i] * localRotationDelta);
}
}
public BoneTest()
{
bone0 = Bone.MakeWithOffset(0, null, Vector2.UnitX, MathUtil.Pi);
bone1 = Bone.MakeWithOffset(1, bone0, Vector2.UnitX, MathUtil.Pi);
bone2 = Bone.MakeWithOffset(2, bone1, Vector2.UnitX, MathUtil.Pi);
inputs = new SkeletonInputs(3);
bone0.SetRotation(inputs, +0.1f);
bone1.SetRotation(inputs, -0.2f);
bone2.SetRotation(inputs, +0.4f);
}
private static Matrix3x2 GetChainedTransform(Bone bone, SkeletonInputs inputs)
{
if (bone == null)
{
return(Matrix3x2.Translation(inputs.Translation));
}
else
{
return(bone.GetChainedTransform(inputs));
}
}
public void ApplyToInputs(SkeletonInputs inputs)
{
inputs.Translation = positions[bones.Count] - bones[bones.Count - 1].Center;
for (int i = 0; i < bones.Count; ++i)
{
float parentRotation = (i + 1 < bones.Count) ? rotations[i + 1] : 0;
float localRotation = rotations[i] - parentRotation;
bones[i].SetRotation(inputs, localRotation);
}
}
public FlatIkApp()
{
renderEnvironment = new WindowedDirect2dRenderEnvironment("FlatIkApp", false);
context = renderEnvironment.D2dContext;
whiteBrush = new SolidColorBrush(context, Color.White);
redBrush = new SolidColorBrush(context, Color.Red);
bones = MakeStandardBones();
inputs = new SkeletonInputs(bones.Count);
solver = new FabrIkSolver();
}
public void DoIteration(SkeletonInputs inputs, Bone sourceBone, Vector2 unposedSource, Vector2 target)
{
Vector2 source = Matrix3x2.TransformPoint(sourceBone.GetChainedTransform(inputs), sourceBone.End);
float decay = 0.9f;
float weight = 1 - decay;
for (var bone = sourceBone; bone != null; bone = bone.Parent)
{
AdjustBone(inputs, bone, source, target, weight);
weight *= decay;
}
}
public static FabrIkChain Make(SkeletonInputs inputs, Bone sourceBone, Vector2 unposedSource, Vector2 target)
{
Vector2 posedSource = Matrix3x2.TransformPoint(sourceBone.GetChainedTransform(inputs), sourceBone.End);
List <Bone> bones = new List <Bone> {
};
List <Vector2> unposedBoneVectors = new List <Vector2> {
};
List <float> rotations = new List <float>();
List <Vector2> positions = new List <Vector2> {
};
positions.Add(posedSource);
Vector2 previousUnposedPosition = unposedSource;
Vector2 previousPosedPosition = posedSource;
for (var bone = sourceBone; bone != null; bone = bone.Parent)
{
var unposedCenter = bone.Center;
var posedCenter = Matrix3x2.TransformPoint(bone.GetChainedTransform(inputs), unposedCenter);
Vector2 unposedBoneVector = previousUnposedPosition - unposedCenter;
Vector2 posedBoneVector = previousPosedPosition - posedCenter;
float rotation = Vector2Utils.AngleBetween(
unposedBoneVector,
posedBoneVector);
bones.Add(bone);
unposedBoneVectors.Add(unposedBoneVector);
rotations.Add(rotation);
positions.Add(posedCenter);
previousUnposedPosition = unposedCenter;
previousPosedPosition = posedCenter;
}
var startTarget = target;
var endTarget = positions[bones.Count];
return(new FabrIkChain(bones, unposedBoneVectors, rotations, positions, target, endTarget));
}
public void DoIteration(SkeletonInputs inputs, Bone sourceBone, Vector2 unposedSource, Vector2 target)
{
Vector2 source = Matrix3x2.TransformPoint(sourceBone.GetChainedTransform(inputs), sourceBone.End);
Vector <float> residuals = Vector <float> .Build.Dense(2);
residuals[0] = target.X - source.X;
residuals[1] = target.Y - source.Y;
List <Bone> bones = GetBoneChain(sourceBone).ToList();
int boneCount = bones.Count;
Matrix <float> jacobian = Matrix <float> .Build.Dense(2, boneCount + 2);
for (int boneIdx = 0; boneIdx < boneCount; ++boneIdx)
{
Vector2 boneGradient = bones[boneIdx].GetGradientOfTransformedPointWithRespectToRotation(inputs, source);
jacobian[0, boneIdx] = boneGradient.X;
jacobian[1, boneIdx] = boneGradient.Y;
}
jacobian[0, boneCount + 0] = RootTranslationWeight;
jacobian[1, boneCount + 1] = RootTranslationWeight;
Vector <float> step = jacobian.PseudoInverse().Multiply(residuals);
for (int boneIdx = 0; boneIdx < boneCount; ++boneIdx)
{
var bone = bones[boneIdx];
bone.IncrementRotation(inputs, step[boneIdx]);
}
Vector2 rootTranslationStep = new Vector2(
step[boneCount + 0],
step[boneCount + 1]);
inputs.Translation += rootTranslationStep * RootTranslationWeight;
}
public void SetRotation(SkeletonInputs inputs, float rotation)
{
inputs.SetRotation(Index, rotation);
}
public void DoIteration(SkeletonInputs inputs, Bone sourceBone, Vector2 unposedSource, Vector2 target)
{
Vector2 source = Matrix3x2.TransformPoint(sourceBone.GetChainedTransform(inputs), sourceBone.End);
List <Bone> bones = GetBoneChain(sourceBone).ToList();
int boneCount = bones.Count;
Vector <float> residuals = Vector <float> .Build.Dense(boneCount * 2 + 2);
for (int boneIdx = 0; boneIdx < boneCount; ++boneIdx)
{
var bone = bones[boneIdx];
var posedCenter = Matrix3x2.TransformPoint(bone.GetChainedTransform(inputs), bone.Center);
var residual = bone.Center - posedCenter;
//var residual = Vector2.Zero;
residuals[boneIdx * 2 + 0] = BoneCenterWeight * residual.X;
residuals[boneIdx * 2 + 1] = BoneCenterWeight * residual.Y;
}
residuals[boneCount * 2 + 0] = IkTargetWeight * (target.X - source.X);
residuals[boneCount * 2 + 1] = IkTargetWeight * (target.Y - source.Y);
Matrix <float> jacobian = Matrix <float> .Build.Dense(2 *boneCount + 2, boneCount);
for (int boneIdx = 0; boneIdx < boneCount; ++boneIdx)
{
for (int targetIdx = 0; targetIdx < boneCount + 1; ++targetIdx)
{
Vector2 boneGradient;
if (targetIdx > boneIdx && targetIdx != boneCount)
{
//target is unaffected by this bone
boneGradient = Vector2.Zero;
}
else
{
Vector2 targetSource;
float weight;
if (targetIdx < boneCount)
{
//target is a bone center
var targetBone = bones[targetIdx];
targetSource = Matrix3x2.TransformPoint(targetBone.GetChainedTransform(inputs), targetBone.Center);
weight = BoneCenterWeight;
}
else
{
//target is IK target
targetSource = source;
weight = IkTargetWeight;
}
boneGradient = weight * bones[boneIdx].GetGradientOfTransformedPointWithRespectToRotation(inputs, targetSource);
}
jacobian[targetIdx * 2 + 0, boneIdx] = boneGradient.X;
jacobian[targetIdx * 2 + 1, boneIdx] = boneGradient.Y;
}
}
Vector <float> step = jacobian.PseudoInverse().Multiply(residuals);
for (int boneIdx = 0; boneIdx < boneCount; ++boneIdx)
{
var bone = bones[boneIdx];
bone.IncrementRotation(inputs, step[boneIdx]);
}
}
public float GetRotation(SkeletonInputs inputs)
{
return(inputs.GetRotation(Index));
}
public void IncrementRotation(SkeletonInputs inputs, float rotationDelta)
{
inputs.IncrementRotation(Index, rotationDelta);
}
public Matrix3x2 GetChainedTransform(SkeletonInputs inputs)
{
Matrix3x2 parentTransform = GetChainedTransform(Parent, inputs);
return(GetLocalTransform(inputs) * parentTransform);
}
public Matrix3x2 GetLocalTransform(SkeletonInputs inputs)
{
float rotation = GetRotation(inputs);
return(Matrix3x2.Rotation(rotation, Center));
}
