20 #ifndef MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_
21 #define MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_
23 #include "mbsim/contours/circle.h"
24 #include "mbsimFlexibleBody/contours/nurbs_disk_2s.h"
26 #include "mbsim/functions_contact.h"
28 namespace MBSimFlexibleBody {
45 double operator()(
const double &alpha,
const void * =NULL) {
47 fmatvec::SqrMat AWK_disk =
nurbsdisk->getFrame()->getOrientation();
48 fmatvec::SqrMat AWK_circle = circle->getFrame()->getOrientation();
51 fmatvec::Vec WP_circle(3,fmatvec::INIT,0.);
52 WP_circle(0) = cos(alpha);
53 WP_circle(1) = sin(alpha);
54 WP_circle = circle->getFrame()->getPosition() + circle->getRadius() * AWK_circle * WP_circle;
57 fmatvec::Vec dWP_circle(3,fmatvec::INIT, 0.);
58 dWP_circle(0) = -sin(alpha);
59 dWP_circle(1) = cos(alpha);
60 fmatvec::Vec circle_tangent = circle->getRadius() * AWK_circle * dWP_circle;
63 MBSim::ContourPointData cp_nurbsdisk;
67 nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::firstTangent);
70 fmatvec::SqrMat A_inv(3,fmatvec::EYE);
71 A_inv(0,0)= cos(cp_nurbsdisk.getLagrangeParameterPosition()(1));
72 A_inv(0,1)= sin(cp_nurbsdisk.getLagrangeParameterPosition()(1));
73 A_inv(1,0)= -sin(cp_nurbsdisk.getLagrangeParameterPosition()(1)) / cp_nurbsdisk.getLagrangeParameterPosition()(0);
74 A_inv(1,1)= cos(cp_nurbsdisk.getLagrangeParameterPosition()(1)) / cp_nurbsdisk.getLagrangeParameterPosition()(0);
75 fmatvec::Vec drphidalpha = A_inv * AWK_disk.T()* circle_tangent;
78 fmatvec::Vec nurbs_radial_tangent = cp_nurbsdisk.getFrameOfReference().getOrientation().col(1);
79 fmatvec::Vec nurbs_azimuthal_tangent = cp_nurbsdisk.getFrameOfReference().getOrientation().col(2);
81 return nurbsdisk->getFrame()->getOrientation().col(2).T() * (circle_tangent - (nurbs_radial_tangent * drphidalpha(0)+ nurbs_azimuthal_tangent * drphidalpha(1)));
84 fmatvec::Vec computeWrD(
const double &alpha) {
86 fmatvec::Vec WP_circle(3,fmatvec::INIT,0.);
87 WP_circle(0) = cos(alpha);
88 WP_circle(1) = sin(alpha);
89 WP_circle = circle->getFrame()->getPosition() + circle->getRadius() * circle->getFrame()->getOrientation() * WP_circle;
92 MBSim::ContourPointData cp_nurbsdisk;
94 nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::position);
95 fmatvec::Vec WP_nurbsdisk = cp_nurbsdisk.getFrameOfReference().getPosition();
97 return WP_circle - WP_nurbsdisk;
106 MBSim::Circle *circle;