00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _KINEMATIC_FUNCTIONS_H_
00021 #define _KINEMATIC_FUNCTIONS_H_
00022
00023 #include "fmatvec/function.h"
00024 #include "mbsim/objectfactory.h"
00025 #include "mbsim/element.h"
00026 #include "mbsim/utils/utils.h"
00027
00028 namespace MBSim {
00029
00030 template<typename Arg>
00031 class TranslationAlongXAxis : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00032 private:
00033 fmatvec::Vec3 r, a;
00034 public:
00035 TranslationAlongXAxis() { a.e(0) = 1; }
00036 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00037 fmatvec::Vec3 operator()(const Arg &q) {
00038 r.e(0) = ToDouble<Arg>::cast(q);
00039 return r;
00040 }
00041 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00042 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00043 bool constParDer() const { return true; }
00044 };
00045
00046 template<typename Arg>
00047 class TranslationAlongYAxis : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00048 private:
00049 fmatvec::Vec3 r, a;
00050 public:
00051 TranslationAlongYAxis() { a.e(1) = 1; }
00052 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00053 fmatvec::Vec3 operator()(const Arg &q) {
00054 r.e(1) = ToDouble<Arg>::cast(q);
00055 return r;
00056 }
00057 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00058 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00059 bool constParDer() const { return true; }
00060 };
00061
00062 template<typename Arg>
00063 class TranslationAlongZAxis : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00064 private:
00065 fmatvec::Vec3 r, a;
00066 public:
00067 TranslationAlongZAxis() { a.e(2) = 1; }
00068 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00069 fmatvec::Vec3 operator()(const Arg &q) {
00070 r.e(2) = ToDouble<Arg>::cast(q);
00071 return r;
00072 }
00073 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00074 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00075 bool constParDer() const { return true; }
00076 };
00077
00078 template<typename Arg>
00079 class TranslationAlongAxesXY : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00080 private:
00081 fmatvec::Vec3 r;
00082 fmatvec::Mat3xV A;
00083 public:
00084 TranslationAlongAxesXY() : A(2) { A.e(0,0) = 1; A.e(1,1) = 1; }
00085 typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00086 fmatvec::Vec3 operator()(const Arg &q) {
00087 r.e(0) = q.e(0);
00088 r.e(1) = q.e(1);
00089 return r;
00090 }
00091 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00092 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(2); }
00093 bool constParDer() const { return true; }
00094 };
00095
00096 template<typename Arg>
00097 class TranslationAlongAxesYZ : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00098 private:
00099 fmatvec::Vec3 r;
00100 fmatvec::Mat3xV A;
00101 public:
00102 TranslationAlongAxesYZ() : A(2) { A.e(1,0) = 1; A.e(2,1) = 1; }
00103 typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00104 fmatvec::Vec3 operator()(const Arg &q) {
00105 r.e(1) = q.e(0);
00106 r.e(2) = q.e(1);
00107 return r;
00108 }
00109 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00110 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(2); }
00111 bool constParDer() const { return true; }
00112 };
00113
00114 template<typename Arg>
00115 class TranslationAlongAxesXZ : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00116 private:
00117 fmatvec::Vec3 r;
00118 fmatvec::Mat3xV A;
00119 public:
00120 TranslationAlongAxesXZ() : A(2) { A.e(0,0) = 1; A.e(2,1) = 1; }
00121 typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00122 fmatvec::Vec3 operator()(const Arg &q) {
00123 r.e(0) = q.e(0);
00124 r.e(2) = q.e(1);
00125 return r;
00126 }
00127 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00128 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(2); }
00129 bool constParDer() const { return true; }
00130 };
00131
00132 template<typename Arg>
00133 class TranslationAlongAxesXYZ : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00134 private:
00135 fmatvec::Vec3 r;
00136 fmatvec::Mat3xV A;
00137 public:
00138 TranslationAlongAxesXYZ() : A(3) { A.e(0,0) = 1; A.e(1,1) = 1; A.e(2,2) = 1; }
00139 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00140 fmatvec::Vec3 operator()(const Arg &q) {
00141 r.e(0) = q.e(0);
00142 r.e(1) = q.e(1);
00143 r.e(2) = q.e(2);
00144 return r;
00145 }
00146 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00147 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(3); }
00148 bool constParDer() const { return true; }
00149 };
00150
00151 template<class Arg>
00152 class TranslationAlongFixedAxis : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00153 private:
00154 fmatvec::Vec3 a;
00155 fmatvec::Vec3 zeros(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &x) { return fmatvec::Vec3(x.rows()); }
00156 public:
00157 TranslationAlongFixedAxis() { }
00158 TranslationAlongFixedAxis(const fmatvec::Vec3 &a_) : a(a_) { }
00159 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00160 fmatvec::Vec3 operator()(const Arg &arg) { return a*arg; }
00161 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return a; }
00162 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(1); }
00163 typename fmatvec::Der<typename fmatvec::Der<fmatvec::Vec3, double>::type, double>::type parDerParDer(const double &arg) { throw MBSimError("parDerParDer is not available for given template parameters."); }
00164 bool constParDer() const { return true; }
00165 void initializeUsingXML(xercesc::DOMElement *element) {
00166 xercesc::DOMElement *e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"axisOfTranslation");
00167 a=FromMatStr<fmatvec::Vec3>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str());
00168 }
00169 xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *parent) { return 0; }
00170 };
00171
00172 template<>
00173 inline fmatvec::Vec3 TranslationAlongFixedAxis<double>::parDerParDer(const double &arg) { return fmatvec::Vec3(); }
00174
00175 template<class Arg>
00176 class LinearTranslation : public fmatvec::Function<fmatvec::Vec3(Arg)> {
00177 private:
00178 typename fmatvec::Der<fmatvec::Vec3, Arg>::type A;
00179 fmatvec::Vec3 b;
00180 fmatvec::Vec3 zeros(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &x) { return fmatvec::Vec3(x.rows()); }
00181 public:
00182 LinearTranslation() { }
00183 LinearTranslation(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &A_) : A(A_) { }
00184 LinearTranslation(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &A_, const fmatvec::Vec3 &b_) : A(A_), b(b_) { }
00185 typename fmatvec::Size<Arg>::type getArgSize() const { return A.cols(); }
00186 fmatvec::Vec3 operator()(const Arg &arg) { return A*arg+b; }
00187 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDer(const Arg &arg) { return A; }
00188 typename fmatvec::Der<fmatvec::Vec3, Arg>::type parDerDirDer(const Arg &arg1Dir, const Arg &arg1) { return typename fmatvec::Der<fmatvec::Vec3, Arg>::type(A.rows(),A.cols()); }
00189 typename fmatvec::Der<typename fmatvec::Der<fmatvec::Vec3, double>::type, double>::type parDerParDer(const double &arg) { throw MBSimError("parDerParDer is not available for given template parameters."); }
00190 bool constParDer() const { return true; }
00191 void initializeUsingXML(xercesc::DOMElement *element) {
00192 xercesc::DOMElement *e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"translationVectors");
00193 A=FromMatStr<typename fmatvec::Der<fmatvec::Vec3, Arg>::type>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str());
00194 e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"offset");
00195 b=e?FromMatStr<fmatvec::Vec3>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str()):zeros(A);
00196 }
00197 xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *parent) { return 0; }
00198 void setSlope(const typename fmatvec::Der<fmatvec::Vec3, Arg>::type &A_) { A = A_; }
00199 void setIntercept(const fmatvec::Vec3 &b_) { b = b_; }
00200 };
00201
00202 template<>
00203 inline fmatvec::Vec3 LinearTranslation<double>::parDerDirDer(const double &arg1Dir, const double &arg1) { return fmatvec::Vec3(); }
00204 template<>
00205 inline fmatvec::Vec3 LinearTranslation<double>::parDerParDer(const double &arg) { return fmatvec::Vec3(); }
00206
00207 template<class Arg>
00208 class RotationAboutXAxis : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00209 private:
00210 fmatvec::RotMat3 A;
00211 fmatvec::Vec3 a;
00212 public:
00213 RotationAboutXAxis() { a.e(0) = 1; A.e(0,0) = 1; }
00214 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00215 fmatvec::RotMat3 operator()(const Arg &q) {
00216 double alpha = ToDouble<Arg>::cast(q);
00217 const double cosq=cos(alpha);
00218 const double sinq=sin(alpha);
00219 A.e(1,1) = cosq;
00220 A.e(2,1) = sinq;
00221 A.e(1,2) = -sinq;
00222 A.e(2,2) = cosq;
00223 return A;
00224 }
00225 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00226 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00227 bool constParDer() const { return true; }
00228 };
00229
00230 template<class Arg>
00231 class RotationAboutYAxis : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00232 private:
00233 fmatvec::RotMat3 A;
00234 fmatvec::Vec3 a;
00235 public:
00236 RotationAboutYAxis() { a.e(1) = 1; A.e(1,1) = 1; }
00237 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00238 fmatvec::RotMat3 operator()(const Arg &q) {
00239 double alpha = ToDouble<Arg>::cast(q);
00240 const double cosq=cos(alpha);
00241 const double sinq=sin(alpha);
00242 A.e(0,0) = cosq;
00243 A.e(2,0) = -sinq;
00244 A.e(0,2) = sinq;
00245 A.e(2,2) = cosq;
00246 return A;
00247 }
00248 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00249 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00250 bool constParDer() const { return true; }
00251 };
00252
00253 template<class Arg>
00254 class RotationAboutZAxis : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00255 private:
00256 fmatvec::RotMat3 A;
00257 fmatvec::Vec3 a;
00258 public:
00259 RotationAboutZAxis() { a.e(2) = 1; A.e(2,2) = 1; }
00260 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00261 fmatvec::RotMat3 operator()(const Arg &q) {
00262 double alpha = ToDouble<Arg>::cast(q);
00263 const double cosq=cos(alpha);
00264 const double sinq=sin(alpha);
00265 A.e(0,0) = cosq;
00266 A.e(1,0) = sinq;
00267 A.e(0,1) = -sinq;
00268 A.e(1,1) = cosq;
00269 return A;
00270 }
00271 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00272 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00273 bool constParDer() const { return true; }
00274 };
00275
00276 template<class Arg>
00277 class RotationAboutFixedAxis : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00278 private:
00279 fmatvec::RotMat3 A;
00280 fmatvec::Vec3 a;
00281 public:
00282 RotationAboutFixedAxis() { }
00283 RotationAboutFixedAxis(const fmatvec::Vec3 &a_) : a(a_) { }
00284 typename fmatvec::Size<Arg>::type getArgSize() const { return 1; }
00285 fmatvec::RotMat3 operator()(const Arg &q) {
00286 double alpha = ToDouble<Arg>::cast(q);
00287 const double cosq=cos(alpha);
00288 const double sinq=sin(alpha);
00289 const double onemcosq=1-cosq;
00290 const double a0a1=a.e(0)*a.e(1);
00291 const double a0a2=a.e(0)*a.e(2);
00292 const double a1a2=a.e(1)*a.e(2);
00293 A.e(0,0) = cosq+onemcosq*a.e(0)*a.e(0);
00294 A.e(1,0) = onemcosq*a0a1+a.e(2)*sinq;
00295 A.e(2,0) = onemcosq*a0a2-a.e(1)*sinq;
00296 A.e(0,1) = onemcosq*a0a1-a.e(2)*sinq;
00297 A.e(1,1) = cosq+onemcosq*a.e(1)*a.e(1);
00298 A.e(2,1) = onemcosq*a1a2+a.e(0)*sinq;
00299 A.e(0,2) = onemcosq*a0a2+a.e(1)*sinq;
00300 A.e(1,2) = onemcosq*a1a2-a.e(0)*sinq;
00301 A.e(2,2) = cosq+onemcosq*a.e(2)*a.e(2);
00302 return A;
00303 }
00304 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) { return a; }
00305 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) { return typename fmatvec::Der<fmatvec::RotMat3, Arg>::type(1); }
00306 typename fmatvec::Der<typename fmatvec::Der<fmatvec::RotMat3, Arg>::type, Arg>::type parDerParDer(const Arg &arg) { throw MBSimError("parDerParDer is not available for given template parameters."); }
00307 bool constParDer() const { return true; }
00308 const fmatvec::Vec3& getAxisOfRotation() const { return a; }
00309 void setAxisOfRotation(const fmatvec::Vec3 &a_) { a = a_; }
00310 void initializeUsingXML(xercesc::DOMElement *element) {
00311 xercesc::DOMElement *e=MBXMLUtils::E(element)->getFirstElementChildNamed(MBSIM%"axisOfRotation");
00312 a=FromMatStr<fmatvec::Vec3>::cast((MBXMLUtils::X()%MBXMLUtils::E(e)->getFirstTextChild()->getData()).c_str());
00313 }
00314 xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *parent) { return 0; }
00315 };
00316
00317 template<>
00318 inline fmatvec::Vec3 RotationAboutFixedAxis<double>::parDerParDer(const double &arg) { return fmatvec::Vec3(); }
00319
00320 template<class Arg>
00321 class RotationAboutAxesXY : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00322 private:
00323 fmatvec::RotMat3 A;
00324 fmatvec::Mat3xV J, Jd;
00325 public:
00326 RotationAboutAxesXY() : J(2), Jd(2) { J.e(0,0) = 1; }
00327 typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00328 fmatvec::RotMat3 operator()(const Arg &q) {
00329 double a=q.e(0);
00330 double b=q.e(1);
00331 double cosa = cos(a);
00332 double sina = sin(a);
00333 double cosb = cos(b);
00334 double sinb = sin(b);
00335
00336 A.e(0,0) = cosb;
00337 A.e(1,0) = sina*sinb;
00338 A.e(2,0) = -cosa*sinb;
00339 A.e(1,1) = cosa;
00340 A.e(2,1) = sina;
00341 A.e(0,2) = sinb;
00342 A.e(1,2) = -sina*cosb;
00343 A.e(2,2) = cosa*cosb;
00344 return A;
00345 }
00346 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00347 double a = q.e(0);
00348 J.e(1,1) = cos(a);
00349 J.e(2,1) = sin(a);
00350 return J;
00351 }
00352 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00353 double a = q.e(0);
00354 double ad = qd.e(0);
00355 Jd.e(1,1) = -sin(a)*ad;
00356 Jd.e(2,1) = cos(a)*ad;
00357 return Jd;
00358 }
00359 };
00360
00361 template<class Arg>
00362 class RotationAboutAxesYZ : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00363 private:
00364 fmatvec::RotMat3 A;
00365 fmatvec::Mat3xV J, Jd;
00366 public:
00367 RotationAboutAxesYZ() : J(2), Jd(2) { J.e(1,0) = 1; }
00368 typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00369 fmatvec::RotMat3 operator()(const Arg &q) {
00370 double b=q.e(0);
00371 double g=q.e(1);
00372 double cosb = cos(b);
00373 double sinb = sin(b);
00374 double cosg = cos(g);
00375 double sing = sin(g);
00376
00377 A.e(0,0) = cosb*cosg;
00378 A.e(1,0) = sing;
00379 A.e(2,0) = -sinb*cosg;
00380 A.e(0,1) = -cosb*sing;
00381 A.e(1,1) = cosg;
00382 A.e(2,1) = sinb*sing;
00383 A.e(0,2) = sinb;
00384 A.e(2,2) = cosb;
00385 return A;
00386 }
00387 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00388 double beta = q.e(0);
00389 J.e(0,1) = sin(beta);
00390 J.e(2,1) = cos(beta);
00391 return J;
00392 }
00393 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00394 double beta = q.e(0);
00395 double betad = qd.e(0);
00396 Jd.e(0,1) = cos(beta)*betad;
00397 Jd.e(2,1) = -sin(beta)*betad;
00398 return Jd;
00399 }
00400 };
00401
00402 template<class Arg>
00403 class RotationAboutAxesXZ : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00404 private:
00405 fmatvec::RotMat3 A;
00406 fmatvec::Mat3xV J, Jd;
00407 public:
00408 RotationAboutAxesXZ() : J(2), Jd(2) { J.e(0,0) = 1; }
00409 typename fmatvec::Size<Arg>::type getArgSize() const { return 2; }
00410 fmatvec::RotMat3 operator()(const Arg &q) {
00411 double a=q.e(0);
00412 double b=q.e(1);
00413 double cosa = cos(a);
00414 double sina = sin(a);
00415 double cosb = cos(b);
00416 double sinb = sin(b);
00417
00418 A.e(0,0) = cosb;
00419 A.e(1,0) = cosa*sinb;
00420 A.e(2,0) = sina*sinb;
00421 A.e(0,1) = -sinb;
00422 A.e(1,1) = cosa*cosb;
00423 A.e(2,1) = sina*cosb;
00424 A.e(1,2) = -sina;
00425 A.e(2,2) = cosa;
00426
00427 return A;
00428 }
00429 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00430 double a = q.e(0);
00431 J.e(1,1) = -sin(a);
00432 J.e(2,1) = cos(a);
00433 return J;
00434 }
00435 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00436 double a = q.e(0);
00437 double ad = qd.e(0);
00438 Jd.e(1,1) = -cos(a)*ad;
00439 Jd.e(2,1) = -sin(a)*ad;
00440 return Jd;
00441 }
00442 };
00443
00447 template<class Arg>
00448 class RotationAboutAxesXYZ : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00449 private:
00450 fmatvec::RotMat3 A;
00451 fmatvec::Mat3xV J, Jd;
00452 public:
00453 RotationAboutAxesXYZ() : J(3), Jd(3) { J.e(0,0) = 1; }
00454 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00455 fmatvec::RotMat3 operator()(const Arg &q) {
00456 double a=q.e(0);
00457 double b=q.e(1);
00458 double g=q.e(2);
00459 double cosa = cos(a);
00460 double sina = sin(a);
00461 double cosb = cos(b);
00462 double sinb = sin(b);
00463 double cosg = cos(g);
00464 double sing = sin(g);
00465 A.e(0,0) = cosb*cosg;
00466 A.e(1,0) = sina*sinb*cosg+cosa*sing;
00467 A.e(2,0) = -cosa*sinb*cosg+sina*sing;
00468 A.e(0,1) = -cosb*sing;
00469 A.e(1,1) = -sing*sinb*sina+cosa*cosg;
00470 A.e(2,1) = cosa*sinb*sing+sina*cosg;
00471 A.e(0,2) = sinb;
00472 A.e(1,2) = -sina*cosb;
00473 A.e(2,2) = cosa*cosb;
00474 return A;
00475 }
00476 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00477 double a = q.e(0);
00478 double b = q.e(1);
00479 double cosa = cos(a);
00480 double sina = sin(a);
00481 double cosb = cos(b);
00482
00483
00484 J.e(0,2) = sin(b);
00485
00486 J.e(1,1) = cosa;
00487 J.e(1,2) = -sina*cosb;
00488
00489 J.e(2,1) = sina;
00490 J.e(2,2) = cosa*cosb;
00491 return J;
00492 }
00493 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00494 double a = q.e(0);
00495 double b = q.e(1);
00496 double ad = qd.e(0);
00497 double bd = qd.e(1);
00498 double cosa = cos(a);
00499 double sina = sin(a);
00500 double cosb = cos(b);
00501 double sinb = sin(b);
00502
00503
00504 Jd.e(0,2) = cosb*bd;
00505
00506 Jd.e(1,1) = -sina*ad;
00507 Jd.e(1,2) = -cosa*cosb*ad + sina*sinb*bd;
00508
00509 Jd.e(2,1) = cosa*ad;
00510 Jd.e(2,2) = -sina*cosb*ad - cosa*sinb*bd;
00511 return Jd;
00512 }
00513 };
00514
00515 template<class Arg>
00516 class RotationAboutAxesXYZ2 : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00517 private:
00518 fmatvec::RotMat3 A;
00519 fmatvec::Mat3xV J, Jd;
00520 public:
00521 RotationAboutAxesXYZ2() : J(3), Jd(3) { J.e(2,2) = 1; }
00522 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00523 fmatvec::RotMat3 operator()(const Arg &q) {
00524 double a=q.e(0);
00525 double b=q.e(1);
00526 double g=q.e(2);
00527 double cosa = cos(a);
00528 double sina = sin(a);
00529 double cosb = cos(b);
00530 double sinb = sin(b);
00531 double cosg = cos(g);
00532 double sing = sin(g);
00533 A.e(0,0) = cosb*cosg;
00534 A.e(1,0) = sina*sinb*cosg+cosa*sing;
00535 A.e(2,0) = -cosa*sinb*cosg+sina*sing;
00536 A.e(0,1) = -cosb*sing;
00537 A.e(1,1) = -sing*sinb*sina+cosa*cosg;
00538 A.e(2,1) = cosa*sinb*sing+sina*cosg;
00539 A.e(0,2) = sinb;
00540 A.e(1,2) = -sina*cosb;
00541 A.e(2,2) = cosa*cosb;
00542 return A;
00543 }
00544 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00545 double b = q.e(1);
00546 double g = q.e(2);
00547 J.e(0,0) = cos(b)*cos(g);
00548 J.e(0,1) = sin(g);
00549
00550 J.e(1,0) = -cos(b)*sin(g);
00551 J.e(1,1) = cos(g);
00552
00553 J.e(2,0) = sin(b);
00554
00555
00556 return J;
00557 }
00558 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00559 double b = q.e(1);
00560 double g = q.e(2);
00561 double bd = qd.e(1);
00562 double gd = qd.e(2);
00563 Jd.e(0,0) = -sin(b)*cos(g)*bd - cos(b)*sin(g)*gd;
00564 Jd.e(0,1) = cos(g)*gd;
00565
00566 Jd.e(1,0) = sin(b)*sin(g)*bd - cos(b)*cos(g)*gd;
00567 Jd.e(1,1) = -sin(g)*gd;
00568
00569 Jd.e(2,0) = cos(b)*bd;
00570
00571
00572 return Jd;
00573 }
00574 };
00575
00576 template<class Arg>
00577 class RotationAboutAxesZXZ : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00578 private:
00579 fmatvec::RotMat3 A;
00580 fmatvec::Mat3xV J, Jd;
00581 public:
00582 RotationAboutAxesZXZ() : J(3), Jd(3) { J.e(2,0) = 1; }
00583 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00584 fmatvec::RotMat3 operator()(const Arg &q) {
00585 double psi=q.e(0);
00586 double theta=q.e(1);
00587 double phi=q.e(2);
00588 double spsi = sin(psi);
00589 double stheta = sin(theta);
00590 double sphi = sin(phi);
00591 double cpsi = cos(psi);
00592 double ctheta = cos(theta);
00593 double cphi = cos(phi);
00594 A.e(0,0) = cpsi*cphi-spsi*ctheta*sphi;
00595 A.e(1,0) = spsi*cphi+cpsi*ctheta*sphi;
00596 A.e(2,0) = stheta*sphi;
00597 A.e(0,1) = -cpsi*sphi-spsi*ctheta*cphi;
00598 A.e(1,1) = -spsi*sphi+cpsi*ctheta*cphi;
00599 A.e(2,1) = stheta*cphi;
00600 A.e(0,2) = spsi*stheta;
00601 A.e(1,2) = -cpsi*stheta;
00602 A.e(2,2) = ctheta;
00603 return A;
00604 }
00605 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00606 double psi=q.e(0);
00607 double theta=q.e(1);
00608
00609 J.e(0,1) = cos(psi);
00610 J.e(0,2) = sin(psi)*sin(theta);
00611
00612 J.e(1,1) = sin(psi);
00613 J.e(1,2) = -cos(psi)*sin(theta);
00614
00615
00616 J.e(2,2) = cos(theta);
00617 return J;
00618 }
00619 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00620 double psi=q.e(0);
00621 double theta=q.e(1);
00622 double psid=qd.e(0);
00623 double thetad=qd.e(1);
00624 Jd.e(0,1) = -sin(psi)*psid;
00625 Jd.e(0,2) = cos(psi)*sin(theta)*psid + sin(psi)*cos(theta)*thetad;
00626 Jd.e(1,1) = cos(psi)*psid;
00627 Jd.e(1,2) = sin(psi)*sin(theta)*psid - cos(psi)*cos(theta)*thetad;
00628 Jd.e(2,2) = -sin(theta)*thetad;
00629 return Jd;
00630 }
00631 };
00632
00633 template<class Arg>
00634 class RotationAboutAxesZYX : public fmatvec::Function<fmatvec::RotMat3(Arg)> {
00635 private:
00636 fmatvec::RotMat3 A;
00637 fmatvec::Mat3xV J, Jd;
00638 public:
00639 RotationAboutAxesZYX() : J(3), Jd(3) { J.e(2,0) = 1; }
00640 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00641 fmatvec::RotMat3 operator()(const Arg &q) {
00642 double a=q.e(0);
00643 double b=q.e(1);
00644 double g=q.e(2);
00645 double cosa = cos(a);
00646 double sina = sin(a);
00647 double cosb = cos(b);
00648 double sinb = sin(b);
00649 double cosg = cos(g);
00650 double sing = sin(g);
00651 A.e(0,0) = cosa*cosb;
00652 A.e(1,0) = sina*cosb;
00653 A.e(2,0) = -sinb;
00654 A.e(0,1) = -sina*cosg+cosa*sinb*sing;
00655 A.e(1,1) = cosa*cosg+sina*sinb*sing;
00656 A.e(2,1) = cosb*sing;
00657 A.e(0,2) = sina*sing+cosa*sinb*cosg;
00658 A.e(1,2) = -cosa*sing+sina*sinb*cosg;
00659 A.e(2,2) = cosb*cosg;
00660 return A;
00661 }
00662 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDer(const Arg &q) {
00663 double a = q.e(0);
00664 double b = q.e(1);
00665 double cosa = cos(a);
00666 double sina = sin(a);
00667 double cosb = cos(b);
00668
00669 J.e(0,1) = -sina;
00670 J.e(0,2) = cosa*cosb;
00671
00672 J.e(1,1) = cosa;
00673 J.e(1,2) = sina*cosb;
00674
00675
00676 J.e(2,2) = -sin(b);
00677 return J;
00678 }
00679 typename fmatvec::Der<fmatvec::RotMat3, Arg>::type parDerDirDer(const Arg &qd, const Arg &q) {
00680 double a = q.e(0);
00681 double b = q.e(1);
00682 double ad = qd.e(0);
00683 double bd = qd.e(1);
00684 double cosa = cos(a);
00685 double sina = sin(a);
00686 double cosb = cos(b);
00687 double sinb = sin(b);
00688 Jd.e(0,1) = -cosa*ad;
00689 Jd.e(0,2) = -sina*cosb*ad - cosa*sinb*bd;
00690 Jd.e(1,1) = -sina*ad;
00691 Jd.e(1,2) = cosa*cosb*ad - sina*sinb*bd;
00692 Jd.e(2,2) = -cosb*bd;
00693 return Jd;
00694 }
00695 };
00696
00697 template<class Arg>
00698 class RotationAboutAxesXYZMapping : public fmatvec::Function<fmatvec::MatV(Arg)> {
00699 private:
00700 fmatvec::MatV T;
00701 public:
00702 RotationAboutAxesXYZMapping() : T(3,3,fmatvec::Eye()) { }
00703 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00704 fmatvec::MatV operator()(const Arg &q) {
00705 double alpha = q.e(0);
00706 double beta = q.e(1);
00707 double cos_beta = cos(beta);
00708 double sin_beta = sin(beta);
00709 double cos_alpha = cos(alpha);
00710 double sin_alpha = sin(alpha);
00711 double tan_beta = sin_beta/cos_beta;
00712 T.e(0,1) = tan_beta*sin_alpha;
00713 T.e(0,2) = -tan_beta*cos_alpha;
00714 T.e(1,1) = cos_alpha;
00715 T.e(1,2) = sin_alpha;
00716 T.e(2,1) = -sin_alpha/cos_beta;
00717 T.e(2,2) = cos_alpha/cos_beta;
00718 return T;
00719 }
00720 };
00721
00722 template<class Arg>
00723 class RotationAboutAxesXYZMapping2 : public fmatvec::Function<fmatvec::MatV(Arg)> {
00724 private:
00725 fmatvec::MatV T;
00726 public:
00727 RotationAboutAxesXYZMapping2() : T(3,3,fmatvec::Eye()) { }
00728 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00729 fmatvec::MatV operator()(const Arg &q) {
00730 double beta = q.e(1);
00731 double gamma = q.e(2);
00732 double cos_beta = cos(beta);
00733 double sin_beta = sin(beta);
00734 double cos_gamma = cos(gamma);
00735 double sin_gamma = sin(gamma);
00736 double tan_beta = sin_beta/cos_beta;
00737 T.e(0,0) = cos_gamma/cos_beta;
00738 T.e(0,1) = -sin_gamma/cos_beta;
00739 T.e(1,0) = sin_gamma;
00740 T.e(1,1) = cos_gamma;
00741 T.e(2,0) = -cos_gamma*tan_beta;
00742 T.e(2,1) = sin_gamma*tan_beta;
00743 return T;
00744 }
00745 };
00746
00747 template<class Arg>
00748 class RotationAboutAxesZXZMapping : public fmatvec::Function<fmatvec::MatV(Arg)> {
00749 private:
00750 fmatvec::MatV T;
00751 public:
00752 RotationAboutAxesZXZMapping() : T(3,3) { T.e(0,2) = 1; }
00753 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00754 fmatvec::MatV operator()(const Arg &q) {
00755 double psi = q.e(0);
00756 double theta = q.e(1);
00757 double cos_theta = cos(theta);
00758 double sin_theta = sin(theta);
00759 double cos_psi = cos(psi);
00760 double sin_psi = sin(psi);
00761 double tan_theta = sin_theta/cos_theta;
00762
00763 T.e(0,0) = -sin_psi/tan_theta;
00764 T.e(0,1) = cos_psi/tan_theta;
00765 T.e(1,0) = cos_psi;
00766 T.e(1,1) = sin_psi;
00767 T.e(2,0) = sin_psi/sin_theta;
00768 T.e(2,1) = -cos_psi/sin_theta;
00769
00770 return T;
00771 }
00772 };
00773
00774 template<class Arg>
00775 class RotationAboutAxesZXZMapping2 : public fmatvec::Function<fmatvec::MatV(Arg)> {
00776 private:
00777 fmatvec::MatV T;
00778 public:
00779 RotationAboutAxesZXZMapping2() : T(3,3,fmatvec::Eye()) { }
00780 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00781 fmatvec::MatV operator()(const Arg &q) {
00782 double theta = q.e(1);
00783 double phi = q.e(2);
00784 double cos_theta = cos(theta);
00785 double sin_theta = sin(theta);
00786 double cos_phi = cos(phi);
00787 double sin_phi = sin(phi);
00788 double tan_theta = sin_theta/cos_theta;
00789
00790 T.e(0,0) = sin_phi/sin_theta;
00791 T.e(0,1) = cos_phi/sin_theta;
00792 T.e(1,0) = cos_phi;
00793 T.e(1,1) = -sin_phi;
00794 T.e(2,0) = -sin_phi/tan_theta;
00795 T.e(2,1) = -cos_phi/tan_theta;
00796
00797 return T;
00798 }
00799 };
00800
00801 template<class Arg>
00802 class RotationAboutAxesZYXMapping : public fmatvec::Function<fmatvec::MatV(Arg)> {
00803 private:
00804 fmatvec::MatV T;
00805 public:
00806 RotationAboutAxesZYXMapping() : T(3,3) { T.e(0,2) = 1; }
00807 typename fmatvec::Size<Arg>::type getArgSize() const { return 3; }
00808 fmatvec::MatV operator()(const Arg &q) {
00809 double alpha = q.e(0);
00810 double beta = q.e(1);
00811 double cos_beta = cos(beta);
00812 double sin_beta = sin(beta);
00813 double cos_alpha = cos(alpha);
00814 double sin_alpha = sin(alpha);
00815 double tan_beta = sin_beta/cos_beta;
00816 T.e(0,0) = cos_alpha*tan_beta;
00817 T.e(0,1) = sin_alpha*tan_beta;
00818 T.e(1,0) = -sin_alpha;
00819 T.e(1,1) = cos_alpha;
00820 T.e(2,0) = cos_alpha/cos_beta;
00821 T.e(2,1) = sin_alpha/cos_beta;
00822 return T;
00823 }
00824 };
00825
00826 }
00827
00828 #endif