384 lines
10 KiB
C
384 lines
10 KiB
C
|
/*
|
||
|
|
* 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& stream, const MATRIX3x3<T>& matrix );
|
||
|
|
|
||
|
|
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 a11 is the component [1,1].
|
||
|
|
* @param a12 is the component [1,2].
|
||
|
|
* @param a13 is the component [1,3].
|
||
|
|
* @param a20 is the component [2,0].
|
||
|
|
* @param a21 is the component [2,1].
|
||
|
|
* @param a00 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( void );
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @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( void ) 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( void ) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Compute the determinant of the matrix.
|
||
|
|
*
|
||
|
|
* @return the determinant value.
|
||
|
|
*/
|
||
|
|
T Determinant( void ) 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( void ) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Get the transpose of the matrix.
|
||
|
|
*
|
||
|
|
* @return the transpose matrix.
|
||
|
|
*/
|
||
|
|
MATRIX3x3 Transpose( void ) const;
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Output to a stream.
|
||
|
|
*/
|
||
|
|
friend std::ostream& operator<<<T>( std::ostream& stream, const MATRIX3x3<T>& matrix );
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
// Operators
|
||
|
|
|
||
|
|
//! @brief Matrix multiplication
|
||
|
|
template<class T> MATRIX3x3<T> const operator*( MATRIX3x3<T> const& a, MATRIX3x3<T> const& b );
|
||
|
|
|
||
|
|
//! @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& a, VECTOR2<T> const& b );
|
||
|
|
|
||
|
|
//! @brief Multiplication with a scalar
|
||
|
|
template<class T, class S> MATRIX3x3<T> const operator*( MATRIX3x3<T> const& a, T scalar );
|
||
|
|
template<class T, class S> MATRIX3x3<T> const operator*( T scalar, MATRIX3x3<T> const& matrix );
|
||
|
|
|
||
|
|
// ----------------------
|
||
|
|
// --- 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( void ) 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( void ) const
|
||
|
|
{
|
||
|
|
VECTOR2<T> result( m_data[0][0], m_data[1][1] );
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<class T> MATRIX3x3<T> const operator*( MATRIX3x3<T> const& a, MATRIX3x3<T> const& b )
|
||
|
|
{
|
||
|
|
MATRIX3x3<T> result;
|
||
|
|
|
||
|
|
for( int i = 0; i < 3; i++ )
|
||
|
|
{
|
||
|
|
for( int j = 0; j < 3; j++ )
|
||
|
|
{
|
||
|
|
result.m_data[i][j] = a.m_data[i][0] * b.m_data[0][j] + a.m_data[i][1] * b.m_data[1][j]
|
||
|
|
+ a.m_data[i][2] * b.m_data[2][j];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<class T> VECTOR2<T> const operator*( MATRIX3x3<T> const& matrix,
|
||
|
|
VECTOR2<T> const& vector )
|
||
|
|
{
|
||
|
|
VECTOR2<T> result( 0, 0 );
|
||
|
|
result.x = matrix.m_data[0][0] * vector.x + matrix.m_data[0][1] * vector.y
|
||
|
|
+ matrix.m_data[0][2];
|
||
|
|
result.y = matrix.m_data[1][0] * vector.x + matrix.m_data[1][1] * vector.y
|
||
|
|
+ matrix.m_data[1][2];
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<class T> T MATRIX3x3<T>::Determinant( void ) 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& matrix, S scalar )
|
||
|
|
{
|
||
|
|
MATRIX3x3<T> result;
|
||
|
|
|
||
|
|
for( int i = 0; i < 3; i++ )
|
||
|
|
{
|
||
|
|
for( int j = 0; j < 3; j++ )
|
||
|
|
{
|
||
|
|
result.m_data[i][j] = matrix.m_data[i][j] * scalar;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<class T, class S> MATRIX3x3<T> const operator*( S scalar, MATRIX3x3<T> const& matrix )
|
||
|
|
{
|
||
|
|
return matrix * scalar;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<class T> MATRIX3x3<T> MATRIX3x3<T>::Inverse( void ) 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( void ) 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_ */
|