From a9a7677ca861d4a63d94f746b7052ce124401ed9 Mon Sep 17 00:00:00 2001 From: Branan Riley Date: Wed, 29 Oct 2025 16:39:03 -0700 Subject: [PATCH] more code --- lib/switch.ks | 220 +++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 217 insertions(+), 3 deletions(-) diff --git a/lib/switch.ks b/lib/switch.ks index 448bc35..b60c334 100644 --- a/lib/switch.ks +++ b/lib/switch.ks @@ -1,5 +1,5 @@ local function iterate { - parameter maneuver. + parameter maneuver, no. // `maneuver` is a lexicon of: // "mass" - vehicle mass at start of maneuver // "mu" - gravitational constant @@ -27,7 +27,8 @@ local function iterate { } for iter in RANGE(itmax) { - local q0 is list(u0:x, u0:y, y0:z, du0:x, du0:y, du0:z). + local q0 is list(u0:x, u0:y, u0:z, du0:x, du0:y, du0:z). + local q01 is list(u0:x, u0:y, u0:z, du0:x, du0:y, dy0:z). local x0 is list(r0:x, r0:y, r0:z, v0:x, v0:y, v0:z). local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). local qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). @@ -150,13 +151,71 @@ local function iterate { set qf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). } // burn + // Called to propagate burn arcs + // x0(q0) - state(costate) at start of burn + // xf(qf) - state(costate) at end of burn + rkg031(x0, q0, xf, qf, z, evt, hmax, maneuver, no). + if leg < (num_legs-1) { + local um is sqrt(qf[0]*qf[0]+qf[1]*qf[1]+qf[2]*qf[2]). + set ump to um. + local cbmu is burn:thrust / (mass * um). + for i in range(3) { + set z[i+3][leg6] to cbmu * qf[i]. + } + for i in range(leg7) { + for k in range(3) { + set e[leg6][j] to e[leg6][j] + (qf[k]/um)*z[k+6][j]. + } + } + } + if leg <> 0 { + local r2 is xf[0]*xf[0] + xf[1]*xf[1] + xf[2]*xf[2]. + local rs is xf[0]*qf[0] + xf[1]*qf[1] + xf[2]*qf[2]. + local c3 is -uk / (r2 * sqrt(r2)). + local c4 is -3.0 * c3 * rs / r2. + set e[leg5][l7] to xf[3]*qf[3]+xf[4]*qf[4]+xf[5]*qf[5] - c3*rs - e[leg5][l7]. + set dummy to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0). + for i in range(3) { + set dummy[i] to -c3*qf[i]-c4*xf[i]. + set dummy[i+3] to qf[i+3]. + set dummy[i+6] to - c3 * xf[i]. + set dummy[i+9] to xf[i+3]. + } + for j in range(leg7) { + for k in range(12) { + set e[leg5][j] to -dummy[k]*z[k][j] + e[leg5][j]. + } + } + } set leg to leg + 1. set leg5 to leg + 5. set leg6 to leg + 6. set leg7 to leg + 7. set prev_burn to burn. + set x0 to xf. + set q0 to qf. + set xf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). + set qf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). } + // bveval(xf, qf, z, c, e, dc). // When we have convergeence, break + for i in range(6) { + local i6 is i+6. + set e[i][l7] to dc[i]. + set dc[i+6] to e[i+7][l7]. + set e[leg6][i] to q01[i]. + } + if no { + // called to solve simultaneous equations and determine new initial + // costate and switching times. Q01- initial costate + // adjust(q01, e, z, du, dudt, legmax, atp). + + if du <0.0001 or dudt < 0.001 { + set no to true. + } + } else { + break. + } } } @@ -242,6 +301,151 @@ local function coast { return. } // calculation of partials + set dukr3 to -3.0*ukr3*drm/rm. + set dur03 to -3.0*ukr03*drm0/rm0. + set s1r0 to s1/rm0. + set ds1r0 to (ds1-s1r0*drm0)/rm0. + set s1r to s1/rm. + set ds1r to (ds1-s1r*drm)/rm. + set r02 to 1.0/(rm0*rm0). + set r2 to 1.0/(rm*rm). + set uuk03 to -u*ukr03. + set duuk3 to -du*ukr03-u*dur03. + + local ann is list( + list( + -fd*s1r0 - fm1*r02, + -fd*s2 + ), + list( + fm1*s1r0 + uuk03, + fm1*s2 + ) + ). + local dumm1 is ann[0][0]. + local dan is list( + list( + -fd*ds1r0 - dfd*s1r0 + ukr03*ds2 + dur03*s2, + -dfd*s2 - fd*ds2 + ), + list( + fm1*ds1r0 + df*s1r0 + duuk3, + fm1*ds2 + df*s2 + ) + ). + local dummy is dan[0][0]. + local bnn is list(). + local dbnn is list(). + for i in range(3) { + bnn:add(list()). + dbnn:add(list()). + for j in range(3) { + bnn[i]:add(0.0). + dbnn[i]:add(0.0). + } + } + + amult(ann, dan, bnn, dbnn, x0, q0, xf, qf). + for i in range(3) { + for j in range(3) { + set dphi[i][j] to dbnn[i][j]. + set phi[i][j] to bnn[i][j]. + } + } + for i in range(3) { + set dphi[i][i] to dphi[i][i] + df. + set phi[i][i] to phi[i][i] + f. + } + + set ann[0][0] to ann[0][1]. + set ann[1][0] to ann[1][1]. + set ann[0][1] to -gdm1*ds2 - dgd*s2. + set ann[1][1] to g*s2 - u. + set dan[0][0] to dan[0][1]. + set dan[1][0] to dan[1][1]. + set dan[0][1] to -gdm1*ds2 - dgd*s2. + set dan[1][1] to -du + dg*s2 + g*ds2. + + amult(ann, dan, bnn, dbnn, x0, q0, xf, qf). + for i in range(3) { + for j in range(3) { + local j3 is j+3. + set dphi[i][j3] to dbnn[i][j]. + set phi[i][j3] to bnn[i][j]. + } + } + for i in range(3) { + local i3 is i + 3. + set dphi[i][i3] to dphi[i][i3] + dg. + set phi[i][i3] to phi[i][i3] + g. + } + + set ann[1][0] to -ann[0][0]. + set ann[1][1] to -ann[0][1]. + set ann[0][0] to -fd*s1r - gdm1*r2. + set ann[0][1] to u*ukr3 - gdm1*s1r. + set dan[1][0] to -dan[0][0]. + set dan[1][1] to -dan[0][1]. + set dan[0][0] to -dfd*s1r -fd*ds1r + ukr3*ds2 + dukr3*s2. + set dan[0][1] to -gdm1*ds1r - dgd*s1r + du*ukr3 + u*dukr3. + + amult(ann, dan, bnn, dbnn, x0, q0, xf, qf). + for i in range(3) { + local i3 is i+3. + for j in range(3) { + local j3 is j+3. + set dphi[i3][j3] to dbnn[i][j]. + set phi[i3][j3] to bnn[i][j]. + } + } + for i in range(3) { + local i3 is i + 3. + set dphi[i3][i3] to dphi[i][i3] + dgd. + set phi[i3][i3] to phi[i][i3] + gd. + } + + set ann[0][1] to ann[0][0]. + set ann[1][1] to ann[1][0]. + set ann[1][0] to -dumm1. + set ann[0][0] to -fd*(s0/r01 + r2 + r02) - uuk03*ukr3. + set dan[0][1] to dan[0][0]. + set dan[1][1] to dan[1][0]. + set dan[1][0] to -dummy. + set dan[0][0] to -uuk03*dukr3 - duuk3*ukr3 - dfd*(s0/r01 + r2 + r02) - fd*((ds0 - s0*dr01/r01)/r01 - 2.0*(r2*drm/rm + r02*drm0/rm0)). + + amult(ann, dan, bnn, dbnn, x0, q0, xf, qf). + for i in range(3) { + local i3 is i+3. + for j in range(3) { + set dphi[i3][j] to dbnn[i][j]. + set phi[i3][j] to bnn[i][j]. + } + } + for i in range(3) { + local i3 is i + 3. + set dphi[i3][i] to dphi[i][i3] + dfd. + set phi[i3][i] to phi[i][i3] + fd. + } +} + +local function amult { + parameter ann, dan, bnn, dbnn, x0, q0, xf, qf. + local da is list(). + local a is list(). + for i in range(3) { + da:add(list()). + a:add(list()). + for j in range(2) { + da[i]:add(qf[i]*ann[0][j] + qf[i+3]*ann[1][j] + xf[i]*dan[0][j] + xf[i+3]*dan[1][j]). + a[i]:add(ann[0][j]*xf[i] + ann[1][j]*xf[i+3]). + } + } + for i in range(3) { + for j in range(3) { + set dbnn[i][j] to a[i][0]*q0[j] + a[i][1]*q0[j+3] + da[i][0]*x0[j] + da[i][1]*x0[j+3]. + set bnn[i][j] to a[i][0]*x0[j] + a[i][1]*x0[j+3]. + } + } } local function test_coast { @@ -251,7 +455,17 @@ local function test_coast { local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). local qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). local t is 2047.6868 - 934. - coast(x0, q0, xf, qf, list(), list(), uk, t, false). + local phi is list(). + local dphi is list(). + for i in range(6) { + phi:add(list()). + dphi:add(list()). + for j in range(6) { + phi[i]:add(0.0). + dphi[i]:add(0.0). + } + } + coast(x0, q0, xf, qf, phi, dphi, uk, t, true). print xf. print qf. }