#help_index "Math/ODE"
#help_file "::/Doc/ODE"

//See ::/Doc/Credits.DD.

F64 LowPass1(F64 a, F64 y0, F64 y, F64 dt=1.0)
{//First order low pass filter
        dt = Exp(-a * dt);
        return y0 * dt + y * (1.0 - dt);
}

U0 ODEResetPtrs(CMathODE *ode)
{
        I64  s = ode->n_internal * sizeof(F64);
        F64 *ptr = ode->array_base;

        ode->state_internal = ptr;      ptr(I64) += s;
        ode->state_scale = ptr;         ptr(I64) += s;
        ode->DstateDt = ptr;            ptr(I64) += s;
        ode->initial_state = ptr;       ptr(I64) += s;
        ode->tmp0 = ptr;                        ptr(I64) += s;
        ode->tmp1 = ptr;                        ptr(I64) += s;
        ode->tmp2 = ptr;                        ptr(I64) += s;
        ode->tmp3 = ptr;                        ptr(I64) += s;
        ode->tmp4 = ptr;                        ptr(I64) += s;
        ode->tmp5 = ptr;                        ptr(I64) += s;
        ode->tmp6 = ptr;                        ptr(I64) += s;
        ode->tmp7 = ptr;
}

public CMathODE *ODENew(I64 n, F64 max_tolerance=1e-6, I64 flags=0)
{//Make differential equation ctrl struct. See flags.
        //The tolerance is not precise.
        //You can min_tolerance and it will
        //dynamically adjust tolerance to utilize
        //the CPU.
        I64                      s = n * sizeof(F64);
        CMathODE        *ode = CAlloc(sizeof(CMathODE));

        ode->t_scale = 1.0;
        ode->flags = flags;
        ode->n_internal = ode->n = n;
        ode->h = 1e-6;
        ode->h_min = 1e-64;
        ode->h_max = 1e32;
        ode->max_tolerance = ode->min_tolerance = ode->tolerance_internal = max_tolerance;
        ode->win_task = ode->mem_task = Fs;
        QueueInit(&ode->next_mass);
        QueueInit(&ode->next_spring);
        ode->state = CAlloc(s);
        ode->array_base = MAlloc(12 * s);
        ODEResetPtrs(ode);

        return ode;
}


public Bool ODEPause(CMathODE *ode, Bool val=ON)
{//Pause ODE.
        Bool res;

        if (!ode)
                return OFF;
        res = LBEqual(&ode->flags, ODEf_PAUSED, val);
        if (val)
                while (Bt(&ode->flags, ODEf_BUSY))
                        Yield;

        return res;
}

public U0 ODEDel(CMathODE *ode)
{//Free ODE node, but not masses or springs.
        I64 i;

        if (!ode)
                return;
        ODEPause(ode);
        Free(ode->state);
        Free(ode->array_base);
        if (ode->slave_tasks)
        {
                for (i = 0; i < mp_count; i++)
                        Kill(ode->slave_tasks[i]);
                Free(ode->slave_tasks);
        }
        Free(ode);
}

public I64 ODESize(CMathODE *ode)
{//Mem size of ode ctrl, but not masses and springs.
        if (!ode)
                return 0;
        else
                return MSize2(ode->state) + MSize2(ode->array_base) + MSize2(ode);
}

U0 ODESetMassesPtrs(CMathODE *ode, F64 *state, F64 *DstateDt)
{
        COrder2D3       *ptr1 = state(F64 *) + ode->n, *ptr2 = DstateDt(F64 *) + ode->n;
        CMass           *tmpm = ode->next_mass;

        while (tmpm != &ode->next_mass)
        {
                tmpm->state = ptr1++;
                tmpm->DstateDt = ptr2++;
                tmpm = tmpm->next;
        }
}

U0 ODEState2Internal(CMathODE *ode)
{
        CMass   *tmpm;
        F64             *old_array_base;
        I64              mass_count;

        if (ode->flags & ODEF_HAS_MASSES)
        {
                mass_count = 0;
                tmpm = ode->next_mass;
                while (tmpm != &ode->next_mass)
                {
                        mass_count++;
                        tmpm = tmpm->next;
                }
                old_array_base = ode->array_base;
                ode->n_internal = ode->n + 6 * mass_count;
                ode->array_base = MAlloc(12 * ode->n_internal * sizeof(F64), ode->mem_task);
                Free(old_array_base);
                ODEResetPtrs(ode);

                ODESetMassesPtrs(ode, ode->state_internal, ode->state_internal);
                tmpm = ode->next_mass;
                while (tmpm != &ode->next_mass)
                {
                        MemCopy(tmpm->state, &tmpm->saved_state, sizeof(COrder2D3));
                        tmpm = tmpm->next;
                }
        }
        MemCopy(ode->state_internal, ode->state, ode->n * sizeof(F64));
}

U0 ODEInternal2State(CMathODE *ode)
{
        CMass *tmpm;

        MemCopy(ode->state, ode->state_internal, ode->n * sizeof(F64));
        if (ode->flags & ODEF_HAS_MASSES)
        {
                ODESetMassesPtrs(ode, ode->state_internal, ode->state_internal);
                tmpm = ode->next_mass;
                while (tmpm != &ode->next_mass)
                {
                        MemCopy(&tmpm->saved_state, tmpm->state, sizeof(COrder2D3));
                        tmpm = tmpm->next;
                }
        }
}

public U0 ODERenum(CMathODE *ode)
{//Renumber masses and springs.
        I64              i;
        CSpring *tmps;
        CMass   *tmpm;

        i = 0;
        tmpm = ode->next_mass;
        while (tmpm != &ode->next_mass)
        {
                tmpm->num = i++;
                tmpm = tmpm->next;
        }

        i = 0;
        tmps = ode->next_spring;
        while (tmps != &ode->next_spring)
        {
                tmps->num = i++;
                tmps->end1_num = tmps->end1->num;
                tmps->end2_num = tmps->end2->num;
                tmps = tmps->next;
        }
}

public CMass *MassFind(CMathODE *ode, F64 x, F64 y, F64 z=0)
{//Search for mass nearest to x,y,z.
        CMass   *tmpm, *best_mass = NULL;
        F64              dd, best_dd = F64_MAX;

        tmpm = ode->next_mass;
        while (tmpm != &ode->next_mass)
        {
                dd = Sqr(tmpm->x - x) + Sqr(tmpm->y - y) + Sqr(tmpm->z - z);
                if (dd < best_dd)
                {
                        best_dd = dd;
                        best_mass = tmpm;
                }
                tmpm = tmpm->next;
        }

        return best_mass;
}

public CSpring *SpringFind(CMathODE *ode, F64 x, F64 y, F64 z=0)
{//Find spring midpoint nearest x,y,z.
        CSpring *tmps, *best_spring = NULL;
        F64              dd, best_dd = F64_MAX;

        tmps = ode->next_spring;
        while (tmps != &ode->next_spring)
        {
                dd = Sqr((tmps->end1->x + tmps->end2->x) / 2 - x) +
                         Sqr((tmps->end1->y + tmps->end2->y) / 2 - y) +
                         Sqr((tmps->end1->z + tmps->end2->z) / 2 - z);

                if (dd < best_dd)
                {
                        best_dd = dd;
                        best_spring = tmps;
                }
                tmps = tmps->next;
        }

        return best_spring;
}

public U0 MassOrSpringFind(CMathODE *ode, CMass **res_mass, CSpring **res_spring, F64 x, F64 y, F64 z=0)
{//Find spring or mass nearest x,y,z.
        CMass   *tmpm, *best_mass = NULL;
        CSpring *tmps, *best_spring = NULL;
        F64             dd, best_dd = F64_MAX;

        tmpm = ode->next_mass;
        while (tmpm != &ode->next_mass)
        {
                dd = Sqr(tmpm->x - x) + Sqr(tmpm->y - y) + Sqr(tmpm->z - z);
                if (dd < best_dd)
                {
                        best_dd = dd;
                        best_mass = tmpm;
                }
                tmpm = tmpm->next;
        }

        tmps = ode->next_spring;
        while (tmps != &ode->next_spring)
        {
                dd = Sqr((tmps->end1->x + tmps->end2->x) / 2 - x) +
                         Sqr((tmps->end1->y + tmps->end2->y) / 2 - y) +
                         Sqr((tmps->end1->z + tmps->end2->z) / 2 - z);

                if (dd < best_dd)
                {
                        best_dd = dd;
                        best_spring = tmps;
                        best_mass = NULL;
                }
                tmps = tmps->next;
        }
        if (res_mass)
                *res_mass = best_mass;
        if (res_spring)
                *res_spring = best_spring;
}

public CMass *MassFindNum(CMathODE *ode, I64 num)
{//Return mass number N.
        CMass *tmpm = ode->next_mass;

        while (tmpm != &ode->next_mass)
        {
                if (tmpm->num == num)
                        return tmpm;
                tmpm = tmpm->next;
        }

        return NULL;
}

public U0 ODEResetInactive(CMathODE *ode)
{//Set all masses and springs to ACTIVE for new trial.
        CMass   *tmpm;
        CSpring *tmps;

        tmpm = ode->next_mass;
        while (tmpm != &ode->next_mass)
        {
                tmpm->flags &= ~MSF_INACTIVE;
                tmpm = tmpm->next;
        }
        tmps = ode->next_spring;
        while (tmps != &ode->next_spring)
        {
                tmps->flags &= ~SSF_INACTIVE;
                tmps = tmps->next;
        }
}

U0 ODECalcSprings(CMathODE *ode)
{
        CSpring *tmps = ode->next_spring;
        CMass   *e1, *e2;
        F64              d;
        CD3              p;

        while (tmps != &ode->next_spring)
        {
                if (tmps->flags & SSF_INACTIVE)
                {
                        tmps->displacement = 0;
                        tmps->f = 0;
                }
                else
                {
                        e1 = tmps->end1;
                        e2 = tmps->end2;
                        d = D3Norm(D3Sub(&p, &e2->state->x, &e1->state->x));
                        tmps->displacement = d - tmps->rest_len;
                        tmps->f = tmps->displacement * tmps->const;
                        if (tmps->f > 0 && tmps->flags & SSF_NO_TENSION)
                                tmps->f = 0;
                        else if (tmps->f < 0 && tmps->flags & SSF_NO_COMPRESSION)
                                tmps->f = 0;
                        if (d > 0)
                        {
                                D3MulEqu(&p, tmps->f / d);
                                D3AddEqu(&e1->DstateDt->DxDt, &p);
                                D3SubEqu(&e2->DstateDt->DxDt, &p);
                        }
                }
                tmps = tmps->next;
        }
}

U0 ODECalcDrag(CMathODE *ode)
{
        CMass   *tmpm;
        F64              d, dd;
        CD3              p;

        if (ode->drag_v || ode->drag_v2 || ode->drag_v3)
        {
                tmpm = ode->next_mass;
                while (tmpm != &ode->next_mass)
                {
                        if (!(tmpm->flags & MSF_INACTIVE) && tmpm->drag_profile_factor && (dd = D3NormSqr(&tmpm->state->DxDt)))
                        {
                                d = ode->drag_v;
                                if (ode->drag_v2)
                                        d += ode->drag_v2 * Sqrt(dd);
                                if (ode->drag_v3)
                                        d += dd * ode->drag_v3;
                                D3SubEqu(&tmpm->DstateDt->DxDt, D3Mul(&p, d * tmpm->drag_profile_factor, &tmpm->state->DxDt));
                        }
                        tmpm = tmpm->next;
                }
        }
}

U0 ODEApplyAccelerationLimit(CMathODE *ode)
{
        CMass   *tmpm;
        F64              d;

        if (ode->acceleration_limit)
        {
                tmpm = ode->next_mass;
                while (tmpm != &ode->next_mass)
                {
                        if (!(tmpm->flags & MSF_INACTIVE) && (d = D3Norm(&tmpm->DstateDt->DxDt)) > ode->acceleration_limit)
                                D3MulEqu(&tmpm->DstateDt->DxDt, ode->acceleration_limit / d);
                        tmpm = tmpm->next;
                }
        }
}

U0 ODEMPTask(CMathODE *ode)
{
        while (TRUE)
        {
                while (!Bt(&ode->mp_not_done_flags, Gs->num))
                        Yield;
                if (ode->mp_derive)
                        (*ode->mp_derive)(ode, ode->mp_t, Gs->num, ode->mp_state, ode->mp_DstateDt);
                LBtr(&ode->mp_not_done_flags, Gs->num);
        }
}

U0 ODEMPWake(CMathODE *ode)
{
        I64 i;

        if (!ode->slave_tasks)
        {
                ode->slave_tasks = CAlloc(mp_count * sizeof(CTask *));
                for (i = 0; i < mp_count; i++)
                        ode->slave_tasks[i] = Spawn(&ODEMPTask, ode, "ODE Slave", i);
        }
        for (i = 0; i < mp_count; i++)
        {
                Suspend(ode->slave_tasks[i], FALSE);
                MPInt(I_WAKE, i);
        }
}

U0 ODEMPSleep(CMathODE *ode)
{
        I64 i;

        if (ode->slave_tasks)
        {
                while (ode->mp_not_done_flags)
                        Yield;
                for (i = 0; i < mp_count; i++)
                        Suspend(ode->slave_tasks[i]);
        }
}

U0 ODECallMPDerivative(CMathODE *ode, F64 t, F64 *state, F64 *DstateDt)
{
        ode->mp_t                               = t;
        ode->mp_state                   = state;
        ode->mp_DstateDt                = DstateDt;
        ode->mp_not_done_flags  = 1 << mp_count - 1;
        do
                Yield;
        while (ode->mp_not_done_flags);
}

U0 ODECallDerivative(CMathODE *ode, F64 t, F64 *state, F64 *DstateDt)
{
        CMass *tmpm;

        if (ode->flags & ODEF_HAS_MASSES)
        {
                ODESetMassesPtrs(ode, state, DstateDt);
                tmpm = ode->next_mass;
                while (tmpm != &ode->next_mass)
                {
                        if (!(tmpm->flags & MSF_INACTIVE))
                        {
                                D3Zero(&tmpm->DstateDt->DxDt);
                                D3Copy(&tmpm->DstateDt->x, &tmpm->state->DxDt);
                        }
                        tmpm = tmpm->next;
                }
                ODECalcSprings(ode);
                ODECalcDrag(ode);
                if (ode->mp_derive)
                        ODECallMPDerivative(ode, t, state, DstateDt);
                if (ode->derive)
                        (*ode->derive)(ode, t, state, DstateDt);
                tmpm = ode->next_mass;
                while (tmpm != &ode->next_mass)
                {
                        if (!(tmpm->flags & MSF_INACTIVE))
                        {
                                if (tmpm->flags & MSF_FIXED)
                                {
                                        D3Zero(&tmpm->DstateDt->DxDt);
                                        D3Zero(&tmpm->DstateDt->x);
                                }
                                else if (tmpm->mass)
                                        D3DivEqu(&tmpm->DstateDt->DxDt, tmpm->mass);
                        }
                        tmpm = tmpm->next;
                }
                ODEApplyAccelerationLimit(ode);
        }
        else
        {
                if (ode->mp_derive)
                        ODECallMPDerivative(ode, t, state, DstateDt);
                if (ode->derive)
                        (*ode->derive)(ode, t, state, DstateDt);
        }
}

U0 ODEOneStep(CMathODE *ode)
{
        I64 i;

        ODECallDerivative(ode, ode->t, ode->state_internal, ode->DstateDt);
        for (i = 0; i < ode->n_internal; i++)
                ode->state_internal[i] += ode->h * ode->DstateDt[i];
        ode->t += ode->h;
}

U0 ODERK4OneStep(CMathODE *ode)
{
        I64 i, n = ode->n_internal;
        F64 xh, hh, h6, *dym, *dyt, *yt, *DstateDt;

        dym = ode->tmp0;
        dyt = ode->tmp1;
        yt      = ode->tmp2;
        DstateDt = ode->tmp3;
        hh      = 0.5 * ode->h;
        h6      = ode->h / 6.0;
        xh      = ode->t + hh;

        ODECallDerivative(ode, ode->t, ode->state_internal, ode->DstateDt);
        for (i = 0; i < n; i++)
                yt[i] = ode->state_internal[i] + hh * DstateDt[i];
        ODECallDerivative(ode, xh, yt, dyt);
        for (i = 0; i < n; i++)
                yt[i] = ode->state_internal[i] + hh * dyt[i];
        ODECallDerivative(ode, xh, yt, dym);
        for (i = 0; i < n; i++)
        {
                yt[i] = ode->state_internal[i] + ode->h * dym[i];
                dym[i] += dyt[i];
        }
        ode->t += ode->h;
        ODECallDerivative(ode, ode->t, yt, dyt);
        for (i = 0; i < n; i++)
                ode->state_internal[i] += h6 * (DstateDt[i] + dyt[i] + 2.0 * dym[i]);
}

#define ODEa2 0.2
#define ODEa3 0.3
#define ODEa4 0.6
#define ODEa5 1.0
#define ODEa6 0.875
#define ODEb21 0.2
#define ODEb31 (3.0 / 40.0)
#define ODEb32 (9.0 / 40.0)
#define ODEb41 0.3
#define ODEb42 (-0.9)
#define ODEb43 1.2
#define ODEb51 (-11.0 / 54.0)
#define ODEb52 2.5
#define ODEb53 (-70.0 / 27.0)
#define ODEb54 (35.0 / 27.0)
#define ODEb61 (1631.0 / 55296.0)
#define ODEb62 (175.0 / 512.0)
#define ODEb63 (575.0 / 13824.0)
#define ODEb64 (44275.0 / 110592.0)
#define ODEb65 (253.0 / 4096.0)
#define ODEc1  (37.0 / 378.0)
#define ODEc3  (250.0 / 621.0)
#define ODEc4  (125.0 / 594.0)
#define ODEc6  (512.0 / 1771.0)
#define ODEdc1 (37.0 / 378.0 - 2825.0 / 27648.0)
#define ODEdc3 (250.0 / 621.0 - 18575.0 / 48384.0)
#define ODEdc4 (125.0 / 594.0 - 13525.0 / 55296.0)
#define ODEdc5 (-277.0 / 14336.0)
#define ODEdc6 (512.0 / 1771.0 - 0.25)

U0 ODECashKarp(CMathODE *ode)
{
        I64 i, n = ode->n_internal;
        F64 h = ode->h, *state = ode->state_internal, *DstateDt = ode->DstateDt, *ak2, *ak3, *ak4, *ak5, *ak6, *tmpstate, *stateerr, *outstate;

        ak2 = ode->tmp0;
        ak3 = ode->tmp1;
        ak4 = ode->tmp2;
        ak5 = ode->tmp3;
        ak6 = ode->tmp4;
        tmpstate = ode->tmp5;
        outstate = ode->tmp6;
        stateerr = ode->tmp7;

        for (i = 0; i < n; i++)
                tmpstate[i] = state[i] + ODEb21 * h * DstateDt[i];
        ODECallDerivative(ode, ode->t + ODEa2 * h, tmpstate, ak2);
        for (i = 0; i < n; i++)
                tmpstate[i] =state[i] + h * (ODEb31 * DstateDt[i] + ODEb32 * ak2[i]);
        ODECallDerivative(ode, ode->t + ODEa3 * h, tmpstate, ak3);
        for (i = 0; i < n; i++)
                tmpstate[i] = state[i] + h * (ODEb41 * DstateDt[i] + ODEb42 * ak2[i] + ODEb43 * ak3[i]);
        ODECallDerivative(ode, ode->t + ODEa4 * h, tmpstate, ak4);
        for (i = 0; i < n; i++)
                tmpstate[i] = state[i] + h * (ODEb51 * DstateDt[i] + ODEb52 * ak2[i] + ODEb53 * ak3[i] + ODEb54*ak4[i]);
        ODECallDerivative(ode, ode->t + ODEa5 * h, tmpstate,ak5);
        for (i = 0; i < n; i++)
                tmpstate[i] = state[i] + h * (ODEb61 * DstateDt[i]+ ODEb62 * ak2[i] + ODEb63 * ak3[i] + ODEb64 * ak4[i] + ODEb65 * ak5[i]);
        ODECallDerivative(ode, ode->t + ODEa6 * h, tmpstate, ak6);

        for (i = 0; i < n; i++)
                outstate[i] = state[i] + h * (ODEc1 * DstateDt[i] + ODEc3 * ak3[i] + ODEc4 * ak4[i] + ODEc6 * ak6[i]);
        for (i = 0; i < n; i++)
                stateerr[i] = h * (ODEdc1 * DstateDt[i] + ODEdc3 * ak3[i] + ODEdc4 * ak4[i] + ODEdc5 * ak5[i] + ODEdc6 * ak6[i]);
}

#define SAFETY 0.9
#define PGROW  (-0.2)
#define PSHRNK (-0.25)
#define ERRCON 1.89e-4

U0 ODERK5OneStep(CMathODE *ode)
{
        I64 i;
        F64 errmax, tmp, *tmpstate = ode->tmp6, *stateerr = ode->tmp7;

        while (TRUE)
        {
                ode->h = Clamp(ode->h, ode->h_min, ode->h_max);
                ODECashKarp(ode);
                errmax = 0.0;
                for (i = 0; i < ode->n_internal; i++)
                {
                        tmp = Abs(stateerr[i] / ode->state_scale[i]);
                        if (tmp > errmax)
                                errmax = tmp;
                }
                errmax /= ode->tolerance_internal;
                if (errmax <= 1.0 || ode->h == ode->h_min)
                        break;
                tmp = ode->h * SAFETY * errmax ` PSHRNK;
                if (tmp < 0.1 * ode->h)
                        ode->h *= 0.1;
                else
                        ode->h = tmp;
        }
        ode->t += ode->h;
        if (errmax > ERRCON)
                ode->h *= SAFETY * errmax ` PGROW;
        else
                ode->h *= 5.0;
        ode->h = Clamp(ode->h, ode->h_min, ode->h_max);
        MemCopy(ode->state_internal, tmpstate, sizeof(F64) * ode->n_internal);
}

F64 ode_alloced_factor = 0.75;

U0 ODEsUpdate(CTask *task)
{/* This routine is called by the window mgron a continuous
basis to allow real-time simulation.  It is intended
to provide ress good enough for games.  It uses a runge-kutta
integrator which is a better algorithm than doing it with Euler.

It is adaptive step-sized, so it slows down when an important
event is taking place to improve accuracy, but in this implementation
it has a timeout.
*/
        I64                      i;
        F64                      d, start_time, timeout_time, t_desired, t_initial, interpolation;
        CMathODE        *ode;

        if (task->next_ode == &task->next_ode)
                task->last_ode_time = 0;
        else if (!Bt(&task->win_inhibit, WIf_SELF_ODE))
        {//See GrUpdateTasks() and GrUpdateTaskODEs().
                //We will not pick a time limit based on
                //how busy the CPU is, what percent of the
                //last refresh cycle was spent on ODE's
                //and what the refresh cycle rate was.
                start_time = tS;
                d = 1.0 / winmgr.fps;
                timeout_time = start_time + (task->last_ode_time / d + 0.1) / (winmgr.last_ode_time / d + 0.1) * ode_alloced_factor * d;
                ode = task->next_ode;
                while (ode != &task->next_ode)
                {
                        t_initial = ode->t;
                        d = tS;
                        if (!(ode->flags & ODEF_STARTED))
                        {
                                ode->base_t = d;
                                ode->flags |= ODEF_STARTED;
                        }
                        d -= ode->base_t + t_initial;
                        t_desired = ode->t_scale * d + t_initial;
                        if (ode->flags & ODEF_PAUSED)
                                ode->base_t += t_desired - ode->t; //Slip
                        else
                        {
                                ode->flags |= ODEF_BUSY;
                                if (ode->flags & ODEF_PAUSED)
                                        ode->base_t += t_desired-ode->t; //Slip
                                else
                                {
                                        if (ode->derive || ode->mp_derive)
                                        {
                                                if (ode->mp_derive)
                                                        ODEMPWake(ode);
                                                ODEState2Internal(ode);
                                                MemCopy(ode->initial_state, ode->state_internal, ode->n_internal * sizeof(F64));
                                                while (ode->t < t_desired)
                                                {
                                                        ode->h_max = t_desired - ode->t;
                                                        ODECallDerivative(ode, ode->t, ode->state_internal, ode->DstateDt);
                                                        for (i = 0; i < ode->n_internal; i++)
                                                                ode->state_scale[i] = Abs(ode->state_internal[i]) +
                                                                                                          Abs(ode->DstateDt[i] * ode->h) +
                                                                                                          ode->tolerance_internal;
                                                        ODERK5OneStep(ode);
                                                        if (tS > timeout_time)
                                                        {
                                                                ode->base_t += t_desired - ode->t; //Slip
                                                                goto ode_done;

                                                        }
                                                }

                                                //Interpolate if end time was not exact.
                                                if (ode->t != t_desired)
                                                {
                                                        if (interpolation = ode->t - t_initial)
                                                        {
                                                                interpolation = (t_desired - t_initial) / interpolation;
                                                                if (interpolation != 1.0)
                                                                        for (i = 0; i < ode->n_internal; i++)
                                                                                ode->state_internal[i] = (ode->state_internal[i] -
                                                                                                                                  ode->initial_state[i]) * interpolation +
                                                                                                                                  ode->initial_state[i];
                                                        }
                                                        ode->t = t_desired;
                                                }
ode_done:
                                                ODEInternal2State(ode);

                                                //Convenience call to set vals
                                                ODECallDerivative(ode, ode->t, ode->state_internal, ode->DstateDt);

                                                if (ode->mp_derive)
                                                        ODEMPSleep(ode);
                                        }
                                }
                                ode->flags &= ~ODEF_BUSY;
                        }
                        ode->base_t += (1.0 - ode->t_scale) * d;
                        ode = ode->next;
                }

                //Now, we will dynamically adjust tolerances.

                //We will regulate the tolerances
                //to fill the time we decided was
                //okay to devote to ODE's.
                //Since we might have multiple ODE's
                //active we scale them by the same factor.

                //This algorithm is probably not stable or very good, but it's something.

                //Target is 75% of alloced time.
                d = (tS - start_time) / (timeout_time - start_time) - 0.75;

                ode = task->next_ode;
                while (ode != &task->next_ode)
                {
                        if (!(ode->flags & ODEF_PAUSED) && ode->derive)
                        {
                                if (ode->min_tolerance != ode->max_tolerance)
                                {
                                        if (d > 0)
                                                ode->tolerance_internal *= 10.0 ` d;
                                        else
                                                ode->tolerance_internal *= 2.0 ` d;
                                }
                                ode->tolerance_internal = Clamp(ode->tolerance_internal, ode->min_tolerance, ode->max_tolerance);
                        }
                        ode = ode->next;
                }
                winmgr.ode_time += task->last_ode_time = tS - start_time;
        }
}