private void SetBoneTo(RagdollBone ragdollBone, float weight, float dynamicChildrenWeight, float keyframedChildrenWeight, bool translationEnabled)
{
if ((this.Ragdoll != null) && (this.m_inicialized && this.IsActive))
{
int index = this.m_rigidBodiesToBonesIndices[ragdollBone.m_rigidBodyIndex][0];
MyCharacterBone bone = this.m_bones[index];
Matrix matrix = (bone.Parent != null) ? bone.Parent.AbsoluteTransform : Matrix.Identity;
Matrix matrix3 = (this.m_bodyToBoneRigTransforms[ragdollBone.m_rigidBodyIndex] * this.Ragdoll.GetRigidBodyLocalTransform(ragdollBone.m_rigidBodyIndex)) * Matrix.Invert(bone.BindTransform * matrix);
if (!this.m_animationBlendingHelper.Initialized)
{
this.m_animationBlendingHelper.Init(this.m_bones, this.m_character.AnimationController.Controller);
}
this.m_animationBlendingHelper.BlendWeight(ref weight, bone, this.m_character.AnimationController.Variables);
weight *= MyFakes.RAGDOLL_ANIMATION_WEIGHTING;
float single1 = MathHelper.Clamp(weight, 0f, 1f);
weight = single1;
if (matrix3.IsValid() && (matrix3 != Matrix.Zero))
{
if (weight == 1f)
{
bone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize(matrix3.GetOrientation()));
if (translationEnabled)
{
bone.Translation = matrix3.Translation;
}
}
else
{
bone.Rotation = Quaternion.Slerp(bone.Rotation, Quaternion.CreateFromRotationMatrix(Matrix.Normalize(matrix3.GetOrientation())), weight);
if (translationEnabled)
{
bone.Translation = Vector3.Lerp(bone.Translation, matrix3.Translation, weight);
}
}
}
bone.ComputeAbsoluteTransform(true);
foreach (RagdollBone bone2 in ragdollBone.m_children)
{
float num2 = dynamicChildrenWeight;
if (this.m_keyframedBodies.Contains(bone2.m_rigidBodyIndex))
{
num2 = keyframedChildrenWeight;
}
if (this.IsPartiallySimulated)
{
this.SetBoneTo(bone2, num2, dynamicChildrenWeight, keyframedChildrenWeight, false);
}
else
{
this.SetBoneTo(bone2, num2, dynamicChildrenWeight, keyframedChildrenWeight, !this.Ragdoll.IsRigidBodyPalmOrFoot(bone2.m_rigidBodyIndex) && MyFakes.ENABLE_RAGDOLL_BONES_TRANSLATION);
}
}
}
}
private void SetBoneTo(RagdollBone ragdollBone, float weight, float dynamicChildrenWeight, float keyframedChildrenWeight)
{
int firstBoneIndex = m_rigidBodiesToBonesIndices[ragdollBone.m_rigidBodyIndex].First();
MyCharacterBone bone = m_bones[firstBoneIndex];
//Matrix localTransform = m_ragdoll.GetRigidBodyLocalTransform(ragdollBone.m_rigidBodyIndex);
Matrix localTransform = m_bodyToBoneRigTransforms[ragdollBone.m_rigidBodyIndex] * m_ragdoll.GetRigidBodyLocalTransform(ragdollBone.m_rigidBodyIndex);
Matrix parentMatrix = (bone.Parent != null) ? bone.Parent.AbsoluteTransform : Matrix.Identity;
Matrix absoluteMatrixInverted = Matrix.Invert(bone.BindTransform * parentMatrix);
Matrix finalTransform = localTransform * absoluteMatrixInverted;
//finalTransform = rigidBodyToBoneTransform * finalTransform;
if (weight == 1.0f)
{
bone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)finalTransform.GetOrientation()));
// NOTE: If enabled, sometimes ragdoll bodies got extra translation which leads to disproporced transfomations on limbs, therefore disabled on all bodies except the firs one
if (MyFakes.ENABLE_RAGDOLL_BONES_TRANSLATION)
{
bone.Translation = finalTransform.Translation;
}
}
else
{
bone.Rotation = Quaternion.Slerp(bone.Rotation, Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)finalTransform.GetOrientation())), weight);
// NOTE: If enabled, sometimes ragdoll bodies got extra translation which leads to disproporced transfomations on limbs, therefore disabled
if (MyFakes.ENABLE_RAGDOLL_BONES_TRANSLATION)
{
bone.Translation = Vector3.Lerp(bone.Translation, finalTransform.Translation, weight);
}
}
bone.ComputeAbsoluteTransform();
foreach (var childBone in ragdollBone.m_children)
{
float childWeight = dynamicChildrenWeight;
if (m_keyframedBodies.Contains(childBone.m_rigidBodyIndex))
{
childWeight = keyframedChildrenWeight;
}
SetBoneTo(childBone, childWeight, dynamicChildrenWeight, keyframedChildrenWeight);
}
}
/// <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 = Character.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(Character, null, fromL, upDirection, footDimensions, WorldMatrix, belowCharacterReachableDistance, aboveCharacterReachableDistance, Character.Physics.CharacterCollisionFilter); // this returns world coordinates of support for left foot
var contactRight = MyInverseKinematics.GetClosestFootSupportPosition(Character, null, fromR, upDirection, footDimensions, WorldMatrix, belowCharacterReachableDistance, aboveCharacterReachableDistance, Character.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)
{
rootBone.Translation = modelRootBoneMatrix.Translation -= modelRootBoneMatrix.Translation * verticalShiftUpGain;
rootBone.ComputeAbsoluteTransform();
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)
{
rootBone.Translation = modelRootBoneMatrix.Translation -= modelRootBoneMatrix.Translation * verticalShiftUpGain;
rootBone.ComputeAbsoluteTransform();
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 = Vector3.Zero;
translation.Interpolate3(modelRootBoneMatrix.Translation, verticalTranslation, verticalShiftDownGain);
rootBone.Translation = modelRootBoneMatrix.Translation = translation;
rootBone.ComputeAbsoluteTransform();
}
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 = Vector3.Zero;
translation.Interpolate3(modelRootBoneMatrix.Translation, verticalTranslation, verticalShiftUpGain);
rootBone.Translation = modelRootBoneMatrix.Translation = translation;
rootBone.ComputeAbsoluteTransform();
}
// 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.Translation = modelRootBoneMatrix.Translation;
rootBone.ComputeAbsoluteTransform();
}
// 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);
}
}
/// <summary>
/// Analytic solutions useful fo hands or feet, all in local model space
/// </summary>
/// <param name="desiredEnd">in local model space</param>
/// <param name="firstBone"></param>
/// <param name="secondBone"></param>
/// <param name="finalTransform"></param>
/// <param name="finalBone"></param>
/// <param name="allowFinalBoneTranslation"></param>
/// <returns></returns>
public static bool SolveTwoJointsIk(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
{
Matrix firstBoneAbsoluteTransform = firstBone.AbsoluteTransform;
Matrix secondBoneAbsoluteTransform = secondBone.AbsoluteTransform;
Matrix endBoneAbsoluteTransform = endBone.AbsoluteTransform;
Vector3 origin = firstBoneAbsoluteTransform.Translation;
Vector3 originToCurrentEnd = endBoneAbsoluteTransform.Translation - origin;
Vector3 originToDesiredEnd = desiredEnd - origin;
Vector3 firstBoneVector = secondBoneAbsoluteTransform.Translation - origin;
Vector3 secondBoneVector = originToCurrentEnd - firstBoneVector;
float firstBoneLength = firstBoneVector.Length();
float secondBoneLength = secondBoneVector.Length();
float originToDesiredEndLength = originToDesiredEnd.Length();
float originToCurrentEndLength = originToCurrentEnd.Length();
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
VRageRender.MyRenderProxy.DebugDrawSphere(Vector3.Transform(desiredEnd, WorldMatrix), 0.01f, Color.Red, 1, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToCurrentEnd, WorldMatrix), Color.Yellow, Color.Yellow, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToDesiredEnd, WorldMatrix), Color.Red, Color.Red, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + firstBoneVector, WorldMatrix), Color.Green, Color.Green, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + firstBoneVector, WorldMatrix), Vector3.Transform(origin + firstBoneVector + secondBoneVector, WorldMatrix), Color.Blue, Color.Blue, false);
}
// only two cases, the desired position is reachable or not
bool isDesiredEndReachable = firstBoneLength + secondBoneLength > originToDesiredEndLength;
// alpha = angle between the first bone and originToDesiredEnd vector
double finalAlpha = 0;
// beta = the angle between the first and second bone
double finalBeta = 0;
if (isDesiredEndReachable)
{ // we find proper angles
// cosine law c^2 = a^2 + b^2 - 2*a*b*cos(gamma)
// gamma = acos ( - (c^2 - a^2 - b^2) / (2*a*b) )
// alpha = angle between the first bone and originToDesiredEnd vector
double cosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToDesiredEndLength * originToDesiredEndLength) /
(2 * firstBoneLength * originToDesiredEndLength);
cosAlpha = MathHelper.Clamp(cosAlpha, -1, 1);
finalAlpha = Math.Acos(cosAlpha);
// beta = the angle between the first and second bone
double cosBeta = -(originToDesiredEndLength * originToDesiredEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
(2 * firstBoneLength * secondBoneLength);
cosBeta = MathHelper.Clamp(cosBeta, -1, 1);
finalBeta = Math.Acos(cosBeta);
// now get it to the root bone axis no
finalBeta = Math.PI - finalBeta;
}
// get the current angles
double cCosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToCurrentEndLength * originToCurrentEndLength) /
(2 * firstBoneLength * originToCurrentEndLength);
cCosAlpha = MathHelper.Clamp(cCosAlpha, -1, 1);
double currentAlpha = Math.Acos(cCosAlpha);
double cCosBeta = -(originToCurrentEndLength * originToCurrentEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
(2 * firstBoneLength * secondBoneLength);
cCosBeta = MathHelper.Clamp(cCosBeta, -1, 1);
double currentBeta = Math.Acos(cCosBeta);
currentBeta = Math.PI - currentBeta;
Vector3 currentPlaneNormal = Vector3.Cross(firstBoneVector, originToCurrentEnd);
currentPlaneNormal.Normalize();
// we can now rotate the bones in current plane as if the desired end was on the currentEnd axis
float alphaDif = (float)(finalAlpha - currentAlpha);
float betaDif = (float)(finalBeta - currentBeta);
Matrix firstBoneRotation = Matrix.CreateFromAxisAngle(-currentPlaneNormal, alphaDif);
Matrix secondBoneRotation = Matrix.CreateFromAxisAngle(currentPlaneNormal, betaDif);
// now get the angle between original and final position plane normal
originToCurrentEnd.Normalize();
originToDesiredEnd.Normalize();
double dotProd = originToCurrentEnd.Dot(originToDesiredEnd);
dotProd = MathHelper.Clamp(dotProd, -1, 1);
double delta = Math.Acos(dotProd);
Vector3 planeRotationAxis = Vector3.Cross(originToCurrentEnd, originToDesiredEnd);
planeRotationAxis.Normalize();
// find the rotation matrices for bones in the original plane
Matrix planeRotation = Matrix.CreateFromAxisAngle(planeRotationAxis, (float)delta);
// compute the final rotations
firstBoneRotation = planeRotation * firstBoneRotation;
secondBoneRotation = secondBoneRotation * firstBoneRotation;
// draw the final positions if debug enabled
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
Vector3 rotatedFirst = Vector3.Transform(firstBoneVector, firstBoneRotation);
Vector3 rotatedSecond = Vector3.Transform(secondBoneVector, secondBoneRotation);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + rotatedFirst, WorldMatrix), Color.Purple, Color.Purple, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + rotatedFirst, WorldMatrix), Vector3.Transform(origin + rotatedFirst + rotatedSecond, WorldMatrix), Color.White, Color.White, false);
}
// Now we compute the final absolute transforms for the bones
Matrix firstBoneFinalAbsoluteTransform = firstBoneAbsoluteTransform * firstBoneRotation;
Matrix firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
Matrix localFirstBoneTransform = Matrix.Multiply(firstBoneFinalAbsoluteTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
firstBone.ComputeAbsoluteTransform();
Matrix secondBoneFinalAbsoluteTransform = secondBoneAbsoluteTransform * secondBoneRotation;
Matrix secondBoneParentAbsoluteTransform = secondBone.Parent.AbsoluteTransform;
Matrix localSecondBoneTransform = Matrix.Multiply(secondBoneFinalAbsoluteTransform, Matrix.Invert(secondBone.BindTransform * secondBoneParentAbsoluteTransform));
secondBone.Rotation = Quaternion.CreateFromRotationMatrix(localSecondBoneTransform);
secondBone.ComputeAbsoluteTransform();
// solve the last bone
if (finalBone != null && finalTransform.IsValid() && isDesiredEndReachable)
{
//MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation)
// get the related transformation to original binding pose
MatrixD localTransformRelated;
if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
//localTransformRelated = Matrix.Normalize(localTransformRelated);
// from there get the rotation and translation
finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation;
finalBone.ComputeAbsoluteTransform();
}
return isDesiredEndReachable;
}
public static void RotateBone(MyCharacterBone bone, Vector3 planeNormal, double angle)
{
Matrix rotation = Matrix.CreateFromAxisAngle(planeNormal, (float)angle);
Matrix finalMatrix = bone.AbsoluteTransform * rotation;
Matrix parentTransform = bone.Parent != null ? bone.Parent.AbsoluteTransform : Matrix.Identity;
Matrix localTransform = Matrix.Multiply(finalMatrix, Matrix.Invert(bone.BindTransform * parentTransform));
bone.Rotation = Quaternion.CreateFromRotationMatrix(localTransform);
bone.ComputeAbsoluteTransform();
}
public static bool SolveTwoJointsIkCCD(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
{
Vector3D rootPos, curEnd, targetVector, curVector, crossResult;
double cosAngle, turnAngle;
List<MyCharacterBone> bones = new List<MyCharacterBone>();
bones.Add(firstBone);
bones.Add(secondBone);
bones.Add(endBone);
int tries = 0;
int maxTries = 50;
float stopDistance = 0.00001f;
float gain = 0.6f;
curEnd = Vector3.Zero;
do
{
foreach (MyCharacterBone bone in bones.Reverse<MyCharacterBone>())
{
// first recalculate current final transformation
endBone.ComputeAbsoluteTransform();
// compute the position of the root
Matrix currentMatrix = bone.AbsoluteTransform;
rootPos = (Vector3D)currentMatrix.Translation; // this is this bone root position
curEnd = (Vector3D)endBone.AbsoluteTransform.Translation; // this is our current end of the final bone
// get the difference from desired and and current final position
double distance = Vector3D.DistanceSquared(curEnd, desiredEnd);
// see if i'm already close enough
if (distance > stopDistance)
{
// create the vector to the current effector posm this is the difference vector
curVector = curEnd - rootPos;
// create the desired effector position vector
targetVector = desiredEnd - rootPos;
// normalize the vectors (expensive, requires a sqrt)
curVector.Normalize();
targetVector.Normalize();
// the dot product gives me the cosine of the desired angle
cosAngle = curVector.Dot(targetVector);
// if the dot product returns 1.0, i don't need to rotate as it is 0 degrees
if (cosAngle < 1.0)
{
// use the cross product to check which way to rotate
crossResult = curVector.Cross(targetVector);
crossResult.Normalize();
turnAngle = System.Math.Acos(cosAngle); // get the angle
// get the matrix needed to rotate to the desired position
Matrix rotation = Matrix.CreateFromAxisAngle((Vector3)crossResult, (float)turnAngle * gain);
// get the absolute matrix rotation ie - rotation including all the bones before
Matrix absoluteTransform = Matrix.Normalize(currentMatrix).GetOrientation() * rotation;
// 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) > stopDistance);
// solve the last bone
if (finalBone != null && finalTransform.IsValid())
{
//MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation)
// get the related transformation to original binding posefirstBoneAbsoluteTransform
MatrixD localTransformRelated;
if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
//localTransformRelated = Matrix.Normalize(localTransformRelated);
// from there get the rotation and translation
finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation;
finalBone.ComputeAbsoluteTransform();
}
return Vector3D.DistanceSquared(curEnd, desiredEnd) <= stopDistance;
}
private void SetBoneTo(RagdollBone ragdollBone, float weight, float dynamicChildrenWeight, float keyframedChildrenWeight, bool translationEnabled)
{
if (Ragdoll == null)
{
return;
}
if (!m_inicialized || !IsActive)
{
return;
}
int firstBoneIndex = m_rigidBodiesToBonesIndices[ragdollBone.m_rigidBodyIndex].First();
MyCharacterBone bone = m_bones[firstBoneIndex];
//Matrix localTransform = Ragdoll.GetRigidBodyLocalTransform(ragdollBone.m_rigidBodyIndex);
Matrix localTransform = m_bodyToBoneRigTransforms[ragdollBone.m_rigidBodyIndex] * Ragdoll.GetRigidBodyLocalTransform(ragdollBone.m_rigidBodyIndex);
Matrix parentMatrix = (bone.Parent != null) ? bone.Parent.AbsoluteTransform : Matrix.Identity;
Matrix absoluteMatrixInverted = Matrix.Invert(bone.BindTransform * parentMatrix);
Matrix finalTransform = localTransform * absoluteMatrixInverted;
//finalTransform = rigidBodyToBoneTransform * finalTransform;
Debug.Assert(finalTransform.IsValid() && finalTransform != Matrix.Zero, "Ragdoll - final bone transform is invalid!");
if (finalTransform.IsValid() && finalTransform != Matrix.Zero)
{
if (weight == 1.0f)
{
bone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)finalTransform.GetOrientation()));
// NOTE: If enabled, sometimes ragdoll bodies got extra translation which leads to disproporced transfomations on limbs, therefore disabled on all bodies except the firs one
if (translationEnabled)// || m_character.IsDead)
{
bone.Translation = finalTransform.Translation;
}
}
else
{
bone.Rotation = Quaternion.Slerp(bone.Rotation, Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)finalTransform.GetOrientation())), weight);
// NOTE: If enabled, sometimes ragdoll bodies got extra translation which leads to disproporced transfomations on limbs, therefore disabled
if (translationEnabled)// || m_character.IsDead)
{
bone.Translation = Vector3.Lerp(bone.Translation, finalTransform.Translation, weight);
}
}
}
bone.ComputeAbsoluteTransform();
foreach (var childBone in ragdollBone.m_children)
{
float childWeight = dynamicChildrenWeight;
if (m_keyframedBodies.Contains(childBone.m_rigidBodyIndex))
{
childWeight = keyframedChildrenWeight;
}
//SetBoneTo(childBone, childWeight, dynamicChildrenWeight, keyframedChildrenWeight, MyFakes.ENABLE_RAGDOLL_BONES_TRANSLATION);
if (IsPartiallySimulated)
{
SetBoneTo(childBone, childWeight, dynamicChildrenWeight, keyframedChildrenWeight, false);
}
else
{
SetBoneTo(childBone, childWeight, dynamicChildrenWeight, keyframedChildrenWeight, (Ragdoll.IsRigidBodyPalmOrFoot(childBone.m_rigidBodyIndex)) ? false : MyFakes.ENABLE_RAGDOLL_BONES_TRANSLATION);
}
}
}
public static bool SolveTwoJointsIkCCD(MyCharacterBone[] characterBones, int firstBoneIndex, int secondBoneIndex, int endBoneIndex,
ref Matrix finalTransform, ref MatrixD worldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
{
if (finalBone == null)
return false;
Vector3 desiredEnd = finalTransform.Translation;
//VRageRender.MyRenderProxy.DebugDrawSphere(Vector3D.Transform(desiredEnd, worldMatrix), 0.015f, Color.LightGoldenrodYellow, 1, false);
Vector3 rootPos, curEnd;
Vector3 curVector;
double cosAngle, turnAngle;
int tries = 0;
int maxTries = 50;
float stopDistanceSq = 0.005f * 0.005f;
float gain = 0.65f;
MyCharacterBone firstBone = characterBones[firstBoneIndex];
MyCharacterBone secondBone = characterBones[secondBoneIndex];
MyCharacterBone endBone = characterBones[endBoneIndex];
//unsafe
{
//int* boneIndices = stackalloc int[3];
int[] boneIndices = new int[3];
boneIndices[2] = firstBoneIndex;
boneIndices[1] = secondBoneIndex;
boneIndices[0] = endBoneIndex;
curEnd = Vector3.Zero;
for (int i = 0; i < 3; i++)
{
var bone = characterBones[boneIndices[i]];
Vector3 tempTranslation = bone.BindTransform.Translation;
Quaternion tempRotation = Quaternion.CreateFromRotationMatrix(bone.BindTransform);
bone.SetCompleteTransform(ref tempTranslation, ref tempRotation);
bone.ComputeAbsoluteTransform();
}
endBone.ComputeAbsoluteTransform();
curEnd = endBone.AbsoluteTransform.Translation;
float initialDistSqInv = 1 / (float)Vector3D.DistanceSquared(curEnd, desiredEnd);
do
{
for (int i = 0; i < 3; i++)
{
var bone = characterBones[boneIndices[i]];
// first recalculate current final transformation
endBone.ComputeAbsoluteTransform();
// compute the position of the root
Matrix currentMatrix = bone.AbsoluteTransform;
rootPos = currentMatrix.Translation; // this is this bone root position
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
curVector = curEnd - rootPos;
// create the desired effector position vector
var 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);
}
// solve the last bone
if (finalTransform.IsValid())
{
// get the related transformation to original binding posefirstBoneAbsoluteTransform
MatrixD localTransformRelated;
if (allowFinalBoneTranslation)
localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
else
localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
// localTransformRelated = Matrix.Normalize(localTransformRelated);
// from there get the rotation and translation
finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
if (allowFinalBoneTranslation)
finalBone.Translation = (Vector3)localTransformRelated.Translation;
finalBone.ComputeAbsoluteTransform();
}
return true;//Vector3D.DistanceSquared(curEnd, desiredEnd) <= stopDistanceSq;
}
public override bool DebugDraw()
{
if (MyDebugDrawSettings.DEBUG_DRAW_SHOW_DAMAGE)
{
m_counter++;
if (m_character.CharacterAccumulatedDamage != m_lastDamage)
{
m_counter = 0;
}
VRageRender.MyRenderProxy.DebugDrawText3D(((MyEntity)m_character).WorldMatrix.Translation + ((MyEntity)m_character).WorldMatrix.Up, "Total damage:" + m_lastDamage + " velocity:" + m_lastCharacterVelocity, Color.Red, 1.5f, false, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
if (m_counter > 200)
{
m_character.CharacterAccumulatedDamage = 0;
m_lastCharacterVelocity = 0;
m_counter = 0;
}
m_lastDamage = m_character.CharacterAccumulatedDamage;
m_lastCharacterVelocity = Math.Max(m_lastCharacterVelocity, m_character.Physics.LinearVelocity.Length());
}
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_MISC && m_character.CurrentWeapon != null)
{
VRageRender.MyRenderProxy.DebugDrawAxis(((MyEntity)m_character.CurrentWeapon).WorldMatrix, 1.4f, false);
VRageRender.MyRenderProxy.DebugDrawText3D(((MyEntity)m_character.CurrentWeapon).WorldMatrix.Translation, "Weapon", Color.White, 0.7f, false, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
VRageRender.MyRenderProxy.DebugDrawSphere((m_character.Bones[m_character.WeaponBone].AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation, 0.02f, Color.White, 1, false);
VRageRender.MyRenderProxy.DebugDrawText3D((m_character.Bones[m_character.WeaponBone].AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation, "Weapon Bone", Color.White, 1f, false);
}
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_MISC && m_character.IsUsing != null)
{
Matrix characterMatrix = m_character.IsUsing.WorldMatrix;
characterMatrix.Translation = Vector3.Zero;
characterMatrix = characterMatrix * Matrix.CreateFromAxisAngle(characterMatrix.Up, MathHelper.Pi);
Vector3 position = m_character.IsUsing.PositionComp.GetPosition() - m_character.IsUsing.WorldMatrix.Up * MyDefinitionManager.Static.GetCubeSize(MyCubeSize.Large) / 2.0f;
position = position + characterMatrix.Up * 0.28f - characterMatrix.Forward * 0.22f;
characterMatrix.Translation = position;
VRageRender.MyRenderProxy.DebugDrawAxis(characterMatrix, 1.4f, false);
}
if (MyDebugDrawSettings.DEBUG_DRAW_SUIT_BATTERY_CAPACITY)
{
var world = m_character.PositionComp.WorldMatrix;
VRageRender.MyRenderProxy.DebugDrawText3D(world.Translation + 2f * world.Up, string.Format("{0} MWh", m_character.SuitBattery.ResourceSource.RemainingCapacity),
Color.White, 1f, true, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
}
m_simulatedBonesDebugDraw.Clear();
m_simulatedBonesAbsoluteDebugDraw.Clear();
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_BONES)
{
for (int s = 0; s < m_character.Bones.Count; s++)
{
MyCharacterBone bone2 = m_character.Bones[s];
if (bone2.Parent == null)
{
continue;
}
bone2.ComputeAbsoluteTransform();
var p2m = Matrix.CreateScale(0.1f) * bone2.AbsoluteTransform * m_character.PositionComp.WorldMatrix;
Vector3 p2 = p2m.Translation;
MyCharacterBone bone1 = bone2.Parent;
//bone1.Rotation = Quaternion.Identity;
//bone1.Translation = Vector3.Zero;
// bone1.ComputeAbsoluteTransform();
Vector3 p1 = (bone1.AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation;
VRageRender.MyRenderProxy.DebugDrawLine3D(p1, p2, Color.White, Color.White, false);
Vector3 pCenter = (p1 + p2) * 0.5f;
VRageRender.MyRenderProxy.DebugDrawText3D(pCenter, bone2.Name + " (" + s.ToString() + ")", Color.White, 0.5f, false);
VRageRender.MyRenderProxy.DebugDrawAxis(p2m, 0.1f, false);
}
}
return(true);
}
