//==========================================================================*
// 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);
}
