home *** CD-ROM | disk | FTP | other *** search
/ ftp.hitl.washington.edu / ftp.hitl.washington.edu.tar / ftp.hitl.washington.edu / pub / people / habib / kodak / Drive_S4_10.cc~ < prev    next >
Text File  |  2000-04-18  |  8KB  |  210 lines
  1. #include <math.h>
  2. #include <stdio.h>
  3. #include <stdlib.h>
  4. #include <time.h>
  5. #include <iostream.h>
  6. #include <string.h>
  7. #include <windows.h>
  8. #include <mmsystem.h>  // WinUnix
  9.  
  10. struct point {float x,y,z;};
  11. extern float DTime;
  12. extern struct point eyec, Acceleration, Velocity, lkc_rt, up_rt;
  13. extern float AccelScalar;
  14. extern int CRISIS;
  15. /* getrandom returns a random number between min and max, which must be in
  16.  * integer range. */
  17. char *filename;
  18. FILE *fileJohn;
  19. #define getrandom( min, max ) ((rand() % (int)(((max) + 1) - (min))) + (min))
  20. #define AllVeh 6
  21. int I,J,K,Nveh;
  22. float Damp, Fprint, Freq,Lrand,MinSep,Rand_max,Tau_rand,Tmax,Tspace,Tyme, VehZone, DeadZone;
  23. float (*Pos)[AllVeh], (*Vel)[AllVeh], (*Acce)[AllVeh], (*Acc_rand)[AllVeh], (*Rand)[AllVeh];
  24. float TargetSep,VehLength, Veh_spacing, AccMax,AccDamp,DecMax,Sep,AccSpring,PaceSpeed;
  25.  
  26. void initialise()
  27. {
  28.      filename = new char [100];
  29.      strcpy (filename, "debug2.txt");
  30.      if ((fileJohn = fopen (filename, "w")) == NULL)
  31.         printf ("\n pas de chance l'ami ton ffile iest emmerde...\n");
  32.      
  33.      fprintf (fileJohn, "Time , Rand[0][1] , Acc_rand[0][1] , Acce[0][1] , Vel[0][1] , Pos[0][1] , Pos[0][0] ,  Rand[2][1] , Acc_rand[2][1] , Acce[2][1] , Vel[2][1] , Pos[2][1] , Pos[2][0] \n");
  34.  
  35.      Nveh=AllVeh - 1;                                   // Number of vehicles in lane
  36.      Pos =       new float[3][AllVeh];
  37.      Vel =       new float[3][AllVeh];
  38.      Acce =       new float[3][AllVeh];
  39.      Acc_rand =  new float[3][AllVeh];
  40.      Rand      = new float[3][AllVeh];
  41.  
  42.      Damp=1    ;                              // Friction coefficient [1/sec]
  43.      Freq=.2   ;                              // Ocsillation freq from "spring" [1/sec]
  44.      Tspace=1  ;                              // Time between vehicles used to calc desired TargetSep [sec]
  45.      VehLength=5 ;                            // Length of vehicle [m]
  46.      DeadZone = 5;                            // Extra length added to vehicle to avoid collision.
  47.      Tau_rand=.2    ;                          // Time constant used for EWMA filter in Acc_rand [sec]
  48.      AccMax=4    ;                            // Ymax will be approx one sigma of EWMA distrib [m/sec^2]
  49.      // DecMax=8    ;                            // Maximum braking decel     
  50.      PaceSpeed = 20;                          // 20 m/s
  51.      DecMax = Damp * PaceSpeed + 2 * AccMax;
  52.      
  53.      VehZone = VehLength + DeadZone;               // Calculate the min total length to maintain between vehicles.
  54.      Lrand=DTime/(DTime+Tau_rand);                 // EWMA coefficient based on Tau_rand
  55.      Rand_max=AccMax/(.58*sqrt(Lrand/(2-Lrand)));  // Unif dist scaled to compensate EWMA narrowing of dist
  56.  
  57.     Vel[1][3] = 0;
  58.     for (int j=0; j<=Nveh; j++){
  59.      Acce[0][j]      = Acce[1][j]      = Acce[2][j]      = 0 ; // Set every accel to 0
  60.      Vel[0][j]      = Vel[1][j]      = Vel[2][j]      = 0 ; // Set every element in array to value 0 m/s
  61.      TargetSep=Tspace*fabs(Vel[1][3]) + VehLength;           // Calc Vehicle separtion based on velocity and time separation
  62.      Pos[0][j]      =  Pos[1][j]     = Pos[2][j]      = eyec.z + (j-3)* (TargetSep + VehZone);
  63.      Acc_rand[0][j] = Acc_rand[1][j] = Acc_rand[2][j] = 0 ;
  64.     }
  65.  
  66.  
  67.  
  68.  
  69.  
  70.      MinSep=-TargetSep;
  71.      //
  72.      srand( (unsigned)time( NULL ) );        // Sets the random number generator seed
  73.      //
  74. }
  75. float 
  76.             cPos10,
  77.             cPos11,
  78.             cPos12 ;
  79.  
  80. void Calc_Car_pos()
  81. {
  82. /*
  83.    ! DRIVE_S4_10 (JVJ 1999SEP21 MOD JVJ 1999SEP21)
  84.    ! Program to simulate Nveh vehicles in line with random accels and collision avoidance
  85.    ! Change force on trailing vehicles to be damped, anharmonic oscillator
  86.    ! Trailing cars look at car ahead for relative position, two cars ahead for relative velocity
  87.    ! Change def of AccMax, and greatly reduced DecMax (earlier value would now be written 5*Rand_max)
  88.    ! Made DecMax a fixed value of 8 (not a multiple of AccMax) */
  89.  
  90.  
  91.      // INPUT "Do you want to create a file? [Y/N]",Ans$  ! File or screen output only
  92.      // Fprint=(UPC$(Ans$[1])="Y")
  93.  
  94.        Tyme=Tyme + DTime;                         // Increment time value by fixed amount
  95.  
  96.  
  97.     // If I jump in Hyper space using my configuration. Let them wait for me...
  98.     // or if I collide with the red car.
  99.       if ( ( (eyec.z - Pos[1][0]) > 200) || (eyec.z - Pos[1][2]) < 5)  {
  100.         Pos[1][0] = eyec.z -199;
  101.         Pos[1][1] = eyec.z -190;
  102.         Pos[1][2] = eyec.z - 180;
  103.         PlaySound ("booom.wav",NULL,SND_ASYNC); //WinUnix
  104.     }
  105.  
  106.     // Crisis occurs here.
  107.     if ((fabs(eyec.z - Pos[1][2]) < 40) && (CRISIS == 0) ){ // If I am closer then 40 meters.
  108.         if (rand() < 10000)    {            // then 10000/32000 of the times do a crisis.
  109.             PlaySound ("boom.wav",NULL,SND_ASYNC); //WinUnix
  110.             CRISIS = 1;
  111.             Vel[1][0] = 0; // Fantom vehicle stops.
  112.             cPos10 = Pos[1][0];
  113.             cPos11 = Pos[1][1];
  114.             cPos12 = Pos[1][2];
  115.         }
  116.     }        
  117.  
  118.     // Accelerate the fantom vehicle
  119.     if (fabs(Vel[1][0]) < PaceSpeed) 
  120.        Acce[1][0] = -AccMax / 4;
  121.     else
  122.       Acce[1][0] = 0;
  123.       Vel[1][0] = Vel[1][0] + Acce[1][0] * DTime;
  124.       Pos[1][0] = Pos[1][0] + Vel[1][0] * DTime;
  125.  
  126.       Acce[1][3] = lkc_rt.z * AccelScalar; // To be changed back to lk_rt
  127.       Vel[1][3] = Vel[0][0] = Vel[2][0] = Velocity.z;
  128.       Pos[1][3] = eyec.z;
  129.       TargetSep = Tspace * fabs(Vel[1][3]) + VehLength;
  130.       Pos[0][0] = Pos[2][0] = Pos[1][3] - 3 * (TargetSep + VehZone);
  131.  
  132.     for (int i=0; i<3; i++){
  133.         // This calculates the max absolute velocity of column i.
  134.         /*float memMax = Vel[i][1];
  135.         for (K=2;K<=Nveh;K++)                          
  136.             if (memMax < Vel[i][K]) memMax = Vel[i][K];
  137.         float memMin = Vel[i][1];                         
  138.         for (K=2;K<Nveh;K++)                          
  139.             if (memMin > Vel[i][K]) memMin = Vel[i][K];
  140.         TargetSep=Tspace*__max(fabs(memMax),fabs(memMin)); */
  141.  
  142.        
  143.  
  144.         for (K=1; K<=Nveh; K++){
  145.             if ((i!=1) || (K!=3)) { // Don't calculate the subject vehicle     
  146.                 Rand[i][K]=rand()/32767.0; // generates a float number between 0 and 1.
  147.                 Acc_rand[i][K]=(1-Lrand)*Acc_rand[i][K]+Lrand*Rand_max*(2*Rand[i][K]-1);  // EWMA of random accels
  148.                 Sep=Pos[i][K]-Pos[i][K-1]-VehZone; // Sep is distance to leading vehicle zone.
  149.                 MinSep=__min(Sep,MinSep);          // See how close to (or below) zero Sep ever gets
  150.                 if (Sep <= 0){                        // This is collsion (if Vlength is actual vehicle length)
  151.                     AccSpring=DecMax;                     // Assign to max spring force allowed
  152.                 }
  153.                 else{
  154.                     AccSpring=-Freq*Freq*(Sep-TargetSep*TargetSep/Sep);  // Anharmonic spring force
  155.                     AccSpring=__min(AccSpring,DecMax);        // Clip to max spring force allowed
  156.                 }
  157.                 AccDamp=0;                                // Calc damping from rel vel to each of 2 vehicles ahead
  158.                 for (J=__max(0,K-2); J<= K-1; J++)
  159.                     AccDamp=AccDamp-Damp*(Vel[i][K]-Vel[i][J]);
  160.                 Acce[i][K]=Acc_rand[i][K]+AccDamp+AccSpring;      // Total accel is rand minus damping minus spring
  161.             }// if ((i!=1...))
  162.         }  // for (K=0;K<Nveh)
  163.     }
  164.     for (i=0; i<3; i++){
  165.         for ( K=1; K<= Nveh; K++) {
  166.             if ((i!=1) || (K!=3)) { // Don't calculate the subject vehicle
  167.                 Vel[i][K]=Vel[i][K]+Acce[i][K]*DTime;              // Calc new velocity based on new accel
  168.                 Pos[i][K]=Pos[i][K]+Vel[i][K]*DTime;              // Calc new position based on new velocity
  169.             }
  170.         }
  171.     }
  172.  
  173.     if (CRISIS){
  174.             Pos[1][0] = cPos10 ;
  175.             Pos[1][1] = cPos11 ;
  176.             Pos[1][2] = cPos12 ;
  177.     }
  178.        // fprintf (fileJohn, "%f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \n", Tyme, Rand[0][1], Acc_rand[0][1], Acce[0][1], Vel[0][1], Pos[0][1], Pos[0][0] ,Rand[2][1], Acc_rand[2][1], Acce[2][1], Vel[2][1], Pos[2][1], Pos[2][0]);
  179. }
  180.  
  181. /*
  182.  ----               ^
  183.                     |
  184.   *     ^           |
  185.   *     |           |
  186.   *  VehLength   VehZone = VehLength + DeadZone
  187.   *     |           |
  188.   *     v           |
  189.                     |
  190. ----                v
  191.         ^
  192.         |
  193.         |
  194.         |
  195.        Sep (Positive in our coordinate system, z going down).
  196.         |
  197.         |
  198.         |
  199.         v
  200.  ----               ^
  201.                     |
  202.   *     ^           |
  203.   *     |           |
  204.   *  VehLength   VehZone = VehLength + DeadZone
  205.   *     |           |
  206.   *     v           |
  207.                     |
  208. ----                v
  209.  
  210.   */