46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpVelocityTwistMatrix.h>
57 for (
int i = 0; i < 6; ++i) {
58 for (
int j = 0; j < 6; ++j) {
71 const unsigned int nparam = 6;
72 for (
unsigned int i = 0; i < nparam; ++i) {
73 for (
unsigned int j = 0; j < nparam; ++j) {
226 const unsigned int nparam = 6;
227 for (
unsigned int i = 0; i < nparam; ++i) {
228 for (
unsigned int j = 0; j < nparam; ++j) {
230 for (
unsigned int k = 0; k < nparam; ++k) {
284 const unsigned int nparam = 6;
291 unsigned int m_col = M.
getCols();
292 for (
unsigned int i = 0; i < nparam; ++i) {
293 for (
unsigned int j = 0; j < m_col; ++j) {
295 for (
unsigned int k = 0; k < nparam; ++k) {
317 const unsigned int nparam = 6;
322 "Cannot multiply a (6x6) velocity twist matrix by a "
323 "(%d) column vector",
329 for (
unsigned int i = 0; i < nparam; ++i) {
330 for (
unsigned int j = 0; j < nparam; ++j) {
338 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
453 const unsigned int val_3 = 3;
454 for (
unsigned int i = 0; i < val_3; ++i) {
455 for (
unsigned int j = 0; j < val_3; ++j) {
456 (*this)[i][j] = R[i][j];
457 (*this)[i + 3][j + 3] = R[i][j];
458 (*this)[i][j + 3] = 0;
482 const unsigned int val_3 = 3;
483 for (
unsigned int i = 0; i < val_3; ++i) {
484 for (
unsigned int j = 0; j < val_3; ++j) {
485 (*this)[i][j] = R[i][j];
486 (*this)[i + 3][j + 3] = R[i][j];
487 (*this)[i][j + 3] = skewaR[i][j];
585 const unsigned int val_3 = 3;
586 for (
unsigned int i = 0; i < val_3; ++i) {
587 for (
unsigned int j = 0; j < val_3; ++j) {
588 R[i][j] = (*this)[i][j];
599 const unsigned int val_3 = 3;
600 for (
unsigned int i = 0; i < val_3; ++i) {
601 for (
unsigned int j = 0; j < val_3; ++j) {
602 skTR[i][j] = (*this)[i][j + val_3];
607 const unsigned int index_0 = 0;
608 const unsigned int index_1 = 1;
609 const unsigned int index_2 = 2;
610 tv[index_0] = skT[index_2][index_1];
611 tv[index_1] = skT[index_0][index_2];
612 tv[index_2] = skT[index_1][index_0];
636 typedef std::string::size_type size_type;
641 std::vector<std::string> values(m * n);
642 std::ostringstream oss;
643 std::ostringstream ossFixed;
644 std::ios_base::fmtflags original_flags = oss.flags();
647 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
649 size_type maxBefore = 0;
650 size_type maxAfter = 0;
652 for (
unsigned int i = 0; i < m; ++i) {
653 for (
unsigned int j = 0; j < n; ++j) {
655 oss << (*this)[i][j];
656 if (oss.str().find(
"e") != std::string::npos) {
658 ossFixed << (*this)[i][j];
659 oss.str(ossFixed.str());
662 values[(i * n) + j] = oss.str();
663 size_type thislen = values[(i * n) + j].
size();
664 size_type p = values[(i * n) + j].find(
'.');
666 if (p == std::string::npos) {
677 size_type totalLength = length;
681 maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
692 s <<
"[" << m <<
"," << n <<
"]=\n";
694 for (
unsigned int i = 0; i < m; ++i) {
696 for (
unsigned int j = 0; j < n; ++j) {
697 size_type p = values[(i * n) + j].find(
'.');
698 s.setf(std::ios::right, std::ios::adjustfield);
699 s.width(
static_cast<std::streamsize
>(maxBefore));
700 s << values[(i * n) + j].substr(0, p).c_str();
703 s.setf(std::ios::left, std::ios::adjustfield);
704 if (p != std::string::npos) {
705 s.width(
static_cast<std::streamsize
>(maxAfter));
706 s << values[(i * n) + j].substr(p, maxAfter).c_str();
709 assert(maxAfter > 1);
710 s.width(
static_cast<std::streamsize
>(maxAfter));
720 s.flags(original_flags);
722 return static_cast<int>(maxBefore + maxAfter);
725 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
Implementation of a generic 2D array used as base class for matrices and vectors.
unsigned int getCols() const
double ** rowPtrs
Address of the first element of each rows.
unsigned int size() const
Return the number of elements of the 2D array.
vpArray2D< double > t() const
Compute the transpose of the array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
static Type maximum(const Type &a, const Type &b)
Implementation of a matrix and operations on matrices.
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix t() const
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpVelocityTwistMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const
void extract(vpRotationMatrix &R) const
Extract the rotation matrix from the velocity twist matrix.
vpVelocityTwistMatrix inverse() const
Invert the velocity twist matrix.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
VP_DEPRECATED vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
VP_DEPRECATED void setIdentity()
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)