89 double tmp1 = fabs(data[0]);
90 double tmp2 = fabs(data[1]);
92 if (tmp1 < eps && tmp2 < eps)
96 return tmp1*
sqrt(1+
sqr(data[1]/data[0]));
98 return tmp2*
sqrt(1+
sqr(data[0]/data[1]));
105 double v = this->
Norm();
121 tmp1 = fabs(data[0]);
122 tmp2 = fabs(data[1]);
130 return tmp1*
sqrt(1+
sqr(data[1]/data[0])+
sqr(data[2]/data[0]));
132 return tmp2*
sqrt(1+
sqr(data[0]/data[2])+
sqr(data[1]/data[2]));
137 return tmp2*
sqrt(1+
sqr(data[0]/data[1])+
sqr(data[2]/data[1]));
139 return tmp1*
sqrt(1+
sqr(data[0]/data[2])+
sqr(data[1]/data[2]));
148 double v = this->
Norm();
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);
208 if( trace > epsilon ){
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);
253 if ( fabs(pitch) > (
PI_2-epsilon) ) {
254 yaw =
atan2( -data[1], data[4]);
257 roll =
atan2(data[7], data[8]);
258 yaw =
atan2(data[3], data[0]);
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,
277 if (fabs(data[8]) > 1-epsilon ) {
281 alpha=
atan2(data[3],data[0]);
284 alpha=
atan2(-data[3],-data[0]);
287 alpha=
atan2(data[5], data[2]);
289 gamma=
atan2(data[7], -data[6]);
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;
366 if ((
std::abs(data[1] - data[3]) < epsilon)
367 && (
std::abs(data[2] - data[6])< epsilon)
368 && (
std::abs(data[5] - data[7]) < epsilon))
373 if ((
std::abs(data[1] + data[3]) < epsilon2)
374 && (
std::abs(data[2] + data[6]) < epsilon2)
375 && (
std::abs(data[5] + data[7]) < epsilon2)
376 && (
std::abs(data[0] + data[4] + data[8]-3) < epsilon2))
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))
419 double f = (data[0] + data[4] + data[8] - 1) / 2;
421 x = (data[7] - data[5]);
422 y = (data[2] - data[6]);
423 z = (data[3] - data[1]);
represents rotations in 3 dimensional space.
double Normalize(double eps=epsilon)
friend bool operator==(const Frame &a, const Frame &b)
The literal equality operator==(), also identical.
INLINE S Norm(const Rall1d< T, V, S > &value)
const double PI_2
the value of pi/2
void GetRPY(double &roll, double &pitch, double &yaw) const
double Normalize(double eps=epsilon)
const double PI
the value of pi
static Rotation Quaternion(double x, double y, double z, double w)
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Rotation M
Orientation of the Frame.
double GetRotAngle(Vector &axis, double eps=epsilon) const
static Frame DH_Craig1989(double a, double alpha, double d, double theta)
A concrete implementation of a 3 dimensional vector class.
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
static Rotation RPY(double roll, double pitch, double yaw)
static Frame DH(double a, double alpha, double d, double theta)
Vector operator*(const Vector &arg) const
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Vector p
origine of the Frame
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void Make4x4(double *d)
Reads data from an double array.
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
static Rotation EulerZYZ(double Alfa, double Beta, double Gamma)
represents a frame transformation in 3D space (rotation + translation)
void GetQuaternion(double &x, double &y, double &z, double &w) const
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
friend bool Equal(const Frame &a, const Frame &b, double eps)
static Rotation Rot(const Vector &rotvec, double angle)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
double Norm(double eps=epsilon) const
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
double Norm(double eps=epsilon) const
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)