public static void RotateBone(MyCharacterBone bone, Vector3 planeNormal, double angle)
{
Matrix matrix = Matrix.CreateFromAxisAngle(planeNormal, (float)angle);
Matrix matrix2 = (bone.Parent != null) ? bone.Parent.AbsoluteTransform : Matrix.Identity;
Matrix matrix3 = Matrix.Multiply(bone.AbsoluteTransform * matrix, Matrix.Invert(bone.BindTransform * matrix2));
bone.Rotation = Quaternion.CreateFromRotationMatrix(matrix3);
bone.ComputeAbsoluteTransform(true);
}
public static bool SolveCCDIk(ref Vector3 desiredEnd, List <MyCharacterBone> bones, float stopDistance, int maxTries, float gain, ref Matrix finalTransform, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
{
MyCharacterBone bone = bones.Last <MyCharacterBone>();
int num3 = 0;
Vector3D zero = Vector3.Zero;
while (true)
{
foreach (MyCharacterBone bone2 in bones.Reverse <MyCharacterBone>())
{
bone.ComputeAbsoluteTransform(true);
Matrix absoluteTransform = bone2.AbsoluteTransform;
Vector3D translation = absoluteTransform.Translation;
Matrix matrix2 = bone.AbsoluteTransform;
zero = matrix2.Translation;
if (Vector3D.DistanceSquared(zero, desiredEnd) > stopDistance)
{
Vector3D vectord4 = zero - translation;
Vector3D v = desiredEnd - translation;
vectord4.Normalize();
v.Normalize();
double d = vectord4.Dot(v);
if (d < 1.0)
{
Vector3D vectord5 = vectord4.Cross(v);
vectord5.Normalize();
double num2 = Math.Acos(d);
Matrix matrix3 = Matrix.CreateFromAxisAngle((Vector3)vectord5, ((float)num2) * gain);
Matrix identity = Matrix.Identity;
if (bone2.Parent != null)
{
identity = bone2.Parent.AbsoluteTransform;
}
identity = Matrix.Normalize(identity);
bone2.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Multiply(Matrix.Normalize(absoluteTransform).GetOrientation() * matrix3, Matrix.Invert(bone2.BindTransform * identity)));
bone2.ComputeAbsoluteTransform(true);
}
}
}
num3++;
if ((num3 >= maxTries) || (Vector3D.DistanceSquared(zero, desiredEnd) <= stopDistance))
{
if ((finalBone != null) && 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(Vector3D.DistanceSquared(zero, desiredEnd) <= stopDistance);
}
}
}
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();
}
/// <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 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);
}
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 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++;
}
}
}
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 absoluteTransform = firstBone.AbsoluteTransform;
Matrix matrix2 = secondBone.AbsoluteTransform;
Matrix matrix3 = endBone.AbsoluteTransform;
Vector3 translation = absoluteTransform.Translation;
Vector3 vector2 = matrix3.Translation - translation;
Vector3 vector3 = desiredEnd - translation;
Vector3 vector4 = matrix2.Translation - translation;
Vector3 position = vector2 - vector4;
float num = vector4.Length();
float num2 = position.Length();
float num3 = vector3.Length();
float num4 = vector2.Length();
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
MyRenderProxy.DebugDrawSphere(Vector3.Transform(desiredEnd, WorldMatrix), 0.01f, Color.Red, 1f, false, false, true, false);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(translation, WorldMatrix), Vector3.Transform(translation + vector2, WorldMatrix), Color.Yellow, Color.Yellow, false, false);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(translation, WorldMatrix), Vector3.Transform(translation + vector3, WorldMatrix), Color.Red, Color.Red, false, false);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(translation, WorldMatrix), Vector3.Transform(translation + vector4, WorldMatrix), Color.Green, Color.Green, false, false);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(translation + vector4, WorldMatrix), Vector3.Transform((translation + vector4) + position, WorldMatrix), Color.Blue, Color.Blue, false, false);
}
bool flag = (num + num2) > num3;
double num5 = 0.0;
double num6 = 0.0;
if (flag)
{
num5 = Math.Acos(MathHelper.Clamp((double)(-(((num2 * num2) - (num * num)) - (num3 * num3)) / ((2f * num) * num3)), -1.0, 1.0));
num6 = 3.1415926535897931 - Math.Acos(MathHelper.Clamp((double)(-(((num3 * num3) - (num * num)) - (num2 * num2)) / ((2f * num) * num2)), -1.0, 1.0));
}
Vector3 axis = Vector3.Cross(vector4, vector2);
axis.Normalize();
float angle = (float)(num5 - Math.Acos(MathHelper.Clamp((double)(-(((num2 * num2) - (num * num)) - (num4 * num4)) / ((2f * num) * num4)), -1.0, 1.0)));
Matrix matrix = Matrix.CreateFromAxisAngle(-axis, angle);
vector2.Normalize();
vector3.Normalize();
Vector3 vector7 = Vector3.Cross(vector2, vector3);
vector7.Normalize();
matrix = Matrix.CreateFromAxisAngle(vector7, (float)Math.Acos(MathHelper.Clamp((double)vector2.Dot(vector3), -1.0, 1.0))) * matrix;
Matrix matrix5 = Matrix.CreateFromAxisAngle(axis, (float)(num6 - (3.1415926535897931 - Math.Acos(MathHelper.Clamp((double)(-(((num4 * num4) - (num * num)) - (num2 * num2)) / ((2f * num) * num2)), -1.0, 1.0))))) * matrix;
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
Vector3 vector8 = Vector3.Transform(vector4, matrix);
Vector3 vector9 = Vector3.Transform(position, matrix5);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(translation, WorldMatrix), Vector3.Transform(translation + vector8, WorldMatrix), Color.Purple, Color.Purple, false, false);
MyRenderProxy.DebugDrawLine3D(Vector3.Transform(translation + vector8, WorldMatrix), Vector3.Transform((translation + vector8) + vector9, WorldMatrix), Color.White, Color.White, false, false);
}
Matrix matrix6 = firstBone.Parent.AbsoluteTransform;
Matrix matrix7 = Matrix.Multiply(absoluteTransform * matrix, Matrix.Invert(firstBone.BindTransform * matrix6));
firstBone.Rotation = Quaternion.CreateFromRotationMatrix(matrix7);
firstBone.ComputeAbsoluteTransform(true);
Matrix matrix8 = secondBone.Parent.AbsoluteTransform;
Matrix matrix9 = Matrix.Multiply(matrix2 * matrix5, Matrix.Invert(secondBone.BindTransform * matrix8));
secondBone.Rotation = Quaternion.CreateFromRotationMatrix(matrix9);
secondBone.ComputeAbsoluteTransform(true);
if (((finalBone != null) && finalTransform.IsValid()) & flag)
{
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(flag);
}
/// <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;
}