public void Translate(AVector2D delta) { for (int i = 0; i < JointNo; i++) { jI[i].Joint.x += delta; } }
private double axMaxDeltaV; // max discrepancy of speed caused #endregion #region Construction public A2DWalker() : base() { axMaxDeltaV = 1; RealTime = 0; ZeroThreshold = 0.01; Zero = new AVector2D(0, 0); }
private void PrvPostCalculate() { if (NextJoint != null) { AVector2D rl = NextJoint.x - x; NextLink.r = rl / rl.Length; NextLink.L = rl.TurnedLeft() / rl.LengthSq; } }
protected AVector2D[] SaveAx() { AVector2D[] ret = new AVector2D[JointNo]; for (int i = 0; i < JointNo; i++) { ret[i] = Joints[i].a; } return(ret); }
public AVector2D[] GetAccelerations() { AVector2D[] accelerations = new AVector2D[JointNo]; int i = 0; for (AJoint joint = First; joint != null; joint = joint.NextJoint) { accelerations[i++] = joint.a; } return(accelerations); }
public AVector2D[] GetPositions() { AVector2D[] positions = new AVector2D[JointNo]; int i = 0; for (AJoint joint = First; joint != null; joint = joint.NextJoint) { positions[i++] = joint.x; } return(positions); }
public void PutInPosition(AVector2D x, AVector2D r1, double[] arcs) { Contract.Requires(arcs.Length == JointNo - 2); Joints[0].x = x; Joints[0].v.FillWith(0); Joints[0].a.FillWith(0); Joints[0].NextLink.r = r1.Direction; double arc = (new AVector2D(1, 0)).AngleTo(r1); x += Joints[0].NextLink.r * jI[0].LinkLength; int i; for (i = 1; i < JointNo - 1; i++) { Joints[i].x = x; Joints[i].v.FillWith(0); Joints[i].a.FillWith(0); arc += Math.Min(Math.Max(jI[i].MinArc, arcs[i - 1]), jI[i].MaxArc); Joints[i].NextLink.r.x = Math.Cos(arc); Joints[i].NextLink.r.y = Math.Sin(arc); x += Joints[i].NextLink.r * jI[i].LinkLength; } for (i = 1; i < JointNo - 1; i++) { if (jI[i].IsAlwaysStiff) { jI[i].Joint.BecomeStiff(Joints[i].GetArc()); } else { jI[i].Joint.BecomeNoStiff(); } } Joints[i].x = x; Joints[i].v.FillWith(0); Joints[i].a.FillWith(0); for (i = 0; i < JointNo; i++) { jI[i].Joint.BecomeNoLed(); jI[i].Joint.PostCalculate(); jI[i].ArcActivity = jI[i].ArcLocation = 0; jI[i].IsGrounded = jI[i].IsStanding = jI[i].IsPlunging = jI[i].IsClenching = false; } UpdateInternalImages(true, true, true); axSet = false; }
public AVector2D[] GetVelocities() { AVector2D[] velocities = new AVector2D[JointNo]; int i = 0; for (AJoint joint = First; joint != null; joint = joint.NextJoint) { velocities[i++] = joint.v; } return(velocities); }
public double[] GetLinkArcs() { double[] arcs = new double[JointNo - 1]; int i = 0; AVector2D reference = new AVector2D(1, 0); for (AJoint joint = First; joint.NextJoint != null; joint = joint.NextJoint) { arcs[i++] = reference.AngleTo(joint.NextLink.r); } return(arcs); }
public void UpdateFeTorch(double tau) { if (PrevJoint != null) { PrevJoint.Fe += PrevLink.r.TurnedLeft() * (tau / PrevLink.l); Fe += PrevLink.r.TurnedRight() * (tau / PrevLink.l); } if (NextJoint != null) { Fe += NextLink.r.TurnedRight() * (tau / NextLink.l); NextJoint.Fe += NextLink.r.TurnedLeft() * (tau / NextLink.l); } }
private bool PrvCalculateForward() { if (PrevJoint == null) { Fd.FillWith(0); } a = (A * Fd) + b; if (NextJoint != null) { NextJoint.Fd = (C * Fd) + d; } return(true); }
private void InitJointActive() { RegisterDelegates( (bool impact) => PrvCalculateBackwards(impact), () => PrvCalculateForward(), null /*post calculate*/); BecomeNoOfConstArcAcceleration(); BecomeNoStiff(); Bw = new AVector2D(); Dw = new AVector2D(); P = new AVector2D(); Qw = 0; Q = 0; G = new AVector2D(); Hw = 0; H = 0; }
public void Init( bool is_standing, AVector2D x, AVector2D v, AVector2D a, double tauW, AVector2D r, double linkArcVelocity, int activity) { IsStanding = is_standing; this.X = x.Clone(); this.V = v.Clone(); this.A = a.Clone(); this.TauW = tauW; this.R = r != null?r.Clone() : null; this.LinkArcVelocity = linkArcVelocity; this.Activity = activity; }
public double AngleTo(AVector2D vector2D) { double l_prod = Math.Sqrt((this * this) * (vector2D * vector2D)); double sin = (this.x * vector2D.y - this.y * vector2D.x) / l_prod; double cos = (this.x * vector2D.x + this.y * vector2D.y) / l_prod; if (sin >= 0.7) { // 0.7<sqrt(0.5)~=0.707 return(Math.Acos(cos)); } else { if (cos >= 0.7) { return(Math.Asin(sin)); } else { if (sin <= -0.7) { return(-Math.Acos(cos)); } else { if (sin < 0) { return(-Math.PI - Math.Asin(sin)); } else { // if (sin>=0 && cos<=-0.7) return(Math.PI - Math.Asin(sin)); } } } } }
private void InitJoint() { delegatesBackwards = new List <Func <bool, bool> >(); delegatesForward = new List <Func <bool> >(); delegatesPostCalculate = new List <Action>(); RegisterDelegates( (bool impact) => PrvCalculateBackwards(impact), () => PrvCalculateForward(), () => PrvPostCalculate()); NumericError = 0.00003; CompensationTime = 0.01; m = 0; aW = null; x = new AVector2D(); v = new AVector2D(); Fe = new AVector2D(); Fd = new AVector2D(); a = new AVector2D(); A = new AMatrix2x2(); b = new AVector2D(); C = new AMatrix2x2(); d = new AVector2D(); }
public void BecomeNoLed() { aW = null; }
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; }
public void SetLead(int jointIndex, AVector2D a_w) { joints[jointIndex].BecomeLed(a_w); }
private bool PrvCalculateBackwards(bool impact) { if (IsLed && (IsEnding || NextJoint.IsLed)) { Variant = "1.1"; A.FillWith(0); b = GetLeadingAcc(); C.FillWith(0); d.FillWith(0); } else { if (IsLed) { Variant = "1.2"; double e = GetCentripetalAcc(impact); AVector2D r = NextLink.r; A.FillWith(0); b = GetLeadingAcc(); double scale = r * (NextJoint.A * r); C.FillWith(0); if (scale.Squared() > NumericError.Squared()) { Variant = "1.2.1"; d = r * ((r * GetLeadingAcc() - r * NextJoint.b - e) / scale); } else { Variant = "1.2.2"; d.FillWith(0); } } else { if (IsEnding) //// ruchome zlacze na koncu ukladu { Variant = "1.3"; A = AMatrix2x2.I() / m; b = Fe / m; C.FillWith(0); d.FillWith(0); } else //// zywkle ruchome zlacze w srodku ukladu { Variant = "1.4"; double e = GetCentripetalAcc(impact); AVector2D r = NextLink.r; AMatrix2x2 rrT = (r | r); double scala = 1.0 + m * r * NextJoint.A * r; A = AMatrix2x2.I() / m - rrT / (m * scala); b = A * Fe + r * ((r * NextJoint.b + e) / scala); C = rrT / scala; d = C * Fe - r * (m * (r * NextJoint.b + e) / scala); } } } return(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); }
public void BecomeLed(AVector2D aW) { this.aW = aW; }
public ALink(double l) { this.l = l; r = L = new AVector2D(0, 0); }