1) More support for quad graphs added. Now the AI should

be able to use shortcuts and loops (e.g. using the same quads
   more than once).
2) More AI simplifications, and adjustments to handle off road
   driving with the new quad graph structure.
3) Fixed bug in LinearWorld: last_valid_sector was not defined
   anymore, resulting in crashes when a kart was rescued.


git-svn-id: svn+ssh://svn.code.sf.net/p/supertuxkart/code/main/branches/irrlicht@3543 178a84e3-b1eb-0310-8ba1-8eac791a3b58
This commit is contained in:
hikerstk 2009-05-28 00:43:12 +00:00
parent 62f5c82882
commit 59d5098ade
9 changed files with 208 additions and 357 deletions

View File

@ -56,9 +56,8 @@ void LinearWorld::init()
if (!info.m_on_road)
{
info.m_track_sector =
m_track->findOutOfRoadSector(m_kart[n]->getXYZ(),
Track::RS_DONT_KNOW,
QuadGraph::UNKNOWN_SECTOR );
m_track->getQuadGraph().findOutOfRoadSector(m_kart[n]->getXYZ(),
QuadGraph::UNKNOWN_SECTOR );
}
m_track->getQuadGraph().spatialToTrack(&info.m_curr_track_coords,
@ -107,9 +106,8 @@ void LinearWorld::restartRace()
if (!info.m_on_road)
{
info.m_track_sector =
m_track->findOutOfRoadSector(m_kart[n]->getXYZ(),
Track::RS_DONT_KNOW,
QuadGraph::UNKNOWN_SECTOR );
m_track->getQuadGraph().findOutOfRoadSector(m_kart[n]->getXYZ(),
QuadGraph::UNKNOWN_SECTOR );
}
m_track->getQuadGraph().spatialToTrack(&info.m_curr_track_coords,
@ -153,18 +151,16 @@ void LinearWorld::update(float delta)
m_track->getQuadGraph().findRoadSector(kart->getXYZ(),
&kart_info.m_track_sector);
// Check if the kart is taking a shortcut (if it's not already doing one):
// -----------------------------------------------------------------------
kart_info.m_on_road = kart_info.m_track_sector != QuadGraph::UNKNOWN_SECTOR;
if(!kart_info.m_on_road)
if(kart_info.m_on_road)
{
kart_info.m_last_valid_sector = kart_info.m_track_sector;
}
else
{
// Kart off road. Find the closest sector instead.
kart_info.m_track_sector =
m_track->findOutOfRoadSector(kart->getXYZ(),
kart_info.m_curr_track_coords[0] > 0.0
? Track::RS_RIGHT
: Track::RS_LEFT,
prev_sector );
m_track->getQuadGraph().findOutOfRoadSector(kart->getXYZ(), prev_sector );
}
// Update track coords (=progression)

View File

@ -51,25 +51,40 @@ DefaultRobot::DefaultRobot(const std::string& kart_name,
const Track *track ) :
AutoKart( kart_name, position, init_pos )
{
reset();
m_kart_length = m_kart_properties->getKartModel()->getLength();
m_kart_width = m_kart_properties->getKartModel()->getWidth();
m_track = RaceManager::getTrack();
m_quad_graph = &m_track->getQuadGraph();
m_next_quad_index.reserve(m_quad_graph->getNumNodes());
m_next_node_index.reserve(m_quad_graph->getNumNodes());
m_successor_index.reserve(m_quad_graph->getNumNodes());
// For now pick one part on random, which is not adjusted during the run
std::vector<unsigned int> next;
// Initialise the fields with -1
for(unsigned int i=0; i<m_quad_graph->getNumNodes(); i++)
{
next.clear();
m_quad_graph->getSuccessors(i, next);
int indx = rand() % next.size();
indx = next.size()-1;
m_successor_index.push_back(indx);
m_next_quad_index.push_back(next[indx]);
m_next_node_index.push_back(-1);
m_successor_index.push_back(-1);
}
// For now pick one part on random, which is not adjusted during the run
std::vector<unsigned int> next;
int count=0;
int current_node=0;
while (1)
{
next.clear();
m_quad_graph->getSuccessors(current_node, next);
int indx = rand() % next.size();
m_successor_index[current_node] = indx;
m_next_node_index[current_node] = next[indx];
current_node = next[indx];
if(current_node==0) break;
count++;
if(count>(int)m_quad_graph->getNumNodes())
{
fprintf(stderr, "AI can't find a loop going back to node 0, aborting.\n");
exit(-1);
}
};
const int look_ahead=10;
// Now compute for each node in the graph the list of the next 'look_ahead'
// graph nodes. This is the list of node that is tested in checkCrashes.
@ -85,12 +100,16 @@ DefaultRobot::DefaultRobot(const std::string& kart_name,
int current = i;
for(unsigned int j=0; j<look_ahead; j++)
{
l.push_back(m_next_quad_index[current]);
current = m_next_quad_index[current];
int next = m_next_node_index[current];
if(next==-1) break;
l.push_back(m_next_node_index[current]);
current = m_next_node_index[current];
} // for j<look_ahead
m_all_look_aheads.push_back(l);
}
// Reset must be called after m_quad_graph etc. is set up
reset();
m_world = dynamic_cast<LinearWorld*>(RaceManager::getWorld());
assert(m_world != NULL);
@ -152,9 +171,23 @@ void DefaultRobot::update(float dt)
// This is used to enable firing an item backwards.
m_controls.m_look_back = false;
m_controls.m_nitro = false;
m_track_sector = m_world->m_kart_info[ getWorldKartId() ].m_track_sector;
if(m_successor_index[m_track_sector]!=0)
printf("XX");
// Update the current node:
if(m_track_node!=QuadGraph::UNKNOWN_SECTOR)
{
int old_node = m_track_node;
m_quad_graph->findRoadSector(getXYZ(), &m_track_node,
&m_all_look_aheads[m_track_node]);
// IF the AI is off track (or on a branch of the track it did not
// select to be on), keep the old position.
if(m_track_node==QuadGraph::UNKNOWN_SECTOR ||
m_next_node_index[m_track_node]==-1)
m_track_node = old_node;
}
if(m_track_node==QuadGraph::UNKNOWN_SECTOR)
{
m_track_node = m_quad_graph->findOutOfRoadSector(getXYZ());
}
// The client does not do any AI computations.
if(network_manager->getMode()==NetworkManager::NW_CLIENT)
{
@ -173,21 +206,7 @@ void DefaultRobot::update(float dt)
//Detect if we are going to crash with the track and/or kart
int steps = 0;
// This should not happen (anymore :) ), but it keeps the game running
// in case that m_future_sector becomes undefined.
if(m_future_sector == QuadGraph::UNKNOWN_SECTOR)
{
#ifdef DEBUG
fprintf(stderr,"DefaultRobot: m_future_sector is undefined.\n");
fprintf(stderr,"This shouldn't happen, but can be ignored.\n");
#endif
forceRescue();
m_future_sector = 0;
}
else
{
steps = calcSteps();
}
steps = calcSteps();
computeNearestKarts();
checkCrashes( steps, getXYZ() );
@ -268,7 +287,7 @@ void DefaultRobot::handleBraking()
return;
}
const float MIN_SPEED = m_track->getQuadGraph().getNode(m_track_sector).getPathWidth();
const float MIN_SPEED = 5.0f;
KartInfo &kart_info = m_world->m_kart_info[ getWorldKartId() ];
//We may brake if we are about to get out of the road, but only if the
//kart is on top of the road, and if we won't slow down below a certain
@ -276,13 +295,13 @@ void DefaultRobot::handleBraking()
if ( m_crashes.m_road && kart_info.m_on_road && getVelocityLC().getY() > MIN_SPEED)
{
float kart_ang_diff =
m_quad_graph->getAngleToNext(m_track_sector,
m_successor_index[m_track_sector])
m_quad_graph->getAngleToNext(m_track_node,
m_successor_index[m_track_node])
- getHPR().getHeading();
kart_ang_diff = normalizeAngle(kart_ang_diff);
kart_ang_diff = fabsf(kart_ang_diff);
const float MIN_TRACK_ANGLE = 20.0f;
const float MIN_TRACK_ANGLE = DEGREE_TO_RAD(20.0f);
const float CURVE_INSIDE_PERC = 0.25f;
//Brake only if the road does not goes somewhat straight.
@ -293,7 +312,7 @@ void DefaultRobot::handleBraking()
//even if we are in the inside, because the kart would be 'thrown'
//out of the curve.
if(!(m_world->getDistanceToCenterForKart(getWorldKartId())
> m_track->getQuadGraph().getNode(m_track_sector).getPathWidth() *
> m_quad_graph->getNode(m_track_node).getPathWidth() *
-CURVE_INSIDE_PERC || m_curve_angle > RAD_TO_DEGREE(getMaxSteerAngle())))
{
m_controls.m_brake = false;
@ -303,7 +322,7 @@ void DefaultRobot::handleBraking()
else if( m_curve_angle < -MIN_TRACK_ANGLE ) //Next curve is right
{
if(!(m_world->getDistanceToCenterForKart( getWorldKartId() )
< m_track->getQuadGraph().getNode(m_track_sector).getPathWidth() *
< m_quad_graph->getNode(m_track_node).getPathWidth() *
CURVE_INSIDE_PERC || m_curve_angle < -RAD_TO_DEGREE(getMaxSteerAngle())))
{
m_controls.m_brake = false;
@ -332,7 +351,7 @@ void DefaultRobot::handleBraking()
//-----------------------------------------------------------------------------
void DefaultRobot::handleSteering(float dt)
{
const int next = m_next_quad_index[m_track_sector];
const int next = m_next_node_index[m_track_node];
float steer_angle = 0.0f;
@ -341,7 +360,7 @@ void DefaultRobot::handleSteering(float dt)
*/
//Reaction to being outside of the road
if( fabsf(m_world->getDistanceToCenterForKart( getWorldKartId() )) + 0.5f >
m_track->getQuadGraph().getNode(m_track_sector).getPathWidth() )
m_quad_graph->getNode(m_track_node).getPathWidth() )
{
steer_angle = steerToPoint(m_quad_graph->getQuad(next).getCenter(),
dt );
@ -408,7 +427,7 @@ void DefaultRobot::handleSteering(float dt)
case FT_AVOID_TRACK_CRASH:
if( m_crashes.m_road )
{
steer_angle = steerToAngle(m_track_sector, 0.0f );
steer_angle = steerToAngle(m_track_node, 0.0f );
}
else steer_angle = 0.0f;
@ -817,34 +836,25 @@ void DefaultRobot::checkCrashes( const int STEPS, const Vec3& pos )
//each other, but the first step is twice as big as other steps to avoid
//having karts too close in any direction. The crash with the track can
//tell when a kart is going to get out of the track so it steers.
Vec3 vel_normal;
//in this func we use it as a 2d-vector, but later on it is passed
//to m_track->findRoadSector, there it is used as a 3d-vector
//to find distance to plane, so z must be initialized to zero
Vec3 step_coord;
float kart_distance;
step_coord.setZ(0.0);
m_crashes.clear();
const size_t NUM_KARTS = race_manager->getNumKarts();
//Protection against having vel_normal with nan values
const Vec3 &VEL = getVelocity();
vel_normal.setValue(VEL.getX(), VEL.getY(), 0.0);
float len=vel_normal.length();
Vec3 vel_normal(VEL.getX(), VEL.getY(), 0.0);
float speed=vel_normal.length();
// If the velocity is zero, no sense in checking for crashes in time
if(len==0) return;
if(speed==0) return;
// Time it takes to drive for m_kart_length units.
float dt = m_kart_length / len;
vel_normal/=len;
float dt = m_kart_length / speed;
vel_normal/=speed;
int current_node = m_track_node;
for(int i = 1; STEPS > i; ++i)
{
step_coord = pos + vel_normal* m_kart_length * float(i);
Vec3 step_coord = pos + vel_normal* m_kart_length * float(i);
/* Find if we crash with any kart, as long as we haven't found one
* yet
@ -856,74 +866,28 @@ void DefaultRobot::checkCrashes( const int STEPS, const Vec3& pos )
const Kart* kart = RaceManager::getKart(j);
if(kart==this||kart->isEliminated()) continue; // ignore eliminated karts
const Kart *other_kart = RaceManager::getKart(j);
// Ignore karts ahead that are faster than this kart.
if(getVelocityLC().getY() < other_kart->getVelocityLC().getY())
continue;
Vec3 other_kart_xyz = other_kart->getXYZ() + other_kart->getVelocity()*(i*dt);
kart_distance = (step_coord - other_kart_xyz).length_2d();
float kart_distance = (step_coord - other_kart_xyz).length_2d();
if( kart_distance < m_kart_length &&
getVelocityLC().getY() > other_kart->getVelocityLC().getY())
if( kart_distance < m_kart_length)
m_crashes.m_kart = j;
}
}
/*Find if we crash with the drivelines*/
if(m_sector!=QuadGraph::UNKNOWN_SECTOR)
m_track->getQuadGraph().findRoadSector(step_coord, &m_sector,
/* sectors to test*/ &m_all_look_aheads[m_sector]);
else
m_track->getQuadGraph().findRoadSector(step_coord, &m_sector);
if(current_node!=QuadGraph::UNKNOWN_SECTOR &&
m_next_node_index[current_node]!=-1)
m_quad_graph->findRoadSector(step_coord, &current_node,
/* sectors to test*/ &m_all_look_aheads[current_node]);
#undef SHOW_FUTURE_PATH
#ifdef SHOW_FUTURE_PATH
ssgaSphere *sphere = new ssgaSphere;
#ifdef ERASE_PATH
static ssgaSphere *last_sphere = 0;
if( last_sphere ) scene->remove( last_sphere );
last_sphere = sphere;
#endif
sgVec3 center;
center[0] = step_coord[0];
center[1] = step_coord[1];
center[2] = pos[2];
sphere->setCenter( center );
sphere->setSize( m_kart_properties->getKartModel()->getLength() );
if( m_sector == QuadGraph::UNKNOWN_SECTOR ))
if( current_node == QuadGraph::UNKNOWN_SECTOR)
{
sgVec4 colour;
colour[0] = colour[3] = 255;
colour[1] = colour[2] = 0;
sphere->setColour(colour);
}
else if( i == 1 )
{
sgVec4 colour;
colour[0] = colour[1] = colour[2] = 0;
colour[3] = 255;
sphere->setColour( colour );
}
scene->add( sphere );
#endif
m_future_location = Vec3( step_coord[0], step_coord[1], 0 );
if( m_sector == QuadGraph::UNKNOWN_SECTOR)
{
m_future_sector = m_track->findOutOfRoadSector( step_coord,
Track::RS_DONT_KNOW, m_future_sector );
m_crashes.m_road = true;
break;
return;
}
else
{
m_future_sector = m_sector;
}
}
} // checkCrashes
@ -935,7 +899,7 @@ void DefaultRobot::checkCrashes( const int STEPS, const Vec3& pos )
*/
void DefaultRobot::findNonCrashingPoint(Vec3 *result)
{
unsigned int sector = m_next_quad_index[m_track_sector];
unsigned int sector = m_next_node_index[m_track_node];
int target_sector;
Vec3 direction;
@ -948,7 +912,7 @@ void DefaultRobot::findNonCrashingPoint(Vec3 *result)
{
//target_sector is the sector at the longest distance that we can drive
//to without crashing with the track.
target_sector = m_next_quad_index[sector];
target_sector = m_next_node_index[sector];
//direction is a vector from our kart to the sectors we are testing
direction = m_quad_graph->getQuad(target_sector).getCenter() - getXYZ();
@ -968,42 +932,16 @@ void DefaultRobot::findNonCrashingPoint(Vec3 *result)
{
step_coord = getXYZ()+direction*m_kart_length * float(i);
m_track->getQuadGraph().spatialToTrack(&step_track_coord, step_coord,
m_quad_graph->spatialToTrack(&step_track_coord, step_coord,
sector );
distance = step_track_coord[0] > 0.0f ? step_track_coord[0]
: -step_track_coord[0];
distance = fabsf(step_track_coord[0]);
//If we are outside, the previous sector is what we are looking for
if ( distance + m_kart_width * 0.5f
> m_track->getQuadGraph().getNode(sector).getPathWidth() )
if ( distance + m_kart_width * 0.5f
> m_quad_graph->getNode(sector).getPathWidth() )
{
*result = m_quad_graph->getQuad(sector).getCenter();
#ifdef SHOW_NON_CRASHING_POINT
ssgaSphere *sphere = new ssgaSphere;
static ssgaSphere *last_sphere = 0;
if(last_sphere) scene->remove( last_sphere );
last_sphere = sphere;
sgVec3 center;
center[0] = result[0];
center[1] = result[1];
center[2] = getXYZ().getZ();
sphere->setCenter( center );
sphere->setSize( 0.5f );
sgVec4 colour;
colour[1] = colour[3] = 255;
colour[0] = colour[2] = 0;
sphere->setColour( colour );
scene->add( sphere );
#endif
return;
}
}
@ -1016,12 +954,8 @@ void DefaultRobot::reset()
{
m_time_since_last_shot = 0.0f;
m_start_kart_crash_direction = 0;
m_sector = QuadGraph::UNKNOWN_SECTOR;
m_inner_curve = 0;
m_curve_target_speed = getMaxSpeedOnTerrain();
m_curve_angle = 0.0;
m_future_location = Vec3( 0.0, 0.0, 0.0 );
m_future_sector = 0;
m_time_till_start = -1.0f;
m_crash_time = 0.0f;
m_collided = false;
@ -1030,6 +964,14 @@ void DefaultRobot::reset()
m_distance_ahead = 0.0f;
m_kart_behind = NULL;
m_distance_behind = 0.0f;
m_track_node = QuadGraph::UNKNOWN_SECTOR;
m_quad_graph->findRoadSector(getXYZ(), &m_track_node);
if(m_track_node==QuadGraph::UNKNOWN_SECTOR)
{
fprintf(stderr, "Invalid starting position for '%s' - not on track - can be ignored.\n",
getIdent().c_str());
m_track_node = m_quad_graph->findOutOfRoadSector(getXYZ());
}
AutoKart::reset();
} // reset
@ -1058,15 +1000,20 @@ int DefaultRobot::calcSteps()
//Increase the steps depending on the width, if we steering hard,
//mostly for curves.
#if 0
// FIXME: I don't understand this: if we are steering hard, we check
// for more steps if we hit another kart?? If we steer hard,
// the approximation used (pos + velocity*dt) will be even
// worse, since it doesn't take steering into account.
if( fabsf(m_controls.m_steer) > 0.95 )
{
const int WIDTH_STEPS =
(int)( m_track->getQuadGraph().getNode(m_future_sector).getPathWidth()
(int)( m_quad_graph->getNode(m_future_sector).getPathWidth()
/( m_kart_length * 2.0 ) );
steps += WIDTH_STEPS;
}
#endif
return steps;
} // calcSteps
@ -1122,8 +1069,8 @@ void DefaultRobot::findCurve()
{
float total_dist = 0.0f;
int i;
for(i = m_track_sector; total_dist < getVelocityLC().getY();
i = m_next_quad_index[i])
for(i = m_track_node; total_dist < getVelocityLC().getY();
i = m_next_node_index[i])
{
total_dist += m_quad_graph->getDistanceToNext(i, m_successor_index[i]);
}
@ -1131,9 +1078,8 @@ void DefaultRobot::findCurve()
m_curve_angle =
normalizeAngle(m_quad_graph->getAngleToNext(i, m_successor_index[i])
-m_quad_graph->getAngleToNext(m_track_sector,
m_successor_index[m_track_sector]) );
m_inner_curve = m_curve_angle > 0.0 ? -1 : 1;
-m_quad_graph->getAngleToNext(m_track_node,
m_successor_index[m_track_node]) );
m_curve_target_speed = getMaxSpeedOnTerrain();
} // findCurve

View File

@ -104,15 +104,12 @@ private:
/** Time an item has been collected and not used. */
float m_time_since_last_shot;
int m_future_sector;
Vec3 m_future_location;
float m_time_till_start; //Used to simulate a delay at the start of the
//race, since human players don't accelerate
//at the same time and rarely repeat the a
//previous timing.
int m_inner_curve;//-1 left, 1 = right, 0 = center
float m_curve_target_speed;
float m_curve_angle;
@ -121,21 +118,26 @@ private:
/** Keep a pointer to world. */
LinearWorld *m_world;
/** Cache kart_info.m_track_sector. */
int m_track_sector;
/** The current node the kart is on. This can be different from the value
* in LinearWorld, since it takes the chosen path of the AI into account
* (e.g. the closest point in LinearWorld might be on a branch not
* chosen by the AI). */
int m_track_node;
/** The graph of qudas of this track. */
const QuadGraph *m_quad_graph;
/** Which of the successors of a node was selected by the AI. */
std::vector<int> m_successor_index;
/** For each node in the graph this list contained the chosen next node.
/** For each node in the graph this list contains the chosen next node.
* For normal lap track without branches we always have
* m_next_quad_index[i] = (i+1) % size;
* but if a branch is possible, the AI will select one option here. */
std::vector<int> m_next_quad_index;
* m_next_node_index[i] = (i+1) % size;
* but if a branch is possible, the AI will select one option here.
* If the node is not used, m_next_node_index will be -1. */
std::vector<int> m_next_node_index;
/** For each graph node this list contains a list of the next X
* graph nodes. */
std::vector<std::vector<int> > m_all_look_aheads;
float m_time_since_stuck;
int m_start_kart_crash_direction; //-1 = left, 1 = right, 0 = no crash.
@ -152,8 +154,6 @@ private:
*/
float m_skidding_threshold;
int m_sector;
/*Functions called directly from update(). They all represent an action
*that can be done, and end up setting their respective m_controls
*variable, except handle_race_start() that isn't associated with any

View File

@ -50,8 +50,8 @@ GraphNode::GraphNode(unsigned int index)
// The width is the average width at the beginning and at the end.
m_width = ( (quad[1]-quad[0]).length()
+ (quad[3]-quad[2]).length() ) * 0.5f;
Vec3 lower = (quad[m_index][0]+quad[m_index][1]) * 0.5f;
Vec3 upper = (quad[m_index][2]+quad[m_index][3]) * 0.5f;
Vec3 lower = (quad[0]+quad[1]) * 0.5f;
Vec3 upper = (quad[2]+quad[3]) * 0.5f;
m_line = core::line2df(lower.getX(), lower.getY(),
upper.getX(), upper.getY() );
// Only this 2d point is needed later
@ -66,8 +66,6 @@ GraphNode::GraphNode(unsigned int index)
*/
void GraphNode::addSuccessor(unsigned int to)
{
if(m_index==4 || m_index==5)
printf("XX");
m_vertices.push_back(to);
// m_index is the quad index, so we use m_all_quads
const Quad &this_quad = m_all_quads->getQuad(m_index);
@ -81,8 +79,8 @@ void GraphNode::addSuccessor(unsigned int to)
m_angle_to_next.push_back(theta);
// The length of this quad is the average of the left and right side
float distance_to_next = ( (this_quad[2]-this_quad[1]).length()
+ (this_quad[3]-this_quad[0]).length() ) *0.5f;
float distance_to_next = ( this_quad[2].distance(this_quad[1])
+ this_quad[3].distance(this_quad[0]) ) *0.5f;
// The distance from start for the successor node
m_all_nodes->getNode(to).m_distance_from_start =
std::max(m_all_nodes->getNode(to).m_distance_from_start,
@ -101,6 +99,9 @@ void GraphNode::getDistances(const Vec3 &xyz, Vec3 *result)
{
core::vector2df xyz2d(xyz.getX(), xyz.getY());
core::vector2df closest = m_line.getClosestPoint(xyz2d);
result->setX( (closest-xyz2d).getLength());
if(m_line.getPointOrientation(xyz2d)>0)
result->setX( (closest-xyz2d).getLength()); // to the right
else
result->setX(-(closest-xyz2d).getLength()); // to the left
result->setY( (closest-m_lower_center).getLength());
} // getDistanceFromLine
} // getDistances

View File

@ -279,7 +279,7 @@ void QuadGraph::findRoadSector(const Vec3& xyz, int *sector,
unsigned int max_count = (*sector!=UNKNOWN_SECTOR && all_sectors!=NULL)
? all_sectors->size()
: m_all_nodes.size();
*sector = UNKNOWN_SECTOR;
*sector = UNKNOWN_SECTOR;
for(unsigned int i=0; i<max_count; i++)
{
if(all_sectors)
@ -299,3 +299,72 @@ void QuadGraph::findRoadSector(const Vec3& xyz, int *sector,
return;
} // findRoadSector
//-----------------------------------------------------------------------------
/** findOutOfRoadSector finds the sector where XYZ is, but as it name
implies, it is more accurate for the outside of the track than the
inside, and for STK's needs the accuracy on top of the track is
unacceptable; but if this was a 2D function, the accuracy for out
of road sectors would be perfect.
To find the sector we look for the closest line segment from the
right and left drivelines, and the number of that segment will be
the sector.
The SIDE argument is used to speed up the function only; if we know
that XYZ is on the left or right side of the track, we know that
the closest driveline must be the one that matches that condition.
In reality, the side used in STK is the one from the previous frame,
but in order to move from one side to another a point would go
through the middle, that is handled by findRoadSector() which doesn't
has speed ups based on the side.
NOTE: This method of finding the sector outside of the road is *not*
perfect: if two line segments have a similar altitude (but enough to
let a kart get through) and they are very close on a 2D system,
if a kart is on the air it could be closer to the top line segment
even if it is supposed to be on the sector of the lower line segment.
Probably the best solution would be to construct a quad that reaches
until the next higher overlapping line segment, and find the closest
one to XYZ.
*/
int QuadGraph::findOutOfRoadSector(const Vec3& xyz,
const int curr_sector,
std::vector<int> *all_sectors) const
{
int count = (all_sectors!=NULL) ? all_sectors->size() : getNumNodes();
int current_sector = 0;
if(curr_sector != UNKNOWN_SECTOR && !all_sectors)
{
const int LIMIT = 10; //The limit prevents shortcuts
count = 2*LIMIT;
current_sector = curr_sector - LIMIT;
if(current_sector<0) current_sector += getNumNodes();
}
int min_sector = UNKNOWN_SECTOR;
float min_dist_2 = 999999.0f*999999.0f;
for(int j=0; j<count; j++)
{
int next_sector;
if(all_sectors)
next_sector = (*all_sectors)[j];
else
next_sector = current_sector+1 == getNumNodes() ? 0 : current_sector+1;
float dist_2 = xyz.distance2(getQuad(next_sector).getCenter()-xyz);
if(dist_2<min_dist_2)
{
min_dist_2 = dist_2;
min_sector = next_sector;
}
current_sector = next_sector;
} // for j
if(min_sector==UNKNOWN_SECTOR )
{
printf("unknown sector found.\n");
}
return min_sector;
} // findOutOfRoadSector

View File

@ -56,6 +56,10 @@ public:
const int sector) const;
void findRoadSector(const Vec3& XYZ, int *sector,
std::vector<int> *all_sectors=NULL) const;
int findOutOfRoadSector(const Vec3& xyz,
const int curr_sector=UNKNOWN_SECTOR,
std::vector<int> *all_sectors=NULL
) const;
/** Returns the number of nodes in the graph. */
unsigned int getNumNodes() const { return m_all_nodes.size(); }

View File

@ -71,8 +71,6 @@ void QuadSet::load(const std::string &filename) {
}
for(unsigned int i=0; i<xml->getNumNodes(); i++)
{
if(i==293)
printf("XX");
const XMLNode *xml_node = xml->getNode(i);
if(xml_node->getName()!="quad")
{

View File

@ -50,9 +50,6 @@
#include <plib/ssg.h>
const float Track::NOHIT = -99999.9f;
const int Track::QUAD_TRI_NONE = -1;
const int Track::QUAD_TRI_FIRST = 1;
const int Track::QUAD_TRI_SECOND = 2;
// ----------------------------------------------------------------------------
Track::Track( std::string filename_, float w, float h, bool stretch )
@ -112,153 +109,9 @@ void Track::cleanup()
} // cleanup
//-----------------------------------------------------------------------------
/** Finds on which side of the line segment a given point is.
*/
inline float Track::pointSideToLine( const Vec3& L1, const Vec3& L2,
const Vec3& P ) const
const Vec3& Track::trackToSpatial(const int sector) const
{
return ( L2.getX()-L1.getX() )*( P.getY()-L1.getY() )-( L2.getY()-L1.getY() )*( P.getX()-L1.getX() );
} // pointSideToLine
//-----------------------------------------------------------------------------
/** pointInQuad() works by checking if the given point is 'to the right'
* in clock-wise direction (which would be to look towards the inside of
* the quad) of each line segment that forms the quad. If it is to the
* left of all the segments, then the point is inside. This idea
* works for convex polygons, so we have to test it for the two
* triangles that compose the quad, in case that the quad is concave,
* not for the quad itself.
*/
int Track::pointInQuad
(
const Vec3& A,
const Vec3& B,
const Vec3& C,
const Vec3& D,
const Vec3& POINT
) const
{
if(pointSideToLine( C, A, POINT ) >= 0.0 )
{
//Test the first triangle
if( pointSideToLine( A, B, POINT ) > 0.0 &&
pointSideToLine( B, C, POINT ) >= 0.0 )
return QUAD_TRI_FIRST;
return QUAD_TRI_NONE;
}
//Test the second triangle
if( pointSideToLine( C, D, POINT ) > 0.0 &&
pointSideToLine( D, A, POINT ) > 0.0 )
return QUAD_TRI_SECOND;
return QUAD_TRI_NONE;
} // pointInQuad
//-----------------------------------------------------------------------------
/** findOutOfRoadSector finds the sector where XYZ is, but as it name
implies, it is more accurate for the outside of the track than the
inside, and for STK's needs the accuracy on top of the track is
unacceptable; but if this was a 2D function, the accuracy for out
of road sectors would be perfect.
To find the sector we look for the closest line segment from the
right and left drivelines, and the number of that segment will be
the sector.
The SIDE argument is used to speed up the function only; if we know
that XYZ is on the left or right side of the track, we know that
the closest driveline must be the one that matches that condition.
In reality, the side used in STK is the one from the previous frame,
but in order to move from one side to another a point would go
through the middle, that is handled by findRoadSector() which doesn't
has speed ups based on the side.
NOTE: This method of finding the sector outside of the road is *not*
perfect: if two line segments have a similar altitude (but enough to
let a kart get through) and they are very close on a 2D system,
if a kart is on the air it could be closer to the top line segment
even if it is supposed to be on the sector of the lower line segment.
Probably the best solution would be to construct a quad that reaches
until the next higher overlapping line segment, and find the closest
one to XYZ.
*/
int Track::findOutOfRoadSector
(
const Vec3& XYZ,
const RoadSide SIDE,
const int CURR_SECTOR
) const
{
int sector = QuadGraph::UNKNOWN_SECTOR;
float dist;
//FIXME: it can happen that dist is bigger than nearest_dist for all the
//the points we check (currently a limit of +/- 10), and if so, the
//function will return UNKNOWN_SECTOR, and if the AI get this, it will
//trigger an assertion. I increased the nearest_dist default value from
//99999 to 9999999, which is a lot more than the situation that caused
//the discovery of this problem, but the best way to solve this, is to
//find a better way of handling the shortcuts, and maybe a better way of
//calculating the distance.
float nearest_dist = 9999999;
const int DRIVELINE_SIZE = (int)m_left_driveline.size();
int begin_sector = 0;
int count = DRIVELINE_SIZE;
if(CURR_SECTOR != QuadGraph::UNKNOWN_SECTOR )
{
const int LIMIT = 10; //The limit prevents shortcuts
if( CURR_SECTOR - LIMIT < 0 )
{
begin_sector = DRIVELINE_SIZE - 1 + CURR_SECTOR - LIMIT;
}
else begin_sector = CURR_SECTOR - LIMIT;
count = 2*LIMIT;
}
sgLineSegment3 line_seg;
int next_sector;
for(int j=0; j<count; j++)
{
next_sector = begin_sector+1 == DRIVELINE_SIZE ? 0 : begin_sector+1;
if( SIDE != RS_RIGHT)
{
sgCopyVec3( line_seg.a, m_left_driveline[begin_sector].toFloat() );
sgCopyVec3( line_seg.b, m_left_driveline[next_sector].toFloat() );
dist = sgDistSquaredToLineSegmentVec3( line_seg, XYZ.toFloat() );
if ( dist < nearest_dist )
{
nearest_dist = dist;
sector = begin_sector;
}
} // SIDE != RS_RIGHT
if( SIDE != RS_LEFT )
{
sgCopyVec3( line_seg.a, m_right_driveline[begin_sector].toFloat() );
sgCopyVec3( line_seg.b, m_right_driveline[next_sector].toFloat() );
dist = sgDistSquaredToLineSegmentVec3( line_seg, XYZ.toFloat() );
if ( dist < nearest_dist )
{
nearest_dist = dist;
sector = begin_sector;
}
} // SIDE != RS_LEFT
begin_sector = next_sector;
} // for j
if(sector==QuadGraph::UNKNOWN_SECTOR || sector >=DRIVELINE_SIZE)
{
printf("unknown sector found.\n");
}
return sector;
} // findOutOfRoadSector
//-----------------------------------------------------------------------------
const Vec3& Track::trackToSpatial(const int SECTOR ) const
{
return m_driveline[SECTOR];
return m_quad_graph->getQuad(sector).getCenter();
} // trackToSpatial
//-----------------------------------------------------------------------------

View File

@ -98,14 +98,6 @@ private:
void handleAnimatedTextures(scene::ISceneNode *node, const XMLNode &xml);
public:
enum RoadSide{ RS_DONT_KNOW = -1, RS_LEFT = 0, RS_RIGHT = 1 };
//An enum is not used for the QUAD_TRI_* constants because of limitations
//of the conversion between enums and ints.
static const int QUAD_TRI_NONE;
static const int QUAD_TRI_FIRST;
static const int QUAD_TRI_SECOND;
struct SegmentTriangle
{
int segment;
@ -169,10 +161,6 @@ public:
void drawScaled2D (float x, float y, float w,
float h ) const;
int findOutOfRoadSector(const Vec3& XYZ,
const RoadSide SIDE,
const int CURR_SECTOR
) const;
const Vec3& trackToSpatial (const int SECTOR) const;
void loadTrackModel ();
void addMusic (MusicInformation* mi)
@ -239,10 +227,6 @@ private:
void readDrivelineFromFile(std::vector<Vec3>& line,
const std::string& file_ext);
void convertTrackToBullet(const scene::IMesh *mesh);
float pointSideToLine(const Vec3& L1, const Vec3& L2,
const Vec3& P ) const;
int pointInQuad(const Vec3& A, const Vec3& B,
const Vec3& C, const Vec3& D, const Vec3& POINT ) const;
void getMusicInformation(std::vector<std::string>& filenames,
std::vector<MusicInformation*>& m_music );
}