165 inline Vector() {data[0]=data[1]=data[2] = 0.0;}
168 inline Vector(
double x,
double y,
double z);
194 inline double x()
const;
195 inline double y()
const;
196 inline double z()
const;
197 inline void x(
double);
198 inline void y(
double);
199 inline void z(
double);
309 inline Rotation(
double Xx,
double Yx,
double Zx,
310 double Xy,
double Yy,
double Zy,
311 double Xz,
double Yz,
double Zz);
331 inline void SetInverse();
353 inline static Rotation RotY(
double angle);
355 inline static Rotation RotZ(
double angle);
358 inline void DoRotX(
double angle);
361 inline void DoRotY(
double angle);
364 inline void DoRotZ(
double angle);
398 static Rotation EulerZYZ(
double Alfa,
double Beta,
double Gamma);
415 void GetEulerZYZ(
double& alpha,
double& beta,
double& gamma)
const;
419 static Rotation Quaternion(
double x,
double y,
double z,
double w);
423 void GetQuaternion(
double& x,
double& y,
double& z,
double& w)
const;
435 static Rotation RPY(
double roll,
double pitch,
double yaw);
455 void GetRPY(
double& roll,
double& pitch,
double& yaw)
const;
470 return RPY(Gamma,Beta,Alfa);
493 inline void GetEulerZYX(
double& Alfa,
double& Beta,
double& Gamma)
const {
494 GetRPY(Gamma,Beta,Alfa);
511 return Vector(data[0],data[3],data[6]);
523 return Vector(data[1],data[4],data[7]);
535 return Vector(data[2],data[5],data[8]);
590 void Make4x4(
double* d);
602 inline Frame Inverse()
const;
640 inline static Frame Identity();
645 inline void Integrate(
const Twist& t_this,
double frequency);
693 static Frame DH_Craig1989(
double a,
double alpha,
double d,
double theta);
701 static Frame DH(
double a,
double alpha,
double d,
double theta);
776 inline Twist RefPoint(
const Vector& v_base_AB)
const;
988 inline double x()
const;
989 inline double y()
const;
990 inline void x(
double);
991 inline void y(
double);
1024 inline void Set3DYZ(
const Vector& v);
1027 inline void Set3DZX(
const Vector& v);
1032 inline void Set3DPlane(
const Frame& F_someframe_XY,
const Vector& v_someframe);
1068 inline double operator() (
int i,
int j)
const;
1072 inline void SetInverse();
1076 inline void SetIdentity();
1081 inline void SetRot(
double angle);
1087 inline double GetRot()
const;
1109 inline void Make4x4(
double* d);
1117 inline double operator() (
int i,
int j)
const;
1119 inline void SetInverse();
1120 inline Frame2 Inverse()
const;
1125 inline void SetIdentity();
1126 inline void Integrate(
const Twist& t_this,
double frequency);
1255 #include "frames.inl" Rotation2 M
Orientation of the Frame.
friend Vector operator+(const Vector &lhs, const Vector &rhs)
represents rotations in 3 dimensional space.
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
void UnitZ(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
double operator[](int index) const
Equivalent to double operator()(int index) const.
void UnitX(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
void UnitY(const Vector &X)
Access to the underlying unitvectors of the rotation matrix.
void Set2DPlane(const Frame &F_someframe_XY, const Vector2 &v_XY)
a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
IMETHOD Rotation Rot(const Vector &axis_a_b)
double Normalize(double eps=epsilon)
Vector vel
The velocity of that point.
Wrench()
Does initialise force and torque to zero via the underlying constructor of Vector.
Vector & operator-=(const Vector &arg)
subtracts a vector from the Vector object itself
friend double dot(const Vector &lhs, const Vector &rhs)
void GetEulerZYX(double &Alfa, double &Beta, double &Gamma) const
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Vector2()
Does not initialise to Zero().
Twist()
The default constructor initialises to Zero via the constructor of Vector.
Vector torque
Torque that is applied at the origin of the current ref frame.
Rotation2(double angle_rad)
void ReverseSign()
Reverses the sign of the Vector object itself.
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that. ...
friend bool operator==(const Vector &a, const Vector &b)
The literal equality operator==(), also identical.
friend bool Equal(const Vector &a, const Vector &b, double eps)
Rotation M
Orientation of the Frame.
Rotation2(double ca, double sa)
double Norm(double eps=epsilon) const
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
represents both translational and rotational velocities.
friend Vector operator*(const Vector &lhs, double rhs)
Scalar multiplication is defined.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Vector & operator+=(const Vector &arg)
Adds a vector from the Vector object itself.
A concrete implementation of a 3 dimensional vector class.
Vector rot
The rotational velocity of that point.
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..2.
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
static Rotation Identity()
Gives back an identity rotaton matrix.
Vector p
origine of the Frame
friend Vector operator-(const Vector &lhs, const Vector &rhs)
Vector force
Force that is applied at the origin of the current ref frame.
friend Vector operator/(const Vector &lhs, double rhs)
Scalar division is defined.
friend bool operator!=(const Vector &a, const Vector &b)
The literal inequality operator!=().
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
represents a frame transformation in 3D space (rotation + translation)
Twist(const Vector &_vel, const Vector &_rot)
Vector2 p
origine of the Frame
represents both translational and rotational acceleration.
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
friend void SetToZero(Vector &v)
Wrench(const Vector &_force, const Vector &_torque)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Rotation2()
Default constructor does NOT initialise to Zero().
Vector & operator=(const Vector &arg)
Assignment operator. The normal copy by value semantics.