public override void Constrain(ActorNode node)
{
ActorNode target = m_Target as ActorNode;
if (target == null)
{
return;
}
Vec2D targetTranslation = target.GetWorldTranslation(new Vec2D());
Vec2D ourTranslation = m_Parent.GetWorldTranslation(new Vec2D());
Vec2D toTarget = Vec2D.Subtract(new Vec2D(), ourTranslation, targetTranslation);
float currentDistance = Vec2D.Length(toTarget);
switch (m_Mode)
{
case Mode.Closer:
if (currentDistance < m_Distance)
{
return;
}
break;
case Mode.Further:
if (currentDistance > m_Distance)
{
return;
}
break;
}
if (currentDistance < 0.001)
{
return;
}
Vec2D.Scale(toTarget, toTarget, 1.0f / currentDistance);
Vec2D.Scale(toTarget, toTarget, m_Distance);
Mat2D world = m_Parent.WorldTransform;
Vec2D position = Vec2D.Lerp(new Vec2D(), ourTranslation, Vec2D.Add(new Vec2D(), targetTranslation, toTarget), m_Strength);
world[4] = position[0];
world[5] = position[1];
}
public override void Update(byte dirt)
{
ActorBone bone = m_Parent as ActorBone;
ActorBone parentBone = bone.Parent as ActorBone;
JellyComponent parentBoneJelly = parentBone == null ? null : parentBone.m_Jelly;
Mat2D inverseWorld = new Mat2D();
if (!Mat2D.Invert(inverseWorld, bone.WorldTransform))
{
return;
}
if (m_InTarget != null)
{
Vec2D translation = m_InTarget.GetWorldTranslation(new Vec2D());
Vec2D.TransformMat2D(m_InPoint, translation, inverseWorld);
Vec2D.Normalize(m_InDirection, m_InPoint);
}
else if (parentBone != null)
{
if (parentBone.FirstBone == bone && parentBoneJelly != null && parentBoneJelly.m_OutTarget != null)
{
Vec2D translation = parentBoneJelly.m_OutTarget.GetWorldTranslation(new Vec2D());
Vec2D localParentOut = Vec2D.TransformMat2D(new Vec2D(), translation, inverseWorld);
Vec2D.Normalize(localParentOut, localParentOut);
Vec2D.Negate(m_InDirection, localParentOut);
}
else
{
Vec2D d1 = new Vec2D(1.0f, 0.0f);
Vec2D d2 = new Vec2D(1.0f, 0.0f);
Vec2D.TransformMat2(d1, d1, parentBone.WorldTransform);
Vec2D.TransformMat2(d2, d2, bone.WorldTransform);
Vec2D sum = Vec2D.Add(new Vec2D(), d1, d2);
Vec2D.TransformMat2(m_InDirection, sum, inverseWorld);
Vec2D.Normalize(m_InDirection, m_InDirection);
}
m_InPoint[0] = m_InDirection[0] * m_EaseIn * bone.Length * CurveConstant;
m_InPoint[1] = m_InDirection[1] * m_EaseIn * bone.Length * CurveConstant;
}
else
{
m_InDirection[0] = 1.0f;
m_InDirection[1] = 0.0f;
m_InPoint[0] = m_InDirection[0] * m_EaseIn * bone.Length * CurveConstant;
}
if (m_OutTarget != null)
{
Vec2D translation = m_OutTarget.GetWorldTranslation(new Vec2D());
Vec2D.TransformMat2D(m_OutPoint, translation, inverseWorld);
Vec2D tip = new Vec2D(bone.Length, 0.0f);
Vec2D.Subtract(m_OutDirection, m_OutPoint, tip);
Vec2D.Normalize(m_OutDirection, m_OutDirection);
}
else if (bone.FirstBone != null)
{
ActorBone firstBone = bone.FirstBone;
JellyComponent firstBoneJelly = firstBone.m_Jelly;
if (firstBoneJelly != null && firstBoneJelly.m_InTarget != null)
{
Vec2D translation = firstBoneJelly.m_InTarget.GetWorldTranslation(new Vec2D());
Vec2D worldChildInDir = Vec2D.Subtract(new Vec2D(), firstBone.GetWorldTranslation(new Vec2D()), translation);
Vec2D.TransformMat2(m_OutDirection, worldChildInDir, inverseWorld);
}
else
{
Vec2D d1 = new Vec2D(1.0f, 0.0f);
Vec2D d2 = new Vec2D(1.0f, 0.0f);
Vec2D.TransformMat2(d1, d1, firstBone.WorldTransform);
Vec2D.TransformMat2(d2, d2, bone.WorldTransform);
Vec2D sum = Vec2D.Add(new Vec2D(), d1, d2);
Vec2D.Negate(sum, sum);
Vec2D.TransformMat2(m_OutDirection, sum, inverseWorld);
Vec2D.Normalize(m_OutDirection, m_OutDirection);
}
Vec2D.Normalize(m_OutDirection, m_OutDirection);
Vec2D scaledOut = Vec2D.Scale(new Vec2D(), m_OutDirection, m_EaseOut * bone.Length * CurveConstant);
m_OutPoint[0] = bone.Length;
m_OutPoint[1] = 0.0f;
Vec2D.Add(m_OutPoint, m_OutPoint, scaledOut);
}
else
{
m_OutDirection[0] = -1.0f;
m_OutDirection[1] = 0.0f;
Vec2D scaledOut = Vec2D.Scale(new Vec2D(), m_OutDirection, m_EaseOut * bone.Length * CurveConstant);
m_OutPoint[0] = bone.Length;
m_OutPoint[1] = 0.0f;
Vec2D.Add(m_OutPoint, m_OutPoint, scaledOut);
}
UpdateJellies();
}
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);
}
}
}