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/frustum.h"
00034 #include "mbsim/contours/plane.h"
00035 #include "mbsim/contours/planewithfrustum.h"
00036 #include "mbsim/contours/contour_interpolation.h"
00037 #include "mbsim/contours/contour_quad.h"
00038 #include "mbsim/contours/cuboid.h"
00039 #include "mbsim/contours/compound_contour.h"
00040 #include "mbsim/mbsim_event.h"
00041 #include <fmatvec/function.h>
00042
00043 namespace MBSim {
00044
00045 template<typename Sig> class DistanceFunction;
00046
00052 template<typename Ret, typename Arg>
00053 class DistanceFunction<Ret(Arg)> : public fmatvec::Function<Ret(Arg)> {
00054 public:
00055 virtual ~DistanceFunction() {
00056 }
00057
00062 virtual Ret operator()(const Arg &x) = 0;
00063
00068 virtual double operator[](const Arg& x) { return nrm2(computeWrD(x)); };
00069
00074 virtual fmatvec::Vec3 computeWrD(const Arg& x) = 0;
00075
00076 };
00077
00085 class FuncPairContour1sPoint : public DistanceFunction<double(double)> {
00086 public:
00092 FuncPairContour1sPoint(Point* point_, Contour1s *contour_) : contour(contour_), point(point_), cp() {}
00093
00094
00095 double operator()(const double &alpha) {
00096 fmatvec::Vec3 Wd = computeWrD(alpha);
00097 fmatvec::Vec3 Wt = cp.getFrameOfReference().getOrientation().col(1);
00098 return Wt.T()*Wd;
00099 }
00100
00101 fmatvec::Vec3 computeWrD(const double &alpha) {
00102
00103 cp.getLagrangeParameterPosition()(0) = alpha;
00104 contour->computeRootFunctionPosition(cp);
00105 contour->computeRootFunctionFirstTangent(cp);
00106
00107 return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00108 }
00109
00110
00111 private:
00115 Contour1s *contour;
00116 Point *point;
00117
00121 ContourPointData cp;
00122 };
00123
00131 class FuncPairContour1sCircleHollow : public DistanceFunction<double(double)> {
00132 public:
00138 FuncPairContour1sCircleHollow(CircleHollow* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00139
00140
00141 double operator()(const double &alpha) {
00142 fmatvec::Vec3 Wd = computeWrD(alpha);
00143 return circle->getReferenceOrientation().col(2).T()*Wd;
00144 }
00145
00146 fmatvec::Vec3 computeWrD(const double &alpha) {
00147
00148 cp.getLagrangeParameterPosition()(0) = alpha;
00149 contour->computeRootFunctionPosition(cp);
00150
00151 return circle->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00152 }
00153
00154
00155 private:
00159 Contour1s *contour;
00160 CircleHollow *circle;
00161
00165 ContourPointData cp;
00166 };
00167
00168
00174 class FuncPairPointContourInterpolation : public DistanceFunction<fmatvec::VecV(fmatvec::VecV)> {
00175 public:
00181 FuncPairPointContourInterpolation(Point* point_, ContourInterpolation *contour_) : contour(contour_), point(point_) {}
00182
00183
00184 fmatvec::VecV operator()(const fmatvec::VecV &alpha) {
00185 fmatvec::Mat3xV Wt = contour->computeWt(alpha);
00186 fmatvec::Vec3 WrOC[2];
00187 WrOC[0] = point->getFrame()->getPosition();
00188 WrOC[1] = contour->computeWrOC(alpha);
00189 return Wt.T() * ( WrOC[1] - WrOC[0] );
00190 }
00191
00192 fmatvec::Vec3 computeWrD(const fmatvec::VecV &alpha) {
00193 return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
00194 }
00195
00196
00197 private:
00201 ContourInterpolation *contour;
00202 Point *point;
00203 };
00204
00210 class FuncPairConeSectionCircle : public DistanceFunction<double(double)> {
00211 public:
00219 FuncPairConeSectionCircle(double R_,double a_,double b_) : R(R_), a(a_), b(b_), sec_IN_ci(true) {}
00220
00228 FuncPairConeSectionCircle(double R_,double a_,double b_,bool sec_IN_ci_) : R(R_), a(a_), b(b_), sec_IN_ci(sec_IN_ci_) {}
00229
00230
00231 virtual double operator()(const double &phi) = 0;
00232 double operator[](const double &phi);
00233 virtual fmatvec::Vec3 computeWrD(const double &phi) = 0;
00234
00235
00236
00237 void setDiffVec(fmatvec::Vec3 d_);
00238
00239 void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00240
00241
00242 protected:
00246 double R, a, b;
00247
00251 bool sec_IN_ci;
00252
00256 fmatvec::Vec3 b1, b2;
00257
00261 fmatvec::Vec3 d;
00262 };
00263
00264 inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00265 inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00266 inline double FuncPairConeSectionCircle::operator[](const double &phi) { if(sec_IN_ci) return R - nrm2(computeWrD(phi)); else return nrm2(computeWrD(phi)) - R; }
00267
00274 class FuncPairEllipseCircle : public FuncPairConeSectionCircle {
00275 public:
00283 FuncPairEllipseCircle(double R_,double a_,double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00284
00292 FuncPairEllipseCircle(double R_,double a_,double b_,bool el_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,el_IN_ci_) {}
00293
00294
00295 double operator()(const double &phi);
00296 fmatvec::Vec3 computeWrD(const double &phi);
00297
00298
00299
00300 void setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_);
00301
00302 };
00303
00304 inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec3 b1e_,fmatvec::Vec3 b2e_) {setSectionCOS(b1e_,b2e_);}
00305 inline double FuncPairEllipseCircle::operator()(const double &phi) { 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); }
00306 inline fmatvec::Vec3 FuncPairEllipseCircle::computeWrD(const double &phi) { return d + b1*a*cos(phi) + b2*b*sin(phi); }
00307
00313 class FuncPairHyperbolaCircle : public FuncPairConeSectionCircle {
00314 public:
00322 FuncPairHyperbolaCircle(double R_, double a_, double b_) : FuncPairConeSectionCircle(R_,a_,b_) {}
00323
00331 FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_) : FuncPairConeSectionCircle(R_,a_,b_,hy_IN_ci_) {}
00332
00333
00334 double operator()(const double &phi);
00335 fmatvec::Vec3 computeWrD(const double &phi);
00336
00337 };
00338
00339 inline double FuncPairHyperbolaCircle::operator()(const double &phi) { 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); }
00340 inline fmatvec::Vec3 FuncPairHyperbolaCircle::computeWrD(const double &phi) { return d + b1*a*cosh(phi) + b2*b*sinh(phi); }
00341
00347 class JacobianPairConeSectionCircle : public fmatvec::Function<double(double)> {
00348 public:
00354 JacobianPairConeSectionCircle(double a_, double b_) : a(a_), b(b_) {}
00355
00356
00357 void setDiffVec(fmatvec::Vec3 d_);
00358 void setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_);
00359
00360
00361 protected:
00365 double a, b;
00366
00370 fmatvec::Vec3 b1, b2;
00371
00375 fmatvec::Vec3 d;
00376 };
00377
00378 inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) { d=d_; }
00379 inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_,fmatvec::Vec3 b2_) { b1=b1_; b2=b2_; }
00380
00386 class JacobianPairEllipseCircle : public JacobianPairConeSectionCircle {
00387 public:
00393 JacobianPairEllipseCircle(double a_, double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00394
00395
00396 double operator()(const double &phi);
00397
00398 };
00399
00400 inline double JacobianPairEllipseCircle::operator()(const double &phi) { 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)); }
00401
00407 class JacobianPairHyperbolaCircle : public JacobianPairConeSectionCircle {
00408 public:
00414 JacobianPairHyperbolaCircle(double a_,double b_) : JacobianPairConeSectionCircle(a_,b_) {}
00415
00416
00417 double operator()(const double &phi);
00418
00419 };
00420
00421 inline double JacobianPairHyperbolaCircle::operator()(const double &phi) { 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));}
00422
00429 class FuncPairContour1sLine : public DistanceFunction<double(double)> {
00430 public:
00436 FuncPairContour1sLine(Line* line_, Contour1s *contour_) : contour(contour_), line(line_) {}
00437
00438
00439 virtual double operator()(const double &s) {
00440 throw MBSimError("ERROR (FuncPairContour1sLine::operator): Not implemented!");
00441
00442
00443
00444 }
00445
00446 virtual fmatvec::Vec3 computeWrD(const double &s) {
00447 throw MBSimError("ERROR (FuncPairContour1sLine::computeWrD): Not implemented!");
00448
00449
00450
00451
00452 }
00453
00454 virtual double operator[](const double &s) {
00455 return nrm2(computeWrD(s));
00456 }
00457
00458
00459 private:
00460 Contour1s *contour;
00461 Line *line;
00462 };
00463
00469 class FuncPairContour1sCircleSolid : public DistanceFunction<double(double)> {
00470 public:
00476 FuncPairContour1sCircleSolid(CircleSolid* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00477
00478
00479 double operator()(const double &alpha) {
00480 cp.getLagrangeParameterPosition()(0) = alpha;
00481 fmatvec::Vec3 Wd = computeWrD(alpha);
00482 fmatvec::Vec3 Wt = contour->computeTangent(cp);
00483 return Wt.T()*Wd;
00484 }
00485
00486 fmatvec::Vec3 computeWrD(const double &alpha) {
00487 cp.getLagrangeParameterPosition()(0) = alpha;
00488 contour->computeRootFunctionPosition(cp);
00489 contour->computeRootFunctionFirstTangent(cp);
00490 contour->computeRootFunctionNormal(cp);
00491 WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius()*cp.getFrameOfReference().getOrientation().col(0);
00492 WrOC[1] = cp.getFrameOfReference().getPosition();
00493 return WrOC[1] - WrOC[0];
00494 }
00495
00496
00497 private:
00501 Contour1s *contour;
00502 CircleSolid *circle;
00503
00507 ContourPointData cp;
00508
00512 fmatvec::Vec3 WrOC[2];
00513 };
00514
00525 class Contact1sSearch {
00526 public:
00533 Contact1sSearch(DistanceFunction<double(double)> *func_) : func(func_), jac(0), s0(0.), searchAll(false) {}
00534
00541 Contact1sSearch(DistanceFunction<double(double)> *func_,fmatvec::Function<double(double)> *jac_) : func(func_), jac(jac_), s0(0.), searchAll(false) {}
00542
00543
00544 void setInitialValue(const double &s0_ ) { s0=s0_; }
00545 void setNodes(const fmatvec::Vec &nodes_) {nodes=nodes_;}
00546 void setSearchAll(bool searchAll_) { searchAll=searchAll_; }
00547
00548
00555 void setEqualSpacing(const int &n, const double &x0, const double &dx);
00556
00561 double slv();
00566 fmatvec::Mat slvAll();
00567
00568
00569 private:
00573 DistanceFunction<double(double)> *func;
00574
00578 fmatvec::Function<double(double)> *jac;
00579
00583 double s0;
00584
00588 fmatvec::Vec nodes;
00589
00593 bool searchAll;
00594 };
00595
00596 }
00597
00598 #endif
00599