Vector SolveEdge( simplex_t &simplex, const voronoiedge_t &edge, const simplexvert_t **vertList )
{
const simplexvert_t &p0 = *vertList[edge.vertIndex[0]];
const simplexvert_t &p1 = *vertList[edge.vertIndex[1]];
if ( edge.dot[0] != edge.dot[1] )
{
// UNDONE: We already know the sign (and probably value) of this in ScoreVerts()
float x = -edge.dot[1] / (edge.dot[0] - edge.dot[1]);
if ( x < 0.f )
return SolvePoint( simplex, p1 );
if ( x > 1.f )
return SolvePoint( simplex, p0 );
simplex.vertCount = 2;
// may be aliases, so copy to temp before writing
simplexvert_t t0 = p0, t1 = p1;
simplex.verts[0] = t0;
simplex.verts[1] = t1;
return x * t0.position + (1.f-x)*t1.position;
}
else
{
return SolvePoint( simplex, p0 );
}
}
GlideResult
TaskMacCready::glide_solution(const AircraftState &aircraft)
{
const fixed aircraft_min_height = get_min_height(aircraft);
GlideResult acc_gr;
AircraftState aircraft_predict = get_aircraft_start(aircraft);
for (unsigned i = 0, size = points.size(); i < size; ++i) {
const fixed tp_min_height = std::max(aircraft_min_height,
points[i]->GetElevation());
// perform estimate, ensuring that alt is above previous taskpoint
const GlideResult gr = SolvePoint(*points[i], aircraft_predict,
tp_min_height);
leg_solutions[i] = gr;
// update state
if (i == 0)
acc_gr = gr;
else
acc_gr.Add(gr);
/* make sure the next leg doesn't start below the safety altitude
of the current turn point, because we assume that the pilot
will never progress to the next leg if he's too low */
aircraft_predict.altitude = tp_min_height;
if (positive(gr.altitude_difference))
/* .. but start higher if the last calculation allows it */
aircraft_predict.altitude += gr.altitude_difference;
}
leg_solutions[active_index].CalcDeferred();
acc_gr.CalcDeferred();
return acc_gr;
}
Vector SolveVoronoiRegion4( simplex_t &simplex, const simplexvert_t &newPoint )
{
int i;
const simplexvert_t *vertList[4] = {&simplex.verts[0], &simplex.verts[1], &simplex.verts[2], &newPoint};
// first test the edge planes
// These are the boundaries of the voronoi regions around each vert
// We test them in pairs because verts along an edge share a plane normal
// and that makes it convenient
int vertScores[4];
memset( vertScores, 0, sizeof(vertScores) );
for ( i = 0; i < 6; i++ )
{
const Vector &v1 = vertList[g_VEdges[i].vertIndex[1]]->position;
const Vector &v0 = vertList[g_VEdges[i].vertIndex[0]]->position;
g_VEdges[i].dir = v1 - v0;
VectorNormalize( g_VEdges[i].dir );
g_VEdges[i].score = 0;
g_VEdges[i].dot[0] = DotProduct( v0, g_VEdges[i].dir );
g_VEdges[i].dot[1] = DotProduct( v1, g_VEdges[i].dir );
g_VEdges[i].ScoreVerts( vertScores );
}
// Are we in any of the vert voronoi regions?
// if on the front (vert) side of all 3 planes at that vert, then yes
for ( i = 0; i < 4; i++ )
{
// yep, return this vert
if ( vertScores[i] == 3 )
return SolvePoint( simplex, *vertList[i] );
}
// compute each face's normal (needed for edge & face tests)
for ( i = 0; i < 4; i++ )
{
g_VFaces[i].normal = CrossProduct( g_VEdges[ g_VFaces[i].normalEdgeIndex[0] ].dir, g_VEdges[ g_VFaces[i].normalEdgeIndex[1] ].dir );
VectorNormalize( g_VFaces[i].normal );
g_VFaces[i].faceFlip = 0;
g_VFaces[i].dist = DotProduct( g_VFaces[i].normal, vertList[g_VFaces[i].vertIndex[0]]->position );
float dot = DotProduct( g_VFaces[i].normal, vertList[g_VFaces[i].offVert]->position );
if ( dot > g_VFaces[i].dist )
{
g_VFaces[i].normal *= -1;
g_VFaces[i].dist = -g_VFaces[i].dist;
g_VFaces[i].faceFlip = 1;
}
g_VFaces[i].score = 0;
}
// test for edge voronoi regions
for ( i = 0; i < 6; i++ )
{
const Vector &vert0 = vertList[g_VEdges[i].vertIndex[0]]->position;
voronoiface_t &face0 = g_VFaces[g_VEdges[i].faceIndex[0]];
Vector normal0 = face0.faceFlip ? CrossProduct( face0.normal, g_VEdges[i].dir ) : CrossProduct( g_VEdges[i].dir, face0.normal );
VectorNormalize( normal0 );
float dot0 = DotProduct( normal0, vert0 );
if ( dot0 >= 0 )
{
face0.score++;
}
voronoiface_t &face1 = g_VFaces[g_VEdges[i].faceIndex[1]];
Vector normal1 = face1.faceFlip ? CrossProduct( g_VEdges[i].dir, face1.normal ) : CrossProduct( face1.normal, g_VEdges[i].dir );
VectorNormalize( normal1 );
float dot1 = DotProduct( normal1, vert0 );
if ( dot1 >= 0 )
{
face1.score++;
}
// not inside this edge region
if ( g_VEdges[i].score == 0 || dot0 >= 0 || dot1 >= 0 )
continue;
return SolveEdge( simplex, g_VEdges[i], vertList );
}
int score = 0;
// test each face
for ( i = 0; i < 4; i++ )
{
if ( g_VFaces[i].dist > 0 )
{
score++;
continue;
}
if ( g_VFaces[i].score < 3 )
continue;
return SolveFace( simplex, g_VFaces[i], vertList );
}
// Assert( score == 4 );
return SolveTetrahedron( simplex, vertList );
}
// UNDONE: Collapse these routines into a single general routine?
Vector SolveVoronoiRegion3( simplex_t &simplex, const simplexvert_t &newPoint )
{
int i;
const simplexvert_t *vertList[3] = {&simplex.verts[0], &simplex.verts[1], &newPoint};
// first test the edge planes
// These are the boundaries of the voronoi regions around each vert
// We test them in pairs because verts along an edge share a plane normal
// and that makes it convenient
int vertScores[3];
memset( vertScores, 0, sizeof(vertScores) );
for ( i = 0; i < 3; i++ )
{
const Vector &v1 = vertList[g_VEdges[i].vertIndex[1]]->position;
const Vector &v0 = vertList[g_VEdges[i].vertIndex[0]]->position;
g_VEdges[i].dir = v1 - v0;
VectorNormalize( g_VEdges[i].dir );
g_VEdges[i].score = 0;
g_VEdges[i].dot[0] = DotProduct( v0, g_VEdges[i].dir );
g_VEdges[i].dot[1] = DotProduct( v1, g_VEdges[i].dir );
g_VEdges[i].ScoreVerts( vertScores );
}
// Are we in any of the vert voronoi regions?
// if on the front (vert) side of all 3 planes at that vert, then yes
for ( i = 0; i < 3; i++ )
{
// yep, return this vert
if ( vertScores[i] == 2 )
return SolvePoint( simplex, *vertList[i] );
}
voronoiface_t &face0 = g_VFaces[0];
// compute each face's normal (needed for edge & face tests)
{
face0.normal = CrossProduct( g_VEdges[ face0.normalEdgeIndex[0] ].dir, g_VEdges[ face0.normalEdgeIndex[1] ].dir );
VectorNormalize( face0.normal );
face0.faceFlip = 0;
face0.dist = DotProduct( face0.normal, vertList[face0.vertIndex[0]]->position );
face0.score = 0;
}
// test for edge voronoi regions
for ( i = 0; i < 3; i++ )
{
// assumes 0-1, 0-2, 1-2 edges
int offVert[3] = {2, 1, 0};
const Vector &vert0 = vertList[g_VEdges[i].vertIndex[0]]->position;
Vector normal0 = CrossProduct( face0.normal, g_VEdges[i].dir );
VectorNormalize( normal0 );
float dot0 = DotProduct( normal0, vert0 );
if ( DotProduct( normal0, vertList[offVert[i]]->position ) > dot0 )
{
dot0 = -dot0;
}
if ( dot0 < 0 && g_VEdges[i].score )
{
// must be in this region because we checked the other 2 bounding
// planes in the vert region test
return SolveEdge( simplex, g_VEdges[i], vertList );
}
// origin is on face side
face0.score++;
}
//Assert( face0.score == 3 );
// must be in face region
return SolveFace( simplex, face0, vertList );
}
