QgsVector calcMotion( const QgsPoint &a, const QgsPoint &b, const QgsPoint &c,
double lowerThreshold, double upperThreshold )
{
QgsVector p = a - b;
QgsVector q = c - b;
if ( qgsDoubleNear( p.length(), 0.0 ) || qgsDoubleNear( q.length(), 0.0 ) )
return QgsVector( 0, 0 );
// 2.0 is a magic number from the original JOSM source code
double scale = 2.0 * std::min( p.length(), q.length() );
p = p.normalized();
q = q.normalized();
double dotProduct = p * q;
if ( !dotProductWithinAngleTolerance( dotProduct, lowerThreshold, upperThreshold ) )
{
return QgsVector( 0, 0 );
}
// wonderful nasty hack which has survived through JOSM -> id -> QGIS
// to deal with almost-straight segments (angle is closer to 180 than to 90/270).
if ( dotProduct < -M_SQRT1_2 )
dotProduct += 1.0;
QgsVector new_v = p + q;
// 0.1 magic number from JOSM implementation - think this is to limit each iterative step
return new_v.normalized() * ( 0.1 * dotProduct * scale );
}
double normalizedDotProduct( const QgsPoint &a, const QgsPoint &b, const QgsPoint &c )
{
QgsVector p = a - b;
QgsVector q = c - b;
if ( p.length() > 0 )
p = p.normalized();
if ( q.length() > 0 )
q = q.normalized();
return p * q;
}