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 >
Wrap
C/C++ Source or Header
|
2000-04-18
|
8KB
|
224 lines
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <iostream.h>
#include <string.h>
#include <math.h>
// #include <windows.h>
// #include <mmsystem.h> // WinUnix
struct point {float x,y,z;};
extern float DTime;
extern struct point eyec, Acceleration, Velocity, lkc_rt, up_rt;
extern float AccelScalar;
extern int CRISIS;
/* getrandom returns a random number between min and max, which must be in
* integer range. */
char *filename;
FILE *fileJohn;
#define getrandom( min, max ) ((rand() % (int)(((max) + 1) - (min))) + (min))
#define AllVeh 6
int I,J,K,Nveh;
float Damp, Fprint, Freq,Lrand,MinSep,Rand_max,Tau_rand,Tmax,Tspace,Tyme, VehZone, DeadZone;
float (*Pos)[AllVeh], (*Vel)[AllVeh], (*Acce)[AllVeh], (*Acc_rand)[AllVeh], (*Rand)[AllVeh];
float TargetSep,VehLength, Veh_spacing, AccMax,AccDamp,DecMax,Sep,AccSpring,PaceSpeed;
int max (int i1, int i2)
{
if (i1 > i2) return i1;
else return i2;
}
float min (float f1, float f2)
{
if (f1 > f2) return f1;
else return f2;
}
void initialise()
{
filename = new char [100];
strcpy (filename, "debug2.txt");
if ((fileJohn = fopen (filename, "w")) == NULL)
printf ("\n pas de chance l'ami ton ffile iest emmerde...\n");
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");
Nveh=AllVeh - 1; // Number of vehicles in lane
Pos = new float[3][AllVeh];
Vel = new float[3][AllVeh];
Acce = new float[3][AllVeh];
Acc_rand = new float[3][AllVeh];
Rand = new float[3][AllVeh];
Damp=1 ; // Friction coefficient [1/sec]
Freq=.2 ; // Ocsillation freq from "spring" [1/sec]
Tspace=1 ; // Time between vehicles used to calc desired TargetSep [sec]
VehLength=5 ; // Length of vehicle [m]
DeadZone = 5; // Extra length added to vehicle to avoid collision.
Tau_rand=.2 ; // Time constant used for EWMA filter in Acc_rand [sec]
AccMax=4 ; // Ymax will be approx one sigma of EWMA distrib [m/sec^2]
// DecMax=8 ; // Maximum braking decel
PaceSpeed = 20; // 20 m/s
DecMax = Damp * PaceSpeed + 2 * AccMax;
VehZone = VehLength + DeadZone; // Calculate the min total length to maintain between vehicles.
Lrand=DTime/(DTime+Tau_rand); // EWMA coefficient based on Tau_rand
Rand_max=AccMax/(.58*sqrt(Lrand/(2-Lrand))); // Unif dist scaled to compensate EWMA narrowing of dist
Vel[1][3] = 0;
for (int j=0; j<=Nveh; j++){
Acce[0][j] = Acce[1][j] = Acce[2][j] = 0 ; // Set every accel to 0
Vel[0][j] = Vel[1][j] = Vel[2][j] = 0 ; // Set every element in array to value 0 m/s
TargetSep=Tspace*fabs(Vel[1][3]) + VehLength; // Calc Vehicle separtion based on velocity and time separation
Pos[0][j] = Pos[1][j] = Pos[2][j] = eyec.z + (j-3)* (TargetSep + VehZone);
Acc_rand[0][j] = Acc_rand[1][j] = Acc_rand[2][j] = 0 ;
}
MinSep=-TargetSep;
//
srand( (unsigned)time( NULL ) ); // Sets the random number generator seed
//
}
float
cPos10,
cPos11,
cPos12 ;
void Calc_Car_pos()
{
/*
! DRIVE_S4_10 (JVJ 1999SEP21 MOD JVJ 1999SEP21)
! Program to simulate Nveh vehicles in line with random accels and collision avoidance
! Change force on trailing vehicles to be damped, anharmonic oscillator
! Trailing cars look at car ahead for relative position, two cars ahead for relative velocity
! Change def of AccMax, and greatly reduced DecMax (earlier value would now be written 5*Rand_max)
! Made DecMax a fixed value of 8 (not a multiple of AccMax) */
// INPUT "Do you want to create a file? [Y/N]",Ans$ ! File or screen output only
// Fprint=(UPC$(Ans$[1])="Y")
Tyme=Tyme + DTime; // Increment time value by fixed amount
// If I jump in Hyper space using my configuration. Let them wait for me...
// or if I collide with the red car.
if ( ( (eyec.z - Pos[1][0]) > 200) || (eyec.z - Pos[1][2]) < 5) {
Pos[1][0] = eyec.z -199;
Pos[1][1] = eyec.z -190;
Pos[1][2] = eyec.z - 180;
//PlaySound ("booom.wav",NULL,SND_ASYNC); //WinUnix
}
// Crisis occurs here.
if ((fabs(eyec.z - Pos[1][2]) < 40) && (CRISIS == 0) ){ // If I am closer then 40 meters.
if (rand() < 10000) { // then 10000/32000 of the times do a crisis.
//PlaySound ("boom.wav",NULL,SND_ASYNC); //WinUnix
CRISIS = 1;
Vel[1][0] = 0; // Fantom vehicle stops.
cPos10 = Pos[1][0];
cPos11 = Pos[1][1];
cPos12 = Pos[1][2];
}
}
// Accelerate the fantom vehicle
if (fabs(Vel[1][0]) < PaceSpeed)
Acce[1][0] = -AccMax / 4;
else
Acce[1][0] = 0;
Vel[1][0] = Vel[1][0] + Acce[1][0] * DTime;
Pos[1][0] = Pos[1][0] + Vel[1][0] * DTime;
Acce[1][3] = lkc_rt.z * AccelScalar; // To be changed back to lk_rt
Vel[1][3] = Vel[0][0] = Vel[2][0] = Velocity.z;
Pos[1][3] = eyec.z;
TargetSep = Tspace * fabs(Vel[1][3]) + VehLength;
Pos[0][0] = Pos[2][0] = Pos[1][3] - 3 * (TargetSep + VehZone);
for (int i=0; i<3; i++){
// This calculates the max absolute velocity of column i.
/*float memMax = Vel[i][1];
for (K=2;K<=Nveh;K++)
if (memMax < Vel[i][K]) memMax = Vel[i][K];
float memMin = Vel[i][1];
for (K=2;K<Nveh;K++)
if (memMin > Vel[i][K]) memMin = Vel[i][K];
TargetSep=Tspace*__max(fabs(memMax),fabs(memMin)); */
for (K=1; K<=Nveh; K++){
if ((i!=1) || (K!=3)) { // Don't calculate the subject vehicle
Rand[i][K]=rand()/32767.0; // generates a float number between 0 and 1.
Acc_rand[i][K]=(1-Lrand)*Acc_rand[i][K]+Lrand*Rand_max*(2*Rand[i][K]-1); // EWMA of random accels
Sep=Pos[i][K]-Pos[i][K-1]-VehZone; // Sep is distance to leading vehicle zone.
MinSep=min(Sep,MinSep); // See how close to (or below) zero Sep ever gets
if (Sep <= 0){ // This is collsion (if Vlength is actual vehicle length)
AccSpring=DecMax; // Assign to max spring force allowed
}
else{
AccSpring=-Freq*Freq*(Sep-TargetSep*TargetSep/Sep); // Anharmonic spring force
AccSpring=min(AccSpring,DecMax); // Clip to max spring force allowed
}
AccDamp=0; // Calc damping from rel vel to each of 2 vehicles ahead
for (J=max(0,K-2); J<= K-1; J++)
AccDamp=AccDamp-Damp*(Vel[i][K]-Vel[i][J]);
Acce[i][K]=Acc_rand[i][K]+AccDamp+AccSpring; // Total accel is rand minus damping minus spring
}// if ((i!=1...))
} // for (K=0;K<Nveh)
}
for (i=0; i<3; i++){
for ( K=1; K<= Nveh; K++) {
if ((i!=1) || (K!=3)) { // Don't calculate the subject vehicle
Vel[i][K]=Vel[i][K]+Acce[i][K]*DTime; // Calc new velocity based on new accel
Pos[i][K]=Pos[i][K]+Vel[i][K]*DTime; // Calc new position based on new velocity
}
}
}
if (CRISIS){
Pos[1][0] = cPos10 ;
Pos[1][1] = cPos11 ;
Pos[1][2] = cPos12 ;
}
// 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]);
}
/*
---- ^
|
* ^ |
* | |
* VehLength VehZone = VehLength + DeadZone
* | |
* v |
|
---- v
^
|
|
|
Sep (Positive in our coordinate system, z going down).
|
|
|
v
---- ^
|
* ^ |
* | |
* VehLength VehZone = VehLength + DeadZone
* | |
* v |
|
---- v
*/