bool G_arcSector::inRect(const QRect &rect) const
{
if(getNearestPoint(rect.topRight()) == G_point(rect.topRight())) return true;
if(getNearestPoint(rect.topLeft()) == G_point(rect.topLeft())) return true;
if(getNearestPoint(rect.bottomRight()) == G_point(rect.bottomRight())) return true;
if(getNearestPoint(rect.bottomLeft()) == G_point(rect.bottomLeft())) return true;
if(arc.inRect(rect)) return true;
if(G_segment(arc.getCenter(), arc.getPointOnCurve(0)).inRect(rect)) return true;
if(G_segment(arc.getCenter(), arc.getPointOnCurve(1)).inRect(rect)) return true;
return false;
}
std::pair<std::array<Vector3, 2>, float> Rect3::getNearestPoint(const Ray& ray) const
{
const Vector3& org = ray.getOrigin();
const Vector3& dir = ray.getDirection();
bool foundNearest = false;
float t = 0.0f;
std::array<Vector3, 2> nearestPoints {{ Vector3::ZERO, Vector3::ZERO }};
float distance = 0.0f;
// Check if Ray intersects the rectangle
auto intersectResult = intersects(ray);
if (intersectResult.first)
{
t = intersectResult.second;
nearestPoints[0] = org + dir * t;
nearestPoints[1] = nearestPoints[0]; // Just one point of intersection
foundNearest = true;
}
// Ray is either passing next to the rectangle or parallel to it,
// compare ray to 4 edges of the rectangle
if (!foundNearest)
{
Vector3 scaledAxes[2];
scaledAxes[0] = mExtentHorz * mAxisHorz;
scaledAxes[1] = mExtentVert * mAxisVert;;
distance = std::numeric_limits<float>::max();
for (UINT32 i = 0; i < 2; i++)
{
for (UINT32 j = 0; j < 2; j++)
{
float sign = (float)(2 * j - 1);
Vector3 segCenter = mCenter + sign * scaledAxes[i];
Vector3 segStart = segCenter - scaledAxes[1 - i];
Vector3 segEnd = segCenter + scaledAxes[1 - i];
LineSegment3 segment(segStart, segEnd);
auto segResult = segment.getNearestPoint(ray);
if (segResult.second < distance)
{
nearestPoints = segResult.first;
distance = segResult.second;
}
}
}
}
// Front of the ray is nearest, use found points
if (t >= 0.0f)
{
// Do nothing, we already have the points
}
else // Rectangle is behind the ray origin, find nearest point to origin
{
auto nearestPointToOrg = getNearestPoint(org);
nearestPoints[0] = org;
nearestPoints[1] = nearestPointToOrg.first;
distance = nearestPointToOrg.second;
}
return std::make_pair(nearestPoints, distance);
}
QPointF CalculateLaylines::getNextPoint( const QVector<QPointF> &route, const QPointF &boatPos, const float &offset) {
int nearest_point = getNearestPoint(route, boatPos);
if ( checkIntersection( "obstacles_r", boatPos ) ) {
// if we are inside an obstacle, don't even try
return boatPos;
}
// we got the nearest point in the route
// but could be far, let's find out the real
// nearest point of the route by making a
// projection towards it...
// START SEARCH OF PROJECTION FOR FINETUNE
//2. We'll get projections to each point on the route:
QPointF projection_point;
QLineF route_line, projection_line;
QPointF a, b, c;
if ( route.size() >= 2 ) {
// If nearest is the last one:
if ( route.size() - nearest_point == 1 ) {
a = UwMath::toConformal( route.at( nearest_point - 1 ) );
b = UwMath::toConformal( route.at( nearest_point ) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point - 1 ));
route_line.setP2( route.at( nearest_point ));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
// If nearest is the first one:
} else if ( nearest_point == 0 ) {
a = UwMath::toConformal( route.at( nearest_point) );
b = UwMath::toConformal( route.at( nearest_point + 1) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point));
route_line.setP2( route.at( nearest_point + 1));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
// If nearest is not first or last one:
} else {
a = UwMath::toConformal( route.at( nearest_point) );
b = UwMath::toConformal( route.at( nearest_point + 1) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point));
route_line.setP2( route.at( nearest_point + 1));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
// We must check if our position is between nearest and nearest + 1
// or between nearest - 1 and nearest:
if ( !checkIntersection(route_line, projection_line) ) {
a = UwMath::toConformal( route.at( nearest_point - 1 ) );
b = UwMath::toConformal( route.at( nearest_point ) );
c = UwMath::toConformal( (const QPointF)boatPos );
projection_point = UwMath::getProjectionPoint( a, b, c);
UwMath::fromConformal( projection_point);
route_line.setP1( route.at( nearest_point - 1 ));
route_line.setP2( route.at( nearest_point ));
projection_line.setP1( boatPos);
projection_line.setP2( projection_point);
}
}
}
// Next we'll find the checkpoint:
//3. We'll check if there are obstacles in the projeted triangle:
if ( route.size() < 1 ) {
} else if ( route.size() == 1 ) {
// Route to process only has one point:
QLineF heading( boatPos, route.at( nearest_point));
bool hobs_r = checkIntersection( "obstacles_r", heading );
bool hobs_l = checkIntersection( "obstacles_l", heading );
// Finetune checkpoint in the heading:
if ( hobs_r || hobs_l ) {
this->obstacleFound = true;
bool ready = false;
QLineF last_heading;
while ( !ready ) {
if ( ( hobs_r && checkIntersection( "obstacles_r", heading ) ) ||
( hobs_l && checkIntersection( "obstacles_l", heading ) ) ) {
last_heading = heading;
heading = UwMath::lineToHalf( heading);
} else {
if ( checkOffset( heading, last_heading, offset ) )
ready = true;
else
heading = UwMath::avgLine( heading, last_heading);
}
}
//We won't return the point between boat and checkpoint as the next checkpoint on long-term route:
return heading.p2();
} else {
//We won't return the point between boat and checkpoint as the next checkpoint on long-term route:
return heading.p2();
}
} else if ( route.size() > 1 ) {
// Route to process has more than 1 point:
int i = nearest_point;
QPolygonF triangle;
// 1st point:
triangle << boatPos;
// 2nd point:
if ( checkIntersection( route_line, projection_line) ) {
triangle << projection_point;
} else {
triangle << route.at( nearest_point);
}
// 3rd point:
if ( route.at( nearest_point) == route_line.p2() ) {
triangle << route.at( nearest_point);
i = nearest_point - 1;
} else if ( route.at( nearest_point) == route_line.p1() ) {
triangle << route.at( nearest_point + 1);
}
// // ########################################################################
// // SPECIAL CHECK to find obstacles in the heading of the first triangle ##
// // ########################################################################'
if ( triangle.at( 0 ) == boatPos ) {
QLineF heading( triangle.at( 0), triangle.at( 1));
QPolygonF last_triangle;
bool hobs_r = checkIntersection( "obstacles_r", heading, triangle );
bool hobs_l = checkIntersection( "obstacles_l", heading, triangle );
// FINETUNE CHECKPOINT IN THE HEADING
if ( hobs_r || hobs_l ) {
bool ready = false;
QLineF last_heading;
while ( !ready ) {
if ( ( hobs_r && checkIntersection( "obstacles_r", heading, triangle) ) ||
( hobs_l && checkIntersection( "obstacles_l", heading, triangle)) ) {
last_heading = heading;
heading = UwMath::lineToHalf( heading);
last_triangle = triangle;
triangle = UwMath::triangleToHalf(triangle);
} else {
// if ( checkOffset( heading, last_heading, offset ) )
if ( checkOffset_OnePointVersion( last_triangle, triangle, offset ))
ready = true;
else
triangle = UwMath::avgTriangle_OnePointVersion( triangle, last_triangle);
// heading = UwMath::avgLine( heading, last_heading);
}
}
return heading.p2(); //We won't return the point between boat and checkpoint as the next checkpoint on long-term route:
// return triangle.at(2);
}
}
//qDebug() << Q_FUNC_INFO << ": TriangleCount at specialCheck: " << triangleCount;
// ########################################################################
// END SPECIAL CHECK ##
// ########################################################################
bool obs_r = checkIntersection( "obstacles_r", triangle, triangle );
bool obs_l = checkIntersection( "obstacles_l", triangle, triangle );
//Note: This doesn't work correctly. Here the search for the route fails instantly when obstacles
//are found on the route. What if there is obstacle-free route on the next route interval:
while ( !obs_r && !obs_l && i < (route.size()/* - 2*/ )) {
triangle.clear();
triangle << boatPos;
triangle << route.at( i);
triangle << route.at( i + 1);
obs_r = checkIntersection( "obstacles_r", triangle, triangle );
obs_l = checkIntersection( "obstacles_l", triangle, triangle );
// Finetune checkpoint:
if ( obs_r || obs_l ) {
bool ready = false;
QPolygonF last_triangle;
while ( !ready ) {
if ( obs_r && checkIntersection( "obstacles_r", triangle, triangle) ||
obs_l && checkIntersection( "obstacles_l", triangle, triangle) ) {
last_triangle = triangle;
triangle = UwMath::triangleToHalf_OnePointVersion( triangle);
} else {
if (checkOffset_OnePointVersion( last_triangle, triangle, offset ) ){
ready = true;
}
else{
triangle = UwMath::avgTriangle_OnePointVersion( triangle, last_triangle);
}
}
//If the points in the line segment on the side of the route there is no space for the boat in
//the map anymore and we can quit the search:
if(triangle.at(1) == triangle.at(2))
ready = true;
}
}
i++;
}
return triangle.at( 2);
}
// End the process of searching for the checkpoints.
}
bool checkDistance( const QPoint& _from, const QPoint& _to, const int _distance )
{
return getDistance( _from, getNearestPoint( _from, _to ) ) <= _distance;
}
QPoint getNearestPoint( const QPoint& _from, const QPoint& _to )
{
return getNearestPoint( _from, QRect( _to, QSize( 1, 1 ) ) );
}
//drawing:
void G_segment::draw(QPainter &p, const G_drawstyle &d, bool selected)
{
QRect r = p.window();
if(!inRect(r)) return;
G_point tmp1, tmp2;
if(fabs(p1.getX()) < DRAW_MAX && fabs(p1.getY()) < DRAW_MAX) {
tmp1 = p1;
}
else {
tmp1 = getNearestPoint(G_point(0, 0));
tmp1 += (p1 - tmp1) * (DRAW_MAX) / (p1 - tmp1).length();
}
if(fabs(p2.getX()) < DRAW_MAX && fabs(p2.getY()) < DRAW_MAX) {
tmp2 = p2;
}
else {
tmp2 = getNearestPoint(G_point(0, 0));
tmp2 += (p2 - tmp2) * (DRAW_MAX) / (p2 - tmp2).length();
}
//Now Draw!
if(/*p.device()->isExtDev() ||*/ p.hasViewXForm()) { //draw at higher accuracy to a printer
tmp1 *= 8.;
tmp2 *= 8.;
p.scale(0.125, .125);
int w = d.getPen().width() * 8;
if(w == 0) w = 2;
//if(!p.device()->isExtDev()) w = (d.getPen().width() == 0 ? 4 : 4 * d.getPen().width());
p.setPen(QPen(d.getPen().color(), w, d.getPen().style()));
p.drawLine(tmp1.toQPoint(), tmp2.toQPoint());
p.scale(8, 8);
return;
}
if(selected && KSegView::getSelectType() == KSegView::BORDER_SELECT) {
int width = d.getPen().width() ? d.getPen().width() + 3 : 4;
p.setPen(QPen(G_drawstyle::getBorderColor(d.getPen().color()), width));
p.drawLine(tmp1.toQPoint(), tmp2.toQPoint());
}
p.setPen(d.getPen());
if(selected && KSegView::getSelectType() == KSegView::BLINKING_SELECT) {
QColor c(QTime::currentTime().msec() * 17, 255, 255, QColor::Hsv);
p.setPen(QPen(c, d.getPen().width(), d.getPen().style()));
}
p.drawLine(tmp1.toQPoint(), tmp2.toQPoint());
return;
}
