ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
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
66
vpVelocityTwistMatrix
&
67
vpVelocityTwistMatrix::operator=
(
const
vpVelocityTwistMatrix
&V)
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
88
vpVelocityTwistMatrix::init
()
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
112
vpVelocityTwistMatrix::vpVelocityTwistMatrix
() :
vpMatrix
()
113
{
114
init
() ;
115
}
116
122
vpVelocityTwistMatrix::vpVelocityTwistMatrix
(
const
vpVelocityTwistMatrix
&V) :
vpMatrix
()
123
{
124
init
() ;
125
*
this
= V;
126
}
127
138
vpVelocityTwistMatrix::vpVelocityTwistMatrix
(
const
vpHomogeneousMatrix
&M) :
vpMatrix
()
139
{
140
init
() ;
141
buildFrom
(M);
142
}
143
154
vpVelocityTwistMatrix::vpVelocityTwistMatrix
(
const
vpTranslationVector
&t,
155
const
vpThetaUVector
&thetau) :
vpMatrix
()
156
{
157
init
() ;
158
buildFrom
(t, thetau) ;
159
}
160
171
vpVelocityTwistMatrix::vpVelocityTwistMatrix
(
const
vpTranslationVector
&t,
172
const
vpRotationMatrix
&R)
173
{
174
init
() ;
175
buildFrom
(t,R) ;
176
}
177
187
vpVelocityTwistMatrix::vpVelocityTwistMatrix
(
const
double
tx,
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
206
vpVelocityTwistMatrix::setIdentity
()
207
{
208
init
() ;
209
}
210
211
218
vpVelocityTwistMatrix
219
vpVelocityTwistMatrix::operator*
(
const
vpVelocityTwistMatrix
&V)
const
220
{
221
vpVelocityTwistMatrix
p ;
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
275
vpVelocityTwistMatrix::operator*
(
const
vpMatrix
&M)
const
276
{
277
278
if
(6 != M.
getRows
())
279
{
280
vpERROR_TRACE
(
"vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vpMatrix multiply"
) ;
281
throw
(
vpMatrixException::incorrectMatrixSizeError
) ;
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
310
vpColVector
311
vpVelocityTwistMatrix::operator*
(
const
vpColVector
&v)
const
312
{
313
vpColVector
c(6);
314
315
if
(6 != v.
getRows
())
316
{
317
vpERROR_TRACE
(
"vpVelocityTwistMatrix mismatch in vpVelocityTwistMatrix/vector multiply"
) ;
318
throw
(
vpMatrixException::incorrectMatrixSizeError
) ;
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
345
vpVelocityTwistMatrix
346
vpVelocityTwistMatrix::buildFrom
(
const
vpTranslationVector
&t,
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
373
vpVelocityTwistMatrix
374
vpVelocityTwistMatrix::buildFrom
(
const
vpTranslationVector
&t,
375
const
vpThetaUVector
&thetau)
376
{
377
vpRotationMatrix
R ;
378
R.
buildFrom
(thetau) ;
379
buildFrom
(t,R) ;
380
return
(*
this
) ;
381
382
}
383
384
395
vpVelocityTwistMatrix
396
vpVelocityTwistMatrix::buildFrom
(
const
vpHomogeneousMatrix
&M)
397
{
398
vpTranslationVector
t
;
399
vpRotationMatrix
R ;
400
M.
extract
(R) ;
401
M.
extract
(t) ;
402
403
buildFrom
(t, R) ;
404
return
(*
this
) ;
405
}
406
407
409
vpVelocityTwistMatrix
410
vpVelocityTwistMatrix::inverse
()
const
411
{
412
vpVelocityTwistMatrix
Wi;
413
vpRotationMatrix
R;
extract
(R);
414
vpTranslationVector
T;
extract
(T);
415
vpTranslationVector
RtT ; RtT = -(R.
t
()*T) ;
416
417
Wi.
buildFrom
(RtT,R.
t
());
418
419
return
Wi ;
420
}
421
422
424
void
425
vpVelocityTwistMatrix::inverse
(
vpVelocityTwistMatrix
&Wi)
const
426
{
427
Wi =
inverse
();
428
}
429
431
void
432
vpVelocityTwistMatrix::extract
(
vpRotationMatrix
&R)
const
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
441
vpVelocityTwistMatrix::extract
(
vpTranslationVector
&t)
const
442
{
443
vpRotationMatrix
R;
extract
(R);
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
*/
src
math
transformation
vpVelocityTwistMatrix.cpp
Generated on Thu Oct 24 2013 14:47:36 for ViSP by
1.8.4