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++) {
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) vpForceTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Implementation of a matrix and operations on matrices.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpForceTwistMatrix & operator=(const vpForceTwistMatrix &H)
error that can be emited by ViSP classes.
Implementation of a generic 2D array used as vase class of matrices and vectors.
unsigned int getCols() const
vp_deprecated void setIdentity()
static Type maximum(const Type &a, const Type &b)
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const
unsigned int getRows() const
Implementation of column vector and the associated operations.
int print(std::ostream &s, unsigned int length, char const *intro=0) const
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
double ** rowPtrs
Address of the first element of each rows.