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
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 );
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;
254VectorAcc
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;
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) );
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));
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));
405 return Frame(M.R,p.p);
410 return Twist(p.v,M.w);
414Twist FrameAcc::GetAccTwist()
const {
415 return Twist(p.dv,M.dw);
436 return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
440void 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);
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);
565 tmp.
rot = M.Inverse(arg.
rot);
573 tmp.
rot = M.Inverse(arg.
rot);
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) );
Definition frameacc.hpp:166
RotationAcc M
Rotation,angular velocity, and angular acceleration of frame.
Definition frameacc.hpp:168
VectorAcc p
Translation, velocity and acceleration of origin.
Definition frameacc.hpp:169
Definition frames.hpp:570
Rotation M
Orientation of the Frame.
Definition frames.hpp:573
Vector p
origine of the Frame
Definition frames.hpp:572
Definition frameacc.hpp:111
Vector dw
angular acceration vector
Definition frameacc.hpp:115
Vector w
angular velocity vector
Definition frameacc.hpp:114
Rotation R
rotation matrix
Definition frameacc.hpp:113
represents rotations in 3 dimensional space.
Definition frames.hpp:302
Definition frameacc.hpp:210
VectorAcc rot
rotational velocity and its 1st and 2nd derivative
Definition frameacc.hpp:213
VectorAcc vel
translational velocity and its 1st and 2nd derivative
Definition frameacc.hpp:212
represents both translational and rotational velocities.
Definition frames.hpp:720
Vector rot
The rotational velocity of that point.
Definition frames.hpp:723
Vector vel
The velocity of that point.
Definition frames.hpp:722
Definition frameacc.hpp:62
Vector p
position vector
Definition frameacc.hpp:64
Vector dv
acceleration vector
Definition frameacc.hpp:66
Vector v
velocity vector
Definition frameacc.hpp:65
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:161
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