Blender  V3.3
jntarrayvel.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 
26 #include "jntarrayacc.hpp"
27 
28 namespace KDL
29 {
31  q(size),qdot(size)
32  {
33  }
34  JntArrayVel::JntArrayVel(const JntArray& qin, const JntArray& qdotin):
35  q(qin),qdot(qdotin)
36  {
37  assert(q.rows()==qdot.rows());
38  }
40  q(qin),qdot(q.rows())
41  {
42  }
43 
45  {
46  return q;
47  }
48 
50  {
51  return qdot;
52  }
53 
54  void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
55  {
56  Add(src1.q,src2.q,dest.q);
57  Add(src1.qdot,src2.qdot,dest.qdot);
58  }
59  void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
60  {
61  Add(src1.q,src2,dest.q);
62  dest.qdot=src1.qdot;
63  }
64 
65  void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest)
66  {
67  Subtract(src1.q,src2.q,dest.q);
68  Subtract(src1.qdot,src2.qdot,dest.qdot);
69  }
70  void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest)
71  {
72  Subtract(src1.q,src2,dest.q);
73  dest.qdot=src1.qdot;
74  }
75 
76  void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
77  {
78  Multiply(src.q,factor,dest.q);
79  Multiply(src.qdot,factor,dest.qdot);
80  }
81  void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
82  {
83  Multiply(src.q,factor.grad,dest.q);
84  Multiply(src.qdot,factor.t,dest.qdot);
85  Add(dest.qdot,dest.q,dest.qdot);
86  Multiply(src.q,factor.t,dest.q);
87  }
88 
89  void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest)
90  {
91  Divide(src.q,factor,dest.q);
92  Divide(src.qdot,factor,dest.qdot);
93  }
94  void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest)
95  {
96  Multiply(src.q,(factor.grad/factor.t/factor.t),dest.q);
97  Divide(src.qdot,factor.t,dest.qdot);
98  Subtract(dest.qdot,dest.q,dest.qdot);
99  Divide(src.q,factor.t,dest.q);
100  }
101 
103  {
104  SetToZero(array.q);
105  SetToZero(array.qdot);
106  }
107 
108  bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps)
109  {
110  return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps);
111  }
112 }
113 
114 
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
JntArray deriv() const
Definition: jntarrayvel.cpp:49
JntArray value() const
Definition: jntarrayvel.cpp:44
JntArrayVel(unsigned int size)
Definition: jntarrayvel.cpp:30
unsigned int rows() const
Definition: jntarray.cpp:93
V grad
gradient
Definition: rall1d.h:55
T t
value
Definition: rall1d.h:54
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