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.
double GetRotAngle(Vector &axis, double eps=epsilon) const
INLINE S Norm(const Rall1d< T, V, S > &value)
const double PI_2
the value of pi/2
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.
void GetQuaternion(double &x, double &y, double &z, double &w) const
double Norm(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.
static Rotation RPY(double roll, double pitch, double yaw)
static Frame DH(double a, double alpha, double d, double theta)
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.
double Norm(double eps=epsilon) const
void GetEulerZYZ(double &alpha, double &beta, double &gamma) const
void GetRPY(double &roll, double &pitch, double &yaw) const
Vector operator*(const Vector &arg) const
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)
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)
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)