20 #ifndef _ROTATION_ABOUT_AXES_ZXZ_H_
21 #define _ROTATION_ABOUT_AXES_ZXZ_H_
23 #include "mbsim/functions/function.h"
32 fmatvec::Mat3xV J, Jd;
35 int getArgSize()
const {
return 3; }
36 fmatvec::RotMat3 operator()(
const Arg &q) {
40 double spsi = sin(psi);
41 double stheta = sin(theta);
42 double sphi = sin(phi);
43 double cpsi = cos(psi);
44 double ctheta = cos(theta);
45 double cphi = cos(phi);
46 A.e(0,0) = cpsi*cphi-spsi*ctheta*sphi;
47 A.e(1,0) = spsi*cphi+cpsi*ctheta*sphi;
48 A.e(2,0) = stheta*sphi;
49 A.e(0,1) = -cpsi*sphi-spsi*ctheta*cphi;
50 A.e(1,1) = -spsi*sphi+cpsi*ctheta*cphi;
51 A.e(2,1) = stheta*cphi;
52 A.e(0,2) = spsi*stheta;
53 A.e(1,2) = -cpsi*stheta;
57 typename B::DRetDArg parDer(
const Arg &q) {
62 J.e(0,2) = sin(psi)*sin(theta);
65 J.e(1,2) = -cos(psi)*sin(theta);
68 J.e(2,2) = cos(theta);
71 typename B::DRetDArg parDerDirDer(
const Arg &qd,
const Arg &q) {
75 double thetad=qd.e(1);
76 Jd.e(0,1) = -sin(psi)*psid;
77 Jd.e(0,2) = cos(psi)*sin(theta)*psid + sin(psi)*cos(theta)*thetad;
78 Jd.e(1,1) = cos(psi)*psid;
79 Jd.e(1,2) = sin(psi)*sin(theta)*psid - cos(psi)*cos(theta)*thetad;
80 Jd.e(2,2) = -sin(theta)*thetad;
Definition: rotation_about_axes_zxz.h:28
Definition: planar_contour.h:31