/// <summary>
/// Set the new bone count and default (rest) pose.
/// </summary>
public void Reset(MyCharacterBone[] restPoseBones)
{
m_freeToUse.Clear();
m_taken.Clear();
if (restPoseBones == null)
{
m_boneCount = 0;
m_restPose = null;
return;
}
int boneCount = restPoseBones.Length;
m_boneCount = boneCount;
m_restPose = new List<MyAnimationClip.BoneState>(boneCount);
for (int i = 0; i < boneCount; i++)
{
m_restPose.Add(new MyAnimationClip.BoneState
{
Translation = restPoseBones[i].BindTransform.Translation,
Rotation = Quaternion.CreateFromRotationMatrix(restPoseBones[i].BindTransform)
});
}
m_currentDefaultPose = m_restPose;
}
// Rebuild mapping clip bone indices to character bone indices.
private void RebuildBoneIndices(MyCharacterBone[] characterBones)
{
// find indices of clip bones
// optimize? see m_boneIndices declaration
m_boneIndicesMapping = new int[m_animationClip.Bones.Count];
for (int i = 0; i < m_animationClip.Bones.Count; i++)
{
m_boneIndicesMapping[i] = -1;
for (int j = 0; j < characterBones.Length; j++)
{
if (m_animationClip.Bones[i].Name == characterBones[j].Name)
{
m_boneIndicesMapping[i] = j;
break;
}
}
if (m_boneIndicesMapping[i] == -1)
{
// VRAGE TODO: at least log it? this is unfortunatelly a common situation
//Debug.Fail(String.Format("Clip bone not found in character. {0} in {1}",
// m_animationClip.Bones[i].Name,
// string.Join(",", (from item in characterBones select item.Name).ToArray())));
}
}
}
internal void AddChild(MyCharacterBone child)
{
m_children.Add(child);
}
/// <summary>
/// Translate all bones. Translation vector is given in model space.
/// We expect that absolute transforms are already computed.
/// </summary>
public static void TranslateAllBones(MyCharacterBone[] characterBones, Vector3 translationModelSpace)
{
if (characterBones == null || characterBones.Length < 0)
return;
foreach (MyCharacterBone bone in characterBones)
{
if (bone.Parent == null)
{
bone.Translation += translationModelSpace;
bone.ComputeBoneTransform();
bone.m_changed = false;
}
bone.m_absoluteStorage[bone.Index].Translation += translationModelSpace;
}
}
/// <summary>
/// Compute absolute bone transforms for whole hierarchy.
/// Expects the array to be sorted by depth in hiearachy.
/// </summary>
public static void ComputeAbsoluteTransforms(MyCharacterBone[] bones)
{
foreach (MyCharacterBone bone in bones)
{
if (bone.Parent != null)
{
bone.m_changed = bone.ComputeBoneTransform() || bone.Parent.m_changed; // propagate the change to children
if (bone.m_changed)
Matrix.Multiply(ref bone.m_relativeStorage[bone.Index], ref bone.m_absoluteStorage[bone.Parent.Index], out bone.m_absoluteStorage[bone.Index]);
}
else
{
bone.m_changed = bone.ComputeBoneTransform();
if (bone.m_changed)
bone.m_absoluteStorage[bone.Index] = bone.m_relativeStorage[bone.Index];
}
}
foreach (MyCharacterBone bone in bones)
bone.m_changed = false;
}
/// <summary>
/// Constructor for a bone object
/// </summary>
/// <param name="name">The name of the bone</param>
/// <param name="bindTransform">The initial bind transform for the bone</param>
/// <param name="parent">A parent for this bone</param>
/// <param name="index">Index of this bone in storage arrays.</param>
/// <param name="relativeStorage">reference to matrix array storing all relative transforms of the skeleton</param>
/// <param name="absoluteStorage">reference to matrix array storing all absolute transforms of the skeleton</param>
public MyCharacterBone(string name, MyCharacterBone parent, Matrix bindTransform,
int index, Matrix[] relativeStorage, Matrix[] absoluteStorage)
{
Debug.Assert(index >= 0);
Debug.Assert(relativeStorage != null);
Debug.Assert(absoluteStorage != null);
Index = index;
m_relativeStorage = relativeStorage;
m_absoluteStorage = absoluteStorage;
this.Name = name;
this.m_parent = parent;
Depth = GetHierarchyDepth();
this.m_bindTransform = bindTransform;
this.m_bindTransformInv = Matrix.Invert(bindTransform);
this.m_bindRotationInv = Quaternion.CreateFromRotationMatrix(m_bindTransformInv);
this.m_children = new List<MyCharacterBone>();
if (this.m_parent != null) this.m_parent.AddChild(this);
// Set the skinning bind transform
// That is the inverse of the absolute transform in the bind pose
ComputeAbsoluteTransform();
SkinTransform = Matrix.Invert(AbsoluteTransform);
}
internal void AddChild(MyCharacterBone child)
{
m_children.Add(child);
}
// ------------------------------------------------------------------------------------
/// <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);
}
// ------------------------------------------------------------------------------------
public static bool SolveIkCcd(MyCharacterBone[] characterBones, int boneIndex, int chainLength, ref Vector3D finalPosition)
{
Vector3 desiredEnd = finalPosition;
Vector3 curEnd;
int tries = 0;
int maxTries = 50;
float stopDistanceSq = 0.005f * 0.005f;
MyCharacterBone endBone = characterBones[boneIndex];
MyCharacterBone currentBone = endBone;
int[] boneIndices = m_boneIndicesPreallocated;
// ---- preparation ----
for (int i = 0; i < chainLength; i++)
{
if (currentBone == null)
{
chainLength = i;
break;
}
boneIndices[i] = currentBone.Index;
currentBone = currentBone.Parent;
}
{
curEnd = endBone.AbsoluteTransform.Translation;
float initialDistSqInv = 1 / (float)Vector3D.DistanceSquared(curEnd, desiredEnd);
do
{
for (int i = 0; i < chainLength; i++)
{
var bone = characterBones[boneIndices[i]];
// first recalculate current final transformation
endBone.ComputeAbsoluteTransform();
// compute the position of the root
Matrix currentMatrix = bone.AbsoluteTransform;
Vector3 rootPos = currentMatrix.Translation;
//Vector3 lastEnd = curEnd;
curEnd = endBone.AbsoluteTransform.Translation;
// this is our current end of the final bone
// get the difference from desired and and current final position
double distanceSq = Vector3D.DistanceSquared(curEnd, desiredEnd);
//{
// Color c = Color.FromNonPremultiplied(new Vector4(4 * (float) (distanceSq),
// 1 - 4 * (float) (distanceSq), 0, 1));
// VRageRender.MyRenderProxy.DebugDrawLine3D(
// Vector3D.Transform(lastEnd, worldMatrix),
// Vector3D.Transform(curEnd, worldMatrix), c, c, false);
//}
// see if i'm already close enough
if (distanceSq > stopDistanceSq)
{
// create the vector to the current effector posm this is the difference vector
Vector3 curVector = curEnd - rootPos;
// create the desired effector position vector
Vector3 targetVector = desiredEnd - rootPos;
// normalize the vectors (expensive, requires a sqrt)
// MZ: we don't need to do that
// curVector.Normalize();
// targetVector.Normalize();
double curVectorLenSq = curVector.LengthSquared();
double targetVectorLenSq = targetVector.LengthSquared();
// the dot product gives me the cosine of the desired angle
// cosAngle = curVector.Dot(targetVector);
double dotCurTarget = curVector.Dot(targetVector);
// if the dot product returns 1.0, i don't need to rotate as it is 0 degrees
// MZ: yes, but when does this happen to be exactly 1???
// if (cosAngle < 1.0)
if (dotCurTarget < 0 || dotCurTarget * dotCurTarget < curVectorLenSq * targetVectorLenSq * (1 - MyMathConstants.EPSILON))
{
// use the cross product to check which way to rotate
//var rotationAxis = curVector.Cross(targetVector);
//rotationAxis.Normalize();
//turnAngle = System.Math.Acos(cosAngle); // get the angle
// get the matrix needed to rotate to the desired position
//Matrix rotation = Matrix.CreateFromAxisAngle((Vector3) rotationAxis,
// (float) turnAngle * gain);
// get the absolute matrix rotation ie - rotation including all the bones before
Matrix rotation;
float weight = 1 / (initialDistSqInv * (float)distanceSq + 1);
Vector3 weightedTarget = Vector3.Lerp(curVector, targetVector, weight);
Matrix.CreateRotationFromTwoVectors(ref curVector, ref weightedTarget, out rotation);
Matrix absoluteTransform = Matrix.Normalize(currentMatrix).GetOrientation() * rotation;
// MZ: faster
// compute just the local matrix for the bone - need to multiply with inversion ot its parent matrix and original bind transform
Matrix parentMatrix = Matrix.Identity;
if (bone.Parent != null) parentMatrix = bone.Parent.AbsoluteTransform;
parentMatrix = Matrix.Normalize(parentMatrix); // may have different scale
Matrix localTransform = Matrix.Multiply(absoluteTransform,
Matrix.Invert(bone.BindTransform * parentMatrix));
// now change the current matrix rotation
bone.Rotation = Quaternion.CreateFromRotationMatrix(localTransform);
// and recompute the transformation
bone.ComputeAbsoluteTransform();
}
}
}
// quit if i am close enough or been running long enough
} while (tries++ < maxTries &&
Vector3D.DistanceSquared(curEnd, desiredEnd) > stopDistanceSq);
}
return Vector3D.DistanceSquared(curEnd, desiredEnd) <= stopDistanceSq;
}
// ------------------------------------------------------------------------------------
/// <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;
}
private void MoveTheBodyDown(MyCharacterBone[] characterBones)
{
if (m_offsetFiltering.Count == m_offsetFilteringSampleCount)
{
m_offsetFiltering[m_offsetFilteringCursor++] = m_characterDirDownOffset;
if (m_offsetFilteringCursor == m_offsetFilteringSampleCount)
m_offsetFilteringCursor = 0;
}
else
{
m_offsetFiltering.Add(m_characterDirDownOffset);
}
float filteredOffsetValue = float.MinValue;
foreach (float offset in m_offsetFiltering)
{
filteredOffsetValue = Math.Max(filteredOffsetValue, offset);
}
m_filteredOffsetValue = filteredOffsetValue;
if (m_offsetFilteringCursor >= m_offsetFilteringSampleCount)
m_offsetFilteringCursor = 0;
if (filteredOffsetValue * filteredOffsetValue > MyMathConstants.EPSILON)
{
MyCharacterBone bone = characterBones[0];
while (bone.Parent != null)
bone = bone.Parent;
var rootBoneTranslation = bone.Translation;
m_rootBoneVerticalOffset = filteredOffsetValue * m_currentFeetIkInfluence;
rootBoneTranslation.Y += m_rootBoneVerticalOffset;
bone.Translation = rootBoneTranslation;
bone.ComputeAbsoluteTransform(); // move the skeleton down
}
}
private void BackupIgnoredBones(MyCharacterBone[] characterBones)
{
m_ignoredBonesBackup.Clear();
for (int i = 0; i < characterBones.Length; i++)
if (m_ignoredBonesTable[i])
m_ignoredBonesBackup.Add(characterBones[i].AbsoluteTransform);
}
private void RestoreIgnoredBones(MyCharacterBone[] characterBones)
{
int j = 0;
for (int i = 0; i < characterBones.Length; i++)
if (m_ignoredBonesTable[i])
{
characterBones[i].SetCompleteTransformFromAbsoluteMatrix(m_ignoredBonesBackup[j++], false);
characterBones[i].ComputeAbsoluteTransform();
}
}
// ------------------------------------------------------------------------------------
/// <summary>
/// Solve feet positions.
/// </summary>
/// <param name="enabled">Feet resolving is enabled - this is a parameter because of blending over time.</param>
/// <param name="characterBones">Character bones storage.</param>
/// <param name="allowMovingWithBody">If feet cannot reach, move the body</param>
public void SolveFeet(bool enabled, MyCharacterBone[] characterBones, bool allowMovingWithBody)
{
m_currentFeetIkInfluence = MathHelper.Clamp(m_currentFeetIkInfluence + (enabled ? 0.1f : -0.1f), 0.0f, 1.0f);
if (m_currentFeetIkInfluence <= 0 || TerrainHeightProvider == null || characterBones == null || characterBones.Length == 0 || m_feet.Count == 0)
return;
RecreateIgnoredBonesTableIfNeeded(characterBones);
BackupIgnoredBones(characterBones);
if (allowMovingWithBody)
{
MoveTheBodyDown(characterBones); // move the body down!
}
RestoreIgnoredBones(characterBones);
float refTerrainHeight = TerrainHeightProvider.GetReferenceTerrainHeight();
float maximumNegativeOffsetValue = m_characterDirDownOffsetMax;
bool foundAnyOffset = false;
foreach (var foot in m_feet)
{
if (foot.BoneIndex == -1)
{
foreach (var bone in characterBones)
if (bone.Name == foot.BoneName)
{
foot.BoneIndex = bone.Index;
break;
}
if (foot.BoneIndex == -1) // alright alright, skip this bone
continue;
}
var footBone = characterBones[foot.BoneIndex];
var firstBone = footBone;
var secondBone = footBone;
for (int i = 0; i < foot.ChainLength; i++)
{
secondBone = firstBone;
firstBone = firstBone.Parent;
}
firstBone.ComputeAbsoluteTransform(); // will propagate the transform to children
// obtain terrain height
Vector3 bonePos = footBone.AbsoluteTransform.Translation;
float terrainHeight;
Vector3 terrainNormal;
if (TerrainHeightProvider.GetTerrainHeight(bonePos, out terrainHeight, out terrainNormal))
{
terrainNormal = Vector3.Lerp(foot.LastTerrainNormal, terrainNormal, 0.2f);
}
else
{
terrainHeight = foot.LastTerrainHeight;
terrainNormal = Vector3.Lerp(foot.LastTerrainNormal, Vector3.Up, 0.1f);
foot.LastTerrainHeight *= 0.9f;
}
foot.LastTerrainHeight = terrainHeight;
foot.LastTerrainNormal = terrainNormal;
float terrainHeightDelta = terrainHeight - refTerrainHeight;
// compute IK!
{
float originalBoneY = bonePos.Y;
float unclampedNewBoneY = terrainHeightDelta + (bonePos.Y - m_filteredOffsetValue) / terrainNormal.Y; // we expect the normal to be normalized
bonePos.Y = Math.Min(secondBone.AbsoluteTransform.Translation.Y, unclampedNewBoneY); // do not allow the feet to be above ankle
bonePos.Y = MathHelper.Lerp(originalBoneY, bonePos.Y, m_currentFeetIkInfluence);
maximumNegativeOffsetValue = MathHelper.Clamp(terrainHeightDelta, m_characterDirDownOffsetMin, maximumNegativeOffsetValue); // store worst negative offset
foundAnyOffset = true;
if (originalBoneY > bonePos.Y)
{
bonePos.Y = originalBoneY;
}
SolveIkTwoBones(characterBones, foot, ref bonePos, ref terrainNormal, fromBindPose: false);
}
}
if (!foundAnyOffset)
maximumNegativeOffsetValue = 0;
m_characterDirDownOffset = MathHelper.Lerp(maximumNegativeOffsetValue, m_characterDirDownOffset, m_characterDirDownOffsetSmoothness * m_offsetFiltering.Count / m_offsetFilteringSampleCount);
}
// ------------------------------------------------------------------------------------
public static bool SolveIkCcd(MyCharacterBone[] characterBones, int boneIndex, int chainLength, ref Vector3D finalPosition)
{
Vector3 desiredEnd = finalPosition;
Vector3 curEnd;
int tries = 0;
int maxTries = 50;
float stopDistanceSq = 0.005f * 0.005f;
MyCharacterBone endBone = characterBones[boneIndex];
MyCharacterBone currentBone = endBone;
int[] boneIndices = m_boneIndicesPreallocated;
// ---- preparation ----
for (int i = 0; i < chainLength; i++)
{
if (currentBone == null)
{
chainLength = i;
break;
}
boneIndices[i] = currentBone.Index;
currentBone = currentBone.Parent;
}
{
curEnd = endBone.AbsoluteTransform.Translation;
float initialDistSqInv = 1 / (float)Vector3D.DistanceSquared(curEnd, desiredEnd);
do
{
for (int i = 0; i < chainLength; i++)
{
var bone = characterBones[boneIndices[i]];
// first recalculate current final transformation
endBone.ComputeAbsoluteTransform();
// compute the position of the root
Matrix currentMatrix = bone.AbsoluteTransform;
Vector3 rootPos = currentMatrix.Translation;
//Vector3 lastEnd = curEnd;
curEnd = endBone.AbsoluteTransform.Translation;
// this is our current end of the final bone
// get the difference from desired and and current final position
double distanceSq = Vector3D.DistanceSquared(curEnd, desiredEnd);
//{
// Color c = Color.FromNonPremultiplied(new Vector4(4 * (float) (distanceSq),
// 1 - 4 * (float) (distanceSq), 0, 1));
// VRageRender.MyRenderProxy.DebugDrawLine3D(
// Vector3D.Transform(lastEnd, worldMatrix),
// Vector3D.Transform(curEnd, worldMatrix), c, c, false);
//}
// see if i'm already close enough
if (distanceSq > stopDistanceSq)
{
// create the vector to the current effector posm this is the difference vector
Vector3 curVector = curEnd - rootPos;
// create the desired effector position vector
Vector3 targetVector = desiredEnd - rootPos;
// normalize the vectors (expensive, requires a sqrt)
// MZ: we don't need to do that
// curVector.Normalize();
// targetVector.Normalize();
double curVectorLenSq = curVector.LengthSquared();
double targetVectorLenSq = targetVector.LengthSquared();
// the dot product gives me the cosine of the desired angle
// cosAngle = curVector.Dot(targetVector);
double dotCurTarget = curVector.Dot(targetVector);
// if the dot product returns 1.0, i don't need to rotate as it is 0 degrees
// MZ: yes, but when does this happen to be exactly 1???
// if (cosAngle < 1.0)
if (dotCurTarget < 0 || dotCurTarget * dotCurTarget < curVectorLenSq * targetVectorLenSq * (1 - MyMathConstants.EPSILON))
{
// use the cross product to check which way to rotate
//var rotationAxis = curVector.Cross(targetVector);
//rotationAxis.Normalize();
//turnAngle = System.Math.Acos(cosAngle); // get the angle
// get the matrix needed to rotate to the desired position
//Matrix rotation = Matrix.CreateFromAxisAngle((Vector3) rotationAxis,
// (float) turnAngle * gain);
// get the absolute matrix rotation ie - rotation including all the bones before
Matrix rotation;
float weight = 1 / (initialDistSqInv * (float)distanceSq + 1);
Vector3 weightedTarget = Vector3.Lerp(curVector, targetVector, weight);
Matrix.CreateRotationFromTwoVectors(ref curVector, ref weightedTarget, out rotation);
Matrix absoluteTransform = Matrix.Normalize(currentMatrix).GetOrientation() * rotation;
// MZ: faster
// compute just the local matrix for the bone - need to multiply with inversion ot its parent matrix and original bind transform
Matrix parentMatrix = Matrix.Identity;
if (bone.Parent != null)
{
parentMatrix = bone.Parent.AbsoluteTransform;
}
parentMatrix = Matrix.Normalize(parentMatrix); // may have different scale
Matrix localTransform = Matrix.Multiply(absoluteTransform,
Matrix.Invert(bone.BindTransform * parentMatrix));
// now change the current matrix rotation
bone.Rotation = Quaternion.CreateFromRotationMatrix(localTransform);
// and recompute the transformation
bone.ComputeAbsoluteTransform();
}
}
}
// quit if i am close enough or been running long enough
} while (tries++ < maxTries &&
Vector3D.DistanceSquared(curEnd, desiredEnd) > stopDistanceSq);
}
return(Vector3D.DistanceSquared(curEnd, desiredEnd) <= stopDistanceSq);
}
private void RecreateIgnoredBonesTableIfNeeded(MyCharacterBone[] characterBones)
{
if (m_ignoredBonesTable != null || characterBones == null)
return;
m_ignoredBonesTable = new bool[characterBones.Length];
for (int i = 0; i < characterBones.Length; i++)
{
m_ignoredBonesTable[i] = m_ignoredBoneNames.Contains(characterBones[i].Name);
}
}
/// <summary>
/// Perform inverse kinematics.
/// </summary>
public void UpdateInverseKinematics(ref MyCharacterBone[] characterBonesStorage)
{
if (Variables == null || !IkUpdateEnabled)
return;
float flying;
float falling;
float dead;
float sitting;
float jumping;
float speed;
float firstPersonCamera;
const float speedEps = 0.25f;
Variables.GetValue(MyAnimationVariableStorageHints.StrIdFlying, out flying);
Variables.GetValue(MyAnimationVariableStorageHints.StrIdFalling, out falling);
Variables.GetValue(MyAnimationVariableStorageHints.StrIdDead, out dead);
Variables.GetValue(MyAnimationVariableStorageHints.StrIdSitting, out sitting);
Variables.GetValue(MyAnimationVariableStorageHints.StrIdJumping, out jumping);
Variables.GetValue(MyAnimationVariableStorageHints.StrIdSpeed, out speed);
Variables.GetValue(MyAnimationVariableStorageHints.StrIdFirstPerson, out firstPersonCamera);
if (speed < speedEps)
{
InverseKinematics.ClearCharacterOffsetFilteringSamples();
}
InverseKinematics.SolveFeet(flying <= 0 && falling <= 0 && dead <= 0 && sitting <= 0 && jumping <= 0,
characterBonesStorage, firstPersonCamera <= 0.0f);
}
public void SetCharacterBones(MyCharacterBone[] characterBones,
Matrix[] relativeTransforms, Matrix[] abosoluteTransforms)
{
m_characterBones = characterBones;
CharacterBonesSorted = new MyCharacterBone[m_characterBones.Length];
Array.Copy(m_characterBones, CharacterBonesSorted, m_characterBones.Length);
// sort the bones, deeper in hierarchy they are, later they are evaluated
Array.Sort(CharacterBonesSorted, (x, y) => x.Depth.CompareTo(y.Depth));
m_boneRelativeTransforms = relativeTransforms;
m_boneAbsoluteTransforms = abosoluteTransforms;
Controller.ResultBonesPool.Reset(m_characterBones);
}