doxygen: add newline after \file
[blender.git] / intern / itasc / kdl / jacobian.cpp
1 /** \file itasc/kdl/jacobian.cpp
2  * \ingroup itasc
3  */
4 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5
6 // Version: 1.0
7 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
9 // URL: http://www.orocos.org/kdl
10
11 // This library is free software; you can redistribute it and/or
12 // modify it under the terms of the GNU Lesser General Public
13 // License as published by the Free Software Foundation; either
14 // version 2.1 of the License, or (at your option) any later version.
15
16 // This library is distributed in the hope that it will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
19 // Lesser General Public License for more details.
20
21 // You should have received a copy of the GNU Lesser General Public
22 // License along with this library; if not, write to the Free Software
23 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
24
25 #include "jacobian.hpp"
26
27 namespace KDL
28 {
29     Jacobian::Jacobian(unsigned int _size,unsigned int _nr_blocks):
30         size(_size),nr_blocks(_nr_blocks)
31     {
32         twists = new Twist[size*nr_blocks];
33     }
34
35     Jacobian::Jacobian(const Jacobian& arg):
36                        size(arg.columns()),
37                        nr_blocks(arg.nr_blocks)
38     {
39         twists = new Twist[size*nr_blocks];
40         for(unsigned int i=0;i<size*nr_blocks;i++)
41             twists[i] = arg.twists[i];
42     }
43
44     Jacobian& Jacobian::operator = (const Jacobian& arg)
45     {
46         assert(size==arg.size);
47         assert(nr_blocks==arg.nr_blocks);
48         for(unsigned int i=0;i<size;i++)
49             twists[i]=arg.twists[i];
50         return *this;
51     }
52
53
54     Jacobian::~Jacobian()
55     {
56         delete [] twists;
57     }
58
59     double Jacobian::operator()(int i,int j)const
60     {
61         assert(i<6*(int)nr_blocks&&j<(int)size);
62         return twists[j+6*(int)(floor((double)i/6))](i%6);
63     }
64
65     double& Jacobian::operator()(int i,int j)
66     {
67         assert(i<6*(int)nr_blocks&&j<(int)size);
68         return twists[j+6*(int)(floor((double)i/6))](i%6);
69     }
70
71     unsigned int Jacobian::rows()const
72     {
73         return 6*nr_blocks;
74     }
75
76     unsigned int Jacobian::columns()const
77     {
78         return size;
79     }
80
81     void SetToZero(Jacobian& jac)
82     {
83         for(unsigned int i=0;i<jac.size*jac.nr_blocks;i++)
84             SetToZero(jac.twists[i]);
85     }
86
87     void changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
88     {
89         assert(src1.size==dest.size);
90         assert(src1.nr_blocks==dest.nr_blocks);
91         for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
92             dest.twists[i]=src1.twists[i].RefPoint(base_AB);
93     }
94
95     void changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
96     {
97         assert(src1.size==dest.size);
98         assert(src1.nr_blocks==dest.nr_blocks);
99         for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
100             dest.twists[i]=rot*src1.twists[i];
101     }
102
103     void changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
104     {
105         assert(src1.size==dest.size);
106         assert(src1.nr_blocks==dest.nr_blocks);
107         for(unsigned int i=0;i<src1.size*src1.nr_blocks;i++)
108             dest.twists[i]=frame*src1.twists[i];
109     }
110
111     bool Jacobian::operator ==(const Jacobian& arg)
112     {
113         return Equal((*this),arg);
114     }
115     
116     bool Jacobian::operator!=(const Jacobian& arg)
117     {
118         return !Equal((*this),arg);
119     }
120     
121     bool Equal(const Jacobian& a,const Jacobian& b,double eps)
122     {
123         if(a.rows()==b.rows()&&a.columns()==b.columns()){
124             bool rc=true;
125             for(unsigned int i=0;i<a.columns();i++)
126                 rc&=Equal(a.twists[i],b.twists[i],eps);
127             return rc;
128         }else
129             return false;
130     }
131     
132 }