ViSP  2.9.0
vpForceTwistMatrix.cpp
1 /****************************************************************************
2  *
3  * $Id: vpForceTwistMatrix.cpp 4649 2014-02-07 14:57:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Twist transformation matrix that allows to transform forces from one
36  * frame to an other.
37  *
38  * Authors:
39  * Fabien Spindler
40  *
41  *****************************************************************************/
42 
43 #include <visp/vpForceTwistMatrix.h>
44 
45 // Exception
46 #include <visp/vpException.h>
47 #include <visp/vpMatrixException.h>
48 
49 // Debug trace
50 #include <visp/vpDebug.h>
51 
52 
69 {
70  init() ;
71 
72  for (int i=0; i<6; i++) {
73  for (int j=0; j<6; j++) {
74  rowPtrs[i][j] = M.rowPtrs[i][j];
75  }
76  }
77 
78  return *this;
79 }
80 
81 
86 void
88 {
89  unsigned int i,j ;
90 
91  try {
92  resize(6,6) ;
93  }
94  catch(vpException me) {
95  vpERROR_TRACE("Error caught") ;
96  throw ;
97  }
98 
99  for (i=0 ; i < 6 ; i++) {
100  for (j=0 ; j < 6; j++) {
101  if (i==j)
102  (*this)[i][j] = 1.0 ;
103  else
104  (*this)[i][j] = 0.0;
105  }
106  }
107 }
108 
113 {
114  init() ;
115 }
116 
125 {
126  init() ;
127  *this = F ;
128 }
129 
155 {
156  init() ;
157  buildFrom(M);
158 }
159 
171  const vpThetaUVector &thetau) : vpMatrix()
172 {
173  init() ;
174  buildFrom(tv, thetau) ;
175 }
176 
188  const vpRotationMatrix &R)
189 {
190  init() ;
191  buildFrom(tv,R) ;
192 }
193 
204  const double ty,
205  const double tz,
206  const double tux,
207  const double tuy,
208  const double tuz) : vpMatrix()
209 {
210  init() ;
211  vpTranslationVector T(tx,ty,tz) ;
212  vpThetaUVector tu(tux,tuy,tuz) ;
213  buildFrom(T,tu) ;
214 }
215 
221 void
223 {
224  init() ;
225 }
226 
227 
248 {
249  vpForceTwistMatrix Fout ;
250 
251  for (unsigned int i=0;i<6;i++) {
252  for (unsigned int j=0;j<6;j++) {
253  double s =0 ;
254  for (unsigned int k=0;k<6;k++)
255  s +=rowPtrs[i][k] * F.rowPtrs[k][j];
256  Fout[i][j] = s ;
257  }
258  }
259  return Fout;
260 }
261 
268 vpMatrix
270 {
271 
272  if (6 != M.getRows())
273  {
274  vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vpMatrix multiply") ;
276  }
277 
278  vpMatrix p(6, M.getCols()) ;
279  for (unsigned int i=0;i<6;i++) {
280  for (unsigned int j=0;j<M.getCols();j++) {
281  double s =0 ;
282  for (unsigned int k=0;k<6;k++)
283  s += rowPtrs[i][k] * M[k][j];
284  p[i][j] = s ;
285  }
286  }
287  return p;
288 }
289 
342 {
343  vpColVector Hout(6);
344 
345  if (6 != H.getRows())
346  {
347  vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vector multiply") ;
349  }
350 
351  Hout = 0.0;
352 
353  for (unsigned int i=0;i<6;i++) {
354  for (unsigned int j=0;j<6;j++) {
355  Hout[i]+=rowPtrs[i][j] * H[j];
356  }
357  }
358 
359  return Hout ;
360 }
361 
362 
375  const vpRotationMatrix &R)
376 {
377  unsigned int i, j;
378  vpMatrix skewaR = tv.skew(tv)*R ;
379 
380  for (i=0 ; i < 3 ; i++) {
381  for (j=0 ; j < 3 ; j++) {
382  (*this)[i][j] = R[i][j] ;
383  (*this)[i+3][j+3] = R[i][j] ;
384  (*this)[i+3][j] = skewaR[i][j] ;
385  }
386  }
387  return (*this) ;
388 }
389 
402  const vpThetaUVector &thetau)
403 {
404  vpRotationMatrix R ;
405  R.buildFrom(thetau) ;
406  buildFrom(tv,R) ;
407  return (*this) ;
408 }
409 
410 
423 {
425  vpRotationMatrix R ;
426  M.extract(R) ;
427  M.extract(tv) ;
428 
429  buildFrom(tv, R) ;
430  return (*this) ;
431 }
432 
433 /*
434  * Local variables:
435  * c-basic-offset: 2
436  * End:
437  */
vpForceTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:183
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpERROR_TRACE
Definition: vpDebug.h:395
vpForceTwistMatrix & operator=(const vpForceTwistMatrix &H)
error that can be emited by ViSP classes.
Definition: vpException.h:76
The vpRotationMatrix considers the particular case of a rotation matrix.
vpRotationMatrix buildFrom(const vpThetaUVector &v)
Transform a vector vpThetaUVector into an rotation matrix.
double ** rowPtrs
address of the first element of each rows
Definition: vpMatrix.h:121
vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a for...
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
unsigned int getCols() const
Return the number of columns of the matrix.
Definition: vpMatrix.h:163
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.