24 VectorAcc
operator + (
const VectorAcc& r1,
const VectorAcc& r2) {
25 return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv);
28 VectorAcc
operator - (
const VectorAcc& r1,
const VectorAcc& r2) {
29 return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv);
31 VectorAcc
operator + (
const Vector& r1,
const VectorAcc& r2) {
32 return VectorAcc(r1+r2.p,r2.v,r2.dv);
35 VectorAcc
operator - (
const Vector& r1,
const VectorAcc& r2) {
36 return VectorAcc(r1-r2.p, -r2.v, -r2.dv);
38 VectorAcc
operator + (
const VectorAcc& r1,
const Vector& r2) {
39 return VectorAcc(r1.p+r2,r1.v,r1.dv);
42 VectorAcc
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);
52 VectorAcc
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
59 VectorAcc
operator * (
const VectorAcc& r1,
const Vector& r2) {
60 return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
63 VectorAcc
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 );
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
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
120 return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero());
123 void VectorAcc::ReverseSign() {
132 res.
d =
dot(p,v)/res.
t;
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)
159 bool 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)
166 bool 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)
173 bool 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)
192 RotationAcc
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
199 RotationAcc
operator* (
const Rotation& r1,
const RotationAcc& r2) {
200 return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw);
203 RotationAcc
operator* (
const RotationAcc& r1,
const Rotation& r2) {
204 return RotationAcc( r1.R*r2, r1.w, r1.dw );
221 return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero());
225 return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw));
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));
238 tmp.
p = R.Inverse(arg);
239 tmp.
v = R.Inverse(-w*arg);
240 tmp.
dv = R.Inverse(-dw*arg - w*(R*tmp.
v));
249 tmp.
v = w*tmp.
p + tmp.
dv;
250 tmp.
dv = dw*tmp.
p + w*(tmp.
v + tmp.
dv) + R*arg.
dv;
254 VectorAcc
operator*(
const Rotation& R,
const VectorAcc& x) {
255 return VectorAcc(R*x.p,R*x.v,R*x.dv);
262 tmp.
dv = dw*tmp.
p + w*tmp.
v;
323 bool 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) );
326 bool 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) );
330 bool 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) );
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);
373 return M.Inverse(arg-p);
378 return M.Inverse(arg-p);
383 return FrameAcc(M.Inverse(),-M.Inverse(p));
393 bool Equal(
const FrameAcc& r1,
const FrameAcc& r2,
double eps) {
394 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
396 bool Equal(
const Frame& r1,
const FrameAcc& r2,
double eps) {
397 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
399 bool Equal(
const FrameAcc& r1,
const Frame& r2,
double eps) {
400 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
405 return Frame(M.R,p.p);
410 return Twist(p.v,M.w);
414 Twist FrameAcc::GetAccTwist()
const {
415 return Twist(p.dv,M.dw);
436 return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
440 void TwistAcc::ReverseSign()
453 return TwistAcc(this->vel+this->rot*v_base_AB,this->rot);
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);
489 return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
494 return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
499 return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
505 TwistAcc
operator+(
const TwistAcc& lhs,
const TwistAcc& rhs)
507 return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
510 TwistAcc
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);
565 tmp.
rot = M.Inverse(arg.
rot);
573 tmp.
rot = M.Inverse(arg.
rot);
579 return Twist(vel.p,rot.p);
582 Twist TwistAcc::GetTwistDot()
const {
583 return Twist(vel.v,rot.v);
586 bool Equal(
const TwistAcc& a,
const TwistAcc& b,
double eps) {
587 return (
Equal(a.rot,b.rot,eps)&&
588 Equal(a.vel,b.vel,eps) );
590 bool Equal(
const Twist& a,
const TwistAcc& b,
double eps) {
591 return (
Equal(a.rot,b.rot,eps)&&
592 Equal(a.vel,b.vel,eps) );
594 bool Equal(
const TwistAcc& a,
const Twist& b,
double eps) {
595 return (
Equal(a.rot,b.rot,eps)&&
596 Equal(a.vel,b.vel,eps) );