// ------------------------------------------------------------------------------------ /// <summary> /// Solve IK for chain of two bones + change rotation of end bone. /// </summary> /// <param name="characterBones">bone storage</param> /// <param name="ikChain">description of bone chain</param> /// <param name="finalPosition">desired position of end bone</param> /// <param name="finalNormal">desired normal of end bone - would be projected on plane first bone-second bone-third bone</param> /// <param name="fromBindPose">solve this starting from the bind pose</param> /// <returns>true on success</returns> public static bool SolveIkTwoBones(MyCharacterBone[] characterBones, MyAnimationIkChainExt ikChain, ref Vector3 finalPosition, ref Vector3 finalNormal, bool fromBindPose) { int boneIndex = ikChain.BoneIndex; float finalMinRot = MathHelper.ToRadians(ikChain.MinEndPointRotation); float finalMaxRot = MathHelper.ToRadians(ikChain.MaxEndPointRotation); Vector3 lastPoleVector = ikChain.LastPoleVector; int chainLength = ikChain.ChainLength; bool alignBoneWithTerrain = ikChain.AlignBoneWithTerrain; MyCharacterBone thirdBone = characterBones[boneIndex]; if (thirdBone == null) return false; MyCharacterBone secondBone = thirdBone.Parent; for (int i = 2; i < chainLength; i++) { secondBone = secondBone.Parent; } if (secondBone == null) return false; MyCharacterBone firstBone = secondBone.Parent; if (firstBone == null) return false; if (fromBindPose) { firstBone.SetCompleteBindTransform(); secondBone.SetCompleteBindTransform(); thirdBone.SetCompleteBindTransform(); firstBone.ComputeAbsoluteTransform(true); } Matrix thirdBoneTransformBackup = thirdBone.AbsoluteTransform; //Vector3 firstBoneTransformRightDir = firstBone.AbsoluteTransform.Right; Vector3 firstBoneOrigin = firstBone.AbsoluteTransform.Translation; Vector3 secondBoneOrigin = secondBone.AbsoluteTransform.Translation; Vector3 thirdBoneOrigin = thirdBone.AbsoluteTransform.Translation; // Vector3D finalPosition comes from parameter Vector3 secondMinusFirst = secondBoneOrigin - firstBoneOrigin; Vector3 finalMinusFirst = finalPosition - firstBoneOrigin; //Vector3 finalMinusSecond = finalPosition - secondBoneOrigin; Vector3 poleVectorNormalized; Vector3 thirdMinusFirst = thirdBoneOrigin - firstBoneOrigin; Vector3.Cross(ref secondMinusFirst, ref thirdMinusFirst, out poleVectorNormalized); // project to 2D (only vectors) poleVectorNormalized.Normalize(); poleVectorNormalized = Vector3.Normalize(Vector3.Lerp(poleVectorNormalized, lastPoleVector, m_poleVectorChangeSmoothness)); Vector3 planeDirY = Vector3.Normalize(finalMinusFirst); // finalMinusFirst? thirdMinusFirst? Vector3 planeDirX = Vector3.Normalize(Vector3.Cross(planeDirY, poleVectorNormalized)); //Vector2 firstBoneOrigin2D = new Vector2(0, 0); Vector2 secondBoneOrigin2D = new Vector2(planeDirX.Dot(ref secondMinusFirst), planeDirY.Dot(ref secondMinusFirst)); Vector2 thirdBoneOrigin2D = new Vector2(planeDirX.Dot(ref thirdMinusFirst), planeDirY.Dot(ref thirdMinusFirst)); Vector2 finalPosition2D = new Vector2(planeDirX.Dot(ref finalMinusFirst), planeDirY.Dot(ref finalMinusFirst)); Vector2 terrainNormal2D = new Vector2(planeDirX.Dot(ref finalNormal), planeDirY.Dot(ref finalNormal)); float firstBoneLength = (secondBoneOrigin2D/* - 0*/).Length(); float secondBoneLength = (thirdBoneOrigin2D - secondBoneOrigin2D).Length(); float finalDistance = finalPosition2D.Length(); if (firstBoneLength + secondBoneLength <= finalDistance) { finalPosition2D = (firstBoneLength + secondBoneLength) * finalPosition2D / finalDistance; // too far //return false; } // mid-joint -> wanted position in 2D Vector2 newSecondBoneOrigin2D; { newSecondBoneOrigin2D.Y = (finalPosition2D.Y * finalPosition2D.Y - secondBoneLength * secondBoneLength + firstBoneLength * firstBoneLength) / (2.0f * finalPosition2D.Y); float srqtArg = firstBoneLength * firstBoneLength - newSecondBoneOrigin2D.Y * newSecondBoneOrigin2D.Y; newSecondBoneOrigin2D.X = (float)Math.Sqrt(srqtArg > 0 ? srqtArg : 0); } // project back Vector3 newSecondBoneOrigin = firstBoneOrigin + planeDirX * newSecondBoneOrigin2D.X + planeDirY * newSecondBoneOrigin2D.Y; Vector3 newSecondMinusFirst = newSecondBoneOrigin - firstBoneOrigin; Vector3 newThirdMinusSecond = finalPosition - newSecondBoneOrigin; Vector3 newTerrainNormal = planeDirX * terrainNormal2D.X + planeDirY * terrainNormal2D.Y; newTerrainNormal.Normalize(); // set the rotations in the bones // first bone --------------------------------- Matrix firstBoneAbsoluteFinal = firstBone.AbsoluteTransform; Quaternion rotFirstDelta = Quaternion.CreateFromTwoVectors(secondMinusFirst, newSecondMinusFirst); firstBoneAbsoluteFinal.Right = Vector3.Transform(firstBoneAbsoluteFinal.Right, rotFirstDelta); firstBoneAbsoluteFinal.Up = Vector3.Transform(firstBoneAbsoluteFinal.Up, rotFirstDelta); firstBoneAbsoluteFinal.Forward = Vector3.Transform(firstBoneAbsoluteFinal.Forward, rotFirstDelta); firstBone.SetCompleteTransformFromAbsoluteMatrix(ref firstBoneAbsoluteFinal, true); firstBone.ComputeAbsoluteTransform(); // second bone --------------------------------- Matrix secondBoneAbsoluteFinal = secondBone.AbsoluteTransform; Quaternion rotSecondDelta = Quaternion.CreateFromTwoVectors(thirdBone.AbsoluteTransform.Translation - secondBone.AbsoluteTransform.Translation, newThirdMinusSecond); secondBoneAbsoluteFinal.Right = Vector3.Transform(secondBoneAbsoluteFinal.Right, rotSecondDelta); secondBoneAbsoluteFinal.Up = Vector3.Transform(secondBoneAbsoluteFinal.Up, rotSecondDelta); secondBoneAbsoluteFinal.Forward = Vector3.Transform(secondBoneAbsoluteFinal.Forward, rotSecondDelta); secondBone.SetCompleteTransformFromAbsoluteMatrix(ref secondBoneAbsoluteFinal, true); secondBone.ComputeAbsoluteTransform(); //// third bone ---------------------------------- if (ikChain.EndBoneTransform.HasValue) { MatrixD localTransformRelated = ikChain.EndBoneTransform.Value * MatrixD.Invert((MatrixD)thirdBone.BindTransform * thirdBone.Parent.AbsoluteTransform); thirdBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation())); thirdBone.Translation = (Vector3)localTransformRelated.Translation; thirdBone.ComputeAbsoluteTransform(); } else if (alignBoneWithTerrain) { Matrix footRotation; Vector3 finalRotPoleVec; Vector3.Cross(ref newTerrainNormal, ref Vector3.Up, out finalRotPoleVec); float deltaAngle = MyUtils.GetAngleBetweenVectors(newTerrainNormal, Vector3.Up); if (finalRotPoleVec.Dot(poleVectorNormalized) > 0) deltaAngle = -deltaAngle; deltaAngle = MathHelper.Clamp(deltaAngle, finalMinRot, finalMaxRot); Matrix.CreateFromAxisAngle(ref poleVectorNormalized, deltaAngle, out footRotation); ikChain.LastAligningRotationMatrix = Matrix.Lerp(ikChain.LastAligningRotationMatrix, footRotation, ikChain.AligningSmoothness); Matrix thirdBoneAbsoluteFinal = thirdBoneTransformBackup.GetOrientation() * ikChain.LastAligningRotationMatrix; thirdBoneAbsoluteFinal.Translation = thirdBone.AbsoluteTransform.Translation; thirdBone.SetCompleteTransformFromAbsoluteMatrix(ref thirdBoneAbsoluteFinal, true); thirdBone.ComputeAbsoluteTransform(); } // Debugging of the solver. if (m_showDebugDrawings) { MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Color.Yellow, Color.Red, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(thirdBoneOrigin, ref DebugTransform), Color.Yellow, Color.Red, false); MyRenderProxy.DebugDrawSphere(Vector3D.Transform(finalPosition, ref DebugTransform), 0.05f, Color.Cyan, 1.0f, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin + poleVectorNormalized, ref DebugTransform), Color.PaleGreen, Color.PaleGreen, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirX, ref DebugTransform), Color.White, Color.White, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirY, ref DebugTransform), Color.White, Color.White, false); MyRenderProxy.DebugDrawSphere(Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform), 0.05f, Color.Green, 1.0f, false); MyRenderProxy.DebugDrawAxis(firstBone.AbsoluteTransform * DebugTransform, 0.5f, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(finalPosition, ref DebugTransform), Vector3D.Transform(finalPosition + newTerrainNormal, ref DebugTransform), Color.Black, Color.LightBlue, false); MyRenderProxy.DebugDrawArrow3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform), Color.Green, Color.White, false); } ikChain.LastPoleVector = poleVectorNormalized; return true; }
// ------------------------------------------------------------------------------------ /// <summary> /// Solve IK for chain of two bones + change rotation of end bone. /// </summary> /// <param name="characterBones">bone storage</param> /// <param name="ikChain">description of bone chain</param> /// <param name="finalPosition">desired position of end bone</param> /// <param name="finalNormal">desired normal of end bone - would be projected on plane first bone-second bone-third bone</param> /// <param name="fromBindPose">solve this starting from the bind pose</param> /// <returns>true on success</returns> public static bool SolveIkTwoBones(MyCharacterBone[] characterBones, MyAnimationIkChainExt ikChain, ref Vector3 finalPosition, ref Vector3 finalNormal, bool fromBindPose) { int boneIndex = ikChain.BoneIndex; float finalMinRot = MathHelper.ToRadians(ikChain.MinEndPointRotation); float finalMaxRot = MathHelper.ToRadians(ikChain.MaxEndPointRotation); Vector3 lastPoleVector = ikChain.LastPoleVector; int chainLength = ikChain.ChainLength; bool alignBoneWithTerrain = ikChain.AlignBoneWithTerrain; MyCharacterBone thirdBone = characterBones[boneIndex]; if (thirdBone == null) { return(false); } MyCharacterBone secondBone = thirdBone.Parent; for (int i = 2; i < chainLength; i++) { secondBone = secondBone.Parent; } if (secondBone == null) { return(false); } MyCharacterBone firstBone = secondBone.Parent; if (firstBone == null) { return(false); } if (fromBindPose) { firstBone.SetCompleteBindTransform(); secondBone.SetCompleteBindTransform(); thirdBone.SetCompleteBindTransform(); firstBone.ComputeAbsoluteTransform(true); } Matrix thirdBoneTransformBackup = thirdBone.AbsoluteTransform; //Vector3 firstBoneTransformRightDir = firstBone.AbsoluteTransform.Right; Vector3 firstBoneOrigin = firstBone.AbsoluteTransform.Translation; Vector3 secondBoneOrigin = secondBone.AbsoluteTransform.Translation; Vector3 thirdBoneOrigin = thirdBone.AbsoluteTransform.Translation; // Vector3D finalPosition comes from parameter Vector3 secondMinusFirst = secondBoneOrigin - firstBoneOrigin; Vector3 finalMinusFirst = finalPosition - firstBoneOrigin; //Vector3 finalMinusSecond = finalPosition - secondBoneOrigin; Vector3 poleVectorNormalized; Vector3 thirdMinusFirst = thirdBoneOrigin - firstBoneOrigin; Vector3.Cross(ref secondMinusFirst, ref thirdMinusFirst, out poleVectorNormalized); // project to 2D (only vectors) poleVectorNormalized.Normalize(); poleVectorNormalized = Vector3.Normalize(Vector3.Lerp(poleVectorNormalized, lastPoleVector, m_poleVectorChangeSmoothness)); Vector3 planeDirY = Vector3.Normalize(finalMinusFirst); // finalMinusFirst? thirdMinusFirst? Vector3 planeDirX = Vector3.Normalize(Vector3.Cross(planeDirY, poleVectorNormalized)); //Vector2 firstBoneOrigin2D = new Vector2(0, 0); Vector2 secondBoneOrigin2D = new Vector2(planeDirX.Dot(ref secondMinusFirst), planeDirY.Dot(ref secondMinusFirst)); Vector2 thirdBoneOrigin2D = new Vector2(planeDirX.Dot(ref thirdMinusFirst), planeDirY.Dot(ref thirdMinusFirst)); Vector2 finalPosition2D = new Vector2(planeDirX.Dot(ref finalMinusFirst), planeDirY.Dot(ref finalMinusFirst)); Vector2 terrainNormal2D = new Vector2(planeDirX.Dot(ref finalNormal), planeDirY.Dot(ref finalNormal)); float firstBoneLength = (secondBoneOrigin2D /* - 0*/).Length(); float secondBoneLength = (thirdBoneOrigin2D - secondBoneOrigin2D).Length(); float finalDistance = finalPosition2D.Length(); if (firstBoneLength + secondBoneLength <= finalDistance) { finalPosition2D = (firstBoneLength + secondBoneLength) * finalPosition2D / finalDistance; // too far //return false; } // mid-joint -> wanted position in 2D Vector2 newSecondBoneOrigin2D; { newSecondBoneOrigin2D.Y = (finalPosition2D.Y * finalPosition2D.Y - secondBoneLength * secondBoneLength + firstBoneLength * firstBoneLength) / (2.0f * finalPosition2D.Y); float srqtArg = firstBoneLength * firstBoneLength - newSecondBoneOrigin2D.Y * newSecondBoneOrigin2D.Y; newSecondBoneOrigin2D.X = (float)Math.Sqrt(srqtArg > 0 ? srqtArg : 0); } // project back Vector3 newSecondBoneOrigin = firstBoneOrigin + planeDirX * newSecondBoneOrigin2D.X + planeDirY * newSecondBoneOrigin2D.Y; Vector3 newSecondMinusFirst = newSecondBoneOrigin - firstBoneOrigin; Vector3 newThirdMinusSecond = finalPosition - newSecondBoneOrigin; Vector3 newTerrainNormal = planeDirX * terrainNormal2D.X + planeDirY * terrainNormal2D.Y; newTerrainNormal.Normalize(); // set the rotations in the bones // first bone --------------------------------- Matrix firstBoneAbsoluteFinal = firstBone.AbsoluteTransform; Quaternion rotFirstDelta = Quaternion.CreateFromTwoVectors(secondMinusFirst, newSecondMinusFirst); firstBoneAbsoluteFinal.Right = Vector3.Transform(firstBoneAbsoluteFinal.Right, rotFirstDelta); firstBoneAbsoluteFinal.Up = Vector3.Transform(firstBoneAbsoluteFinal.Up, rotFirstDelta); firstBoneAbsoluteFinal.Forward = Vector3.Transform(firstBoneAbsoluteFinal.Forward, rotFirstDelta); firstBone.SetCompleteTransformFromAbsoluteMatrix(ref firstBoneAbsoluteFinal, true); firstBone.ComputeAbsoluteTransform(); // second bone --------------------------------- Matrix secondBoneAbsoluteFinal = secondBone.AbsoluteTransform; Quaternion rotSecondDelta = Quaternion.CreateFromTwoVectors(thirdBone.AbsoluteTransform.Translation - secondBone.AbsoluteTransform.Translation, newThirdMinusSecond); secondBoneAbsoluteFinal.Right = Vector3.Transform(secondBoneAbsoluteFinal.Right, rotSecondDelta); secondBoneAbsoluteFinal.Up = Vector3.Transform(secondBoneAbsoluteFinal.Up, rotSecondDelta); secondBoneAbsoluteFinal.Forward = Vector3.Transform(secondBoneAbsoluteFinal.Forward, rotSecondDelta); secondBone.SetCompleteTransformFromAbsoluteMatrix(ref secondBoneAbsoluteFinal, true); secondBone.ComputeAbsoluteTransform(); //// third bone ---------------------------------- if (ikChain.EndBoneTransform.HasValue) { MatrixD localTransformRelated = ikChain.EndBoneTransform.Value * MatrixD.Invert((MatrixD)thirdBone.BindTransform * thirdBone.Parent.AbsoluteTransform); thirdBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation())); thirdBone.Translation = (Vector3)localTransformRelated.Translation; thirdBone.ComputeAbsoluteTransform(); } else if (alignBoneWithTerrain) { Matrix footRotation; Vector3 finalRotPoleVec; Vector3.Cross(ref newTerrainNormal, ref Vector3.Up, out finalRotPoleVec); float deltaAngle = MyUtils.GetAngleBetweenVectors(newTerrainNormal, Vector3.Up); if (finalRotPoleVec.Dot(poleVectorNormalized) > 0) { deltaAngle = -deltaAngle; } deltaAngle = MathHelper.Clamp(deltaAngle, finalMinRot, finalMaxRot); Matrix.CreateFromAxisAngle(ref poleVectorNormalized, deltaAngle, out footRotation); ikChain.LastAligningRotationMatrix = Matrix.Lerp(ikChain.LastAligningRotationMatrix, footRotation, ikChain.AligningSmoothness); Matrix thirdBoneAbsoluteFinal = thirdBoneTransformBackup.GetOrientation() * ikChain.LastAligningRotationMatrix; thirdBoneAbsoluteFinal.Translation = thirdBone.AbsoluteTransform.Translation; thirdBone.SetCompleteTransformFromAbsoluteMatrix(ref thirdBoneAbsoluteFinal, true); thirdBone.ComputeAbsoluteTransform(); } // Debugging of the solver. if (m_showDebugDrawings) { MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Color.Yellow, Color.Red, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(thirdBoneOrigin, ref DebugTransform), Color.Yellow, Color.Red, false); MyRenderProxy.DebugDrawSphere(Vector3D.Transform(finalPosition, ref DebugTransform), 0.05f, Color.Cyan, 1.0f, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(secondBoneOrigin + poleVectorNormalized, ref DebugTransform), Color.PaleGreen, Color.PaleGreen, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirX, ref DebugTransform), Color.White, Color.White, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(firstBoneOrigin, ref DebugTransform), Vector3D.Transform(firstBoneOrigin + planeDirY, ref DebugTransform), Color.White, Color.White, false); MyRenderProxy.DebugDrawSphere(Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform), 0.05f, Color.Green, 1.0f, false); MyRenderProxy.DebugDrawAxis(firstBone.AbsoluteTransform * DebugTransform, 0.5f, false); MyRenderProxy.DebugDrawLine3D(Vector3D.Transform(finalPosition, ref DebugTransform), Vector3D.Transform(finalPosition + newTerrainNormal, ref DebugTransform), Color.Black, Color.LightBlue, false); MyRenderProxy.DebugDrawArrow3D(Vector3D.Transform(secondBoneOrigin, ref DebugTransform), Vector3D.Transform(newSecondBoneOrigin, ref DebugTransform), Color.Green, Color.White, false); } ikChain.LastPoleVector = poleVectorNormalized; return(true); }