doxygen: add newline after \file
[blender.git] / intern / itasc / kdl / jntarrayacc.cpp
1 /** \file itasc/kdl/jntarrayacc.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 "jntarrayacc.hpp"
26
27 namespace KDL
28 {
29     JntArrayAcc::JntArrayAcc(unsigned int size):
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     }
43     JntArrayAcc::JntArrayAcc(const JntArray& qin):
44         q(qin),qdot(q.rows()),qdotdot(q.rows())
45     {
46     }
47
48     JntArray JntArrayAcc::value()const
49     {
50         return q;
51     }
52
53     JntArray JntArrayAcc::deriv()const
54     {
55         return qdot;
56     }
57     JntArray JntArrayAcc::dderiv()const
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
160     void SetToZero(JntArrayAcc& array)
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