Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / jntarray.cpp
1 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16 // Lesser General Public License for more details.
17
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
21
22 #include "jntarray.hpp"
23
24 namespace KDL
25 {
26     JntArray::JntArray():
27             size(0),
28             data(NULL)
29     {
30     }
31
32     JntArray::JntArray(unsigned int _size):
33         size(_size)
34     {
35         assert(0 < size);
36         data = new double[size];
37         SetToZero(*this);
38     }
39
40
41     JntArray::JntArray(const JntArray& arg):
42         size(arg.size)
43     {
44         data = ((0 < size) ? new double[size] : NULL);
45         for(unsigned int i=0;i<size;i++)
46             data[i]=arg.data[i];
47     }
48
49     JntArray& JntArray::operator = (const JntArray& arg)
50     {
51         assert(size==arg.size);
52         for(unsigned int i=0;i<size;i++)
53             data[i]=arg.data[i];
54         return *this;
55     }
56
57
58     JntArray::~JntArray()
59     {
60         delete [] data;
61     }
62
63     void JntArray::resize(unsigned int newSize)
64     {
65         delete [] data;
66         size = newSize;
67         data = new double[size];
68         SetToZero(*this);
69     }
70
71     double JntArray::operator()(unsigned int i,unsigned int j)const
72     {
73         assert(i<size&&j==0);
74         assert(0 != size);  // found JntArray containing no data
75         return data[i];
76     }
77
78     double& JntArray::operator()(unsigned int i,unsigned int j)
79     {
80         assert(i<size&&j==0);
81         assert(0 != size);  // found JntArray containing no data
82         return data[i];
83     }
84
85     unsigned int JntArray::rows()const
86     {
87         return size;
88     }
89
90     unsigned int JntArray::columns()const
91     {
92         return 0;
93     }
94
95     void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
96     {
97         assert(src1.size==src2.size&&src1.size==dest.size);
98         for(unsigned int i=0;i<dest.size;i++)
99             dest.data[i]=src1.data[i]+src2.data[i];
100     }
101
102     void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
103     {
104         assert(src1.size==src2.size&&src1.size==dest.size);
105         for(unsigned int i=0;i<dest.size;i++)
106             dest.data[i]=src1.data[i]-src2.data[i];
107     }
108
109     void Multiply(const JntArray& src,const double& factor,JntArray& dest)
110     {
111         assert(src.size==dest.size);
112         for(unsigned int i=0;i<dest.size;i++)
113             dest.data[i]=factor*src.data[i];
114     }
115
116     void Divide(const JntArray& src,const double& factor,JntArray& dest)
117     {
118         assert(src.rows()==dest.size);
119         for(unsigned int i=0;i<dest.size;i++)
120             dest.data[i]=src.data[i]/factor;
121     }
122
123     void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
124     {
125         assert(jac.columns()==src.size);
126         SetToZero(dest);
127         for(unsigned int i=0;i<6;i++)
128             for(unsigned int j=0;j<src.size;j++)
129                 dest(i)+=jac(i,j)*src.data[j];
130     }
131
132     void SetToZero(JntArray& array)
133     {
134         for(unsigned int i=0;i<array.size;i++)
135             array.data[i]=0;
136     }
137
138     bool Equal(const JntArray& src1, const JntArray& src2,double eps)
139     {
140         assert(src1.size==src2.size);
141         bool ret = true;
142         for(unsigned int i=0;i<src1.size;i++)
143             ret = ret && Equal(src1.data[i],src2.data[i],eps);
144         return ret;
145     }
146
147     bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
148     //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
149
150 }
151
152