array4 getDummy(array4 para)
{
array4 ABs = getABs(para);
double g1 = (ABs.a3 - ABs.a1 * ABs.a4) / (1 - ABs.a1 * ABs.a2);
double g2 = (ABs.a4 - ABs.a2 * ABs.a3) / (1 - ABs.a1 * ABs.a2);
return(new array4(para.a3, para.a4, g1, g2));
}
array4 getNxtRungeKutta(array4 y)
{
array4 k1 = getDummy(y);
array4 k2 = getDummy(y + k1 * (double)(setup.dt / 2.0));
array4 k3 = getDummy(y + k2 * (double)(setup.dt / 2.0));
array4 k4 = getDummy(y + k3 * (double)setup.dt);
return((k1 + k2 * 2.0 + k3 * 2.0 + k4) * (setup.dt / 6.0));
}
array4 getNxtNaive(array4 y)
{
array4 vs = getDummy(y);
double g1 = vs.a3;
double g2 = vs.a4;
double w1 = vs.a1;
double w2 = vs.a2;
double dw1 = g1 * setup.dt;
double dw2 = g2 * setup.dt;
double dt1 = setup.dt * (w1 + dw1 / 2.0);
double dt2 = setup.dt * (w2 + dw2 / 2.0);
return(new array4(dt1, dt2, dw1, dw2));
}
array4 getABs(array4 para)
{
double t1d = para.a1;
double t2d = para.a2;
double w1d = para.a3;
double w2d = para.a4;
double A1 = setup.l2 / setup.l1 * (setup.m2 / (setup.m1 + setup.m2)) * Math.Cos(t1d - t2d);
double A2 = setup.l1 / setup.l2 * Math.Cos(t1d - t2d);
double B1 = -setup.l2 / setup.l1 * Math.Pow(w2d, 2.0) * (setup.m2 / (setup.m1 + setup.m2)) * Math.Sin(t1d - t2d) - setup.g / setup.l1 * Math.Sin(t1d);
double B2 = setup.l1 / setup.l2 * Math.Pow(w1d, 2.0) * Math.Sin(t1d - t2d) - setup.g / setup.l2 * Math.Sin(t2d);
return(new array4(A1, A2, B1, B2));
}
void Update()
{
if (!setup.bool_Paused)
{
for (int i = 0; i < setup.steps; i++)
{
array4 R = getNxtRungeKutta(new array4(t1, t2, w1, w2));
t1 += R.a1;
t2 += R.a2;
w1 += R.a3;
w2 += R.a4;
}
}
rod1.rotation = Quaternion.Euler(0, 0, (float)t1 * Mathf.Rad2Deg);
rod2.rotation = Quaternion.Euler(0, 0, (float)t2 * Mathf.Rad2Deg);
}
