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);
}
