24VectorAcc
operator + (
const VectorAcc& r1,
const VectorAcc& r2) {
25 return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv);
28VectorAcc
operator - (
const VectorAcc& r1,
const VectorAcc& r2) {
29 return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv);
31VectorAcc
operator + (
const Vector& r1,
const VectorAcc& r2) {
32 return VectorAcc(r1+r2.p,r2.v,r2.dv);
35VectorAcc
operator - (
const Vector& r1,
const VectorAcc& r2) {
36 return VectorAcc(r1-r2.p, -r2.v, -r2.dv);
38VectorAcc
operator + (
const VectorAcc& r1,
const Vector& r2) {
39 return VectorAcc(r1.p+r2,r1.v,r1.dv);
42VectorAcc
operator - (
const VectorAcc& r1,
const Vector& r2) {
43 return VectorAcc(r1.p-r2, r1.v, r1.dv);
48 return VectorAcc(-r.p,-r.v,-r.dv);
52VectorAcc
operator * (
const VectorAcc& r1,
const VectorAcc& r2) {
53 return VectorAcc(r1.p*r2.p,
55 r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv
59VectorAcc
operator * (
const VectorAcc& r1,
const Vector& r2) {
60 return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
63VectorAcc
operator * (
const Vector& r1,
const VectorAcc& r2) {
64 return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
71 return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
75 return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
78VectorAcc
operator * (
const doubleAcc& r1,
const VectorAcc& r2) {
79 return VectorAcc(r1.t*r2.p,
80 r1.t*r2.v + r1.d*r2.p,
81 r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
85VectorAcc
operator * (
const VectorAcc& r2,
const doubleAcc& r1) {
86 return VectorAcc(r1.t*r2.p,
87 r1.t*r2.v + r1.d*r2.p,
88 r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
92VectorAcc& VectorAcc::operator = (
const VectorAcc& arg) {
99VectorAcc& VectorAcc::operator = (
const Vector& arg) {
106VectorAcc& VectorAcc::operator += (
const VectorAcc& arg) {
112VectorAcc& VectorAcc::operator -= (
const VectorAcc& arg) {
119VectorAcc VectorAcc::Zero() {
120 return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero());
123void VectorAcc::ReverseSign() {
132 res.d =
dot(p,v)/res.t;
133 res.dd = (
dot(p,dv)+
dot(v,v)-res.d*res.d)/res.t;
137doubleAcc
dot(
const VectorAcc& lhs,
const VectorAcc& rhs) {
138 return doubleAcc(
dot(lhs.p,rhs.p),
139 dot(lhs.p,rhs.v)+
dot(lhs.v,rhs.p),
140 dot(lhs.p,rhs.dv)+2*
dot(lhs.v,rhs.v)+
dot(lhs.dv,rhs.p)
144doubleAcc
dot(
const VectorAcc& lhs,
const Vector& rhs) {
145 return doubleAcc(
dot(lhs.p,rhs),
151doubleAcc
dot(
const Vector& lhs,
const VectorAcc& rhs) {
152 return doubleAcc(
dot(lhs,rhs.p),
159bool Equal(
const VectorAcc& r1,
const VectorAcc& r2,
double eps) {
160 return (
Equal(r1.p,r2.p,eps)
161 &&
Equal(r1.v,r2.v,eps)
162 &&
Equal(r1.dv,r2.dv,eps)
166bool Equal(
const Vector& r1,
const VectorAcc& r2,
double eps) {
167 return (
Equal(r1,r2.p,eps)
168 &&
Equal(Vector::Zero(),r2.v,eps)
169 &&
Equal(Vector::Zero(),r2.dv,eps)
173bool Equal(
const VectorAcc& r1,
const Vector& r2,
double eps) {
174 return (
Equal(r1.p,r2,eps)
175 &&
Equal(r1.v,Vector::Zero(),eps)
176 &&
Equal(r1.dv,Vector::Zero(),eps)
184VectorAcc
operator / (
const VectorAcc& r2,
const doubleAcc& r1) {
192RotationAcc
operator* (
const RotationAcc& r1,
const RotationAcc& r2) {
193 return RotationAcc( r1.R * r2.R,
195 r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw
199RotationAcc
operator* (
const Rotation& r1,
const RotationAcc& r2) {
200 return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw);
203RotationAcc
operator* (
const RotationAcc& r1,
const Rotation& r2) {
204 return RotationAcc( r1.R*r2, r1.w, r1.dw );
207RotationAcc& RotationAcc::operator = (
const RotationAcc& arg) {
213RotationAcc& RotationAcc::operator = (
const Rotation& arg) {
220RotationAcc RotationAcc::Identity() {
221 return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero());
224RotationAcc RotationAcc::Inverse()
const {
225 return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw));
228VectorAcc RotationAcc::Inverse(
const VectorAcc& arg)
const {
230 tmp.p = R.Inverse(arg.p);
231 tmp.v = R.Inverse(arg.v - w * arg.p);
232 tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v));
236VectorAcc RotationAcc::Inverse(
const Vector& arg)
const {
238 tmp.p = R.Inverse(arg);
239 tmp.v = R.Inverse(-w*arg);
240 tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v));
245VectorAcc RotationAcc::operator*(
const VectorAcc& arg)
const {
249 tmp.v = w*tmp.p + tmp.dv;
250 tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv;
254VectorAcc
operator*(
const Rotation& R,
const VectorAcc& x) {
255 return VectorAcc(R*x.p,R*x.v,R*x.dv);
258VectorAcc RotationAcc::operator*(
const Vector& arg)
const {
262 tmp.dv = dw*tmp.p + w*tmp.v;
323bool Equal(
const RotationAcc& r1,
const RotationAcc& r2,
double eps) {
324 return (
Equal(r1.w,r2.w,eps) &&
Equal(r1.R,r2.R,eps) &&
Equal(r1.dw,r2.dw,eps) );
326bool Equal(
const Rotation& r1,
const RotationAcc& r2,
double eps) {
327 return (
Equal(Vector::Zero(),r2.w,eps) &&
Equal(r1,r2.R,eps) &&
328 Equal(Vector::Zero(),r2.dw,eps) );
330bool Equal(
const RotationAcc& r1,
const Rotation& r2,
double eps) {
331 return (
Equal(r1.w,Vector::Zero(),eps) &&
Equal(r1.R,r2,eps) &&
332 Equal(r1.dw,Vector::Zero(),eps) );
338FrameAcc& FrameAcc::operator = (
const FrameAcc& arg) {
344FrameAcc FrameAcc::Identity() {
345 return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero());
351 return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
355 return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
359 return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
362VectorAcc FrameAcc::operator *(
const VectorAcc & arg)
const
366VectorAcc FrameAcc::operator *(
const Vector & arg)
const
371VectorAcc FrameAcc::Inverse(
const VectorAcc& arg)
const
373 return M.Inverse(arg-p);
376VectorAcc FrameAcc::Inverse(
const Vector& arg)
const
378 return M.Inverse(arg-p);
381FrameAcc FrameAcc::Inverse()
const
383 return FrameAcc(M.Inverse(),-M.Inverse(p));
386FrameAcc& FrameAcc::operator =(
const Frame & arg)
393bool Equal(
const FrameAcc& r1,
const FrameAcc& r2,
double eps) {
394 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
396bool Equal(
const Frame& r1,
const FrameAcc& r2,
double eps) {
397 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
399bool Equal(
const FrameAcc& r1,
const Frame& r2,
double eps) {
400 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
404Frame FrameAcc::GetFrame()
const {
405 return Frame(M.R,p.p);
409Twist FrameAcc::GetTwist()
const {
410 return Twist(p.v,M.w);
414Twist FrameAcc::GetAccTwist()
const {
415 return Twist(p.dv,M.dw);
434TwistAcc TwistAcc::Zero()
436 return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
440void TwistAcc::ReverseSign()
446TwistAcc TwistAcc::RefPoint(
const VectorAcc& v_base_AB)
453 return TwistAcc(this->vel+this->rot*v_base_AB,this->rot);
456TwistAcc& TwistAcc::operator-=(
const TwistAcc& arg)
463TwistAcc& TwistAcc::operator+=(
const TwistAcc& arg)
473 return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
478 return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
483 return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
487TwistAcc
operator*(
const TwistAcc& lhs,
const doubleAcc& rhs)
489 return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
492TwistAcc
operator*(
const doubleAcc& lhs,
const TwistAcc& rhs)
494 return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
497TwistAcc
operator/(
const TwistAcc& lhs,
const doubleAcc& rhs)
499 return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
505TwistAcc
operator+(
const TwistAcc& lhs,
const TwistAcc& rhs)
507 return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
510TwistAcc
operator-(
const TwistAcc& lhs,
const TwistAcc& rhs)
512 return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
518 return TwistAcc(-arg.vel,-arg.rot);
525TwistAcc RotationAcc::Inverse(
const TwistAcc& arg)
const
527 return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
530TwistAcc RotationAcc::operator * (
const TwistAcc& arg)
const
532 return TwistAcc((*
this)*arg.vel,(*
this)*arg.rot);
535TwistAcc RotationAcc::Inverse(
const Twist& arg)
const
537 return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
540TwistAcc RotationAcc::operator * (
const Twist& arg)
const
542 return TwistAcc((*
this)*arg.vel,(*
this)*arg.rot);
546TwistAcc FrameAcc::operator * (
const TwistAcc& arg)
const
550 tmp.vel = M*arg.vel+p*tmp.rot;
554TwistAcc FrameAcc::operator * (
const Twist& arg)
const
558 tmp.vel = M*arg.vel+p*tmp.rot;
562TwistAcc FrameAcc::Inverse(
const TwistAcc& arg)
const
565 tmp.rot = M.Inverse(arg.rot);
566 tmp.vel = M.Inverse(arg.vel-p*arg.rot);
570TwistAcc FrameAcc::Inverse(
const Twist& arg)
const
573 tmp.rot = M.Inverse(arg.rot);
574 tmp.vel = M.Inverse(arg.vel-p*arg.rot);
578Twist TwistAcc::GetTwist()
const {
579 return Twist(vel.p,rot.p);
582Twist TwistAcc::GetTwistDot()
const {
583 return Twist(vel.v,rot.v);
586bool Equal(
const TwistAcc& a,
const TwistAcc& b,
double eps) {
587 return (
Equal(a.rot,b.rot,eps)&&
588 Equal(a.vel,b.vel,eps) );
590bool Equal(
const Twist& a,
const TwistAcc& b,
double eps) {
591 return (
Equal(a.rot,b.rot,eps)&&
592 Equal(a.vel,b.vel,eps) );
594bool Equal(
const TwistAcc& a,
const Twist& b,
double eps) {
595 return (
Equal(a.rot,b.rot,eps)&&
596 Equal(a.vel,b.vel,eps) );
bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps)
Definition frameacc.inl:159
VectorAcc operator*(const VectorAcc &r1, const VectorAcc &r2)
Definition frameacc.inl:52
VectorAcc operator-(const VectorAcc &r1, const VectorAcc &r2)
Definition frameacc.inl:28
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition frameacc.inl:180
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition frameacc.inl:137
VectorAcc operator+(const VectorAcc &r1, const VectorAcc &r2)
Definition frameacc.inl:24
Rall2d< double, double, double > doubleAcc
Definition frameacc.hpp:41