public AJointIntImage(JointLinkConstants jls, AJointActive joint) : this() { jls.Verify(); IsAlwaysStiff = jls.IsAlwaysStiff; MinArc = jls.MinArc; MaxArc = jls.MaxArc; LinkLength = jls.LinkLength; Joint = joint; }
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; }
public void SetStiff(int jointIndex, double arc) { AJointActive joint = (AJointActive)joints[jointIndex]; joint.BecomeStiff(arc); }
public void SetArcAcceleration(int jointIndex, double arcAcc) { AJointActive joint = (AJointActive)joints[jointIndex]; joint.BecomeOfConstArcAcceleration(arcAcc); }
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; }
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); }