39 #include <visp3/core/vpException.h>
40 #include <visp3/core/vpVelocityTwistMatrix.h>
57 for (
int i = 0; i < 6; ++i) {
58 for (
int j = 0; j < 6; ++j) {
71 for (
unsigned int i = 0; i < 6; ++i) {
72 for (
unsigned int j = 0; j < 6; ++j) {
226 for (
unsigned int i = 0; i < 6; ++i) {
227 for (
unsigned int j = 0; j < 6; ++j) {
229 for (
int k = 0; k < 6; ++k) {
285 unsigned int m_col = M.
getCols();
286 for (
unsigned int i = 0; i < 6; ++i) {
287 for (
unsigned int j = 0; j < m_col; ++j) {
289 for (
unsigned int k = 0; k < 6; ++k) {
315 "Cannot multiply a (6x6) velocity twist matrix by a "
316 "(%d) column vector",
322 for (
unsigned int i = 0; i < 6; ++i) {
323 for (
unsigned int j = 0; j < 6; ++j) {
344 for (
unsigned int i = 0; i < 3; ++i) {
345 for (
unsigned int j = 0; j < 3; ++j) {
346 (*this)[i][j] = R[i][j];
347 (*this)[i + 3][j + 3] = R[i][j];
348 (*this)[i][j + 3] = 0;
372 for (
unsigned int i = 0; i < 3; ++i) {
373 for (
unsigned int j = 0; j < 3; ++j) {
374 (*this)[i][j] = R[i][j];
375 (*this)[i + 3][j + 3] = R[i][j];
376 (*this)[i][j + 3] = skewaR[i][j];
474 for (
unsigned int i = 0; i < 3; ++i) {
475 for (
unsigned int j = 0; j < 3; ++j) {
476 R[i][j] = (*this)[i][j];
487 for (
unsigned int i = 0; i < 3; ++i) {
488 for (
unsigned int j = 0; j < 3; ++j) {
489 skTR[i][j] = (*this)[i][j + 3];
520 typedef std::string::size_type size_type;
525 std::vector<std::string> values(m * n);
526 std::ostringstream oss;
527 std::ostringstream ossFixed;
528 std::ios_base::fmtflags original_flags = oss.flags();
531 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
533 size_type maxBefore = 0;
534 size_type maxAfter = 0;
536 for (
unsigned int i = 0; i < m; ++i) {
537 for (
unsigned int j = 0; j < n; ++j) {
539 oss << (*this)[i][j];
540 if (oss.str().find(
"e") != std::string::npos) {
542 ossFixed << (*this)[i][j];
543 oss.str(ossFixed.str());
546 values[(i * n) + j] = oss.str();
547 size_type thislen = values[(i * n) + j].
size();
548 size_type p = values[(i * n) + j].find(
'.');
550 if (p == std::string::npos) {
561 size_type totalLength = length;
565 maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
576 s <<
"[" << m <<
"," << n <<
"]=\n";
578 for (
unsigned int i = 0; i < m; ++i) {
580 for (
unsigned int j = 0; j < n; ++j) {
581 size_type p = values[(i * n) + j].find(
'.');
582 s.setf(std::ios::right, std::ios::adjustfield);
583 s.width(
static_cast<std::streamsize
>(maxBefore));
584 s << values[(i * n) + j].substr(0, p).c_str();
587 s.setf(std::ios::left, std::ios::adjustfield);
588 if (p != std::string::npos) {
589 s.width(
static_cast<std::streamsize
>(maxAfter));
590 s << values[(i * n) + j].substr(p, maxAfter).c_str();
593 assert(maxAfter > 1);
594 s.width(
static_cast<std::streamsize
>(maxAfter));
604 s.flags(original_flags);
606 return static_cast<int>(maxBefore + maxAfter);
609 #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 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
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)
vp_deprecated void setIdentity()