ViSP  2.8.0
vpVelocityTwistMatrix.cpp
1 /****************************************************************************
2  *
3  * $Id: vpVelocityTwistMatrix.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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  * Velocity twist transformation matrix.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpVelocityTwistMatrix.h>
43 
44 // Exception
45 #include <visp/vpException.h>
46 #include <visp/vpMatrixException.h>
47 
48 // Debug trace
49 #include <visp/vpDebug.h>
50 
51 
68 {
69  init() ;
70 
71  for (int i=0; i<6; i++)
72  {
73  for (int j=0; j<6; j++)
74  {
75  rowPtrs[i][j] = V.rowPtrs[i][j];
76  }
77  }
78 
79  return *this;
80 }
81 
82 
87 void
89 {
90  unsigned int i,j ;
91 
92  try {
93  resize(6,6) ;
94  }
95  catch(vpException me)
96  {
97  vpERROR_TRACE("Error caught") ;
98  throw ;
99  }
100 
101  for (i=0 ; i < 6 ; i++)
102  for (j=0 ; j < 6; j++)
103  if (i==j)
104  (*this)[i][j] = 1.0 ;
105  else
106  (*this)[i][j] = 0.0;
107 }
108 
113 {
114  init() ;
115 }
116 
123 {
124  init() ;
125  *this = V;
126 }
127 
139 {
140  init() ;
141  buildFrom(M);
142 }
143 
155  const vpThetaUVector &thetau) : vpMatrix()
156 {
157  init() ;
158  buildFrom(t, thetau) ;
159 }
160 
172  const vpRotationMatrix &R)
173 {
174  init() ;
175  buildFrom(t,R) ;
176 }
177 
188  const double ty,
189  const double tz,
190  const double tux,
191  const double tuy,
192  const double tuz) : vpMatrix()
193 {
194  init() ;
195  vpTranslationVector T(tx,ty,tz) ;
196  vpThetaUVector tu(tux,tuy,tuz) ;
197  buildFrom(T,tu) ;
198 }
199 
205 void
207 {
208  init() ;
209 }
210 
211 
220 {
222 
223  for (unsigned int i=0;i<6;i++)
224  for (unsigned int j=0;j<6;j++)
225  {
226  double s =0 ;
227  for (int k=0;k<6;k++)
228  s +=rowPtrs[i][k] * V.rowPtrs[k][j];
229  p[i][j] = s ;
230  }
231  return p;
232 }
233 
274 vpMatrix
276 {
277 
278  if (6 != M.getRows())
279  {
280  vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vpMatrix multiply") ;
282  }
283 
284  vpMatrix p(6, M.getCols()) ;
285  for (unsigned int i=0;i<6;i++)
286  for (unsigned int j=0;j<M.getCols();j++)
287  {
288  double s =0 ;
289  for (unsigned int k=0;k<6;k++)
290  s += rowPtrs[i][k] * M[k][j];
291  p[i][j] = s ;
292  }
293  return p;
294 }
295 
312 {
313  vpColVector c(6);
314 
315  if (6 != v.getRows())
316  {
317  vpERROR_TRACE("vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vector multiply") ;
319  }
320 
321  c = 0.0;
322 
323  for (unsigned int i=0;i<6;i++) {
324  for (unsigned int j=0;j<6;j++) {
325  {
326  c[i]+=rowPtrs[i][j] * v[j];
327  }
328  }
329  }
330 
331  return c ;
332 }
333 
334 
347  const vpRotationMatrix &R)
348 {
349  unsigned int i, j;
350  vpMatrix skewaR = t.skew(t)*R ;
351 
352  for (i=0 ; i < 3 ; i++)
353  for (j=0 ; j < 3 ; j++)
354  {
355  (*this)[i][j] = R[i][j] ;
356  (*this)[i+3][j+3] = R[i][j] ;
357  (*this)[i][j+3] = skewaR[i][j] ;
358 
359  }
360  return (*this) ;
361 }
362 
375  const vpThetaUVector &thetau)
376 {
377  vpRotationMatrix R ;
378  R.buildFrom(thetau) ;
379  buildFrom(t,R) ;
380  return (*this) ;
381 
382 }
383 
384 
397 {
399  vpRotationMatrix R ;
400  M.extract(R) ;
401  M.extract(t) ;
402 
403  buildFrom(t, R) ;
404  return (*this) ;
405 }
406 
407 
411 {
415  vpTranslationVector RtT ; RtT = -(R.t()*T) ;
416 
417  Wi.buildFrom(RtT,R.t());
418 
419  return Wi ;
420 }
421 
422 
424 void
426 {
427  Wi = inverse();
428 }
429 
431 void
433 {
434  for (unsigned int i=0 ; i < 3 ; i++)
435  for (unsigned int j=0 ; j < 3; j++)
436  R[i][j] = (*this)[i][j];
437 }
438 
440 void
442 {
444  vpMatrix skTR(3,3);
445  for (unsigned int i=0 ; i < 3 ; i++)
446  for (unsigned int j=0 ; j < 3; j++)
447  skTR[i][j] = (*this)[i][j+3];
448 
449  vpMatrix skT = skTR*R.t();
450  t[0] = skT[2][1];
451  t[1] = skT[0][2];
452  t[2] = skT[1][0];
453 }
454 
455 /*
456  * Local variables:
457  * c-basic-offset: 2
458  * End:
459  */
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
void resize(const unsigned int nrows, const unsigned int ncols, const bool nullify=true)
Definition: vpMatrix.cpp:174
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:379
error that can be emited by ViSP classes.
Definition: vpException.h:75
vpRotationMatrix t() const
transpose
The vpRotationMatrix considers the particular case of a rotation matrix.
vpRotationMatrix buildFrom(const vpThetaUVector &v)
Transform a vector vpThetaUVector into an rotation matrix.
vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const
double ** rowPtrs
address of the first element of each rows
Definition: vpMatrix.h:119
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
vpVelocityTwistMatrix inverse() const
invert the twist matrix
vpMatrix t() const
Definition: vpMatrix.cpp:1176
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:159
vpVelocityTwistMatrix & operator=(const vpVelocityTwistMatrix &V)
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:157
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
void extract(vpRotationMatrix &R) const
extract the rotational matrix from the twist matrix