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