public HkRigidBody GetBodyBindedToBone(MyCharacterBone myCharacterBone)
{
if (Ragdoll == null)
{
Debug.Fail("Ragdoll is not initialized!");
return null;
}
if (myCharacterBone == null)
{
Debug.Fail("Invalid parameter - cannot be null! ");
return null;
}
foreach (var pair in m_ragdollBonesMappings)
{
if (pair.Value.Bones.Contains(myCharacterBone.Name))
{
Debug.Assert(Ragdoll.RigidBodies.IsValidIndex(m_rigidBodies[pair.Key]), "Invalid rigid body index!");
return Ragdoll.RigidBodies[m_rigidBodies[pair.Key]];
}
}
Debug.Fail("Requested bone was not found in mappings!");
return null;
}
/// <summary>
/// Constructs the new mapper
/// </summary>
/// <param name="ragdoll">The ragdoll model</param>
/// <param name="bones">List of the mapped bones</param>
public MyRagdollMapper(MyCharacter character, MyCharacterBone[] bones)
{
Debug.Assert(character.Physics.Ragdoll != null, "Creating ragdoll mapper without ragdoll?");
Debug.Assert(bones != null && bones.Length > 0, "Creating ragdoll mapper without mapped bones?");
m_rigidBodiesToBonesIndices = new Dictionary<int, List<int>>();
m_character = character;
m_bones = bones;
m_rigidBodies = new Dictionary<string, int>();
m_keyframedBodies = new List<int>();
m_dynamicBodies = new List<int>();
IsActive = false;
m_inicialized = false;
IsPartiallySimulated = false;
}
/// <summary>
/// Assign this bone to the correct bone in the model
/// </summary>
/// <param name="model"></param>
public void SetModel(MyCharacter model)
{
if (ClipBone == null)
return;
// Find this bone
int index;
m_assignedBone = model.FindBone(ClipBone.Name, out index);
if (ClipBone.Name == model.Definition.SpineBone)
CalculateSpineAdditionalRotation = true;
else
CalculateSpineAdditionalRotation = false;
if (ClipBone.Name == model.Definition.HeadBone)
CalculateHeadAdditionalRotation = true;
else
CalculateHeadAdditionalRotation = false;
//"l_Forearm") || (ClipBone.Name == "r_Forearm"))
if ((ClipBone.Name == model.Definition.LeftForearmBone) || (ClipBone.Name == model.Definition.RightForearmBone))
CalculateHandAdditionalRotation = true;
else
CalculateHandAdditionalRotation = false;
if (ClipBone.Name == model.Definition.LeftUpperarmBone)
CalculateUpperHandAdditionalRotation = 1;
else
if (ClipBone.Name == model.Definition.RightUpperarmBone)
CalculateUpperHandAdditionalRotation = -1;
else
CalculateUpperHandAdditionalRotation = 0;
}
/// <summary>
/// Get the bones from the model and create a bone class object for
/// each bone. We use our bone class to do the real animated bone work.
/// </summary>
public virtual void ObtainBones()
{
MyCharacterBone[] characterBones = new MyCharacterBone[Model.Bones.Length];
for (int i = 0; i < Model.Bones.Length; i++)
{
MyModelBone bone = Model.Bones[i];
Matrix boneTransform = bone.Transform;
// Create the bone object and add to the heirarchy
MyCharacterBone newBone = new MyCharacterBone(bone.Name, boneTransform, bone.Parent != -1 ? characterBones[bone.Parent] : null);
// Add to the bone array for this model
characterBones[i] = newBone;
}
// pass array of bones to animation controller
m_compAnimationController.CharacterBones = characterBones;
}
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 void Clear()
{
m_currentKeyframe = 0;
m_isConst = false;
m_assignedBone = null;
Rotation = default(Quaternion);
Translation = Vector3.Zero;
Player = null;
Keyframe1 = null;
Keyframe2 = null;
m_clipBone = null;
}
private void DrawBoneHierarchy(MyCharacter character, ref MatrixD parentTransform, MyCharacterBone[] characterBones, List<MyAnimationClip.BoneState> rawBones, int boneIndex)
{
// ----------------------------
// raw animation data
MatrixD currentTransform = rawBones != null ? Matrix.CreateTranslation(rawBones[boneIndex].Translation) * parentTransform : MatrixD.Identity;
currentTransform = rawBones != null ? Matrix.CreateFromQuaternion(rawBones[boneIndex].Rotation) * currentTransform : currentTransform;
if (rawBones != null)
{
MyRenderProxy.DebugDrawLine3D(currentTransform.Translation, parentTransform.Translation, Color.Green, Color.Green, false);
}
bool anyChildren = false;
for (int i = 0; characterBones[boneIndex].GetChildBone(i) != null; i++)
{
var childBone = characterBones[boneIndex].GetChildBone(i);
DrawBoneHierarchy(character, ref currentTransform, characterBones, rawBones,
m_boneRefToIndex[childBone]);
anyChildren = true;
}
if (!anyChildren && rawBones != null)
{
MyRenderProxy.DebugDrawLine3D(currentTransform.Translation, currentTransform.Translation + currentTransform.Left * 0.05f, Color.Green, Color.Cyan, false);
}
// ----------------------------
// final animation data - after IK, ragdoll...
MyRenderProxy.DebugDrawText3D(Vector3D.Transform(characterBones[boneIndex].AbsoluteTransform.Translation, character.PositionComp.WorldMatrix), characterBones[boneIndex].Name, Color.Lime, 0.4f, false);
if (characterBones[boneIndex].Parent != null)
{
Vector3D boneStartPos = Vector3D.Transform(characterBones[boneIndex].AbsoluteTransform.Translation, character.PositionComp.WorldMatrix);
Vector3D boneEndPos = Vector3D.Transform(characterBones[boneIndex].Parent.AbsoluteTransform.Translation, character.PositionComp.WorldMatrix);
MyRenderProxy.DebugDrawLine3D(boneStartPos, boneEndPos, Color.Purple, Color.Purple, false);
}
if (!anyChildren)
{
Vector3D boneStartPos = Vector3D.Transform(characterBones[boneIndex].AbsoluteTransform.Translation, character.PositionComp.WorldMatrix);
Vector3D boneEndPos = Vector3D.Transform(characterBones[boneIndex].AbsoluteTransform.Translation + characterBones[boneIndex].AbsoluteTransform.Left * 0.05f, character.PositionComp.WorldMatrix);
MyRenderProxy.DebugDrawLine3D(boneStartPos, boneEndPos, Color.Purple, Color.Red, false);
}
}
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;
}
private void SetBoneTo(RagdollBone ragdollBone, float weight, float dynamicChildrenWeight, float keyframedChildrenWeight, bool translationEnabled)
{
if (Ragdoll == null)
{
return;
}
if (!m_inicialized || !IsActive)
{
return;
}
int firstBoneIndex = m_rigidBodiesToBonesIndices[ragdollBone.m_rigidBodyIndex].First();
MyCharacterBone bone = m_bones[firstBoneIndex];
//Matrix localTransform = Ragdoll.GetRigidBodyLocalTransform(ragdollBone.m_rigidBodyIndex);
Matrix localTransform = m_bodyToBoneRigTransforms[ragdollBone.m_rigidBodyIndex] * Ragdoll.GetRigidBodyLocalTransform(ragdollBone.m_rigidBodyIndex);
Matrix parentMatrix = (bone.Parent != null) ? bone.Parent.AbsoluteTransform : Matrix.Identity;
Matrix absoluteMatrixInverted = Matrix.Invert(bone.BindTransform * parentMatrix);
Matrix finalTransform = localTransform * absoluteMatrixInverted;
//finalTransform = rigidBodyToBoneTransform * finalTransform;
Debug.Assert(finalTransform.IsValid() && finalTransform != Matrix.Zero, "Ragdoll - final bone transform is invalid!");
if (finalTransform.IsValid() && finalTransform != Matrix.Zero)
{
if (weight == 1.0f)
{
bone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)finalTransform.GetOrientation()));
// NOTE: If enabled, sometimes ragdoll bodies got extra translation which leads to disproporced transfomations on limbs, therefore disabled on all bodies except the firs one
if (translationEnabled)// || m_character.IsDead)
{
bone.Translation = finalTransform.Translation;
}
}
else
{
bone.Rotation = Quaternion.Slerp(bone.Rotation, Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)finalTransform.GetOrientation())), weight);
// NOTE: If enabled, sometimes ragdoll bodies got extra translation which leads to disproporced transfomations on limbs, therefore disabled
if (translationEnabled)// || m_character.IsDead)
{
bone.Translation = Vector3.Lerp(bone.Translation, finalTransform.Translation, weight);
}
}
}
bone.ComputeAbsoluteTransform();
foreach (var childBone in ragdollBone.m_children)
{
float childWeight = dynamicChildrenWeight;
if (m_keyframedBodies.Contains(childBone.m_rigidBodyIndex))
{
childWeight = keyframedChildrenWeight;
}
SetBoneTo(childBone, childWeight, dynamicChildrenWeight, keyframedChildrenWeight, MyFakes.ENABLE_RAGDOLL_BONES_TRANSLATION);
}
}
public override void DebugDraw()
{
if (MyDebugDrawSettings.DEBUG_DRAW_SHOW_DAMAGE)
{
m_counter++;
if (m_character.CharacterAccumulatedDamage != m_lastDamage)
{
m_counter = 0;
}
VRageRender.MyRenderProxy.DebugDrawText3D(((MyEntity)m_character).WorldMatrix.Translation + ((MyEntity)m_character).WorldMatrix.Up, "Total damage:" + m_lastDamage + " velocity:" + m_lastCharacterVelocity, Color.Red, 1.5f, false, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
if (m_counter > 200)
{
m_character.CharacterAccumulatedDamage = 0;
m_lastCharacterVelocity = 0;
m_counter = 0;
}
m_lastDamage = m_character.CharacterAccumulatedDamage;
m_lastCharacterVelocity = Math.Max(m_lastCharacterVelocity, m_character.Physics.LinearVelocity.Length());
}
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_MISC && m_character.CurrentWeapon != null)
{
VRageRender.MyRenderProxy.DebugDrawAxis(((MyEntity)m_character.CurrentWeapon).WorldMatrix, 1.4f, false);
VRageRender.MyRenderProxy.DebugDrawText3D(((MyEntity)m_character.CurrentWeapon).WorldMatrix.Translation, "Weapon", Color.White, 0.7f, false, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
VRageRender.MyRenderProxy.DebugDrawSphere((m_character.AnimationController.CharacterBones[m_character.WeaponBone].AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation, 0.02f, Color.White, 1, false);
VRageRender.MyRenderProxy.DebugDrawText3D((m_character.AnimationController.CharacterBones[m_character.WeaponBone].AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation, "Weapon Bone", Color.White, 1f, false);
}
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_MISC && m_character.IsUsing != null)
{
Matrix characterMatrix = m_character.IsUsing.WorldMatrix;
characterMatrix.Translation = Vector3.Zero;
characterMatrix = characterMatrix * Matrix.CreateFromAxisAngle(characterMatrix.Up, MathHelper.Pi);
Vector3 position = m_character.IsUsing.PositionComp.GetPosition() - m_character.IsUsing.WorldMatrix.Up * MyDefinitionManager.Static.GetCubeSize(MyCubeSize.Large) / 2.0f;
position = position + characterMatrix.Up * 0.28f - characterMatrix.Forward * 0.22f;
characterMatrix.Translation = position;
VRageRender.MyRenderProxy.DebugDrawAxis(characterMatrix, 1.4f, false);
}
if (MyDebugDrawSettings.DEBUG_DRAW_SUIT_BATTERY_CAPACITY)
{
var world = m_character.PositionComp.WorldMatrix;
VRageRender.MyRenderProxy.DebugDrawText3D(world.Translation + 2f * world.Up, string.Format("{0} MWh", m_character.SuitBattery.ResourceSource.RemainingCapacity),
Color.White, 1f, true, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
}
m_simulatedBonesDebugDraw.Clear();
m_simulatedBonesAbsoluteDebugDraw.Clear();
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_BONES)
{
m_character.AnimationController.UpdateTransformations();
for (int s = 0; s < m_character.AnimationController.CharacterBones.Length; s++)
{
MyCharacterBone bone2 = m_character.AnimationController.CharacterBones[s];
if (bone2.Parent == null)
{
continue;
}
//bone2.ComputeAbsoluteTransform();
var p2m = Matrix.CreateScale(0.1f) * bone2.AbsoluteTransform * m_character.PositionComp.WorldMatrix;
Vector3 p2 = p2m.Translation;
MyCharacterBone bone1 = bone2.Parent;
//bone1.Rotation = Quaternion.Identity;
//bone1.Translation = Vector3.Zero;
// bone1.ComputeAbsoluteTransform();
Vector3 p1 = (bone1.AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation;
VRageRender.MyRenderProxy.DebugDrawLine3D(p1, p2, Color.White, Color.White, false);
Vector3 pCenter = (p1 + p2) * 0.5f;
VRageRender.MyRenderProxy.DebugDrawText3D(pCenter, bone2.Name + " (" + s.ToString() + ")", Color.Red, 0.5f, false);
VRageRender.MyRenderProxy.DebugDrawAxis(p2m, 0.1f, false);
}
}
}
public void InitLight(MyCharacterDefinition definition)
{
m_light = MyLights.AddLight();
m_lightGlareSize = definition.LightGlareSize;
m_light.Start(MyLight.LightTypeEnum.PointLight | MyLight.LightTypeEnum.Spotlight, 0.5f);
/// todo: defaults should be supplied from Environemnt.sbc
m_light.GlossFactor = 0;
m_light.DiffuseFactor = 3.14f;
m_light.UseInForwardRender = true;
m_light.LightOwner = MyLight.LightOwnerEnum.SmallShip;
m_light.ShadowDistance = 20;
m_light.ReflectorFalloff = 10;
m_light.ReflectorTexture = "Textures\\Lights\\dual_reflector_2.dds";
m_light.ReflectorColor = MyCharacter.REFLECTOR_COLOR;
m_light.ReflectorConeMaxAngleCos = MyCharacter.REFLECTOR_CONE_ANGLE;
m_light.ReflectorRange = MyCharacter.REFLECTOR_RANGE;
m_light.ReflectorGlossFactor = MyCharacter.REFLECTOR_GLOSS_FACTOR;
m_light.ReflectorDiffuseFactor = MyCharacter.REFLECTOR_DIFFUSE_FACTOR;
m_light.Color = MyCharacter.POINT_COLOR;
m_light.SpecularColor = new Vector3(MyCharacter.POINT_COLOR_SPECULAR);
m_light.Range = MyCharacter.POINT_LIGHT_RANGE;
MyCharacterBone leftGlareBone = null;
if (definition.LeftLightBone != String.Empty)
{
leftGlareBone = m_skinnedEntity.AnimationController.FindBone(definition.LeftLightBone, out m_leftLightIndex);
}
if (leftGlareBone != null)
{
m_leftGlare = MyLights.AddLight();
m_leftGlare.Start(MyLight.LightTypeEnum.None, 1.5f);
m_leftGlare.LightOn = false;
m_leftGlare.LightOwner = MyLight.LightOwnerEnum.SmallShip;
m_leftGlare.UseInForwardRender = false;
m_leftGlare.GlareOn = false;
m_leftGlare.GlareQuerySize = 0.2f;
m_leftGlare.GlareType = VRageRender.Lights.MyGlareTypeEnum.Directional;
m_leftGlare.GlareMaterial = definition.LeftGlare;
}
MyCharacterBone rightGlareBone = null;
if (definition.RightLightBone != String.Empty)
{
rightGlareBone = m_skinnedEntity.AnimationController.FindBone(definition.RightLightBone, out m_rightLightIndex);
}
if (rightGlareBone != null)
{
m_rightGlare = MyLights.AddLight();
m_rightGlare.Start(MyLight.LightTypeEnum.None, 1.5f);
m_rightGlare.LightOn = false;
m_rightGlare.LightOwner = MyLight.LightOwnerEnum.SmallShip;
m_rightGlare.UseInForwardRender = false;
m_rightGlare.GlareOn = false;
m_rightGlare.GlareQuerySize = 0.2f;
m_rightGlare.GlareType = VRageRender.Lights.MyGlareTypeEnum.Directional;
m_rightGlare.GlareMaterial = definition.RightGlare;
}
}
public override bool DebugDraw()
{
if (MyDebugDrawSettings.DEBUG_DRAW_SHOW_DAMAGE)
{
m_counter++;
if (m_character.CharacterAccumulatedDamage != m_lastDamage)
{
m_counter = 0;
}
VRageRender.MyRenderProxy.DebugDrawText3D(((MyEntity)m_character).WorldMatrix.Translation + ((MyEntity)m_character).WorldMatrix.Up, "Total damage:" + m_lastDamage + " velocity:" + m_lastCharacterVelocity, Color.Red, 1.5f, false, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
if (m_counter > 200)
{
m_character.CharacterAccumulatedDamage = 0;
m_lastCharacterVelocity = 0;
m_counter = 0;
}
m_lastDamage = m_character.CharacterAccumulatedDamage;
m_lastCharacterVelocity = Math.Max(m_lastCharacterVelocity, m_character.Physics.LinearVelocity.Length());
}
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_MISC && m_character.CurrentWeapon != null)
{
VRageRender.MyRenderProxy.DebugDrawAxis(((MyEntity)m_character.CurrentWeapon).WorldMatrix, 1.4f, false);
VRageRender.MyRenderProxy.DebugDrawText3D(((MyEntity)m_character.CurrentWeapon).WorldMatrix.Translation, "Weapon", Color.White, 0.7f, false, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
VRageRender.MyRenderProxy.DebugDrawSphere((m_character.Bones[m_character.WeaponBone].AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation, 0.02f, Color.White, 1, false);
VRageRender.MyRenderProxy.DebugDrawText3D((m_character.Bones[m_character.WeaponBone].AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation, "Weapon Bone", Color.White, 1f, false);
}
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_MISC && m_character.IsUsing != null)
{
Matrix characterMatrix = m_character.IsUsing.WorldMatrix;
characterMatrix.Translation = Vector3.Zero;
characterMatrix = characterMatrix * Matrix.CreateFromAxisAngle(characterMatrix.Up, MathHelper.Pi);
Vector3 position = m_character.IsUsing.PositionComp.GetPosition() - m_character.IsUsing.WorldMatrix.Up * MyDefinitionManager.Static.GetCubeSize(MyCubeSize.Large) / 2.0f;
position = position + characterMatrix.Up * 0.28f - characterMatrix.Forward * 0.22f;
characterMatrix.Translation = position;
VRageRender.MyRenderProxy.DebugDrawAxis(characterMatrix, 1.4f, false);
}
if (MyDebugDrawSettings.DEBUG_DRAW_SUIT_BATTERY_CAPACITY)
{
var world = m_character.PositionComp.WorldMatrix;
VRageRender.MyRenderProxy.DebugDrawText3D(world.Translation + 2f * world.Up, string.Format("{0} MWh", m_character.SuitBattery.RemainingCapacity),
Color.White, 1f, true, MyGuiDrawAlignEnum.HORISONTAL_CENTER_AND_VERTICAL_CENTER);
}
// return true;
/*
* Matrix headMatrix = m_headMatrix * WorldMatrix;
* VRageRender.MyRenderProxy.DebugDrawAxis(headMatrix, 2.4f, false);
*
* VRageRender.MyRenderProxy.DebugDrawAxis(m_weaponDummyMatrix, 2.4f, false);
*
* VRageRender.MyRenderProxy.DebugDrawAxis(Matrix.CreateWorld(headMatrix.Translation, Vector3.Forward, Vector3.Up), 2.4f, false);
*/
//return true;
m_simulatedBonesDebugDraw.Clear();
m_simulatedBonesAbsoluteDebugDraw.Clear();
//Physics.CharacterProxy.GetPoseModelSpace(m_simulatedBonesDebugDraw);
//Physics.CharacterProxy.GetPoseLocalSpace(m_simulatedBones);
if (MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_BONES)
{
if (MyFakes.USE_HAVOK_ANIMATION_HANDS)
{
for (int s = 0; s < m_simulatedBonesDebugDraw.Count; s++)
{
MyCharacterBone bone2 = m_character.Bones[s];
Matrix absolute2 = m_simulatedBonesDebugDraw[s];// *bone2.BindTransform;
if (bone2.Parent == null)
{
m_simulatedBonesAbsoluteDebugDraw.Add(absolute2);
continue;
}
MyCharacterBone bone1 = bone2.Parent;
Matrix absolute1 = m_simulatedBonesAbsoluteDebugDraw[m_character.Bones.IndexOf(bone1)];
//absolute2 = absolute2 * absolute1;
m_simulatedBonesAbsoluteDebugDraw.Add(absolute2);
var p2m = absolute2 * m_character.PositionComp.WorldMatrix;
Vector3 p2 = p2m.Translation;
//bone1.Rotation = Quaternion.Identity;
//bone1.Translation = Vector3.Zero;
Vector3 p1 = (absolute1 * m_character.PositionComp.WorldMatrix).Translation;
VRageRender.MyRenderProxy.DebugDrawLine3D(p1, p2, Color.White, Color.White, false);
Vector3 pCenter = (p1 + p2) * 0.5f;
VRageRender.MyRenderProxy.DebugDrawText3D(pCenter, bone2.Name + " (" + s.ToString() + ")", Color.White, 0.5f, false);
VRageRender.MyRenderProxy.DebugDrawAxis(p2m, 0.5f, false);
}
}
else
{
for (int s = 0; s < m_character.Bones.Count; s++)
{
MyCharacterBone bone2 = m_character.Bones[s];
if (bone2.Parent == null)
{
continue;
}
bone2.ComputeAbsoluteTransform();
var p2m = Matrix.CreateScale(0.1f) * bone2.AbsoluteTransform * m_character.PositionComp.WorldMatrix;
Vector3 p2 = p2m.Translation;
MyCharacterBone bone1 = bone2.Parent;
//bone1.Rotation = Quaternion.Identity;
//bone1.Translation = Vector3.Zero;
// bone1.ComputeAbsoluteTransform();
Vector3 p1 = (bone1.AbsoluteTransform * m_character.PositionComp.WorldMatrix).Translation;
VRageRender.MyRenderProxy.DebugDrawLine3D(p1, p2, Color.White, Color.White, false);
Vector3 pCenter = (p1 + p2) * 0.5f;
VRageRender.MyRenderProxy.DebugDrawText3D(pCenter, bone2.Name + " (" + s.ToString() + ")", Color.White, 0.5f, false);
VRageRender.MyRenderProxy.DebugDrawAxis(p2m, 0.1f, false);
if (s == 0)
{
// MyDebugDraw.DrawAxis((bone2.AbsoluteTransform * WorldMatrix), 1000, 1, false);
}
}
}
}
return(true);
}
private void DrawBoneHierarchy(ref MatrixD parentTransform, MyCharacterBone[] characterBones, List<MyAnimationClip.BoneState> rawBones, int boneIndex)
{
MatrixD currentTransform = Matrix.CreateTranslation(rawBones[boneIndex].Translation) * parentTransform;
currentTransform = Matrix.CreateFromQuaternion(rawBones[boneIndex].Rotation) * currentTransform;
MyRenderProxy.DebugDrawLine3D(currentTransform.Translation, parentTransform.Translation, Color.Green, Color.Green, false);
if (characterBones[boneIndex].Parent == null)
MyRenderProxy.DebugDrawText3D(currentTransform.Translation, characterBones[boneIndex].Name, Color.Green, 1.0f, false);
for (int i = 0; characterBones[boneIndex].GetChildBone(i) != null; i++)
{
var childBone = characterBones[boneIndex].GetChildBone(i);
DrawBoneHierarchy(ref currentTransform, characterBones, rawBones,
m_boneRefToIndex[childBone]);
}
}
/// <summary>
/// Get the bones from the model and create a bone class object for
/// each bone. We use our bone class to do the real animated bone work.
/// </summary>
private void ObtainBones()
{
m_bones.Clear();
foreach (MyModelBone bone in Model.Bones)
{
Matrix boneTransform = bone.Transform;
if (Model.DataVersion < 01047001)
{
if (bone.Name.Contains("Dummy"))
{ //0.009860006
boneTransform = boneTransform * Matrix.CreateScale(0.009860006f);
}
}
// Create the bone object and add to the heirarchy
MyCharacterBone newBone = new MyCharacterBone(bone.Name, boneTransform, bone.Parent != -1 ? m_bones[bone.Parent] : null);
if (Model.DataVersion < 01050001)
{
newBone.CompatibilityMode = true;
}
// Add to the bones for this model
m_bones.Add(newBone);
}
}
// Rebuild mapping clip bone indices to character bone indices.
private void RebuildBoneIndices(MyCharacterBone[] characterBones)
{
// find indices of clip bones
// optimize? see m_boneIndices declaration
m_boneIndicesMapping = new int[m_animationClip.Bones.Count];
for (int i = 0; i < m_animationClip.Bones.Count; i++)
{
m_boneIndicesMapping[i] = -1;
for (int j = 0; j < characterBones.Length; j++)
{
if (m_animationClip.Bones[i].Name == characterBones[j].Name)
{
m_boneIndicesMapping[i] = j;
break;
}
}
if (m_boneIndicesMapping[i] == -1)
{
// VRAGE TODO: at least log it? this is unfortunatelly a common situation
//Debug.Fail(String.Format("Clip bone not found in character. {0} in {1}",
// m_animationClip.Bones[i].Name,
// string.Join(",", (from item in characterBones select item.Name).ToArray())));
}
}
}
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;
}
/// <summary>
/// Updates feet bones positions, locations and rotation using IK, based on current character state
/// </summary>
private void UpdateFeet()
{
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("UpdateFeetPlacement standing");
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_SETTINGS)
{
MyFeetIKSettings feetDebugSettings;
CharacterDefinition.FeetIKSettings.TryGetValue(MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_MOVEMENT_STATE, out feetDebugSettings);
Matrix leftFootMatrix = Bones[m_leftAnkleBone].AbsoluteTransform;
Matrix rightFootMatrix = Bones[m_rightAnkleBone].AbsoluteTransform;
Vector3 upDirection = WorldMatrix.Up;
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.DebugDrawLine3D(fromL, fromL + upDirection * feetDebugSettings.AboveReachableDistance, Color.Yellow, Color.Yellow, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(fromL, fromL - upDirection * feetDebugSettings.BelowReachableDistance, Color.Red, Color.Red, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(fromR, fromR + upDirection * feetDebugSettings.AboveReachableDistance, Color.Yellow, Color.Yellow, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(fromR, fromR - upDirection * feetDebugSettings.BelowReachableDistance, Color.Red, Color.Red, false);
Matrix leftFoot = Matrix.CreateScale(feetDebugSettings.FootSize) * WorldMatrix;
Matrix rightFoot = Matrix.CreateScale(feetDebugSettings.FootSize) * WorldMatrix;
leftFoot.Translation = fromL;
rightFoot.Translation = fromR;
VRageRender.MyRenderProxy.DebugDrawOBB(leftFoot, Color.White, 1f, false, false);
VRageRender.MyRenderProxy.DebugDrawOBB(rightFoot, Color.White, 1f, false, false);
}
MyFeetIKSettings feetSettings;
var characterBones = Character.AnimationController.CharacterBones;
if (CharacterDefinition.FeetIKSettings.TryGetValue(MovementState, out feetSettings))
{
// If Feet IK placement is enabled for this character movement state, let's calculate new foot positions
if (feetSettings.Enabled)
{
UpdateFeetPlacement(WorldMatrix.Up,
feetSettings.BelowReachableDistance,
feetSettings.AboveReachableDistance,
feetSettings.VerticalShiftUpGain,
feetSettings.VerticalShiftDownGain,
feetSettings.FootSize);
}
}
else if (characterBones[m_rootBone].Translation != Vector3.Zero)
{
// Otherwise remove the applied translation on the root bone
characterBones[m_rootBone].Translation = characterBones[m_rootBone].Translation.LengthSquared() > 0.001f ? characterBones[m_rootBone].Translation * 0.1f : Vector3.Zero;
characterBones[m_rootBone].ComputeAbsoluteTransform();
}
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("Storing bones transforms");
// After the feet placement we need to save new bone transformations
for (int i = 0; i < Bones.Length; i++)
{
MyCharacterBone bone = Bones[i];
bone.ComputeBoneTransform();
Character.BoneRelativeTransforms[i] = bone.RelativeTransform;
}
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
}
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;
}
/// <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!");
// angle height above ground
float ankleHeight = footDimensions.Y;
// get the current foot matrix and location
Matrix invWorld = Character.PositionComp.WorldMatrixInvScaled;
MyCharacterBone rootBone = Bones[m_rootBone]; // root bone is used to transpose the model up or down
Matrix modelRootBoneMatrix = rootBone.AbsoluteTransform;
// current model shift in local coords - this is not changed by animations
float verticalShift = modelRootBoneMatrix.Translation.Y;
// current local feet matrices
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 fromLWrld = Vector3.Transform(leftFootGroundPosition, WorldMatrix); // we get this position in the world
Vector3 fromRWrld = Vector3.Transform(rightFootGroundPosition, WorldMatrix);
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("GetClosestFootPosition");
// find the closest ground support, raycasting from Up to Down
var contactLeftWrld = MyInverseKinematics.GetClosestFootSupportPosition(Character, null, fromLWrld, upDirection, footDimensions, WorldMatrix, belowCharacterReachableDistance, aboveCharacterReachableDistance, Character.Physics.CharacterCollisionFilter); // this returns world coordinates of support for left foot
var contactRightWrld = MyInverseKinematics.GetClosestFootSupportPosition(Character, null, fromRWrld, upDirection, footDimensions, WorldMatrix, belowCharacterReachableDistance, aboveCharacterReachableDistance, Character.Physics.CharacterCollisionFilter);
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("Characters root shift estimation");
// if we got hit only for one feet, we do nothing, but slowly return back root bone (character vertical shift) position if it was changed from original
// that happends very likely when the support below is too far for one leg
if (contactLeftWrld == null || contactRightWrld == null)
{
rootBone.Translation -= rootBone.Translation * verticalShiftUpGain;
rootBone.ComputeAbsoluteTransform();
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
return;
}
// Here we recalculate if we shift the root of the character to reach bottom or top
// get the desired foot world coords
Vector3 supportLWrld = contactLeftWrld.Value.Position;
Vector3 supportRWrld = contactRightWrld.Value.Position;
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_CLOSESTSUPPORTPOSITION)
{
VRageRender.MyRenderProxy.DebugDrawText3D(supportLWrld, "Foot support position", Color.Blue, 1, false);
VRageRender.MyRenderProxy.DebugDrawText3D(supportRWrld, "Foot support position", Color.Blue, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere(supportLWrld, 0.03f, Color.Blue, 0, false);
VRageRender.MyRenderProxy.DebugDrawSphere(supportRWrld, 0.03f, Color.Blue, 0, false);
}
// If the terrain is slope we need to shift the desired position up a little..
float leftCrossProd = Vector3.Dot(WorldMatrix.Up, contactLeftWrld.Value.Normal);
float rightCrossProd = Vector3.Dot(WorldMatrix.Up, contactRightWrld.Value.Normal);
float leftSlopeShiftFactor = (1f - leftCrossProd) * footDimensions.Z * 0.5f;
float rightSlopeShiftFactor = (1f - rightCrossProd) * footDimensions.Z * 0.5f;
// First get the local coords of the ankles as if were driven by animation only - correct it if root bone location was shifted
Vector3 leftAnklePosFromAnim = Vector3.Up * (leftFootMatrix.Translation.Y - verticalShift);
Vector3 rightAnklePosFromAnim = Vector3.Up * (rightFootMatrix.Translation.Y - verticalShift);
// Now compute the desired ankle's positions in the model's local space
Vector3 leftAnkleDesiredPosition = Vector3.Transform(supportLWrld, invWorld) + leftAnklePosFromAnim + Vector3.Up * (FEET_ABOVE_GROUND_OFFSET + leftSlopeShiftFactor);
Vector3 rightAnkleDesiredPosition = Vector3.Transform(supportRWrld, invWorld) + rightAnklePosFromAnim + Vector3.Up * (FEET_ABOVE_GROUND_OFFSET + rightSlopeShiftFactor);
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_ANKLE_DESIREDPOSITION)
{
VRageRender.MyRenderProxy.DebugDrawText3D(Vector3.Transform(leftAnkleDesiredPosition, WorldMatrix), "Ankle desired position", Color.Purple, 1, false);
VRageRender.MyRenderProxy.DebugDrawText3D(Vector3.Transform(rightAnkleDesiredPosition, WorldMatrix), "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;
float currentLeftAnkleHeight = leftFootMatrix.Translation.Y;
float currentRightAnkleHeight = rightFootMatrix.Translation.Y;
// if we the distances are too big, so we will not be able to set the position, we can skip it
if (Math.Abs(leftAnkleDesiredHeight - rightAnkleDesiredHeight) > aboveCharacterReachableDistance)
{
rootBone.Translation = modelRootBoneMatrix.Translation -= modelRootBoneMatrix.Translation * verticalShiftUpGain;
rootBone.ComputeAbsoluteTransform();
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
return;
}
// if we got one of the supports below the character root, we must check whether we can reach it, if yes, we need to crouch to reach it
if ((((leftAnkleDesiredHeight > -belowCharacterReachableDistance) && (leftAnkleDesiredHeight < ankleHeight)) || // left support is below model and is reachable
((rightAnkleDesiredHeight > -belowCharacterReachableDistance) && (rightAnkleDesiredHeight < ankleHeight))) && // right support is below model and is reachable
// finally check if character is shifted down, the other feet won't get too high
(Math.Max(leftAnkleDesiredHeight, rightAnkleDesiredHeight) - Math.Min(leftAnkleDesiredHeight, rightAnkleDesiredHeight) < aboveCharacterReachableDistance))
{
// then we can try to reach down according to the difference
float distanceBelow = Math.Min(leftAnkleDesiredHeight - currentLeftAnkleHeight, rightAnkleDesiredHeight - currentRightAnkleHeight);// -verticalShift;// -ankleHeight;
Vector3 verticalTranslation = Vector3.Up * distanceBelow;
Vector3 translation = Vector3.Zero;
translation.Interpolate3(modelRootBoneMatrix.Translation, modelRootBoneMatrix.Translation + verticalTranslation, verticalShiftDownGain);
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_ANKLE_DESIREDPOSITION)
{
VRageRender.MyRenderProxy.DebugDrawLine3D(WorldMatrix.Translation, Vector3.Transform(Vector3.Up * distanceBelow, WorldMatrix), Color.Purple, Color.Purple, false);
VRageRender.MyRenderProxy.DebugDrawText3D(WorldMatrix.Translation, "Computed height", Color.Purple, 1, false);
}
rootBone.Translation = modelRootBoneMatrix.Translation = translation;
rootBone.ComputeAbsoluteTransform();
}
else // if both supports are up, we need to get up, however, that should be done by rigid body as well.. we limit it only by reachable distance, so it is bounded to rigid body position
if ((leftAnkleDesiredHeight > ankleHeight) && (leftAnkleDesiredHeight < aboveCharacterReachableDistance) &&
(rightAnkleDesiredHeight > ankleHeight) && (rightAnkleDesiredHeight < aboveCharacterReachableDistance))
{
// move up to reach the highest support
float distanceAbove = Math.Max(leftAnkleDesiredHeight - currentLeftAnkleHeight, rightAnkleDesiredHeight - currentRightAnkleHeight);// -verticalShift;// -ankleHeight;
Vector3 verticalTranslation = Vector3.Up * distanceAbove;
Vector3 translation = Vector3.Zero;
translation.Interpolate3(modelRootBoneMatrix.Translation, modelRootBoneMatrix.Translation + verticalTranslation, verticalShiftUpGain);
rootBone.Translation = modelRootBoneMatrix.Translation = translation;
rootBone.ComputeAbsoluteTransform();
}
// finally if we can not get into right vertical position for foot placement, slowly reset the vertical shift
else
{
modelRootBoneMatrix.Translation -= modelRootBoneMatrix.Translation * verticalShiftUpGain;
rootBone.Translation = modelRootBoneMatrix.Translation;
rootBone.ComputeAbsoluteTransform();
}
// Hard limit to root's shift in vertical position
//if (characterVerticalShift < -underFeetReachableDistance)
//{
// modelRootBoneMatrix.Translation = -upDirection * underFeetReachableDistance;
// rootBone.SetBindTransform(modelRootBoneMatrix);
// characterVerticalShift = -underFeetReachableDistance; // get the new height
//}
//if (characterVerticalShift > underFeetReachableDistance)
//{
// modelRootBoneMatrix.Translation = upDirection * underFeetReachableDistance;
// rootBone.SetBindTransform(modelRootBoneMatrix);
// characterVerticalShift = underFeetReachableDistance; // get the new height
//}
// Then we need to recalculate all other bones matrices so we get proper data for children, since we changed the root position
//foreach (var b in m_bones) b.ComputeAbsoluteTransform();
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
VRageRender.MyRenderProxy.GetRenderProfiler().StartProfilingBlock("CalculateFeetPlacement");
// Then recalculate feet positions only if we can reach the final and if we are in the limits
if ((-belowCharacterReachableDistance < leftAnkleDesiredHeight) && (leftAnkleDesiredHeight < aboveCharacterReachableDistance)) // and the found foot support height is not over limit
{
CalculateFeetPlacement(
m_leftHipBone,
m_leftKneeBone,
m_leftAnkleBone,
leftAnkleDesiredPosition,
contactLeftWrld.Value.Normal,
footDimensions,
leftFootMatrix.Translation.Y - verticalShift <= ankleHeight);
}
if ((-belowCharacterReachableDistance < rightAnkleDesiredHeight) && (rightAnkleDesiredHeight < aboveCharacterReachableDistance))
{
CalculateFeetPlacement(
m_rightHipBone,
m_rightKneeBone,
m_rightAnkleBone,
rightAnkleDesiredPosition,
contactRightWrld.Value.Normal,
footDimensions,
rightFootMatrix.Translation.Y - verticalShift <= ankleHeight);
}
VRageRender.MyRenderProxy.GetRenderProfiler().EndProfilingBlock();
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_BONES)
{
List <Matrix> left = new List <Matrix> {
Bones[m_leftHipBone].AbsoluteTransform, Bones[m_leftKneeBone].AbsoluteTransform, Bones[m_leftAnkleBone].AbsoluteTransform
};
List <Matrix> right = new List <Matrix> {
Bones[m_rightHipBone].AbsoluteTransform, Bones[m_rightKneeBone].AbsoluteTransform, Bones[m_rightAnkleBone].AbsoluteTransform
};
DebugDrawBones(left);
DebugDrawBones(right);
VRageRender.MyRenderProxy.DebugDrawText3D(WorldMatrix.Translation, "Rigid body", Color.Yellow, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere(WorldMatrix.Translation, 0.05f, Color.Yellow, 0, false);
VRageRender.MyRenderProxy.DebugDrawText3D((modelRootBoneMatrix * WorldMatrix).Translation, "Character root bone", Color.Yellow, 1, false);
VRageRender.MyRenderProxy.DebugDrawSphere((modelRootBoneMatrix * WorldMatrix).Translation, 0.07f, Color.Red, 0, false);
}
}
/// <summary>
/// Analytic solutions useful fo hands or feet, all in local model space
/// </summary>
/// <param name="desiredEnd">in local model space</param>
/// <param name="firstBone"></param>
/// <param name="secondBone"></param>
/// <param name="finalTransform"></param>
/// <param name="finalBone"></param>
/// <param name="allowFinalBoneTranslation"></param>
/// <returns></returns>
public static bool SolveTwoJointsIk(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
{
Matrix firstBoneAbsoluteTransform = firstBone.AbsoluteTransform;
Matrix secondBoneAbsoluteTransform = secondBone.AbsoluteTransform;
Matrix endBoneAbsoluteTransform = endBone.AbsoluteTransform;
Vector3 origin = firstBoneAbsoluteTransform.Translation;
Vector3 originToCurrentEnd = endBoneAbsoluteTransform.Translation - origin;
Vector3 originToDesiredEnd = desiredEnd - origin;
Vector3 firstBoneVector = secondBoneAbsoluteTransform.Translation - origin;
Vector3 secondBoneVector = originToCurrentEnd - firstBoneVector;
float firstBoneLength = firstBoneVector.Length();
float secondBoneLength = secondBoneVector.Length();
float originToDesiredEndLength = originToDesiredEnd.Length();
float originToCurrentEndLength = originToCurrentEnd.Length();
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
VRageRender.MyRenderProxy.DebugDrawSphere(Vector3.Transform(desiredEnd, WorldMatrix), 0.01f, Color.Red, 1, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToCurrentEnd, WorldMatrix), Color.Yellow, Color.Yellow, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToDesiredEnd, WorldMatrix), Color.Red, Color.Red, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + firstBoneVector, WorldMatrix), Color.Green, Color.Green, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + firstBoneVector, WorldMatrix), Vector3.Transform(origin + firstBoneVector + secondBoneVector, WorldMatrix), Color.Blue, Color.Blue, false);
}
// only two cases, the desired position is reachable or not
bool isDesiredEndReachable = firstBoneLength + secondBoneLength > originToDesiredEndLength;
// alpha = angle between the first bone and originToDesiredEnd vector
double finalAlpha = 0;
// beta = the angle between the first and second bone
double finalBeta = 0;
if (isDesiredEndReachable)
{ // we find proper angles
// cosine law c^2 = a^2 + b^2 - 2*a*b*cos(gamma)
// gamma = acos ( - (c^2 - a^2 - b^2) / (2*a*b) )
// alpha = angle between the first bone and originToDesiredEnd vector
double cosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToDesiredEndLength * originToDesiredEndLength) /
(2 * firstBoneLength * originToDesiredEndLength);
cosAlpha = MathHelper.Clamp(cosAlpha, -1, 1);
finalAlpha = Math.Acos(cosAlpha);
// beta = the angle between the first and second bone
double cosBeta = -(originToDesiredEndLength * originToDesiredEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
(2 * firstBoneLength * secondBoneLength);
cosBeta = MathHelper.Clamp(cosBeta, -1, 1);
finalBeta = Math.Acos(cosBeta);
// now get it to the root bone axis no
finalBeta = Math.PI - finalBeta;
}
// get the current angles
double cCosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToCurrentEndLength * originToCurrentEndLength) /
(2 * firstBoneLength * originToCurrentEndLength);
cCosAlpha = MathHelper.Clamp(cCosAlpha, -1, 1);
double currentAlpha = Math.Acos(cCosAlpha);
double cCosBeta = -(originToCurrentEndLength * originToCurrentEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
(2 * firstBoneLength * secondBoneLength);
cCosBeta = MathHelper.Clamp(cCosBeta, -1, 1);
double currentBeta = Math.Acos(cCosBeta);
currentBeta = Math.PI - currentBeta;
Vector3 currentPlaneNormal = Vector3.Cross(firstBoneVector, originToCurrentEnd);
currentPlaneNormal.Normalize();
// we can now rotate the bones in current plane as if the desired end was on the currentEnd axis
float alphaDif = (float)(finalAlpha - currentAlpha);
float betaDif = (float)(finalBeta - currentBeta);
Matrix firstBoneRotation = Matrix.CreateFromAxisAngle(-currentPlaneNormal, alphaDif);
Matrix secondBoneRotation = Matrix.CreateFromAxisAngle(currentPlaneNormal, betaDif);
// now get the angle between original and final position plane normal
originToCurrentEnd.Normalize();
originToDesiredEnd.Normalize();
double dotProd = originToCurrentEnd.Dot(originToDesiredEnd);
dotProd = MathHelper.Clamp(dotProd, -1, 1);
double delta = Math.Acos(dotProd);
Vector3 planeRotationAxis = Vector3.Cross(originToCurrentEnd, originToDesiredEnd);
planeRotationAxis.Normalize();
// find the rotation matrices for bones in the original plane
Matrix planeRotation = Matrix.CreateFromAxisAngle(planeRotationAxis, (float)delta);
// compute the final rotations
firstBoneRotation = planeRotation * firstBoneRotation;
secondBoneRotation = secondBoneRotation * firstBoneRotation;
// draw the final positions if debug enabled
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
Vector3 rotatedFirst = Vector3.Transform(firstBoneVector, firstBoneRotation);
Vector3 rotatedSecond = Vector3.Transform(secondBoneVector, secondBoneRotation);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + rotatedFirst, WorldMatrix), Color.Purple, Color.Purple, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + rotatedFirst, WorldMatrix), Vector3.Transform(origin + rotatedFirst + rotatedSecond, WorldMatrix), Color.White, Color.White, false);
}
// Now we compute the final absolute transforms for the bones
Matrix firstBoneFinalAbsoluteTransform = firstBoneAbsoluteTransform * firstBoneRotation;
Matrix firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
Matrix localFirstBoneTransform = Matrix.Multiply(firstBoneFinalAbsoluteTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
firstBone.ComputeAbsoluteTransform();
Matrix secondBoneFinalAbsoluteTransform = secondBoneAbsoluteTransform * secondBoneRotation;
Matrix secondBoneParentAbsoluteTransform = secondBone.Parent.AbsoluteTransform;
Matrix localSecondBoneTransform = Matrix.Multiply(secondBoneFinalAbsoluteTransform, Matrix.Invert(secondBone.BindTransform * secondBoneParentAbsoluteTransform));
secondBone.Rotation = Quaternion.CreateFromRotationMatrix(localSecondBoneTransform);
secondBone.ComputeAbsoluteTransform();
// solve the last bone
if (finalBone != null && finalTransform.IsValid() && isDesiredEndReachable)
{
//MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation)
// get the related transformation to original binding pose
MatrixD localTransformRelated;
if (allowFinalBoneTranslation) localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
else localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
//localTransformRelated = Matrix.Normalize(localTransformRelated);
// from there get the rotation and translation
finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
if (allowFinalBoneTranslation) finalBone.Translation = (Vector3)localTransformRelated.Translation;
finalBone.ComputeAbsoluteTransform();
}
return isDesiredEndReachable;
}
private void IKFeetStepSounds(MyEntity3DSoundEmitter walkEmitter, MySoundPair cueEnum)
{
var movementState = m_character.GetCurrentMovementState();
if (movementState.GetMode() == MyCharacterMovement.Flying)
{
return;
}
if (movementState.GetSpeed() != m_lastUpdateMovementState.GetSpeed())
{
walkEmitter.StopSound(true);
m_lastStepTime = 0;
}
int usedMinimumDelay = int.MaxValue;
if (movementState.GetDirection() != MyCharacterMovement.NoDirection)
{
switch (movementState.GetSpeed())
{
case MyCharacterMovement.NormalSpeed:
usedMinimumDelay = m_stepMinimumDelayWalk;
break;
case MyCharacterMovement.Fast:
usedMinimumDelay = m_stepMinimumDelayRun;
break;
case MyCharacterMovement.VeryFast:
usedMinimumDelay = m_stepMinimumDelaySprint;
break;
}
}
bool minimumDelayExceeded = false;
minimumDelayExceeded = (MySandboxGame.TotalGamePlayTimeInMilliseconds - m_lastStepTime) >= usedMinimumDelay;
//MyRenderProxy.DebugDrawAABB(m_character.PositionComp.WorldAABB, Color.White);
if (minimumDelayExceeded)
{
int leftAnkleBoneIndex, rightAnkleBoneIndex;
MyCharacterBone leftAnkleBone = m_character.AnimationController != null
? m_character.AnimationController.FindBone(m_character.Definition.LeftAnkleBoneName, out leftAnkleBoneIndex)
: null;
MyCharacterBone rightAnkleBone = m_character.AnimationController != null
? m_character.AnimationController.FindBone(m_character.Definition.RightAnkleBoneName, out rightAnkleBoneIndex)
: null;
Vector3 posLeftFoot = leftAnkleBone != null
? leftAnkleBone.AbsoluteTransform.Translation
: m_character.PositionComp.LocalAABB.Center;
Vector3 posRightFoot = rightAnkleBone != null
? rightAnkleBone.AbsoluteTransform.Translation
: m_character.PositionComp.LocalAABB.Center;
float ankleHeight;
MyFeetIKSettings settingsIK;
if (m_character.Definition.FeetIKSettings != null &&
m_character.Definition.FeetIKSettings.TryGetValue(MyCharacterMovementEnum.Standing, out settingsIK))
{
ankleHeight = settingsIK.FootSize.Y;
}
else
{
ankleHeight = DEFAULT_ANKLE_HEIGHT;
}
float charSpeed = 0f;
if (m_character.AnimationController != null)
{
m_character.AnimationController.Variables.GetValue(MyAnimationVariableStorageHints.StrIdSpeed, out charSpeed);
}
if (posLeftFoot.Y - ankleHeight < m_character.PositionComp.LocalAABB.Min.Y ||
posRightFoot.Y - ankleHeight < m_character.PositionComp.LocalAABB.Min.Y)
{
if (charSpeed > 0.05f)
{
walkEmitter.PlaySound(cueEnum);
}
m_lastStepTime = MySandboxGame.TotalGamePlayTimeInMilliseconds;
}
}
m_lastUpdateMovementState = movementState;
}
/// <summary>
/// Assign this bone to the correct bone in the model
/// </summary>
/// <param name="model"></param>
public void SetModel(MySkinnedEntity skinnedEntity)
{
if (ClipBone == null)
return;
// Find this bone
int index;
m_assignedBone = skinnedEntity.FindBone(ClipBone.Name, out index);
}
public unsafe void BlendWeight(ref float weight, MyCharacterBone bone, IMyVariableStorage <float> controllerVariables)
{
if (this.m_boneIndexToData.Length > bone.Index)
{
float num;
float num2;
BoneData data = this.m_boneIndexToData[bone.Index];
if (!controllerVariables.GetValue(this.m_defautlBlendTimeId, out this.m_defaultBlendTime))
{
this.m_defaultBlendTime = 2.5f;
}
if (!controllerVariables.GetValue(data.WeightId, out num) || (num < 0f))
{
num = -1f;
}
if (!controllerVariables.GetValue(data.BlendTimeId, out num2) || (num2 < 0f))
{
num2 = -1f;
}
if ((num < 0f) || (num2 < 0f))
{
float maxValue = float.MaxValue;
float num6 = float.MaxValue;
LayerData[] layers = data.Layers;
int index = 0;
while (true)
{
float num8;
float num9;
if (index >= layers.Length)
{
if (num < 0f)
{
if (maxValue == float.MaxValue)
{
return;
}
num = maxValue;
}
if (num2 < 0f)
{
num2 = (num6 == float.MaxValue) ? this.m_defaultBlendTime : num6;
}
break;
}
LayerData data2 = layers[index];
if (controllerVariables.GetValue(data2.LayerId, out num8))
{
maxValue = Math.Min(maxValue, num8);
}
if (controllerVariables.GetValue(data2.LayerBlendTimeId, out num9))
{
num6 = Math.Min(num6, num9);
}
index++;
}
}
double totalMilliseconds = TIMER.ElapsedTimeSpan.TotalMilliseconds;
data.BlendTimeMs = num2 * 1000f;
if (num != data.TargetWeight)
{
data.StartedMs = totalMilliseconds;
BoneData *dataPtr1 = (BoneData *)ref data;
dataPtr1->StartingWeight = (data.PrevWeight == -1f) ? weight : data.PrevWeight;
data.TargetWeight = num;
}
double amount = MathHelper.Clamp((double)((totalMilliseconds - data.StartedMs) / data.BlendTimeMs), (double)0.0, (double)1.0);
weight = (float)MathHelper.Lerp((double)data.StartingWeight, (double)data.TargetWeight, amount);
data.PrevWeight = weight;
this.m_boneIndexToData[bone.Index] = data;
}
}
