diff options
Diffstat (limited to 'include/math')
| -rw-r--r-- | include/math/box2.h | 476 | ||||
| -rw-r--r-- | include/math/math_util.h | 56 | ||||
| -rw-r--r-- | include/math/matrix3x3.h | 404 | ||||
| -rw-r--r-- | include/math/vector2d.h | 579 |
4 files changed, 1515 insertions, 0 deletions
diff --git a/include/math/box2.h b/include/math/box2.h new file mode 100644 index 0000000..5bf6ddd --- /dev/null +++ b/include/math/box2.h @@ -0,0 +1,476 @@ +/* + * This program source code file is part of KiCad, a free EDA CAD application. + * + * Copyright (C) 2012 SoftPLC Corporation, Dick Hollenbeck <dick@softplc.com> + * Copyright (C) 2012 Kicad Developers, see change_log.txt for contributors. + * 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 __BOX2_H +#define __BOX2_H + +#include <math/vector2d.h> +#include <limits> + +#include <boost/optional.hpp> + +/** + * Class BOX2 + * handles a 2-D bounding box, built on top of an origin point + * and size vector, both of templated class Vec + */ +template <class Vec> +class BOX2 +{ +private: + Vec m_Pos; // Rectangle Origin + Vec m_Size; // Rectangle Size + +public: + typedef typename Vec::coord_type coord_type; + typedef typename Vec::extended_type ecoord_type; + typedef typename std::numeric_limits<coord_type> coord_limits; + + BOX2() {}; + + BOX2( const Vec& aPos, const Vec& aSize ) : + m_Pos( aPos ), + m_Size( aSize ) + { + Normalize(); + } + + void SetMaximum() + { + m_Pos.x = m_Pos.y = coord_limits::min() / 2 + coord_limits::epsilon(); + m_Size.x = m_Size.y = coord_limits::max() - coord_limits::epsilon(); + } + + Vec Centre() const + { + return Vec( m_Pos.x + ( m_Size.x / 2 ), + m_Pos.y + ( m_Size.y / 2 ) ); + } + + /** + * @brief Compute the bounding box from a given list of points. + * + * @param aPointList is the list points of the object. + */ + template <class Container> + void Compute( const Container& aPointList ) + { + Vec vmin, vmax; + + typename Container::const_iterator i; + + if( !aPointList.size() ) + return; + + vmin = vmax = aPointList[0]; + + for( i = aPointList.begin(); i != aPointList.end(); ++i ) + { + Vec p( *i ); + vmin.x = std::min( vmin.x, p.x ); + vmin.y = std::min( vmin.y, p.y ); + vmax.x = std::max( vmax.x, p.x ); + vmax.y = std::max( vmax.y, p.y ); + } + + SetOrigin( vmin ); + SetSize( vmax - vmin ); + } + + /** + * Function Move + * moves the rectangle by the \a aMoveVector. + * @param aMoveVector A point that is the value to move this rectangle + */ + void Move( const Vec& aMoveVector ) + { + m_Pos += aMoveVector; + } + + /** + * Function Normalize + * ensures that the height ant width are positive. + */ + BOX2<Vec>& Normalize() + { + if( m_Size.y < 0 ) + { + m_Size.y = -m_Size.y; + m_Pos.y -= m_Size.y; + } + + if( m_Size.x < 0 ) + { + m_Size.x = -m_Size.x; + m_Pos.x -= m_Size.x; + } + + return *this; + } + + /** + * Function Contains + * @param aPoint = the point to test + * @return true if aPoint is inside the boundary box. A point on a edge is seen as inside + */ + bool Contains( const Vec& aPoint ) const + { + Vec rel_pos = aPoint - m_Pos; + Vec size = m_Size; + + if( size.x < 0 ) + { + size.x = -size.x; + rel_pos.x += size.x; + } + + if( size.y < 0 ) + { + size.y = -size.y; + rel_pos.y += size.y; + } + + return ( rel_pos.x >= 0 ) && ( rel_pos.y >= 0 ) && ( rel_pos.y <= size.y) && ( rel_pos.x <= size.x); + } + + /** + * Function Contains + * @param x = the x coordinate of the point to test + * @param y = the x coordinate of the point to test + * @return true if point is inside the boundary box. A point on a edge is seen as inside + */ + bool Contains( coord_type x, coord_type y ) const { return Contains( Vec( x, y ) ); } + + /** + * Function Contains + * @param aRect = the BOX2 to test + * @return true if aRect is Contained. A common edge is seen as contained + */ + bool Contains( const BOX2<Vec>& aRect ) const + { + return Contains( aRect.GetOrigin() ) && Contains( aRect.GetEnd() ); + } + + const Vec& GetSize() const { return m_Size; } + coord_type GetX() const { return m_Pos.x; } + coord_type GetY() const { return m_Pos.y; } + + const Vec& GetOrigin() const { return m_Pos; } + const Vec& GetPosition() const { return m_Pos; } + const Vec GetEnd() const { return Vec( GetRight(), GetBottom() ); } + + coord_type GetWidth() const { return m_Size.x; } + coord_type GetHeight() const { return m_Size.y; } + coord_type GetRight() const { return m_Pos.x + m_Size.x; } + coord_type GetBottom() const { return m_Pos.y + m_Size.y; } + + // Compatibility aliases + coord_type GetLeft() const { return GetX(); } + coord_type GetTop() const { return GetY(); } + void MoveTopTo( coord_type aTop ) { m_Pos.y = aTop; } + void MoveBottomTo( coord_type aBottom ) { m_Size.y = aBottom - m_Pos.y; } + void MoveLeftTo( coord_type aLeft ) { m_Pos.x = aLeft; } + void MoveRightTo( coord_type aRight ) { m_Size.x = aRight - m_Pos.x; } + + void SetOrigin( const Vec& pos ) { m_Pos = pos; } + void SetOrigin( coord_type x, coord_type y ) { m_Pos.x = x; m_Pos.y = y; } + void SetSize( const Vec& size ) { m_Size = size; } + void SetSize( coord_type w, coord_type h ) { m_Size.x = w; m_Size.y = h; } + void Offset( coord_type dx, coord_type dy ) { m_Pos.x += dx; m_Pos.y += dy; } + void Offset( const Vec& offset ) + { + m_Pos.x += offset.x; m_Pos.y += + offset.y; + } + + void SetX( coord_type val ) { m_Pos.x = val; } + void SetY( coord_type val ) { m_Pos.y = val; } + void SetWidth( coord_type val ) { m_Size.x = val; } + void SetHeight( coord_type val ) { m_Size.y = val; } + void SetEnd( coord_type x, coord_type y ) { SetEnd( Vec( x, y ) ); } + void SetEnd( const Vec& pos ) + { + m_Size.x = pos.x - m_Pos.x; m_Size.y = pos.y - m_Pos.y; + } + + /** + * Function Intersects + * @return bool - true if the argument rectangle intersects this rectangle. + * (i.e. if the 2 rectangles have at least a common point) + */ + bool Intersects( const BOX2<Vec>& aRect ) const + { + // this logic taken from wxWidgets' geometry.cpp file: + bool rc; + + BOX2<Vec> me( *this ); + BOX2<Vec> rect( aRect ); + me.Normalize(); // ensure size is >= 0 + rect.Normalize(); // ensure size is >= 0 + + // calculate the left common area coordinate: + int left = std::max( me.m_Pos.x, rect.m_Pos.x ); + // calculate the right common area coordinate: + int right = std::min( me.m_Pos.x + me.m_Size.x, rect.m_Pos.x + rect.m_Size.x ); + // calculate the upper common area coordinate: + int top = std::max( me.m_Pos.y, aRect.m_Pos.y ); + // calculate the lower common area coordinate: + int bottom = std::min( me.m_Pos.y + me.m_Size.y, rect.m_Pos.y + rect.m_Size.y ); + + // if a common area exists, it must have a positive (null accepted) size + if( left <= right && top <= bottom ) + rc = true; + else + rc = false; + + return rc; + } + + const std::string Format() const + { + std::stringstream ss; + + ss << "( box corner " << m_Pos.Format() << " w " << m_Size.x << " h " << m_Size.y << " )"; + + return ss.str(); + } + + /** + * Function Inflate + * inflates the rectangle horizontally by \a dx and vertically by \a dy. If \a dx + * and/or \a dy is negative the rectangle is deflated. + */ + BOX2<Vec>& Inflate( coord_type dx, coord_type dy ) + { + if( m_Size.x >= 0 ) + { + if( m_Size.x < -2 * dx ) + { + // Don't allow deflate to eat more width than we have, + m_Pos.x += m_Size.x / 2; + m_Size.x = 0; + } + else + { + // The inflate is valid. + m_Pos.x -= dx; + m_Size.x += 2 * dx; + } + } + else // size.x < 0: + { + if( m_Size.x > -2 * dx ) + { + // Don't allow deflate to eat more width than we have, + m_Pos.x -= m_Size.x / 2; + m_Size.x = 0; + } + else + { + // The inflate is valid. + m_Pos.x += dx; + m_Size.x -= 2 * dx; // m_Size.x <0: inflate when dx > 0 + } + } + + if( m_Size.y >= 0 ) + { + if( m_Size.y < -2 * dy ) + { + // Don't allow deflate to eat more height than we have, + m_Pos.y += m_Size.y / 2; + m_Size.y = 0; + } + else + { + // The inflate is valid. + m_Pos.y -= dy; + m_Size.y += 2 * dy; + } + } + else // size.y < 0: + { + if( m_Size.y > 2 * dy ) + { + // Don't allow deflate to eat more height than we have, + m_Pos.y -= m_Size.y / 2; + m_Size.y = 0; + } + else + { + // The inflate is valid. + m_Pos.y += dy; + m_Size.y -= 2 * dy; // m_Size.y <0: inflate when dy > 0 + } + } + + return *this; + } + + /** + * Function Inflate + * inflates the rectangle horizontally and vertically by \a aDelta. If \a aDelta + * is negative the rectangle is deflated. + */ + BOX2<Vec>& Inflate( int aDelta ) + { + Inflate( aDelta, aDelta ); + return *this; + } + + /** + * Function Merge + * modifies the position and size of the rectangle in order to contain \a aRect. It is + * mainly used to calculate bounding boxes. + * @param aRect The rectangle to merge with this rectangle. + */ + BOX2<Vec>& Merge( const BOX2<Vec>& aRect ) + { + Normalize(); // ensure width and height >= 0 + BOX2<Vec> rect = aRect; + rect.Normalize(); // ensure width and height >= 0 + Vec end = GetEnd(); + Vec rect_end = rect.GetEnd(); + + // Change origin and size in order to contain the given rect + m_Pos.x = std::min( m_Pos.x, rect.m_Pos.x ); + m_Pos.y = std::min( m_Pos.y, rect.m_Pos.y ); + end.x = std::max( end.x, rect_end.x ); + end.y = std::max( end.y, rect_end.y ); + SetEnd( end ); + return *this; + } + + /** + * Function Merge + * modifies the position and size of the rectangle in order to contain the given point. + * @param aPoint The point to merge with the rectangle. + */ + BOX2<Vec>& Merge( const Vec& aPoint ) + { + Normalize(); // ensure width and height >= 0 + + Vec end = GetEnd(); + // Change origin and size in order to contain the given rect + m_Pos.x = std::min( m_Pos.x, aPoint.x ); + m_Pos.y = std::min( m_Pos.y, aPoint.y ); + end.x = std::max( end.x, aPoint.x ); + end.y = std::max( end.y, aPoint.y ); + SetEnd( end ); + return *this; + } + + /** + * Function GetArea + * returns the area of the rectangle. + * @return The area of the rectangle. + */ + ecoord_type GetArea() const + { + return (ecoord_type) GetWidth() * (ecoord_type) GetHeight(); + } + + /** + * Function GetArea + * returns the length of the diagonal of the rectangle. + * @return The area of the diagonal. + */ + ecoord_type Diagonal() const + { + return m_Size.EuclideanNorm(); + } + + ecoord_type SquaredDistance( const Vec& aP ) const + { + ecoord_type x2 = m_Pos.x + m_Size.x; + ecoord_type y2 = m_Pos.y + m_Size.y; + ecoord_type xdiff = std::max( aP.x < m_Pos.x ? m_Pos.x - aP.x : m_Pos.x - x2, (ecoord_type) 0 ); + ecoord_type ydiff = std::max( aP.y < m_Pos.y ? m_Pos.y - aP.y : m_Pos.y - y2, (ecoord_type) 0 ); + return xdiff * xdiff + ydiff * ydiff; + } + + ecoord_type Distance( const Vec& aP ) const + { + return sqrt( SquaredDistance( aP ) ); + } + + /** + * Function SquaredDistance + * returns the square of the minimum distance between self and box aBox + * @param aBox: the other box + * @return The distance, squared + */ + ecoord_type SquaredDistance( const BOX2<Vec>& aBox ) const + { + ecoord_type s = 0; + + if( aBox.m_Pos.x + aBox.m_Size.x < m_Pos.x ) + { + ecoord_type d = aBox.m_Pos.x + aBox.m_Size.x - m_Pos.x; + s += d * d; + } + else if( aBox.m_Pos.x > m_Pos.x + m_Size.x ) + { + ecoord_type d = aBox.m_Pos.x - m_Size.x - m_Pos.x; + s += d * d; + } + + if( aBox.m_Pos.y + aBox.m_Size.y < m_Pos.y ) + { + ecoord_type d = aBox.m_Pos.y + aBox.m_Size.y - m_Pos.y; + s += d * d; + } + else if( aBox.m_Pos.y > m_Pos.y + m_Size.y ) + { + ecoord_type d = aBox.m_Pos.y - m_Size.y - m_Pos.y; + s += d * d; + } + + return s; + } + + /** + * Function Distance + * returns the minimum distance between self and box aBox + * @param aBox: the other box + * @return The distance + */ + ecoord_type Distance( const BOX2<Vec>& aBox ) const + { + return sqrt( SquaredDistance( aBox ) ); + } +}; + +/* Default specializations */ +typedef BOX2<VECTOR2I> BOX2I; +typedef BOX2<VECTOR2D> BOX2D; + +typedef boost::optional<BOX2I> OPT_BOX2I; + +// FIXME should be removed to avoid multiple typedefs for the same type +typedef BOX2D DBOX; + +#endif diff --git a/include/math/math_util.h b/include/math/math_util.h new file mode 100644 index 0000000..cb3ce50 --- /dev/null +++ b/include/math/math_util.h @@ -0,0 +1,56 @@ +/* + * This program source code file is part of KICAD, a free EDA CAD application. + * + * Copyright (c) 2005 Michael Niedermayer <michaelni@gmx.at> + * Copyright (C) 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 __MATH_UTIL_H +#define __MATH_UTIL_H + +#include <stdint.h> + +/** + * Function rescale() + * + * Scales a number (value) by rational (numerator/denominator). Numerator must be <= denominator. + */ + +template <typename T> +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 <> +int rescale( int aNumerator, int aValue, int aDenominator ); + +template <> +int64_t rescale( int64_t aNumerator, int64_t aValue, int64_t aDenominator ); + +#endif // __MATH_UTIL_H diff --git a/include/math/matrix3x3.h b/include/math/matrix3x3.h new file mode 100644 index 0000000..7719cb3 --- /dev/null +++ b/include/math/matrix3x3.h @@ -0,0 +1,404 @@ +/* + * This program source code file is part of KICAD, a free EDA CAD application. + * + * Copyright (C) 2012 Torsten Hueter, torstenhtr <at> gmx.de + * Copyright (C) 2012 Kicad Developers, see change_log.txt for contributors. + * + * Matrix class (3x3) + * + * 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 MATRIX3X3_H_ +#define MATRIX3X3_H_ + +#include <math/vector2d.h> + +/** + * Class MATRIX3x3 describes a general 3x3 matrix. + * + * Any linear transformation in 2D can be represented + * by a homogeneous 3x3 transformation matrix. Given a vector x, the linear transformation + * with the transformation matrix M is given as + * + * x' = M * x + * + * To represent an affine transformation, homogeneous coordinates have to be used. That means + * the 2D-vector (x, y) has to be extended to a 3D-vector by a third component (x, y, 1). + * + * Transformations can be easily combined by matrix multiplication. + * + * A * (B * x ) = (A * B) * x + * ( A, B: transformation matrices, x: vector ) + * + * This class was implemented using templates, so flexible type combinations are possible. + * + */ + +// Forward declaration for template friends +template <class T> +class MATRIX3x3; + +template <class T> +std::ostream& operator<<( std::ostream& aStream, const MATRIX3x3<T>& aMatrix ); + +template <class T> +class MATRIX3x3 +{ +public: + T m_data[3][3]; + + /** + * @brief Constructor + * + * Initialize all matrix members to zero. + */ + MATRIX3x3(); + + /** + * @brief Constructor + * + * Initialize with given matrix members + * + * @param a00 is the component [0,0]. + * @param a01 is the component [0,1]. + * @param a02 is the component [0,2]. + * @param a10 is the component [1,0]. + * @param a11 is the component [1,1]. + * @param a12 is the component [1,2]. + * @param a20 is the component [2,0]. + * @param a21 is the component [2,1]. + * @param a22 is the component [2,2]. + */ + MATRIX3x3( T a00, T a01, T a02, T a10, T a11, T a12, T a20, T a21, T a22 ); + + /** + * @brief Set the matrix to the identity matrix. + * + * The diagonal components of the matrix are set to 1. + */ + void SetIdentity(); + + /** + * @brief Set the translation components of the matrix. + * + * @param aTranslation is the translation, specified as 2D-vector. + */ + void SetTranslation( VECTOR2<T> aTranslation ); + + /** + * @brief Get the translation components of the matrix. + * + * @return is the translation (2D-vector). + */ + VECTOR2<T> GetTranslation() const; + + /** + * @brief Set the rotation components of the matrix. + * + * The angle needs to have a positive value for an anti-clockwise rotation. + * + * @param aAngle is the rotation angle in [rad]. + */ + void SetRotation( T aAngle ); + + /** + * @brief Set the scale components of the matrix. + * + * @param aScale contains the scale factors, specified as 2D-vector. + */ + void SetScale( VECTOR2<T> aScale ); + + /** + * @brief Get the scale components of the matrix. + * + * @return the scale factors, specified as 2D-vector. + */ + VECTOR2<T> GetScale() const; + + /** + * @brief Compute the determinant of the matrix. + * + * @return the determinant value. + */ + T Determinant() const; + + /** + * @brief Determine the inverse of the matrix. + * + * The inverse of a transformation matrix can be used to revert a transformation. + * + * x = Minv * ( M * x ) + * ( M: transformation matrix, Minv: inverse transformation matrix, x: vector) + * + * @return the inverse matrix. + */ + MATRIX3x3 Inverse() const; + + /** + * @brief Get the transpose of the matrix. + * + * @return the transpose matrix. + */ + MATRIX3x3 Transpose() const; + + /** + * @brief Output to a stream. + */ + friend std::ostream& operator<<<T>( std::ostream& aStream, const MATRIX3x3<T>& aMatrix ); + +}; + +// Operators + +//! @brief Matrix multiplication +template <class T> MATRIX3x3<T> const operator*( MATRIX3x3<T> const& aA, MATRIX3x3<T> const& aB ); + +//! @brief Multiplication with a 2D vector, the 3rd z-component is assumed to be 1 +template <class T> VECTOR2<T> const operator*( MATRIX3x3<T> const& aA, VECTOR2<T> const& aB ); + +//! @brief Multiplication with a scalar +template <class T, class S> MATRIX3x3<T> const operator*( MATRIX3x3<T> const& aA, T aScalar ); +template <class T, class S> MATRIX3x3<T> const operator*( T aScalar, MATRIX3x3<T> const& aMatrix ); + +// ---------------------- +// --- Implementation --- +// ---------------------- + +template <class T> +MATRIX3x3<T>::MATRIX3x3() +{ + for( int j = 0; j < 3; j++ ) + { + for( int i = 0; i < 3; i++ ) + { + m_data[i][j] = 0.0; + } + } +} + + +template <class T> +MATRIX3x3<T>::MATRIX3x3( T a00, T a01, T a02, T a10, T a11, T a12, T a20, T a21, T a22 ) +{ + m_data[0][0] = a00; + m_data[0][1] = a01; + m_data[0][2] = a02; + + m_data[1][0] = a10; + m_data[1][1] = a11; + m_data[1][2] = a12; + + m_data[2][0] = a20; + m_data[2][1] = a21; + m_data[2][2] = a22; +} + + +template <class T> +void MATRIX3x3<T>::SetIdentity( void ) +{ + for( int j = 0; j < 3; j++ ) + { + for( int i = 0; i < 3; i++ ) + { + if( i == j ) + m_data[i][j] = 1.0; + else + m_data[i][j] = 0.0; + } + } +} + + +template <class T> +void MATRIX3x3<T>::SetTranslation( VECTOR2<T> aTranslation ) +{ + m_data[0][2] = aTranslation.x; + m_data[1][2] = aTranslation.y; +} + + +template <class T> +VECTOR2<T> MATRIX3x3<T>::GetTranslation() const +{ + VECTOR2<T> result; + result.x = m_data[0][2]; + result.y = m_data[1][2]; + + return result; +} + + +template <class T> +void MATRIX3x3<T>::SetRotation( T aAngle ) +{ + T cosValue = cos( aAngle ); + T sinValue = sin( aAngle ); + m_data[0][0] = cosValue; + m_data[0][1] = -sinValue; + m_data[1][0] = sinValue; + m_data[1][1] = cosValue; +} + + +template <class T> +void MATRIX3x3<T>::SetScale( VECTOR2<T> aScale ) +{ + m_data[0][0] = aScale.x; + m_data[1][1] = aScale.y; +} + + +template <class T> +VECTOR2<T> MATRIX3x3<T>::GetScale() const +{ + VECTOR2<T> result( m_data[0][0], m_data[1][1] ); + + return result; +} + + +template <class T> +MATRIX3x3<T> const operator*( MATRIX3x3<T> const& aA, MATRIX3x3<T> const& aB ) +{ + MATRIX3x3<T> result; + + for( int i = 0; i < 3; i++ ) + { + for( int j = 0; j < 3; j++ ) + { + result.m_data[i][j] = aA.m_data[i][0] * aB.m_data[0][j] + + aA.m_data[i][1] * aB.m_data[1][j] + + aA.m_data[i][2] * aB.m_data[2][j]; + } + } + + return result; +} + + +template <class T> +VECTOR2<T> const operator*( MATRIX3x3<T> const& aMatrix, VECTOR2<T> const& aVector ) +{ + VECTOR2<T> result( 0, 0 ); + result.x = aMatrix.m_data[0][0] * aVector.x + aMatrix.m_data[0][1] * aVector.y + + aMatrix.m_data[0][2]; + result.y = aMatrix.m_data[1][0] * aVector.x + aMatrix.m_data[1][1] * aVector.y + + aMatrix.m_data[1][2]; + + return result; +} + + +template <class T> +T MATRIX3x3<T>::Determinant() const +{ + return m_data[0][0] * ( m_data[1][1] * m_data[2][2] - m_data[1][2] * m_data[2][1] ) + - m_data[0][1] * ( m_data[1][0] * m_data[2][2] - m_data[1][2] * m_data[2][0] ) + + m_data[0][2] * ( m_data[1][0] * m_data[2][1] - m_data[1][1] * m_data[2][0] ); +} + + +template <class T, class S> +MATRIX3x3<T> const operator*( MATRIX3x3<T> const& aMatrix, S aScalar ) +{ + MATRIX3x3<T> result; + + for( int i = 0; i < 3; i++ ) + { + for( int j = 0; j < 3; j++ ) + { + result.m_data[i][j] = aMatrix.m_data[i][j] * aScalar; + } + } + + return result; +} + + +template <class T, class S> +MATRIX3x3<T> const operator*( S aScalar, MATRIX3x3<T> const& aMatrix ) +{ + return aMatrix * aScalar; +} + + +template <class T> +MATRIX3x3<T> MATRIX3x3<T>::Inverse() const +{ + MATRIX3x3<T> result; + + result.m_data[0][0] = m_data[1][1] * m_data[2][2] - m_data[2][1] * m_data[1][2]; + result.m_data[0][1] = m_data[0][2] * m_data[2][1] - m_data[2][2] * m_data[0][1]; + result.m_data[0][2] = m_data[0][1] * m_data[1][2] - m_data[1][1] * m_data[0][2]; + + result.m_data[1][0] = m_data[1][2] * m_data[2][0] - m_data[2][2] * m_data[1][0]; + result.m_data[1][1] = m_data[0][0] * m_data[2][2] - m_data[2][0] * m_data[0][2]; + result.m_data[1][2] = m_data[0][2] * m_data[1][0] - m_data[1][2] * m_data[0][0]; + + result.m_data[2][0] = m_data[1][0] * m_data[2][1] - m_data[2][0] * m_data[1][1]; + result.m_data[2][1] = m_data[0][1] * m_data[2][0] - m_data[2][1] * m_data[0][0]; + result.m_data[2][2] = m_data[0][0] * m_data[1][1] - m_data[1][0] * m_data[0][1]; + + return result * ( 1.0 / Determinant() ); +} + + +template <class T> +MATRIX3x3<T> MATRIX3x3<T>::Transpose() const +{ + MATRIX3x3<T> result; + + for( int i = 0; i < 3; i++ ) + { + for( int j = 0; j < 3; j++ ) + { + result.m_data[j][i] = m_data[i][j]; + } + } + + return result; +} + + +template <class T> +std::ostream& operator<<( std::ostream& aStream, const MATRIX3x3<T>& aMatrix ) +{ + for( int i = 0; i < 3; i++ ) + { + aStream << "| "; + + for( int j = 0; j < 3; j++ ) + { + aStream << aMatrix.m_data[i][j]; + aStream << " "; + } + + aStream << "|"; + aStream << "\n"; + } + + return aStream; +} + + +/* Default specializations */ +typedef MATRIX3x3<double> MATRIX3x3D; + +#endif /* MATRIX3X3_H_ */ diff --git a/include/math/vector2d.h b/include/math/vector2d.h new file mode 100644 index 0000000..bba2a4a --- /dev/null +++ b/include/math/vector2d.h @@ -0,0 +1,579 @@ +/* + * This program source code file is part of KICAD, a free EDA CAD application. + * + * Copyright (C) 2010 Virtenio GmbH, Torsten Hueter, torsten.hueter <at> virtenio.de + * Copyright (C) 2012 SoftPLC Corporation, Dick Hollenbeck <dick@softplc.com> + * Copyright (C) 2012 Kicad Developers, see change_log.txt for contributors. + * 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 VECTOR2D_H_ +#define VECTOR2D_H_ + +#include <cmath> +#include <climits> +#include <iostream> +#include <sstream> +#include <cmath> + +#include <math/math_util.h> + +#ifdef WX_COMPATIBILITY + #include <wx/gdicmn.h> +#endif + +/** + * Class VECTOR2_TRAITS + * traits class for VECTOR2. + */ +template <class T> +struct VECTOR2_TRAITS +{ + ///> extended range/precision types used by operations involving multiple + ///> multiplications to prevent overflow. + typedef T extended_type; +}; + +template <> +struct VECTOR2_TRAITS<int> +{ + typedef int64_t extended_type; + static const extended_type ECOORD_MAX = 0x7fffffffffffffffULL; + static const extended_type ECOORD_MIN = 0x8000000000000000ULL; +}; + +// Forward declarations for template friends +template <class T> +class VECTOR2; +template <class T> +std::ostream& operator<<( std::ostream& aStream, const VECTOR2<T>& aVector ); + +/** + * Class VECTOR2 + * defines a general 2D-vector/point. + * + * This class uses templates to be universal. Several operators are provided to help + * easy implementing of linear algebra equations. + * + */ +template <class T = int> +class VECTOR2 : public VECTOR2_TRAITS<T> +{ +public: + typedef typename VECTOR2_TRAITS<T>::extended_type extended_type; + typedef T coord_type; + + T x, y; + + // Constructors + + /// Construct a 2D-vector with x, y = 0 + VECTOR2(); + +#ifdef WX_COMPATIBILITY + /// Constructor with a wxPoint as argument + VECTOR2( const wxPoint& aPoint ); + + /// Constructor with a wxSize as argument + VECTOR2( const wxSize& aSize ); +#endif + + /// Construct a vector with given components x, y + VECTOR2( T x, T y ); + + /// Initializes a vector from another specialization. Beware of rouding + /// issues. + template <typename CastingType> + VECTOR2( const VECTOR2<CastingType>& aVec ) + { + x = (T) aVec.x; + y = (T) aVec.y; + } + + /// Casts a vector to another specialized subclass. Beware of rouding + /// issues. + template <typename CastedType> + VECTOR2<CastedType> operator()() const + { + return VECTOR2<CastedType>( (CastedType) x, (CastedType) y ); + } + + /// Destructor + // virtual ~VECTOR2(); + + /** + * Function Euclidean Norm + * computes the Euclidean norm of the vector, which is defined as sqrt(x ** 2 + y ** 2). + * It is used to calculate the length of the vector. + * @return Scalar, the euclidean norm + */ + T EuclideanNorm() const; + + /** + * Function Squared Euclidean Norm + * computes the squared euclidean norm of the vector, which is defined as (x ** 2 + y ** 2). + * It is used to calculate the length of the vector. + * @return Scalar, the euclidean norm + */ + extended_type SquaredEuclideanNorm() const; + + + /** + * Function Perpendicular + * computes the perpendicular vector + * @return Perpendicular vector + */ + VECTOR2<T> Perpendicular() const; + + /** + * Function Resize + * returns a vector of the same direction, but length specified in aNewLength + * @param aNewLength: length of the rescaled vector + * @return rescaled vector + */ + VECTOR2<T> Resize( T aNewLength ) const; + + /** + * Function Angle + * computes the angle of the vector + * @return vector angle, in radians + */ + double Angle() const; + + /** + * Function Rotate + * rotates the vector by a given angle + * @param aAngle rotation angle in radians + * @return rotated vector + */ + VECTOR2<T> Rotate( double aAngle ) const; + + /** + * Function Format + * returns the vector formatted as a string + * @return the formatted string + */ + const std::string Format() const; + + /** + * Function Cross() + * computes cross product of self with aVector + */ + extended_type Cross( const VECTOR2<T>& aVector ) const; + + /** + * Function Dot() + * computes dot product of self with aVector + */ + extended_type Dot( const VECTOR2<T>& aVector ) const; + + + // Operators + + /// Assignment operator + VECTOR2<T>& operator=( const VECTOR2<T>& aVector ); + + /// Vector addition operator + VECTOR2<T> operator+( const VECTOR2<T>& aVector ) const; + + /// Scalar addition operator + VECTOR2<T> operator+( const T& aScalar ) const; + + /// Compound assignment operator + VECTOR2<T>& operator+=( const VECTOR2<T>& aVector ); + + /// Compound assignment operator + VECTOR2<T>& operator+=( const T& aScalar ); + + /// Vector subtraction operator + VECTOR2<T> operator-( const VECTOR2<T>& aVector ) const; + + /// Scalar subtraction operator + VECTOR2<T> operator-( const T& aScalar ) const; + + /// Compound assignment operator + VECTOR2<T>& operator-=( const VECTOR2<T>& aVector ); + + /// Compound assignment operator + VECTOR2<T>& operator-=( const T& aScalar ); + + /// Negate Vector operator + VECTOR2<T> operator-(); + + /// Scalar product operator + extended_type operator*( const VECTOR2<T>& aVector ) const; + + /// Multiplication with a factor + VECTOR2<T> operator*( const T& aFactor ) const; + + /// Division with a factor + VECTOR2<T> operator/( const T& aFactor ) const; + + /// Equality operator + bool operator==( const VECTOR2<T>& aVector ) const; + + /// Not equality operator + bool operator!=( const VECTOR2<T>& aVector ) const; + + /// Smaller than operator + bool operator<( const VECTOR2<T>& aVector ) const; + bool operator<=( const VECTOR2<T>& aVector ) const; + + /// Greater than operator + bool operator>( const VECTOR2<T>& aVector ) const; + bool operator>=( const VECTOR2<T>& aVector ) const; + + friend std::ostream & operator<< <T> ( std::ostream & stream, const VECTOR2<T> &vector ); +}; + + +// ---------------------- +// --- Implementation --- +// ---------------------- + +template <class T> +VECTOR2<T>::VECTOR2() +{ + x = y = 0.0; +} + + +#ifdef WX_COMPATIBILITY +template <class T> +VECTOR2<T>::VECTOR2( wxPoint const& aPoint ) +{ + x = T( aPoint.x ); + y = T( aPoint.y ); +} + + +template <class T> +VECTOR2<T>::VECTOR2( wxSize const& aSize ) +{ + x = T( aSize.x ); + y = T( aSize.y ); +} +#endif + +template <class T> +VECTOR2<T>::VECTOR2( T aX, T aY ) +{ + x = aX; + y = aY; +} + + +template <class T> +T VECTOR2<T>::EuclideanNorm() const +{ + return sqrt( (extended_type) x * x + (extended_type) y * y ); +} + + +template <class T> +typename VECTOR2<T>::extended_type VECTOR2<T>::SquaredEuclideanNorm() const +{ + return (extended_type) x * x + (extended_type) y * y; +} + + +template <class T> +double VECTOR2<T>::Angle() const +{ + return atan2( y, x ); +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::Perpendicular() const +{ + VECTOR2<T> perpendicular( -y, x ); + return perpendicular; +} + + +template <class T> +VECTOR2<T>& VECTOR2<T>::operator=( const VECTOR2<T>& aVector ) +{ + x = aVector.x; + y = aVector.y; + return *this; +} + + +template <class T> +VECTOR2<T>& VECTOR2<T>::operator+=( const VECTOR2<T>& aVector ) +{ + x += aVector.x; + y += aVector.y; + return *this; +} + + +template <class T> +VECTOR2<T>& VECTOR2<T>::operator+=( const T& aScalar ) +{ + x += aScalar; + y += aScalar; + return *this; +} + + +template <class T> +VECTOR2<T>& VECTOR2<T>::operator-=( const VECTOR2<T>& aVector ) +{ + x -= aVector.x; + y -= aVector.y; + return *this; +} + + +template <class T> +VECTOR2<T>& VECTOR2<T>::operator-=( const T& aScalar ) +{ + x -= aScalar; + y -= aScalar; + return *this; +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::Rotate( double aAngle ) const +{ + double sa = sin( aAngle ); + double ca = cos( aAngle ); + + return VECTOR2<T> ( T( (double) x * ca - (double) y * sa ), + T( (double) x * sa + (double) y * ca ) ); +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::Resize( T aNewLength ) const +{ + if( x == 0 && y == 0 ) + return VECTOR2<T> ( 0, 0 ); + + extended_type l_sq_current = (extended_type) x * x + (extended_type) y * y; + extended_type l_sq_new = (extended_type) aNewLength * aNewLength; + + 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 ) ) ) * sign( aNewLength ); +} + + +template <class T> +const std::string VECTOR2<T>::Format() const +{ + std::stringstream ss; + + ss << "( xy " << x << " " << y << " )"; + + return ss.str(); +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::operator+( const VECTOR2<T>& aVector ) const +{ + return VECTOR2<T> ( x + aVector.x, y + aVector.y ); +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::operator+( const T& aScalar ) const +{ + return VECTOR2<T> ( x + aScalar, y + aScalar ); +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::operator-( const VECTOR2<T>& aVector ) const +{ + return VECTOR2<T> ( x - aVector.x, y - aVector.y ); +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::operator-( const T& aScalar ) const +{ + return VECTOR2<T> ( x - aScalar, y - aScalar ); +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::operator-() +{ + return VECTOR2<T> ( -x, -y ); +} + + +template <class T> +typename VECTOR2<T>::extended_type VECTOR2<T>::operator*( const VECTOR2<T>& aVector ) const +{ + return aVector.x * x + aVector.y * y; +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::operator*( const T& aFactor ) const +{ + VECTOR2<T> vector( x * aFactor, y * aFactor ); + return vector; +} + + +template <class T> +VECTOR2<T> VECTOR2<T>::operator/( const T& aFactor ) const +{ + VECTOR2<T> vector( x / aFactor, y / aFactor ); + return vector; +} + + +template <class T> +VECTOR2<T> operator*( const T& aFactor, const VECTOR2<T>& aVector ) +{ + VECTOR2<T> vector( aVector.x * aFactor, aVector.y * aFactor ); + return vector; +} + + +template <class T> +typename VECTOR2<T>::extended_type VECTOR2<T>::Cross( const VECTOR2<T>& aVector ) const +{ + return (extended_type) x * (extended_type) aVector.y - + (extended_type) y * (extended_type) aVector.x; +} + + +template <class T> +typename VECTOR2<T>::extended_type VECTOR2<T>::Dot( const VECTOR2<T>& aVector ) const +{ + return (extended_type) x * (extended_type) aVector.x + + (extended_type) y * (extended_type) aVector.y; +} + + +template <class T> +bool VECTOR2<T>::operator<( const VECTOR2<T>& aVector ) const +{ + return ( *this * *this ) < ( aVector * aVector ); +} + + +template <class T> +bool VECTOR2<T>::operator<=( const VECTOR2<T>& aVector ) const +{ + return ( *this * *this ) <= ( aVector * aVector ); +} + + +template <class T> +bool VECTOR2<T>::operator>( const VECTOR2<T>& aVector ) const +{ + return ( *this * *this ) > ( aVector * aVector ); +} + + +template <class T> +bool VECTOR2<T>::operator>=( const VECTOR2<T>& aVector ) const +{ + return ( *this * *this ) >= ( aVector * aVector ); +} + + +template <class T> +bool VECTOR2<T>::operator==( VECTOR2<T> const& aVector ) const +{ + return ( aVector.x == x ) && ( aVector.y == y ); +} + + +template <class T> +bool VECTOR2<T>::operator!=( VECTOR2<T> const& aVector ) const +{ + return ( aVector.x != x ) || ( aVector.y != y ); +} + + +template <class T> +const VECTOR2<T> LexicographicalMax( const VECTOR2<T>& aA, const VECTOR2<T>& aB ) +{ + if( aA.x > aB.x ) + return aA; + else if( aA.x == aB.x && aA.y > aB.y ) + return aA; + + return aB; +} + + +template <class T> +const VECTOR2<T> LexicographicalMin( const VECTOR2<T>& aA, const VECTOR2<T>& aB ) +{ + if( aA.x < aB.x ) + return aA; + else if( aA.x == aB.x && aA.y < aB.y ) + return aA; + + return aB; +} + + +template <class T> +const int LexicographicalCompare( const VECTOR2<T>& aA, const VECTOR2<T>& aB ) +{ + if( aA.x < aB.x ) + return -1; + else if( aA.x > aB.x ) + return 1; + else // aA.x == aB.x + { + if( aA.y < aB.y ) + return -1; + else if( aA.y > aB.y ) + return 1; + else + return 0; + } +} + + +template <class T> +std::ostream& operator<<( std::ostream& aStream, const VECTOR2<T>& aVector ) +{ + aStream << "[ " << aVector.x << " | " << aVector.y << " ]"; + return aStream; +} + + +/* Default specializations */ +typedef VECTOR2<double> VECTOR2D; +typedef VECTOR2<int> VECTOR2I; + +/* Compatibility typedefs */ +// FIXME should be removed to avoid multiple typedefs for the same type +typedef VECTOR2<double> DPOINT; +typedef DPOINT DSIZE; + +#endif // VECTOR2D_H_ |