/* -*- Mode: C -*-* */ /********************************************************************\ * File: ext-func.c * * Date: 12/02/1997 * * Author: Cem Unsal * * Robotics Institute, Carnegie-Mellon University * * unsal@ri.cmu.edu * * * * Description: * * External functions necessary for some of the Sensor and Human * * Driver Modeling files. * * * * This file is distributed under the conditions described in the * * file 'CONDITIONS' which should accompany this file. * \********************************************************************/ #include #include #include #define Pi 3.1415927 #define mmax(x,y) (x >= y) ? x : y #define mmin(x,y) (x <= y) ? x : y /* Function: arcsin * Date: April 1997 * * Description: * Returns arcsine of the angle */ double arcsin(double x) { return asin(x); } /* Function: dist5 * Date: July 1997 * * Description: * This function returns a vector of ranges for the given number of rays * for a shortrange sensor. Used by the detailed sensor model. * This subroutine looks for the intersection of the rays and the lines * defining the vehicles in range. * * Inputs: * senx, seny: Sensor position (global) * senor: Sensor orientation (global) * vehx[],vehy[]: Positions of the vehicles in range(global) * ca[], sa[]: Orientations of the vehicles in range (vgam's) * vl, vw: Vehicle lenght and width * rayvec[]: Orientation of the rays emitted from the sensor (global) * * Output: * distvec[]: Calculated distance for each ray (no dedection = 10000) */ double dist5(double senx, double seny, double senor, double vehx[], double vehy[], double ca[], double sa[], double n_of_veh, double vl, double vw, double rayvec[], double distvec[]) { int posx[] = {1, 1, -1, -1, 1}; int posy[] = {1, -1, -1, 1, 1}; int i,j,k; double dist, soldist; double x2a, y2a, x2b, y2b, a2, a1; double solx, soly; for (i = 0; i < 5; i++) { distvec[i] = 10000; // slope of the ray a1 = senor + rayvec[i]; for (j = 0; j < n_of_veh; j++) // for each vehicle, calculate intersections, and // take the closest intersection { for (k = 0; k < 4; k++) { //coordinates of the two corners x2a = vehx[j]+posx[k]*vl/2*ca[j]+posy[k]*vw/2*sa[j]; y2a = vehy[j]+posx[k]*vl/2*sa[j]-posy[k]*vw/2*ca[j]; x2b = vehx[j]+posx[k+1]*vl/2*ca[j]+posy[k+1]*vw/2*sa[j]; y2b = vehy[j]+posx[k+1]*vl/2*sa[j]-posy[k+1]*vw/2*ca[j]; // slope of the edge a2 = atan2(y2b-y2a, x2b-x2a); // if a1 = a2, no solution... dist. still 10000 if (a1 != a2) { // calculate intersection and distance solx = (tan(a1)*senx-tan(a2)*x2a-seny+y2a)/(tan(a1)-tan(a2)); soly = tan(a1)*(solx-senx)+seny; // check if the intersection is in the segment defining the // edge and the solution is in the 'field of view' if (solx >= (mmin(x2a,x2b))-0.01 && solx <= (mmax(x2a,x2b))+0.01 && soly >= (mmin(y2a,y2b))-0.01 && soly <= (mmax(y2a,y2b))+0.01 && fabs(atan2(soly-seny,solx-senx)-senor) <= fabs(rayvec[0])) { soldist = sqrt((senx-solx)*(senx-solx) +(seny-soly)*(seny-soly)); distvec[i] = mmin(distvec[i],soldist); } } } } } return 1; } /* Function: dist6 * Date: August 1997 * * Description: * This function returns a vector of ranges for the given number of rays * for a shortrange sensor. Used by the detailed sensor model. * This subroutine looks for the intersection of the rays and the lines * defining the vehicles in range. * * Inputs: * senx, seny: Sensor position (global) * senor: Sensor orientation (global) * vehx[],vehy[]: Positions of the vehicles in range(global) * ca[], sa[]: Orientations of the vehicles in range (vgam's) * n_of_veh: Number of vehicles * vl, vw: Vehicle lenght and width * maxrange: Maximum sensor range * hfov: Half the field of view * * Output: * reading[]: reading = [distance azimuth] * Distance and azimuth angle for closest vehicle */ double dist6(double senx, double seny, double senor, double vehx[], double vehy[], double ca[], double sa[], double n_of_veh, double vl, double vw, double maxrange, double hfov, double reading[]) { int xc[] = {1, 1, 0, -1, -1, 0}; // (Pseudo-)vertex coordinates int yc[] = {1, -1, -1 , -1, 1, 1}; // These values are multiplied // with vl and vw (See below) /* 4_____5______0 * | y | * | | | * | +--> x | * |____________| * 3 2 1 */ int i, j; int nj=6; double dist, az; double vrtxx, vrtxy; reading[0]=10; reading[1]=0; for (i = 0; i < n_of_veh; i++) { for (j = 0; j < nj-1; j++) { // vertex coordinates vrtxx = vehx[i]+xc[j]*vl/2*ca[i]+yc[j]*vw/2*sa[i]; vrtxy = vehy[i]+xc[j]*vl/2*sa[i]-yc[j]*vw/2*ca[i]; // distance and azimuth angle to vertex dist = eucdist(senx,seny,vrtxx,vrtxy); az = atan2(vrtxy-seny,vrtxx-senx)-senor; if (fabs(az) >= 2*Pi-fabs(az)) az = -(2*Pi-fabs(az))*sign2(az); // check if in range and in field-of-view and closer than current if (dist < maxrange && fabs(az) < hfov && dist < reading[0]) { reading[0]=dist; reading[1]=az; } } } return 1; } /* Function: eucdist * Date: March 1997 * * Description: * 2-D distance measurement */ double eucdist(double x1, double y1, double x2, double y2) { return sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)); } /* Function: min_of_array * Date: July 1997 * * Description: * This function returns the value and the index of the * array element with minimum value. * May be replaced with a SHIFT function in the future. */ double min_of_array (double vec[], double vecsize, double result[]) { int k; // result[0] = value ; result[1] = index result[0] = 10001; // Without these, the function does not work result[1] = 100; // if one of the results is not returned to dummy // variable (See below). // [Probably fixed by now - Dec. 1997] for (k = 0; k < vecsize; k++) { if (vec[k] <= result[0]) { result[0] = vec[k]; result[1]= k; } } return 2; //result[1]; // This is needed if no initialization above } /* Function: natlog * Date: April 1997 * * Description: * Returns natural logarithm */ double natlog (double x) { return log(x); }