Пример #1
0
 public AJointIntImage(JointLinkConstants jls, AJointActive joint)
     : this()
 {
     jls.Verify();
     IsAlwaysStiff = jls.IsAlwaysStiff;
     MinArc        = jls.MinArc;
     MaxArc        = jls.MaxArc;
     LinkLength    = jls.LinkLength;
     Joint         = joint;
 }
Пример #2
0
        protected void StopArcMovement(int index)
        {
            AJointActive ith_joint = (AJointActive)Joints[index];

            jI[index].ArcActivity = (int)Math.Sign(-jI[index].ArcVelocity);
            jI[index].IsClenching = true;
            ith_joint.BecomeOfConstArcAcceleration(-jI[index].ArcVelocity);
            ImpactCore();
            ith_joint.BecomeNoOfConstArcAcceleration();
            jI[index].IsClenching = false;
            jI[index].ArcActivity = 0;
        }
Пример #3
0
        public void SetStiff(int jointIndex, double arc)
        {
            AJointActive joint = (AJointActive)joints[jointIndex];

            joint.BecomeStiff(arc);
        }
Пример #4
0
        public void SetArcAcceleration(int jointIndex, double arcAcc)
        {
            AJointActive joint = (AJointActive)joints[jointIndex];

            joint.BecomeOfConstArcAcceleration(arcAcc);
        }
Пример #5
0
        protected void ComputeAccelerations(bool gravitation_on, double[] taus, bool impact)
        {
            Calculate2ndDerivatives(gravitation_on, taus, impact);
            UpdateInternalImages(false, false, true);
            int  iteration  = 0;
            bool once_again = true;

            while (once_again)
            {
                if (++iteration > 30)
                {
                    throw new InvalidOperationException("Shuffling");
                }

                once_again = false;

                // ewentualnie odrywamy od ziemi i od ograniczeń na kąty
                for (int i = 0; i < JointNo; i++)
                {
                    // weryfikacja, czy nie odrywa sie od gleby
                    if (!jI[i].IsPlunging && jI[i].IsStanding && iteration < JointNo && jI[i].NoNotStanding < 4)
                    {
                        AVector2D Fi = jI[i].Joint.Fe + jI[i].Joint.Fd;
                        if (i != JointNo - 1)
                        {
                            Fi -= Joints[i + 1].Fd;
                        }

                        if (Fi.y > ZeroThreshold)
                        {
                            jI[i].IsStanding = false;
                            jI[i].Joint.BecomeNoLed();
                            Calculate2ndDerivatives(gravitation_on, taus, impact);
                            UpdateInternalImages(false, false, true);
                            if (jI[i].Joint.a.y < 0)
                            {
                                jI[i].IsStanding = true;
                                jI[i].Joint.BecomeLed(Zero);
                                Calculate2ndDerivatives(gravitation_on, taus, impact);
                                UpdateInternalImages(false, false, true);
                            }
                            else
                            {
                                once_again = true;
                                break;
                            }
                        }
                    }

                    // weryfikacja czy nie odrywa się od ograniczenia na kąt
                    if (!jI[i].IsAlwaysStiff && !jI[i].IsClenching && jI[i].ArcActivity != 0 && iteration < JointNo && jI[i].NoNotActive < 4)
                    {
                        AJointActive joint = (AJointActive)Joints[i];
                        if (joint.TauW * jI[i].ArcActivity < -ZeroThreshold)
                        {
                            jI[i].ArcActivity = 0;
                            jI[i].Joint.BecomeNoStiff();
                            Calculate2ndDerivatives(gravitation_on, taus, impact);
                            UpdateInternalImages(false, false, true);
                            if (jI[i].ArcAcceleration * jI[i].ArcLocation > 0)
                            {
                                jI[i].ArcActivity = -jI[i].ArcLocation;
                                jI[i].Joint.BecomeStiff(jI[i].GetArcBound());
                                Calculate2ndDerivatives(gravitation_on, taus, impact);
                                UpdateInternalImages(false, false, true);
                            }
                            else
                            {
                                once_again = true;
                                break;
                            }
                        }
                    }

                    // czy aby jednak nie stoi
                    if (!jI[i].IsPlunging && jI[i].IsGrounded && !jI[i].IsStanding)
                    {
                        if (jI[i].Joint.a.y < 0)
                        {
                            jI[i].IsStanding = true;
                            jI[i].Joint.BecomeLed(new AVector2D(0, 0));
                            Calculate2ndDerivatives(gravitation_on, taus, impact);
                            UpdateInternalImages(false, false, true);
                            once_again = true;
                            break;
                        }
                    }

                    // czy aby ograniczenie nie powinno zadziałać
                    if (!jI[i].IsAlwaysStiff && !jI[i].IsClenching && jI[i].ArcLocation != 0 && jI[i].ArcActivity == 0)
                    {
                        if (jI[i].ArcLocation * jI[i].ArcAcceleration > 0)
                        {
                            jI[i].ArcActivity = -jI[i].ArcLocation;
                            jI[i].Joint.BecomeStiff(jI[i].GetArcBound());
                            Calculate2ndDerivatives(gravitation_on, taus, impact);
                            UpdateInternalImages(false, false, true);
                            once_again = true;
                            break;
                        }
                    }
                }
            }

            for (int i = 0; i < JointNo; i++)
            {
                if (jI[i].ArcLocation != 0 && jI[i].ArcActivity == 0)
                {
                    jI[i].ArcLocation = 0;
                    jI[i].NoNotActive++;
                }

                if (jI[i].IsGrounded && !jI[i].IsStanding)
                {
                    jI[i].IsGrounded = false;
                    jI[i].NoNotStanding++;
                }
            }

            axSet = true;
        }
Пример #6
0
        private bool PrvCalculateBackwards(bool impact) // prawda jesli przejmuje odpowiedzialnosc za obliczenia.
        {
            Bw.FillWith(0);
            Dw.FillWith(0);
            P.FillWith(0);
            G.FillWith(0);
            Qw = Q = Hw = H = 0;

            AJointActive NJ = (!IsEnding && NextJoint.IsActivable) ? (AJointActive)NextJoint : null;

            if (IsEnding || (!IsActive && !NJ.IsActive))
            {
                return(false);
            }
            else
            if (IsLed && NextJoint.IsActive && !NextJoint.IsEnding)
            {
                #region złącze prowadzone i następne aktywne
                Variant = "3.1";
                double     e   = GetCentripetalAcc(impact);
                AVector2D  r   = NextLink.r;
                AVector2D  L   = NextLink.L;
                AMatrix2x2 A_1 = NextJoint.A;
                AVector2D  b_1 = NextJoint.b;

                double k0 = -r * (A_1 * r);
                double k1 = r * (A_1 * L) - r * NJ.Bw;
                double k3 = -r * (A_1 * L);
                double k4 = r * GetLeadingAcc() - r * b_1 - e;

                double k5 = NJ.P * r - L * (A_1 * r);
                double k6 = -NJ.P * L + NJ.Qw + L * (A_1 * L) - L * NJ.Bw;
                double k8 = NJ.P * L - L * (A_1 * L);
                double k9 = NJ.Q - L * b_1 + L * GetLeadingAcc() - NJ.GetProperArcAcceleration(impact);

                double k11 = k5 * k1 - k0 * k6;
                if (Math.Abs(k11) < NumericError * 2)
                {
                    // it is impossible to determine both force and torch
                    Variant = "3.1.1";
                    double eq1 = Math.Abs(k0) + Math.Abs(k1);
                    double eq2 = Math.Abs(k5) + Math.Abs(k6);
                    if (eq1 < NumericError || eq1 < eq2)
                    {
                        Variant = "3.1.1.1";
                        k0      = 1;
                        k1      = k3 = k4 = 0;
                    }

                    if (eq2 < NumericError || eq2 <= eq1)
                    {
                        Variant = "3.1.1.2";
                        k6      = 1;
                        k5      = k8 = k9 = 0;
                    }

                    k11 = k5 * k1 - k0 * k6;
                }

                double k14 = (k6 * k3 - k1 * k8) / k11;
                double k15 = (k0 * k8 - k5 * k3) / k11;
                double k16 = (k4 * k6 - k1 * k9) / k11;
                double k17 = (k0 * k9 - k5 * k4) / k11;

                G.FillWith(0);
                Hw = k15;
                H  = k17;

                C.FillWith(0);
                Dw = r * k14 + L - L * k15;
                d  = r * k16 - L * k17;

                A.FillWith(0);
                Bw.FillWith(0);
                b = GetLeadingAcc();

                P.FillWith(0);
                Qw = L * A_1 * Dw + L * NJ.Bw * Hw;
                Q  = L * A_1 * d + L * NJ.Bw * H + L * NJ.b - L * b;
                #endregion
            }
            else
            if (IsLed)
            {
                #region złącze prowadzone i następne nieaktywne
                Variant = "3.2";
                double     e   = GetCentripetalAcc(impact);
                AVector2D  r   = NextLink.r;
                AVector2D  L   = NextLink.L;
                AMatrix2x2 A_1 = NextJoint.A;
                AVector2D  b_1 = NextJoint.b;

                double scala = r * (A_1 * r);

                if (Math.Abs(scala) > NumericError)
                {           // jeśli się da, to wywrzej wpływ na i+1-sze złącze
                    Variant = "3.2.1";
                    C.FillWith(0);
                    Dw = L - (r * (r * (A_1 * L) / scala));
                    d  = r * (r * (GetLeadingAcc() - b_1) - e) / scala;
                }
                else
                {           // ale jeśli się nie da to odpuść.
                    Variant = "3.2.2";
                    C.FillWith(0);
                    Dw.FillWith(0);
                    d.FillWith(0);
                }

                A.FillWith(0);
                Bw.FillWith(0);
                b = GetLeadingAcc();

                P.FillWith(0);
                Qw = L * A_1 * Dw - L * Bw;
                Q  = L * A_1 * d + L * b_1 - L * b;
                #endregion
            }
            else
            if (!NextJoint.IsActive || NextJoint.IsEnding)
            {
                #region złącze nieprowadzone i następne nieaktywne
                Variant = "3.3";
                double     e   = GetCentripetalAcc(impact);
                AVector2D  r   = NextLink.r;
                AVector2D  L   = NextLink.L;
                AMatrix2x2 A_1 = NextJoint.A;
                AVector2D  b_1 = NextJoint.b;

                double scala = r * (A_1 * r) * m + 1;

                C  = r | r / scala;
                Dw = L - r * (r * (A_1 * L) * m / scala);
                d  = r * (r * Fe - (r * b_1 + e) * m) / scala;

                A  = (AMatrix2x2.I() - C) / m;
                Bw = Dw / -m;
                b  = (Fe - d) / m;

                P  = L * A_1 * C - L * A;
                Qw = L * A_1 * Dw - L * Bw;
                Q  = L * A_1 * d + L * b_1 - L * b;
                #endregion
            }
            else
            {
                #region aktywne zlacze w srodku ukladu
                Variant = "3.4";
                double     e   = GetCentripetalAcc(impact);
                AVector2D  r   = NextLink.r;
                AVector2D  L   = NextLink.L;
                AMatrix2x2 A_1 = NextJoint.A;
                AVector2D  b_1 = NextJoint.b;

                double    k0 = 1.0 / -m - r * (A_1 * r);
                double    k1 = r * (A_1 * L) - r * NJ.Bw;
                AVector2D k2 = r / m;
                double    k3 = -r * (A_1 * L);
                double    k4 = r * Fe / m - r * b_1 - e;

                double    k5 = NJ.P * r - L * (A_1 * r);
                double    k6 = -NJ.P * L + NJ.Qw + L * (A_1 * L) - L * NJ.Bw + L.LengthSq / m;
                AVector2D k7 = L / m;
                double    k8 = NJ.P * L - L * (A_1 * L) - L.LengthSq / m;
                double    k9 = NJ.Q - L * b_1 + L * Fe / m - NJ.GetProperArcAcceleration(impact);

                double    k11 = k5 * k1 - k0 * k6;
                AVector2D k12 = (k6 * k2 - k1 * k7) / k11;
                AVector2D k13 = (k0 * k7 - k5 * k2) / k11;
                double    k14 = (k6 * k3 - k1 * k8) / k11;
                double    k15 = (k0 * k8 - k5 * k3) / k11;
                double    k16 = (k4 * k6 - k1 * k9) / k11;
                double    k17 = (k0 * k9 - k5 * k4) / k11;

                G  = k13;
                Hw = k15;
                H  = k17;

                C  = (r | k12) - (L | k13);
                Dw = r * k14 + L - L * k15;
                d  = r * k16 - L * k17;

                A  = (AMatrix2x2.I() - C) / m;
                Bw = Dw / -m;
                b  = (Fe - d) / m;

                P  = L * A_1 * C + L * NJ.Bw * G - L * A;
                Qw = L * A_1 * Dw + L * NJ.Bw * Hw - L * Bw;
                Q  = L * A_1 * d + L * NJ.Bw * H + L * NJ.b - L * b;
                #endregion
            }

            return(true);
        }