// ------------------------------------------------------------------------------------
/// <summary>
/// Solve IK for chain of two bones + change rotation of end bone.
/// </summary>
/// <param name="characterBones">bone storage</param>
/// <param name="ikChain">description of bone chain</param>
/// <param name="finalPosition">desired position of end bone</param>
/// <param name="finalNormal">desired normal of end bone - would be projected on plane first bone-second bone-third bone</param>
/// <param name="fromBindPose">solve this starting from the bind pose</param>
/// <returns>true on success</returns>
public static bool SolveIkTwoBones(MyCharacterBone[] characterBones, MyAnimationIkChainExt ikChain, ref Vector3 finalPosition, ref Vector3 finalNormal, bool fromBindPose)
{
int boneIndex = ikChain.BoneIndex;
float finalMinRot = MathHelper.ToRadians(ikChain.MinEndPointRotation);
float finalMaxRot = MathHelper.ToRadians(ikChain.MaxEndPointRotation);
Vector3 lastPoleVector = ikChain.LastPoleVector;
int chainLength = ikChain.ChainLength;
bool alignBoneWithTerrain = ikChain.AlignBoneWithTerrain;
MyCharacterBone thirdBone = characterBones[boneIndex];
if (thirdBone == null)
{
return(false);
}
MyCharacterBone secondBone = thirdBone.Parent;
for (int i = 2; i < chainLength; i++)
{
secondBone = secondBone.Parent;
}
if (secondBone == null)
{
return(false);
}
MyCharacterBone firstBone = secondBone.Parent;
if (firstBone == null)
{
return(false);
}
if (fromBindPose)
{
firstBone.SetCompleteBindTransform();
secondBone.SetCompleteBindTransform();
thirdBone.SetCompleteBindTransform();
firstBone.ComputeAbsoluteTransform(true);
}
Matrix thirdBoneTransformBackup = thirdBone.AbsoluteTransform;
//Vector3 firstBoneTransformRightDir = firstBone.AbsoluteTransform.Right;
Vector3 firstBoneOrigin = firstBone.AbsoluteTransform.Translation;
Vector3 secondBoneOrigin = secondBone.AbsoluteTransform.Translation;
Vector3 thirdBoneOrigin = thirdBone.AbsoluteTransform.Translation;
// Vector3D finalPosition comes from parameter
Vector3 secondMinusFirst = secondBoneOrigin - firstBoneOrigin;
Vector3 finalMinusFirst = finalPosition - firstBoneOrigin;
//Vector3 finalMinusSecond = finalPosition - secondBoneOrigin;
Vector3 poleVectorNormalized;
Vector3 thirdMinusFirst = thirdBoneOrigin - firstBoneOrigin;
Vector3.Cross(ref secondMinusFirst, ref thirdMinusFirst, out poleVectorNormalized);
// project to 2D (only vectors)
poleVectorNormalized.Normalize();
poleVectorNormalized = Vector3.Normalize(Vector3.Lerp(poleVectorNormalized, lastPoleVector, m_poleVectorChangeSmoothness));
Vector3 planeDirY = Vector3.Normalize(finalMinusFirst); // finalMinusFirst? thirdMinusFirst?
Vector3 planeDirX = Vector3.Normalize(Vector3.Cross(planeDirY, poleVectorNormalized));
//Vector2 firstBoneOrigin2D = new Vector2(0, 0);
Vector2 secondBoneOrigin2D = new Vector2(planeDirX.Dot(ref secondMinusFirst), planeDirY.Dot(ref secondMinusFirst));
Vector2 thirdBoneOrigin2D = new Vector2(planeDirX.Dot(ref thirdMinusFirst), planeDirY.Dot(ref thirdMinusFirst));
Vector2 finalPosition2D = new Vector2(planeDirX.Dot(ref finalMinusFirst), planeDirY.Dot(ref finalMinusFirst));
Vector2 terrainNormal2D = new Vector2(planeDirX.Dot(ref finalNormal), planeDirY.Dot(ref finalNormal));
float firstBoneLength = (secondBoneOrigin2D /* - 0*/).Length();
float secondBoneLength = (thirdBoneOrigin2D - secondBoneOrigin2D).Length();
float finalDistance = finalPosition2D.Length();
if (firstBoneLength + secondBoneLength <= finalDistance)
{
finalPosition2D = (firstBoneLength + secondBoneLength) * finalPosition2D / finalDistance;
// too far
//return false;
}
// mid-joint -> wanted position in 2D
Vector2 newSecondBoneOrigin2D;
{
newSecondBoneOrigin2D.Y = (finalPosition2D.Y * finalPosition2D.Y - secondBoneLength * secondBoneLength + firstBoneLength * firstBoneLength) / (2.0f * finalPosition2D.Y);
float srqtArg = firstBoneLength * firstBoneLength - newSecondBoneOrigin2D.Y * newSecondBoneOrigin2D.Y;
newSecondBoneOrigin2D.X = (float)Math.Sqrt(srqtArg > 0 ? srqtArg : 0);
}
// project back
Vector3 newSecondBoneOrigin = firstBoneOrigin + planeDirX * newSecondBoneOrigin2D.X + planeDirY * newSecondBoneOrigin2D.Y;
Vector3 newSecondMinusFirst = newSecondBoneOrigin - firstBoneOrigin;
Vector3 newThirdMinusSecond = finalPosition - newSecondBoneOrigin;
Vector3 newTerrainNormal = planeDirX * terrainNormal2D.X + planeDirY * terrainNormal2D.Y;
newTerrainNormal.Normalize();
// set the rotations in the bones
// first bone ---------------------------------
Matrix firstBoneAbsoluteFinal = firstBone.AbsoluteTransform;
Quaternion rotFirstDelta = Quaternion.CreateFromTwoVectors(secondMinusFirst, newSecondMinusFirst);
firstBoneAbsoluteFinal.Right = Vector3.Transform(firstBoneAbsoluteFinal.Right, rotFirstDelta);
firstBoneAbsoluteFinal.Up = Vector3.Transform(firstBoneAbsoluteFinal.Up, rotFirstDelta);
firstBoneAbsoluteFinal.Forward = Vector3.Transform(firstBoneAbsoluteFinal.Forward, rotFirstDelta);
firstBone.SetCompleteTransformFromAbsoluteMatrix(ref firstBoneAbsoluteFinal, true);
firstBone.ComputeAbsoluteTransform();
// second bone ---------------------------------
Matrix secondBoneAbsoluteFinal = secondBone.AbsoluteTransform;
Quaternion rotSecondDelta = Quaternion.CreateFromTwoVectors(thirdBone.AbsoluteTransform.Translation - secondBone.AbsoluteTransform.Translation, newThirdMinusSecond);
secondBoneAbsoluteFinal.Right = Vector3.Transform(secondBoneAbsoluteFinal.Right, rotSecondDelta);
secondBoneAbsoluteFinal.Up = Vector3.Transform(secondBoneAbsoluteFinal.Up, rotSecondDelta);
secondBoneAbsoluteFinal.Forward = Vector3.Transform(secondBoneAbsoluteFinal.Forward, rotSecondDelta);
secondBone.SetCompleteTransformFromAbsoluteMatrix(ref secondBoneAbsoluteFinal, true);
secondBone.ComputeAbsoluteTransform();
//// third bone ----------------------------------
if (ikChain.EndBoneTransform.HasValue)
{
MatrixD localTransformRelated = ikChain.EndBoneTransform.Value * MatrixD.Invert((MatrixD)thirdBone.BindTransform * thirdBone.Parent.AbsoluteTransform);
thirdBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
thirdBone.Translation = (Vector3)localTransformRelated.Translation;
thirdBone.ComputeAbsoluteTransform();
}
else if (alignBoneWithTerrain)
{
Matrix footRotation;
Vector3 finalRotPoleVec;
Vector3.Cross(ref newTerrainNormal, ref Vector3.Up, out finalRotPoleVec);
float deltaAngle = MyUtils.GetAngleBetweenVectors(newTerrainNormal, Vector3.Up);
if (finalRotPoleVec.Dot(poleVectorNormalized) > 0)
{
deltaAngle = -deltaAngle;
}
deltaAngle = MathHelper.Clamp(deltaAngle, finalMinRot, finalMaxRot);
Matrix.CreateFromAxisAngle(ref poleVectorNormalized, deltaAngle, out footRotation);
ikChain.LastAligningRotationMatrix = Matrix.Lerp(ikChain.LastAligningRotationMatrix, footRotation, ikChain.AligningSmoothness);
Matrix thirdBoneAbsoluteFinal = thirdBoneTransformBackup.GetOrientation() * ikChain.LastAligningRotationMatrix;
thirdBoneAbsoluteFinal.Translation = thirdBone.AbsoluteTransform.Translation;
thirdBone.SetCompleteTransformFromAbsoluteMatrix(ref thirdBoneAbsoluteFinal, true);
thirdBone.ComputeAbsoluteTransform();
}
// Debugging of the solver.
if (m_showDebugDrawings)
{
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin, ref DebugTransform),
Color.Yellow, Color.Red, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(thirdBoneOrigin, ref DebugTransform),
Color.Yellow, Color.Red, false);
MyRenderProxy.DebugDrawSphere(Vector3D.Transform(finalPosition, ref DebugTransform), 0.05f, Color.Cyan, 1.0f, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin + poleVectorNormalized, ref DebugTransform),
Color.PaleGreen, Color.PaleGreen, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirX, ref DebugTransform),
Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirY, ref DebugTransform),
Color.White, Color.White, false);
MyRenderProxy.DebugDrawSphere(Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform), 0.05f, Color.Green, 1.0f, false);
MyRenderProxy.DebugDrawAxis(firstBone.AbsoluteTransform * DebugTransform, 0.5f, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(finalPosition, ref DebugTransform), Vector3D.Transform(finalPosition + newTerrainNormal, ref DebugTransform),
Color.Black, Color.LightBlue, false);
MyRenderProxy.DebugDrawArrow3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform),
Color.Green, Color.White, false);
}
ikChain.LastPoleVector = poleVectorNormalized;
return(true);
}
// ------------------------------------------------------------------------------------
/// <summary>
/// Solve IK for chain of two bones + change rotation of end bone.
/// </summary>
/// <param name="characterBones">bone storage</param>
/// <param name="ikChain">description of bone chain</param>
/// <param name="finalPosition">desired position of end bone</param>
/// <param name="finalNormal">desired normal of end bone - would be projected on plane first bone-second bone-third bone</param>
/// <param name="fromBindPose">solve this starting from the bind pose</param>
/// <returns>true on success</returns>
public static bool SolveIkTwoBones(MyCharacterBone[] characterBones, MyAnimationIkChainExt ikChain, ref Vector3 finalPosition, ref Vector3 finalNormal, bool fromBindPose)
{
int boneIndex = ikChain.BoneIndex;
float finalMinRot = MathHelper.ToRadians(ikChain.MinEndPointRotation);
float finalMaxRot = MathHelper.ToRadians(ikChain.MaxEndPointRotation);
Vector3 lastPoleVector = ikChain.LastPoleVector;
int chainLength = ikChain.ChainLength;
bool alignBoneWithTerrain = ikChain.AlignBoneWithTerrain;
MyCharacterBone thirdBone = characterBones[boneIndex];
if (thirdBone == null)
return false;
MyCharacterBone secondBone = thirdBone.Parent;
for (int i = 2; i < chainLength; i++)
{
secondBone = secondBone.Parent;
}
if (secondBone == null)
return false;
MyCharacterBone firstBone = secondBone.Parent;
if (firstBone == null)
return false;
if (fromBindPose)
{
firstBone.SetCompleteBindTransform();
secondBone.SetCompleteBindTransform();
thirdBone.SetCompleteBindTransform();
firstBone.ComputeAbsoluteTransform(true);
}
Matrix thirdBoneTransformBackup = thirdBone.AbsoluteTransform;
//Vector3 firstBoneTransformRightDir = firstBone.AbsoluteTransform.Right;
Vector3 firstBoneOrigin = firstBone.AbsoluteTransform.Translation;
Vector3 secondBoneOrigin = secondBone.AbsoluteTransform.Translation;
Vector3 thirdBoneOrigin = thirdBone.AbsoluteTransform.Translation;
// Vector3D finalPosition comes from parameter
Vector3 secondMinusFirst = secondBoneOrigin - firstBoneOrigin;
Vector3 finalMinusFirst = finalPosition - firstBoneOrigin;
//Vector3 finalMinusSecond = finalPosition - secondBoneOrigin;
Vector3 poleVectorNormalized;
Vector3 thirdMinusFirst = thirdBoneOrigin - firstBoneOrigin;
Vector3.Cross(ref secondMinusFirst, ref thirdMinusFirst, out poleVectorNormalized);
// project to 2D (only vectors)
poleVectorNormalized.Normalize();
poleVectorNormalized = Vector3.Normalize(Vector3.Lerp(poleVectorNormalized, lastPoleVector, m_poleVectorChangeSmoothness));
Vector3 planeDirY = Vector3.Normalize(finalMinusFirst); // finalMinusFirst? thirdMinusFirst?
Vector3 planeDirX = Vector3.Normalize(Vector3.Cross(planeDirY, poleVectorNormalized));
//Vector2 firstBoneOrigin2D = new Vector2(0, 0);
Vector2 secondBoneOrigin2D = new Vector2(planeDirX.Dot(ref secondMinusFirst), planeDirY.Dot(ref secondMinusFirst));
Vector2 thirdBoneOrigin2D = new Vector2(planeDirX.Dot(ref thirdMinusFirst), planeDirY.Dot(ref thirdMinusFirst));
Vector2 finalPosition2D = new Vector2(planeDirX.Dot(ref finalMinusFirst), planeDirY.Dot(ref finalMinusFirst));
Vector2 terrainNormal2D = new Vector2(planeDirX.Dot(ref finalNormal), planeDirY.Dot(ref finalNormal));
float firstBoneLength = (secondBoneOrigin2D/* - 0*/).Length();
float secondBoneLength = (thirdBoneOrigin2D - secondBoneOrigin2D).Length();
float finalDistance = finalPosition2D.Length();
if (firstBoneLength + secondBoneLength <= finalDistance)
{
finalPosition2D = (firstBoneLength + secondBoneLength) * finalPosition2D / finalDistance;
// too far
//return false;
}
// mid-joint -> wanted position in 2D
Vector2 newSecondBoneOrigin2D;
{
newSecondBoneOrigin2D.Y = (finalPosition2D.Y * finalPosition2D.Y - secondBoneLength * secondBoneLength + firstBoneLength * firstBoneLength) / (2.0f * finalPosition2D.Y);
float srqtArg = firstBoneLength * firstBoneLength - newSecondBoneOrigin2D.Y * newSecondBoneOrigin2D.Y;
newSecondBoneOrigin2D.X = (float)Math.Sqrt(srqtArg > 0 ? srqtArg : 0);
}
// project back
Vector3 newSecondBoneOrigin = firstBoneOrigin + planeDirX * newSecondBoneOrigin2D.X + planeDirY * newSecondBoneOrigin2D.Y;
Vector3 newSecondMinusFirst = newSecondBoneOrigin - firstBoneOrigin;
Vector3 newThirdMinusSecond = finalPosition - newSecondBoneOrigin;
Vector3 newTerrainNormal = planeDirX * terrainNormal2D.X + planeDirY * terrainNormal2D.Y;
newTerrainNormal.Normalize();
// set the rotations in the bones
// first bone ---------------------------------
Matrix firstBoneAbsoluteFinal = firstBone.AbsoluteTransform;
Quaternion rotFirstDelta = Quaternion.CreateFromTwoVectors(secondMinusFirst, newSecondMinusFirst);
firstBoneAbsoluteFinal.Right = Vector3.Transform(firstBoneAbsoluteFinal.Right, rotFirstDelta);
firstBoneAbsoluteFinal.Up = Vector3.Transform(firstBoneAbsoluteFinal.Up, rotFirstDelta);
firstBoneAbsoluteFinal.Forward = Vector3.Transform(firstBoneAbsoluteFinal.Forward, rotFirstDelta);
firstBone.SetCompleteTransformFromAbsoluteMatrix(ref firstBoneAbsoluteFinal, true);
firstBone.ComputeAbsoluteTransform();
// second bone ---------------------------------
Matrix secondBoneAbsoluteFinal = secondBone.AbsoluteTransform;
Quaternion rotSecondDelta = Quaternion.CreateFromTwoVectors(thirdBone.AbsoluteTransform.Translation - secondBone.AbsoluteTransform.Translation, newThirdMinusSecond);
secondBoneAbsoluteFinal.Right = Vector3.Transform(secondBoneAbsoluteFinal.Right, rotSecondDelta);
secondBoneAbsoluteFinal.Up = Vector3.Transform(secondBoneAbsoluteFinal.Up, rotSecondDelta);
secondBoneAbsoluteFinal.Forward = Vector3.Transform(secondBoneAbsoluteFinal.Forward, rotSecondDelta);
secondBone.SetCompleteTransformFromAbsoluteMatrix(ref secondBoneAbsoluteFinal, true);
secondBone.ComputeAbsoluteTransform();
//// third bone ----------------------------------
if (ikChain.EndBoneTransform.HasValue)
{
MatrixD localTransformRelated = ikChain.EndBoneTransform.Value * MatrixD.Invert((MatrixD)thirdBone.BindTransform * thirdBone.Parent.AbsoluteTransform);
thirdBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
thirdBone.Translation = (Vector3)localTransformRelated.Translation;
thirdBone.ComputeAbsoluteTransform();
}
else if (alignBoneWithTerrain)
{
Matrix footRotation;
Vector3 finalRotPoleVec;
Vector3.Cross(ref newTerrainNormal, ref Vector3.Up, out finalRotPoleVec);
float deltaAngle = MyUtils.GetAngleBetweenVectors(newTerrainNormal, Vector3.Up);
if (finalRotPoleVec.Dot(poleVectorNormalized) > 0)
deltaAngle = -deltaAngle;
deltaAngle = MathHelper.Clamp(deltaAngle, finalMinRot, finalMaxRot);
Matrix.CreateFromAxisAngle(ref poleVectorNormalized, deltaAngle, out footRotation);
ikChain.LastAligningRotationMatrix = Matrix.Lerp(ikChain.LastAligningRotationMatrix, footRotation, ikChain.AligningSmoothness);
Matrix thirdBoneAbsoluteFinal = thirdBoneTransformBackup.GetOrientation() * ikChain.LastAligningRotationMatrix;
thirdBoneAbsoluteFinal.Translation = thirdBone.AbsoluteTransform.Translation;
thirdBone.SetCompleteTransformFromAbsoluteMatrix(ref thirdBoneAbsoluteFinal, true);
thirdBone.ComputeAbsoluteTransform();
}
// Debugging of the solver.
if (m_showDebugDrawings)
{
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin, ref DebugTransform),
Color.Yellow, Color.Red, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(thirdBoneOrigin, ref DebugTransform),
Color.Yellow, Color.Red, false);
MyRenderProxy.DebugDrawSphere(Vector3D.Transform(finalPosition, ref DebugTransform), 0.05f, Color.Cyan, 1.0f, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin + poleVectorNormalized, ref DebugTransform),
Color.PaleGreen, Color.PaleGreen, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirX, ref DebugTransform),
Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirY, ref DebugTransform),
Color.White, Color.White, false);
MyRenderProxy.DebugDrawSphere(Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform), 0.05f, Color.Green, 1.0f, false);
MyRenderProxy.DebugDrawAxis(firstBone.AbsoluteTransform * DebugTransform, 0.5f, false);
MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(finalPosition, ref DebugTransform), Vector3D.Transform(finalPosition + newTerrainNormal, ref DebugTransform),
Color.Black, Color.LightBlue, false);
MyRenderProxy.DebugDrawArrow3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform),
Color.Green, Color.White, false);
}
ikChain.LastPoleVector = poleVectorNormalized;
return true;
}