21 #ifndef FUNCTIONS_CONTACT_H_
22 #define FUNCTIONS_CONTACT_H_
24 #include <mbsim/contour.h>
25 #include <mbsim/contours/contour1s.h>
26 #include <mbsim/contours/point.h>
27 #include "mbsim/contours/line.h"
28 #include <mbsim/contours/circle_solid.h>
29 #include "mbsim/contours/circle_hollow.h"
30 #include "mbsim/contours/circle.h"
31 #include "mbsim/contours/frustum2d.h"
32 #include "mbsim/contours/edge.h"
33 #include "mbsim/contours/frustum.h"
34 #include "mbsim/contours/plane.h"
35 #include "mbsim/contours/contour2s.h"
36 #include "mbsim/contours/planewithfrustum.h"
37 #include "mbsim/contours/contour_interpolation.h"
38 #include "mbsim/contours/contour_quad.h"
39 #include "mbsim/contours/cuboid.h"
40 #include "mbsim/contours/compound_contour.h"
41 #include "mbsim/mbsim_event.h"
42 #include <mbsim/functions/function.h>
53 template <
typename Ret,
typename Arg>
63 virtual Ret operator()(
const Arg &x) = 0;
69 virtual double operator[](
const Arg& x) {
return nrm2(computeWrD(x)); }
75 virtual fmatvec::Vec3 computeWrD(
const Arg& x) = 0;
96 double operator()(
const double &alpha) {
97 fmatvec::Vec3 Wd = computeWrD(alpha);
98 fmatvec::Vec3 Wt =
cp.getFrameOfReference().getOrientation().col(1);
102 fmatvec::Vec3 computeWrD(
const double &alpha) {
104 cp.getLagrangeParameterPosition()(0) = alpha;
105 contour->computeRootFunctionPosition(
cp);
106 contour->computeRootFunctionFirstTangent(
cp);
108 return point->getFrame()->getPosition() -
cp.getFrameOfReference().getPosition();
143 fmatvec::Vec3 Wd = computeWrD(alpha);
144 fmatvec::Vec3 Wt1 =
cp.getFrameOfReference().getOrientation().col(1);
145 fmatvec::Vec3 Wt2 =
cp.getFrameOfReference().getOrientation().col(2);
147 Wt(0) = Wt1.T() * Wd;
148 Wt(1) = Wt2.T() * Wd;
153 cp.getLagrangeParameterPosition()(0) = alpha(0);
154 cp.getLagrangeParameterPosition()(1) = alpha(1);
155 contour->computeRootFunctionPosition(
cp);
156 contour->computeRootFunctionFirstTangent(
cp);
157 contour->computeRootFunctionSecondTangent(
cp);
158 return point->getFrame()->getPosition() -
cp.getFrameOfReference().getPosition();
191 double operator()(
const double &alpha) {
192 fmatvec::Vec3 Wd = computeWrD(alpha);
193 return circle->getReferenceOrientation().col(2).T() * Wd;
196 fmatvec::Vec3 computeWrD(
const double &alpha) {
198 cp.getLagrangeParameterPosition()(0) = alpha;
199 contour->computeRootFunctionPosition(
cp);
201 return circle->getFrame()->getPosition() -
cp.getFrameOfReference().getPosition();
210 CircleHollow *circle;
233 fmatvec::VecV operator()(
const fmatvec::VecV &alpha) {
234 fmatvec::Mat3xV Wt =
contour->computeWt(alpha);
235 fmatvec::Vec3 WrOC[2];
236 WrOC[0] = point->getFrame()->getPosition();
237 WrOC[1] =
contour->computeWrOC(alpha);
238 return Wt.T() * (WrOC[1] - WrOC[0]);
241 fmatvec::Vec3 computeWrD(
const fmatvec::VecV &alpha) {
242 return contour->computeWrOC(alpha) - point->getFrame()->getPosition();
282 virtual double operator()(
const double &phi) = 0;
283 double operator[](
const double &phi);
284 virtual fmatvec::Vec3 computeWrD(
const double &phi) = 0;
288 void setDiffVec(fmatvec::Vec3 d_);
290 void setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_);
307 fmatvec::Vec3
b1, b2;
315 inline void FuncPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) {
318 inline void FuncPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_) {
322 inline double FuncPairConeSectionCircle::operator[](
const double &phi) {
324 return R -
nrm2(computeWrD(phi));
326 return nrm2(computeWrD(phi)) -
R;
360 double operator()(
const double &phi);
361 fmatvec::Vec3 computeWrD(
const double &phi);
365 void setEllipseCOS(fmatvec::Vec3 b1e_, fmatvec::Vec3 b2e_);
369 inline void FuncPairEllipseCircle::setEllipseCOS(fmatvec::Vec3 b1e_, fmatvec::Vec3 b2e_) {
370 setSectionCOS(b1e_, b2e_);
372 inline double FuncPairEllipseCircle::operator()(
const double &phi) {
373 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);
375 inline fmatvec::Vec3 FuncPairEllipseCircle::computeWrD(
const double &phi) {
376 return d +
b1 * a * cos(phi) + b2 * b * sin(phi);
409 double operator()(
const double &phi);
410 fmatvec::Vec3 computeWrD(
const double &phi);
414 inline double FuncPairHyperbolaCircle::operator()(
const double &phi) {
415 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);
417 inline fmatvec::Vec3 FuncPairHyperbolaCircle::computeWrD(
const double &phi) {
418 return d +
b1 * a * cosh(phi) + b2 * b * sinh(phi);
438 void setDiffVec(fmatvec::Vec3 d_);
439 void setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_);
451 fmatvec::Vec3
b1, b2;
459 inline void JacobianPairConeSectionCircle::setDiffVec(fmatvec::Vec3 d_) {
462 inline void JacobianPairConeSectionCircle::setSectionCOS(fmatvec::Vec3 b1_, fmatvec::Vec3 b2_) {
484 double operator()(
const double &phi);
488 inline double JacobianPairEllipseCircle::operator()(
const double &phi) {
489 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));
509 double operator()(
const double &phi);
513 inline double JacobianPairHyperbolaCircle::operator()(
const double &phi) {
514 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));
531 contour(contour_), line(line_) {
535 virtual double operator()(
const double &s) {
536 THROW_MBSIMERROR(
"(FuncPairContour1sLine::operator): Not implemented!");
542 virtual fmatvec::Vec3 computeWrD(
const double &s) {
543 THROW_MBSIMERROR(
"(FuncPairContour1sLine::computeWrD): Not implemented!");
550 virtual double operator[](
const double &s) {
551 return nrm2(computeWrD(s));
573 contour(contour_), circle(circle_) {
577 double operator()(
const double &alpha) {
578 cp.getLagrangeParameterPosition()(0) = alpha;
579 fmatvec::Vec3 Wd = computeWrD(alpha);
580 fmatvec::Vec3 Wt =
contour->computeTangent(
cp);
584 fmatvec::Vec3 computeWrD(
const double &alpha) {
585 cp.getLagrangeParameterPosition()(0) = alpha;
586 contour->computeRootFunctionPosition(
cp);
587 contour->computeRootFunctionFirstTangent(
cp);
589 WrOC[0] = circle->getFrame()->getPosition() - circle->getRadius() *
cp.getFrameOfReference().getOrientation().col(0);
590 WrOC[1] =
cp.getFrameOfReference().getPosition();
591 return WrOC[1] - WrOC[0];
610 fmatvec::Vec3 WrOC[2];
657 void setJacobianFunction(Function<
double(
double)> *jac_) {
661 void setInitialValue(
const double &s0_) {
667 void setSearchAll(
bool searchAll_) {
678 void setEqualSpacing(
const int &n,
const double &x0,
const double &dx);
700 Function<double(double)> *
jac;
749 void setInitialValue(
const fmatvec::Vec2 &s0_) {
756 void setSearchAll(
bool searchAll_) {
770 void setEqualSpacing(
const int nU,
const int nV,
const double U0,
const double V0,
const double dU,
const double dV);
799 Function<fmatvec::Mat(fmatvec::Mat)> *
jac;
JacobianPairHyperbolaCircle(double a_, double b_)
constructor
Definition: functions_contact.h:504
double a
length in b1- and b2-direction
Definition: functions_contact.h:446
JacobianPairEllipseCircle(double a_, double b_)
constructor
Definition: functions_contact.h:479
Definition: functions_contact.h:46
root function for planar pairing Ellipse and Circle
Definition: functions_contact.h:335
Contour1s * contour
contours
Definition: functions_contact.h:116
FuncPairContour1sCircleHollow(CircleHollow *circle_, Contour1s *contour_)
constructor
Definition: functions_contact.h:188
virtual double operator[](const Arg &x)
Definition: functions_contact.h:69
fmatvec::Vec3 d
distance-vector of circle- and cone-section-midpoint
Definition: functions_contact.h:456
root function for pairing CylinderFlexible and CircleHollow
Definition: functions_contact.h:181
FuncPairPointContourInterpolation(Point *point_, ContourInterpolation *contour_)
constructor
Definition: functions_contact.h:230
FuncPairEllipseCircle(double R_, double a_, double b_, bool el_IN_ci_)
constructor
Definition: functions_contact.h:355
double nrm2(const Vector< Ref, double > &x)
FuncPairContour1sPoint(Point *point_, Contour1s *contour_)
constructor
Definition: functions_contact.h:93
root function for pairing Contour1s and Point
Definition: functions_contact.h:86
root function for pairing Contour1s and Line
Definition: functions_contact.h:523
Contour1s * contour
contours
Definition: functions_contact.h:209
ContourPointData cp
contour point data for saving old values
Definition: functions_contact.h:605
FuncPairEllipseCircle(double R_, double a_, double b_)
constructor
Definition: functions_contact.h:344
FuncPairContour1sLine(Line *line_, Contour1s *contour_)
constructor
Definition: functions_contact.h:530
root function for pairing Contour1s and Circle
Definition: functions_contact.h:565
JacobianPairConeSectionCircle(double a_, double b_)
constructor
Definition: functions_contact.h:433
FuncPairHyperbolaCircle(double R_, double a_, double b_, bool hy_IN_ci_)
constructor
Definition: functions_contact.h:404
base root function for planar pairing ConeSection and Circle
Definition: functions_contact.h:259
Jacobian of root function for planar pairing Ellipse and Circle.
Definition: functions_contact.h:472
base Jacobian of root function for planar pairing ConeSection and Circle
Definition: functions_contact.h:426
root function for pairing Contour2s and Point
Definition: functions_contact.h:130
fmatvec::Vec3 b1
normed base-vectors of cone-section
Definition: functions_contact.h:451
Contour1s * contour
contours
Definition: functions_contact.h:599
fmatvec::Vec3 b1
normed base-vectors of cone-section
Definition: functions_contact.h:307
Jacobian of root function for planar pairing Hyperbola and Circle.
Definition: functions_contact.h:497
ContourPointData cp
contour point data for saving old values
Definition: functions_contact.h:171
FuncPairContour2sPoint(Point *point_, Contour2s *contour_)
constructor
Definition: functions_contact.h:137
Contour2s * contour
contours
Definition: functions_contact.h:166
ContourInterpolation * contour
contours
Definition: functions_contact.h:250
FuncPairHyperbolaCircle(double R_, double a_, double b_)
constructor
Definition: functions_contact.h:393
ContourPointData cp
contour point data for saving old values
Definition: functions_contact.h:122
FuncPairConeSectionCircle(double R_, double a_, double b_, bool sec_IN_ci_)
constructor
Definition: functions_contact.h:277
struct for data-management for single point on a contour to describe contact kinematics ...
Definition: contour_pdata.h:38
root function for pairing ContourInterpolation and Point
Definition: functions_contact.h:223
ContourPointData cp
contour point data for saving old values
Definition: functions_contact.h:215
FuncPairContour1sCircleSolid(CircleSolid *circle_, Contour1s *contour_)
constructor
Definition: functions_contact.h:572
fmatvec::Vec3 d
distance-vector of cone-section- and circle-midpoint
Definition: functions_contact.h:312
FuncPairConeSectionCircle(double R_, double a_, double b_)
constructor
Definition: functions_contact.h:268
root function for planar pairing Hyperbola and Circle
Definition: functions_contact.h:384
double R
radius of circle as well as length in b1- and b2-direction
Definition: functions_contact.h:297
bool sec_IN_ci
cone-section in circle
Definition: functions_contact.h:302