hschumann2/TempleOS-Source-Code

1 2U0 MyDerivative(CMathODE *ode,F64,COrder2D3 *,COrder2D3 *)3{4//The forces due to springs and drag are5  //automatically handled by the6  //ode code.  We can add new forces7  //here.8  CTask *task=ode->win_task;9  F64 d,dd;10  CD3 p,p2;11  MyMass *tmpm1,*tmpm2;12 13  //Collisions14  tmpm1=ode->next_mass;15  while (tmpm1!=&ode->next_mass) {16    tmpm2=tmpm1->next;17    while (tmpm2!=&ode->next_mass) {18      D3Sub(&p,&tmpm2->state->x,&tmpm1->state->x);19      dd=D3NormSqr(&p);20      if (dd<=Sqr(tmpm1->radius+tmpm2->radius)) {21        d=Sqrt(dd)+0.0001;22        dd=10.0*Sqr(Sqr(Sqr(tmpm1->radius+tmpm2->radius)-dd));23        D3MulEqu(&p,dd/d);24        D3AddEqu(&tmpm2->DstateDt->DxDt,&p);25        D3SubEqu(&tmpm1->DstateDt->DxDt,&p);26      }27      tmpm2=tmpm2->next;28    }29    tmpm1=tmpm1->next;30  }31 32  tmpm1=ode->next_mass;33  while (tmpm1!=&ode->next_mass) {34    if (!(tmpm1->flags&MSF_FIXED))35      tmpm1->DstateDt->DyDt+=10.0*tmpm1->mass; //Gravity36    tmpm1=tmpm1->next;37  }38 39  if (cursor_mass) {40    p2.x=ms.pos.x-task->pix_left-task->scroll_x;41    p2.y=ms.pos.y-task->pix_top-task->scroll_y;42    p2.z=0;43    D3Sub(&p,&p2,&cursor_mass->state->x);44    d=10.0*D3NormSqr(&p);45    D3MulEqu(&p,d);46    D3AddEqu(&cursor_mass->DstateDt->DxDt,&p);47  }48}49