Exemplo n.º 1
0
        public bool Compute()
        {
            double targetW     = 20;
            double continuityW = tangentContinuity * 10;
            double stiffnessW  = 50;

            double[,] Js = ThetaFromJob(paramCount);

            Q_opt = Maths.Add(Maths.Add(Maths.Multiply(2d * targetW, Maths.Multiply(Maths.Transpose(Js), Js)), Maths.Add(Maths.Multiply(2d * stiffnessW, Stiffnes_D),
                                                                                                                         Maths.Multiply(2d * continuityW, Continuity_T))), Maths.Multiply((double)Mathf.Pow(10, -6), Maths.Identity(paramCount)));

            double[,] B_opt = Maths.Add(Maths.Multiply(-2d * targetW, Maths.Multiply(Maths.Transpose(Js), Delta_s_prime)), Maths.Multiply(2d * continuityW, Maths.Multiply(Continuity_T, Theta)));
            b_opt           = Maths.ArrayToColumnArray(B_opt);

            alglib.minqpstate  state_opt;
            alglib.minqpreport rep;

            alglib.minqpcreate(paramCount, out state_opt);
            alglib.minqpsetquadraticterm(state_opt, Q_opt);
            alglib.minqpsetlinearterm(state_opt, b_opt);
            alglib.minqpsetstartingpoint(state_opt, delta_theta_0);
            alglib.minqpsetbc(state_opt, lowerBound, upperBound);

            alglib.minqpsetscale(state_opt, scale);

            alglib.minqpsetalgobleic(state_opt, 0.0, 0.0, 0.0, 0);
            alglib.minqpoptimize(state_opt);
            alglib.minqpresults(state_opt, out delta_theta, out rep);

            return(true);
        }
Exemplo n.º 2
0
        public bool Setup()
        {
            // rotation curves * hierarchy size + root position curves
            p     = 3 * hierarchySize + 3;
            theta = GetAllValues(p);

            currentState = new State()
            {
                position = MeshController.transform.InverseTransformPoint(oTransform.position),
                rotation = oTransform.rotation
            };
            desiredState = new State()
            {
                position = MeshController.transform.InverseTransformPoint(targetPosition),
                rotation = targetRotation
            };

            double[,] Js = ds_dtheta(p);

            double[,] DT_D = new double[p, p];
            //Root rotation
            for (int i = 0; i < 3; i++)
            {
                DT_D[i, i] = 1d;
            }
            //nonRoot rotation
            for (int i = 3; i < (hierarchySize - 1) * 3; i++)
            {
                if (fullHierarchy[i / 3].TryGetComponent <RigGoalController>(out RigGoalController controller))
                {
                    DT_D[i, i] = controller.stiffness;
                }
                else
                {
                    DT_D[i, i] = 1d;
                }
            }
            //root position
            for (int i = 0; i < 3; i++)
            {
                int j = 3 * hierarchySize + i;
                if (fullHierarchy[0].TryGetComponent <RigGoalController>(out RigGoalController controller))
                {
                    DT_D[i, i] = controller.stiffness;
                }
                else
                {
                    DT_D[j, j] = 1d;
                }
            }

            double[,] Delta_s_prime = new double[7, 1];
            for (int i = 0; i < 3; i++)
            {
                Delta_s_prime[i, 0] = desiredState.position[i] - currentState.position[i];
            }
            if ((currentState.rotation * Quaternion.Inverse(desiredState.rotation)).w < 0)
            {
                desiredState.rotation = new Quaternion(-desiredState.rotation.x, -desiredState.rotation.y, -desiredState.rotation.z, -desiredState.rotation.w);
            }
            for (int i = 0; i < 4; i++)
            {
                Delta_s_prime[i + 3, 0] = desiredState.rotation[i] - currentState.rotation[i];
            }

            double wm = 20;
            double wd = 50;

            Q_opt = Maths.Add(Maths.Add(Maths.Multiply(2d * wm, Maths.Multiply(Maths.Transpose(Js), Js)), Maths.Multiply(2d * wd, DT_D)), Maths.Multiply((double)Mathf.Pow(10, -6), Maths.Identity(p)));

            double[,] B_opt = Maths.Multiply(-2d * wm, Maths.Multiply(Maths.Transpose(Js), Delta_s_prime));
            b_opt           = Maths.ArrayToColumnArray(B_opt);

            lowerBound = InitializeUBound(p);
            upperBound = InitializeVBound(p);

            delta_theta_0 = new double[p];

            s = new double[p];
            for (int i = 0; i < p; i++)
            {
                s[i]             = 1d;
                delta_theta_0[i] = 0d;
            }

            return(true);
        }
Exemplo n.º 3
0
        public bool TrySolver()
        {
            ObjectAnimation.curves[AnimatableProperty.PositionX].GetKeyIndex(startFrame, out int firstIndex);
            int firstFrame = ObjectAnimation.curves[AnimatableProperty.PositionX].keys[firstIndex].frame;

            ObjectAnimation.curves[AnimatableProperty.PositionX].GetKeyIndex(endFrame, out int lastIndex);
            int lastFrame = ObjectAnimation.curves[AnimatableProperty.PositionX].keys[lastIndex].frame;

            if (currentFrame < firstFrame)
            {
                return(false);
            }
            if (currentFrame > lastFrame)
            {
                return(false);
            }


            RequiredKeyframeIndices = FindRequiredTangents(firstFrame, lastFrame, ObjectAnimation.GetCurve(AnimatableProperty.PositionX));
            int K = RequiredKeyframeIndices.Count;
            int totalKeyframes = ObjectAnimation.GetCurve(AnimatableProperty.PositionX).keys.Count;
            int n = 6;
            int p = 24 * K;

            double[] theta = GetAllTangents(p, K, RequiredKeyframeIndices);
            double[,] Theta = ColumnArrayToArray(theta);
            State currentState = GetCurrentState(currentFrame);
            State desiredState = new State()
            {
                position          = positionTarget,
                euler_orientation = rotationTarget.eulerAngles,
                time = currentFrame
            };


            double[,] Js   = ds_dtheta(currentFrame, n, p, K, RequiredKeyframeIndices);
            double[,] DT_D = new double[p, p];
            for (int i = 0; i < p; i++)
            {
                DT_D[i, i] = 0d * 0d;
            }

            double[,] Delta_s_prime = new double[6, 1];
            for (int i = 0; i <= 2; i++)
            {
                Delta_s_prime[i, 0] = desiredState.position[i] - currentState.position[i];
            }
            for (int i = 3; i <= 5; i++)
            {
                Delta_s_prime[i, 0] = -Mathf.DeltaAngle(desiredState.euler_orientation[i - 3], currentState.euler_orientation[i - 3]);
            }

            double[,] TT_T = new double[p, p];
            for (int j = 0; j < p; j++)
            {
                TT_T[j, j] = 1d;
                if (j % 4 == 0 || j % 4 == 1)
                {
                    TT_T[j + 2, j] = -1d;
                }
                else
                {
                    TT_T[j - 2, j] = -1d;
                }
            }
            double wm = 100d;
            double wb = tangentEnergy;
            double wd = 1d;

            double[,] Q_opt = Maths.Add(Maths.Add(Maths.Multiply(2d * wm, Maths.Multiply(Maths.Transpose(Js), Js)), Maths.Add(Maths.Multiply(2d * wd, DT_D), Maths.Multiply(2d * wb, TT_T))), Maths.Multiply((double)Mathf.Pow(10, -6), Maths.Identity(p)));

            double[,] B_opt = Maths.Add(Maths.Multiply(-2d * wm, Maths.Multiply(Maths.Transpose(Js), Delta_s_prime)), Maths.Multiply(2d * wb, Maths.Multiply(TT_T, Theta)));
            double[] b_opt = Maths.ArrayToColumnArray(B_opt);

            double[] delta_theta_0 = new double[p];
            double[] delta_theta;
            double[] s = new double[p];
            for (int i = 0; i < p; i++)
            {
                s[i]             = 1d;
                delta_theta_0[i] = 0d;
            }

            alglib.minqpstate  state_opt;
            alglib.minqpreport rep;

            alglib.minqpcreate(p, out state_opt);
            alglib.minqpsetquadraticterm(state_opt, Q_opt);
            alglib.minqpsetlinearterm(state_opt, b_opt);
            alglib.minqpsetstartingpoint(state_opt, delta_theta_0);

            alglib.minqpsetscale(state_opt, s);

            alglib.minqpsetalgobleic(state_opt, 0.0, 0.0, 0.0, 0);
            alglib.minqpoptimize(state_opt);
            alglib.minqpresults(state_opt, out delta_theta, out rep);


            double[] new_theta = new double[p];
            for (int i = 0; i < p; i++)
            {
                new_theta[i] = delta_theta[i] + theta[i];
            }

            for (int i = 0; i < p; i++)
            {
                if (System.Double.IsNaN(delta_theta[i]))
                {
                    return(false);
                }
            }

            for (int i = 0; i < 6; i++)
            {
                AnimatableProperty property = (AnimatableProperty)i;
                Curve curve = ObjectAnimation.curves[property];

                for (int k = 0; k < K; k++)
                {
                    Vector2 inTangent  = new Vector2((float)new_theta[4 * (i * K + k) + 0], (float)new_theta[4 * (i * K + k) + 1]);
                    Vector2 outTangent = new Vector2((float)new_theta[4 * (i * K + k) + 2], (float)new_theta[4 * (i * K + k) + 3]);
                    ModifyTangents(curve, RequiredKeyframeIndices[k], inTangent, outTangent);
                }
            }
            return(true);
        }
Exemplo n.º 4
0
 double[,] ds_dtheta(int frame, int n, int p, int K, List <int> requiredKeyframes)
 {
     double[,] Js1 = ds_dc(frame, n);
     double[,] Js2 = dc_dtheta(frame, n, p, K, requiredKeyframes);
     return(Maths.Multiply(Js1, Js2));
 }