42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpVelocityTwistMatrix.h>
60 for (
int i = 0; i < 6; i++) {
61 for (
int j = 0; j < 6; j++) {
74 for (
unsigned int i = 0; i < 6; i++)
75 for (
unsigned int j = 0; j < 6; j++)
223 for (
unsigned int i = 0; i < 6; i++) {
224 for (
unsigned int j = 0; j < 6; j++) {
226 for (
int k = 0; k < 6; k++)
281 for (
unsigned int i = 0; i < 6; i++) {
282 for (
unsigned int j = 0; j < M.
getCols(); j++) {
284 for (
unsigned int k = 0; k < 6; k++)
309 "Cannot multiply a (6x6) velocity twist matrix by a "
310 "(%d) column vector",
316 for (
unsigned int i = 0; i < 6; i++) {
317 for (
unsigned int j = 0; j < 6; j++) {
340 for (
unsigned int i = 0; i < 3; i++) {
341 for (
unsigned int j = 0; j < 3; j++) {
342 (*this)[i][j] = R[i][j];
343 (*this)[i + 3][j + 3] = R[i][j];
344 (*this)[i][j + 3] = 0;
368 for (
unsigned int i = 0; i < 3; i++) {
369 for (
unsigned int j = 0; j < 3; j++) {
370 (*this)[i][j] = R[i][j];
371 (*this)[i + 3][j + 3] = R[i][j];
372 (*this)[i][j + 3] = skewaR[i][j];
468 for (
unsigned int i = 0; i < 3; i++)
469 for (
unsigned int j = 0; j < 3; j++)
470 R[i][j] = (*
this)[i][j];
479 for (
unsigned int i = 0; i < 3; i++)
480 for (
unsigned int j = 0; j < 3; j++)
481 skTR[i][j] = (*
this)[i][j + 3];
510 typedef std::string::size_type size_type;
515 std::vector<std::string> values(m * n);
516 std::ostringstream oss;
517 std::ostringstream ossFixed;
518 std::ios_base::fmtflags original_flags = oss.flags();
521 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
523 size_type maxBefore = 0;
524 size_type maxAfter = 0;
526 for (
unsigned int i = 0; i < m; ++i) {
527 for (
unsigned int j = 0; j < n; ++j) {
529 oss << (*this)[i][j];
530 if (oss.str().find(
"e") != std::string::npos) {
532 ossFixed << (*this)[i][j];
533 oss.str(ossFixed.str());
536 values[i * n + j] = oss.str();
537 size_type thislen = values[i * n + j].size();
538 size_type p = values[i * n + j].find(
'.');
540 if (p == std::string::npos) {
550 size_type totalLength = length;
554 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
563 s <<
"[" << m <<
"," << n <<
"]=\n";
565 for (
unsigned int i = 0; i < m; i++) {
567 for (
unsigned int j = 0; j < n; j++) {
568 size_type p = values[i * n + j].find(
'.');
569 s.setf(std::ios::right, std::ios::adjustfield);
570 s.width((std::streamsize)maxBefore);
571 s << values[i * n + j].substr(0, p).c_str();
574 s.setf(std::ios::left, std::ios::adjustfield);
575 if (p != std::string::npos) {
576 s.width((std::streamsize)maxAfter);
577 s << values[i * n + j].substr(p, maxAfter).c_str();
579 assert(maxAfter > 1);
580 s.width((std::streamsize)maxAfter);
590 s.flags(original_flags);
592 return (
int)(maxBefore + maxAfter);
595 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
605 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)