ilikecats/stk-code_catmod
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Try to make determineTurnRadius works with any plane

Browse Source
This commit is contained in:
Benau
2016-09-26 00:11:49 +08:00
parent bff7154e03
commit 3659ad9068
2 changed files with 44 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
src/karts/controller/skidding_ai.cpp
View File

@@ -173,6 +173,7 @@ void SkiddingAI::reset()
m_curve_center = Vec3(0,0,0);
m_current_track_direction = DriveNode::DIR_STRAIGHT;
m_item_to_collect = NULL;
m_last_direction_node = 0;
m_avoid_item_close = false;
m_skid_probability_state = SKID_PROBAB_NOT_YET;
m_last_item_random = NULL;
@@ -2112,20 +2113,17 @@ void SkiddingAI::findNonCrashingPointFixed(Vec3 *aim_position, int *last_node)
*/
void SkiddingAI::determineTrackDirection()
{
const DriveGraph *qg = DriveGraph::get();
unsigned int succ = m_successor_index[m_track_node];
unsigned int next = qg->getNode(m_track_node)->getSuccessor(succ);
//float angle_to_track = qg->getNode(m_track_node)->getAngleToSuccessor(succ)
// - m_kart->getHeading();
Vec3 track_direction = -qg->getNode(m_track_node)->getCenter()
+ qg->getNode(next)->getCenter();
//Vec3 kart_direction = qg->getNode(m_track_node)->getCenter() + m_kart->getVelocity();
float angle_to_track = 0;
const DriveGraph *dg = DriveGraph::get();
unsigned int succ = m_successor_index[m_track_node];
unsigned int next = dg->getNode(m_track_node)->getSuccessor(succ);
float angle_to_track = 0.0f;
if (m_kart->getVelocity().length() > 0.0f)
angle_to_track = track_direction.angle(m_kart->getVelocity().normalized());
{
Vec3 track_direction = -dg->getNode(m_track_node)->getCenter()
+ dg->getNode(next)->getCenter();
angle_to_track =
track_direction.angle(m_kart->getVelocity().normalized());
}
angle_to_track = normalizeAngle(angle_to_track);
// In certain circumstances (esp. S curves) it is possible that the
@@ -2145,7 +2143,7 @@ void SkiddingAI::determineTrackDirection()
return;
}
qg->getNode(next)->getDirectionData(m_successor_index[next],
dg->getNode(next)->getDirectionData(m_successor_index[next],
&m_current_track_direction,
&m_last_direction_node);
@@ -2153,7 +2151,7 @@ void SkiddingAI::determineTrackDirection()
m_curve[CURVE_QG]->clear();
for(unsigned int i=m_track_node; i<=m_last_direction_node; i++)
{
m_curve[CURVE_QG]->addPoint(qg->getNode(i)->getCenter());
m_curve[CURVE_QG]->addPoint(dg->getNode(i)->getCenter());
}
#endif
@@ -2183,13 +2181,10 @@ void SkiddingAI::handleCurve()
// kart will already point towards the direction of the circle), and
// the case that the kart is facing wrong was already tested for before
const DriveGraph *qg = DriveGraph::get();
Vec3 xyz = m_kart->getXYZ();
Vec3 tangent = m_kart->getTrans()(Vec3(0,0,1)) - xyz;
Vec3 last_xyz = qg->getNode(m_last_direction_node)->getCenter();
const DriveGraph *dg = DriveGraph::get();
const Vec3& last_xyz = dg->getNode(m_last_direction_node)->getCenter();
determineTurnRadius(xyz, tangent, last_xyz,
&m_curve_center, &m_current_curve_radius);
determineTurnRadius(last_xyz, &m_curve_center, &m_current_curve_radius);
assert(!std::isnan(m_curve_center.getX()));
assert(!std::isnan(m_curve_center.getY()));
assert(!std::isnan(m_curve_center.getZ()));
@@ -2205,11 +2200,12 @@ void SkiddingAI::handleCurve()
// Pick either the lower left or right point:
int index = m_current_track_direction==DriveNode::DIR_LEFT
? 0 : 1;
float r = (m_curve_center - *(qg->getNode(i))[index]).length();
Vec3 curve_center_wc = m_kart->getTrans()(m_curve_center);
float r = (curve_center_wc - *(dg->getNode(i))[index]).length();
if(m_current_curve_radius < r)
{
last_xyz = *(qg->getNode(i)[index]);
determineTurnRadius(xyz, tangent, last_xyz,
last_xyz = *(dg->getNode(i)[index]);
determineTurnRadius(last_xyz,
&m_curve_center, &m_current_curve_radius);
m_last_direction_node = i;
break;
@@ -2219,10 +2215,10 @@ void SkiddingAI::handleCurve()
}
#endif
#if defined(AI_DEBUG) && defined(AI_DEBUG_CIRCLES)
m_curve[CURVE_PREDICT1]->makeCircle(m_curve_center,
m_curve[CURVE_PREDICT1]->makeCircle(m_kart->getTrans()(m_curve_center),
m_current_curve_radius);
m_curve[CURVE_PREDICT1]->addPoint(last_xyz);
m_curve[CURVE_PREDICT1]->addPoint(m_curve_center);
m_curve[CURVE_PREDICT1]->addPoint(m_kart->getTrans()(m_curve_center));
m_curve[CURVE_PREDICT1]->addPoint(xyz);
#endif
@@ -2277,15 +2273,16 @@ bool SkiddingAI::canSkid(float steer_fraction)
}
const float MIN_SKID_SPEED = 5.0f;
const DriveGraph *qg = DriveGraph::get();
Vec3 last_xyz = qg->getNode(m_last_direction_node)->getCenter();
const DriveGraph *dg = DriveGraph::get();
Vec3 last_xyz = m_kart->getTrans().inverse()
(dg->getNode(m_last_direction_node)->getCenter());
// Only try skidding when a certain minimum speed is reached.
if(m_kart->getSpeed()<MIN_SKID_SPEED) return false;
// Estimate how long it takes to finish the curve
Vec3 diff_kart = m_kart->getXYZ() - m_curve_center;
Vec3 diff_last = last_xyz - m_curve_center;
Vec3 diff_kart = -m_curve_center;
Vec3 diff_last = last_xyz - m_curve_center;
float angle_kart = atan2(diff_kart.getX(), diff_kart.getZ());
float angle_last = atan2(diff_last.getX(), diff_last.getZ());
float angle = m_current_track_direction == DriveNode::DIR_RIGHT
@@ -2472,52 +2469,46 @@ void SkiddingAI::setSteering(float angle, float dt)
// ----------------------------------------------------------------------------
/** Determine the center point and radius of a circle given two points on
* the ccircle and the tangent at the first point. This is done as follows:
* the circle and the tangent at the first point. This is done as follows:
* 1. Determine the line going through the center point start+end, which is
* orthogonal to the vector from start to end. This line goes through the
* center of the circle.
* 2. Determine the line going through the first point and is orthogonal
* to the given tangent.
* 3. The intersection of these two lines is the center of the circle.
* \param[in] start First point.
* \param[in] tangent Tangent at first point.
* \param[in] end Second point on circle.
* \param[out] center Center point of the circle.
* \param[out] center Center point of the circle (local coordinate).
* \param[out] radius Radius of the circle.
*/
void SkiddingAI::determineTurnRadius(const Vec3 &start,
const Vec3 &tangent,
const Vec3 &end,
Vec3 *center,
float *radius)
void SkiddingAI::determineTurnRadius(const Vec3 &end, Vec3 *center,
float *radius)
{
// Convert end point to local coordinate, so start will be 0, 0, 0
Vec3 lc = m_kart->getTrans().inverse()(end);
// 1) Line through middle of start+end
Vec3 mid = 0.5f*(start+end);
Vec3 direction = end-start;
Vec3 mid = 0.5f * lc;
Vec3 direction = lc;
Vec3 orthogonal(direction.getZ(), 0, -direction.getX());
Vec3 q1 = mid + orthogonal;
Vec3 q1 = mid + orthogonal;
irr::core::line2df line1(mid.getX(), mid.getZ(),
q1.getX(), q1.getZ() );
Vec3 ortho_tangent(tangent.getZ(), 0, -tangent.getX());
Vec3 q2 = start + ortho_tangent;
irr::core::line2df line2(start.getX(), start.getZ(),
q2.getX(), q2.getZ());
irr::core::line2df line2(0, 0, 1, 0);
irr::core::vector2df result;
if(line1.intersectWith(line2, result, /*checkOnlySegments*/false))
{
*center = Vec3(result.X, start.getY(), result.Y);
*radius = (start - *center).length();
Vec3 lc_center(result.X, 0, result.Y);
*center = lc_center;
*radius = lc_center.length();
}
else
{
// No intersection. In this case assume that the two points are
// on a semicircle, in which case the center is at 0.5*(start+end):
*center = 0.5f*(start+end);
*radius = 0.5f*(end-start).length();
*center = mid;
*radius = 0.5f*(lc).length();
}
return;
} // determineTurnRadius

6
src/karts/controller/skidding_ai.hpp
View File

@@ -265,11 +265,7 @@ private:
void findNonCrashingPoint(Vec3 *result, int *last_node);
void determineTrackDirection();
void determineTurnRadius(const Vec3 &start,
const Vec3 &start_direction,
const Vec3 &end,
Vec3 *center,
float *radius);
void determineTurnRadius(const Vec3 &end, Vec3 *center, float *radius);
virtual bool canSkid(float steer_fraction);
virtual void setSteering(float angle, float dt);
void handleCurve();