ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpForceTwistMatrix.cpp
1
/****************************************************************************
2
*
3
* $Id: vpForceTwistMatrix.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
* 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
67
vpForceTwistMatrix
&
68
vpForceTwistMatrix::operator=
(
const
vpForceTwistMatrix
&M)
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
87
vpForceTwistMatrix::init
()
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
112
vpForceTwistMatrix::vpForceTwistMatrix
() :
vpMatrix
()
113
{
114
init
() ;
115
}
116
124
vpForceTwistMatrix::vpForceTwistMatrix
(
const
vpForceTwistMatrix
&F) :
vpMatrix
()
125
{
126
init
() ;
127
*
this
= F ;
128
}
129
154
vpForceTwistMatrix::vpForceTwistMatrix
(
const
vpHomogeneousMatrix
&M) :
vpMatrix
()
155
{
156
init
() ;
157
buildFrom
(M);
158
}
159
170
vpForceTwistMatrix::vpForceTwistMatrix
(
const
vpTranslationVector
&t,
171
const
vpThetaUVector
&thetau) :
vpMatrix
()
172
{
173
init
() ;
174
buildFrom
(t, thetau) ;
175
}
176
187
vpForceTwistMatrix::vpForceTwistMatrix
(
const
vpTranslationVector
&t,
188
const
vpRotationMatrix
&R)
189
{
190
init
() ;
191
buildFrom
(t,R) ;
192
}
193
203
vpForceTwistMatrix::vpForceTwistMatrix
(
const
double
tx,
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
222
vpForceTwistMatrix::setIdentity
()
223
{
224
init
() ;
225
}
226
227
246
vpForceTwistMatrix
247
vpForceTwistMatrix::operator*
(
const
vpForceTwistMatrix
&F)
const
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
269
vpForceTwistMatrix::operator*
(
const
vpMatrix
&M)
const
270
{
271
272
if
(6 != M.
getRows
())
273
{
274
vpERROR_TRACE
(
"vpForceTwistMatrix mismatch in vpForceTwistMatrix/vpMatrix multiply"
) ;
275
throw
(
vpMatrixException::incorrectMatrixSizeError
) ;
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
340
vpColVector
341
vpForceTwistMatrix::operator*
(
const
vpColVector
&H)
const
342
{
343
vpColVector
Hout(6);
344
345
if
(6 != H.
getRows
())
346
{
347
vpERROR_TRACE
(
"vpForceTwistMatrix mismatch in vpForceTwistMatrix/vector multiply"
) ;
348
throw
(
vpMatrixException::incorrectMatrixSizeError
) ;
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
373
vpForceTwistMatrix
374
vpForceTwistMatrix::buildFrom
(
const
vpTranslationVector
&t,
375
const
vpRotationMatrix
&R)
376
{
377
unsigned
int
i, j;
378
vpMatrix
skewaR = t.
skew
(t)*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
400
vpForceTwistMatrix
401
vpForceTwistMatrix::buildFrom
(
const
vpTranslationVector
&t,
402
const
vpThetaUVector
&thetau)
403
{
404
vpRotationMatrix
R ;
405
R.
buildFrom
(thetau) ;
406
buildFrom
(t,R) ;
407
return
(*
this
) ;
408
}
409
410
421
vpForceTwistMatrix
422
vpForceTwistMatrix::buildFrom
(
const
vpHomogeneousMatrix
&M)
423
{
424
vpTranslationVector
t
;
425
vpRotationMatrix
R ;
426
M.
extract
(R) ;
427
M.
extract
(t) ;
428
429
buildFrom
(t, R) ;
430
return
(*
this
) ;
431
}
432
433
/*
434
* Local variables:
435
* c-basic-offset: 2
436
* End:
437
*/
src
math
transformation
vpForceTwistMatrix.cpp
Generated on Thu Nov 7 2013 03:14:02 for ViSP by
1.8.4