41 #include <visp3/core/vpVelocityTwistMatrix.h> 42 #include <visp3/core/vpException.h> 62 for (
int i=0; i<6; i++)
64 for (
int j=0; j<6; j++)
80 for (
unsigned int i=0 ; i < 6 ; i++)
81 for (
unsigned int j=0 ; j < 6; j++)
192 for (
unsigned int i=0;i<6;i++)
193 for (
unsigned int j=0;j<6;j++)
196 for (
int k=0;k<6;k++)
244 "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
249 for (
unsigned int i=0;i<6;i++)
250 for (
unsigned int j=0;j<M.
getCols();j++)
253 for (
unsigned int k=0;k<6;k++)
278 "Cannot multiply a (6x6) velocity twist matrix by a (%d) column vector",
284 for (
unsigned int i=0;i<6;i++) {
285 for (
unsigned int j=0;j<6;j++) {
313 for (i=0 ; i < 3 ; i++)
314 for (j=0 ; j < 3 ; j++)
316 (*this)[i][j] = R[i][j] ;
317 (*this)[i+3][j+3] = R[i][j] ;
318 (*this)[i][j+3] = skewaR[i][j] ;
394 for (
unsigned int i=0 ; i < 3 ; i++)
395 for (
unsigned int j=0 ; j < 3; j++)
396 R[i][j] = (*
this)[i][j];
405 for (
unsigned int i=0 ; i < 3 ; i++)
406 for (
unsigned int j=0 ; j < 3; j++)
407 skTR[i][j] = (*
this)[i][j+3];
437 typedef std::string::size_type size_type;
442 std::vector<std::string> values(m*n);
443 std::ostringstream oss;
444 std::ostringstream ossFixed;
445 std::ios_base::fmtflags original_flags = oss.flags();
448 ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
450 size_type maxBefore=0;
451 size_type maxAfter=0;
453 for (
unsigned int i=0;i<m;++i) {
454 for (
unsigned int j=0;j<n;++j){
456 oss << (*this)[i][j];
457 if (oss.str().find(
"e")!=std::string::npos){
459 ossFixed << (*this)[i][j];
460 oss.str(ossFixed.str());
463 values[i*n+j]=oss.str();
464 size_type thislen=values[i*n+j].size();
465 size_type p=values[i*n+j].find(
'.');
467 if (p==std::string::npos){
477 size_type totalLength=length;
481 maxAfter=std::min(maxAfter, totalLength-maxBefore);
482 if (maxAfter==1) maxAfter=0;
487 if (intro) s <<intro;
488 s <<
"["<<m<<
","<<n<<
"]=\n";
490 for (
unsigned int i=0;i<m;i++) {
492 for (
unsigned int j=0;j<n;j++){
493 size_type p=values[i*n+j].find(
'.');
494 s.setf(std::ios::right, std::ios::adjustfield);
495 s.width((std::streamsize)maxBefore);
496 s <<values[i*n+j].substr(0,p).c_str();
499 s.setf(std::ios::left, std::ios::adjustfield);
500 if (p!=std::string::npos){
501 s.width((std::streamsize)maxAfter);
502 s <<values[i*n+j].substr(p,maxAfter).c_str();
505 s.width((std::streamsize)maxAfter);
515 s.flags(original_flags);
517 return (
int)(maxBefore+maxAfter);
520 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 534 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) Implementation of a matrix and operations on matrices.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
Extract the rotation matrix from the velocity twist matrix.
error that can be emited by ViSP classes.
unsigned int getRows() const
Return the number of rows of the 2D array.
Implementation of a generic 2D array used as vase class of matrices and vectors.
void extract(vpRotationMatrix &R) const
vpRotationMatrix t() const
static Type maximum(const Type &a, const Type &b)
Implementation of a rotation matrix and operations on such kind of matrices.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
unsigned int getCols() const
Return the number of columns of the 2D array.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a velocity twist matrix and operations on such kind of matrices.
vp_deprecated void setIdentity()
vpVelocityTwistMatrix inverse() const
Invert the velocity twist matrix.
Implementation of column vector and the associated operations.
vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double ** rowPtrs
Address of the first element of each rows.