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