public static bool SolveTwoJointsIk(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, Vector3 normal, bool preferPositiveAngle = true, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true, bool minimizeRotation = true)
{
//TODO: Implement this new method, that will consider signed/unsigned angles of bone configurations for analytic solution and use a passed normal parameter for proper rotations configuration
throw new NotImplementedException();
//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)
//{
// CosineLaw(firstBoneLength, secondBoneLength, originToDesiredEndLength, out finalAlpha, out finalBeta);
//}
// get the current angles between bones
//Vector3 planeNormal = Vector3.Cross(originToCurrentEnd, firstBoneVector);
//planeNormal.Normalize();
//double currentAlpha = GetAngleSigned(originToCurrentEnd, firstBoneVector, planeNormal);
//double delta = GetAngleSigned(originToCurrentEnd, originToDesiredEnd, planeNormal);
//finalAlpha = 1f;
//planeNormal.X = 0.94f; planeNormal.Y = 0.27f; planeNormal.Z = -0.18f;
//RotateBone(firstBone, planeNormal, delta);
//if (delta < 0)
//{
// finalAlpha = -finalAlpha;
//}
//if (finalAlpha.IsValid()) RotateBone(firstBone, planeNormal, finalAlpha);
//currentBeta = Math.PI - currentBeta;
//// check if the current Alpha is positive or negative oritented
//Vector3 crossProduct = Vector3.Cross(firstBoneVector, originToCurrentEnd);
////crossProduct.Normalize();
//float sinThetaX = crossProduct.X / (firstBoneVector.Length() * originToCurrentEnd.Length()) * Vector3.Normalize(crossProduct).X;
//float sinThetaY = crossProduct.Y / (firstBoneVector.Length() * originToCurrentEnd.Length()) * Vector3.Normalize(crossProduct).Y;
//float sinThetaZ = crossProduct.Z / (firstBoneVector.Length() * originToCurrentEnd.Length()) * Vector3.Normalize(crossProduct).Z;
//double theta = 0;
//if (crossProduct.X != 0)
//{
// theta = Math.Asin(sinThetaX);
//}
//else if (crossProduct.Y != 0)
//{
// theta = Math.Asin(sinThetaY);
//}
//else
//{
// theta = Math.Asin(sinThetaZ);
//}
//if (currentAlpha != theta)
//{
// currentAlpha = -currentAlpha;
// currentBeta = -currentBeta;
//}
// get the angle between original and final position plane normal
//originToCurrentEnd.Normalize();
//originToDesiredEnd.Normalize();
//double dotProd = Vector3.Dot(Vector3.Normalize();
//dotProd = MathHelper.Clamp(dotProd, -1, 1);
//double delta = Math.Acos(dotProd);
// set the alpha to positive/negative so the total rotation is minimized
//if (currentAlpha + delta + finalAlpha > currentAlpha + delta - finalAlpha)
//{
// finalAlpha = -finalAlpha;
// finalBeta = -finalBeta;
//}
//else
//{
// //finalBeta = Math.PI-finalBeta;
//}
//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 we compute the final absolute transforms for the bones
//Matrix firstBoneFinalTransform = firstBoneAbsoluteTransform * firstBoneRotation;
//Matrix firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
//Matrix localFirstBoneTransform = Matrix.Multiply(firstBoneFinalTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
//firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
//firstBone.ComputeAbsoluteTransform();
//Matrix secondBoneFinalTransform = secondBoneAbsoluteTransform * secondBoneRotation;
//Matrix secondBoneParentAbsoluteTransform = secondBone.Parent.AbsoluteTransform;
//Matrix localSecondBoneTransform = Matrix.Multiply(secondBoneFinalTransform, Matrix.Invert(secondBone.BindTransform * secondBoneParentAbsoluteTransform));
//secondBone.Rotation = Quaternion.CreateFromRotationMatrix(localSecondBoneTransform);
//secondBone.ComputeAbsoluteTransform();
////Vector3 planeRotationAxis = planeNormal;//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();
// minimize the rotation of the second joint from it's origin
// this will mean to rotate around originToDesiredEnd vector
//if (minimizeRotation && isDesiredEndReachable)
//{
// Vector3 a = originToDesiredEnd;
// Vector3 v = secondBone.AbsoluteTransform.Translation - firstBone.AbsoluteTransform.Translation;
// Vector3 k = firstBoneVector;
// a.Normalize();
// v.Normalize();
// k.Normalize();
// float A = Vector3.Dot(a, v) * Vector3.Dot(a, k) - Vector3.Dot(Vector3.Cross(Vector3.Cross(a, v), a), k);
// float B = Vector3.Dot(Vector3.Cross(a, v), k);
// float C = Vector3.Dot(v, k);
// double theta1 = Math.Atan(2 * B / (C - A));
// double theta2 = Math.PI + theta1;
// double finalTheta = 0;
// double secondDerivate1 = (A - C) / 2 * Math.Cos(theta1) - B * Math.Sin(theta1);
// if (secondDerivate1 > 0)
// {
// finalTheta = theta2;
// }
// else
// {
// finalTheta = theta1;
// }
// Matrix minimizingMatrix = Matrix.CreateFromAxisAngle(a, (float)finalTheta);
// Matrix firstBoneFinalMinimizedTransform = firstBone.AbsoluteTransform * minimizingMatrix;
// firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
// localFirstBoneTransform = Matrix.Multiply(firstBoneFinalMinimizedTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
// firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
// firstBone.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;
return(true);
}
/// <summary>
/// Constructor for a bone object
/// </summary>
/// <param name="name">The name of the bone</param>
/// <param name="bindTransform">The initial bind transform for the bone</param>
/// <param name="parent">A parent for this bone</param>
public MyCharacterBone(string name, Matrix bindTransform, MyCharacterBone parent)
{
this.Name = name;
this.m_parent = parent;
this.m_bindTransform = bindTransform;
this.m_children = new List<MyCharacterBone>();
if (this.m_parent != null) this.m_parent.AddChild(this);
// Set the skinning bind transform
// That is the inverse of the absolute transform in the bind pose
ComputeAbsoluteTransform();
SkinTransform = Matrix.Invert(AbsoluteTransform);
}
internal void AddChild(MyCharacterBone child)
{
m_children.Add(child);
}
public static bool SolveTwoJointsIk(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, Vector3 normal, bool preferPositiveAngle = true, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true, bool minimizeRotation = true)
{
throw new NotImplementedException();
}
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 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 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>
/// 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();
}
protected override void CalculateTransforms(float distance)
{
ProfilerShort.Begin("MyCharacter.CalculateTransforms");
base.CalculateTransforms(distance);
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("UpdateLeftHandItemPosition");
if (m_leftHandItem != null)
{
UpdateLeftHandItemPosition();
}
VRageRender.MyRenderProxy.GetRenderProfiler().StartNextBlock("Calculate Hand IK");
if (this == MySession.Static.ControlledEntity)
{
// (OM) Note: only controlled character can get it's aimed point from camera, otherwise all character's will aim the same direction
// set the aimed point explicitly using AimedPoint property
m_aimedPoint = GetAimedPointFromCamera();
}
if (m_currentWeapon != null)
{
if (!MyPerGameSettings.CheckUseAnimationInsteadOfIK(m_currentWeapon))
{
UpdateWeaponPosition();
//mainly IK and some zoom + ironsight stuff
if (m_handItemDefinition.SimulateLeftHand && m_leftHandIKStartBone != -1 && m_leftHandIKEndBone != -1 && (!UseAnimationForWeapon && m_animationToIKState == 0))
{
MatrixD leftHand = (MatrixD)m_handItemDefinition.LeftHand * ((MyEntity)m_currentWeapon).WorldMatrix;
CalculateHandIK(m_leftHandIKStartBone, m_leftForearmBone, m_leftHandIKEndBone, ref leftHand);
}
if (m_handItemDefinition.SimulateRightHand && m_rightHandIKStartBone != -1 && m_rightHandIKEndBone != -1 && (!UseAnimationForWeapon || m_animationToIKState != 0) && IsSitting == false)
{
MatrixD rightHand = (MatrixD)m_handItemDefinition.RightHand * ((MyEntity)m_currentWeapon).WorldMatrix;
CalculateHandIK(m_rightHandIKStartBone, m_rightForearmBone, m_rightHandIKEndBone, ref rightHand);
}
}
else
{
GetHeadMatrix(true); // CH: REMOVE ME! I'M A TERRIBLE HACK!
Debug.Assert(m_rightHandItemBone != -1, "Invalid bone for weapon.");
if (m_rightHandItemBone != -1)
{
//use animation for right hand item
MyCharacterBone boneRightHand = AnimationController.CharacterBones[m_rightHandItemBone];
((MyEntity)m_currentWeapon).PositionComp.WorldMatrix = boneRightHand.AbsoluteTransform * PositionComp.WorldMatrix;
}
}
}
else
{
GetHeadMatrix(true); // CH: REMOVE ME! I'M A TERRIBLE HACK!
}
VRageRender.MyRenderProxy.GetRenderProfiler().StartNextBlock("ComputeBoneTransform");
var characterBones = AnimationController.CharacterBones;
if (characterBones == null)
{
return;
}
for (int i = 0; i < characterBones.Length; i++)
{
MyCharacterBone bone = characterBones[i];
bone.ComputeBoneTransform();
BoneRelativeTransforms[i] = bone.RelativeTransform;
}
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
ProfilerShort.End();
}
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;
}
protected override void CalculateTransforms(float distance)
{
ProfilerShort.Begin("MyCharacter.CalculateTransforms");
base.CalculateTransforms(distance);
if (m_headBoneIndex >= 0 && AnimationController.CharacterBones != null && (IsInFirstPersonView || ForceFirstPersonCamera) && ControllerInfo.IsLocallyControlled() && !IsBot)
{
Vector3 headHorizontalTranslation = AnimationController.CharacterBones[m_headBoneIndex].AbsoluteTransform.Translation;
headHorizontalTranslation.Y = 0;
MyCharacterBone.TranslateAllBones(AnimationController.CharacterBones, -headHorizontalTranslation);
}
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("Calculate Hand IK");
if (this == MySession.Static.ControlledEntity)
{
// (OM) Note: only controlled character can get it's aimed point from camera, otherwise all character's will aim the same direction
// set the aimed point explicitly using AimedPoint property
m_aimedPoint = GetAimedPointFromCamera();
}
VRageRender.MyRenderProxy.GetRenderProfiler().StartNextBlock("Update anim IK");
AnimationController.UpdateInverseKinematics(); // since we already have absolute transforms
VRageRender.MyRenderProxy.GetRenderProfiler().StartNextBlock("UpdateLeftHandItemPosition");
if (m_leftHandItem != null)
{
UpdateLeftHandItemPosition();
}
if (m_currentWeapon != null && WeaponPosition != null)
{
if (!MyPerGameSettings.CheckUseAnimationInsteadOfIK(m_currentWeapon))
{
WeaponPosition.Update();
//mainly IK and some zoom + ironsight stuff
if (m_handItemDefinition.SimulateLeftHand && m_leftHandIKStartBone != -1 && m_leftHandIKEndBone != -1 && (!UseAnimationForWeapon))
{
MatrixD leftHand = (MatrixD)m_handItemDefinition.LeftHand * ((MyEntity)m_currentWeapon).WorldMatrix;
CalculateHandIK(m_leftHandIKStartBone, m_leftForearmBone, m_leftHandIKEndBone, ref leftHand);
}
if (m_handItemDefinition.SimulateRightHand && m_rightHandIKStartBone != -1 && m_rightHandIKEndBone != -1 && (!UseAnimationForWeapon) && IsSitting == false)
{
MatrixD rightHand = (MatrixD)m_handItemDefinition.RightHand * ((MyEntity)m_currentWeapon).WorldMatrix;
CalculateHandIK(m_rightHandIKStartBone, m_rightForearmBone, m_rightHandIKEndBone, ref rightHand);
}
}
else
{
GetHeadMatrix(true); // CH: REMOVE ME! I'M A TERRIBLE HACK!
Debug.Assert(m_rightHandItemBone != -1, "Invalid bone for weapon.");
if (m_rightHandItemBone != -1)
{
//use animation for right hand item
MyCharacterBone boneRightHand = AnimationController.CharacterBones[m_rightHandItemBone];
((MyEntity)m_currentWeapon).PositionComp.WorldMatrix = boneRightHand.AbsoluteTransform * PositionComp.WorldMatrix;
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW)
{
MyRenderProxy.DebugDrawAxis(((MyEntity)m_currentWeapon).PositionComp.WorldMatrix, 0.5f, false);
}
}
}
}
else
{
if (WeaponPosition != null)
{
WeaponPosition.UpdateIkTransitions();
}
GetHeadMatrix(true); // CH: REMOVE ME! I'M A TERRIBLE HACK!
}
VRageRender.MyRenderProxy.GetRenderProfiler().StartNextBlock("ComputeBoneTransform");
var characterBones = AnimationController.CharacterBones;
if (characterBones == null)
{
return;
}
for (int i = 0; i < characterBones.Length; i++)
{
MyCharacterBone bone = characterBones[i];
bone.ComputeBoneTransform();
BoneRelativeTransforms[i] = bone.RelativeTransform;
}
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
ProfilerShort.End();
}
internal void AddChild(MyCharacterBone child)
{
m_children.Add(child);
}
/// <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);
}
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);
}
}
}
/// <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 SolveTwoJointsIk(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, Vector3 normal, bool preferPositiveAngle = true, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true, bool minimizeRotation = true)
{
//TODO: Implement this new method, that will consider signed/unsigned angles of bone configurations for analytic solution and use a passed normal parameter for proper rotations configuration
throw new NotImplementedException();
//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)
//{
// CosineLaw(firstBoneLength, secondBoneLength, originToDesiredEndLength, out finalAlpha, out finalBeta);
//}
// get the current angles between bones
//Vector3 planeNormal = Vector3.Cross(originToCurrentEnd, firstBoneVector);
//planeNormal.Normalize();
//double currentAlpha = GetAngleSigned(originToCurrentEnd, firstBoneVector, planeNormal);
//double delta = GetAngleSigned(originToCurrentEnd, originToDesiredEnd, planeNormal);
//finalAlpha = 1f;
//planeNormal.X = 0.94f; planeNormal.Y = 0.27f; planeNormal.Z = -0.18f;
//RotateBone(firstBone, planeNormal, delta);
//if (delta < 0)
//{
// finalAlpha = -finalAlpha;
//}
//if (finalAlpha.IsValid()) RotateBone(firstBone, planeNormal, finalAlpha);
//currentBeta = Math.PI - currentBeta;
//// check if the current Alpha is positive or negative oritented
//Vector3 crossProduct = Vector3.Cross(firstBoneVector, originToCurrentEnd);
////crossProduct.Normalize();
//float sinThetaX = crossProduct.X / (firstBoneVector.Length() * originToCurrentEnd.Length()) * Vector3.Normalize(crossProduct).X;
//float sinThetaY = crossProduct.Y / (firstBoneVector.Length() * originToCurrentEnd.Length()) * Vector3.Normalize(crossProduct).Y;
//float sinThetaZ = crossProduct.Z / (firstBoneVector.Length() * originToCurrentEnd.Length()) * Vector3.Normalize(crossProduct).Z;
//double theta = 0;
//if (crossProduct.X != 0)
//{
// theta = Math.Asin(sinThetaX);
//}
//else if (crossProduct.Y != 0)
//{
// theta = Math.Asin(sinThetaY);
//}
//else
//{
// theta = Math.Asin(sinThetaZ);
//}
//if (currentAlpha != theta)
//{
// currentAlpha = -currentAlpha;
// currentBeta = -currentBeta;
//}
// get the angle between original and final position plane normal
//originToCurrentEnd.Normalize();
//originToDesiredEnd.Normalize();
//double dotProd = Vector3.Dot(Vector3.Normalize();
//dotProd = MathHelper.Clamp(dotProd, -1, 1);
//double delta = Math.Acos(dotProd);
// set the alpha to positive/negative so the total rotation is minimized
//if (currentAlpha + delta + finalAlpha > currentAlpha + delta - finalAlpha)
//{
// finalAlpha = -finalAlpha;
// finalBeta = -finalBeta;
//}
//else
//{
// //finalBeta = Math.PI-finalBeta;
//}
//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 we compute the final absolute transforms for the bones
//Matrix firstBoneFinalTransform = firstBoneAbsoluteTransform * firstBoneRotation;
//Matrix firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
//Matrix localFirstBoneTransform = Matrix.Multiply(firstBoneFinalTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
//firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
//firstBone.ComputeAbsoluteTransform();
//Matrix secondBoneFinalTransform = secondBoneAbsoluteTransform * secondBoneRotation;
//Matrix secondBoneParentAbsoluteTransform = secondBone.Parent.AbsoluteTransform;
//Matrix localSecondBoneTransform = Matrix.Multiply(secondBoneFinalTransform, Matrix.Invert(secondBone.BindTransform * secondBoneParentAbsoluteTransform));
//secondBone.Rotation = Quaternion.CreateFromRotationMatrix(localSecondBoneTransform);
//secondBone.ComputeAbsoluteTransform();
////Vector3 planeRotationAxis = planeNormal;//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();
// minimize the rotation of the second joint from it's origin
// this will mean to rotate around originToDesiredEnd vector
//if (minimizeRotation && isDesiredEndReachable)
//{
// Vector3 a = originToDesiredEnd;
// Vector3 v = secondBone.AbsoluteTransform.Translation - firstBone.AbsoluteTransform.Translation;
// Vector3 k = firstBoneVector;
// a.Normalize();
// v.Normalize();
// k.Normalize();
// float A = Vector3.Dot(a, v) * Vector3.Dot(a, k) - Vector3.Dot(Vector3.Cross(Vector3.Cross(a, v), a), k);
// float B = Vector3.Dot(Vector3.Cross(a, v), k);
// float C = Vector3.Dot(v, k);
// double theta1 = Math.Atan(2 * B / (C - A));
// double theta2 = Math.PI + theta1;
// double finalTheta = 0;
// double secondDerivate1 = (A - C) / 2 * Math.Cos(theta1) - B * Math.Sin(theta1);
// if (secondDerivate1 > 0)
// {
// finalTheta = theta2;
// }
// else
// {
// finalTheta = theta1;
// }
// Matrix minimizingMatrix = Matrix.CreateFromAxisAngle(a, (float)finalTheta);
// Matrix firstBoneFinalMinimizedTransform = firstBone.AbsoluteTransform * minimizingMatrix;
// firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
// localFirstBoneTransform = Matrix.Multiply(firstBoneFinalMinimizedTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
// firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
// firstBone.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;
return true;
}
