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/contour2s.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/mbsim_event.h"
00042 #include <mbsim/functions/function.h>
00043
00044 namespace MBSim {
00045
00046 template <typename Sig> class DistanceFunction;
00047
00053 template <typename Ret, typename Arg>
00054 class DistanceFunction<Ret(Arg)> : public Function<Ret(Arg)> {
00055 public:
00056 virtual ~DistanceFunction() {
00057 }
00058
00063 virtual Ret operator()(const Arg &x) = 0;
00064
00069 virtual double operator[](const Arg& x) { return nrm2(computeWrD(x)); }
00070
00075 virtual fmatvec::Vec3 computeWrD(const Arg& x) = 0;
00076
00077 };
00078
00086 class FuncPairContour1sPoint : public DistanceFunction<double(double)> {
00087 public:
00093 FuncPairContour1sPoint(Point* point_, Contour1s *contour_) : contour(contour_), point(point_), cp() {}
00094
00095
00096 double operator()(const double &alpha) {
00097 fmatvec::Vec3 Wd = computeWrD(alpha);
00098 fmatvec::Vec3 Wt = cp.getFrameOfReference().getOrientation().col(1);
00099 return Wt.T() * Wd;
00100 }
00101
00102 fmatvec::Vec3 computeWrD(const double &alpha) {
00103
00104 cp.getLagrangeParameterPosition()(0) = alpha;
00105 contour->computeRootFunctionPosition(cp);
00106 contour->computeRootFunctionFirstTangent(cp);
00107
00108 return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00109 }
00110
00111
00112 private:
00116 Contour1s *contour;
00117 Point *point;
00118
00122 ContourPointData cp;
00123 };
00124
00130 class FuncPairContour2sPoint : public DistanceFunction<fmatvec::Vec(fmatvec::Vec)> {
00131 public:
00137 FuncPairContour2sPoint(Point* point_, Contour2s *contour_) :
00138 contour(contour_), point(point_), cp() {
00139 }
00140
00141
00142 fmatvec::Vec operator()(const fmatvec::Vec &alpha) {
00143 fmatvec::Vec3 Wd = computeWrD(alpha);
00144 fmatvec::Vec3 Wt1 = cp.getFrameOfReference().getOrientation().col(1);
00145 fmatvec::Vec3 Wt2 = cp.getFrameOfReference().getOrientation().col(2);
00146 fmatvec::Vec Wt(2, fmatvec::NONINIT);
00147 Wt(0) = Wt1.T() * Wd;
00148 Wt(1) = Wt2.T() * Wd;
00149 return Wt;
00150 }
00151
00152 fmatvec::Vec3 computeWrD(const fmatvec::Vec &alpha) {
00153 cp.getLagrangeParameterPosition()(0) = alpha(0);
00154 cp.getLagrangeParameterPosition()(1) = alpha(1);
00155 contour->computeRootFunctionPosition(cp);
00156 contour->computeRootFunctionFirstTangent(cp);
00157 contour->computeRootFunctionSecondTangent(cp);
00158 return point->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00159 }
00160
00161
00162 private:
00166 Contour2s *contour;
00167 Point *point;
00171 ContourPointData cp;
00172 };
00173
00181 class FuncPairContour1sCircleHollow : public DistanceFunction<double(double)> {
00182 public:
00188 FuncPairContour1sCircleHollow(CircleHollow* circle_, Contour1s *contour_) : contour(contour_), circle(circle_) {}
00189
00190
00191 double operator()(const double &alpha) {
00192 fmatvec::Vec3 Wd = computeWrD(alpha);
00193 return circle->getReferenceOrientation().col(2).T() * Wd;
00194 }
00195
00196 fmatvec::Vec3 computeWrD(const double &alpha) {
00197
00198 cp.getLagrangeParameterPosition()(0) = alpha;
00199 contour->computeRootFunctionPosition(cp);
00200
00201 return circle->getFrame()->getPosition() - cp.getFrameOfReference().getPosition();
00202 }
00203
00204
00205 private:
00209 Contour1s *contour;
00210 CircleHollow *circle;
00211
00215 ContourPointData cp;
00216 };
00217
00223 class FuncPairPointContourInterpolation : public DistanceFunction<fmatvec::VecV(fmatvec::VecV)> {
00224 public:
00230 FuncPairPointContourInterpolation(Point* point_, ContourInterpolation *contour_) : contour(contour_), point(point_) {}
00231
00232
00233 fmatvec::VecV operator()(const fmatvec::VecV &alpha) {
00234 fmatvec::Mat3xV Wt = contour->computeWt(alpha);
00235 fmatvec::Vec3 WrOC[2];
00236 WrOC[0] = point->getFrame()->getPosition();
00237 WrOC[1] = contour->computeWrOC(alpha);
00238 return Wt.T() * (WrOC[1] - WrOC[0]);
00239 }
00240
00241 fmatvec::Vec3 computeWrD(const fmatvec::VecV &alpha) {
00242 return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
00243 }
00244
00245
00246 private:
00250 ContourInterpolation *contour;
00251 Point *point;
00252 };
00253
00259 class FuncPairConeSectionCircle : public DistanceFunction<double(double)> {
00260 public:
00268 FuncPairConeSectionCircle(double R_,double a_,double b_) : R(R_), a(a_), b(b_), sec_IN_ci(true) {}
00269
00277 FuncPairConeSectionCircle(double R_, double a_, double b_, bool sec_IN_ci_) :
00278 R(R_), a(a_), b(b_), sec_IN_ci(sec_IN_ci_) {
00279 }
00280
00281
00282 virtual double operator()(const double &phi) = 0;
00283 double operator[](const double &phi);
00284 virtual fmatvec::Vec3 computeWrD(const double &phi) = 0;
00285
00286
00287
00288 void setDiffVec(fmatvec::Vec3 d_);
00289
00290 void setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_);
00291
00292
00293 protected:
00297 double R, a, b;
00298
00302 bool sec_IN_ci;
00303
00307 fmatvec::Vec3 b1, b2;
00308
00312 fmatvec::Vec3 d;
00313 };
00314
00315 inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) {
00316 d = d_;
00317 }
00318 inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_) {
00319 b1 = b1_;
00320 b2 = b2_;
00321 }
00322 inline double FuncPairConeSectionCircle::operator[](const double &phi) {
00323 if (sec_IN_ci)
00324 return R - nrm2(computeWrD(phi));
00325 else
00326 return nrm2(computeWrD(phi)) - R;
00327 }
00328
00335 class FuncPairEllipseCircle : public FuncPairConeSectionCircle {
00336 public:
00344 FuncPairEllipseCircle(double R_, double a_, double b_) :
00345 FuncPairConeSectionCircle(R_, a_, b_) {
00346 }
00347
00355 FuncPairEllipseCircle(double R_, double a_, double b_, bool el_IN_ci_) :
00356 FuncPairConeSectionCircle(R_, a_, b_, el_IN_ci_) {
00357 }
00358
00359
00360 double operator()(const double &phi);
00361 fmatvec::Vec3 computeWrD(const double &phi);
00362
00363
00364
00365 void setEllipseCOS(fmatvec::Vec3 b1e_, fmatvec::Vec3 b2e_);
00366
00367 };
00368
00369 inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec3 b1e_, fmatvec::Vec3 b2e_) {
00370 setSectionCOS(b1e_, b2e_);
00371 }
00372 inline double FuncPairEllipseCircle::operator()(const double &phi) {
00373 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);
00374 }
00375 inline fmatvec::Vec3 FuncPairEllipseCircle::computeWrD(const double &phi) {
00376 return d + b1 * a * cos(phi) + b2 * b * sin(phi);
00377 }
00378
00384 class FuncPairHyperbolaCircle : public FuncPairConeSectionCircle {
00385 public:
00393 FuncPairHyperbolaCircle(double R_, double a_, double b_) :
00394 FuncPairConeSectionCircle(R_, a_, b_) {
00395 }
00396
00404 FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_) :
00405 FuncPairConeSectionCircle(R_, a_, b_, hy_IN_ci_) {
00406 }
00407
00408
00409 double operator()(const double &phi);
00410 fmatvec::Vec3 computeWrD(const double &phi);
00411
00412 };
00413
00414 inline double FuncPairHyperbolaCircle::operator()(const double &phi) {
00415 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);
00416 }
00417 inline fmatvec::Vec3 FuncPairHyperbolaCircle::computeWrD(const double &phi) {
00418 return d + b1 * a * cosh(phi) + b2 * b * sinh(phi);
00419 }
00420
00426 class JacobianPairConeSectionCircle : public Function<double(double)> {
00427 public:
00433 JacobianPairConeSectionCircle(double a_, double b_) :
00434 a(a_), b(b_) {
00435 }
00436
00437
00438 void setDiffVec(fmatvec::Vec3 d_);
00439 void setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_);
00440
00441
00442 protected:
00446 double a, b;
00447
00451 fmatvec::Vec3 b1, b2;
00452
00456 fmatvec::Vec3 d;
00457 };
00458
00459 inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) {
00460 d = d_;
00461 }
00462 inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_) {
00463 b1 = b1_;
00464 b2 = b2_;
00465 }
00466
00472 class JacobianPairEllipseCircle : public JacobianPairConeSectionCircle {
00473 public:
00479 JacobianPairEllipseCircle(double a_, double b_) :
00480 JacobianPairConeSectionCircle(a_, b_) {
00481 }
00482
00483
00484 double operator()(const double &phi);
00485
00486 };
00487
00488 inline double JacobianPairEllipseCircle::operator()(const double &phi) {
00489 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));
00490 }
00491
00497 class JacobianPairHyperbolaCircle : public JacobianPairConeSectionCircle {
00498 public:
00504 JacobianPairHyperbolaCircle(double a_, double b_) :
00505 JacobianPairConeSectionCircle(a_, b_) {
00506 }
00507
00508
00509 double operator()(const double &phi);
00510
00511 };
00512
00513 inline double JacobianPairHyperbolaCircle::operator()(const double &phi) {
00514 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));
00515 }
00516
00523 class FuncPairContour1sLine : public DistanceFunction<double(double)> {
00524 public:
00530 FuncPairContour1sLine(Line* line_, Contour1s *contour_) :
00531 contour(contour_), line(line_) {
00532 }
00533
00534
00535 virtual double operator()(const double &s) {
00536 THROW_MBSIMERROR("(FuncPairContour1sLine::operator): Not implemented!");
00537
00538
00539
00540 }
00541
00542 virtual fmatvec::Vec3 computeWrD(const double &s) {
00543 THROW_MBSIMERROR("(FuncPairContour1sLine::computeWrD): Not implemented!");
00544
00545
00546
00547
00548 }
00549
00550 virtual double operator[](const double &s) {
00551 return nrm2(computeWrD(s));
00552 }
00553
00554
00555 private:
00556 Contour1s *contour;
00557 Line *line;
00558 };
00559
00565 class FuncPairContour1sCircleSolid : public DistanceFunction<double(double)> {
00566 public:
00572 FuncPairContour1sCircleSolid(CircleSolid* circle_, Contour1s *contour_) :
00573 contour(contour_), circle(circle_) {
00574 }
00575
00576
00577 double operator()(const double &alpha) {
00578 cp.getLagrangeParameterPosition()(0) = alpha;
00579 fmatvec::Vec3 Wd = computeWrD(alpha);
00580 fmatvec::Vec3 Wt = contour->computeTangent(cp);
00581 return Wt.T() * Wd;
00582 }
00583
00584 fmatvec::Vec3 computeWrD(const double &alpha) {
00585 cp.getLagrangeParameterPosition()(0) = alpha;
00586 contour->computeRootFunctionPosition(cp);
00587 contour->computeRootFunctionFirstTangent(cp);
00588 contour->computeRootFunctionNormal(cp);
00589 WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius() * cp.getFrameOfReference().getOrientation().col(0);
00590 WrOC[1] = cp.getFrameOfReference().getPosition();
00591 return WrOC[1] - WrOC[0];
00592 }
00593
00594
00595 private:
00599 Contour1s *contour;
00600 CircleSolid *circle;
00601
00605 ContourPointData cp;
00606
00610 fmatvec::Vec3 WrOC[2];
00611 };
00612
00623 class Contact1sSearch {
00624 public:
00628 Contact1sSearch() :
00629 func(0), jac(0), s0(0.), searchAll(false) {
00630 }
00631
00638 Contact1sSearch(DistanceFunction<double(double)> *func_) :
00639 func(func_), jac(0), s0(0.), searchAll(false) {
00640 }
00641
00648 Contact1sSearch(DistanceFunction<double(double)> *func_, Function<double(double)> *jac_) :
00649 func(func_), jac(jac_), s0(0.), searchAll(false) {
00650 }
00651
00652
00653 void setFunction(DistanceFunction<double(double)> *func_) {
00654 func = func_;
00655 }
00656
00657 void setJacobianFunction(Function<double(double)> *jac_) {
00658 jac = jac_;
00659 }
00660
00661 void setInitialValue(const double &s0_) {
00662 s0 = s0_;
00663 }
00664 void setNodes(const fmatvec::Vec &nodes_) {
00665 nodes = nodes_;
00666 }
00667 void setSearchAll(bool searchAll_) {
00668 searchAll = searchAll_;
00669 }
00670
00671
00678 void setEqualSpacing(const int &n, const double &x0, const double &dx);
00679
00684 double slv();
00689 fmatvec::Mat slvAll();
00690
00691 private:
00695 DistanceFunction<double(double)> *func;
00696
00700 Function<double(double)> *jac;
00701
00705 double s0;
00706
00710 fmatvec::Vec nodes;
00711
00715 bool searchAll;
00716 };
00717
00726 class Contact2sSearch {
00727 public:
00734 Contact2sSearch(DistanceFunction<fmatvec::Vec(fmatvec::Vec)> *func_) :
00735 func(func_), jac(0), s0(2), searchAll(false) {
00736 }
00737
00744 Contact2sSearch(DistanceFunction<fmatvec::Vec(fmatvec::Vec)> *func_, Function<fmatvec::Mat(fmatvec::Mat)> *jac_) :
00745 func(func_), jac(jac_), s0(2), searchAll(false) {
00746 }
00747
00748
00749 void setInitialValue(const fmatvec::Vec2 &s0_) {
00750 s0 = s0_;
00751 }
00752 void setNodes(const fmatvec::Vec &nodesU_, const fmatvec::Vec &nodesV_) {
00753 nodesU = nodesU_;
00754 nodesV = nodesV_;
00755 }
00756 void setSearchAll(bool searchAll_) {
00757 searchAll = searchAll_;
00758 }
00759
00760
00770 void setEqualSpacing(const int nU, const int nV, const double U0, const double V0, const double dU, const double dV);
00771
00776 fmatvec::Vec2 slv();
00777
00778
00779
00780
00781
00782
00783
00784 protected:
00788 std::vector<double> searchVdirection(double u);
00789
00790 private:
00794 DistanceFunction<fmatvec::Vec(fmatvec::Vec)> *func;
00795
00799 Function<fmatvec::Mat(fmatvec::Mat)> *jac;
00800
00804 fmatvec::Vec2 s0;
00805
00809 fmatvec::Vec nodesU, nodesV;
00810
00814 bool searchAll;
00815 };
00816 }
00817
00818 #endif
00819