Go to the documentation of this file.
61 return Frame(Rotation(
89 double tmp1 = fabs(
data[0]);
90 double tmp2 = fabs(
data[1]);
92 if (tmp1 < eps && tmp2 < eps)
105 double v = this->
Norm();
121 tmp1 = fabs(
data[0]);
122 tmp2 = fabs(
data[1]);
148 double v = this->
Norm();
159 bool Equal(
const Rotation& a,
const Rotation& b,
double eps) {
160 return (
Equal(a.data[0],b.data[0],eps) &&
161 Equal(a.data[1],b.data[1],eps) &&
162 Equal(a.data[2],b.data[2],eps) &&
163 Equal(a.data[3],b.data[3],eps) &&
164 Equal(a.data[4],b.data[4],eps) &&
165 Equal(a.data[5],b.data[5],eps) &&
166 Equal(a.data[6],b.data[6],eps) &&
167 Equal(a.data[7],b.data[7],eps) &&
168 Equal(a.data[8],b.data[8],eps) );
192 double x2, y2, z2, w2;
193 x2 = x*x; y2 = y*y; z2 = z*z; w2 = w*w;
194 return Rotation(w2+x2-y2-z2, 2*x*y-2*w*z, 2*x*z+2*w*y,
195 2*x*y+2*w*z, w2-x2+y2-z2, 2*y*z-2*w*x,
196 2*x*z-2*w*y, 2*y*z+2*w*x, w2-x2-y2+z2);
206 double trace = (*this)(0,0) + (*
this)(1,1) + (*
this)(2,2);
209 double s = 0.5 /
sqrt(trace + 1.0);
211 x = ( (*this)(2,1) - (*
this)(1,2) ) * s;
212 y = ( (*this)(0,2) - (*
this)(2,0) ) * s;
213 z = ( (*this)(1,0) - (*
this)(0,1) ) * s;
215 if ( (*
this)(0,0) > (*
this)(1,1) && (*
this)(0,0) > (*
this)(2,2) ){
216 double s = 2.0 *
sqrt( 1.0 + (*
this)(0,0) - (*
this)(1,1) - (*
this)(2,2));
217 w = ((*this)(2,1) - (*
this)(1,2) ) / s;
219 y = ((*this)(0,1) + (*
this)(1,0) ) / s;
220 z = ((*this)(0,2) + (*
this)(2,0) ) / s;
221 }
else if ((*
this)(1,1) > (*
this)(2,2)) {
222 double s = 2.0 *
sqrt( 1.0 + (*
this)(1,1) - (*
this)(0,0) - (*
this)(2,2));
223 w = ((*this)(0,2) - (*
this)(2,0) ) / s;
224 x = ((*this)(0,1) + (*
this)(1,0) ) / s;
226 z = ((*this)(1,2) + (*
this)(2,1) ) / s;
228 double s = 2.0 *
sqrt( 1.0 + (*
this)(2,2) - (*
this)(0,0) - (*
this)(1,1) );
229 w = ((*this)(1,0) - (*
this)(0,1) ) / s;
230 x = ((*this)(0,2) + (*
this)(2,0) ) / s;
231 y = ((*this)(1,2) + (*
this)(2,1) ) / s;
239 double ca1,cb1,cc1,sa1,sb1,sc1;
240 ca1 =
cos(yaw); sa1 =
sin(yaw);
241 cb1 =
cos(pitch);sb1 =
sin(pitch);
242 cc1 =
cos(roll);sc1 =
sin(roll);
243 return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
244 sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
245 -sb1,cb1*sc1,cb1*cc1);
263 double sa,ca,sb,cb,sg,cg;
264 sa =
sin(Alfa);ca =
cos(Alfa);
265 sb =
sin(Beta);cb =
cos(Beta);
266 sg =
sin(Gamma);cg =
cos(Gamma);
267 return Rotation( ca*cb*cg-sa*sg, -ca*cb*sg-sa*cg, ca*sb,
268 sa*cb*cg+ca*sg, -sa*cb*sg+ca*cg, sa*sb,
298 Vector rotvec = rotaxis;
309 double ct =
cos(angle);
310 double st =
sin(angle);
312 double m_vt_0=vt*rotvec(0);
313 double m_vt_1=vt*rotvec(1);
314 double m_vt_2=vt*rotvec(2);
315 double m_st_0=rotvec(0)*st;
316 double m_st_1=rotvec(1)*st;
317 double m_st_2=rotvec(2)*st;
318 double m_vt_0_1=m_vt_0*rotvec(1);
319 double m_vt_0_2=m_vt_0*rotvec(2);
320 double m_vt_1_2=m_vt_1*rotvec(2);
322 ct + m_vt_0*rotvec(0),
326 ct + m_vt_1*rotvec(1),
330 ct + m_vt_2*rotvec(2)
361 double epsilon2 = eps*10;
387 double xx = (
data[0] + 1) / 2;
388 double yy = (
data[4] + 1) / 2;
389 double zz = (
data[8] + 1) / 2;
390 double xy = (
data[1] +
data[3]) / 4;
391 double xz = (
data[2] +
data[6]) / 4;
392 double yz = (
data[5] +
data[7]) / 4;
394 if ((xx > yy) && (xx > zz))
425 angle =
atan2(axis.Norm()/2,f);
430 bool operator==(
const Rotation& a,
const Rotation& b) {
434 return ( a.data[0]==b.data[0] &&
435 a.data[1]==b.data[1] &&
436 a.data[2]==b.data[2] &&
437 a.data[3]==b.data[3] &&
438 a.data[4]==b.data[4] &&
439 a.data[5]==b.data[5] &&
440 a.data[6]==b.data[6] &&
441 a.data[7]==b.data[7] &&
442 a.data[8]==b.data[8] );
double Normalize(double eps=epsilon)
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
static Rotation Quaternion(double x, double y, double z, double w)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
Vector p
origine of the Frame
double Norm(double eps=epsilon) const
void GetRPY(double &roll, double &pitch, double &yaw) const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
double Normalize(double eps=epsilon)
void Make4x4(double *d)
Reads data from an double array.
A concrete implementation of a 3 dimensional vector class.
bool operator==(const Rotation &a, const Rotation &b)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
const double PI_2
the value of pi/2
void GetQuaternion(double &x, double &y, double &z, double &w) const
represents a frame transformation in 3D space (rotation + translation)
const double PI
the value of pi
double GetRotAngle(Vector &axis, double eps=epsilon) const
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
static Frame DH(double a, double alpha, double d, double theta)
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
static Rotation Rot(const Vector &rotvec, double angle)
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Rotation M
Orientation of the Frame.
static Rotation RPY(double roll, double pitch, double yaw)
Vector2()
Does not initialise to Zero().
double Norm(double eps=epsilon) const
Vector()
Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that.
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
represents rotations in 3 dimensional space.
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)