25 #ifndef WMATRIXFIXED_H
26 #define WMATRIXFIXED_H
31 #include <boost/static_assert.hpp>
32 #include <boost/lexical_cast.hpp>
33 #include <boost/tokenizer.hpp>
43 #include <osg/Matrixd>
49 #include "../../WDefines.h"
50 #include "../../WStringUtils.h"
51 #include "../../WTypeTraits.h"
53 #include "../../exceptions/WOutOfBounds.h"
58 #define ValueStoreTemplate template< typename, size_t, size_t > class
71 template<
typename ValueT,
size_t Rows,
size_t Cols >
100 const ValueT&
operator()(
size_t row,
size_t col )
const throw()
102 return m_values[ row * Cols + col ];
114 template <
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
117 for(
size_t row = 0; row < Rows; ++row )
119 for(
size_t col = 0; col < Cols; ++col )
145 template<
typename ValueT,
size_t Rows,
size_t Cols, ValueStoreTemplate ValueStoreT = ValueStore >
152 template<
typename ValueTT,
size_t Rowss,
size_t Colss, ValueStoreTemplate ValueStoreTT >
207 for(
size_t row = 0; row < Rows; ++row )
209 for(
size_t col = 0; col < Cols; ++col )
225 BOOST_STATIC_ASSERT( Rows == 3 );
242 BOOST_STATIC_ASSERT( Rows == 4 );
257 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
271 for(
size_t i = 0; i < std::min( Rows, Cols ); ++i )
273 m( i, i ) = ValueT( 1 );
286 for(
size_t row = 0; row < Rows; ++row )
288 for(
size_t col = 0; col < Cols; ++col )
290 m( row, col ) = ValueT( 0 );
313 template<
typename RHSValueT,
size_t RHSRows,
size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
315 size_t colOffset = 0 )
337 template<
typename RHSValueT,
size_t RHSRows,
size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
340 size_t colOffset = 0 )
343 for(
size_t row = 0; row < Rows; ++row )
345 for(
size_t col = 0; col < Cols; ++col )
347 if( ( row >= rowOffset ) && ( col >= colOffset ) )
350 size_t srcRow = row - rowOffset;
351 size_t srcCol = col - colOffset;
354 if( ( srcRow < RHSRows ) && ( srcCol < RHSCols ) )
356 result( row, col ) = src( srcRow, srcCol );
360 result( row, col ) = m( row, col );
365 result( row, col ) = m( row, col );
382 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
385 for(
size_t col = 0; col < Cols; col++ )
387 at( index, col ) = vec( col, 0 );
401 for(
size_t col = 0; col < Cols; col++ )
403 result( col, 0 ) =
at( index, col );
419 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
422 for(
size_t row = 0; row < Rows; row++ )
424 at( row, index ) = vec( row, 0 );
438 for(
size_t row = 0; row < Rows; row++ )
440 result( row, 0 ) =
at( row, index );
455 operator Eigen::Matrix< ValueT, Rows, Cols >()
const
457 Eigen::Matrix< ValueT, Rows, Cols > m;
458 for(
size_t row = 0; row < Rows; ++row )
460 for(
size_t col = 0; col < Cols; ++col )
473 operator osg::Vec2d()
const
476 BOOST_STATIC_ASSERT( Rows == 2 );
477 return osg::Vec2d(
operator[]( 0 ),
operator[]( 1 ) );
485 operator osg::Vec2f()
const
488 BOOST_STATIC_ASSERT( Rows == 2 );
489 return osg::Vec2f(
operator[]( 0 ),
operator[]( 1 ) );
497 operator osg::Vec3d()
const
500 BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
501 return osg::Vec3d(
operator[]( 0 ),
operator[]( 1 ),
operator[]( 2 ) );
509 operator osg::Vec3f()
const
512 BOOST_STATIC_ASSERT( ( Rows == 3 ) || ( Rows == 4 ) );
513 return osg::Vec3f(
operator[]( 0 ),
operator[]( 1 ),
operator[]( 2 ) );
521 operator osg::Vec4d()
const
524 BOOST_STATIC_ASSERT( Rows == 4 );
525 return osg::Vec4d(
operator[]( 0 ),
operator[]( 1 ),
operator[]( 2 ),
operator[]( 3 ) );
533 operator osg::Vec4f()
const
536 BOOST_STATIC_ASSERT( Rows == 4 );
537 return osg::Vec4f(
operator[]( 0 ),
operator[]( 1 ),
operator[]( 2 ),
operator[]( 3 ) );
545 operator osg::Matrixd()
const
547 BOOST_STATIC_ASSERT( Rows == 4 );
548 BOOST_STATIC_ASSERT( Cols == 4 );
551 for(
size_t row = 0; row < 4; ++row )
553 for(
size_t col = 0; col < 4; ++col )
571 template<
typename TargetType >
572 TargetType
as()
const
574 return operator TargetType();
585 template <
typename ResultValueType, ValueStoreTemplate ResultValueStore >
600 for(
size_t row = 0; row < Rows; ++row )
602 for(
size_t col = 0; col < Cols; ++col )
616 BOOST_STATIC_ASSERT( Rows == 4 );
617 BOOST_STATIC_ASSERT( Cols == 4 );
619 for(
size_t row = 0; row < 4; ++row )
621 for(
size_t col = 0; col < 4; ++col )
635 BOOST_STATIC_ASSERT( Rows == 3 );
636 BOOST_STATIC_ASSERT( Cols == 1 );
650 BOOST_STATIC_ASSERT( Rows == 3 );
651 BOOST_STATIC_ASSERT( Cols == 1 );
665 BOOST_STATIC_ASSERT( Rows == 4 );
666 BOOST_STATIC_ASSERT( Cols == 1 );
681 BOOST_STATIC_ASSERT( Rows == 4 );
682 BOOST_STATIC_ASSERT( Cols == 1 );
702 template <
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
722 template<
typename RHSValueT,
size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
726 typedef typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result ResultValueType;
730 for( std::size_t row = 0; row < Rows; ++row )
732 for( std::size_t col = 0; col < RHSCols; ++col )
734 m( row, col ) = ResultValueType();
736 for( std::size_t i = 0; i < Cols; ++i )
738 m( row, col ) +=
operator()( row, i ) * rhs( i, col );
751 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
765 template<
typename RHSValueT >
770 for(
size_t row = 0; row < Rows; ++row )
772 for(
size_t col = 0; col < Cols; ++col )
786 template<
typename RHSValueT >
800 template<
typename RHSValueT >
804 typedef typename WTypeTraits::TypePromotion< ValueT, RHSValueT >::Result ResultT;
805 return operator*( ResultT( 1 ) / static_cast< ResultT >( rhs ) );
814 template<
typename RHSValueT >
828 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
833 for(
size_t row = 0; row < Rows; ++row )
835 for(
size_t col = 0; col < Cols; ++col )
837 m( row, col ) =
operator()( row, col ) + rhs( row, col );
849 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
863 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
868 for(
size_t row = 0; row < Rows; ++row )
870 for(
size_t col = 0; col < Cols; ++col )
872 m( row, col ) =
operator()( row, col ) - rhs( row, col );
884 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
917 const ValueT&
operator()(
size_t row,
size_t col )
const throw()
932 BOOST_STATIC_ASSERT( Cols == 1 );
946 BOOST_STATIC_ASSERT( Cols == 1 );
963 if( ( row >= Rows ) || ( col >= Cols ) )
965 throw WOutOfBounds(
"Index pair (" + boost::lexical_cast< std::string >( row ) +
", " + boost::lexical_cast< std::string >( col ) +
966 ") is invalid for " + boost::lexical_cast< std::string >( Rows ) +
"x" + boost::lexical_cast< std::string >( Cols ) +
986 if( ( row >= Rows ) || ( col >= Cols ) )
988 throw WOutOfBounds(
"Index pair (" + boost::lexical_cast< std::string >( row ) +
", " + boost::lexical_cast< std::string >( col ) +
989 ") is invalid for " + boost::lexical_cast< std::string >( Rows ) +
"x" + boost::lexical_cast< std::string >( Cols ) +
1002 BOOST_STATIC_ASSERT( Rows >= 1 );
1003 BOOST_STATIC_ASSERT( Cols == 1 );
1012 const ValueT&
x()
const throw()
1014 BOOST_STATIC_ASSERT( Rows >= 1 );
1015 BOOST_STATIC_ASSERT( Cols == 1 );
1026 BOOST_STATIC_ASSERT( Rows >= 2 );
1027 BOOST_STATIC_ASSERT( Cols == 1 );
1036 const ValueT&
y()
const throw()
1038 BOOST_STATIC_ASSERT( Rows >= 2 );
1039 BOOST_STATIC_ASSERT( Cols == 1 );
1050 BOOST_STATIC_ASSERT( Rows >= 3 );
1051 BOOST_STATIC_ASSERT( Cols == 1 );
1060 const ValueT&
z()
const throw()
1062 BOOST_STATIC_ASSERT( Rows >= 3 );
1063 BOOST_STATIC_ASSERT( Cols == 1 );
1079 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1083 for(
size_t row = 0; eq && ( row < Rows ); ++row )
1085 for(
size_t col = 0; eq && ( col < Cols ); ++col )
1087 eq = eq && (
operator()( row, col ) == rhs( row, col ) );
1101 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1102 bool operator<( const WMatrixFixed< RHSValueT, Rows, Cols, RHSValueStoreT >& rhs )
const throw()
1106 for(
size_t row = 0; eq && ( row < Rows ); ++row )
1108 for(
size_t col = 0; eq && ( col < Cols ); ++col )
1110 eq = eq && (
operator()( row, col ) == rhs( row, col ) );
1111 result = (
operator()( row, col ) < rhs( row, col ) );
1124 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1142 template<
typename RHSValueT, ValueStoreTemplate RHSValueStoreT >
1143 void setValues(
const RHSValueStoreT< RHSValueT, Rows, Cols >& values )
1145 for( std::size_t i = 0; i < Rows; ++i )
1147 for( std::size_t j = 0; j < Cols; ++j )
1149 m_values( i, j ) =
static_cast< ValueT
>( values( i, j ) );
1181 template <
typename ScalarT,
1182 typename RHSValueT,
size_t RHSRows,
size_t RHSCols, ValueStoreTemplate RHSValueStoreT >
1207 template<
typename AValueT, ValueStoreTemplate AValueStoreT,
1208 typename BValueT, ValueStoreTemplate BValueStoreT,
1213 typedef typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result ResultType;
1214 ResultType r = ResultType();
1215 for(
size_t i = 0; i < Rows; ++i )
1217 r += a( i, 0 ) * b( i, 0 );
1235 template<
typename AValueT, ValueStoreTemplate AValueStoreT,
1236 typename BValueT, ValueStoreTemplate BValueStoreT >
1240 typedef WMatrixFixed< typename WTypeTraits::TypePromotion< AValueT, BValueT >::Result, 3, 1 > ResultT;
1245 v[0] = a[1] * b[2] - a[2] * b[1];
1246 v[1] = a[2] * b[0] - a[0] * b[2];
1247 v[2] = a[0] * b[1] - a[1] * b[0];
1261 template<
typename ValueT, ValueStoreTemplate ValueStoreT,
size_t Rows >
1264 ValueT r = ValueT();
1265 for(
size_t i = 0; i < Rows; ++i )
1267 r += a( i, 0 ) * a( i, 0 );
1282 template<
typename ValueT, ValueStoreTemplate ValueStoreT,
size_t Cols >
1285 ValueT r = ValueT();
1286 for(
size_t i = 0; i < Cols; ++i )
1288 r += a( 0, i ) * a( 0, i );
1303 template<
typename ValueT, ValueStoreTemplate ValueStoreT,
size_t Rows >
1306 return sqrt( length2( a ) );
1319 template<
typename ValueT, ValueStoreTemplate ValueStoreT,
size_t Cols >
1322 return sqrt( length2( a ) );
1336 template<
typename RHSValueT,
size_t Rows,
size_t Cols, ValueStoreTemplate RHSValueStoreT >
1340 return m *
static_cast< RHSValueT
>( 1.0 / length( m ) );
1355 template<
typename ValueT, std::
size_t Size,
template<
typename, std::
size_t, std::
size_t >
class ValueStoreT >
1376 template<
typename ValueT, std::
size_t Rows, std::
size_t Cols,
template<
typename, std::
size_t, std::
size_t >
class ValueStoreT >
1380 for(
size_t row = 0; row < mat.
getRows(); ++row )
1382 for(
size_t col = 0; col < mat.
getColumns(); ++col )
1384 res( col, row ) = mat( row, col );
1403 template<
typename ValueT,
size_t Rows,
size_t Cols, ValueStoreTemplate ValueStoreT >
1404 std::ostream& operator<<( std::ostream& out, const WMatrixFixed< ValueT, Rows, Cols, ValueStoreT >& m )
1407 for(
size_t row = 0; row < m.getRows(); ++row )
1409 for(
size_t col = 0; col < m.getColumns(); ++col )
1411 out << m( row, col ) <<
";";
1430 template<
typename ValueT,
size_t Rows,
size_t Cols, ValueStoreTemplate ValueStoreT >
1434 typedef boost::tokenizer< boost::char_separator< char > > Tokenizer;
1438 boost::char_separator< char > separators(
" ;" );
1439 Tokenizer t( s, separators );
1441 Tokenizer::iterator it = t.begin();
1442 for( std::size_t row = 0; row < Rows; ++row )
1444 for( std::size_t col = 0; col < Cols; ++col )
1450 m( row, col ) = boost::lexical_cast< ValueT >( *it );
1458 #endif // WMATRIXFIXED_H