public static bool SolveTwoJointsIkCCD(MyCharacterBone[] characterBones, int firstBoneIndex, int secondBoneIndex, int endBoneIndex, ref Matrix finalTransform, ref MatrixD worldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
{
Matrix bindTransform;
if (finalBone == null)
{
return(false);
}
Vector3 translation = finalTransform.Translation;
int num = 0;
int num2 = 50;
float num3 = 2.5E-05f;
MyCharacterBone bone1 = characterBones[firstBoneIndex];
MyCharacterBone bone3 = characterBones[secondBoneIndex];
MyCharacterBone bone = characterBones[endBoneIndex];
int[] numArray = new int[] { endBoneIndex };
numArray[2] = firstBoneIndex;
numArray[1] = secondBoneIndex;
Vector3 zero = Vector3.Zero;
for (int i = 0; i < 3; i++)
{
MyCharacterBone bone4 = characterBones[numArray[i]];
bindTransform = bone4.BindTransform;
Vector3 vector5 = bindTransform.Translation;
Quaternion rotation = Quaternion.CreateFromRotationMatrix(bone4.BindTransform);
bone4.SetCompleteTransform(ref vector5, ref rotation);
bone4.ComputeAbsoluteTransform(true);
}
bone.ComputeAbsoluteTransform(true);
zero = bone.AbsoluteTransform.Translation;
float num4 = 1f / ((float)Vector3D.DistanceSquared(zero, translation));
while (true)
{
int index = 0;
while (true)
{
if (index >= 3)
{
num++;
if ((num < num2) && (Vector3D.DistanceSquared(zero, translation) > num3))
{
break;
}
if (finalTransform.IsValid())
{
MatrixD xd = !allowFinalBoneTranslation ? (finalTransform.GetOrientation() * MatrixD.Invert(finalBone.BindTransform * finalBone.Parent.AbsoluteTransform)) : (finalTransform * MatrixD.Invert(finalBone.BindTransform * finalBone.Parent.AbsoluteTransform));
finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)xd.GetOrientation()));
if (allowFinalBoneTranslation)
{
finalBone.Translation = (Vector3)xd.Translation;
}
finalBone.ComputeAbsoluteTransform(true);
}
return(true);
}
MyCharacterBone bone2 = characterBones[numArray[index]];
bone.ComputeAbsoluteTransform(true);
Matrix absoluteTransform = bone2.AbsoluteTransform;
Vector3 vector2 = absoluteTransform.Translation;
zero = bone.AbsoluteTransform.Translation;
double num7 = Vector3D.DistanceSquared(zero, translation);
if (num7 > num3)
{
Vector3 vector4 = zero - vector2;
Vector3 v = translation - vector2;
double num8 = vector4.LengthSquared();
double num9 = v.LengthSquared();
double num10 = vector4.Dot(v);
if ((num10 < 0.0) || ((num10 * num10) < ((num8 * num9) * 0.99998998641967773)))
{
Matrix matrix3;
Vector3 toVector = Vector3.Lerp(vector4, v, 1f / ((num4 * ((float)num7)) + 1f));
Matrix.CreateRotationFromTwoVectors(ref vector4, ref toVector, out matrix3);
bindTransform = Matrix.Normalize(absoluteTransform);
Matrix identity = Matrix.Identity;
if (bone2.Parent != null)
{
identity = bone2.Parent.AbsoluteTransform;
}
identity = Matrix.Normalize(identity);
bone2.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Multiply(bindTransform.GetOrientation() * matrix3, Matrix.Invert(bone2.BindTransform * identity)));
bone2.ComputeAbsoluteTransform(true);
}
}
index++;
}
}
}
/// <summary>
/// Set the bone based on the supplied position value
/// </summary>
/// <param name="position"></param>
public void SetPosition(float position)
{
if (ClipBone == null)
{
return;
}
List <AnimationClip.Keyframe> keyframes = ClipBone.Keyframes;
if (keyframes.Count == 0)
{
return;
}
// If our current position is less that the first keyframe
// we move the position backward until we get to the right keyframe
while (position < Keyframe1.Time && m_currentKeyframe > 0)
{
// We need to move backwards in time
m_currentKeyframe--;
SetKeyframes();
}
// If our current position is greater than the second keyframe
// we move the position forward until we get to the right keyframe
while (position >= Keyframe2.Time && m_currentKeyframe < ClipBone.Keyframes.Count - 2)
{
// We need to move forwards in time
m_currentKeyframe++;
SetKeyframes();
}
if (Keyframe1 == Keyframe2)
{
// Keyframes are equal
m_rotation = Keyframe1.Rotation;
m_translation = Keyframe1.Translation;
}
else
{
// Interpolate between keyframes
float t = (float)((position - Keyframe1.Time) / (Keyframe2.Time - Keyframe1.Time));
if (t > 1)
{
t = 1;
}
if (t < 0)
{
t = 0;
}
Quaternion.Slerp(ref Keyframe1.Rotation, ref Keyframe2.Rotation, t, out m_rotation);
Vector3.Lerp(ref Keyframe1.Translation, ref Keyframe2.Translation, t, out m_translation);
}
m_valid = true;
if (m_assignedBone != null)
{
Quaternion rotation = m_rotation;
//if (m_assignedBone.Name == "Soldier Spine2")
if (CalculateSpineAdditionalRotation)
{
//rotation = m_rotation * Quaternion.CreateFromAxisAngle(Vector3.Forward, -1.2f);
rotation = m_rotation * Player.SpineAdditionalRotation;
}
if (CalculateHeadAdditionalRotation)
{
//rotation = m_rotation * Quaternion.CreateFromAxisAngle(Vector3.Forward, -1.2f);
rotation = m_rotation * Player.HeadAdditionalRotation;
}
if (CalculateHandAdditionalRotation)
{
//rotation = m_rotation * Quaternion.CreateFromAxisAngle(Vector3.Forward, -1.2f);
rotation = m_rotation * Player.HandAdditionalRotation;
}
if (CalculateUpperHandAdditionalRotation != 0)
{
//rotation = m_rotation * Quaternion.CreateFromAxisAngle(Vector3.Forward, -1.2f);
Quaternion rot = Player.UpperHandAdditionalRotation;
if (CalculateUpperHandAdditionalRotation == -1)
{
rot = Quaternion.Inverse(Player.UpperHandAdditionalRotation);
}
rotation = m_rotation * rot;
}
// Send to the model
// Make it a matrix first
Matrix m;
Matrix.CreateFromQuaternion(ref rotation, out m);
m.Translation = m_translation;
m_assignedBone.SetCompleteTransform(m, Player.Weight);
}
}