float AI::calcSpeedLimit(AI_Car *c, const BEZIER* patch, const BEZIER * nextpatch, float friction, float extraradius=0)
{
assert(patch);
//adjust the radius at corner exit to allow a higher speed.
//this will get the car to accellerate out of corner
//double track_width = GetPatchWidthVector(*patch).Magnitude();
double adjusted_radius = GetPatchRadius(*patch);
if (nextpatch)
{
if (GetPatchRadius(*nextpatch) > adjusted_radius &&
GetPatchRadius(*patch) > LOOKAHEAD_MIN_RADIUS)
{
adjusted_radius += extraradius;
}
}
//no downforce
//float v1 = sqrt(friction * GRAVITY * adjusted_radius);
//take into account downforce
double denom = (1.0 - std::min(1.01, adjusted_radius * -(c->car->GetAerodynamicDownforceCoefficient()) * friction * c->car->GetInvMass()));
double real = (friction * GRAVITY * adjusted_radius) / denom;
double v2 = 1000.0; //some really big number
if (real > 0)
v2 = sqrt(real);
//std::cout << v2 << ", " << sqrt(friction * GRAVITY * adjusted_radius) << ", " << GetPatchRadius(*patch) << ", " << acos((-GetPatchDirection(*patch)).Normalize().dot(GetPatchDirection(*patch->GetNextPatch()).Normalize()))*180.0/3.141593 << " --- " << -GetPatchDirection(*patch) << " --- " << GetPatchDirection(*patch->GetNextPatch()) << std::endl;
return v2;
}
void AI::updateSteer(AI_Car *c)
{
c->steerlook.clear();
const BEZIER *curr_patch_ptr = GetCurrentPatch(c->car);
//if car has no contact with track, just let it roll
if (!curr_patch_ptr)
{
if (!c->last_patch) return;
//if car is off track, steer the car towards the last patch it was on
//this should get the car back on track
else curr_patch_ptr = c->last_patch;
}
c->last_patch = curr_patch_ptr; //store the last patch car was on
BEZIER curr_patch = RevisePatch(curr_patch_ptr, c->use_racingline);
#ifdef VISUALIZE_AI_DEBUG
c->steerlook.push_back(curr_patch);
#endif
//if there is no next patch (probably a non-closed track), let it roll
if (!curr_patch.next_patch) return;
BEZIER next_patch = RevisePatch(curr_patch.next_patch, c->use_racingline);
//find the point to steer towards
float track_width = GetPatchWidthVector(curr_patch).Magnitude();
float lookahead = track_width * LOOKAHEAD_FACTOR1 +
c->car->GetVelocity().Magnitude() * LOOKAHEAD_FACTOR2;
lookahead = 1.0;
float length = 0.0;
MATHVECTOR <float, 3> dest_point = GetPatchFrontCenter(next_patch);
while (length < lookahead)
{
#ifdef VISUALIZE_AI_DEBUG
c->steerlook.push_back(next_patch);
#endif
length += GetPatchDirection(next_patch).Magnitude()*2.0;
dest_point = GetPatchFrontCenter(next_patch);
//if there is no next patch for whatever reason, stop lookahead
if (!next_patch.next_patch)
{
length = lookahead;
break;
}
next_patch = RevisePatch(next_patch.next_patch, c->use_racingline);
//if next patch is a very sharp corner, stop lookahead
if (GetPatchRadius(next_patch) < LOOKAHEAD_MIN_RADIUS)
{
length = lookahead;
break;
}
}
MATHVECTOR <float, 3> next_position = TransformToWorldspace(dest_point);
MATHVECTOR <float, 3> car_position = c->car->GetCenterOfMassPosition();
MATHVECTOR <float, 3> car_orientation = direction::Forward;
(c->car->GetOrientation()).RotateVector(car_orientation);
MATHVECTOR <float, 3> desire_orientation = next_position - car_position;
//car's direction on the horizontal plane
car_orientation[2] = 0;
//desired direction on the horizontal plane
desire_orientation[2] = 0;
car_orientation = car_orientation.Normalize();
desire_orientation = desire_orientation.Normalize();
//the angle between car's direction and unit y vector (forward direction)
double alpha = Angle(car_orientation[0], car_orientation[1]);
//the angle between desired direction and unit y vector (forward direction)
double beta = Angle(desire_orientation[0], desire_orientation[1]);
//calculate steering angle and direction
double angle = beta - alpha;
//angle += steerAwayFromOthers(c, dt, othercars, angle); //sum in traffic avoidance bias
if (angle > -360.0 && angle <= -180.0)
angle = -(360.0 + angle);
else if (angle > -180.0 && angle <= 0.0)
angle = - angle;
else if (angle > 0.0 && angle <= 180.0)
angle = - angle;
else if (angle > 180.0 && angle <= 360.0)
angle = 360.0 - angle;
float optimum_range = c->car->GetOptimumSteeringAngle();
angle = clamp(angle, -optimum_range, optimum_range);
float steer_value = angle / c->car->GetMaxSteeringAngle();
if (steer_value > 1.0) steer_value = 1.0;
else if (steer_value < -1.0) steer_value = -1.0;
assert(!isnan(steer_value));
c->inputs[CARINPUT::STEER_RIGHT] = steer_value;
}
void AiCarStandard::UpdateSteer()
{
#ifdef VISUALIZE_AI_DEBUG
steerlook.clear();
#endif
const Bezier *curr_patch_ptr = GetCurrentPatch(car);
//if car has no contact with track, just let it roll
if (!curr_patch_ptr)
{
if (!last_patch) return;
//if car is off track, steer the car towards the last patch it was on
//this should get the car back on track
else curr_patch_ptr = last_patch;
}
last_patch = curr_patch_ptr; //store the last patch car was on
Bezier curr_patch = RevisePatch(curr_patch_ptr, use_racingline);
#ifdef VISUALIZE_AI_DEBUG
steerlook.push_back(curr_patch);
#endif
// if there is no next patch (probably a non-closed track), let it roll
if (!curr_patch.GetNextPatch()) return;
Bezier next_patch = RevisePatch(curr_patch.GetNextPatch(), use_racingline);
// find the point to steer towards
float lookahead = 1.0;
float length = 0.0;
Vec3 dest_point = GetPatchFrontCenter(next_patch);
while (length < lookahead)
{
#ifdef VISUALIZE_AI_DEBUG
steerlook.push_back(next_patch);
#endif
length += GetPatchDirection(next_patch).Magnitude()*2.0;
dest_point = GetPatchFrontCenter(next_patch);
// if there is no next patch for whatever reason, stop lookahead
if (!next_patch.GetNextPatch())
{
length = lookahead;
break;
}
next_patch = RevisePatch(next_patch.GetNextPatch(), use_racingline);
// if next patch is a very sharp corner, stop lookahead
if (GetPatchRadius(next_patch) < LOOKAHEAD_MIN_RADIUS)
{
length = lookahead;
break;
}
}
btVector3 car_position = car->GetCenterOfMass();
btVector3 car_orientation = quatRotate(car->GetOrientation(), Direction::forward);
btVector3 desire_orientation = ToBulletVector(dest_point) - car_position;
//car's direction on the horizontal plane
car_orientation[2] = 0;
//desired direction on the horizontal plane
desire_orientation[2] = 0;
car_orientation.normalize();
desire_orientation.normalize();
//the angle between car's direction and unit y vector (forward direction)
double alpha = Angle(car_orientation[0], car_orientation[1]);
//the angle between desired direction and unit y vector (forward direction)
double beta = Angle(desire_orientation[0], desire_orientation[1]);
//calculate steering angle and direction
double angle = beta - alpha;
//angle += steerAwayFromOthers(c, dt, othercars, angle); //sum in traffic avoidance bias
if (angle > -360.0 && angle <= -180.0)
angle = -(360.0 + angle);
else if (angle > -180.0 && angle <= 0.0)
angle = - angle;
else if (angle > 0.0 && angle <= 180.0)
angle = - angle;
else if (angle > 180.0 && angle <= 360.0)
angle = 360.0 - angle;
float optimum_range = car->GetTire(FRONT_LEFT).getIdealSlipAngle() * SIMD_DEGS_PER_RAD;
angle = clamp(angle, -optimum_range, optimum_range);
float steer_value = angle / car->GetMaxSteeringAngle();
if (steer_value > 1.0) steer_value = 1.0;
else if (steer_value < -1.0) steer_value = -1.0;
assert(!std::isnan(steer_value));
inputs[CarInput::STEER_RIGHT] = steer_value;
}