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();
}
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;
}