src/physics.c

   1 /*
   2  * See Licensing and Copyright notice in naev.h
   3  */
   4
   5
   6
   7 #include "physics.h"
   8
   9 #include "naev.h"
  10
  11 #include <stdlib.h>
  12 #include <stdio.h>
  13 #include <string.h>
  14
  15 #include "log.h"
  16
  17
  18 /*
  19  * M I S C
  20  */
  21 static double angle_cleanup( double a )
  22 {
  23    double na;
  24    if (FABS(a) >= 2.*M_PI) {
  25       na = fmod(a, 2.*M_PI);
  26       if (a < 0.)
  27          na += 2.*M_PI;
  28       return  na;
  29    }
  30    return a;
  31 }
  32 /**
  33  * @brief Gets the difference between two angles.
  34  *
  35  *    @param ref Reference angle.
  36  *    @param a Angle to get distance from ref.
  37  */
  38 double angle_diff( const double ref, double a )
  39 {
  40    double d;
  41    double a1, a2;
  42
  43    /* Get angles. */
  44    a1 = angle_cleanup(ref);
  45    a2 = angle_cleanup(a);
  46    d  = a2 - a1;
  47
  48    /* Filter offsets. */
  49    d  = (d < M_PI)  ? d : d - 2.*M_PI;
  50    d  = (d > -M_PI) ? d : d + 2.*M_PI;
  51    return d;
  52 }
  53 /**
  54  * @brief Limits the speed of an object.
  55  *
  56  *    @param vel Velocity vector to limit.
  57  *    @param speed Maximum speed.
  58  *    @param dt Current delta tick.
  59  */
  60 void limit_speed( Vector2d* vel, const double speed, const double dt )
  61 {
  62    double vmod;
  63
  64    vmod = VMOD(*vel);
  65    if (vmod > speed) /* shouldn't go faster */
  66       vect_pset( vel, (vmod-speed)*(1. - dt*3.) +  speed, VANGLE(*vel) );
  67 }
  68
  69
  70 /*
  71  *
  72  * V E C T O R 2 D
  73  *
  74  */
  75 /**
  76  * @brief Set the vector value using cartesian coordinates
  77  *
  78  *    @param v Vector to set.
  79  *    @param x X value for vector.
  80  *    @param y Y value for vector.
  81  */
  82 void vect_cset( Vector2d* v, const double x, const double y )
  83 {
  84    v->x     = x;
  85    v->y     = y;
  86    v->mod   = MOD(x,y);
  87    v->angle = ANGLE(x,y);
  88 }
  89
  90
  91 /**
  92  * @brief Creates a minimal vector only valid for blitting and not other operations.
  93  *
  94  *    @param v Vector to set.
  95  *    @param x X value for vector.
  96  *    @param y Y value for vector.
  97  */
  98 void vect_csetmin( Vector2d* v, const double x, const double y )
  99 {
 100    v->x = x;
 101    v->y = y;
 102 }
 103
 104
 105 /**
 106  * @brief Set the vector value using polar coordinates.
 107  *
 108  *    @param v Vector to set.
 109  *    @param mod Modulus of the vector.
 110  *    @param angle Angle of the vector.
 111  */
 112 void vect_pset( Vector2d* v, const double mod, const double angle )
 113 {
 114    v->mod   = mod;
 115    v->angle = angle;
 116    v->x     = v->mod*cos(v->angle);
 117    v->y     = v->mod*sin(v->angle);
 118 }
 119
 120
 121 /**
 122  * @brief Copies vector src to dest.
 123  *
 124  *    @param dest Destination vector.
 125  *    @param src Vector to copy.
 126  */
 127 void vectcpy( Vector2d* dest, const Vector2d* src )
 128 {
 129    dest->x     = src->x;
 130    dest->y     = src->y;
 131    dest->mod   = src->mod;
 132    dest->angle = src->angle;
 133 }
 134
 135
 136 /**
 137  * @brief Sets a vector to NULL.
 138  *
 139  *    @param v Vector to set to NULL.
 140  */
 141 void vectnull( Vector2d* v )
 142 {
 143    v->x     = 0.;
 144    v->y     = 0.;
 145    v->mod   = 0.;
 146    v->angle = 0.;
 147 }
 148
 149
 150 /**
 151  * @brief Get the direction pointed to by two vectors (from ref to v).
 152  *
 153  *    @param ref Reference vector.
 154  *    @param v Vector to get angle from reference vector.
 155  *    @return Angle between ref and v.
 156  */
 157 double vect_angle( const Vector2d* ref, const Vector2d* v )
 158 {
 159    double x,y;
 160
 161    x = v->x - ref->x;
 162    y = v->y - ref->y;
 163
 164    return ANGLE( x, y );
 165 }
 166
 167
 168 /**
 169  * @brief Adds x and y to the current vector
 170  *
 171  *    @param v Vector to add x and y to.
 172  *    @param x X value to add to vector.
 173  *    @param y Y value to add to vector.
 174  */
 175 void vect_cadd( Vector2d* v, const double x, const double y )
 176 {
 177    v->x    += x;
 178    v->y    += y;
 179    v->mod   = MOD(v->x,v->y);
 180    v->angle = ANGLE(v->x,v->y);
 181 }
 182
 183
 184 /**
 185  * @brief Mirrors a vector off another, stores results in vector.
 186  *
 187  *    @param r Resulting vector of the reflection.
 188  *    @param v Vector to reflect.
 189  *    @param n Normal to reflect off of.
 190  */
 191 void vect_reflect( Vector2d* r, Vector2d* v, Vector2d* n )
 192 {
 193    double dot;
 194
 195    dot      = vect_dot( v, n );
 196    r->x     = v->x - ((2. * dot) * n->x);
 197    r->y     = v->y - ((2. * dot) * n->y);
 198    r->mod   = MOD(r->x,r->y);
 199    r->angle = MOD(r->x,r->y);
 200 }
 201
 202
 203 /**
 204  * @brief Vector dot product.
 205  *
 206  *    @param a Vector 1 for dot product.
 207  *    @param b Vector 2 for dot product.
 208  *    @return Dot product of vectors.
 209  */
 210 double vect_dot( Vector2d* a, Vector2d* b )
 211 {
 212    return a->x * b->x + a->y * b->y;
 213 }
 214
 215 /*
 216  * S O L I D
 217  */
 218 #if HAS_FREEBSD
 219 /**
 220  * @brief Updates the solid's position using an euler integration.
 221  *
 222  * Simple method
 223  *
 224  *   d^2 x(t) / d t^2 = a, a = constant (acceleration)
 225  *   x'(0) = v, x(0) = p
 226  *
 227  *   d x(t) / d t = a*t + v, v = constant (initial velocity)
 228  *   x(t) = a/2*t + v*t + p, p = constant (initial position)
 229  *
 230  *   since d t isn't actually diferential this gives us ERROR!
 231  *   so watch out with big values for dt
 232  *
 233  */
 234 static void simple_update (Solid *obj, const double dt)
 235 {
 236    double px,py, vx,vy, ax;
 237
 238    /* make sure angle doesn't flip */
 239    obj->dir += M_PI/180.*obj->dir_vel*dt;
 240    if (obj->dir >= 2*M_PI)
 241       obj->dir -= 2*M_PI;
 242    if (obj->dir < 0.)
 243       obj->dir += 2*M_PI;
 244
 245    /* Initial positions. */
 246    px = obj->pos.x;
 247    py = obj->pos.y;
 248    vx = obj->vel.x;
 249    vy = obj->vel.y;
 250
 251    if (obj->force_x != 0.) { /* force applied on object */
 252       ax = obj->force_x / obj->mass;
 253       /*ay = obj->force_x / obj->mass;*/
 254
 255       vx += ax*cos(obj->dir) * dt;
 256       vy += ax*sin(obj->dir) * dt;
 257
 258       px += vx*dt + 0.5*ax * dt*dt;
 259       py += vy*dt; /* + 0.5*ay * dt*dt; */
 260
 261       obj->vel.mod = MOD(vx,vy);
 262       obj->vel.angle = ANGLE(vx,vy);
 263    }
 264    else {
 265       px += vx*dt;
 266       py += vy*dt;
 267    }
 268
 269    /* Update position and velocity. */
 270    vect_cset( &obj->vel, vx, vy );
 271    vect_cset( &obj->pos, px, py );
 272 }
 273 #endif /* HAS_FREEBSD */
 274
 275
 276 /**
 277  * @fn static void rk4_update (Solid *obj, const double dt)
 278  *
 279  * @brief Runge-Kutta method of updating a solid based on it's acceleration.
 280  *
 281  * Runge-Kutta 4 method
 282  *
 283  *   d^2 x(t) / d t^2 = a, a = constant (acceleration)
 284  *   x'(0) = v, x(0) = p
 285  *   x'' = f( t, x, x' ) = ( x' , a )
 286  *
 287  *   x_{n+1} = x_n + h/6 (k1 + 2*k2 + 3*k3 + k4)
 288  *    h = (b-a)/2
 289  *    k1 = f(t_n, X_n ), X_n = (x_n, x'_n)
 290  *    k2 = f(t_n + h/2, X_n + h/2*k1)
 291  *    k3 = f(t_n + h/2, X_n + h/2*k2)
 292  *    k4 = f(t_n + h, X_n + h*k3)
 293  *
 294  *   x_{n+1} = x_n + h/6*(6x'_n + 3*h*a, 4*a)
 295  *
 296  *
 297  * Main advantage comes thanks to the fact that NAEV is on a 2d plane.
 298  *  Therefore RK chops it up in chunks and actually creates a tiny curve
 299  *  instead of aproximating the curve for a tiny straight line.
 300  */
 301 #if !HAS_FREEBSD
 302 #define RK4_MIN_H 0.01 /**< Minimal pass we want. */
 303 static void rk4_update (Solid *obj, const double dt)
 304 {
 305    int i, N; /* for iteration, and pass calcualtion */
 306    double h, px,py, vx,vy; /* pass, and position/velocity values */
 307    double ix,iy, tx,ty, ax; /* initial and temporary cartesian vector values */
 308
 309    /* Initial RK parameters. */
 310    N = (dt>RK4_MIN_H) ? (int)(dt/RK4_MIN_H) : 1 ;
 311    h = dt / (double)N; /* step */
 312
 313    /* Initial positions and velocity. */
 314    px = obj->pos.x;
 315    py = obj->pos.y;
 316    vx = obj->vel.x;
 317    vy = obj->vel.y;
 318
 319    if (obj->force_x > 0.) { /* force applied on object */
 320
 321       /* Movement Quantity Theorem:  m*a = \sum f */
 322       ax = obj->force_x / obj->mass;
 323       /*ay = obj->force.x / obj->mass;*/
 324
 325       for (i=0; i < N; i++) { /* iterations */
 326
 327          /* x component */
 328          tx = ix = vx;
 329          tx += 2.*ix + h*tx;
 330          tx += 2.*ix + h*tx;
 331          tx += ix + h*tx;
 332          tx *= h/6.;
 333
 334          px += tx;
 335          vx += ax*cos(obj->dir) * h;
 336
 337          /* y component */
 338          ty = iy = vy;
 339          ty += 2.*(iy + h/2.*ty);
 340          ty += 2.*(iy + h/2.*ty);
 341          ty += iy + h*ty;
 342          ty *= h/6.;
 343
 344          py += ty;
 345          vy += ax*sin(obj->dir) * h;
 346
 347          /* rotation. */
 348          obj->dir += M_PI/180.*obj->dir_vel*h;
 349       }
 350       vect_cset( &obj->vel, vx, vy );
 351    }
 352    else { /* euler method -> p = v*t + 0.5*a*t^2 (no accel, so no error) */
 353       px += dt*vx;
 354       py += dt*vy;
 355       obj->dir += M_PI/180.*obj->dir_vel*dt;
 356    }
 357    vect_cset( &obj->pos, px, py );
 358
 359    /* Sanity check. */
 360    if (obj->dir >= 2.*M_PI)
 361       obj->dir -= 2.*M_PI;
 362    else if (obj->dir < 0.)
 363       obj->dir += 2.*M_PI;
 364 }
 365 #endif /* !HAS_FREEBSD */
 366
 367
 368 /**
 369  * @brief Initializes a new Solid.
 370  *
 371  *    @param dest Solid to initialize.
 372  *    @param mass Mass to set solid to.
 373  *    @param dir Solid initial direction.
 374  *    @param pos Initial solid position.
 375  *    @param vel Initial solid velocity.
 376  */
 377 void solid_init( Solid* dest, const double mass, const double dir,
 378       const Vector2d* pos, const Vector2d* vel )
 379 {
 380    memset(dest, 0, sizeof(Solid));
 381
 382    dest->mass = mass;
 383
 384    /* Set direction velocity. */
 385    dest->dir_vel = 0.;
 386
 387    /* Set force. */
 388    dest->force_x = 0.;
 389    /*dest->force_y = 0.;*/
 390
 391    /* Set direction. */
 392    dest->dir = dir;
 393    if ((dest->dir > 2.*M_PI) || (dest->dir < 0.))
 394       dest->dir = fmod(dest->dir, 2.*M_PI);
 395
 396    /* Set velocity. */
 397    if (vel == NULL)
 398       vectnull( &dest->vel );
 399    else
 400       vectcpy( &dest->vel, vel );
 401
 402    /* Set position. */
 403    if (pos == NULL)
 404       vectnull( &dest->pos );
 405    else
 406       vectcpy( &dest->pos, pos);
 407
 408 /*
 409  * FreeBSD seems to have a bug with optimizations in rk4_update causing it to
 410  * eventually become NaN.
 411  */
 412 #if HAS_FREEBSD
 413    dest->update = simple_update;
 414 #else /* HAS_FREEBSD */
 415    dest->update = rk4_update;
 416 #endif /* HAS_FREEBSD */
 417 }
 418
 419
 420 /**
 421  * @brief Creates a new Solid.
 422  *
 423  *    @param mass Mass to set solid to.
 424  *    @param dir Solid initial direction.
 425  *    @param pos Initial solid position.
 426  *    @param vel Initial solid velocity.
 427  *    @return A newly created solid.
 428  */
 429 Solid* solid_create( const double mass, const double dir,
 430       const Vector2d* pos, const Vector2d* vel )
 431 {
 432    Solid* dyn = malloc(sizeof(Solid));
 433    if (dyn==NULL) ERR("Out of Memory");
 434    solid_init( dyn, mass, dir, pos, vel );
 435    return dyn;
 436 }
 437
 438
 439 /**
 440  * @brief Frees an existing solid.
 441  *
 442  *    @param src Solid to free.
 443  */
 444 void solid_free( Solid* src )
 445 {
 446    free(src);
 447 }
 448