Blender  V3.3
jntarrayacc.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 "jntarrayacc.hpp"
26 
27 namespace KDL
28 {
30  q(size),qdot(size),qdotdot(size)
31  {
32  }
33  JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin,const JntArray& qdotdotin):
34  q(qin),qdot(qdotin),qdotdot(qdotdotin)
35  {
36  assert(q.rows()==qdot.rows()&&qdot.rows()==qdotdot.rows());
37  }
38  JntArrayAcc::JntArrayAcc(const JntArray& qin, const JntArray& qdotin):
39  q(qin),qdot(qdotin),qdotdot(q.rows())
40  {
41  assert(q.rows()==qdot.rows());
42  }
44  q(qin),qdot(q.rows()),qdotdot(q.rows())
45  {
46  }
47 
49  {
50  return q;
51  }
52 
54  {
55  return qdot;
56  }
58  {
59  return qdotdot;
60  }
61 
62  void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
63  {
64  Add(src1.q,src2.q,dest.q);
65  Add(src1.qdot,src2.qdot,dest.qdot);
66  Add(src1.qdotdot,src2.qdotdot,dest.qdotdot);
67  }
68  void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
69  {
70  Add(src1.q,src2.q,dest.q);
71  Add(src1.qdot,src2.qdot,dest.qdot);
72  dest.qdotdot=src1.qdotdot;
73  }
74  void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
75  {
76  Add(src1.q,src2,dest.q);
77  dest.qdot=src1.qdot;
78  dest.qdotdot=src1.qdotdot;
79  }
80 
81  void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest)
82  {
83  Subtract(src1.q,src2.q,dest.q);
84  Subtract(src1.qdot,src2.qdot,dest.qdot);
85  Subtract(src1.qdotdot,src2.qdotdot,dest.qdotdot);
86  }
87  void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest)
88  {
89  Subtract(src1.q,src2.q,dest.q);
90  Subtract(src1.qdot,src2.qdot,dest.qdot);
91  dest.qdotdot=src1.qdotdot;
92  }
93  void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest)
94  {
95  Subtract(src1.q,src2,dest.q);
96  dest.qdot=src1.qdot;
97  dest.qdotdot=src1.qdotdot;
98  }
99 
100  void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
101  {
102  Multiply(src.q,factor,dest.q);
103  Multiply(src.qdot,factor,dest.qdot);
104  Multiply(src.qdotdot,factor,dest.qdotdot);
105  }
106  void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
107  {
108  Multiply(src.qdot,factor.grad*2,dest.qdot);
109  Multiply(src.qdotdot,factor.t,dest.qdotdot);
110  Add(dest.qdot,dest.qdotdot,dest.qdotdot);
111  Multiply(src.q,factor.grad,dest.q);
112  Multiply(src.qdot,factor.t,dest.qdot);
113  Add(dest.qdot,dest.q,dest.qdot);
114  Multiply(src.q,factor.t,dest.q);
115  }
116  void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
117  {
118  Multiply(src.q,factor.dd,dest.q);
119  Multiply(src.qdot,factor.d*2,dest.qdot);
120  Multiply(src.qdotdot,factor.t,dest.qdotdot);
121  Add(dest.qdotdot,dest.qdot,dest.qdotdot);
122  Add(dest.qdotdot,dest.q,dest.qdotdot);
123  Multiply(src.q,factor.d,dest.q);
124  Multiply(src.qdot,factor.t,dest.qdot);
125  Add(dest.qdot,dest.q,dest.qdot);
126  Multiply(src.q,factor.t,dest.q);
127  }
128 
129  void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest)
130  {
131  Divide(src.q,factor,dest.q);
132  Divide(src.qdot,factor,dest.qdot);
133  Divide(src.qdotdot,factor,dest.qdotdot);
134  }
135  void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest)
136  {
137  Multiply(src.q,(2*factor.grad*factor.grad)/(factor.t*factor.t*factor.t),dest.q);
138  Multiply(src.qdot,(2*factor.grad)/(factor.t*factor.t),dest.qdot);
139  Divide(src.qdotdot,factor.t,dest.qdotdot);
140  Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
141  Add(dest.qdotdot,dest.q,dest.qdotdot);
142  Multiply(src.q,factor.grad/(factor.t*factor.t),dest.q);
143  Divide(src.qdot,factor.t,dest.qdot);
144  Subtract(dest.qdot,dest.q,dest.qdot);
145  Divide(src.q,factor.t,dest.q);
146  }
147  void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest)
148  {
149  Multiply(src.q,(2*factor.d*factor.d)/(factor.t*factor.t*factor.t)-factor.dd/(factor.t*factor.t),dest.q);
150  Multiply(src.qdot,(2*factor.d)/(factor.t*factor.t),dest.qdot);
151  Divide(src.qdotdot,factor.t,dest.qdotdot);
152  Subtract(dest.qdotdot,dest.qdot,dest.qdotdot);
153  Add(dest.qdotdot,dest.q,dest.qdotdot);
154  Multiply(src.q,factor.d/(factor.t*factor.t),dest.q);
155  Divide(src.qdot,factor.t,dest.qdot);
156  Subtract(dest.qdot,dest.q,dest.qdot);
157  Divide(src.q,factor.t,dest.q);
158  }
159 
161  {
162  SetToZero(array.q);
163  SetToZero(array.qdot);
164  SetToZero(array.qdotdot);
165  }
166 
167  bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps)
168  {
169  return (Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps)&&Equal(src1.qdotdot,src2.qdotdot,eps));
170  }
171 }
172 
173 
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
JntArray deriv() const
Definition: jntarrayacc.cpp:53
JntArray dderiv() const
Definition: jntarrayacc.cpp:57
JntArray value() const
Definition: jntarrayacc.cpp:48
JntArrayAcc(unsigned int size)
Definition: jntarrayacc.cpp:29
unsigned int rows() const
Definition: jntarray.cpp:93
V grad
gradient
Definition: rall1d.h:55
T t
value
Definition: rall1d.h:54
V d
1st derivative
Definition: rall2d.h:56
V dd
2nd derivative
Definition: rall2d.h:57
T t
value
Definition: rall2d.h:55
SyclQueue void void * src
SyclQueue void * dest
Definition: chain.cpp:27
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
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
const btScalar eps
Definition: poly34.cpp:11