Blender  V3.3
framevel.hpp
Go to the documentation of this file.
1 /*****************************************************************************
2  * \file
3  * This file contains the definition of classes for a
4  * Rall Algebra of (subset of) the classes defined in frames,
5  * i.e. classes that contain a pair (value,derivative) and define operations on that pair
6  * this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
7  * Defines VectorVel, RotationVel, FrameVel. Look at Frames.h for details on how to work
8  * with Frame objects.
9  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
10  *
11  * \version
12  * ORO_Geometry V0.2
13  *
14  * \par History
15  * - $log$
16  *
17  * \par Release
18  * $Name: $
19  ****************************************************************************/
20 
21 #ifndef KDL_FRAMEVEL_H
22 #define KDL_FRAMEVEL_H
23 
24 #include "utilities/utility.h"
25 #include "utilities/rall1d.h"
26 #include "utilities/traits.h"
27 
28 #include "frames.hpp"
29 
30 
31 
32 namespace KDL {
33 
35 
36 IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
37  return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
38 }
39 
40 IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
41  return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
42 }
43 
45  random(F.t);
46  random(F.grad);
47 }
49  posrandom(F.t);
50  posrandom(F.grad);
51 }
52 
53 }
54 
55 template <>
56 struct Traits<KDL::doubleVel> {
57  typedef double valueType;
59 };
60 
61 namespace KDL {
62 
63 class TwistVel;
64 class VectorVel;
65 class FrameVel;
66 class RotationVel;
67 
68 class VectorVel
69 // = TITLE
70 // An VectorVel is a Vector and its first derivative
71 // = CLASS TYPE
72 // Concrete
73 {
74 public:
75  Vector p; // position vector
76  Vector v; // velocity vector
77 public:
78  VectorVel():p(),v(){}
79  VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
80  explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
81 
82  Vector value() const { return p;}
83  Vector deriv() const { return v;}
84 
86  IMETHOD VectorVel& operator = (const Vector& arg);
89  IMETHOD static VectorVel Zero();
90  IMETHOD void ReverseSign();
91  IMETHOD doubleVel Norm() const;
92  IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
93  IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
94  IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
95  IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
96  IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
97  IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
98  IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
99  IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
100  IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
101  IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
102  IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
103  IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
104  IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
105  IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
106 
107  IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
108  IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
109  IMETHOD friend void SetToZero(VectorVel& v);
110 
111 
112  IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps);
113  IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps);
114  IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps);
116  IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
117  IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
118  IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
119 };
120 
121 
122 
124 // = TITLE
125 // An RotationVel is a Rotation and its first derivative, a rotation vector
126 // = CLASS TYPE
127 // Concrete
128 {
129 public:
130  Rotation R; // Rotation matrix
131  Vector w; // rotation vector
132 public:
133  RotationVel():R(),w() {}
134  explicit RotationVel(const Rotation& R_):R(R_),w(Vector::Zero()){}
135  RotationVel(const Rotation& R_,const Vector& _w):R(R_),w(_w){}
136 
137 
138  Rotation value() const { return R;}
139  Vector deriv() const { return w;}
140 
141 
144  IMETHOD VectorVel UnitX() const;
145  IMETHOD VectorVel UnitY() const;
146  IMETHOD VectorVel UnitZ() const;
147  IMETHOD static RotationVel Identity();
148  IMETHOD RotationVel Inverse() const;
149  IMETHOD VectorVel Inverse(const VectorVel& arg) const;
150  IMETHOD VectorVel Inverse(const Vector& arg) const;
151  IMETHOD VectorVel operator*(const VectorVel& arg) const;
152  IMETHOD VectorVel operator*(const Vector& arg) const;
153  IMETHOD void DoRotX(const doubleVel& angle);
154  IMETHOD void DoRotY(const doubleVel& angle);
155  IMETHOD void DoRotZ(const doubleVel& angle);
156  IMETHOD static RotationVel RotX(const doubleVel& angle);
157  IMETHOD static RotationVel RotY(const doubleVel& angle);
158  IMETHOD static RotationVel RotZ(const doubleVel& angle);
159  IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
160  // rotvec has arbitrary norm
161  // rotation around a constant vector !
162  IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
163  // rotvec is normalized.
164  // rotation around a constant vector !
165  IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
166  IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
167  IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
168  IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps);
169  IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps);
170  IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps);
171 
172  IMETHOD TwistVel Inverse(const TwistVel& arg) const;
173  IMETHOD TwistVel Inverse(const Twist& arg) const;
174  IMETHOD TwistVel operator * (const TwistVel& arg) const;
175  IMETHOD TwistVel operator * (const Twist& arg) const;
176 };
177 
178 
179 
180 
181 class FrameVel
182 // = TITLE
183 // An FrameVel is a Frame and its first derivative, a Twist vector
184 // = CLASS TYPE
185 // Concrete
186 // = CAVEATS
187 //
188 {
189 public:
192 public:
194 
195  explicit FrameVel(const Frame& T_):
196  M(T_.M),p(T_.p) {}
197 
198  FrameVel(const Frame& T_,const Twist& _t):
199  M(T_.M,_t.rot),p(T_.p,_t.vel) {}
200 
201  FrameVel(const RotationVel& _M,const VectorVel& _p):
202  M(_M),p(_p) {}
203 
204 
205  Frame value() const { return Frame(M.value(),p.value());}
206  Twist deriv() const { return Twist(p.deriv(),M.deriv());}
207 
208 
209  IMETHOD FrameVel& operator = (const Frame& arg);
210  IMETHOD FrameVel& operator = (const FrameVel& arg);
211  IMETHOD static FrameVel Identity();
212  IMETHOD FrameVel Inverse() const;
213  IMETHOD VectorVel Inverse(const VectorVel& arg) const;
214  IMETHOD VectorVel operator*(const VectorVel& arg) const;
215  IMETHOD VectorVel operator*(const Vector& arg) const;
216  IMETHOD VectorVel Inverse(const Vector& arg) const;
217  IMETHOD Frame GetFrame() const;
218  IMETHOD Twist GetTwist() const;
219  IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
220  IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
221  IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
222  IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps);
223  IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps);
224  IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps);
225 
226  IMETHOD TwistVel Inverse(const TwistVel& arg) const;
227  IMETHOD TwistVel Inverse(const Twist& arg) const;
228  IMETHOD TwistVel operator * (const TwistVel& arg) const;
229  IMETHOD TwistVel operator * (const Twist& arg) const;
230 };
231 
232 
233 
234 
235 
236 //very similar to Wrench class.
237 class TwistVel
238 // = TITLE
239 // This class represents a TwistVel. This is a velocity and rotational velocity together
240 {
241 public:
244 public:
245 
246 // = Constructors
247  TwistVel():vel(),rot() {};
248  TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
249  TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
250  TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
251 
252  Twist value() const {
253  return Twist(vel.value(),rot.value());
254  }
255  Twist deriv() const {
256  return Twist(vel.deriv(),rot.deriv());
257  }
258 // = Operators
259  IMETHOD TwistVel& operator-=(const TwistVel& arg);
260  IMETHOD TwistVel& operator+=(const TwistVel& arg);
261 
262 // = External operators
263  IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
264  IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
265  IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
266 
267  IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
268  IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
269  IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
270 
271  IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
272  IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
273  IMETHOD friend TwistVel operator-(const TwistVel& arg);
274  IMETHOD friend void SetToZero(TwistVel& v);
275 
276 
277 // = Zero
278  static IMETHOD TwistVel Zero();
279 
280 // = Reverse Sign
281  IMETHOD void ReverseSign();
282 
283 // = Change Reference point
284  IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
285  // Changes the reference point of the TwistVel.
286  // The VectorVel v_base_AB is expressed in the same base as the TwistVel
287  // The VectorVel v_base_AB is a VectorVel from the old point to
288  // the new point.
289  // Complexity : 6M+6A
290 
291  // = Equality operators
292  // do not use operator == because the definition of Equal(.,.) is slightly
293  // different. It compares whether the 2 arguments are equal in an eps-interval
294  IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps);
295  IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps);
296  IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps);
297 
298 // = Conversion to other entities
299  IMETHOD Twist GetTwist() const;
300  IMETHOD Twist GetTwistDot() const;
301 // = Friends
302  friend class RotationVel;
303  friend class FrameVel;
304 
305 };
306 
307 IMETHOD bool Equal(const VectorVel&, const VectorVel&, double = epsilon);
308 IMETHOD bool Equal(const Vector&, const VectorVel&, double = epsilon);
309 IMETHOD bool Equal(const VectorVel&, const Vector&, double = epsilon);
310 IMETHOD bool Equal(const RotationVel&, const RotationVel&, double = epsilon);
311 IMETHOD bool Equal(const Rotation&, const RotationVel&, double = epsilon);
312 IMETHOD bool Equal(const RotationVel&, const Rotation&, double = epsilon);
313 IMETHOD bool Equal(const FrameVel&, const FrameVel&, double = epsilon);
314 IMETHOD bool Equal(const Frame&, const FrameVel&, double = epsilon);
315 IMETHOD bool Equal(const FrameVel&, const Frame&, double = epsilon);
316 IMETHOD bool Equal(const TwistVel&, const TwistVel&, double = epsilon);
317 IMETHOD bool Equal(const Twist&, const TwistVel&, double = epsilon);
318 IMETHOD bool Equal(const TwistVel&, const Twist&, double = epsilon);
319 
320 IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
321  return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
322 }
323 
324 IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
325  return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
326 }
327 IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
328  return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
329 }
330 
331 IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
332  return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
333 }
334 
335 IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
336  return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
337 }
338 
339 IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
340  return FrameVel(
341  addDelta(a.M,da.rot,dt),
342  addDelta(a.p,da.vel,dt)
343  );
344 }
345 
347  random(a.p);
348  random(a.v);
349 }
351  random(a.vel);
352  random(a.rot);
353 }
354 
356  random(R.R);
357  random(R.w);
358 }
359 
361  random(F.M);
362  random(F.p);
363 }
365  posrandom(a.p);
366  posrandom(a.v);
367 }
369  posrandom(a.vel);
370  posrandom(a.rot);
371 }
372 
374  posrandom(R.R);
375  posrandom(R.w);
376 }
377 
379  posrandom(F.M);
380  posrandom(F.p);
381 }
382 
383 #ifdef KDL_INLINE
384 #include "framevel.inl"
385 #endif
386 
387 } // namespace
388 
389 #endif
390 
391 
392 
393 
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
struct Frame Frame
Definition: frames.hpp:261
ATTR_WARN_UNUSED_RESULT const BMVert * v
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition: btVector3.h:356
FrameVel(const RotationVel &_M, const VectorVel &_p)
Definition: framevel.hpp:201
IMETHOD friend bool Equal(const FrameVel &r1, const FrameVel &r2, double eps)
FrameVel(const Frame &T_, const Twist &_t)
Definition: framevel.hpp:198
Frame value() const
Definition: framevel.hpp:205
IMETHOD FrameVel & operator=(const Frame &arg)
Definition: framevel.inl:68
Twist deriv() const
Definition: framevel.hpp:206
IMETHOD Twist GetTwist() const
Definition: framevel.inl:87
IMETHOD friend bool Equal(const Frame &r1, const FrameVel &r2, double eps)
FrameVel(const Frame &T_)
Definition: framevel.hpp:195
IMETHOD friend bool Equal(const FrameVel &r1, const Frame &r2, double eps)
VectorVel p
Definition: framevel.hpp:191
IMETHOD Frame GetFrame() const
Definition: framevel.inl:83
IMETHOD VectorVel operator*(const VectorVel &arg) const
Definition: framevel.inl:44
IMETHOD FrameVel Inverse() const
Definition: framevel.inl:63
RotationVel M
Definition: framevel.hpp:190
static IMETHOD FrameVel Identity()
Definition: framevel.inl:26
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
V grad
gradient
Definition: rall1d.h:55
T t
value
Definition: rall1d.h:54
IMETHOD VectorVel UnitY() const
Definition: framevel.inl:119
RotationVel(const Rotation &R_, const Vector &_w)
Definition: framevel.hpp:135
static IMETHOD RotationVel RotZ(const doubleVel &angle)
Definition: framevel.inl:187
IMETHOD RotationVel & operator=(const RotationVel &arg)
Definition: framevel.inl:104
IMETHOD RotationVel Inverse() const
Definition: framevel.inl:133
Vector deriv() const
Definition: framevel.hpp:139
IMETHOD VectorVel UnitX() const
Definition: framevel.inl:115
static IMETHOD RotationVel Identity()
Definition: framevel.inl:129
static IMETHOD RotationVel RotX(const doubleVel &angle)
Definition: framevel.inl:171
IMETHOD friend bool Equal(const RotationVel &r1, const RotationVel &r2, double eps)
static IMETHOD RotationVel Rot(const Vector &rotvec, const doubleVel &angle)
Definition: framevel.inl:192
RotationVel(const Rotation &R_)
Definition: framevel.hpp:134
IMETHOD friend bool Equal(const Rotation &r1, const RotationVel &r2, double eps)
IMETHOD VectorVel UnitZ() const
Definition: framevel.inl:123
IMETHOD friend bool Equal(const RotationVel &r1, const Rotation &r2, double eps)
IMETHOD void DoRotX(const doubleVel &angle)
Definition: framevel.inl:167
IMETHOD void DoRotY(const doubleVel &angle)
Definition: framevel.inl:175
static IMETHOD RotationVel Rot2(const Vector &rotvec, const doubleVel &angle)
Definition: framevel.inl:201
IMETHOD void DoRotZ(const doubleVel &angle)
Definition: framevel.inl:183
static IMETHOD RotationVel RotY(const doubleVel &angle)
Definition: framevel.inl:179
Rotation value() const
Definition: framevel.hpp:138
IMETHOD VectorVel operator*(const VectorVel &arg) const
Definition: framevel.inl:152
represents rotations in 3 dimensional space.
Definition: frames.hpp:299
IMETHOD friend bool Equal(const Twist &a, const TwistVel &b, double eps)
IMETHOD friend TwistVel operator/(const TwistVel &lhs, double rhs)
Twist deriv() const
Definition: framevel.hpp:255
static IMETHOD TwistVel Zero()
Definition: framevel.inl:376
IMETHOD friend void SetToZero(TwistVel &v)
TwistVel(const Twist &p, const Twist &v)
Definition: framevel.hpp:249
IMETHOD TwistVel & operator+=(const TwistVel &arg)
Definition: framevel.inl:405
IMETHOD friend TwistVel operator-(const TwistVel &lhs, const TwistVel &rhs)
IMETHOD friend TwistVel operator*(double lhs, const TwistVel &rhs)
Twist value() const
Definition: framevel.hpp:252
IMETHOD friend TwistVel operator*(const doubleVel &lhs, const TwistVel &rhs)
IMETHOD Twist GetTwist() const
Definition: framevel.inl:526
IMETHOD void ReverseSign()
Definition: framevel.inl:382
VectorVel vel
Definition: framevel.hpp:242
TwistVel(const Twist &p)
Definition: framevel.hpp:250
IMETHOD Twist GetTwistDot() const
Definition: framevel.inl:530
IMETHOD friend TwistVel operator+(const TwistVel &lhs, const TwistVel &rhs)
IMETHOD TwistVel & operator-=(const TwistVel &arg)
Definition: framevel.inl:398
IMETHOD friend TwistVel operator-(const TwistVel &arg)
IMETHOD friend TwistVel operator*(const TwistVel &lhs, double rhs)
IMETHOD friend bool Equal(const TwistVel &a, const Twist &b, double eps)
IMETHOD TwistVel RefPoint(const VectorVel &v_base_AB)
Definition: framevel.inl:388
VectorVel rot
Definition: framevel.hpp:243
IMETHOD friend TwistVel operator*(const TwistVel &lhs, const doubleVel &rhs)
IMETHOD friend TwistVel operator/(const TwistVel &lhs, const doubleVel &rhs)
IMETHOD friend bool Equal(const TwistVel &a, const TwistVel &b, double eps)
TwistVel(const VectorVel &_vel, const VectorVel &_rot)
Definition: framevel.hpp:248
represents both translational and rotational velocities.
Definition: frames.hpp:679
static IMETHOD VectorVel Zero()
Definition: framevel.inl:309
IMETHOD friend doubleVel dot(const VectorVel &lhs, const Vector &rhs)
VectorVel(const Vector &_p, const Vector &_v)
Definition: framevel.hpp:79
IMETHOD friend VectorVel operator+(const VectorVel &r1, const VectorVel &r2)
IMETHOD friend void SetToZero(VectorVel &v)
Vector value() const
Definition: framevel.hpp:82
IMETHOD friend bool Equal(const Vector &r1, const VectorVel &r2, double eps)
IMETHOD friend VectorVel operator*(const Rotation &R, const VectorVel &x)
Vector deriv() const
Definition: framevel.hpp:83
IMETHOD friend VectorVel operator-(const VectorVel &r1, const VectorVel &r2)
VectorVel(const Vector &_p)
Definition: framevel.hpp:80
IMETHOD friend doubleVel dot(const Vector &lhs, const VectorVel &rhs)
IMETHOD friend VectorVel operator/(const VectorVel &r1, double r2)
IMETHOD VectorVel & operator=(const VectorVel &arg)
Definition: framevel.inl:288
IMETHOD friend bool Equal(const VectorVel &r1, const Vector &r2, double eps)
IMETHOD friend doubleVel dot(const VectorVel &lhs, const VectorVel &rhs)
IMETHOD friend VectorVel operator*(const VectorVel &r1, const VectorVel &r2)
IMETHOD friend bool Equal(const VectorVel &r1, const VectorVel &r2, double eps)
IMETHOD VectorVel & operator-=(const VectorVel &arg)
Definition: framevel.inl:303
IMETHOD void ReverseSign()
Definition: framevel.inl:312
IMETHOD doubleVel Norm() const
Definition: framevel.inl:316
IMETHOD VectorVel & operator+=(const VectorVel &arg)
Definition: framevel.inl:298
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:143
#define rot(x, k)
#define F
#define R
static unsigned a[3]
Definition: RandGen.cpp:78
Definition: chain.cpp:27
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
Rall1d< double > doubleVel
Definition: framevel.hpp:34
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:44
IMETHOD Vector addDelta(const Vector &a, const Vector &da, double dt=1)
IMETHOD void posrandom(doubleVel &F)
Definition: framevel.hpp:48
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cpp:22
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
const btScalar eps
Definition: poly34.cpp:11
KDL::doubleVel derivType
Definition: framevel.hpp:58
Traits are traits classes to determine the type of a derivative of another type.
Definition: traits.h:41
#define IMETHOD
Definition: utility.h:43