bool
AbstractTaskFactory::TestFAITriangle()
{
if (!is_unique())
return false;
if (m_task.TaskSize() != 4)
return false;
const fixed d1 = m_task.getTaskPoint(1)->GetVectorPlanned().Distance;
const fixed d2 = m_task.getTaskPoint(2)->GetVectorPlanned().Distance;
const fixed d3 = m_task.getTaskPoint(3)->GetVectorPlanned().Distance;
return TestFAITriangle(d1, d2, d3);
}
bool
AbstractTaskFactory::TestFAITriangle()
{
bool valid = is_unique();
if (m_task.task_size()==4) {
const fixed d1 = m_task.getTaskPoint(1)->get_vector_planned().Distance;
const fixed d2 = m_task.getTaskPoint(2)->get_vector_planned().Distance;
const fixed d3 = m_task.getTaskPoint(3)->get_vector_planned().Distance;
valid = TestFAITriangle(d1, d2, d3);
}
else
valid = false;
return valid;
}
bool
FAITrianglePointValidator::IsFAITrianglePoint(const Waypoint& wp,
bool right) const
{
if (fai_triangle_point_invalid)
return false;
if (!task)
return true;
if (t_size == 0)
return true;
const GeoPoint &p = wp.location;
// replace start
if (t_index == 0) {
assert(t_size <= 4 && t_size > 0);
switch (t_size) {
case 1:
return true;
case 2:
return p.Distance(task->GetPoint(1).GetLocation()) > min_fai_leg;
default: // size == 3 or 4
if (!IsFAIAngle(p, task->GetPoint(1).GetLocation(),
task->GetPoint(2).GetLocation(), right))
return false;
if (t_size == 3) {
return TestFAITriangle(p.Distance(task->GetPoint(1).GetLocation()),
leg2,
task->GetPoint(2).GetLocation().Distance(p));
} else if (t_size == 4) {
return (wp == task->GetPoint(3).GetWaypoint()) &&
TestFAITriangle(p.Distance(task->GetPoint(1).GetLocation()),
leg2,
leg3);
}
}
}
// append or replace point #1
if (t_index == 1) {
assert(t_size > 0);
if (t_size <= 2)
return p.Distance(task->GetPoint(0).GetLocation()) > min_fai_leg;
// size == 3 or 4
if (!IsFAIAngle(task->GetPoint(0).GetLocation(),
p,
task->GetPoint(2).GetLocation(), right))
return false;
if (t_size == 3) {
return TestFAITriangle(p.Distance(task->GetPoint(0).GetLocation()),
p.Distance(task->GetPoint(2).GetLocation()),
task->GetPoint(2).GetLocation().
Distance(task->GetPoint(0).GetLocation()));
} else if (t_size == 4) {
return TestFAITriangle(p.Distance(task->GetPoint(0).GetLocation()),
p.Distance(task->GetPoint(2).GetLocation()),
leg3);
}
}
// append or replace point #2
if (t_index == 2) {
assert(t_size >= 2);
if (!IsFAIAngle(task->GetPoint(0).GetLocation(),
task->GetPoint(1).GetLocation(),
p, right))
return false;
if (t_size < 4) { // no finish point yet
return TestFAITriangle(leg1,
p.Distance(task->GetPoint(1).GetLocation()),
p.Distance(task->GetPoint(0).GetLocation()));
} else { // already finish point(#3) exists
return task->GetPoint(0).GetWaypoint() == task->GetPoint(3).GetWaypoint() &&
TestFAITriangle(leg1,
p.Distance(task->GetPoint(1).GetLocation()),
p.Distance(task->GetPoint(0).GetLocation()));
}
}
// append or replace finish
if (t_index == 3) {
assert (t_size == 3 || t_size == 4);
return (wp == task->GetPoint(0).GetWaypoint()) &&
TestFAITriangle(leg1,
leg2,
p.Distance(task->GetPoint(2).GetLocation()));
}
return true;
}