40 #include <visp3/core/vpDebug.h>
41 #include <visp3/core/vpException.h>
42 #include <visp3/core/vpForceTwistMatrix.h>
59 for (
int i = 0; i < 6; ++i) {
60 for (
int j = 0; j < 6; ++j) {
73 for (
unsigned int i = 0; i < 6; ++i) {
74 for (
unsigned int j = 0; j < 6; ++j) {
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) {
300 "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.
getRows(), M.
getCols()));
303 unsigned int m_col = M.
getCols();
305 for (
unsigned int i = 0; i < 6; ++i) {
306 for (
unsigned int j = 0; j < m_col; ++j) {
308 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;
556 typedef std::string::size_type size_type;
561 std::vector<std::string> values(m * n);
562 std::ostringstream oss;
563 std::ostringstream ossFixed;
564 std::ios_base::fmtflags original_flags = oss.flags();
567 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
569 size_type maxBefore = 0;
570 size_type maxAfter = 0;
572 for (
unsigned int i = 0; i < m; ++i) {
573 for (
unsigned int j = 0; j < n; ++j) {
575 oss << (*this)[i][j];
576 if (oss.str().find(
"e") != std::string::npos) {
578 ossFixed << (*this)[i][j];
579 oss.str(ossFixed.str());
582 values[(i * n) + j] = oss.str();
583 size_type thislen = values[(i * n) + j].
size();
584 size_type p = values[(i * n) + j].find(
'.');
586 if (p == std::string::npos) {
597 size_type totalLength = length;
601 maxAfter = std::min<size_type>(maxAfter, totalLength - maxBefore);
612 s <<
"[" << m <<
"," << n <<
"]=\n";
614 for (
unsigned int i = 0; i < m; ++i) {
616 for (
unsigned int j = 0; j < n; ++j) {
617 size_type p = values[(i * n) + j].find(
'.');
618 s.setf(std::ios::right, std::ios::adjustfield);
619 s.width(
static_cast<std::streamsize
>(maxBefore));
620 s << values[(i * n) + j].substr(0, p).c_str();
623 s.setf(std::ios::left, std::ios::adjustfield);
624 if (p != std::string::npos) {
625 s.width(
static_cast<std::streamsize
>(maxAfter));
626 s << values[(i * n) + j].substr(p, maxAfter).c_str();
629 assert(maxAfter > 1);
630 s.width(
static_cast<std::streamsize
>(maxAfter));
640 s.flags(original_flags);
642 return static_cast<int>(maxBefore + maxAfter);
645 #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.
vpForceTwistMatrix & operator=(const vpForceTwistMatrix &H)
vpForceTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const
int print(std::ostream &s, unsigned int length, char const *intro=nullptr) const
vp_deprecated void setIdentity()
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.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.