[Add] Trying to work with a PID system for smart missiles.

src/physics.c

@@ -9,13 +9,15 @@
 // MISC
 // ================
 double angle_diff(const double ref, double a) {
+  double d;
   if(a < M_PI) a += 2*M_PI;
-  double d = fmod((a-ref), 2*M_PI);
+  d = fmod((a-ref), 2*M_PI);
   return (d <= M_PI) ? d : d - 2*M_PI;
 }
 
 void limit_speed(Vec2* vel, const double speed, const double dt) {
-  double vmod = VMOD(*vel);
+  double vmod;
+  vmod = VMOD(*vel);
   if(vmod > speed) // Should not go faster.
     vect_pset(vel, (vmod-speed)*(1.-dt*3.) + speed, VANGLE(*vel));
 }
@@ -94,15 +96,15 @@ static void simple_update(Solid* obj, const double dt) {
   if(obj->dir < 0.)      obj->dir += 2*M_PI;
 
   double px, py, vx, vy;
-  px = VX(obj->pos);
+  px = obj->pos->x;
-  py = VY(obj->pos);
+  py = obj->pos->y;
-  vx = VX(obj->vel);
+  vx = obj->vel->x;
-  vy = VY(obj->vel);
+  vy = obj->vel->y;
 
   if(obj->force.mod) { // Force applied on an object.
     double ax, ay;
-    ax = VX(obj->force)/obj->mass;
+    ax = obj->force->x/obj->mass;
-    ay = VY(obj->force)/obj->mass;
+    ay = obj->force->y/obj->mass;
 
     vx += ax*dt;
     vy += ay*dt;
@@ -138,27 +140,27 @@ static void simple_update(Solid* obj, const double dt) {
 
 #define RK4_MIN_H 0.01 // Minimal pass we want.
 static void rk4_update(Solid* obj, const double dt) {
+  int i, N;                       // For iteration and pass calculation.
+  double h, px, py, vx, vy;       // Pass and position/velocity values.
+  double ix, iy, tx, ty, ax, ay;  // Initial and temp cartesian vector values.
   // Make sure angle doesn't flip.
   obj->dir += M_PI/180.*obj->dir_vel*dt;
   if(obj->dir >= 2.*M_PI) obj->dir -= 2*M_PI;
   else if(obj->dir < 0.)  obj->dir += 2*M_PI;
 
-  int N = (dt > RK4_MIN_H) ? (int)(dt/RK4_MIN_H) : 1;
+  N = (dt > RK4_MIN_H) ? (int)(dt/RK4_MIN_H) : 1;
-  double h = dt / (double)N; // Step.
+  h = dt / (double)N; // Step.
 
-  double px, py, vx, vy;
   px = obj->pos.x;
   py = obj->pos.y;
   vx = obj->vel.x;
   vy = obj->vel.y;
 
   if(obj->force.mod) {  // Force applied on object.
-    int i;
+    // Movement quantity theorem : m*a = \sum f.
-    double ix, iy, tx, ty; // Initial and temp cartesian vector values.
+    ax = obj->force.x / obj->mass;
-
+    ay = obj->force.y / obj->mass;
-    double ax, ay;
+    
-    ax = VX(obj->force)/obj->mass;
-    ay = VY(obj->force)/obj->mass;
     for(i = 0; i < N; i++) {
       // X component.
       tx = ix = vx;
@@ -182,6 +184,7 @@ static void rk4_update(Solid* obj, const double dt) {
     }
     vect_cset(&obj->vel, vx, vy);
   } else {
+    // Euler method -> p = v*t + 0.5*a*t^2 (no accel, so no error).
     px += dt*vx;
     py += dt*vy;
   }

src/weapon.c

@@ -18,6 +18,10 @@
 #define VOICE_PRIORITY_BOLT  10  // Default.
 #define VOICE_PRIORITY_AMMO  8  // Higher.
 
+// PID values.
+#define IMIN   -50000
+#define IMAX    50000
+
 // Some stuff from pilot.
 extern Pilot** pilot_stack;
 extern int pilots;
@@ -37,7 +41,10 @@ typedef struct Weapon_ {
 
   // Update position and render.
   void(*update)(struct Weapon_*, const double, WeaponLayer); // Position update and render.
-  void(*think)(struct Weapon_*, const double); // Some missiles need to be inteligent.
+  void(*think)(struct Weapon_*, const double); // Some missiles need to be inteligent.a
+
+  double pid_last;
+  double pid_int;
 } Weapon;
 
 // Behind Pilot layer.
@@ -143,10 +150,43 @@ static void think_seeker(Weapon* w, const double dt) {
   limit_speed(&w->solid->vel, w->outfit->u.amm.speed, dt);
 }
 
+// ========================================================
 // Smart seeker brain. Much better at homing.
+//
+// SYSTEM DYNAMICS
+//  INPUT:
+//    MV = missile velocity
+//    RP = target relative position.
+//
+//  Constants:
+//    KE = error constant.
+//    KF = face constant.
+//
+//  INTERNAL VARIABLES:
+//    FD = face dir.
+//    E = error.
+//    PID = PID
+//
+//  OUTPUT:
+//    T = turn modifier.
+//
+//  -- Fancy diagram.
+//
+//  MV +  FD +                    T
+//  ---->0----->[PID]----[KF]---+---->
+//      -^     -^               |
+//  RP   |      | E             |
+//  -----+      |------[KE]------
+//
+// ========================================================
 static void think_smart(Weapon* w, const double dt) {
-  double diff;
+  Vec2 tv;
-  Vec2 tv, sv;
+  
+  // Controler stuff.
+  double ke, kf;          // Constants
+  double fd, e, ed, ei;   // Internal variables.
+  double t;               // Internal variables.
+  double kp, ki, kd;      // PID.
 
   if(w->target == w->parent) return; // No self shooting here.
 
@@ -158,12 +198,32 @@ static void think_smart(Weapon* w, const double dt) {
   }
 
   if(SDL_GetTicks() > (w->timer + w->outfit->u.amm.lockon)) {
-    vect_cset(&tv, VX(p->solid->pos) + VX(p->solid->vel),
+    // Begin controler.
-              VY(p->solid->pos) + VY(p->solid->vel));
+    //  Constants.
-    vect_cset(&sv, VX(w->solid->pos) + VX(w->solid->vel),
+    kp = 1.;
-              VY(w->solid->pos) + VY(w->solid->vel));
+    kd = 1.;
-    diff = angle_diff(w->solid->dir, vect_angle(&tv, &sv));
+    ki = 1.;
-    w->solid->dir_vel = 10*diff*w->outfit->u.amm.turn; // Face the target.
+    
+    // Calculate fd.
+    vect_cset(&tv, p->solid->pos.x - w->solid->pos.x,
+              p->solid->pos.y - w->solid->pos.y);
+    e = angle_diff(VANGLE(w->solid->vel), VANGLE(tv));
+
+    e = fd;
+    ed = fd - ke*w->pid_last;
+    w->pid_int += e;
+    if(w->pid_int > IMAX) w->pid_int = IMAX;
+    else if(w->pid_int < IMIN) w->pid_int = IMIN;
+    ei = w->pid_int;
+    t = e*kp + ed*kd + ei*ki;
+
+    // Final output.
+    t *= kf;
+    w->pid_last = t;
+
+    // End controller.
+
+    w->solid->dir_vel = t * w->outfit->u.amm.turn; // Face the target.
     if(w->solid->dir_vel > w->outfit->u.amm.turn)
       w->solid->dir_vel = w->outfit->u.amm.turn;
     else if(w->solid->dir_vel < -w->outfit->u.amm.turn)