void Solve1(BoneChain fk1, Vec2D worldTargetTranslation)
{
Mat2D iworld = fk1.m_ParentWorldInverse;
var pA = new Vec2D();
fk1.m_Bone.GetWorldTranslation(pA);
var pBT = new Vec2D(worldTargetTranslation);
// To target in worldspace
Vec2D toTarget = Vec2D.Subtract(new Vec2D(), pBT, pA);
// Note this is directional, hence not transformMat2d
Vec2D toTargetLocal = Vec2D.TransformMat2(new Vec2D(), toTarget, iworld);
float r = (float)Math.Atan2(toTargetLocal[1], toTargetLocal[0]);
ConstrainRotation(fk1, r);
fk1.m_Angle = r;
}
void ConstrainRotation(BoneChain fk, float rotation)
{
ActorBone bone = fk.m_Bone;
Mat2D parentWorld = bone.Parent.WorldTransform;
Mat2D transform = bone.Transform;
TransformComponents c = fk.m_TransformComponents;
if (rotation == 0.0f)
{
Mat2D.Identity(transform);
}
else
{
Mat2D.FromRotation(transform, rotation);
}
// Translate
transform[4] = c.X;
transform[5] = c.Y;
// Scale
float scaleX = c.ScaleX;
float scaleY = c.ScaleY;
transform[0] *= scaleX;
transform[1] *= scaleX;
transform[2] *= scaleY;
transform[3] *= scaleY;
// Skew
float skew = c.Skew;
if (skew != 0.0)
{
transform[2] = transform[0] * skew + transform[2];
transform[3] = transform[1] * skew + transform[3];
}
Mat2D.Multiply(bone.WorldTransform, parentWorld, transform);
}
public override void CompleteResolve()
{
base.CompleteResolve();
if (m_InfluencedBones == null || m_InfluencedBones.Length == 0)
{
return;
}
// Initialize solver.
ActorBone start = m_InfluencedBones[0].m_Bone;
ActorBone end = m_InfluencedBones[m_InfluencedBones.Length - 1].m_Bone;
int count = 0;
while (end != null && end != start.Parent)
{
count++;
end = end.Parent as ActorBone;
}
bool allIn = count < 3;
end = m_InfluencedBones[m_InfluencedBones.Length - 1].m_Bone;
m_FKChain = new BoneChain[count];
int idx = count - 1;
while (end != null && end != start.Parent)
{
BoneChain bc = new BoneChain();
bc.m_Bone = end;
bc.m_Angle = 0.0f;
bc.m_Included = allIn;
bc.m_TransformComponents = new TransformComponents();
bc.m_ParentWorldInverse = new Mat2D();
bc.m_Index = idx;
m_FKChain[idx--] = bc;
end = end.Parent as ActorBone;
}
// Make sure bones are good.
m_BoneData = new List <BoneChain>();
foreach (InfluencedBone bone in m_InfluencedBones)
{
BoneChain item = Array.Find(m_FKChain, chainItem => chainItem.m_Bone == bone.m_Bone);
if (item == null)
{
Console.WriteLine("Bone not in chain: " + bone.m_Bone.Name);
continue;
}
m_BoneData.Add(item);
}
if (!allIn)
{
// Influenced bones are in the IK chain.
for (int i = 0; i < m_BoneData.Count - 1; i++)
{
BoneChain item = m_BoneData[i];
item.m_Included = true;
m_FKChain[item.m_Index + 1].m_Included = true;
}
}
// Finally mark dependencies.
foreach (InfluencedBone bone in m_InfluencedBones)
{
// Don't mark dependency on parent as ActorComponent already does this.
if (bone.m_Bone == m_Parent)
{
continue;
}
m_Actor.AddDependency(this, bone.m_Bone);
}
if (m_Target != null)
{
m_Actor.AddDependency(this, m_Target);
}
// All the first level children of the influenced bones should depend on the final bone.
BoneChain tip = m_FKChain[m_FKChain.Length - 1];
foreach (BoneChain fk in m_FKChain)
{
if (fk == tip)
{
continue;
}
ActorBone bone = fk.m_Bone;
foreach (ActorNode node in bone.Children)
{
BoneChain item = Array.Find(m_FKChain, chainItem => chainItem.m_Bone == node);
if (item != null)
{
// node is in the FK chain.
continue;
}
m_Actor.AddDependency(node, tip.m_Bone);
}
}
}
void Solve2(BoneChain fk1, BoneChain fk2, Vec2D worldTargetTranslation)
{
ActorBone b1 = fk1.m_Bone;
ActorBone b2 = fk2.m_Bone;
BoneChain firstChild = m_FKChain[fk1.m_Index + 1];
Mat2D iworld = fk1.m_ParentWorldInverse;
Vec2D pA = b1.GetWorldTranslation(new Vec2D());
Vec2D pC = firstChild.m_Bone.GetWorldTranslation(new Vec2D());
Vec2D pB = b2.GetTipWorldTranslation(new Vec2D());;
Vec2D pBT = new Vec2D(worldTargetTranslation);
pA = Vec2D.TransformMat2D(pA, pA, iworld);
pC = Vec2D.TransformMat2D(pC, pC, iworld);
pB = Vec2D.TransformMat2D(pB, pB, iworld);
pBT = Vec2D.TransformMat2D(pBT, pBT, iworld);
// http://mathworld.wolfram.com/LawofCosines.html
Vec2D av = Vec2D.Subtract(new Vec2D(), pB, pC);
float a = Vec2D.Length(av);
Vec2D bv = Vec2D.Subtract(new Vec2D(), pC, pA);
float b = Vec2D.Length(bv);
Vec2D cv = Vec2D.Subtract(new Vec2D(), pBT, pA);
float c = Vec2D.Length(cv);
float A = (float)Math.Acos(Math.Max(-1, Math.Min(1, (-a * a + b * b + c * c) / (2 * b * c))));
float C = (float)Math.Acos(Math.Max(-1, Math.Min(1, (a * a + b * b - c * c) / (2 * a * b))));
float r1, r2;
if (b2.Parent != b1)
{
BoneChain secondChild = m_FKChain[fk1.m_Index + 2];
Mat2D secondChildWorldInverse = secondChild.m_ParentWorldInverse;
pC = firstChild.m_Bone.GetWorldTranslation(new Vec2D());
pB = b2.GetTipWorldTranslation(new Vec2D());
Vec2D avec = Vec2D.Subtract(new Vec2D(), pB, pC);
Vec2D avLocal = Vec2D.TransformMat2(new Vec2D(), avec, secondChildWorldInverse);
float angleCorrection = (float)-Math.Atan2(avLocal[1], avLocal[0]);
if (m_InvertDirection)
{
r1 = (float)Math.Atan2(cv[1], cv[0]) - A;
r2 = -C + PI + angleCorrection;
}
else
{
r1 = A + (float)Math.Atan2(cv[1], cv[0]);
r2 = C - PI + angleCorrection;
}
}
else if (m_InvertDirection)
{
r1 = (float)Math.Atan2(cv[1], cv[0]) - A;
r2 = -C + PI;
}
else
{
r1 = A + (float)Math.Atan2(cv[1], cv[0]);
r2 = C - PI;
}
ConstrainRotation(fk1, r1);
ConstrainRotation(firstChild, r2);
if (firstChild != fk2)
{
ActorBone bone = fk2.m_Bone;
Mat2D.Multiply(bone.WorldTransform, bone.Parent.WorldTransform, bone.Transform);
}
// Simple storage, need this for interpolation.
fk1.m_Angle = r1;
firstChild.m_Angle = r2;
}
public override void Constrain(ActorNode node)
{
ActorNode target = m_Target as ActorNode;
if (target == null)
{
return;
}
Vec2D worldTargetTranslation = new Vec2D();
target.GetWorldTranslation(worldTargetTranslation);
if (m_InfluencedBones.Length == 0)
{
return;
}
// Decompose the chain.
foreach (BoneChain item in m_FKChain)
{
ActorBone bone = item.m_Bone;
Mat2D parentWorld = bone.Parent.WorldTransform;
Mat2D.Invert(item.m_ParentWorldInverse, parentWorld);
Mat2D.Multiply(bone.Transform, item.m_ParentWorldInverse, bone.WorldTransform);
Mat2D.Decompose(bone.Transform, item.m_TransformComponents);
}
int count = m_BoneData.Count;
if (count == 1)
{
Solve1(m_BoneData[0], worldTargetTranslation);
}
else if (count == 2)
{
Solve2(m_BoneData[0], m_BoneData[1], worldTargetTranslation);
}
else
{
BoneChain tip = m_BoneData[count - 1];
for (int i = 0; i < count - 1; i++)
{
BoneChain item = m_BoneData[i];
Solve2(item, tip, worldTargetTranslation);
for (int j = item.m_Index + 1; j < m_FKChain.Length - 1; j++)
{
BoneChain fk = m_FKChain[j];
Mat2D.Invert(fk.m_ParentWorldInverse, fk.m_Bone.Parent.WorldTransform);
}
}
}
// At the end, mix the FK angle with the IK angle by strength
if (m_Strength != 1.0)
{
foreach (BoneChain fk in m_FKChain)
{
if (!fk.m_Included)
{
ActorBone bone = fk.m_Bone;
Mat2D.Multiply(bone.WorldTransform, bone.Parent.WorldTransform, bone.Transform);
continue;
}
float fromAngle = fk.m_TransformComponents.Rotation % PI2;
float toAngle = fk.m_Angle % PI2;
float diff = toAngle - fromAngle;
if (diff > PI)
{
diff -= PI2;
}
else if (diff < -PI)
{
diff += PI2;
}
float angle = fromAngle + diff * m_Strength;
ConstrainRotation(fk, angle);
}
}
}
