43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpException.h>
45 #include <visp3/core/vpForceTwistMatrix.h>
62 for (
int i = 0; i < 6; i++) {
63 for (
int j = 0; j < 6; j++) {
76 for (
unsigned int i = 0; i < 6; i++) {
77 for (
unsigned int j = 0; j < 6; j++) {
159 buildFrom(t, thetau);
246 const double tuy,
const double tuz)
276 for (
unsigned int i = 0; i < 6; i++) {
277 for (
unsigned int j = 0; j < 6; j++) {
279 for (
unsigned int k = 0; k < 6; k++)
299 "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.
getRows(), M.
getCols()));
303 for (
unsigned int i = 0; i < 6; i++) {
304 for (
unsigned int j = 0; j < M.
getCols(); j++) {
306 for (
unsigned int k = 0; k < 6; k++)
367 "Cannot multiply a (6x6) force/torque twist matrix by "
368 "a %d dimension column vector",
374 for (
unsigned int i = 0; i < 6; i++) {
375 for (
unsigned int j = 0; j < 6; j++) {
376 Hout[i] +=
rowPtrs[i][j] * H[j];
406 for (
unsigned int i = 0; i < 3; i++) {
407 for (
unsigned int j = 0; j < 3; j++) {
408 (*this)[i][j] = R[i][j];
409 (*this)[i + 3][j + 3] = R[i][j];
410 (*this)[i + 3][j] = skewaR[i][j];
435 for (
unsigned int i = 0; i < 3; i++) {
436 for (
unsigned int j = 0; j < 3; j++) {
437 (*this)[i][j] = R[i][j];
438 (*this)[i + 3][j + 3] = R[i][j];
439 (*this)[i + 3][j] = 0;
554 typedef std::string::size_type size_type;
559 std::vector<std::string> values(m * n);
560 std::ostringstream oss;
561 std::ostringstream ossFixed;
562 std::ios_base::fmtflags original_flags = oss.flags();
565 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
567 size_type maxBefore = 0;
568 size_type maxAfter = 0;
570 for (
unsigned int i = 0; i < m; ++i) {
571 for (
unsigned int j = 0; j < n; ++j) {
573 oss << (*this)[i][j];
574 if (oss.str().find(
"e") != std::string::npos) {
576 ossFixed << (*this)[i][j];
577 oss.str(ossFixed.str());
580 values[i * n + j] = oss.str();
581 size_type thislen = values[i * n + j].size();
582 size_type p = values[i * n + j].find(
'.');
584 if (p == std::string::npos) {
594 size_type totalLength = length;
598 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
607 s <<
"[" << m <<
"," << n <<
"]=\n";
609 for (
unsigned int i = 0; i < m; i++) {
611 for (
unsigned int j = 0; j < n; j++) {
612 size_type p = values[i * n + j].find(
'.');
613 s.setf(std::ios::right, std::ios::adjustfield);
614 s.width((std::streamsize)maxBefore);
615 s << values[i * n + j].substr(0, p).c_str();
618 s.setf(std::ios::left, std::ios::adjustfield);
619 if (p != std::string::npos) {
620 s.width((std::streamsize)maxAfter);
621 s << values[i * n + j].substr(p, maxAfter).c_str();
623 assert(maxAfter > 1);
624 s.width((std::streamsize)maxAfter);
634 s.flags(original_flags);
636 return (
int)(maxBefore + maxAfter);
639 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
649 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)