//==========================================================================* // Utility to normalize a 2D vector //--------------------------------------------------------------------------* TVec2d TUtils::VecUnit( const TVec2d& v ) { double h = myhypot(v.x, v.y); if( h == 0 ) return TVec2d(0, 0); else return TVec2d(v.x / h, v.y / h); }
//==========================================================================* // Utility to find distance of a point on a line (Lot auf Linie) //--------------------------------------------------------------------------* double TUtils::DistPtFromLine( double ptx, double pty, double px, double py, double vx, double vy ) { double t = ClosestPtOnLine(ptx, pty, px, py, vx, vy); double qx = px + vx * t; double qy = py + vy * t; double dist = myhypot(ptx - qx, pty - qy); return dist; }
static void QLalgo2 (int n, double *d, double *e, double** V) { /* * - > n : Dimen*sion. * -> d : Diagonale of tridiagonal matrix. * -> e[1..n-1] : off-diagonal, output from Householder * -> V : matrix output von Householder * <- d : eigenvalues * <- e : garbage? * <- V : basis of eigenvectors, according to d * * Symmetric tridiagonal QL algorithm, iterative * Computes the eigensystem from a tridiagonal matrix in roughtly 3N^3 operations * * code adapted from Java JAMA package, function tql2. */ int i, k, l, m; double f = 0.0; double tst1 = 0.0; double eps = 2.22e-16; /* Math.pow(2.0,-52.0); == 2.22e-16 */ /* shift input e */ for (i = 1; i < n; i++) { e[i-1] = e[i]; } e[n-1] = 0.0; /* never changed again */ for (l = 0; l < n; l++) { /* Find small subdiagonal element */ if (tst1 < fabs(d[l]) + fabs(e[l])) tst1 = fabs(d[l]) + fabs(e[l]); m = l; while (m < n) { if (fabs(e[m]) <= eps*tst1) { /* if (fabs(e[m]) + fabs(d[m]+d[m+1]) == fabs(d[m]+d[m+1])) { */ break; } m++; } /* If m == l, d[l] is an eigenvalue, */ /* otherwise, iterate. */ if (m > l) { /* TODO: check the case m == n, should be rejected here!? */ int iter = 0; do { /* while (fabs(e[l]) > eps*tst1); */ double dl1, h; double g = d[l]; double p = (d[l+1] - g) / (2.0 * e[l]); double r = myhypot(p, 1.); iter = iter + 1; /* Could check iteration count here */ /* Compute implicit shift */ if (p < 0) { r = -r; } d[l] = e[l] / (p + r); d[l+1] = e[l] * (p + r); dl1 = d[l+1]; h = g - d[l]; for (i = l+2; i < n; i++) { d[i] -= h; } f = f + h; /* Implicit QL transformation. */ p = d[m]; { double c = 1.0; double c2 = c; double c3 = c; double el1 = e[l+1]; double s = 0.0; double s2 = 0.0; for (i = m-1; i >= l; i--) { c3 = c2; c2 = c; s2 = s; g = c * e[i]; h = c * p; r = myhypot(p, e[i]); e[i+1] = s * r; s = e[i] / r; c = p / r; p = c * d[i] - s * g; d[i+1] = h + s * (c * g + s * d[i]); /* Accumulate transformation. */ for (k = 0; k < n; k++) { h = V[k][i+1]; V[k][i+1] = s * V[k][i] + c * h; V[k][i] = c * V[k][i] - s * h; } } p = -s * s2 * c3 * el1 * e[l] / dl1; e[l] = s * p; d[l] = c * p; } /* Check for convergence. */ } while (fabs(e[l]) > eps*tst1); } d[l] = d[l] + f; e[l] = 0.0; } /* Sort eigenvalues and corresponding vectors. */ #if 1 /* TODO: really needed here? So far not, but practical and only O(n^2) */ { int j; double p; for (i = 0; i < n-1; i++) { k = i; p = d[i]; for (j = i+1; j < n; j++) { if (d[j] < p) { k = j; p = d[j]; } } if (k != i) { d[k] = d[i]; d[i] = p; for (j = 0; j < n; j++) { p = V[j][i]; V[j][i] = V[j][k]; V[j][k] = p; } } } } #endif } /* QLalgo2 */
//==========================================================================* // Update // // ATTENTION oCar ist opponents car, so we can use our shortcuts!!! //--------------------------------------------------------------------------* void TOpponent::Update( const PCarElt MyCar, double MyDirX, double MyDirY, float &MinDistBack, double &MinTimeSlot) { if((CarState & RM_CAR_STATE_NO_SIMU) && // omit cars out of race (CarState & RM_CAR_STATE_PIT) == 0 ) // if not in pit return; oInfo.State.Speed = myhypot(CarSpeedX,CarSpeedY);// Speed of car // Track relative speed of opponents car TVec2d ToRight = oTrack->Normale(DistanceFromStartLine); oInfo.State.TrackVelLong = ToRight.x * CarSpeedY - ToRight.y * CarSpeedX; oInfo.State.TrackVelLat = ToRight.x * CarSpeedX + ToRight.y * CarSpeedY; // Track relative yaw of other car. oInfo.State.TrackYaw = CarYaw - TUtils::VecAngle(ToRight) - PI / 2; DOUBLE_NORM_PI_PI(oInfo.State.TrackYaw); // Average velocity of other car. oInfo.State.AvgVelLong = oInfo.State.AvgVelLong * AVG_KEEP + CarPubGlobVelX * AVG_CHANGE; oInfo.State.AvgVelLat = oInfo.State.AvgVelLat * AVG_KEEP + CarPubGlobVelY * AVG_CHANGE; oInfo.State.CarAvgVelLong = MyDirX * oInfo.State.AvgVelLong + MyDirY * oInfo.State.AvgVelLat; //oInfo.State.CarAvgVelLat = MyDirY * oInfo.State.AvgVelLong - MyDirX * oInfo.State.AvgVelLat; // Average acceleration of other car. oInfo.State.AvgAccLong = oInfo.State.AvgAccLong * AVG_KEEP + CarPubGlobAccX * AVG_CHANGE; oInfo.State.AvgAccLat = oInfo.State.AvgAccLat * AVG_KEEP + CarPubGlobAccY * AVG_CHANGE; oInfo.State.CarAvgAccLong = MyDirX * oInfo.State.AvgAccLong + MyDirY * oInfo.State.AvgAccLat; oInfo.State.CarAvgAccLat = MyDirY * oInfo.State.AvgAccLong - MyDirX * oInfo.State.AvgAccLat; // Offset from track center line. oInfo.State.Offset = -CarToMiddle; if(oCar == MyCar) return; // Car-Car relative calculations ... // calc other cars position, velocity relative to my car (global coords). double DistX = CarPubGlobPosX - MyCar->pub.DynGCg.pos.x; double DistY = CarPubGlobPosY - MyCar->pub.DynGCg.pos.y; double DiffVelX = CarSpeedX - MyCar->_speed_X; double DiffVelY = CarSpeedY - MyCar->_speed_Y; // work out relative position, velocity in local coords (coords of my car). oInfo.State.CarDistLong = MyDirX * DistX + MyDirY * DistY; oInfo.State.CarDistLat = MyDirY * DistX - MyDirX * DistY; oInfo.State.CarDiffVelLong = MyDirX * DiffVelX + MyDirY * DiffVelY; oInfo.State.CarDiffVelLat = MyDirY * DiffVelX - MyDirX * DiffVelY; oInfo.State.MinDistLong = (MyCar->_dimension_x + CarLength) / 2; oInfo.State.MinDistLat = (MyCar->_dimension_y + CarWidth) / 2; double MyVelAng = atan2(MyCar->_speed_Y, MyCar->_speed_X); double MyYaw = MyCar->_yaw - MyVelAng; DOUBLE_NORM_PI_PI(MyYaw); double OppYaw = CarYaw - MyVelAng; DOUBLE_NORM_PI_PI(OppYaw); // Additional distance needed while yawing of both cars double ExtSide = (oInfo.State.MinDistLong - oInfo.State.MinDistLat) * (fabs(sin(MyYaw)) + fabs(sin(OppYaw))); oInfo.State.MinDistLat += ExtSide + SIDE_MARGIN; oInfo.State.MinDistLong += TDriver::LengthMargin; // Distance of car from start of track. double MyPos = RtGetDistFromStart((tCarElt*)MyCar); double HisPos = RtGetDistFromStart((tCarElt*)oCar); double RelPos = HisPos - MyPos; double TrackLen = oTrack->Length(); if (RelPos > TrackLen / 2) RelPos -= TrackLen; else if (RelPos < -TrackLen / 2) RelPos += TrackLen; oInfo.State.RelPos = RelPos; if (fabs(CarToMiddle) - oTrack->Width() > 1.0) // If opponent is outside of track { // we assume it is in the pitlane if ((RelPos > MinDistBack) // Opponent is near && (RelPos < 5)) // and not in front { MinDistBack = (tdble) RelPos; } double T = -RelPos/oInfo.State.TrackVelLong; // Time to start out of pit if ((T > 0) // Opponent is back or aside && (T < 200)) // and arrives within 20 sec { if (MinTimeSlot > T) MinTimeSlot = T; } } }
//==========================================================================* // Utility to get length of 3D-Vector in 2D projection //--------------------------------------------------------------------------* double TUtils::VecLenXY( const TVec3d& v ) { return myhypot(v.y, v.x); }