Various fixes to geometry & math library.

common/geometry/shape_collisions.cpp

@@ -23,11 +23,13 @@
  */
 
 #include <math/vector2d.h>
+#include <math.h>
 
 #include <geometry/shape.h>
 #include <geometry/shape_line_chain.h>
 #include <geometry/shape_circle.h>
 #include <geometry/shape_rect.h>
+#include <geometry/shape_segment.h>
 
 typedef VECTOR2I::extended_type ecoord;
 
@@ -45,25 +47,21 @@ static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_CIRCLE& aB, int
         return false;
 
     if( aNeedMTV )
-        aMTV = delta.Resize( sqrt( abs( min_dist_sq - dist_sq ) ) + 1 );
+        aMTV = delta.Resize( min_dist - sqrt( dist_sq ) + 3 );  // fixme: apparent rounding error
 
     return true;
 }
 
 
-static inline  bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance,
-                             bool aNeedMTV, VECTOR2I& aMTV )
+static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int aClearance,
+                            bool aNeedMTV, VECTOR2I& aMTV )
 {
     const VECTOR2I c = aB.GetCenter();
     const VECTOR2I p0 = aA.GetPosition();
     const VECTOR2I size = aA.GetSize();
-    const ecoord r = aB.GetRadius();
-    const ecoord min_dist = aClearance + r;
-    const ecoord min_dist_sq = min_dist * min_dist;
-
-    if( aA.BBox( 0 ).Contains( c ) )
-        return true;
-
+    const int r = aB.GetRadius();
+    const int min_dist = aClearance + r;
+    
     const VECTOR2I vts[] =
     {
         VECTOR2I( p0.x,          p0.y ),
@@ -73,33 +71,35 @@ static inline  bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int a
         VECTOR2I( p0.x,          p0.y )
     };
 
-    ecoord nearest_seg_dist_sq = VECTOR2I::ECOORD_MAX;
+    int nearest_seg_dist = INT_MAX;
     VECTOR2I nearest;
 
     bool inside = c.x >= p0.x && c.x <= ( p0.x + size.x )
                   && c.y >= p0.y && c.y <= ( p0.y + size.y );
 
-    if( !inside )
+
+    if( !aNeedMTV && inside )
+        return true;
+
+    for( int i = 0; i < 4; i++ )
     {
-        for( int i = 0; i < 4; i++ )
+        const SEG seg( vts[i], vts[i + 1] );
+
+        VECTOR2I pn = seg.NearestPoint( c );
+
+        int d = ( pn - c ).EuclideanNorm();
+
+        if( ( d < min_dist ) && !aNeedMTV )
+            return true;
+
+        if( d < nearest_seg_dist )
         {
-            const SEG seg( vts[i], vts[i + 1] );
-            ecoord dist_sq = seg.SquaredDistance( c );
-
-            if( dist_sq < min_dist_sq )
-            {
-                if( !aNeedMTV )
-                    return true;
-                else
-                {
-                    nearest = seg.NearestPoint( c );
-                    nearest_seg_dist_sq = dist_sq;
-                }
-            }
+            nearest = pn;
+            nearest_seg_dist = d;
         }
     }
 
-    if( nearest_seg_dist_sq >= min_dist_sq && !inside )
+    if( nearest_seg_dist >= min_dist && !inside )
         return false;
 
     VECTOR2I delta = c - nearest;
@@ -107,25 +107,77 @@ static inline  bool Collide( const SHAPE_RECT& aA, const SHAPE_CIRCLE& aB, int a
     if( !aNeedMTV )
         return true;
 
+   
     if( inside )
-        aMTV = -delta.Resize( sqrt( abs( r * r + nearest_seg_dist_sq ) + 1 ) );
+        aMTV = -delta.Resize( abs( min_dist + 1 + nearest_seg_dist ) + 1 );
     else
-        aMTV = delta.Resize( sqrt( abs( r * r - nearest_seg_dist_sq ) + 1 ) );
+        aMTV = delta.Resize( abs( min_dist + 1 - nearest_seg_dist ) + 1 );
+
 
     return true;
 }
 
 
+static VECTOR2I pushoutForce( const SHAPE_CIRCLE& aA, const SEG& aB, int aClearance )
+{
+    VECTOR2I nearest = aB.NearestPoint( aA.GetCenter() );
+    VECTOR2I f (0, 0);
+
+    int dist = ( nearest - aA.GetCenter() ).EuclideanNorm();
+    int min_dist = aClearance + aA.GetRadius();
+
+    if( dist < min_dist )
+        f = ( aA.GetCenter() - nearest ).Resize ( min_dist - dist + 10 );
+
+    return f;
+}
+
+
 static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_LINE_CHAIN& aB, int aClearance,
                             bool aNeedMTV, VECTOR2I& aMTV )
 {
+    bool found = false;
+    VECTOR2I::extended_type clSq = (VECTOR2I::extended_type) aClearance * aClearance;
+
+
     for( int s = 0; s < aB.SegmentCount(); s++ )
     {
+
         if( aA.Collide( aB.CSegment( s ), aClearance ) )
-            return true;
+        {
+            found = true;
+            break;
+        }
     }
 
-    return false;
+    if( !aNeedMTV || !found )
+        return found;
+    
+    SHAPE_CIRCLE cmoved( aA );
+    VECTOR2I f_total( 0, 0 );
+
+    for( int s = 0; s < aB.SegmentCount(); s++ )
+    {
+        VECTOR2I f = pushoutForce( cmoved, aB.CSegment( s ), aClearance );
+        cmoved.SetCenter( cmoved.GetCenter() + f );
+        f_total += f;
+    }
+    
+    aMTV = f_total;
+    return found;
+}
+
+
+static inline bool Collide( const SHAPE_CIRCLE& aA, const SHAPE_SEGMENT& aSeg, int aClearance,
+                            bool aNeedMTV, VECTOR2I& aMTV )
+{
+	bool col = aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2);
+
+    if( col && aNeedMTV )
+    {
+        aMTV = pushoutForce( aA, aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2);
+    }
+    return col;
 }
 
 
@@ -155,74 +207,123 @@ static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_LINE_CHAIN& aB, in
 }
 
 
-bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance,
-                    bool aNeedMTV, VECTOR2I& aMTV )
+static inline bool Collide( const SHAPE_RECT& aA, const SHAPE_SEGMENT& aSeg, int aClearance,
+                            bool aNeedMTV, VECTOR2I& aMTV )
 {
-    switch( aA->Type() )
-    {
-    case SH_RECT:
-        switch( aB->Type() )
-        {
-        case SH_CIRCLE:
-            return Collide( *static_cast<const SHAPE_RECT*>( aA ),
-                *static_cast<const SHAPE_CIRCLE*>( aB ), aClearance, aNeedMTV, aMTV );
+	return aA.Collide( aSeg.GetSeg(), aClearance + aSeg.GetWidth() / 2 );
+}
 
-        case SH_LINE_CHAIN:
-            return Collide( *static_cast<const SHAPE_RECT*>( aA ),
-                *static_cast<const SHAPE_LINE_CHAIN*>( aB ), aClearance, aNeedMTV, aMTV );
 
-        default:
-            break;
-        }
-        break;
+static inline bool Collide( const SHAPE_SEGMENT& aA, const SHAPE_SEGMENT& aB, int aClearance,
+                            bool aNeedMTV, VECTOR2I& aMTV )
+{
+	return aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2 );
+}
 
-    case SH_CIRCLE:
-        switch( aB->Type() )
-        {
-        case SH_RECT:
-            return Collide( *static_cast<const SHAPE_RECT*>( aB ),
-                *static_cast<const SHAPE_CIRCLE*>( aA ), aClearance, aNeedMTV, aMTV );
 
-        case SH_CIRCLE:
-            return Collide( *static_cast<const SHAPE_CIRCLE*>( aA ),
-                *static_cast<const SHAPE_CIRCLE*>( aB ), aClearance, aNeedMTV, aMTV );
+static inline bool Collide( const SHAPE_LINE_CHAIN& aA, const SHAPE_SEGMENT& aB, int aClearance,
+                            bool aNeedMTV, VECTOR2I& aMTV )
+{
+	if( aA.Collide( aB.GetSeg(), aClearance + aB.GetWidth() / 2 ) )
+		return true;
 
-        case SH_LINE_CHAIN:
-            return Collide( *static_cast<const SHAPE_CIRCLE*>( aA ),
-                *static_cast<const SHAPE_LINE_CHAIN *>( aB ), aClearance, aNeedMTV, aMTV );
+	return false;
+}
 
-        default:
-            break;
-        }
-        break;
 
-    case SH_LINE_CHAIN:
-        switch( aB->Type() )
-        {
-        case SH_RECT:
-            return Collide( *static_cast<const SHAPE_RECT*>( aB ),
-                *static_cast<const SHAPE_LINE_CHAIN*>( aA ), aClearance, aNeedMTV, aMTV );
+template<class ShapeAType, class ShapeBType> bool
+CollCase( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV )
+{
+	return Collide (*static_cast<const ShapeAType*>( aA ),
+					*static_cast<const ShapeBType*>( aB ),
+					aClearance, aNeedMTV, aMTV);
+}
 
-        case SH_CIRCLE:
-            return Collide( *static_cast<const SHAPE_CIRCLE*>( aB ),
-                *static_cast<const SHAPE_LINE_CHAIN*>( aA ), aClearance, aNeedMTV, aMTV );
+bool CollideShapes( const SHAPE* aA, const SHAPE* aB, int aClearance, bool aNeedMTV, VECTOR2I& aMTV )
+{
+	switch( aA->Type() )
+	{
+		case SH_RECT:
+			switch( aB->Type() )
+			{
+				case SH_CIRCLE: 
+					return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );
 
-        case SH_LINE_CHAIN:
-            return Collide( *static_cast<const SHAPE_LINE_CHAIN*>( aA ),
-                *static_cast<const SHAPE_LINE_CHAIN*>( aB ), aClearance, aNeedMTV, aMTV );
+				case SH_LINE_CHAIN: 
+					return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );
 
-        default:
-            break;
-        }
-        break;
+				case SH_SEGMENT: 
+					return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
 
-    default:
-        break;
-    }
+				default:
+					break;
+			}
 
-    assert( 0 );    // unsupported_collision
+		case SH_CIRCLE:
+			switch( aB->Type() )
+			{
+				case SH_RECT:
+					return CollCase<SHAPE_RECT, SHAPE_CIRCLE>( aB, aA, aClearance, aNeedMTV, aMTV );
 
-    return false;
+				case SH_CIRCLE:
+					return CollCase<SHAPE_CIRCLE, SHAPE_CIRCLE>( aA, aB, aClearance, aNeedMTV, aMTV );
+
+				case SH_LINE_CHAIN:
+					return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );
+
+				case SH_SEGMENT:
+					return CollCase<SHAPE_CIRCLE, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
+				default:
+					break;
+			}
+
+		case SH_LINE_CHAIN:
+			switch( aB->Type() )
+			{
+				case SH_RECT:
+					return CollCase<SHAPE_RECT, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );
+
+				case SH_CIRCLE:
+					return CollCase<SHAPE_CIRCLE, SHAPE_LINE_CHAIN>( aB, aA, aClearance, aNeedMTV, aMTV );
+
+				case SH_LINE_CHAIN:
+					return CollCase<SHAPE_LINE_CHAIN, SHAPE_LINE_CHAIN>( aA, aB, aClearance, aNeedMTV, aMTV );
+
+				case SH_SEGMENT:
+					return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
+
+				default:
+					break;
+			}
+
+		case SH_SEGMENT:
+			switch( aB->Type() )
+			{
+				case SH_RECT:
+					return CollCase<SHAPE_RECT, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );
+
+				case SH_CIRCLE:
+					return CollCase<SHAPE_CIRCLE, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );
+
+				case SH_LINE_CHAIN:
+					return CollCase<SHAPE_LINE_CHAIN, SHAPE_SEGMENT>( aB, aA, aClearance, aNeedMTV, aMTV );
+
+				case SH_SEGMENT:
+					return CollCase<SHAPE_SEGMENT, SHAPE_SEGMENT>( aA, aB, aClearance, aNeedMTV, aMTV );
+
+				default:
+					break;
+			}
+
+		default:
+			break;
+	}
+
+	bool unsupported_collision = true;
+
+	assert( unsupported_collision == false );
+
+	return false;
 }
 
 

common/geometry/shape_line_chain.cpp

@@ -135,6 +135,9 @@ int SHAPE_LINE_CHAIN::Distance( const VECTOR2I& aP ) const
 {
     int d = INT_MAX;
 
+    if( IsClosed() && PointInside( aP ) )
+        return 0;
+
     for( int s = 0; s < SegmentCount(); s++ )
         d = std::min( d, CSegment( s ).Distance( aP ) );
 
@@ -179,7 +182,7 @@ int SHAPE_LINE_CHAIN::Split( const VECTOR2I& aP )
 
 int SHAPE_LINE_CHAIN::Find( const VECTOR2I& aP ) const
 {
-    for( int s = 0; s< PointCount(); s++ )
+    for( int s = 0; s < PointCount(); s++ )
         if( CPoint( s ) == aP )
             return s;
 
@@ -187,6 +190,16 @@ int SHAPE_LINE_CHAIN::Find( const VECTOR2I& aP ) const
 }
 
 
+int SHAPE_LINE_CHAIN::FindSegment( const VECTOR2I& aP ) const
+{
+    for( int s = 0; s < SegmentCount(); s++ )
+        if( CSegment( s ).Distance( aP ) <= 1 )
+            return s;
+
+    return -1;
+}
+
+
 const SHAPE_LINE_CHAIN SHAPE_LINE_CHAIN::Slice( int aStartIndex, int aEndIndex ) const
 {
     SHAPE_LINE_CHAIN rv;
@@ -261,6 +274,9 @@ int SHAPE_LINE_CHAIN::Intersect( const SHAPE_LINE_CHAIN& aChain, INTERSECTIONS&
 
             if( a.Collinear( b ) )
             {
+                is.our = a;
+                is.their = b;
+
                 if( a.Contains( b.A ) ) { is.p = b.A; aIp.push_back( is ); }
                 if( a.Contains( b.B ) ) { is.p = b.B; aIp.push_back( is ); }
                 if( b.Contains( a.A ) ) { is.p = a.A; aIp.push_back( is ); }
@@ -282,44 +298,6 @@ int SHAPE_LINE_CHAIN::Intersect( const SHAPE_LINE_CHAIN& aChain, INTERSECTIONS&
     }
 
     return aIp.size();
-
-    for( int s1 = 0; s1 < SegmentCount(); s1++ )
-    {
-        for( int s2 = 0; s2 < aChain.SegmentCount(); s2++ )
-        {
-            const SEG& a = CSegment( s1 );
-            const SEG& b = aChain.CSegment( s2 );
-            OPT_VECTOR2I p = a.Intersect( b );
-            INTERSECTION is;
-
-            if( p )
-            {
-                is.p = *p;
-                is.our = a;
-                is.their = b;
-                aIp.push_back( is );
-            }
-            else if( a.Collinear( b ) )
-            {
-                if( a.A != b.A && a.A != b.B && b.Contains( a.A ) )
-                {
-                    is.p = a.A;
-                    is.our = a;
-                    is.their = b;
-                    aIp.push_back( is );
-                }
-                else if( a.B != b.A && a.B != b.B && b.Contains( a.B ) )
-                {
-                    is.p = a.B;
-                    is.our = a;
-                    is.their = b;
-                    aIp.push_back( is );
-                }
-            }
-        }
-    }
-
-    return aIp.size();
 }
 
 
@@ -372,10 +350,13 @@ bool SHAPE_LINE_CHAIN::PointInside( const VECTOR2I& aP ) const
 
 bool SHAPE_LINE_CHAIN::PointOnEdge( const VECTOR2I& aP ) const
 {
-    if( SegmentCount() < 1 )
+	if( !PointCount() )
+		return false;
+
+	else if( PointCount() == 1 )
         return m_points[0] == aP;
 
-    for( int i = 1; i < SegmentCount(); i++ )
+    for( int i = 0; i < SegmentCount(); i++ )
     {
         const SEG s = CSegment( i );
 
@@ -534,3 +515,30 @@ const std::string SHAPE_LINE_CHAIN::Format() const
 
     return ss.str();
 }
+
+
+bool SHAPE_LINE_CHAIN::CompareGeometry ( const SHAPE_LINE_CHAIN & aOther ) const
+{
+    SHAPE_LINE_CHAIN a(*this), b(aOther);
+    a.Simplify();
+    b.Simplify();
+
+    if(a.m_points.size() != b.m_points.size())
+        return false;
+
+    for(int i = 0; i < a.PointCount(); i++)
+        if(a.CPoint(i) != b.CPoint(i))
+            return false;
+    return true;
+}
+
+bool SHAPE_LINE_CHAIN::Intersects( const SHAPE_LINE_CHAIN& aChain ) const
+{
+    INTERSECTIONS dummy;
+    return Intersect(aChain, dummy) != 0;
+}
+
+SHAPE* SHAPE_LINE_CHAIN::Clone() const
+{
+    return new SHAPE_LINE_CHAIN( *this );
+}

include/geometry/seg.h

@@ -244,14 +244,14 @@ public:
      */
     bool Collinear( const SEG& aSeg ) const
     {
-        ecoord qa = A.y - B.y;
-        ecoord qb = B.x - A.x;
-        ecoord qc = -qa * A.x - qb * A.y;
-
-        ecoord d1 = std::abs( aSeg.A.x * qa + aSeg.A.y * qb + qc );
-        ecoord d2 = std::abs( aSeg.B.x * qa + aSeg.B.y * qb + qc );
-
-        return ( d1 <= 1 && d2 <= 1 );
+        ecoord qa1 = A.y - B.y;
+        ecoord qb1 = B.x - A.x;
+        ecoord qc1 = -qa1 * A.x - qb1 * A.y;
+        ecoord qa2 = aSeg.A.y - aSeg.B.y;
+        ecoord qb2 = aSeg.B.x - aSeg.A.x;
+        ecoord qc2 = -qa2 * aSeg.A.x - qb2 * aSeg.A.y;
+
+        return ( qa1 == qa2 ) && ( qb1 == qb2 ) && ( qc1 == qc2 );
     }
 
     /**

include/geometry/shape_circle.h

@@ -38,9 +38,20 @@ public:
         SHAPE( SH_CIRCLE ), m_radius( aRadius ), m_center( aCenter )
     {}
 
+    SHAPE_CIRCLE ( const SHAPE_CIRCLE& aOther ) : 
+        SHAPE( SH_CIRCLE ),
+        m_radius( aOther.m_radius ),
+        m_center( aOther.m_center )
+    {};
+
     ~SHAPE_CIRCLE()
     {}
 
+    SHAPE* Clone() const
+    {
+        return new SHAPE_CIRCLE( *this );
+    }
+
     const BOX2I BBox( int aClearance = 0 ) const
     {
         const VECTOR2I rc( m_radius + aClearance, m_radius + aClearance );

include/geometry/shape_line_chain.h

@@ -116,6 +116,8 @@ public:
     ~SHAPE_LINE_CHAIN()
     {}
 
+    SHAPE *Clone() const;
+
     /**
      * Function Clear()
      * Removes all points from the line chain.
@@ -366,6 +368,11 @@ public:
         }
     }
 
+    void Insert( int aVertex, const VECTOR2I& aP )
+    {
+        m_points.insert( m_points.begin() + aVertex, aP );
+    }
+
     /**
      * Function Replace()
      *
@@ -417,6 +424,15 @@ public:
      */
     int Find( const VECTOR2I& aP ) const;
 
+    /**
+     * Function FindSegment()
+     *
+     * Searches for segment containing point aP.
+     * @param aP the point to be looked for
+     * @return index of the correspoinding segment in the line chain or negative when not found.
+     */
+    int FindSegment( const VECTOR2I& aP ) const;
+
     /**
      * Function Slice()
      *
@@ -441,6 +457,8 @@ public:
         VECTOR2I m_origin;
     };
 
+    bool Intersects( const SHAPE_LINE_CHAIN& aChain ) const;
+
     /**
      * Function Intersect()
      *
@@ -533,6 +551,8 @@ public:
         return false;
     }
 
+    bool CompareGeometry( const SHAPE_LINE_CHAIN & aOther ) const;
+
 private:
     /// array of vertices
     std::vector<VECTOR2I> m_points;

include/geometry/shape_rect.h

@@ -53,10 +53,22 @@ public:
      * Constructor
      * Creates a rectangle defined by top-left corner aP0, width aW and height aH.
      */
-     SHAPE_RECT( const VECTOR2I& aP0, int aW, int aH ) :
+    SHAPE_RECT( const VECTOR2I& aP0, int aW, int aH ) :
         SHAPE( SH_RECT ), m_p0( aP0 ), m_w( aW ), m_h( aH )
     {}
 
+    SHAPE_RECT ( const SHAPE_RECT& aOther ) : 
+        SHAPE( SH_RECT ),
+        m_p0( aOther.m_p0 ),
+        m_w( aOther.m_w ),
+        m_h( aOther.m_h )
+    {};
+
+    SHAPE* Clone() const
+    {
+        return new SHAPE_RECT( *this );
+    }
+
     /// @copydoc SHAPE::BBox()
     const BOX2I BBox( int aClearance = 0 ) const
     {
@@ -90,11 +102,11 @@ public:
         if( BBox( 0 ).Contains( aSeg.A ) || BBox( 0 ).Contains( aSeg.B ) )
             return true;
 
-         VECTOR2I vts[] = { VECTOR2I( m_p0.x, m_p0.y ),
-                            VECTOR2I( m_p0.x, m_p0.y + m_h ),
-                            VECTOR2I( m_p0.x + m_w, m_p0.y + m_h ),
-                            VECTOR2I( m_p0.x + m_w, m_p0.y ),
-                            VECTOR2I( m_p0.x, m_p0.y ) };
+        VECTOR2I vts[] = { VECTOR2I( m_p0.x, m_p0.y ),
+                           VECTOR2I( m_p0.x, m_p0.y + m_h ),
+                           VECTOR2I( m_p0.x + m_w, m_p0.y + m_h ),
+                           VECTOR2I( m_p0.x + m_w, m_p0.y ),
+                           VECTOR2I( m_p0.x, m_p0.y ) };
 
         for( int i = 0; i < 4; i++ )
         {

include/math/math_util.h

@@ -40,6 +40,11 @@ T rescale( T aNumerator, T aValue, T aDenominator )
     return aNumerator * aValue / aDenominator;
 }
 
+template <typename T>
+int sign( T val )
+{
+    return ( T( 0 ) < val) - ( val < T( 0 ) );
+}
 
 // explicit specializations for integer types, taking care of overflow.
 template <>

include/math/vector2d.h

@@ -28,6 +28,7 @@
 #ifndef VECTOR2D_H_
 #define VECTOR2D_H_
 
+#include <cmath>
 #include <climits>
 #include <iostream>
 #include <sstream>
@@ -376,7 +377,7 @@ VECTOR2<T> VECTOR2<T>::Resize( T aNewLength ) const
 
     return VECTOR2<T> (
         ( x < 0 ? -1 : 1 ) * sqrt( rescale( l_sq_new, (extended_type) x * x, l_sq_current ) ),
-        ( y < 0 ? -1 : 1 ) * sqrt( rescale( l_sq_new, (extended_type) y * y, l_sq_current ) ) );
+        ( y < 0 ? -1 : 1 ) * sqrt( rescale( l_sq_new, (extended_type) y * y, l_sq_current ) ) ) * sign( aNewLength );
 }