57 static const char rcsid[] =
"$Id: homogen.cpp,v 1.15 2006/11/15 18:35:17 gourdeau Exp $";
64 using namespace NEWMAT;
77 translation(1,4) =
a(1);
78 translation(2,4) =
a(2);
79 translation(3,4) =
a(3);
82 cerr <<
"trans: wrong size in input vector." << endl;
84 translation.
Release();
return translation;
153 Real c, s, vers, kx, ky, kz;
167 rot(1,1) = kx*kx*vers+c;
168 rot(1,2) = kx*ky*vers-kz*s;
169 rot(1,3) = kx*kz*vers+ky*s;
170 rot(2,1) = kx*ky*vers+kz*s;
171 rot(2,2) = ky*ky*vers+c;
172 rot(2,3) = ky*kz*vers-kx*s;
173 rot(3,1) = kx*kz*vers-ky*s;
174 rot(3,2) = ky*kz*vers+kx*s;
175 rot(3,3) = kz*kz*vers+c;
186 Real ca, sa, cb, sb, cc, sc;
198 rot(1,2) = sa*sb*cc-ca*sc;
199 rot(1,3) = ca*sb*cc+sa*sc;
201 rot(2,2) = sa*sb*sc+ca*cc;
202 rot(2,3) = ca*sb*sc-sa*cc;
215 Real c1, s1, ca, sa, c2, s2;
226 rot(1,1) = c1*c2-s1*ca*s2;
227 rot(1,2) = -c1*s2-s1*ca*c2;
229 rot(2,1) = s1*c2+c1*ca*s2;
230 rot(2,2) = -s1*s2+c1*ca*c2;
262 k(4) = atan2(sqrt(a*a + b*b + c*c),(R(1,1) + R(2,2) + R(3,3)-1));
263 k(1) = (R(3,2)-R(2,3))/(2*sin(k(4)));
264 k(2) = (R(1,3)-R(3,1))/(2*sin(k(4)));
265 k(3) = (R(2,1)-R(1,2))/(2*sin(k(4)));
277 k(1) = atan2(-R(1,2),-R(1,3));
280 }
else if (R(3,1)==-1) {
281 k(1) = atan2(R(1,2),R(1,3));
285 k(1) = atan2(R(3,2), R(3,3));
286 k(2) = atan2(-R(3,1), sqrt(R(1,1)*R(1,1) + R(2,1)*R(2,1)));
287 k(3) = atan2(R(2,1), R(1,1));
299 if ((R(3,3)==1) || (R(3,3)==-1)) {
301 a(2) = ((R(3,3) == 1) ? 0.0 :
M_PI);
302 a(3) = atan2(R(2,1),R(1,1));
304 a(1) = atan2(R(1,3), -R(2,3));
305 a(2) = atan2(sqrt(R(1,3)*R(1,3) + R(2,3)*R(2,3)), R(3,3));
306 a(3) = atan2(R(3,1), R(3,2));
ReturnMatrix rotd(const Real theta, const ColumnVector &k1, const ColumnVector &k2)
Rotation around an arbitrary line.
ReturnMatrix trans(const ColumnVector &a)
Translation.
ReturnMatrix rpy(const ColumnVector &a)
Roll Pitch Yaw rotation.
Real SumSquare(const BaseMatrix &B)
ReturnMatrix rotx(const Real alpha)
Rotation around x axis.
ReturnMatrix irotk(const Matrix &R)
Obtain axis from a rotation matrix.
ReturnMatrix ieulzxz(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
ReturnMatrix rotz(const Real gamma)
Rotation around z axis.
The usual rectangular matrix.
FloatVector FloatVector * a
ReturnMatrix roty(const Real beta)
Rotation around x axis.
ReturnMatrix irpy(const Matrix &R)
Obtain Roll, Pitch and Yaw from a rotation matrix.
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
FloatVector FloatVector FloatVector * alpha
ReturnMatrix eulzxz(const ColumnVector &a)
Euler ZXZ rotation.
Real fourbyfourident[]
Used to initialize a matrix.
static const char rcsid[]
RCS/CVS version.
ReturnMatrix rotk(const Real theta, const ColumnVector &k)
Rotation around arbitrary axis.