physics.c

/*
 * See Licensing and Copyright notice in naev.h
 */
#include <stdio.h>
#include <stdlib.h>
 
#include "naev.h"
#include "log.h"
#include "nstring.h"
#include "physics.h"
 
/*
 * M I S C
 */
static double angle_cleanup( double a )
{
   if (FABS(a) >= 2.*M_PI) {
      double na = fmod(a, 2.*M_PI);
      if (a < 0.)
         na += 2.*M_PI;
      return  na;
   }
   return a;
}
double angle_diff( double ref, double a )
{
   /* Get angles. */
   double a1 = angle_cleanup(ref);
   double a2 = angle_cleanup(a);
   double d  = a2 - a1;
 
   /* Filter offsets. */
   d  = (d < M_PI)  ? d : d - 2.*M_PI;
   d  = (d > -M_PI) ? d : d + 2.*M_PI;
   return d;
}
 
static void solid_update_euler( Solid *obj, double dt )
{
   double px,py, vx,vy, ax,ay, th;
   double cdir, sdir;
 
   /* make sure angle doesn't flip */
   obj->dir += obj->dir_vel*dt;
   if (obj->dir >= 2*M_PI)
      obj->dir -= 2*M_PI;
   if (obj->dir < 0.)
      obj->dir += 2*M_PI;
 
   /* Initial positions. */
   px = obj->pos.x;
   py = obj->pos.y;
   vx = obj->vel.x;
   vy = obj->vel.y;
   th = obj->thrust;
 
   /* Save direction. */
   sdir = sin(obj->dir);
   cdir = cos(obj->dir);
 
   /* Get acceleration. */
   ax = th*cdir / obj->mass;
   ay = th*sdir / obj->mass;
 
   /* Symplectic Euler */
   vx += ax*dt;
   vy += ay*dt;
   px += vx*dt;
   py += vy*dt;
 
   /* Update position and velocity. */
   vec2_cset( &obj->vel, vx, vy );
   vec2_cset( &obj->pos, px, py );
}
 
#define RK4_MIN_H 0.01 
static void solid_update_rk4( Solid *obj, double dt )
{
   int N; /* for iteration, and pass calculation */
   double h, px,py, vx,vy; /* pass, and position/velocity values */
   double ix,iy, tx,ty, th; /* initial and temporary cartesian vector values */
   double vmod, vang;
   int vint;
   int limit; /* limit speed? */
 
   /* Initial positions and velocity. */
   px = obj->pos.x;
   py = obj->pos.y;
   vx = obj->vel.x;
   vy = obj->vel.y;
   limit = (obj->speed_max >= 0.);
 
   /* Initial RK parameters. */
   if (dt > RK4_MIN_H)
      N = (int)(dt / RK4_MIN_H);
   else
      N = 1;
   vmod = MOD( vx, vy );
   vint = (int) vmod/100.;
   if (N < vint)
      N = vint;
   h = dt / (double)N; /* step */
 
   /* Movement Quantity Theorem:  m*a = \sum f */
   th = obj->thrust  / obj->mass;
 
   for (int i=0; i < N; i++) { /* iterations */
      /* Calculate acceleration for the frame. */
      double ax = th*cos(obj->dir);
      double ay = th*sin(obj->dir);
 
      /* Limit the speed. */
      if (limit) {
         vmod = MOD( vx, vy );
         if (vmod > obj->speed_max) {
            /* We limit by applying a force against it. */
            vang  = ANGLE( vx, vy ) + M_PI;
            vmod  = 3. * (vmod - obj->speed_max);
 
            /* Update accel. */
            ax += vmod * cos(vang);
            ay += vmod * sin(vang);
         }
      }
 
      /* x component */
      tx = ix = ax;
      tx += 2.*ix + h*tx;
      tx += 2.*ix + h*tx;
      tx += ix + h*tx;
      tx *= h/6.;
 
      vx += tx;
      px += vx * h;
 
      /* y component */
      ty = iy = ay;
      ty += 2.*iy + h*ty;
      ty += 2.*iy + h*ty;
      ty += iy + h*ty;
      ty *= h/6.;
 
      vy += ty;
      py += vy * h;
 
      /* rotation. */
      obj->dir += obj->dir_vel*h;
   }
   vec2_cset( &obj->vel, vx, vy );
   vec2_cset( &obj->pos, px, py );
 
   /* Validity check. */
   if (obj->dir >= 2.*M_PI)
      obj->dir -= 2.*M_PI;
   else if (obj->dir < 0.)
      obj->dir += 2.*M_PI;
}
 
double solid_maxspeed( const Solid *s, double speed, double thrust )
{
   return speed + thrust / (s->mass * 3.);
}
 
void solid_init( Solid* dest, double mass, double dir,
      const vec2* pos, const vec2* vel, int update )
{
   memset(dest, 0, sizeof(Solid));
 
   dest->mass = mass;
 
   /* Set direction velocity. */
   dest->dir_vel = 0.;
 
   /* Set force. */
   dest->thrust  = 0.;
 
   /* Set direction. */
   dest->dir = dir;
   if ((dest->dir > 2.*M_PI) || (dest->dir < 0.))
      dest->dir = fmod(dest->dir, 2.*M_PI);
 
   /* Set velocity. */
   if (vel == NULL)
      vectnull( &dest->vel );
   else
      dest->vel = *vel;
 
   /* Set position. */
   if (pos == NULL)
      vectnull( &dest->pos );
   else
      dest->pos = *pos;
 
   /* Misc. */
   dest->speed_max = -1.; /* Negative is invalid. */
 
   /* Handle update. */
   switch (update) {
      case SOLID_UPDATE_RK4:
         dest->update = solid_update_rk4;
         break;
 
      case SOLID_UPDATE_EULER:
         dest->update = solid_update_euler;
         break;
 
      default:
         WARN(_("Solid initialization did not specify correct update function!"));
         dest->update = solid_update_rk4;
         break;
   }
}
 
Solid* solid_create( double mass, double dir,
      const vec2* pos, const vec2* vel, int update )
{
   Solid* dyn = malloc(sizeof(Solid));
   if (dyn==NULL)
      ERR(_("Out of Memory"));
   solid_init( dyn, mass, dir, pos, vel, update );
   return dyn;
}
 
void solid_free( Solid* src )
{
   free(src);
}