@@ -259,11 +259,7 @@ namespace dynamicgraph {
259
259
{
260
260
void operator ()( const dg::Vector& r,dg::Matrix & res )
261
261
{
262
- unsigned imax=r.size (),jmax=r.size ();
263
- if (( nbr!=0 )&&(nbc!=0 )) { imax=nbr; jmax=nbc; }
264
- res.resize (imax,jmax);
265
- for ( unsigned int i=0 ;i<imax;++i )
266
- for ( unsigned int j=0 ;j<jmax;++j ) if ( i==j ) res (i,i)=r (i); else res (i,j)=0 ;
262
+ res = r.asDiagonal ();
267
263
}
268
264
public:
269
265
Diagonalizer ( void ) : nbr(0 ),nbc(0 ) {}
@@ -461,7 +457,7 @@ namespace dynamicgraph {
461
457
: public UnaryOpHeader<MatrixHomogeneous,Matrix>
462
458
{
463
459
void operator ()( const MatrixHomogeneous& M,dg::Matrix& res )
464
- { res=(dg::Matrix&)M ; }
460
+ { res=M. matrix () ; }
465
461
};
466
462
REGISTER_UNARY_OP (HomoToMatrix,HomoToMatrix);
467
463
@@ -770,14 +766,8 @@ namespace dynamicgraph {
770
766
{
771
767
void operator () ( const dynamicgraph::Matrix& R,const dynamicgraph::Vector& t, MatrixHomogeneous& H ) const
772
768
{
773
- for ( int i=0 ;i<3 ;++i )
774
- {
775
- H (i,3 )=t (i);
776
- for ( int j=0 ;j<3 ;++j )
777
- H (i,j) = R (i,j);
778
- H (3 ,i) = 0 ;
779
- }
780
- H (3 ,3 )=1 .;
769
+ H.linear () = R;
770
+ H.translation () = t;
781
771
}
782
772
};
783
773
REGISTER_BINARY_OP (Composer,Compose_R_and_T);
@@ -791,7 +781,7 @@ namespace dynamicgraph {
791
781
792
782
void convolution ( const MemoryType &f1,const dynamicgraph::Matrix & f2,dynamicgraph::Vector &res )
793
783
{
794
- const int nconv = f1.size (),nsig=f2.rows ();
784
+ const Vector::Index nconv = (Vector::Index) f1.size (),nsig=f2.rows ();
795
785
sotDEBUG (15 ) << " Size: " << nconv << " x" << nsig << std::endl;
796
786
if ( nconv>f2.cols () ) return ; // TODO: error, this should not happen
797
787
@@ -813,7 +803,7 @@ namespace dynamicgraph {
813
803
void operator ()( const dynamicgraph::Vector& v1,const dynamicgraph::Matrix& m2,dynamicgraph::Vector& res )
814
804
{
815
805
memory.push_front ( v1 );
816
- while ( memory.size () > m2.cols () ) memory.pop_back ();
806
+ while ( (Vector::Index) memory.size () > m2.cols () ) memory.pop_back ();
817
807
convolution ( memory,m2,res );
818
808
}
819
809
};
0 commit comments