void AiCarStandard::AnalyzeOthers(float dt, const CarDynamics cars[], const int cars_num)
{
const float half_carlength = 1.25;
const btVector3 throttle_axis = Direction::forward;
for (int i = 0; i != cars_num; ++i)
{
const CarDynamics * icar = &cars[i];
if (icar != car)
{
OtherCarInfo & info = othercars[icar];
// find direction of other cars in our frame
btVector3 relative_position = icar->GetCenterOfMass() - car->GetCenterOfMass();
relative_position = quatRotate(-car->GetOrientation(), relative_position);
// only make a move if the other car is within our distance limit
float fore_position = relative_position.dot(throttle_axis);
btVector3 myvel = quatRotate(-car->GetOrientation(), car->GetVelocity());
btVector3 othervel = quatRotate(-icar->GetOrientation(), icar->GetVelocity());
float speed_diff = othervel.dot(throttle_axis) - myvel.dot(throttle_axis);
const float fore_position_offset = -half_carlength;
if (fore_position > fore_position_offset)
{
const Bezier * othercarpatch = GetCurrentPatch(icar);
const Bezier * mycarpatch = GetCurrentPatch(car);
if (othercarpatch && mycarpatch)
{
Vec3 mypos = ToMathVector<float>(car->GetCenterOfMass());
Vec3 otpos = ToMathVector<float>(icar->GetCenterOfMass());
float my_track_placement = GetHorizontalDistanceAlongPatch(*mycarpatch, mypos);
float their_track_placement = GetHorizontalDistanceAlongPatch(*othercarpatch, otpos);
float speed_diff_denom = clamp(speed_diff, -100, -0.01);
float eta = (fore_position - fore_position_offset) / -speed_diff_denom;
if (!info.active)
info.eta = eta;
else
info.eta = RateLimit(info.eta, eta, 10.f*dt, 10000.f*dt);
info.horizontal_distance = their_track_placement - my_track_placement;
info.fore_distance = fore_position;
info.active = true;
}
else
{
info.active = false;
}
}
else
{
info.active = false;
}
}
}
}
void AI::analyzeOthers(AI_Car *c, float dt, const std::list <CAR> & othercars)
{
//const float speed = std::max(1.0f,c->car->GetVelocity().Magnitude());
const float half_carlength = 1.25; //in meters
//std::cout << speed << ": " << authority << std::endl;
//const MATHVECTOR <float, 3> steer_right_axis = direction::Right;
const MATHVECTOR <float, 3> throttle_axis = direction::Forward;
#ifdef VISUALIZE_AI_DEBUG
//c->avoidancedraw->ClearLine();
#endif
for (std::list <CAR>::const_iterator i = othercars.begin(); i != othercars.end(); ++i)
{
if (&(*i) != c->car)
{
struct AI_Car::OTHERCARINFO & info = c->othercars[&(*i)];
//find direction of othercar in our frame
MATHVECTOR <float, 3> relative_position = i->GetCenterOfMassPosition() - c->car->GetCenterOfMassPosition();
(-c->car->GetOrientation()).RotateVector(relative_position);
//std::cout << relative_position.dot(throttle_axis) << ", " << relative_position.dot(steer_right_axis) << std::endl;
//only make a move if the other car is within our distance limit
float fore_position = relative_position.dot(throttle_axis);
//float speed_diff = i->GetVelocity().dot(throttle_axis) - c->car->GetVelocity().dot(throttle_axis); //positive if other car is faster
MATHVECTOR <float, 3> myvel = c->car->GetVelocity();
MATHVECTOR <float, 3> othervel = i->GetVelocity();
(-c->car->GetOrientation()).RotateVector(myvel);
(-i->GetOrientation()).RotateVector(othervel);
float speed_diff = othervel.dot(throttle_axis) - myvel.dot(throttle_axis); //positive if other car is faster
//std::cout << speed_diff << std::endl;
//float distancelimit = clamp(distancelimitcoeff*-speed_diff, distancelimitmin, distancelimitmax);
const float fore_position_offset = -half_carlength;
if (fore_position > fore_position_offset)// && fore_position < distancelimit) //only pay attention to cars roughly in front of us
{
//float horizontal_distance = relative_position.dot(steer_right_axis); //fallback method if not on a patch
//float orig_horiz = horizontal_distance;
const BEZIER * othercarpatch = GetCurrentPatch(&(*i));
const BEZIER * mycarpatch = GetCurrentPatch(c->car);
if (othercarpatch && mycarpatch)
{
float my_track_placement = GetHorizontalDistanceAlongPatch(*mycarpatch, TransformToPatchspace(c->car->GetCenterOfMassPosition()));
float their_track_placement = GetHorizontalDistanceAlongPatch(*othercarpatch, TransformToPatchspace(i->GetCenterOfMassPosition()));
float speed_diff_denom = clamp(speed_diff, -100, -0.01);
float eta = (fore_position-fore_position_offset)/-speed_diff_denom;
info.fore_distance = fore_position;
if (!info.active)
info.eta = eta;
else
info.eta = RateLimit(info.eta, eta, 10.f*dt, 10000.f*dt);
float horizontal_distance = their_track_placement - my_track_placement;
//if (!info.active)
info.horizontal_distance = horizontal_distance;
/*else
info.horizontal_distance = RateLimit(info.horizontal_distance, horizontal_distance, spacingdistance*dt, spacingdistance*dt);*/
//std::cout << info.horizontal_distance << ", " << info.eta << std::endl;
info.active = true;
}
else
info.active = false;
//std::cout << orig_horiz << ", " << horizontal_distance << ", " << fore_position << ", " << speed_diff << std::endl;
/*if (!min_horizontal_distance)
min_horizontal_distance = optional <float> (horizontal_distance);
else if (std::abs(min_horizontal_distance.get()) > std::abs(horizontal_distance))
min_horizontal_distance = optional <float> (horizontal_distance);*/
}
else
info.active = false;
/*#ifdef VISUALIZE_AI_DEBUG
if (info.active)
{
c->avoidancedraw->AddLinePoint(c->car->GetCenterOfMassPosition());
MATHVECTOR <float, 3> feeler1(speed*info.eta,0,0);
c->car->GetOrientation().RotateVector(feeler1);
MATHVECTOR <float, 3> feeler2(0,-info.horizontal_distance,0);
c->car->GetOrientation().RotateVector(feeler2);
c->avoidancedraw->AddLinePoint(c->car->GetCenterOfMassPosition()+feeler1+feeler2);
c->avoidancedraw->AddLinePoint(c->car->GetCenterOfMassPosition());
}
#endif*/
}
}
}
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 AI::updateGasBrake(AI_Car *c)
{
c->brakelook.clear();
float brake_value = 0.0;
float gas_value = 0.5;
const float speed_percent = 1.0;
if (c->car->GetEngineRPM() < c->car->GetEngineStallRPM())
c->inputs[CARINPUT::START_ENGINE] = 1.0;
else
c->inputs[CARINPUT::START_ENGINE] = 0.0;
calcMu(c);
const BEZIER *curr_patch_ptr = GetCurrentPatch(c->car);
//if car is not on track, just let it roll
if (!curr_patch_ptr)
{
c->inputs[CARINPUT::THROTTLE] = 0.8;
c->inputs[CARINPUT::BRAKE] = 0.0;
return;
}
BEZIER curr_patch = RevisePatch(curr_patch_ptr, c->use_racingline);
//BEZIER curr_patch = *curr_patch_ptr;
MATHVECTOR <float, 3> patch_direction = TransformToWorldspace(GetPatchDirection(curr_patch));
//this version uses the velocity along tangent vector. it should calculate a lower current speed,
//hence higher gas value or lower brake value
//float currentspeed = c->car->chassis().cm_velocity().component(direction_vector);
float currentspeed = c->car->GetVelocity().dot(patch_direction.Normalize());
//this version just uses the velocity, do not care about the direction
//float currentspeed = c->car->chassis().cm_velocity().magnitude();
//check speed against speed limit of current patch
float speed_limit = 0;
if (!curr_patch.next_patch)
{
speed_limit = calcSpeedLimit(c, &curr_patch, NULL, c->lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude())*speed_percent;
}
else
{
BEZIER next_patch = RevisePatch(curr_patch.next_patch, c->use_racingline);
speed_limit = calcSpeedLimit(c, &curr_patch, &next_patch, c->lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude())*speed_percent;
}
speed_limit *= c->difficulty;
float speed_diff = speed_limit - currentspeed;
if (speed_diff < 0.0)
{
if (-speed_diff < MIN_SPEED_DIFF) //no need to brake if diff is small
{
brake_value = 0.0;
}
else
{
brake_value = -speed_diff / MAX_SPEED_DIFF;
if (brake_value > 1.0) brake_value = 1.0;
}
gas_value = 0.0;
}
else if (isnan(speed_diff) || speed_diff > MAX_SPEED_DIFF)
{
gas_value = 1.0;
brake_value = 0.0;
}
else
{
gas_value = speed_diff / MAX_SPEED_DIFF;
brake_value = 0.0;
}
//check upto maxlookahead distance
float maxlookahead = calcBrakeDist(c, currentspeed, 0.0, c->longitude_mu)+10;
//maxlookahead = 0.1;
float dist_checked = 0.0;
float brake_dist = 0.0;
BEZIER patch_to_check = curr_patch;
#ifdef VISUALIZE_AI_DEBUG
c->brakelook.push_back(patch_to_check);
#endif
while (dist_checked < maxlookahead)
{
BEZIER * unmodified_patch_to_check = patch_to_check.next_patch;
//if there is no next patch(probably a non-closed track, just let it roll
if (!patch_to_check.next_patch)
{
brake_value = 0.0;
dist_checked = maxlookahead;
break;
}
else
patch_to_check = RevisePatch(patch_to_check.next_patch, c->use_racingline);
#ifdef VISUALIZE_AI_DEBUG
c->brakelook.push_back(patch_to_check);
#endif
//speed_limit = calcSpeedLimit(c, &patch_to_check, c->lateral_mu)*speed_percent;
if (!patch_to_check.next_patch)
{
speed_limit = calcSpeedLimit(c, &patch_to_check, NULL, c->lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude())*speed_percent;
}
else
{
BEZIER next_patch = RevisePatch(patch_to_check.next_patch, c->use_racingline);
speed_limit = calcSpeedLimit(c, &patch_to_check, &next_patch, c->lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude())*speed_percent;
}
dist_checked += GetPatchDirection(patch_to_check).Magnitude();
brake_dist = calcBrakeDist(c, currentspeed, speed_limit, c->longitude_mu);
//if (brake_dist + CORNER_BRAKE_OFFSET > dist_checked)
if (brake_dist > dist_checked)
{
//std::cout << "brake: limit " << speed_limit << ", cur " << currentspeed << ", brake " << brake_dist << ", dist " << dist_checked << std::endl;
/*brake_value = (brake_dist + CORNER_BRAKE_OFFSET - dist_checked)*CORNER_BRAKE_GAIN;
if (brake_value > 1.0) brake_value = 1.0;*/
brake_value = 1.0;
gas_value = 0.0;
break;
}
}
//std::cout << speed_limit << std::endl;
if (c->car->GetGear() == 0)
{
c->inputs[CARINPUT::SHIFT_UP] = 1.0;
gas_value = 0.2;
}
else
{
c->inputs[CARINPUT::SHIFT_UP] = 0.0;
}
/*float trafficbrake = brakeFromOthers(c, dt, othercars, speed_diff); //consider traffic avoidance bias
if (trafficbrake > 0)
{
gas_value = 0.0;
brake_value = std::max(trafficbrake, brake_value);
}*/
c->inputs[CARINPUT::THROTTLE] = RateLimit(c->inputs[CARINPUT::THROTTLE], gas_value,
THROTTLE_RATE_LIMIT, THROTTLE_RATE_LIMIT);
c->inputs[CARINPUT::BRAKE] = RateLimit(c->inputs[CARINPUT::BRAKE], brake_value,
BRAKE_RATE_LIMIT, BRAKE_RATE_LIMIT);
//c->inputs[CARINPUT::THROTTLE] = 0.0;
//c->inputs[CARINPUT::BRAKE] = 1.0;
}
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;
}
void AiCarStandard::UpdateGasBrake()
{
#ifdef VISUALIZE_AI_DEBUG
brakelook.clear();
#endif
float brake_value = 0.0;
float gas_value = 0.5;
if (car->GetEngine().GetRPM() < car->GetEngine().GetStallRPM())
inputs[CarInput::START_ENGINE] = 1.0;
else
inputs[CarInput::START_ENGINE] = 0.0;
CalcMu();
const Bezier * curr_patch_ptr = GetCurrentPatch(car);
if (!curr_patch_ptr)
{
// if car is not on track, just let it roll
inputs[CarInput::THROTTLE] = 0.8;
inputs[CarInput::BRAKE] = 0.0;
return;
}
Bezier curr_patch = RevisePatch(curr_patch_ptr, use_racingline);
const Vec3 patch_direction = GetPatchDirection(curr_patch).Normalize();
const Vec3 car_velocity = ToMathVector<float>(car->GetVelocity());
float currentspeed = car_velocity.dot(patch_direction);
// check speed against speed limit of current patch
float speed_limit = 0;
if (!curr_patch.GetNextPatch())
{
speed_limit = CalcSpeedLimit(&curr_patch, NULL, lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude());
}
else
{
Bezier next_patch = RevisePatch(curr_patch.GetNextPatch(), use_racingline);
speed_limit = CalcSpeedLimit(&curr_patch, &next_patch, lateral_mu, GetPatchWidthVector(*curr_patch_ptr).Magnitude());
}
speed_limit *= difficulty;
float speed_diff = speed_limit - currentspeed;
if (speed_diff < 0.0)
{
if (-speed_diff < MIN_SPEED_DIFF) //no need to brake if diff is small
{
brake_value = 0.0;
}
else
{
brake_value = -speed_diff / MAX_SPEED_DIFF;
if (brake_value > 1.0) brake_value = 1.0;
}
gas_value = 0.0;
}
else if (std::isnan(speed_diff) || speed_diff > MAX_SPEED_DIFF)
{
gas_value = 1.0;
brake_value = 0.0;
}
else
{
gas_value = speed_diff / MAX_SPEED_DIFF;
brake_value = 0.0;
}
// check upto maxlookahead distance
float maxlookahead = CalcBrakeDist(currentspeed, 0.0, longitude_mu)+10;
float dist_checked = 0.0;
float brake_dist = 0.0;
Bezier patch_to_check = curr_patch;
#ifdef VISUALIZE_AI_DEBUG
brakelook.push_back(patch_to_check);
#endif
while (dist_checked < maxlookahead)
{
Bezier * unmodified_patch_to_check = patch_to_check.GetNextPatch();
if (!patch_to_check.GetNextPatch())
{
// if there is no next patch(probably a non-closed track, just let it roll
brake_value = 0.0;
dist_checked = maxlookahead;
break;
}
else
{
patch_to_check = RevisePatch(patch_to_check.GetNextPatch(), use_racingline);
}
#ifdef VISUALIZE_AI_DEBUG
brakelook.push_back(patch_to_check);
#endif
if (!patch_to_check.GetNextPatch())
{
speed_limit = CalcSpeedLimit(&patch_to_check, NULL, lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude());
}
else
{
Bezier next_patch = RevisePatch(patch_to_check.GetNextPatch(), use_racingline);
speed_limit = CalcSpeedLimit(&patch_to_check, &next_patch, lateral_mu, GetPatchWidthVector(*unmodified_patch_to_check).Magnitude());
}
dist_checked += GetPatchDirection(patch_to_check).Magnitude();
brake_dist = CalcBrakeDist(currentspeed, speed_limit, longitude_mu);
if (brake_dist > dist_checked)
{
brake_value = 1.0;
gas_value = 0.0;
break;
}
}
gas_value = RateLimit(inputs[CarInput::THROTTLE], gas_value, THROTTLE_RATE_LIMIT, THROTTLE_RATE_LIMIT);
brake_value = RateLimit(inputs[CarInput::BRAKE], brake_value, BRAKE_RATE_LIMIT, BRAKE_RATE_LIMIT);
inputs[CarInput::THROTTLE] = gas_value;
inputs[CarInput::BRAKE] = brake_value;
}