All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
functions_contact.h
1 /* Copyright (C) 2004-2015 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: thorsten.schindler@mytum.de
18  */
19 
20 #ifndef MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_
21 #define MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_
22 
23 #include "mbsim/contours/circle.h"
24 #include "mbsimFlexibleBody/contours/nurbs_disk_2s.h"
25 //#include "mbsim/utils/function.h"
26 #include "mbsim/functions_contact.h"
27 
28 namespace MBSimFlexibleBody {
29 
35  class FuncPairCircleNurbsDisk2s : public MBSim::DistanceFunction<double(double)> {
36  public:
42  FuncPairCircleNurbsDisk2s(MBSim::Circle* circle_, NurbsDisk2s* nurbsdisk_) : nurbsdisk(nurbsdisk_), circle(circle_) {}
43 
44  /* INHERITED INTERFACE OF DISTANCEFUNCTION */
45  double operator()(const double &alpha) {
46  //Parameters of the AWK of the nurbs disk and the circle
47  fmatvec::SqrMat3 AWK_disk = nurbsdisk->getFrame()->getOrientation();
48  fmatvec::SqrMat3 AWK_circle = circle->getFrame()->getOrientation();
49 
50  //Point on the Circle
51  fmatvec::Vec3 WP_circle(fmatvec::INIT,0.); //world-coordinates of the point on the circle
52  WP_circle(0) = cos(alpha);
53  WP_circle(1) = sin(alpha);
54  WP_circle = circle->getFrame()->getPosition() + circle->getRadius() * AWK_circle * WP_circle;
55 
56  //derivatives of a point on the circle in world-coordinates with respect to the circle-parameter alpha
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; //not normalised tangent on the circle
61 
62  //compute radial and azimuthal nurbsdisk-coordinates out of alpha (saved in the LagrangeParameterPosition)
63  MBSim::ContourPointData cp_nurbsdisk;
64  cp_nurbsdisk.getLagrangeParameterPosition() = nurbsdisk->transformCW( AWK_disk.T() * (WP_circle - nurbsdisk->getFrame()->getPosition()) )(0,1); // position of the point in the cylinder-coordinates of the disk
65 
66  //get the position and the derivatives on the disk
67  nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::Frame::firstTangent);
68 
69  //compute the derivates of the radial and the azimuthal coordinates with respect to alpha
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; // AWK_disk * A_inv * trans(AWK_disk)* circle_tangent CHANGED
76 
77  //compution of the single elements in the function
78  fmatvec::Vec nurbs_radial_tangent = cp_nurbsdisk.getFrameOfReference().getOrientation().col(1);
79  fmatvec::Vec nurbs_azimuthal_tangent = cp_nurbsdisk.getFrameOfReference().getOrientation().col(2);
80 
81  return nurbsdisk->getFrame()->getOrientation().col(2).T() * (circle_tangent - (nurbs_radial_tangent * drphidalpha(0)+ nurbs_azimuthal_tangent * drphidalpha(1)));
82  }
83 
84  fmatvec::Vec3 computeWrD(const double &alpha) {
85  //point on the circle
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;
90 
91  //get the position on the nurbsdisk
92  MBSim::ContourPointData cp_nurbsdisk;
93  cp_nurbsdisk.getLagrangeParameterPosition() = nurbsdisk->transformCW(nurbsdisk->getFrame()->getOrientation().T()*(WP_circle - nurbsdisk->getFrame()->getPosition()))(0,1); // position of the point in the cylinder-coordinates of the disk
94  nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::Frame::position);
95  fmatvec::Vec WP_nurbsdisk = cp_nurbsdisk.getFrameOfReference().getPosition();
96 
97  return WP_circle - WP_nurbsdisk;
98  }
99  /***************************************************/
100 
101  private:
105  NurbsDisk2s *nurbsdisk;
106  MBSim::Circle *circle;
107  };
108 
109 }
110 
111 #endif /* MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_ */
112 
FuncPairCircleNurbsDisk2s(MBSim::Circle *circle_, NurbsDisk2s *nurbsdisk_)
constructor
Definition: functions_contact.h:42
root function for pairing Circle and NurbsDisk2s
Definition: functions_contact.h:35
NurbsDisk2s * nurbsdisk
contours
Definition: functions_contact.h:105

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML