Go to the documentation of this file.
53 template <
class T,
class V=T,
class S=T>
105 {
t=r.t;
d=r.d;
dd=r.dd;
return *
this;}
110 d = (
d-
t*rhs.d)/rhs.t;
118 d = (
d*rhs.t+
t*rhs.d);
119 dd = (
dd*rhs.t+S(2)*
d*rhs.d+
t*rhs.dd);
225 template <
class T,
class V,
class S>
230 tmp.d = (lhs.d-tmp.t*rhs.d)/rhs.t;
231 tmp.dd= (lhs.dd-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
235 template <
class T,
class V,
class S>
236 INLINE Rall2d<T,V,S>
operator *(
const Rall2d<T,V,S>& lhs,
const Rall2d<T,V,S>& rhs)
240 tmp.d = (lhs.d*rhs.t+lhs.t*rhs.d);
241 tmp.dd = (lhs.dd*rhs.t+S(2)*lhs.d*rhs.d+lhs.t*rhs.dd);
245 template <
class T,
class V,
class S>
246 INLINE Rall2d<T,V,S>
operator +(
const Rall2d<T,V,S>& lhs,
const Rall2d<T,V,S>& rhs)
251 template <
class T,
class V,
class S>
252 INLINE Rall2d<T,V,S>
operator -(
const Rall2d<T,V,S>& lhs,
const Rall2d<T,V,S>& rhs)
254 return Rall2d<T,V,S>(lhs.t-rhs.t,lhs.d-rhs.d,lhs.dd-rhs.dd);
257 template <
class T,
class V,
class S>
263 template <
class T,
class V,
class S>
266 return Rall2d<T,V,S>(s*v.t,s*v.d,s*v.dd);
269 template <
class T,
class V,
class S>
275 template <
class T,
class V,
class S>
281 template <
class T,
class V,
class S>
287 template <
class T,
class V,
class S>
293 template <
class T,
class V,
class S>
299 template <
class T,
class V,
class S>
304 tmp.d = (-tmp.t*rhs.d)/rhs.t;
305 tmp.dd= (-S(2)*tmp.d*rhs.d-tmp.t*rhs.dd)/rhs.t;
310 template <
class T,
class V,
class S>
317 template <
class T,
class V,
class S>
323 tmp.dd = tmp.d*arg.d+tmp.t*arg.dd;
327 template <
class T,
class V,
class S>
328 INLINE Rall2d<T,V,S>
log(
const Rall2d<T,V,S>& arg)
333 tmp.dd = (arg.dd-tmp.d*arg.d)/arg.t;
337 template <
class T,
class V,
class S>
338 INLINE Rall2d<T,V,S>
sin(
const Rall2d<T,V,S>& arg)
342 return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd - (v1*arg.d)*arg.d );
345 template <
class T,
class V,
class S>
346 INLINE Rall2d<T,V,S>
cos(
const Rall2d<T,V,S>& arg)
353 template <
class T,
class V,
class S>
354 INLINE Rall2d<T,V,S>
tan(
const Rall2d<T,V,S>& arg)
358 return Rall2d<T,V,S>(v1,v2*arg.d, v2*(arg.dd+(S(2)*v1*
sqr(arg.d))));
361 template <
class T,
class V,
class S>
366 return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
369 template <
class T,
class V,
class S>
374 return Rall2d<T,V,S>(v1,v2*arg.d,v2*arg.dd + (v1*arg.d)*arg.d );
377 template <
class T,
class V,
class S>
385 template <
class T,
class V,
class S>
390 S(2)*(
sqr(arg.d)+arg.t*arg.dd)
394 template <
class T,
class V,
class S>
395 INLINE Rall2d<T,V,S>
pow(
const Rall2d<T,V,S>& arg,
double m)
398 tmp.t =
pow(arg.t,m);
399 T v2 = (m/arg.t)*tmp.t;
401 tmp.dd = (S((m-1))/arg.t)*tmp.d*arg.d + v2*arg.dd;
405 template <
class T,
class V,
class S>
406 INLINE Rall2d<T,V,S>
sqrt(
const Rall2d<T,V,S>& arg)
411 tmp.d = (S(0.5)/tmp.t)*arg.d;
412 tmp.dd = (S(0.5)*arg.dd-
sqr(tmp.d))/tmp.t;
416 template <
class T,
class V,
class S>
424 tmp.dd = (arg.dd+arg.t*
sqr(tmp.d))/v;
428 template <
class T,
class V,
class S>
436 tmp.dd = (arg.dd+arg.t*
sqr(tmp.d))/v;
441 template <
class T,
class V,
class S>
442 INLINE Rall2d<T,V,S>
atan(
const Rall2d<T,V,S>& x)
449 tmp.dd = x.dd/v-(S(2)*x.t)*
sqr(tmp.d);
453 template <
class T,
class V,
class S>
454 INLINE Rall2d<T,V,S>
atan2(
const Rall2d<T,V,S>& y,
const Rall2d<T,V,S>& x)
457 tmp.t =
atan2(y.t,x.t);
459 tmp.d = (x.t*y.d-x.d*y.t)/v;
460 tmp.dd = ( x.t*y.dd-x.dd*y.t-S(2)*(x.t*x.d+y.t*y.d)*tmp.d ) / v;
464 template <
class T,
class V,
class S>
471 template <
class T,
class V,
class S>
472 INLINE Rall2d<T,V,S>
hypot(
const Rall2d<T,V,S>& y,
const Rall2d<T,V,S>& x)
475 tmp.t =
hypot(y.t,x.t);
476 tmp.d = (x.t*x.d+y.t*y.d)/tmp.t;
477 tmp.dd = (
sqr(x.d)+x.t*x.dd+
sqr(y.d)+y.t*y.dd-
sqr(tmp.d))/tmp.t;
482 template <
class T,
class V,
class S>
485 return Norm(value.t);
491 template <
class T,
class V,
class S>
501 template <
class T,
class V,
class S>
503 const T& beta,
const Rall2d<T,V,S>& b,Rall2d<T,V,S>& result ) {
504 LinCombR(alfa, a.t, beta, b.t, result.t);
506 LinCombR(alfa, a.dd, beta, b.dd, result.dd);
509 template <
class T,
class V,
class S>
517 template <
class T,
class V,
class S>
525 template <
class T,
class V,
class S>
528 return (
Equal(x.t,y.t,eps)&&
534 template <
class T,
class V,
class S>
540 return (x.t == y.t &&
547 template <
class T,
class V,
class S>
INLINE void SetToIdentity(Rall1d< T, V, S > &value)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
INLINE Rall2d< T, V, S > & operator*=(const Rall2d< T, V, S > &rhs)
INLINE Rall2d< T, V, S > & operator/=(const Rall2d< T, V, S > &rhs)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall2d< T, V, S > & operator=(S c)
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
static INLINE Rall2d< T, V, S > Identity()
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > operator/(const Rall1d< T, V, S > &lhs, const Rall1d< T, V, S > &rhs)
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
bool operator==(const Rotation &a, const Rotation &b)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall2d< T, V, S > & operator-=(const Rall2d< T, V, S > &rhs)
INLINE S Norm(const Rall1d< T, V, S > &value)
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
INLINE Rall2d< T, V, S > & operator+=(const Rall2d< T, V, S > &rhs)
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > LinComb(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b)
INLINE bool operator!=(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
INLINE void LinCombR(S alfa, const Rall1d< T, V, S > &a, const T &beta, const Rall1d< T, V, S > &b, Rall1d< T, V, S > &result)
INLINE Rall1d< T, V, S > sinh(const Rall1d< T, V, S > &arg)
const typedef T & Arg
Arg is used for passing the element to a function.
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
static INLINE Rall2d< T, V, S > Zero()
INLINE Rall1d< T, V, S > cosh(const Rall1d< T, V, S > &arg)
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
void SetToZero(Jacobian &jac)
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > tanh(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)