Trivial Solutions
                    The LoseThos 64-bit PC Operating System
File:/LT/Adam/Ode.APZ
This, and all LoseThos files, are public domain.  Do whatever you like.
God is watching; God is just; God is never out-done in generosity.  He talks!

LoseThos code compiles with the 64-bit LoseThos compiler/assembler I wrote.
Boot the LoseThos CD and you can compile it.  Not in Kansas anymore!

Click here for preliminary compiler information you need.

#help_index "Math/Ode;Ode"
#help_file "::/LT/Doc/Ode"

//See ::/LT/Doc/Acknowledgements.TXZ (5).

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 OdeRstPtrs(Ode *ode)
{
  I64 s=ode->n_internal*sizeof(F64);
  F64 *ptr=ode->array_base;

  ode->state_internal=ptr;
  ptr><(U64)+=s;
  ode->state_scale=ptr;
  ptr><(U64)+=s;
  ode->DstateDt=ptr;
  ptr><(U64)+=s;
  ode->initial_state=ptr;
  ptr><(U64)+=s;
  ode->temp0=ptr;
  ptr><(U64)+=s;
  ode->temp1=ptr;
  ptr><(U64)+=s;
  ode->temp2=ptr;
  ptr><(U64)+=s;
  ode->temp3=ptr;
  ptr><(U64)+=s;
  ode->temp4=ptr;
  ptr><(U64)+=s;
  ode->temp5=ptr;
  ptr><(U64)+=s;
  ode->temp6=ptr;
  ptr><(U64)+=s;
  ode->temp7=ptr;
}

public Ode *OdeNew(U64 n,F64 max_tolerance=1e-6,U64 flags=0)
{
//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);
  Ode *ode=MAlloc(sizeof(Ode));

  ode->t=0;
  ode->t_scale=1.0;
  ode->base_t=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;
  ode->derivative=NULL;
  ode->next_mass=ode->last_mass=&ode->next_mass;
  ode->next_spring=ode->last_spring=&ode->next_spring;
  ode->acceleration_limit=ode->drag_v=ode->drag_v2=ode->drag_v3=0;

  ode->state=CAlloc(s);
  ode->array_base=MAlloc(12*s);
  OdeRstPtrs(ode);
  ode->ode_signature=ODE_SIGNATURE_VAL;
  return ode;
}

public U0 OdeDel(Ode *ode)
{
  if (!ode) return;
  ode->ode_signature=0;
  Free(ode->state);
  Free(ode->array_base);
  Free(ode);
}

public U64 OdeSize(Ode *ode)
{
  U64 result=0;
  if (!ode) return 0;
  result+=MSize2(ode->state);
  result+=MSize2(ode->array_base);
  result+=MSize2(ode);
  return result;
}



U0 OdeSetMassesPtrs(Ode *ode,F64 *state,F64 *DstateDt)
{
  Order2D3 *ptr1=state><(F64 *)+ode->n,
           *ptr2=DstateDt><(F64 *)+ode->n;
  MassStruct *tempm=ode->next_mass;
  while (tempm!=&ode->next_mass) {
    tempm->state=ptr1++;
    tempm->DstateDt=ptr2++;
    tempm=tempm->next;
  }
}

U0 OdeState2Internal(Ode *ode)
{
  MassStruct *tempm;
  F64 *old_array_base;
  I64 mass_cnt;

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

    OdeSetMassesPtrs(ode,ode->state_internal,ode->state_internal);
    tempm=ode->next_mass;
    while (tempm!=&ode->next_mass) {
      MemCpy(tempm->state,&tempm->saved_state,sizeof(Order2D3));
      tempm=tempm->next;
    }
  }
  MemCpy(ode->state_internal,ode->state,ode->n*sizeof(F64));
}

U0 OdeInternal2State(Ode *ode)
{
  MassStruct *tempm;
  MemCpy(ode->state,ode->state_internal,ode->n*sizeof(F64));
  if (ode->flags&ODEF_HAS_MASSES) {
    OdeSetMassesPtrs(ode,ode->state_internal,ode->state_internal);
    tempm=ode->next_mass;
    while (tempm!=&ode->next_mass) {
      MemCpy(&tempm->saved_state,tempm->state,sizeof(Order2D3));
      tempm=tempm->next;
    }
  }
}

public U0 OdeRenum(Ode *ode)
{
  I64 i;
  SpringStruct *temps;
  MassStruct *tempm;

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

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

public MassStruct *MassFind(Ode *ode,F64 x,F64 y,F64 z=0)
{
  MassStruct *tempm,*best_mass=NULL;
  F64 d,best_d=MAX_F64;

  tempm=ode->next_mass;
  while (tempm!=&ode->next_mass) {
    d=Sqr(tempm->x-x)+Sqr(tempm->y-y)+Sqr(tempm->z-z);
    if (d<best_d) {
      best_d=d;
      best_mass=tempm;
    }
    tempm=tempm->next;
  }
  return best_mass;
}

public SpringStruct *SpringFind(Ode *ode,F64 x,F64 y,F64 z=0)
{
  SpringStruct *temps,*best_spring=NULL;
  F64 d,best_d=MAX_F64;

  temps=ode->next_spring;
  while (temps!=&ode->next_spring) {
    d=Sqr((temps->end1->x+temps->end2->x)/2-x)+
      Sqr((temps->end1->y+temps->end2->y)/2-y)+
      Sqr((temps->end1->z+temps->end2->z)/2-z);
    if (d<best_d) {
      best_d=d;
      best_spring=temps;
    }
    temps=temps->next;
  }
  return best_spring;
}

public U0 MassOrSpringFind(Ode *ode,MassStruct **result_mass,SpringStruct **result_spring,
  F64 x,F64 y,F64 z=0)
{
  MassStruct   *tempm,*best_mass=NULL;
  SpringStruct *temps,*best_spring=NULL;
  F64 d,best_d=MAX_F64;

  tempm=ode->next_mass;
  while (tempm!=&ode->next_mass) {
    d=Sqr(tempm->x-x)+Sqr(tempm->y-y)+Sqr(tempm->z-z);
    if (d<best_d) {
      best_d=d;
      best_mass=tempm;
    }
    tempm=tempm->next;
  }

  temps=ode->next_spring;
  while (temps!=&ode->next_spring) {
    d=Sqr((temps->end1->x+temps->end2->x)/2-x)+
      Sqr((temps->end1->y+temps->end2->y)/2-y)+
      Sqr((temps->end1->z+temps->end2->z)/2-z);
    if (d<best_d) {
      best_d=d;
      best_spring=temps;
      best_mass=NULL;
    }
    temps=temps->next;
  }
  if (result_mass)   *result_mass  =best_mass;
  if (result_spring) *result_spring=best_spring;
}

public MassStruct *MassFindNum(Ode *ode,I64 num)
{
  MassStruct *tempm=ode->next_mass;
  while (tempm!=&ode->next_mass) {
    if (tempm->num==num)
      return tempm;
    tempm=tempm->next;
  }
  return NULL;
}

public U0 OdeRstInactive(Ode *ode)
{
  MassStruct *tempm;
  SpringStruct *temps;
  tempm=ode->next_mass;
  while (tempm!=&ode->next_mass) {
    tempm->flags&=~MSF_INACTIVE;
    tempm=tempm->next;
  }
  temps=ode->next_spring;
  while (temps!=&ode->next_spring) {
    temps->flags&=~SSF_INACTIVE;
    temps=temps->next;
  }
}

U0 OdeCalcSprings(Ode *ode)
{
  SpringStruct *temps=ode->next_spring;
  MassStruct *e1,*e2;
  F64 d;
  D3 p;
  while (temps!=&ode->next_spring) {
    if (temps->flags&SSF_INACTIVE) {
      temps->displacement=0;
      temps->f=0;
    } else {
      e1=temps->end1;
      e2=temps->end2;
      d=D3Norm(D3Sub(&e2->state->x,&e1->state->x,&p));
      temps->displacement=d-temps->rest_len;
      temps->f=temps->displacement*temps->constant;
      if (temps->f>0 && temps->flags&SSF_NO_TENSION)
        temps->f=0;
      else if (temps->f<0 && temps->flags&SSF_NO_COMPRESSION)
        temps->f=0;
      if (d>0) {
        D3MulEqu(&p,temps->f/d);
        D3AddEqu(&e1->DstateDt->DxDt,&p);
        D3SubEqu(&e2->DstateDt->DxDt,&p);
      }
    }
    temps=temps->next;
  }
}

U0 OdeCalcDrag(Ode *ode)
{
  MassStruct *tempm;
  F64 d,dd;
  D3 p;
  if (ode->drag_v || ode->drag_v2 || ode->drag_v3) {
    tempm=ode->next_mass;
    while (tempm!=&ode->next_mass) {
      if (!(tempm->flags & MSF_INACTIVE) &&
          tempm->drag_profile_factor &&
          (dd=D3SqrNorm(&tempm->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(&tempm->DstateDt->DxDt,
          D3Mul(d*tempm->drag_profile_factor,&tempm->state->DxDt,&p));
      }
      tempm=tempm->next;
    }
  }
}

U0 OdeApplyAccelerationLimit(Ode *ode)
{
  MassStruct *tempm;
  F64 d;
  if (ode->acceleration_limit) {
    tempm=ode->next_mass;
    while (tempm!=&ode->next_mass) {
      if (!(tempm->flags & MSF_INACTIVE) &&
          (d=D3Norm(&tempm->DstateDt->DxDt))>ode->acceleration_limit)
        D3MulEqu(&tempm->DstateDt->DxDt,ode->acceleration_limit/d);
      tempm=tempm->next;
    }
  }
}

U0 OdeCallDerivative(Ode *ode,F64 t,F64 *state,F64 *DstateDt)
{
  MassStruct *tempm;
  if (ode->flags&ODEF_HAS_MASSES) {
    OdeSetMassesPtrs(ode,state,DstateDt);
    tempm=ode->next_mass;
    while (tempm!=&ode->next_mass) {
      if (!(tempm->flags&MSF_INACTIVE)) {
        D3Zero(&tempm->DstateDt->DxDt);
        D3Copy(&tempm->DstateDt->x,&tempm->state->DxDt);
      }
      tempm=tempm->next;
    }
    OdeCalcSprings(ode);
    OdeCalcDrag(ode);
    (*ode->derivative)(ode,t,state,DstateDt);
    tempm=ode->next_mass;
    while (tempm!=&ode->next_mass) {
      if (!(tempm->flags&MSF_INACTIVE)) {
        if (tempm->flags&MSF_FIXED) {
          D3Zero(&tempm->DstateDt->DxDt);
          D3Zero(&tempm->DstateDt->x);
        } else if (tempm->mass)
          D3DivEqu(&tempm->DstateDt->DxDt,tempm->mass);
      }
      tempm=tempm->next;
    }
    OdeApplyAccelerationLimit(ode);
  } else
    (*ode->derivative)(ode,t,state,DstateDt);
}

U0 OdeOneStep(Ode *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(Ode *ode)
{
  I64 i,n=ode->n_internal;
  F64 xh,hh,h6;
  F64 *dym,*dyt,*yt,*DstateDt;

  dym =ode->temp0;
  dyt =ode->temp1;
  yt  =ode->temp2;
  DstateDt=ode->temp3;
  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(Ode *ode)
{
  I64 i,n=ode->n_internal;
  F64 h=ode->h,*state=ode->state_internal,*DstateDt=ode->DstateDt,*ak2,*ak3,*ak4,*ak5,*ak6,*tempstate,*stateerr,*outstate;

  ak2=ode->temp0;
  ak3=ode->temp1;
  ak4=ode->temp2;
  ak5=ode->temp3;
  ak6=ode->temp4;
  tempstate=ode->temp5;
  outstate=ode->temp6;
  stateerr=ode->temp7;

  for (i=0;i<n;i++)
    tempstate[i]=state[i]+ODEb21*h*DstateDt[i];
  OdeCallDerivative(ode,ode->t+ODEa2*h,tempstate,ak2);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb31*DstateDt[i]+ODEb32*ak2[i]);
  OdeCallDerivative(ode,ode->t+ODEa3*h,tempstate,ak3);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb41*DstateDt[i]+ODEb42*ak2[i]+ODEb43*ak3[i]);
  OdeCallDerivative(ode,ode->t+ODEa4*h,tempstate,ak4);
  for (i=0;i<n;i++)
    tempstate[i]=state[i]+h*(ODEb51*DstateDt[i]+ODEb52*ak2[i]+ODEb53*ak3[i]+ODEb54*ak4[i]);
  OdeCallDerivative(ode,ode->t+ODEa5*h,tempstate,ak5);
  for (i=0;i<n;i++)
    tempstate[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,tempstate,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(Ode *ode)
{
  I64 i;
  F64 errmax,temp;
  F64 *tempstate=ode->temp6,*stateerr=ode->temp7;
  while (TRUE) {
    ode->h=Limit(ode->h,ode->h_min,ode->h_max);
    OdeCashKarp(ode);
    errmax=0.0;
    for (i=0;i<ode->n_internal;i++) {
      temp=Abs(stateerr[i]/ode->state_scale[i]);
      if (temp>errmax)
        errmax=temp;
    }
    errmax/=ode->tolerance_internal;
    if (errmax<=1.0 || ode->h==ode->h_min) break;
    temp=ode->h*SAFETY*errmax`PSHRNK;
    if (temp<0.1*ode->h)
      ode->h*=0.1;
    else
      ode->h=temp;
  }
  ode->t+=ode->h;
  if (errmax>ERRCON)
    ode->h*=SAFETY*errmax`PGROW;
  else
    ode->h*=5.0;
  ode->h=Limit(ode->h,ode->h_min,ode->h_max);
  MemCpy(ode->state_internal,tempstate,sizeof(F64)*ode->n_internal);
}

F64 ode_allocated_factor=0.75;

U0 OdesUpdate(TaskStruct *task)
{
/*
This routine is called by the window manager on a continuous
basis to allow real-time simulation.  It is intended
to provide results 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 my implementation
it has a timeout.

*/
  I64 i;
  F64 d,start_time,timeout_time,t_desired,t_initial,interpolation;
  Ode *ode;
  BoolI8 old_suspend;

  if (task->next_ode==&task->next_ode)
    task->last_ode_time=0;
  else {
    //See GrUpdateWins()
    if (task!=Fs)
      old_suspend=LBts(&task->task_flags,TASKf_SUSPENDED);

    //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=tNP;
    d=1.0/win_actual_refresh;
    timeout_time=start_time+
        (task->last_ode_time/d+0.1)/(last_refresh_ode_time/d+0.1)*
         ode_allocated_factor*d;
    ode=task->next_ode;
    while (ode!=&task->next_ode) {
      if (ode->ode_signature==ODE_SIGNATURE_VAL) {
        t_initial=ode->t;
        d=tNP;
        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 || Bt(&task->task_flags,TASKf_PAUSED))
          ode->base_t+=t_desired-ode->t; //Slip
        else if (ode->derivative) {
          OdeState2Internal(ode);
          MemCpy(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 (tNP>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);
        }
        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 allocated time.
    d=(tNP-start_time)/(timeout_time-start_time)-0.75;

    ode=task->next_ode;
    while (ode!=&task->next_ode) {
      if (ode->ode_signature==ODE_SIGNATURE_VAL &&
          !(ode->flags&ODEF_PAUSED) &&
          !Bt(&task->task_flags,TASKf_PAUSED) &&
          ode->derivative) {
        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=Limit(ode->tolerance_internal,ode->min_tolerance,ode->max_tolerance);
      }
      ode=ode->next;
    }
    refresh_ode_time+=task->last_ode_time=tNP-start_time;
    //See GrUpdateWins()
    if (task!=Fs)
      LBEqu(&task->task_flags,TASKf_SUSPENDED,old_suspend);
  }
}
#help_index ""