29 return FrameVel(RotationVel::Identity(),VectorVel::Zero());
33 FrameVel
operator *(
const FrameVel& lhs,
const FrameVel& rhs)
35 return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
37 FrameVel
operator *(
const FrameVel& lhs,
const Frame& rhs)
39 return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
41 FrameVel
operator *(
const Frame& lhs,
const FrameVel& rhs)
43 return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p );
57 return M.Inverse(arg-p);
62 return M.Inverse(arg-p);
67 return FrameVel(M.Inverse(),-M.Inverse(p));
75 bool Equal(
const FrameVel& r1,
const FrameVel& r2,
double eps) {
76 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
78 bool Equal(
const Frame& r1,
const FrameVel& r2,
double eps) {
79 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
81 bool Equal(
const FrameVel& r1,
const Frame& r2,
double eps) {
82 return (
Equal(r1.M,r2.M,eps) &&
Equal(r1.p,r2.p,eps));
88 return (r1.p == r2.p &&
99 return (r1.p == r2.p &&
108 return Equal(r1, r2);
110 return (r1.p == r2.p &&
121 return Frame(M.R,p.p);
125 return Twist(p.v,M.w);
129 RotationVel
operator* (
const RotationVel& r1,
const RotationVel& r2) {
130 return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w );
133 RotationVel
operator* (
const Rotation& r1,
const RotationVel& r2) {
134 return RotationVel( r1*r2.R, r1*r2.w );
137 RotationVel
operator* (
const RotationVel& r1,
const Rotation& r2) {
138 return RotationVel( r1.R*r2, r1.w );
167 return RotationVel(Rotation::Identity(),Vector::Zero());
177 R.Inverse(arg.
v-w*arg.
p)
182 Vector tmp=R.Inverse(arg);
245 VectorVel
operator + (
const VectorVel& r1,
const VectorVel& r2) {
246 return VectorVel(r1.p+r2.p,r1.v+r2.v);
249 VectorVel
operator - (
const VectorVel& r1,
const VectorVel& r2) {
250 return VectorVel(r1.p-r2.p,r1.v-r2.v);
253 VectorVel
operator + (
const VectorVel& r1,
const Vector& r2) {
254 return VectorVel(r1.p+r2,r1.v);
257 VectorVel
operator - (
const VectorVel& r1,
const Vector& r2) {
258 return VectorVel(r1.p-r2,r1.v);
261 VectorVel
operator + (
const Vector& r1,
const VectorVel& r2) {
262 return VectorVel(r1+r2.p,r2.v);
265 VectorVel
operator - (
const Vector& r1,
const VectorVel& r2) {
266 return VectorVel(r1-r2.p,-r2.v);
271 return VectorVel(-r.p,-r.v);
280 VectorVel
operator * (
const VectorVel& r1,
const VectorVel& r2) {
281 return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p);
284 VectorVel
operator * (
const VectorVel& r1,
const Vector& r2) {
285 return VectorVel(r1.p*r2, r1.v*r2);
288 VectorVel
operator * (
const Vector& r1,
const VectorVel& r2) {
289 return VectorVel(r1*r2.p, r1*r2.v);
296 return VectorVel(r1*r2.p, r1*r2.v);
300 return VectorVel(r1.p*r2, r1.v*r2);
306 return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
310 return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
314 return VectorVel(r1.p/r2, r1.v/r2);
318 return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t);
321 VectorVel
operator*(
const Rotation& R,
const VectorVel& x) {
322 return VectorVel(R*x.p,R*x.v);
347 return VectorVel(Vector::Zero(),Vector::Zero());
349 void VectorVel::ReverseSign() {
354 double n = p.Norm(eps);
360 bool Equal(
const VectorVel& r1,
const VectorVel& r2,
double eps) {
361 return (
Equal(r1.p,r2.p,eps) &&
Equal(r1.v,r2.v,eps));
363 bool Equal(
const Vector& r1,
const VectorVel& r2,
double eps) {
364 return (
Equal(r1,r2.p,eps) &&
Equal(Vector::Zero(),r2.v,eps));
366 bool Equal(
const VectorVel& r1,
const Vector& r2,
double eps) {
367 return (
Equal(r1.p,r2,eps) &&
Equal(r1.v,Vector::Zero(),eps));
371 return Equal(r1, r2);
373 return (r1.p == r2.p &&
382 return Equal(r1, r2);
384 return (r1 == r2.p &&
385 Vector::Zero() == r2.v);
393 return Equal(r1, r2);
395 return (r1.p == r2 &&
396 r1.v == Vector::Zero() );
404 bool Equal(
const RotationVel& r1,
const RotationVel& r2,
double eps) {
405 return (
Equal(r1.w,r2.w,eps) &&
Equal(r1.R,r2.R,eps));
407 bool Equal(
const Rotation& r1,
const RotationVel& r2,
double eps) {
408 return (
Equal(Vector::Zero(),r2.w,eps) &&
Equal(r1,r2.R,eps));
410 bool Equal(
const RotationVel& r1,
const Rotation& r2,
double eps) {
411 return (
Equal(r1.w,Vector::Zero(),eps) &&
Equal(r1.R,r2,eps));
413 bool operator==(
const RotationVel& r1,
const RotationVel& r2) {
415 return Equal(r1, r2);
417 return (r1.w == r2.w &&
421 bool operator!=(
const RotationVel& r1,
const RotationVel& r2) {
426 return Equal(r1, r2);
428 return (Vector::Zero() == r2.w &&
437 return Equal(r1, r2);
439 return (r1.w == Vector::Zero() &&
448 bool Equal(
const TwistVel& a,
const TwistVel& b,
double eps) {
449 return (
Equal(a.rot,b.rot,eps)&&
450 Equal(a.vel,b.vel,eps) );
452 bool Equal(
const Twist& a,
const TwistVel& b,
double eps) {
453 return (
Equal(a.rot,b.rot,eps)&&
454 Equal(a.vel,b.vel,eps) );
456 bool Equal(
const TwistVel& a,
const Twist& b,
double eps) {
457 return (
Equal(a.rot,b.rot,eps)&&
458 Equal(a.vel,b.vel,eps) );
464 return (a.rot == b.rot &&
475 return (a.rot == b.rot &&
484 return Equal(r1, r2);
486 return (a.rot == b.rot &&
507 return TwistVel(VectorVel::Zero(),VectorVel::Zero());
511 void TwistVel::ReverseSign()
524 return TwistVel(this->vel+this->rot*v_base_AB,this->rot);
544 return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
549 return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
554 return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
560 return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
565 return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
570 return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
576 TwistVel
operator+(
const TwistVel& lhs,
const TwistVel& rhs)
578 return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
581 TwistVel
operator-(
const TwistVel& lhs,
const TwistVel& rhs)
583 return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
589 return TwistVel(-arg.vel,-arg.rot);
642 tmp.
rot = M.Inverse(arg.
rot);
650 tmp.
rot = M.Inverse(arg.
rot);
656 return Twist(vel.p,rot.p);
659 Twist TwistVel::GetTwistDot()
const {
660 return Twist(vel.v,rot.v);