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++)
220 for (
unsigned int i = 0; i < 6; i++) {
221 for (
unsigned int j = 0; j < 6; j++) {
223 for (
int k = 0; k < 6; k++)
278 for (
unsigned int i = 0; i < 6; i++) {
279 for (
unsigned int j = 0; j < M.
getCols(); j++) {
281 for (
unsigned int k = 0; k < 6; k++)
306 "Cannot multiply a (6x6) velocity twist matrix by a "
307 "(%d) column vector",
313 for (
unsigned int i = 0; i < 6; i++) {
314 for (
unsigned int j = 0; j < 6; j++) {
337 for (
unsigned int i = 0; i < 3; i++) {
338 for (
unsigned int j = 0; j < 3; j++) {
339 (*this)[i][j] = R[i][j];
340 (*this)[i + 3][j + 3] = R[i][j];
341 (*this)[i][j + 3] = 0;
365 for (
unsigned int i = 0; i < 3; i++) {
366 for (
unsigned int j = 0; j < 3; j++) {
367 (*this)[i][j] = R[i][j];
368 (*this)[i + 3][j + 3] = R[i][j];
369 (*this)[i][j + 3] = skewaR[i][j];
465 for (
unsigned int i = 0; i < 3; i++)
466 for (
unsigned int j = 0; j < 3; j++)
467 R[i][j] = (*
this)[i][j];
476 for (
unsigned int i = 0; i < 3; i++)
477 for (
unsigned int j = 0; j < 3; j++)
478 skTR[i][j] = (*
this)[i][j + 3];
507 typedef std::string::size_type size_type;
512 std::vector<std::string> values(m * n);
513 std::ostringstream oss;
514 std::ostringstream ossFixed;
515 std::ios_base::fmtflags original_flags = oss.flags();
518 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
520 size_type maxBefore = 0;
521 size_type maxAfter = 0;
523 for (
unsigned int i = 0; i < m; ++i) {
524 for (
unsigned int j = 0; j < n; ++j) {
526 oss << (*this)[i][j];
527 if (oss.str().find(
"e") != std::string::npos) {
529 ossFixed << (*this)[i][j];
530 oss.str(ossFixed.str());
533 values[i * n + j] = oss.str();
534 size_type thislen = values[i * n + j].size();
535 size_type p = values[i * n + j].find(
'.');
537 if (p == std::string::npos) {
547 size_type totalLength = length;
551 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
560 s <<
"[" << m <<
"," << n <<
"]=\n";
562 for (
unsigned int i = 0; i < m; i++) {
564 for (
unsigned int j = 0; j < n; j++) {
565 size_type p = values[i * n + j].find(
'.');
566 s.setf(std::ios::right, std::ios::adjustfield);
567 s.width((std::streamsize)maxBefore);
568 s << values[i * n + j].substr(0, p).c_str();
571 s.setf(std::ios::left, std::ios::adjustfield);
572 if (p != std::string::npos) {
573 s.width((std::streamsize)maxAfter);
574 s << values[i * n + j].substr(p, maxAfter).c_str();
576 assert(maxAfter > 1);
577 s.width((std::streamsize)maxAfter);
587 s.flags(original_flags);
589 return (
int)(maxBefore + maxAfter);
592 #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.
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()