Blender  V3.3
jntarray.cpp
Go to the documentation of this file.
1 
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 "jntarray.hpp"
26 
27 namespace KDL
28 {
30  size(0),
31  data(NULL)
32  {
33  }
34 
35  JntArray::JntArray(unsigned int _size):
36  size(_size)
37  {
38  assert(0 < size);
39  data = new double[size];
40  SetToZero(*this);
41  }
42 
43 
45  size(arg.size)
46  {
47  data = ((0 < size) ? new double[size] : NULL);
48  for(unsigned int i=0;i<size;i++)
49  data[i]=arg.data[i];
50  }
51 
53  {
54  assert(size==arg.size);
55  for(unsigned int i=0;i<size;i++)
56  data[i]=arg.data[i];
57  return *this;
58  }
59 
60 
62  {
63  delete [] data;
64  }
65 
66  void JntArray::resize(unsigned int newSize)
67  {
68  delete [] data;
69  size = newSize;
70  data = new double[size];
71  SetToZero(*this);
72  }
73 
74  double JntArray::operator[](unsigned int i)const
75  {
76  assert(i<size);
77  return data[i];
78  }
79 
80  double& JntArray::operator[](unsigned int i)
81  {
82  assert(i<size);
83  return data[i];
84  }
85 
86  double* JntArray::operator()(unsigned int i)
87  {
88  if (i>=size)
89  return NULL;
90  return &data[i];
91  }
92 
93  unsigned int JntArray::rows()const
94  {
95  return size;
96  }
97 
98  unsigned int JntArray::columns()const
99  {
100  return 0;
101  }
102 
103  void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
104  {
105  assert(src1.size==src2.size&&src1.size==dest.size);
106  for(unsigned int i=0;i<dest.size;i++)
107  dest.data[i]=src1.data[i]+src2.data[i];
108  }
109 
110  void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
111  {
112  assert(src1.size==src2.size&&src1.size==dest.size);
113  for(unsigned int i=0;i<dest.size;i++)
114  dest.data[i]=src1.data[i]-src2.data[i];
115  }
116 
117  void Multiply(const JntArray& src,const double& factor,JntArray& dest)
118  {
119  assert(src.size==dest.size);
120  for(unsigned int i=0;i<dest.size;i++)
121  dest.data[i]=factor*src.data[i];
122  }
123 
124  void Divide(const JntArray& src,const double& factor,JntArray& dest)
125  {
126  assert(src.rows()==dest.size);
127  for(unsigned int i=0;i<dest.size;i++)
128  dest.data[i]=src.data[i]/factor;
129  }
130 
131  void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
132  {
133  assert(jac.columns()==src.size);
134  SetToZero(dest);
135  for(unsigned int i=0;i<6;i++)
136  for(unsigned int j=0;j<src.size;j++)
137  dest(i)+=jac(i,j)*src.data[j];
138  }
139 
141  {
142  for(unsigned int i=0;i<array.size;i++)
143  array.data[i]=0;
144  }
145 
146  bool Equal(const JntArray& src1, const JntArray& src2,double eps)
147  {
148  assert(src1.size==src2.size);
149  bool ret = true;
150  for(unsigned int i=0;i<src1.size;i++)
151  ret = ret && Equal(src1.data[i],src2.data[i],eps);
152  return ret;
153  }
154 
155  bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
156  //bool operator!=(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
157 
158 }
159 
160 
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
unsigned int columns() const
Definition: jacobian.cpp:76
double * operator()(unsigned int i)
Definition: jntarray.cpp:86
unsigned int rows() const
Definition: jntarray.cpp:93
friend void SetToZero(JntArray &array)
Definition: jntarray.cpp:140
JntArray & operator=(const JntArray &arg)
Definition: jntarray.cpp:52
void resize(unsigned int newSize)
Definition: jntarray.cpp:66
double operator[](unsigned int i) const
Definition: jntarray.cpp:74
unsigned int columns() const
Definition: jntarray.cpp:98
represents both translational and rotational velocities.
Definition: frames.hpp:679
size_t size() const
SyclQueue void void * src
SyclQueue void * dest
Definition: chain.cpp:27
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:377
void Add(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:103
void Divide(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:124
void Multiply(const JntArray &src, const double &factor, JntArray &dest)
Definition: jntarray.cpp:117
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:81
void Subtract(const JntArray &src1, const JntArray &src2, JntArray &dest)
Definition: jntarray.cpp:110
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Definition: jntarray.cpp:131
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
const btScalar eps
Definition: poly34.cpp:11
return ret