void CConvoy::Update(void)
{
if(m_HitPoints <= 0)
{
Die();
return;
}
ConvoyTopMid = *GetFrame(0);
ConvoyTopMid.MoveUp(5.0f);
if (IsAtCurrentWaypoint())
NextWaypoint();
// Update the ambient engine sound
GetPosition(&m_CurPosition, 0);
CVector3f Velocity;
Velocity.x = (m_CurPosition.x - m_LastPosition.x) / g_FrameTime;
Velocity.y = (m_CurPosition.y - m_LastPosition.y) / g_FrameTime;
Velocity.z = (m_CurPosition.z - m_LastPosition.z) / g_FrameTime;
m_pEngineSFX->UpdateSound(m_CurPosition, Velocity);
m_LastPosition = m_CurPosition;
// Update the physics
m_RigidBody->StepIntegrate(g_FrameTime);
// Temp Moving
if(m_CurConvoyMoveState == CONVOY_MOVING)
MoveTowardWaypoint();
// Update
CGameObject::Update();
}
/*
Removes and return the next waypoint in the multipath corresponding to given id.
*/
float3 CPathManager::NextWaypoint(unsigned int pathId, float3 callerPos, float minDistance,
int numRetries, int ownerId) const
{
SCOPED_TIMER("PFS");
//0 indicate a no-path id.
if(pathId == 0)
return float3(-1,-1,-1);
if(numRetries>4)
return float3(-1,-1,-1);
//Find corresponding multipath.
std::map<unsigned int, MultiPath*>::const_iterator pi = pathMap.find(pathId);
if(pi == pathMap.end())
return float3(-1,-1,-1);
MultiPath* multiPath = pi->second;
if(callerPos==ZeroVector){
if(!multiPath->detailedPath.path.empty())
callerPos=multiPath->detailedPath.path.back();
}
//check if detailed path need bettering
if(!multiPath->estimatedPath.path.empty()
&& (multiPath->estimatedPath.path.back().SqDistance2D(callerPos) < Square(MIN_DETAILED_DISTANCE * SQUARE_SIZE)
|| multiPath->detailedPath.path.size() <= 2)){
if(!multiPath->estimatedPath2.path.empty() //if so check if estimated path also need bettering
&& (multiPath->estimatedPath2.path.back().SqDistance2D(callerPos) < Square(MIN_ESTIMATE_DISTANCE * SQUARE_SIZE)
|| multiPath->estimatedPath.path.size() <= 2)){
Estimate2ToEstimate(*multiPath, callerPos, ownerId);
}
if(multiPath->caller)
multiPath->caller->UnBlock();
EstimateToDetailed(*multiPath, callerPos, ownerId);
if(multiPath->caller)
multiPath->caller->Block();
}
//Repeat until a waypoint distant enought are found.
float3 waypoint;
do {
//Get next waypoint.
if(multiPath->detailedPath.path.empty()) {
if(multiPath->estimatedPath2.path.empty() && multiPath->estimatedPath.path.empty())
return multiPath->finalGoal;
else
return NextWaypoint(pathId,callerPos,minDistance,numRetries+1,ownerId);
} else {
waypoint = multiPath->detailedPath.path.back();
multiPath->detailedPath.path.pop_back();
}
} while(callerPos.SqDistance2D(waypoint) < Square(minDistance) && waypoint != multiPath->detailedPath.pathGoal);
return waypoint;
}
// Transform from Cartesian x,y coordinates to Frenet s,d coordinates
std::pair<double,double> Map::getFrenet(double x, double y, double theta) const {
int next_wp = NextWaypoint(x,y, theta);
int prev_wp;
prev_wp = next_wp-1;
if(next_wp == 0) {
prev_wp = num_wp-1;
}
double n_x = map_waypoints_x[next_wp]-map_waypoints_x[prev_wp];
double n_y = map_waypoints_y[next_wp]-map_waypoints_y[prev_wp];
double x_x = x - map_waypoints_x[prev_wp];
double x_y = y - map_waypoints_y[prev_wp];
// find the projection of x onto n
double proj_norm = (x_x*n_x+x_y*n_y)/(n_x*n_x+n_y*n_y);
double proj_x = proj_norm*n_x;
double proj_y = proj_norm*n_y;
double frenet_d = distance(x_x,x_y,proj_x,proj_y);
//see if d value is positive or negative by comparing it to a center point
double center_x = 1000-map_waypoints_x[prev_wp];
double center_y = 2000-map_waypoints_y[prev_wp];
double centerToPos = distance(center_x,center_y,x_x,x_y);
double centerToRef = distance(center_x,center_y,proj_x,proj_y);
if(centerToPos <= centerToRef) {
frenet_d *= -1;
}
// calculate s value
double frenet_s = 0;
for(int i = 0; i < prev_wp; i++) {
frenet_s += distance(map_waypoints_x[i],map_waypoints_y[i],map_waypoints_x[i+1],map_waypoints_y[i+1]);
}
frenet_s += distance(0,0,proj_x,proj_y);
return {frenet_s,frenet_d};
}
/*
Removes and return the next waypoint in the multipath corresponding to given id.
*/
float3 CPathManager::NextWaypoint(unsigned int pathId, float3 callerPos, float minDistance,
int numRetries, int ownerId, bool synced) const
{
SCOPED_TIMER("PFS");
// 0 indicates a no-path id
if (pathId == 0)
return float3(-1.0f, -1.0f, -1.0f);
if (numRetries > 4)
return float3(-1.0f, -1.0f, -1.0f);
//Find corresponding multipath.
std::map<unsigned int, MultiPath*>::const_iterator pi = pathMap.find(pathId);
if (pi == pathMap.end())
return float3(-1.0f, -1.0f, -1.0f);
MultiPath* multiPath = pi->second;
if (callerPos == ZeroVector) {
if (!multiPath->detailedPath.path.empty())
callerPos = multiPath->detailedPath.path.back();
}
// check if detailed path needs bettering
if (!multiPath->estimatedPath.path.empty() &&
(multiPath->estimatedPath.path.back().SqDistance2D(callerPos) < Square(MIN_DETAILED_DISTANCE * SQUARE_SIZE) ||
multiPath->detailedPath.path.size() <= 2)) {
if (!multiPath->estimatedPath2.path.empty() && // if so, check if estimated path also needs bettering
(multiPath->estimatedPath2.path.back().SqDistance2D(callerPos) < Square(MIN_ESTIMATE_DISTANCE * SQUARE_SIZE) ||
multiPath->estimatedPath.path.size() <= 2)) {
Estimate2ToEstimate(*multiPath, callerPos, ownerId, synced);
}
if (multiPath->caller) {
multiPath->caller->UnBlock();
}
EstimateToDetailed(*multiPath, callerPos, ownerId);
if (multiPath->caller) {
multiPath->caller->Block();
}
}
float3 waypoint;
do {
// get the next waypoint from the high-res path
//
// if this is not possible, then either we are
// at the goal OR the path could not reach all
// the way to it (ie. a GoalOutOfRange result)
if (multiPath->detailedPath.path.empty()) {
if (multiPath->estimatedPath2.path.empty() && multiPath->estimatedPath.path.empty()) {
if (multiPath->searchResult == IPath::Ok) {
return multiPath->finalGoal;
} else {
return float3(-1.0f, -1.0f, -1.0f);
}
} else {
return NextWaypoint(pathId, callerPos, minDistance, numRetries + 1, ownerId, synced);
}
} else {
waypoint = multiPath->detailedPath.path.back();
multiPath->detailedPath.path.pop_back();
}
} while (callerPos.SqDistance2D(waypoint) < Square(minDistance) && waypoint != multiPath->detailedPath.pathGoal);
return waypoint;
}
/*
Removes and return the next waypoint in the multipath corresponding to given id.
*/
float3 CPathManager::NextWaypoint(unsigned int pathId, float3 callerPos, float minDistance, int numRetries) {
#ifdef PROFILE_TIME
LARGE_INTEGER starttime;
QueryPerformanceCounter(&starttime);
#endif
//0 indicate a no-path id.
if(pathId == 0)
return float3(-1,-1,-1);
if(numRetries>4)
return float3(-1,-1,-1);
//Find corresponding multipath.
map<unsigned int, MultiPath*>::iterator pi = pathMap.find(pathId);
if(pi == pathMap.end())
return float3(-1,-1,-1);
MultiPath* multiPath = pi->second;
if(callerPos==ZeroVector){
if(!multiPath->detailedPath.path.empty())
callerPos=multiPath->detailedPath.path.back();
}
//check if detailed path need bettering
if(!multiPath->estimatedPath.path.empty()
&& (multiPath->estimatedPath.path.back().distance2D(callerPos) < MIN_DETAILED_DISTANCE * SQUARE_SIZE
|| multiPath->detailedPath.path.size() <= 2)){
if(!multiPath->estimatedPath2.path.empty() //if so check if estimated path also need bettering
&& (multiPath->estimatedPath2.path.back().distance2D(callerPos) < MIN_ESTIMATE_DISTANCE * SQUARE_SIZE
|| multiPath->estimatedPath.path.size() <= 2)){
Estimate2ToEstimate(*multiPath, callerPos);
}
if(multiPath->caller)
multiPath->caller->UnBlock();
EstimateToDetailed(*multiPath, callerPos);
if(multiPath->caller)
multiPath->caller->Block();
}
//Repeat until a waypoint distant enought are found.
float3 waypoint;
do {
//Get next waypoint.
if(multiPath->detailedPath.path.empty()) {
if(multiPath->estimatedPath2.path.empty() && multiPath->estimatedPath.path.empty())
return multiPath->finalGoal;
else
return NextWaypoint(pathId,callerPos,minDistance,numRetries+1);
} else {
waypoint = multiPath->detailedPath.path.back();
multiPath->detailedPath.path.pop_back();
}
} while(callerPos.distance2D(waypoint) < minDistance && waypoint != multiPath->detailedPath.pathGoal);
//Return the result.
#ifdef PROFILE_TIME
LARGE_INTEGER stop;
QueryPerformanceCounter(&stop);
profiler.AddTime("AI:PFS",stop.QuadPart - starttime.QuadPart);
#endif
return waypoint;
}
