void CalculateHandIK(int upperarmIndex, int forearmIndex, int palmIndex, ref MatrixD targetTransform)
{
var characterBones = AnimationController.CharacterBones;
Debug.Assert(characterBones.IsValidIndex(upperarmIndex), "UpperArm index for IK is invalid");
Debug.Assert(characterBones.IsValidIndex(forearmIndex), "ForeArm index for IK is invalid");
Debug.Assert(characterBones.IsValidIndex(palmIndex), "Palm index for IK is invalid");
MatrixD invWorld = PositionComp.WorldMatrixNormalizedInv;
Matrix localFinalTransform = targetTransform * invWorld;
Vector3 finalPos = localFinalTransform.Translation;
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
VRageRender.MyRenderProxy.DebugDrawText3D(targetTransform.Translation, "Hand target transform", Color.Purple, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere(targetTransform.Translation, 0.03f, Color.Purple, 1, false);
VRageRender.MyRenderProxy.DebugDrawAxis((MatrixD)targetTransform, 0.03f, false);
}
//MyInverseKinematics.SolveCCDIk(ref finalPos, bones, 0.0005f, 5, 0.5f, ref localFinalTransform, endBone);
if (characterBones.IsValidIndex(upperarmIndex) && characterBones.IsValidIndex(forearmIndex) &&
characterBones.IsValidIndex(palmIndex))
{
MatrixD worldMatrix = PositionComp.WorldMatrix;
MyInverseKinematics.SolveTwoJointsIkCCD(characterBones,
upperarmIndex, forearmIndex, palmIndex, ref localFinalTransform, ref worldMatrix, characterBones[palmIndex], true);
}
}
void CalculateHandIK(int startBoneIndex, int endBoneIndex, ref MatrixD targetTransform)
{
MyCharacterBone endBone = AnimationController.CharacterBones[endBoneIndex];
MyCharacterBone startBone = AnimationController.CharacterBones[startBoneIndex];
// Solve IK Problem
List <MyCharacterBone> bones = new List <MyCharacterBone>();
for (int i = startBoneIndex; i <= endBoneIndex; i++)
{
bones.Add(AnimationController.CharacterBones[i]);
}
MatrixD invWorld = PositionComp.WorldMatrixNormalizedInv;
Matrix localFinalTransform = targetTransform * invWorld;
Vector3 finalPos = localFinalTransform.Translation;
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
VRageRender.MyRenderProxy.DebugDrawText3D(targetTransform.Translation, "Hand target transform", Color.Purple, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere(targetTransform.Translation, 0.03f, Color.Purple, 1, false);
VRageRender.MyRenderProxy.DebugDrawAxis((MatrixD)targetTransform, 0.03f, false);
}
Vector3 targetPosition = targetTransform.Translation;
//MyAnimationInverseKinematics.SolveIkTwoBones(AnimationController.CharacterBones, ikChainDesc, ref targetPosition,
// ref Vector3.Zero, fromBindPose: true);
MyInverseKinematics.SolveCCDIk(ref finalPos, bones, 0.0005f, 5, 0.5f, ref localFinalTransform, endBone);
//MyInverseKinematics.SolveTwoJointsIk(ref finalPos, bones[0], bones[1], bones[2], ref localFinalTransform, WorldMatrix, bones[3],false);
}
/// <summary>
/// This calculates new bone rotations for legs so the foot is placed on supported ground or object, which is found by RayCast
/// </summary>
/// <param name="hipBoneIndex">it is the index of the hip bone in m_bones list</param>
/// <param name="kneeBoneIndex">it is the index of the hip bone in m_bones list</param>
/// <param name="FeetBoneIndex">it is the index of the hip bone in m_bones list</param>
/// <param name="footPosition">this is the model space of the final foot placement, it is then recalculated to local model space</param>
void CalculateFeetPlacement(int hipBoneIndex, int kneeBoneIndex, int ankleBoneIndex, Vector3 footPosition, Vector3 footNormal, Vector3 footDimensions, bool setFootTransform)
{
Vector3 endPos = footPosition;
List <MyCharacterBone> boneTransforms = new List <MyCharacterBone>();
boneTransforms.Add(Bones[hipBoneIndex]);
boneTransforms.Add(Bones[kneeBoneIndex]);
boneTransforms.Add(Bones[ankleBoneIndex]);
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("Calculate foot transform");
Matrix finalAnkleTransform = Matrix.Identity;
if (setFootTransform)
{
// compute rotation based on normal of foot support position
Vector3 currentUp = WorldMatrix.Up;
currentUp.Normalize();
Vector3 crossResult = Vector3.Cross(currentUp, footNormal);
crossResult.Normalize();
double cosAngle = currentUp.Dot(footNormal);
cosAngle = MathHelper.Clamp(cosAngle, -1, 1);
double turnAngle = Math.Acos(cosAngle); // get the angle
Matrix rotation = Matrix.CreateFromAxisAngle((Vector3)crossResult, (float)turnAngle);
// now rotate the model world to the rotation
if (rotation.IsValid())
{
finalAnkleTransform = WorldMatrix * rotation;
}
else
{
finalAnkleTransform = WorldMatrix;
}
// but the position of this world will be different
finalAnkleTransform.Translation = Vector3.Transform(footPosition, WorldMatrix);
// compute transformation in model space
MatrixD invWorld = PositionComp.WorldMatrixNormalizedInv;
finalAnkleTransform = finalAnkleTransform * invWorld;
// get the original ankleSpace
MatrixD originalAngleTransform = Bones[ankleBoneIndex].BindTransform * Bones[ankleBoneIndex].Parent.AbsoluteTransform;
// now it needs to be related to rig transform
finalAnkleTransform = originalAngleTransform.GetOrientation() * finalAnkleTransform.GetOrientation();
}
finalAnkleTransform.Translation = footPosition;
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("IK Calculation");
MyInverseKinematics.SolveTwoJointsIk(ref endPos, Bones[hipBoneIndex], Bones[kneeBoneIndex], Bones[ankleBoneIndex], ref finalAnkleTransform, WorldMatrix, setFootTransform ? Bones[ankleBoneIndex] : null, false);
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
// draw our foot placement transformation
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_ANKLE_FINALPOS)
{
Matrix debug = finalAnkleTransform * WorldMatrix;
Matrix debug2 = Bones[ankleBoneIndex].AbsoluteTransform * WorldMatrix;
VRageRender.MyRenderProxy.DebugDrawText3D(debug.Translation, "Final ankle position", Color.Red, 1.0f, false);
VRageRender.MyRenderProxy.DebugDrawOBB(Matrix.CreateScale(footDimensions) * debug, Color.Red, 1, false, false);
VRageRender.MyRenderProxy.DebugDrawText3D(debug2.Translation, "Actual ankle position", Color.Green, 1.0f, false);
VRageRender.MyRenderProxy.DebugDrawOBB(Matrix.CreateScale(footDimensions) * debug2, Color.Green, 1, false, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(debug.Translation, debug.Translation + debug.Forward * 0.5f, Color.Yellow, Color.Yellow, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(debug.Translation, debug.Translation + debug.Up * 0.2f, Color.Purple, Color.Purple, false);
}
}
/// <summary>
/// This updates the foot placement in the world using raycasting and finding closest support.
/// </summary>
/// <param name="upDirection">This direction is used to raycast from feet - must be normalized!</param>
/// <param name="underFeetReachableDistance">How below the original position can character reach down with legs</param>
/// <param name="maxFootHeight">How high from the original position can be foot placed</param>
/// <param name="verticalChangeGainUp">How quickly we raise up the character</param>
/// <param name="verticalChangeGainDown">How quickly we crouch down</param>
/// <param name="maxDistanceSquared">This is the maximal error in foot placement</param>
/// <param name="footDimensions">This is foot dimensions, in Y axis is the ankle's height</param>
/// <param name="footPlacementDistanceSquared">This is the distance limit between calculated and current position to start IK on foot placement</param>
void UpdateFeetPlacement(Vector3 upDirection, float belowCharacterReachableDistance, float aboveCharacterReachableDistance, float verticalShiftUpGain, float verticalShiftDownGain, Vector3 footDimensions)
{
Debug.Assert(footDimensions != Vector3.Zero, "void UpdateFeetPlacement(...) : foot dimensions can not be zero!");
float ankleHeight = footDimensions.Y;
// get the current foot matrix and location
Matrix invWorld = PositionComp.WorldMatrixInvScaled;
MyCharacterBone rootBone = Bones[m_rootBone]; // root bone is used to transpose the model up or down
Matrix modelRootBoneMatrix = rootBone.AbsoluteTransform;
float verticalShift = modelRootBoneMatrix.Translation.Y;
Matrix leftFootMatrix = Bones[m_leftAnkleBone].AbsoluteTransform;
Matrix rightFootMatrix = Bones[m_rightAnkleBone].AbsoluteTransform;
// ok first we get the closest support to feet and we need to know from where to raycast for each foot in world coords
// we need to raycast from original ground position of the feet, no from the character shifted position
// we cast from the ground of the model space, assuming the model's local space up vector is in Y axis
Vector3 leftFootGroundPosition = new Vector3(leftFootMatrix.Translation.X, 0, leftFootMatrix.Translation.Z);
Vector3 rightFootGroundPosition = new Vector3(rightFootMatrix.Translation.X, 0, rightFootMatrix.Translation.Z);
Vector3 fromL = Vector3.Transform(leftFootGroundPosition, WorldMatrix); // we get this position in the world
Vector3 fromR = Vector3.Transform(rightFootGroundPosition, WorldMatrix);
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("GetClosestFootPosition");
// find the closest ground support, raycasting from Up to Down
var contactLeft = MyInverseKinematics.GetClosestFootSupportPosition(this, null, fromL, upDirection, footDimensions, WorldMatrix, belowCharacterReachableDistance, aboveCharacterReachableDistance, Physics.CharacterCollisionFilter); // this returns world coordinates of support for left foot
var contactRight = MyInverseKinematics.GetClosestFootSupportPosition(this, null, fromR, upDirection, footDimensions, WorldMatrix, belowCharacterReachableDistance, aboveCharacterReachableDistance, Physics.CharacterCollisionFilter);
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("Characters root shift estimation");
// if we got hit only for one feet, we do nothing, but slowly return back root bone (character vertical shift) position if it was changed from original
// that happends very likely when the support below is too far for one leg
if (contactLeft == null || contactRight == null)
{
modelRootBoneMatrix.Translation -= modelRootBoneMatrix.Translation * verticalShiftUpGain;
rootBone.SetBindTransform(modelRootBoneMatrix);
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
return;
}
// Here we recalculate if we shift the root of the character to reach bottom or top
// get the desired foot world coords
Vector3 supportL = contactLeft.Value.Position;
Vector3 supportR = contactRight.Value.Position;
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_CLOSESTSUPPORTPOSITION)
{
VRageRender.MyRenderProxy.DebugDrawText3D(supportL, "Foot support position", Color.Blue, 1, false);
VRageRender.MyRenderProxy.DebugDrawText3D(supportR, "Foot support position", Color.Blue, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere(supportL, 0.03f, Color.Blue, 0, false);
VRageRender.MyRenderProxy.DebugDrawSphere(supportR, 0.03f, Color.Blue, 0, false);
}
// Get the vector between actual feet position and possible support in local model coords
// local model space coord of desired position + shift it up of ankle heights
Vector3 leftAnkleDesiredPosition = Vector3.Transform(supportL, invWorld) + ((leftFootMatrix.Translation.Y - verticalShift) * upDirection);
Vector3 rightAnkleDesiredPosition = Vector3.Transform(supportR, invWorld) + ((rightFootMatrix.Translation.Y - verticalShift) * upDirection);
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_ANKLE_DESIREDPOSITION)
{
VRageRender.MyRenderProxy.DebugDrawText3D(supportL, "Ankle desired position", Color.Purple, 1, false);
VRageRender.MyRenderProxy.DebugDrawText3D(supportR, "Ankle desired position", Color.Purple, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere(Vector3.Transform(leftAnkleDesiredPosition, WorldMatrix), 0.03f, Color.Purple, 0, false);
VRageRender.MyRenderProxy.DebugDrawSphere(Vector3.Transform(rightAnkleDesiredPosition, WorldMatrix), 0.03f, Color.Purple, 0, false);
}
// Get the height of found support related to character's position in model's local space, assuming it's Y axis
float leftAnkleDesiredHeight = leftAnkleDesiredPosition.Y;
float rightAnkleDesiredHeight = rightAnkleDesiredPosition.Y;
// if we the distances are too big, so we will not be able to set the position, we can skip it
if (Math.Abs(leftAnkleDesiredHeight - rightAnkleDesiredHeight) > aboveCharacterReachableDistance)
{
modelRootBoneMatrix.Translation -= modelRootBoneMatrix.Translation * verticalShiftUpGain;
rootBone.SetBindTransform(modelRootBoneMatrix);
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
return;
}
// if we got one of the supports below the character root, we must check whether we can reach it, if yes, we need to crouch to reach it
if ((((leftAnkleDesiredHeight > -belowCharacterReachableDistance) && (leftAnkleDesiredHeight < ankleHeight)) || // left support is below model and is reachable
((rightAnkleDesiredHeight > -belowCharacterReachableDistance) && (rightAnkleDesiredHeight < ankleHeight))) && // right support is below model and is reachable
// finally check if character is shifted down, the other feet won't get too high
(Math.Max(leftAnkleDesiredHeight, rightAnkleDesiredHeight) - Math.Min(leftAnkleDesiredHeight, rightAnkleDesiredHeight) < aboveCharacterReachableDistance))
{
// then we can try to reach down according to the difference
float distanceBelow = Math.Min(leftAnkleDesiredHeight, rightAnkleDesiredHeight) - ankleHeight;
Vector3 verticalTranslation = upDirection * distanceBelow;
Vector3 translation = modelRootBoneMatrix.Translation;
translation.Interpolate3(modelRootBoneMatrix.Translation, verticalTranslation, verticalShiftDownGain);
modelRootBoneMatrix.Translation = translation;
rootBone.SetBindTransform(modelRootBoneMatrix);
}
else // if both supports are up, we need to get up, however, that should be done by rigid body as well.. we limit it only by reachable distance, so it is bounded to rigid body position
if ((leftAnkleDesiredHeight > ankleHeight) && (leftAnkleDesiredHeight < aboveCharacterReachableDistance) &&
(rightAnkleDesiredHeight > ankleHeight) && (rightAnkleDesiredHeight < aboveCharacterReachableDistance))
{
// move up to reach the highest support
float distanceAbove = Math.Max(leftAnkleDesiredHeight, rightAnkleDesiredHeight) - ankleHeight;
Vector3 verticalTranslation = upDirection * distanceAbove;
Vector3 translation = modelRootBoneMatrix.Translation;
translation.Interpolate3(modelRootBoneMatrix.Translation, verticalTranslation, verticalShiftUpGain);
modelRootBoneMatrix.Translation = translation;
rootBone.SetBindTransform(modelRootBoneMatrix);
}
// finally if we can not get into right vertical position for foot placement, slowly reset the vertical shift
else
{
modelRootBoneMatrix.Translation -= modelRootBoneMatrix.Translation * verticalShiftUpGain;
rootBone.SetBindTransform(modelRootBoneMatrix);
}
// Hard limit to root's shift in vertical position
//if (characterVerticalShift < -underFeetReachableDistance)
//{
// modelRootBoneMatrix.Translation = -upDirection * underFeetReachableDistance;
// rootBone.SetBindTransform(modelRootBoneMatrix);
// characterVerticalShift = -underFeetReachableDistance; // get the new height
//}
//if (characterVerticalShift > underFeetReachableDistance)
//{
// modelRootBoneMatrix.Translation = upDirection * underFeetReachableDistance;
// rootBone.SetBindTransform(modelRootBoneMatrix);
// characterVerticalShift = underFeetReachableDistance; // get the new height
//}
// Then we need to recalculate all other bones matrices so we get proper data for children, since we changed the root position
//foreach (var b in m_bones) b.ComputeAbsoluteTransform();
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("CalculateFeetPlacement");
// Then recalculate feet positions only if we can reach the final and if we are in the limits
if ((-belowCharacterReachableDistance < leftAnkleDesiredHeight) && (leftAnkleDesiredHeight < aboveCharacterReachableDistance)) // and the found foot support height is not over limit
{
CalculateFeetPlacement(
m_leftHipBone,
m_leftKneeBone,
m_leftAnkleBone,
leftAnkleDesiredPosition,
contactLeft.Value.Normal,
footDimensions,
leftFootMatrix.Translation.Y - verticalShift <= ankleHeight);
}
if ((-belowCharacterReachableDistance < rightAnkleDesiredHeight) && (rightAnkleDesiredHeight < aboveCharacterReachableDistance))
{
CalculateFeetPlacement(
m_rightHipBone,
m_rightKneeBone,
m_rightAnkleBone,
rightAnkleDesiredPosition,
contactRight.Value.Normal,
footDimensions,
rightFootMatrix.Translation.Y - verticalShift <= ankleHeight);
}
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_BONES)
{
List <Matrix> left = new List <Matrix> {
Bones[m_leftHipBone].AbsoluteTransform, Bones[m_leftKneeBone].AbsoluteTransform, Bones[m_leftAnkleBone].AbsoluteTransform
};
List <Matrix> right = new List <Matrix> {
Bones[m_rightHipBone].AbsoluteTransform, Bones[m_rightKneeBone].AbsoluteTransform, Bones[m_rightAnkleBone].AbsoluteTransform
};
debugDrawBones(left);
debugDrawBones(right);
VRageRender.MyRenderProxy.DebugDrawText3D(WorldMatrix.Translation, "Rigid body", Color.Yellow, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere(WorldMatrix.Translation, 0.05f, Color.Yellow, 0, false);
VRageRender.MyRenderProxy.DebugDrawText3D((modelRootBoneMatrix * WorldMatrix).Translation, "Character root bone", Color.Yellow, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere((modelRootBoneMatrix * WorldMatrix).Translation, 0.07f, Color.Red, 0, false);
}
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("Storing bones transforms");
// After the feet placement we need to save new bone transformations
for (int i = 0; i < Bones.Count; i++)
{
MyCharacterBone bone = Bones[i];
BoneRelativeTransforms[i] = bone.ComputeBoneTransform();
}
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
}