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