00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef FUNCTIONS_CONTACT_H_
00022 #define FUNCTIONS_CONTACT_H_
00023
00024 #include <mbsim/contour.h>
00025 #include <mbsim/contours/contour1s.h>
00026 #include <mbsim/contours/point.h>
00027 #include "mbsim/contours/line.h"
00028 #include <mbsim/contours/circle_solid.h>
00029 #include "mbsim/contours/circle_hollow.h"
00030 #include "mbsim/contours/circle.h"
00031 #include "mbsim/contours/frustum2d.h"
00032 #include "mbsim/contours/edge.h"
00033 #include "mbsim/contours/face.h"
00034 #include "mbsim/contours/frustum.h"
00035 #include "mbsim/contours/plane.h"
00036 #include "mbsim/contours/planewithfrustum.h"
00037 #include "mbsim/contours/contour_interpolation.h"
00038 #include "mbsim/contours/contour_quad.h"
00039 #include "mbsim/contours/cuboid.h"
00040 #include "mbsim/contours/compound_contour.h"
00041 #include "mbsim/utils/function.h"
00042 #include "mbsim/mbsim_event.h"
00043
00044 namespace MBSim {
00045
00051 template<class Ret, class Arg>
00052 class DistanceFunction : public Function1<Ret,Arg> {
00053 public:
00054
00059 virtual Ret operator()(const Arg& x, const void * =NULL) = 0;
00060
00065 virtual double operator[](const Arg& x) { return nrm2(computeWrD(x)); };
00066
00071 virtual fmatvec::Vec3 computeWrD(const Arg& x) = 0;
00072
00073 };
00074
00082 class FuncPairContour1sPoint : public DistanceFunction<double,double> {
00083 public:
00089 FuncPairContour1sPoint(Point* point_, Contour1s *contour_) : contour(contour_), point(point_), cp(fmatvec::VecV(1)) {}
00090
00091
00092 double operator()(const double &alpha, const void * =NULL) {
00093 fmatvec::Vec3 Wd = computeWrD(alpha);
00094 fmatvec::Vec3 Wt = cp.getFrameOfReference().getOrientation().col(1);
00095 return Wt.T()*Wd;
00096 }
00097
00098 fmatvec::Vec3 computeWrD(const double &alpha) {
00099
00100 cp.getLagrangeParameterPosition()(0) = alpha;
00101 contour->computeRootFunctionPosition(cp);
00102 contour->computeRootFunctionFirstTangent(cp);
00103
00104 return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00105 }
00106
00107
00108 private:
00112 Contour1s *contour;
00113 Point *point;
00114
00118 ContourPointData cp;
00119 };
00120
00128 class FuncPairContour1sCircleHollow : public DistanceFunction<double,double> {
00129 public:
00135 FuncPairContour1sCircleHollow(CircleHollow* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00136
00137
00138 double operator()(const double &alpha, const void * =NULL) {
00139 fmatvec::Vec3 Wd = computeWrD(alpha);
00140 return circle->getReferenceOrientation().col(2).T()*Wd;
00141 }
00142
00143 fmatvec::Vec3 computeWrD(const double &alpha) {
00144
00145 cp.getLagrangeParameterPosition()(0) = alpha;
00146 contour->computeRootFunctionPosition(cp);
00147
00148 return circle->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00149 }
00150
00151
00152 private:
00156 Contour1s *contour;
00157 CircleHollow *circle;
00158
00162 ContourPointData cp;
00163 };
00164
00165
00171 class FuncPairPointContourInterpolation : public DistanceFunction<fmatvec::VecV,fmatvec::VecV> {
00172 public:
00178 FuncPairPointContourInterpolation(Point* point_, ContourInterpolation *contour_) : contour(contour_), point(point_) {}
00179
00180
00181 fmatvec::VecV operator()(const fmatvec::VecV &alpha, const void * =NULL) {
00182 fmatvec::Mat3xV Wt = contour->computeWt(alpha);
00183 fmatvec::Vec3 WrOC[2];
00184 WrOC[0] = point->getFrame()->getPosition();
00185 WrOC[1] = contour->computeWrOC(alpha);
00186 return Wt.T() * ( WrOC[1] - WrOC[0] );
00187 }
00188
00189 fmatvec::Vec3 computeWrD(const fmatvec::VecV &alpha) {
00190 return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
00191 }
00192
00193
00194 private:
00198 ContourInterpolation *contour;
00199 Point *point;
00200 };
00201
00207 class FuncPairConeSectionCircle : public DistanceFunction<double,double> {
00208 public:
00216 FuncPairConeSectionCircle(double R_,double a_,double b_) : R(R_), a(a_), b(b_), sec_IN_ci(true) {}
00217
00225 FuncPairConeSectionCircle(double R_,double a_,double b_,bool sec_IN_ci_) : R(R_), a(a_), b(b_), sec_IN_ci(sec_IN_ci_) {}
00226
00227
00228 virtual double operator()(const double &phi, const void * =NULL) = 0;
00229 double operator[](const double &phi);
00230 virtual fmatvec::Vec3 computeWrD(const double &phi) = 0;
00231
00232
00233
00234 void setDiffVec(fmatvec::Vec3 d_);
00235
00236 void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00237
00238
00239 protected:
00243 double R, a, b;
00244
00248 bool sec_IN_ci;
00249
00253 fmatvec::Vec3 b1, b2;
00254
00258 fmatvec::Vec3 d;
00259 };
00260
00261 inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00262 inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00263 inline double FuncPairConeSectionCircle::operator[](const double &phi) { if(sec_IN_ci) return R - nrm2(computeWrD(phi)); else return nrm2(computeWrD(phi)) - R; }
00264
00271 class FuncPairEllipseCircle : public FuncPairConeSectionCircle {
00272 public:
00280 FuncPairEllipseCircle(double R_,double a_,double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00281
00289 FuncPairEllipseCircle(double R_,double a_,double b_,bool el_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,el_IN_ci_) {}
00290
00291
00292 double operator()(const double &phi, const void * =NULL);
00293 fmatvec::Vec3 computeWrD(const double &phi);
00294
00295
00296
00297 void setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_);
00298
00299 };
00300
00301 inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_) {setSectionCOS(b1e_,b2e_);}
00302 inline double FuncPairEllipseCircle::operator()(const double &phi, const void *) { return -2*b*(b2(0)*d(0) + b2(1)*d(1) + b2(2)*d(2))*cos(phi) + 2*a*(b1(0)*d(0) + b1(1)*d(1) + b1(2)*d(2))*sin(phi) + ((a*a) - (b*b))*sin(2*phi); }
00303 inline fmatvec::Vec3 FuncPairEllipseCircle::computeWrD(const double &phi) { return d + b1*a*cos(phi) + b2*b*sin(phi); }
00304
00310 class FuncPairHyperbolaCircle : public FuncPairConeSectionCircle {
00311 public:
00319 FuncPairHyperbolaCircle(double R_, double a_, double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00320
00328 FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,hy_IN_ci_) {}
00329
00330
00331 double operator()(const double &phi, const void * =NULL);
00332 fmatvec::Vec3 computeWrD(const double &phi);
00333
00334 };
00335
00336 inline double FuncPairHyperbolaCircle::operator()(const double &phi, const void *) { return -2*b*(b2(0)*d(0) + b2(1)*d(1) + b2(2)*d(2))*cosh(phi) - 2*a*(b1(0)*d(0) + b1(1)*d(1) + b1(2)*d(2))*sinh(phi) - ((a*a) + (b*b))*sinh(2*phi); }
00337 inline fmatvec::Vec3 FuncPairHyperbolaCircle::computeWrD(const double &phi) { return d + b1*a*cosh(phi) + b2*b*sinh(phi); }
00338
00344 class JacobianPairConeSectionCircle : public Function1<double,double> {
00345 public:
00351 JacobianPairConeSectionCircle(double a_, double b_) : a(a_), b(b_) {}
00352
00353
00354 void setDiffVec(fmatvec::Vec3 d_);
00355 void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00356
00357
00358 protected:
00362 double a, b;
00363
00367 fmatvec::Vec3 b1, b2;
00368
00372 fmatvec::Vec3 d;
00373 };
00374
00375 inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00376 inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00377
00383 class JacobianPairEllipseCircle : public JacobianPairConeSectionCircle {
00384 public:
00390 JacobianPairEllipseCircle(double a_, double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00391
00392
00393 double operator()(const double &phi, const void * =NULL);
00394
00395 };
00396
00397 inline double JacobianPairEllipseCircle::operator()(const double &phi, const void *) { return 2.*(b*(b2(0)*d(0) + b2(1)*d(1) + b2(2)*d(2))*sin(phi) + a*(b1(0)*d(0) + b1(1)*d(1) + b1(2)*d(2))*cos(phi) + ((a*a) - (b*b))*cos(2*phi)); }
00398
00404 class JacobianPairHyperbolaCircle : public JacobianPairConeSectionCircle {
00405 public:
00411 JacobianPairHyperbolaCircle(double a_,double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00412
00413
00414 double operator()(const double &phi, const void * =NULL);
00415
00416 };
00417
00418 inline double JacobianPairHyperbolaCircle::operator()(const double &phi, const void *) { return -2*(b*(b2(0)*d(0) + b2(1)*d(1) + b2(2)*d(2))*sinh(phi) + a*(b1(0)*d(0) + b1(1)*d(1) + b1(2)*d(2))*cosh(phi) + ((a*a) + (b*b))*cosh(2*phi));}
00419
00426 class FuncPairContour1sLine : public DistanceFunction<double,double> {
00427 public:
00433 FuncPairContour1sLine(Line* line_, Contour1s *contour_) : contour(contour_), line(line_) {}
00434
00435
00436 virtual double operator()(const double &s, const void * =NULL) {
00437 throw MBSimError("ERROR (FuncPairContour1sLine::operator): Not implemented!");
00438
00439
00440
00441 }
00442
00443 virtual fmatvec::Vec3 computeWrD(const double &s) {
00444 throw MBSimError("ERROR (FuncPairContour1sLine::computeWrD): Not implemented!");
00445
00446
00447
00448
00449 }
00450
00451 virtual double operator[](const double &s) {
00452 return nrm2(computeWrD(s));
00453 }
00454
00455
00456 private:
00457 Contour1s *contour;
00458 Line *line;
00459 };
00460
00466 class FuncPairContour1sCircleSolid : public DistanceFunction<double,double> {
00467 public:
00473 FuncPairContour1sCircleSolid(CircleSolid* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00474
00475
00476 double operator()(const double &alpha, const void * =NULL) {
00477 cp.getLagrangeParameterPosition() = fmatvec::VecV(1,fmatvec::INIT,alpha);
00478 fmatvec::Vec3 Wd = computeWrD(alpha);
00479 fmatvec::Vec3 Wt = contour->computeTangent(cp);
00480 return Wt.T()*Wd;
00481 }
00482
00483 fmatvec::Vec3 computeWrD(const double &alpha) {
00484 cp.getLagrangeParameterPosition() = fmatvec::VecV(1,fmatvec::INIT,alpha);
00485 contour->computeRootFunctionPosition(cp);
00486 contour->computeRootFunctionFirstTangent(cp);
00487 contour->computeRootFunctionNormal(cp);
00488 WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius()*cp.getFrameOfReference().getOrientation().col(0);
00489 WrOC[1] = cp.getFrameOfReference().getPosition();
00490 return WrOC[1] - WrOC[0];
00491 }
00492
00493
00494 private:
00498 Contour1s *contour;
00499 CircleSolid *circle;
00500
00504 ContourPointData cp;
00505
00509 fmatvec::Vec3 WrOC[2];
00510 };
00511
00522 class Contact1sSearch {
00523 public:
00530 Contact1sSearch(DistanceFunction<double,double> *func_) : func(func_), jac(0), s0(0.), searchAll(false) {}
00531
00538 Contact1sSearch(DistanceFunction<double,double> *func_,Function1<double,double> *jac_) : func(func_), jac(jac_), s0(0.), searchAll(false) {}
00539
00540
00541 void setInitialValue(const double &s0_ ) { s0=s0_; }
00542 void setNodes(const fmatvec::Vec &nodes_) {nodes=nodes_;}
00543 void setSearchAll(bool searchAll_) { searchAll=searchAll_; }
00544
00545
00552 void setEqualSpacing(const int &n, const double &x0, const double &dx);
00553
00558 double slv();
00563 fmatvec::Mat slvAll();
00564
00565
00566 private:
00570 DistanceFunction<double,double> *func;
00571
00575 Function1<double,double> *jac;
00576
00580 double s0;
00581
00585 fmatvec::Vec nodes;
00586
00590 bool searchAll;
00591 };
00592
00593 }
00594
00595 #endif
00596