common: minimum version of the shape library

common/CMakeLists.txt

@@ -162,6 +162,10 @@ set(COMMON_SRCS
   tool/tool_event.cpp
   tool/tool_interactive.cpp
   tool/context_menu.cpp
+  
+  geometry/seg.cpp
+  geometry/shape_line_chain.cpp
+  geometry/shape_collisions.cpp
   )
 
 add_library(common STATIC ${COMMON_SRCS})

common/geometry/seg.cpp

+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2013 CERN
+ * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+
+#include <geometry/seg.h>
+
+template <typename T> int sgn(T val) {
+    return (T(0) < val) - (val < T(0));
+}
+
+bool SEG::PointCloserThan (const VECTOR2I& aP, int dist) const
+{
+	VECTOR2I   	  d = b - a;
+	ecoord dist_sq = (ecoord) dist * dist;
+
+ 	SEG::ecoord l_squared = d.Dot(d);
+    SEG::ecoord t = d.Dot(aP - a);
+
+
+    if( t <= 0 || !l_squared )
+    	return (aP - a).SquaredEuclideanNorm() < dist_sq;
+    else if( t >= l_squared ) 
+    	return (aP - b).SquaredEuclideanNorm() < dist_sq;
+
+
+	int dxdy =  abs(d.x) - abs(d.y);	
+
+    if( (dxdy >= -1 && dxdy <= 1) || abs(d.x) <= 1 || abs(d.y) <= 1)
+    {
+    	int ca = -sgn(d.y);
+    	int cb = sgn(d.x);
+    	int cc = -ca * a.x - cb * a.y;
+    	
+		ecoord num = ca * aP.x + cb * aP.y + cc;
+		num *= num;
+
+		if(ca && cb)
+			num >>= 1;
+
+		if(num > (dist_sq + 100))
+			return false;
+		else if(num < (dist_sq - 100))
+			return true;
+    }
+
+	VECTOR2I nearest;    
+    nearest.x = a.x + rescale(t, (ecoord)d.x, l_squared);
+    nearest.y = a.y + rescale(t, (ecoord)d.y, l_squared);
+
+    return (nearest - aP).SquaredEuclideanNorm() <= dist_sq;
+}
+
+SEG::ecoord SEG::SquaredDistance( const SEG& aSeg ) const 
+{
+		// fixme: rather inefficient.... 
+		if(Intersect(aSeg))
+			return 0;
+
+		const VECTOR2I pts[4] =
+		{
+			aSeg.NearestPoint(a) - a,
+			aSeg.NearestPoint(b) - b,
+			NearestPoint(aSeg.a) - aSeg.a,
+			NearestPoint(aSeg.b) - aSeg.b
+			};
+
+		ecoord m = VECTOR2I::ECOORD_MAX;
+		for (int i = 0; i<4 ; i++)
+			m = std::min(m, pts[i].SquaredEuclideanNorm());
+		return m;
+}
+	
+OPT_VECTOR2I SEG::Intersect( const SEG& aSeg, bool aIgnoreEndpoints, bool aLines ) const
+{
+	const VECTOR2I e (b - a);
+	const VECTOR2I f (aSeg.b - aSeg.a);
+	const VECTOR2I ac (aSeg.a - a);
+	
+	ecoord d = f.Cross(e);
+	ecoord p = f.Cross(ac);
+	ecoord q = e.Cross(ac);
+	
+	if(d == 0)
+		return OPT_VECTOR2I();
+	if (!aLines && d > 0 && (q < 0 || q > d || p < 0 || p > d))
+		return OPT_VECTOR2I();
+	if (!aLines && d < 0 && (q < d || p < d || p > 0 || q > 0))
+		return OPT_VECTOR2I();
+	if (!aLines && aIgnoreEndpoints && (q == 0 || q == d) && (p == 0 || p == d))
+		return OPT_VECTOR2I();
+		
+	
+	 VECTOR2I  ip ( aSeg.a.x + rescale(q, (ecoord)f.x, d),
+					  aSeg.a.y + rescale(q, (ecoord)f.y, d) );
+
+	 return ip;
+}
+
+
+bool SEG::ccw ( const VECTOR2I& a, const VECTOR2I& b, const VECTOR2I &c ) const
+{
+	return (ecoord)(c.y - a.y) * (b.x - a.x) > (ecoord)(b.y - a.y) * (c.x - a.x);
+}
+
+bool SEG::Collide( const SEG& aSeg, int aClearance ) const 
+{
+	// check for intersection 
+	// fixme: move to a method
+	if( ccw(a,aSeg.a,aSeg.b) != ccw(b,aSeg.a,aSeg.b) && ccw(a,b,aSeg.a) != ccw(a,b,aSeg.b) )
+		return true;
+
+#define CHK(_seg, _pt) \
+	if( (_seg).PointCloserThan (_pt, aClearance ) ) return true;
+	
+	CHK(*this, aSeg.a);
+	CHK(*this, aSeg.b);
+	CHK(aSeg, a);
+	CHK(aSeg, b);
+
+#undef CHK
+
+	return false;
+}
+		
+bool SEG::Contains(const VECTOR2I& aP) const
+{
+	return PointCloserThan(aP, 1);
+} 
+

common/geometry/shape_collisions.cpp

+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2013 CERN
+ * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+#include <math/vector2d.h>
+
+#include <geometry/shape.h>
+#include <geometry/shape_line_chain.h>
+#include <geometry/shape_circle.h>
+#include <geometry/shape_rect.h>
+
+typedef typename VECTOR2I::extended_type ecoord;
+
+static inline bool Collide( const SHAPE_CIRCLE& a, const SHAPE_CIRCLE& b, int clearance, bool needMTV, VECTOR2I& aMTV )
+{
+	ecoord min_dist = clearance + a.GetRadius() + b.GetRadius();
+	ecoord min_dist_sq = min_dist * min_dist;
+		
+	const VECTOR2I delta = b.GetCenter() - a.GetCenter();
+
+	ecoord dist_sq = delta.SquaredEuclideanNorm();
+
+	if ( dist_sq >= min_dist_sq )
+		return false;
+	
+	if ( needMTV )
+		aMTV = delta.Resize( sqrt (abs(min_dist_sq - dist_sq)) + 1);
+
+	return true;
+}
+
+static inline  bool Collide( const SHAPE_RECT& a, const SHAPE_CIRCLE& b, int clearance, bool needMTV, VECTOR2I& aMTV )
+{
+	const VECTOR2I c = b.GetCenter();
+	const VECTOR2I p0 = a.GetPosition();
+	const VECTOR2I size = a.GetSize();
+	const ecoord r = b.GetRadius();
+	const ecoord min_dist = clearance + r;
+	const ecoord min_dist_sq = min_dist * min_dist;
+
+	if (a.BBox(0).Contains(c))
+		return true;
+			
+	const VECTOR2I vts[] = { 
+						VECTOR2I(p0.x, 			p0.y),
+						VECTOR2I(p0.x,			p0.y + size.y),
+						VECTOR2I(p0.x + size.x, p0.y + size.y),
+						VECTOR2I(p0.x + size.x, p0.y),
+						VECTOR2I(p0.x, 			p0.y) };
+
+	ecoord nearest_seg_dist_sq = VECTOR2I::ECOORD_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)
+	{
+
+		for (int i = 0; i < 4; i++)
+		{
+			const SEG seg (vts[i], vts[i+1]);
+			ecoord dist_sq = seg.SquaredDistance ( c );
+		
+
+			if(dist_sq < min_dist_sq)
+			{
+				if(!needMTV)
+					return true;
+				else
+				{
+					nearest = seg.NearestPoint ( c );
+					nearest_seg_dist_sq = dist_sq;
+				}
+			}
+		}
+	}
+
+	if(nearest_seg_dist_sq >= min_dist_sq && !inside)
+		return false;
+
+	VECTOR2I delta = c - nearest;
+
+	if(!needMTV)
+		return true;
+
+	if(inside)
+		aMTV = -delta.Resize(sqrt(abs(r * r + nearest_seg_dist_sq) + 1));
+	else
+		aMTV = delta.Resize(sqrt(abs(r * r - nearest_seg_dist_sq) + 1));
+
+	return true;
+}
+
+static inline  bool Collide( const SHAPE_CIRCLE& a, const SHAPE_LINE_CHAIN& b, int clearance, bool needMTV, VECTOR2I& aMTV )
+{
+	for (int s = 0; s < b.SegmentCount(); s++)
+	{
+		if ( a.Collide (b.CSegment(s), clearance))
+			return true;
+	}
+	
+	return false;	
+}
+
+static inline bool Collide( const SHAPE_LINE_CHAIN& a, const SHAPE_LINE_CHAIN& b, int clearance, bool needMTV, VECTOR2I& aMTV )
+{
+	for( int i = 0; i < b.SegmentCount() ;i++)
+		if(a.Collide(b.CSegment(i), clearance))
+			return true;
+	return false;
+}
+
+
+static inline bool Collide( const SHAPE_RECT& a, const SHAPE_LINE_CHAIN& b, int clearance, bool needMTV, VECTOR2I& aMTV )
+{
+	for (int s = 0; s < b.SegmentCount(); s++)
+	{
+		SEG seg = b.CSegment(s);
+			if ( a.Collide (seg, clearance))
+				return true;
+	}
+
+	return false;
+}
+
+
+
+bool CollideShapes ( const SHAPE *a, const SHAPE *b, int clearance, bool needMTV, VECTOR2I& aMTV )
+{
+	switch(a->Type())
+	{
+		case SH_RECT:
+			switch(b->Type())
+			{
+				case SH_CIRCLE:
+					return Collide( *static_cast<const SHAPE_RECT *> (a), *static_cast<const SHAPE_CIRCLE *> (b), clearance, needMTV, aMTV );
+				case SH_LINE_CHAIN:
+					return Collide( *static_cast<const SHAPE_RECT *> (a), *static_cast<const SHAPE_LINE_CHAIN *> (b), clearance, needMTV, aMTV );
+				default:
+					break;
+			}
+
+		case SH_CIRCLE:
+			switch(b->Type())
+			{
+				case SH_RECT:
+					return Collide( *static_cast<const SHAPE_RECT *> (b), *static_cast<const SHAPE_CIRCLE *> (a), clearance, needMTV, aMTV );
+				case SH_CIRCLE:
+					return Collide( *static_cast<const SHAPE_CIRCLE *> (a), *static_cast<const SHAPE_CIRCLE *> (b), clearance, needMTV, aMTV );
+				case SH_LINE_CHAIN:
+					return Collide( *static_cast<const SHAPE_CIRCLE *> (a), *static_cast<const SHAPE_LINE_CHAIN *> (b), clearance, needMTV, aMTV );
+				default:
+					break;
+			}
+
+		case SH_LINE_CHAIN:
+			switch(b->Type())
+			{
+				case SH_RECT:
+					return Collide( *static_cast<const SHAPE_RECT *> (b), *static_cast<const SHAPE_LINE_CHAIN *> (a), clearance, needMTV, aMTV );
+				case SH_CIRCLE:
+					return Collide( *static_cast<const SHAPE_CIRCLE *> (b), *static_cast<const SHAPE_LINE_CHAIN *> (a), clearance, needMTV, aMTV );
+				case SH_LINE_CHAIN:
+					return Collide( *static_cast<const SHAPE_LINE_CHAIN *> (a), *static_cast<const SHAPE_LINE_CHAIN *> (b), clearance, needMTV, aMTV );
+				default:
+					break;
+			}
+		default:
+			break;
+	}
+
+	bool unsupported_collision = true;
+
+	assert(unsupported_collision == false);
+	return false;
+}
+
+
+bool SHAPE::Collide ( const SHAPE *aShape, int aClerance, VECTOR2I& aMTV ) const
+{
+	return CollideShapes( this, aShape, aClerance, true, aMTV);
+}
+
+bool SHAPE::Collide ( const SHAPE *aShape, int aClerance ) const
+{
+	VECTOR2I dummy;
+	return CollideShapes( this, aShape, aClerance, false, dummy);
+}
+		
\ No newline at end of file

include/geometry/seg.h

+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2013 CERN
+ * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+#ifndef __SEG_H
+#define __SEG_H
+
+#include <cstdio>
+#include <climits>
+
+#include <math/vector2d.h>
+
+#include <boost/optional/optional.hpp>
+
+typedef boost::optional<VECTOR2I> OPT_VECTOR2I;
+
+class SEG {
+
+	private:
+		typedef VECTOR2I::extended_type ecoord;
+
+	public:
+
+		friend inline std::ostream& operator<<( std::ostream& aStream, const SEG& aSeg );
+
+		/* Start and the of the segment. Public, to make access simpler. These are references
+		 * to an object the segment belongs to (e.g. a line chain) or references to locally stored points
+		 * (m_a, m_b).
+		 */
+		VECTOR2I& a, b;
+
+		/** Default constructor
+		 * Creates an empty (0, 0) segment, locally-referenced 
+		 */
+		SEG(): a(m_a), b(m_b)
+		{
+			a = m_a;
+			b = m_b;
+			m_is_local = true;
+			m_index = -1;
+		}
+
+		/** 
+		 * Constructor
+		 * Creates a segment between (x1, y1) and (x2, y2), locally referenced
+		 */
+		SEG ( int x1, int y1, int x2, int y2 ) : a(m_a), b(m_b)
+		{
+			m_a = VECTOR2I(x1, y1);
+			m_b = VECTOR2I(x2, y2);
+			a = m_a;
+			b = m_b;
+			m_is_local = true;
+			m_index = -1;
+		}
+
+		/** 
+		 * Constructor
+		 * Creates a segment between (aA) and (aB), locally referenced
+		 */
+		SEG ( const VECTOR2I& aA, const VECTOR2I& aB ): a(m_a), b(m_b), m_a(aA), m_b(aB)
+		{
+			a = m_a;
+			b = m_b;
+			m_is_local = true;
+			m_index = -1;
+		}
+	
+		/**
+		 * Constructor
+		 * Creates a segment between (aA) and (aB), referenced to a multi-segment shape
+		 * @param aA reference to the start point in the parent shape
+		 * @param aB reference to the end point in the parent shape
+		 * @param aIndex index of the segment within the parent shape
+		 */
+		SEG ( VECTOR2I& aA, VECTOR2I& aB, int aIndex ): a(aA), b(aB)
+		{
+			m_is_local = false;
+			m_index = aIndex;
+		}
+
+		/**
+		 * Copy constructor
+		 */
+		SEG ( const SEG& seg ): a(m_a), b(m_b)
+		{
+			if (seg.m_is_local)
+			{
+				m_a = seg.m_a;
+				m_b = seg.m_b;
+				a = m_a;
+				b = m_b;
+				m_is_local = true;
+				m_index = -1;
+			} else {
+				a = seg.a;
+				b = seg.b;
+				m_index = seg.m_index;
+				m_is_local = false;
+			}
+		}
+
+		SEG& operator=(const SEG& seg)
+		{
+			a = seg.a;
+			b = seg.b;
+			m_a = seg.m_a;
+			m_b = seg.m_b;
+			m_index = seg.m_index;
+			m_is_local = seg.m_is_local;
+			return *this;
+		}
+
+    	/**
+     	 * Function LineProject()
+     	 *
+     	 * Computes the perpendicular projection point of aP on a line passing through
+     	 * ends of the segment.
+     	 * @param aP point to project
+     	 * @return projected point
+     	 */
+    	VECTOR2I LineProject( const VECTOR2I& aP ) const;
+
+    	/**
+     	 * Function Side()
+     	 *
+     	 * Determines on which side of directed line passing via segment ends point aP lies.
+     	 * @param aP point to determine the orientation wrs to self
+     	 * @return: < 0: left, 0 : on the line, > 0 : right
+     	 */
+    	int Side( const VECTOR2I& aP ) const
+    	{
+		    const ecoord det = (b - a).Cross(aP - a); 
+    	 	return det < 0 ? -1 : (det > 0 ? 1 : 0);
+		}
+
+    	/**
+     	 * Function LineDistance()
+     	 *
+     	 * Returns the closest Euclidean distance between point aP and the line defined by 
+     	 * the ends of segment (this).
+     	 * @param aDetermineSide: when true, the sign of the returned value indicates
+     	 * the side of the line at which we are (negative = left)
+     	 * @return the distance
+     	 */
+    	int LineDistance( const VECTOR2I& aP, bool aDetermineSide = false ) const;
+
+    	/**
+     	 * Function NearestPoint()
+     	 *
+     	 * Computes a point on the segment (this) that is closest to point aP.
+     	 * @return: nearest point
+     	 */
+    	const VECTOR2I NearestPoint( const VECTOR2I &aP ) const;
+
+
+    	/**
+    	 * Function Intersect()
+    	 *
+    	 * Computes intersection point of segment (this) with segment aSeg.
+    	 * @param aSeg: segment to intersect with
+    	 * @param aIgnoreEndpoints: don't treat corner cases (i.e. end of one segment touching the other)
+    	 * as intersections.
+    	 * @param aLines: treat segments as infinite lines
+    	 * @return intersection point, if exists
+    	 */
+		OPT_VECTOR2I Intersect( const SEG& aSeg, bool aIgnoreEndpoints = false, bool aLines = false ) const;
+
+
+
+		/**
+		 * Function IntersectLines()
+		 *
+		 * Computes the intersection point of lines passing through ends of (this) and aSeg
+		 * @param aSeg segment defining the line to intersect with
+		 * @return intersection point, if exists
+		 */
+		OPT_VECTOR2I IntersectLines(  const SEG& aSeg ) const
+		{
+			return Intersect ( aSeg, false, true );
+		}
+
+		bool Collide( const SEG& aSeg, int aClearance ) const;
+
+		/**
+		 * Function Distance()
+		 * 
+		 * Computes minimum Euclidean distance to segment aSeg.
+		 * @param aSeg other segment
+		 * @return minimum distance
+		 */
+		
+		ecoord SquaredDistance( const SEG& aSeg ) const ;
+		
+		int Distance( const SEG& aSeg ) const 
+		{
+			return sqrt ( SquaredDistance(aSeg) );
+		}
+		
+
+		/**
+		 * Function Distance()
+		 * 
+		 * Computes minimum Euclidean distance to point aP.
+		 * @param aP the point
+		 * @return minimum distance
+		 */
+		 
+		ecoord SquaredDistance( const VECTOR2I& aP ) const 
+		{
+			return (NearestPoint(aP) - aP).SquaredEuclideanNorm();
+		}
+
+		int Distance( const VECTOR2I& aP ) const 
+		{
+			return sqrt ( SquaredDistance( aP) );
+		}
+
+		
+		/**
+		 * Function Collinear()
+		 * 
+		 * Checks if segment aSeg lies on the same line as (this).
+		 * @param aSeg the segment to chech colinearity with
+		 * @return true, when segments are collinear.
+		 */
+
+		bool Collinear( const SEG& aSeg ) const 
+		{
+			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);
+		}
+
+		/**
+		 * Function Length()
+		 * 
+		 * Returns the length (this)
+		 * @return length
+		 */
+		int Length() const
+		{
+			return (a - b).EuclideanNorm();
+		}
+
+		/**
+		 * Function Index()
+		 * 
+		 * Return the index of this segment in its parent shape (applicable only to non-local segments)
+		 * @return index value
+		 */
+		int Index() const
+		{
+			return m_index;
+		}
+
+
+		bool Contains(const VECTOR2I& aP) const;
+
+		bool PointCloserThan ( const VECTOR2I& aP, int dist) const;
+
+	//	friend std::ostream& operator<<( std::ostream& stream, const SEG& aSeg );
+	private:
+
+		bool ccw ( const VECTOR2I& a, const VECTOR2I& b, const VECTOR2I &c ) const;
+		
+		///> locally stored start/end coordinates (used when m_is_local == true)
+		VECTOR2I m_a, m_b;
+		///> index withing the parent shape (used when m_is_local == false)
+		int m_index;
+		///> locality flag
+		bool m_is_local;
+};
+
+inline VECTOR2I SEG::LineProject( const VECTOR2I& aP ) const
+{
+	// fixme: numerical errors for large integers
+	assert(false);
+    /*const VECTOR2I   		  d 	  = aB - aA;
+    ecoord   det     = d.Dot(d);
+    ecoord   dxdy    = (ecoord) d.x * d.y;
+    
+    ecoord   qx      =
+        ( (extended_type) aA.x * d.y * d.y + (extended_type) d.x * d.x * x - dxdy *
+          (aA.y - y) ) / det;
+    extended_type   qy =
+        ( (extended_type) aA.y * d.x * d.x + (extended_type) d.y * d.y * y - dxdy *
+          (aA.x - x) ) / det;
+
+    return VECTOR2<T> ( (T) qx, (T) qy );*/
+}
+
+
+
+inline int SEG::LineDistance( const VECTOR2I& aP, bool aDetermineSide ) const
+{
+    ecoord   p   = a.y - b.y;
+    ecoord   q   = b.x - a.x;
+	ecoord   r   = -p * a.x - q * a.y;
+
+    ecoord   dist = ( p * aP.x + q * aP.y + r ) / sqrt( p * p + q * q );
+    return aDetermineSide ? dist : abs(dist);
+}
+
+
+
+inline const VECTOR2I SEG::NearestPoint(const VECTOR2I& aP) const
+{
+    VECTOR2I   		  d = b - a;
+    ecoord   l_squared = d.Dot(d);
+
+    if( l_squared == 0 )
+        return a;
+    
+    ecoord t = d.Dot(aP - a);
+        
+    if( t < 0 )
+        return a;
+    else if( t > l_squared )
+        return b;
+    
+    int xp = rescale(t, (ecoord)d.x, l_squared);
+    int yp = rescale(t, (ecoord)d.y, l_squared);
+
+    return a + VECTOR2I(xp, yp);
+}
+
+inline std::ostream& operator<<( std::ostream& aStream, const SEG& aSeg )
+{
+    if(aSeg.m_is_local)
+    	aStream << "[ local " << aSeg.a << " - " << aSeg.b << " ]";
+    return aStream;
+}
+
+#endif // __SEG_H
+

include/geometry/shape.h

+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2013 CERN
+ * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+#ifndef __SHAPE_H
+#define __SHAPE_H
+
+#include <math/vector2d.h>
+#include <math/box2.h>
+
+#include <geometry/seg.h>
+
+/**
+ * Enum ShapeType
+ * Lists all supported shapes
+ */
+
+enum ShapeType {
+	SH_RECT = 0, 	///> axis-aligned rectangle
+	SH_SEGMENT,		///> line segment
+	SH_LINE_CHAIN,	///> line chain (polyline)
+	SH_CIRCLE		///> circle
+};
+
+/**
+ * Class SHAPE
+ * 
+ * Represents an abstract shape on 2D plane. All SHAPEs implement SHAPE interface.
+ */	
+class SHAPE {
+	protected:
+		typedef typename VECTOR2I::extended_type ecoord;
+
+	public:
+		/**
+		 * Constructor
+		 *
+		 * Creates an empty shape of type aType
+		 */
+
+		SHAPE ( ShapeType aType ): m_type( aType ) { };
+		
+		// Destructor
+		virtual ~SHAPE() {};
+
+		/**
+		 * Function Type()
+		 *
+		 * Returns the type of the shape.
+		 * @retval the type
+		 */
+		ShapeType Type() const { return m_type; }
+
+		/**
+		 * Function Clone()
+		 *
+		 * Returns a dynamically allocated copy of the shape
+		 * @retval copy of the shape
+		 */
+		virtual SHAPE* Clone() const { assert(false); };
+
+		/**
+		 * Function Collide()
+		 *
+		 * Checks if the boundary of shape (this) lies closer to the point aP than aClearance, indicating
+		 * a collision.
+		 * @return true, if there is a collision.
+		 */
+		virtual bool Collide ( const VECTOR2I& aP, int aClearance = 0 ) const
+		{
+			return Collide(SEG(aP, aP), aClearance);
+		}
+		
+		/**
+		 * Function Collide()
+		 *
+		 * Checks if the boundary of shape (this) lies closer to the shape aShape than aClearance, indicating
+		 * a collision.
+		 * @return true, if there is a collision.
+		 */
+		virtual bool Collide ( const SHAPE *aShape, int aClerance, VECTOR2I& aMTV ) const;
+		virtual bool Collide ( const SHAPE *aShape, int aClerance = 0 ) const;
+		/**
+		 * Function Collide()
+		 *
+		 * Checks if the boundary of shape (this) lies closer to the segment aSeg than aClearance, indicating
+		 * a collision.
+		 * @return true, if there is a collision.
+		 */
+		virtual bool Collide ( const SEG& aSeg, int aClearance = 0) const = 0;
+	
+		/**
+		 * Function Collide()
+		 *
+		 * Computes a bounding box of the shape, with a margin of aClearance
+		 * a collision.
+		 * @aClearance how much the bounding box is expanded wrs to the minimum enclosing rectangle for the shape.
+		 * @return the bounding box.
+		 */
+		virtual const BOX2I BBox ( int aClearance = 0 ) const = 0;
+
+		/**
+		 * Function Centre()
+		 *
+		 * Computes a center-of-mass of the shape
+		 * @return the center-of-mass point
+		 */
+		virtual VECTOR2I Centre() const
+		{
+			return BBox(0).Centre(); // if nothing better is available....
+		}
+
+	private:
+		///> type of our shape
+		ShapeType m_type;
+
+};
+
+bool CollideShapes ( const SHAPE *a, const SHAPE *b, int clearance, bool needMTV, VECTOR2I& aMTV );
+
+#endif // __SHAPE_H

include/geometry/shape_circle.h

+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2013 CERN
+ * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+#ifndef __SHAPE_CIRCLE_H
+#define __SHAPE_CIRCLE_H
+
+#include "shape.h"
+
+class SHAPE_CIRCLE : public SHAPE {
+
+public:
+	SHAPE_CIRCLE(): 
+		SHAPE( SH_CIRCLE ), m_radius (0) {};
+			
+	SHAPE_CIRCLE( const VECTOR2I& aCenter, int aRadius ): 
+		SHAPE( SH_CIRCLE ), m_radius (aRadius), m_center(aCenter) {};
+
+	~SHAPE_CIRCLE() {};
+
+	const BOX2I BBox(int aClearance = 0) const
+	{
+		const VECTOR2I rc (m_radius + aClearance, m_radius + aClearance);
+		return BOX2I (m_center - rc, rc * 2);
+	}
+
+
+	bool Collide(const SEG& aSeg, int aClearance = 0) const
+	{
+		int rc = aClearance + m_radius;
+		return aSeg.Distance(m_center) <= rc;
+	}
+
+	void SetRadius(int aRadius)
+	{
+		m_radius = aRadius;
+	}
+
+	void SetCenter (const VECTOR2I& aCenter)
+	{
+		m_center = aCenter;
+	}
+
+	int GetRadius() const 
+	{
+		return m_radius;
+	}
+
+	const VECTOR2I GetCenter() const
+	{
+		return m_center;
+	}
+private:
+	int m_radius;
+	VECTOR2I m_center;
+};
+
+#endif

include/geometry/shape_index.h

+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2013 CERN
+ * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+#ifndef __SHAPE_INDEX_H
+#define __SHAPE_INDEX_H
+
+#include <boost/unordered_map.hpp>
+
+template <class T> const SHAPE *defaultShapeFunctor( const T aItem )
+{
+	return aItem->GetShape();
+}
+
+template <class T, const SHAPE *(ShapeFunctor)(const T) = defaultShapeFunctor<T> >
+
+class SHAPE_INDEX_LIST {
+	
+	struct ShapeEntry {
+		ShapeEntry(T aParent)
+		{
+			shape = ShapeFunctor(aParent);
+			bbox = shape->BBox(0);
+			parent = aParent;
+		}
+
+		~ShapeEntry()
+		{
+			
+		}
+
+		T parent;
+		const SHAPE *shape;
+		BOX2I bbox;
+	};
+
+	typedef std::vector<ShapeEntry> ShapeVec;
+	typedef typename std::vector<ShapeEntry>::iterator ShapeVecIter;	
+	
+public:
+
+// "Normal" iterator interface, for STL algorithms. 
+	class iterator {
+
+		public:
+			iterator() {};
+
+			iterator( ShapeVecIter aCurrent)
+				: m_current(aCurrent) {};
+
+			iterator(const iterator &b) : 
+				m_current(b.m_current) {};
+
+			T operator*() const
+			{
+				return (*m_current).parent;
+			}
+
+			void operator++()
+			{
+				++m_current;
+			}
+
+			iterator& operator++(int dummy)
+			{
+				++m_current;
+				return *this;
+			}
+
+			bool operator ==( const iterator& rhs ) const
+			{
+				return m_current == rhs.m_current;
+			}
+
+			bool operator !=( const iterator& rhs ) const
+			{
+				return m_current != rhs.m_current;
+			}
+
+			const iterator& operator=(const iterator& rhs) 
+			{
+				m_current = rhs.m_current;
+				return *this;
+			}
+
+		private:
+			ShapeVecIter m_current;
+	};
+
+// "Query" iterator, for iterating over a set of spatially matching shapes.
+	class query_iterator {
+		public:
+
+			query_iterator()
+			{
+
+			}
+
+			query_iterator(  ShapeVecIter aCurrent, ShapeVecIter aEnd, SHAPE *aShape, int aMinDistance, bool aExact)
+				: m_end(aEnd),
+				  m_current(aCurrent),
+				  m_shape(aShape),
+				  m_minDistance(aMinDistance),
+				  m_exact(aExact)
+			{
+				if(aShape)
+				{
+					m_refBBox = aShape->BBox();
+					next();
+				}
+			}
+
+			query_iterator(const query_iterator &b)
+				: m_end(b.m_end),
+				  m_current(b.m_current),
+				  m_shape(b.m_shape),
+				  m_minDistance(b.m_minDistance),
+				  m_exact(b.m_exact),
+				  m_refBBox(b.m_refBBox)
+			{
+
+			}
+
+			
+			T operator*() const
+			{
+				return (*m_current).parent;
+			}
+
+			query_iterator& operator++()
+			{
+				++m_current;
+				next();
+			 	return *this;
+			}
+
+			query_iterator& operator++(int dummy)
+			{
+				++m_current;
+				next();
+				return *this;
+			}
+
+			bool operator ==( const query_iterator& rhs ) const
+			{
+				return m_current == rhs.m_current;
+			}
+
+			bool operator !=( const query_iterator& rhs ) const
+			{
+				return m_current != rhs.m_current;
+			}
+
+			const query_iterator& operator=(const query_iterator& rhs) 
+			{
+				m_end = rhs.m_end;
+				m_current = rhs.m_current;
+				m_shape = rhs.m_shape;
+				m_minDistance = rhs.m_minDistance;
+				m_exact = rhs.m_exact;
+				m_refBBox = rhs.m_refBBox;
+				return *this;
+			}
+
+		private:
+
+			void next()
+			{
+				while(m_current != m_end)
+				{
+					if (m_refBBox.Distance(m_current->bbox) <= m_minDistance)
+					{
+						if(!m_exact || m_current->shape->Collide(m_shape, m_minDistance))
+							return;
+					}
+					++m_current;
+				}
+			}
+
+			ShapeVecIter m_end;
+			ShapeVecIter m_current;
+			BOX2I m_refBBox;
+			bool m_exact;
+			SHAPE *m_shape;
+			int m_minDistance;
+	};
+
+	void Add(T aItem)
+	{
+		ShapeEntry s (aItem);
+
+		m_shapes.push_back(s);
+	}
+	
+	void Remove(const T aItem)
+	{
+		ShapeVecIter i;
+		
+		for(i=m_shapes.begin(); i!=m_shapes.end();++i)
+		{
+			if(i->parent == aItem)
+				break;
+		}
+
+		if(i == m_shapes.end())
+			return;
+
+		m_shapes.erase(i);
+	}
+
+	int Size() const
+	{
+		return m_shapes.size();
+	}
+
+	template<class Visitor>
+		int Query( const SHAPE *aShape, int aMinDistance, Visitor &v, bool aExact = true) //const
+		{
+			ShapeVecIter i;
+			int n = 0;
+			VECTOR2I::extended_type minDistSq = (VECTOR2I::extended_type) aMinDistance * aMinDistance;
+
+			BOX2I refBBox = aShape->BBox();
+
+			for(i = m_shapes.begin(); i!=m_shapes.end(); ++i)
+			{
+				if (refBBox.SquaredDistance(i->bbox) <= minDistSq)
+				{
+					if(!aExact || i->shape->Collide(aShape, aMinDistance))
+					{
+						n++;
+						if(!v( i->parent ))
+							return n;
+					}
+				}
+			}
+			return n;
+		}
+
+	void Clear()
+	{
+		m_shapes.clear();
+	}
+	
+	query_iterator qbegin( SHAPE *aShape, int aMinDistance, bool aExact ) 
+	{
+		return query_iterator( m_shapes.begin(), m_shapes.end(), aShape, aMinDistance, aExact);
+	}
+
+	const query_iterator qend() 
+	{
+			return query_iterator( m_shapes.end(), m_shapes.end(), NULL, 0, false );
+	}
+
+	iterator begin()
+	{
+		return iterator( m_shapes.begin() );
+	}
+
+	iterator end()
+	{
+		return iterator( m_shapes.end() );
+	}
+
+private:
+
+	ShapeVec m_shapes;
+};
+
+#endif

include/geometry/shape_rect.h

+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2013 CERN
+ * @author Tomasz Wlostowski <tomasz.wlostowski@cern.ch>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+#ifndef __SHAPE_RECT_H
+#define __SHAPE_RECT_H
+
+#include <geometry/shape.h>
+#include <geometry/shape_line_chain.h>
+#include <geometry/shape_circle.h>
+#include <geometry/seg.h>
+
+class SHAPE_RECT : public SHAPE {
+	public:
+		/**
+		 * Constructor
+		 * Creates an empty (0-sized) rectangle
+		 */
+		SHAPE_RECT(): 
+			SHAPE( SH_RECT ), m_w (0), m_h(0) {};
+			
+		/**
+		 * Constructor
+		 * Creates a rectangle defined by top-left corner (x0, y0), width w and height h.
+		 */
+		SHAPE_RECT( int x0, int y0, int w, int h ):
+			SHAPE(SH_RECT), m_p0(x0, y0), m_w(w), m_h(h) {};
+
+		/**
+		 * Constructor
+		 * Creates a rectangle defined by top-left corner p0, width w and height h.
+		 */
+		 SHAPE_RECT( const VECTOR2I &p0, int w, int h ):
+			SHAPE(SH_RECT), m_p0(p0), m_w(w), m_h(h) {};
+
+		/// @copydoc SHAPE::BBox()
+		const BOX2I BBox(int aClearance = 0) const
+		{
+			BOX2I bbox( VECTOR2I (m_p0.x - aClearance, m_p0.y - aClearance ),
+						  VECTOR2I (m_w + 2 * aClearance, m_h + 2 * aClearance ));
+			//printf("bb : %s\n",bbox.Format().c_str());
+			return bbox;
+		}
+
+		/**
+		 * Function Diagonal()
+		 * 
+		 * Returns length of the diagonal of the rectangle
+		 * @return diagonal length
+		 */
+		int Diagonal() const
+		{
+			return VECTOR2I(m_w, m_h).EuclideanNorm();
+		}
+
+		/// @copydoc SHAPE::Collide()
+		bool Collide(const SEG& aSeg, int aClearance = 0) const
+		{
+			//VECTOR2I pmin = VECTOR2I(std::min(aSeg.a.x, aSeg.b.x), std::min(aSeg.a.y, aSeg.b.y));
+			//VECTOR2I pmax = VECTOR2I(std::max(aSeg.a.x, aSeg.b.x), std::max(aSeg.a.y, aSeg.b.y));
+			//BOX2I r(pmin, VECTOR2I(pmax.x - pmin.x, pmax.y - pmin.y));
+
+			//if (BBox(0).SquaredDistance(r) > aClearance * aClearance)
+			//	return false;
+	
+			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) };
+
+			for (int i = 0; i < 4; i++)
+			{
+				SEG s(vts[i], vts[i+1], i);
+				if(s.Distance(aSeg) <= aClearance)
+					return true;
+			}
+			
+			return false;
+		};
+
+		/**
+		 * Function GetPosition()
+		 * 
+		 * @return top-left corner of the rectangle
+		 */
+		const VECTOR2I& GetPosition() const { return m_p0; }
+		
+		/**
+		 * Function GetSize()
+		 * 
+		 * @return size of the rectangle
+		 */
+		const VECTOR2I GetSize() const { return VECTOR2I(m_w, m_h); }
+
+		/**
+		 * Function GetWidth()
+		 * 
+		 * @return width of the rectangle
+		 */
+ 		const int GetWidth() const { return m_w; }
+
+		/**
+		 * Function GetHeight()
+		 * 
+		 * @return height of the rectangle
+		 */
+		const int GetHeight() const { return m_h; }
+
+	private:
+		///> Top-left corner
+		VECTOR2I m_p0;
+		///> Width
+		int m_w;
+		///> Height
+		int m_h;
+	};
+
+#endif // __SHAPE_RECT_H