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