22 FrameVel& FrameVel::operator = (
const FrameVel& arg) {
28 FrameVel FrameVel::Identity() {
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 );
55 VectorVel FrameVel::Inverse(
const VectorVel& arg)
const 57 return M.Inverse(arg-p);
60 VectorVel FrameVel::Inverse(
const Vector& arg)
const 62 return M.Inverse(arg-p);
65 FrameVel FrameVel::Inverse()
const 67 return FrameVel(M.Inverse(),-M.Inverse(p));
70 FrameVel& FrameVel::operator = (
const Frame& arg) {
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 &&
120 Frame FrameVel::GetFrame()
const {
121 return Frame(M.R,p.p);
124 Twist FrameVel::GetTwist()
const {
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 );
141 RotationVel& RotationVel::operator = (
const RotationVel& arg) {
146 RotationVel& RotationVel::operator = (
const Rotation& arg) {
152 VectorVel RotationVel::UnitX()
const {
153 return VectorVel(R.UnitX(),w*R.UnitX());
156 VectorVel RotationVel::UnitY()
const {
157 return VectorVel(R.UnitY(),w*R.UnitY());
160 VectorVel RotationVel::UnitZ()
const {
161 return VectorVel(R.UnitZ(),w*R.UnitZ());
166 RotationVel RotationVel::Identity() {
167 return RotationVel(Rotation::Identity(),Vector::Zero());
170 RotationVel RotationVel::Inverse()
const {
171 return RotationVel(R.Inverse(),-R.Inverse(w));
174 VectorVel RotationVel::Inverse(
const VectorVel& arg)
const {
175 Vector tmp=R.Inverse(arg.p);
176 return VectorVel(tmp,
177 R.Inverse(arg.v-w*arg.p)
181 VectorVel RotationVel::Inverse(
const Vector& arg)
const {
182 Vector tmp=R.Inverse(arg);
183 return VectorVel(tmp,
191 return VectorVel(tmp,w*tmp+R*arg.v);
196 return VectorVel(tmp,w*tmp);
204 void RotationVel::DoRotX(
const doubleVel& angle) {
205 w+=R*Vector(angle.grad,0,0);
208 RotationVel RotationVel::RotX(
const doubleVel& angle) {
209 return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0));
212 void RotationVel::DoRotY(
const doubleVel& angle) {
213 w+=R*Vector(0,angle.grad,0);
216 RotationVel RotationVel::RotY(
const doubleVel& angle) {
217 return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0));
220 void RotationVel::DoRotZ(
const doubleVel& angle) {
221 w+=R*Vector(0,0,angle.grad);
224 RotationVel RotationVel::RotZ(
const doubleVel& angle) {
225 return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad));
235 return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad);
238 RotationVel RotationVel::Rot2(
const Vector& rotvec,
const doubleVel& angle)
241 return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
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);
325 VectorVel& VectorVel::operator = (
const VectorVel& arg) {
330 VectorVel& VectorVel::operator = (
const Vector& arg) {
335 VectorVel& VectorVel::operator += (
const VectorVel& arg) {
340 VectorVel& VectorVel::operator -= (
const VectorVel& arg) {
346 VectorVel VectorVel::Zero() {
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 &&
505 TwistVel TwistVel::Zero()
507 return TwistVel(VectorVel::Zero(),VectorVel::Zero());
511 void TwistVel::ReverseSign()
517 TwistVel TwistVel::RefPoint(
const VectorVel& v_base_AB)
524 return TwistVel(this->vel+this->rot*v_base_AB,this->rot);
527 TwistVel& TwistVel::operator-=(
const TwistVel& arg)
534 TwistVel& TwistVel::operator+=(
const TwistVel& arg)
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);
602 TwistVel RotationVel::Inverse(
const TwistVel& arg)
const 604 return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
609 return TwistVel((*
this)*arg.vel,(*
this)*arg.rot);
612 TwistVel RotationVel::Inverse(
const Twist& arg)
const 614 return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
619 return TwistVel((*
this)*arg.vel,(*
this)*arg.rot);
627 tmp.vel = M*arg.vel+p*tmp.rot;
635 tmp.vel = M*arg.vel+p*tmp.rot;
639 TwistVel FrameVel::Inverse(
const TwistVel& arg)
const 642 tmp.rot = M.Inverse(arg.rot);
643 tmp.vel = M.Inverse(arg.vel-p*arg.rot);
647 TwistVel FrameVel::Inverse(
const Twist& arg)
const 650 tmp.rot = M.Inverse(arg.rot);
651 tmp.vel = M.Inverse(arg.vel-p*arg.rot);
655 Twist TwistVel::GetTwist()
const {
656 return Twist(vel.p,rot.p);
659 Twist TwistVel::GetTwistDot()
const {
660 return Twist(vel.v,rot.v);
FrameVel operator*(const FrameVel &lhs, const FrameVel &rhs)
void SetToZero(VectorVel &v)
INLINE S Norm(const Rall1d< T, V, S > &value)
bool Equal(const FrameVel &r1, const FrameVel &r2, double eps)
IMETHOD doubleVel dot(const VectorVel &lhs, const VectorVel &rhs)
VectorVel operator/(const VectorVel &r1, double r2)
bool operator!=(const FrameVel &r1, const FrameVel &r2)
VectorVel operator+(const VectorVel &r1, const VectorVel &r2)
bool operator==(const FrameVel &r1, const FrameVel &r2)
VectorVel operator-(const VectorVel &r1, const VectorVel &r2)
IMETHOD Rotation Rot(const Vector &axis_a_b)
Rall1d< double > doubleVel