home *** CD-ROM | disk | FTP | other *** search
/ NetNews Usenet Archive 1992 #27 / NN_1992_27.iso / spool / alt / sources / 2611 / autopilot.c next >
Encoding:
C/C++ Source or Header  |  1992-11-23  |  4.5 KB  |  203 lines
  1. #ifndef lint
  2. static    char    sccsid[] = "@(#)autopilot.c 1.1 92/05/28 SMI" ;
  3.     /* from autopilot.c 1.3 92/05/28 SMI */
  4. #endif
  5.  
  6. /*
  7.  * Copyright (c) 1986 by Sun Microsystems, Inc.
  8.  */
  9.  
  10. /*
  11.  * this is the autopilot function.
  12.  *
  13.  * Go through the list of objects, select the closest object within view.
  14.  * If that object is closer than AUTO_RANGE:
  15.  *   place cursor on that object.
  16.  *   If object is closer than LASER_RANGE, fire lasers.
  17.  *
  18.  * If no objects closer than AUTO_THRESH:
  19.  *   Set delta to (0.1,0.0) or (0.0,0.1) depending on time.
  20.  *
  21.  *
  22.  */
  23.  
  24.  
  25.  
  26.  
  27. #include <stdio.h>
  28. #include <math.h>
  29. #include "parameters.h"
  30. #include "dstar.h"
  31. #include "object_types.h"
  32.  
  33.  
  34.  
  35.  
  36. #define    MAX_TURN_ERROR2    (MAX_TURN_ERROR*MAX_TURN_ERROR)
  37.  
  38. extern    int    debug_level ;
  39.  
  40.  
  41.  
  42. auto_control(object, auto_range, max_turn, max_slew)
  43.     Object    *object ;
  44.     float    auto_range, max_turn, max_slew ;
  45. {
  46.     int    i,j ;
  47.     Pt3d    v1, v2 ;
  48.     float    d ;
  49.     Object    *obj2 = &objects[0] ;
  50.     Object    *closest = NULL ;
  51.     Mat3d    xform ;
  52.     float    dist ;
  53.  
  54.     object->flags &= ~LASER_FLAG ;
  55.  
  56.     if(object->target != NULL)
  57.     {
  58.       v1.x = object->target->Posn.x - object->Posn.x ;
  59.       v1.y = object->target->Posn.y - object->Posn.y ;
  60.       v1.z = object->target->Posn.z - object->Posn.z ;
  61.       dist = v1.x*v1.x + v1.y*v1.y + v1.z*v1.z ;
  62.  
  63.       /* we've got a victim, find delta to take us there:
  64.        *
  65.        * Build vector to victim in world coords.
  66.        * transform to ship coords, ignore z.
  67.        * normalize (we already have distance), multiply by feedback gain
  68.        * limit to max turn rate.
  69.        */
  70.  
  71.       v2.x = v1.x*object->Right.x +
  72.          v1.y*object->Right.y +
  73.          v1.z*object->Right.z ;
  74.       v2.y = v1.x*object->Up.x +
  75.          v1.y*object->Up.y +
  76.          v1.z*object->Up.z ;
  77.  
  78.       if(auto_fire && dist < LASER_RANGE*LASER_RANGE*2.0)
  79.         object->flags |= LASER_FLAG ;
  80.       
  81.       if(dist < LASER_RANGE*LASER_RANGE)
  82.         dist = 1.0 / sqrt(dist) ;
  83.       else
  84.         dist = FEEDBACK_GAIN / sqrt(dist) ;
  85.  
  86.       v2.x *= dist ;
  87.       v2.y *= dist ;
  88.  
  89.       if(v2.x >  max_turn) v2.x =  max_turn ;
  90.       if(v2.x < -max_turn) v2.x = -max_turn ;
  91.       if(v2.y >  max_turn) v2.y =  max_turn ;
  92.       if(v2.y < -max_turn) v2.y = -max_turn ;
  93.     }
  94.     else
  95.     {
  96.       v2.x = SCAN_TURN ;
  97.       v2.y = 0.0 ;
  98.     }
  99.     set_delta(object, &v2, max_slew) ;
  100.     adjust_vector(object,&object->Delta) ;
  101. #ifdef    COMMENT
  102.     for(i=0; i<MAX_OBJECTS; ++i)
  103.     {
  104.       if(obj2 != object &&
  105.         (obj2->class == OBJ_ACTIVE  ||
  106.          obj2->class == OBJ_MISSILE) )
  107.       {
  108.         dist = 
  109.           (obj2->Posn.x - object->Posn.x)*(obj2->Posn.x - object->Posn.x) +
  110.           (obj2->Posn.y - object->Posn.y)*(obj2->Posn.y - object->Posn.y) +
  111.           (obj2->Posn.z - object->Posn.z)*(obj2->Posn.z - object->Posn.z);
  112.         if(dist < mindist)
  113.         {
  114.           v1.x = obj2->Posn.x - object->Posn.x ;
  115.           v1.y = obj2->Posn.y - object->Posn.y ;
  116.           v1.z = obj2->Posn.z - object->Posn.z ;
  117.           d = v1.x * object->Pointing.x +
  118.           v1.y * object->Pointing.y +
  119.           v1.z * object->Pointing.z ;
  120.           if(d > 0.0 && d*d > dist * 0.5)    /* cos(45)**2 */
  121.           {
  122.         closest = obj2 ;
  123.         mindist = dist ;
  124.           }
  125.         }
  126.       }
  127.     ++obj2 ;
  128.     }
  129.  
  130.  
  131.     if(closest != NULL)
  132.     {
  133.       if(auto_fire && mindist < LASER_RANGE*LASER_RANGE*2.0)
  134.         object->flags |= LASER_FLAG ;
  135.       
  136.       /* we've got a victim, find delta to take us there:
  137.        *
  138.        * Build vector to victim in world coords.
  139.        * transform to ship coords, ignore z.
  140.        * normalize (we already have distance), multiply by feedback gain
  141.        * limit to max turn rate.
  142.        */
  143.  
  144.       v1.x = closest->Posn.x - object->Posn.x ;
  145.       v1.y = closest->Posn.y - object->Posn.y ;
  146.       v1.z = closest->Posn.z - object->Posn.z ;
  147.  
  148.       v2.x = v1.x*object->Right.x +
  149.          v1.y*object->Right.y +
  150.          v1.z*object->Right.z ;
  151.       v2.y = v1.x*object->Up.x +
  152.          v1.y*object->Up.y +
  153.          v1.z*object->Up.z ;
  154.  
  155.       if(mindist < LASER_RANGE*LASER_RANGE)
  156.         mindist = 1.0 / sqrt(mindist) ;
  157.       else
  158.         mindist = FEEDBACK_GAIN / sqrt(mindist) ;
  159.  
  160.       v2.x *= mindist ;
  161.       v2.y *= mindist ;
  162.  
  163.       if(v2.x >  max_turn) v2.x =  max_turn ;
  164.       if(v2.x < -max_turn) v2.x = -max_turn ;
  165.       if(v2.y >  max_turn) v2.y =  max_turn ;
  166.       if(v2.y < -max_turn) v2.y = -max_turn ;
  167.     }
  168.     else
  169.     {
  170.       v2.x = SCAN_TURN ;
  171.       v2.y = 0.0 ;
  172.     }
  173.     set_delta(object, &v2, max_slew) ;
  174.     adjust_vector(object,&object->Delta) ;
  175. #endif    COMMENT
  176. }
  177.  
  178.  
  179.  
  180. set_delta(object, vector, max_slew)
  181.     Object        *object ;
  182.     Pt3d        *vector ;
  183.     float        max_slew ;
  184. {
  185.     Pt3d        v1 ;
  186. register float        d1, max_turn ;
  187.  
  188.     v1.x = vector->x - object->Delta.x ;
  189.     v1.y = vector->y - object->Delta.y ;
  190.     d1 = v1.x*v1.x + v1.y*v1.y ;
  191.     if(d1 > MAX_TURN_ERROR2)
  192.     {
  193.       d1 = sqrt(d1) ;
  194.       max_turn = Dtime * max_slew ;
  195.       if(d1 > max_turn)
  196.         d1 = max_turn/d1 ;
  197.       else
  198.         d1 = 1.0 ;
  199.       object->Delta.x += d1*v1.x ;
  200.       object->Delta.y += d1*v1.y ;
  201.     }
  202. }
  203.