Exemplo n.º 1
0
        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);
        }
Exemplo n.º 2
0
        /// <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);
        }
Exemplo n.º 3
0
 internal void AddChild(MyCharacterBone child)
 {
     m_children.Add(child);
 }
Exemplo n.º 4
0
 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();
 }
Exemplo n.º 5
0
        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;
        }
Exemplo n.º 7
0
        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);
        }
Exemplo n.º 8
0
        /// <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();
        }
Exemplo n.º 9
0
        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();
        }
Exemplo n.º 10
0
        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;
        }
Exemplo n.º 11
0
        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();
        }
Exemplo n.º 12
0
 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();        
 }
Exemplo n.º 15
0
        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);
        }
Exemplo n.º 16
0
        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);
                }
            }
        }
Exemplo n.º 17
0
        /// <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;
        }