|
mbsimflexiblebody
3.2.0
|
00001 /* Copyright (C) 2004-2011 MBSim Development Team 00002 * 00003 * This library is free software; you can redistribute it and/or 00004 * modify it under the terms of the GNU Lesser General Public 00005 * License as published by the Free Software Foundation; either 00006 * version 2.1 of the License, or (at your option) any later version. 00007 * 00008 * This library is distributed in the hope that it will be useful, 00009 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00010 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00011 * Lesser General Public License for more details. 00012 * 00013 * You should have received a copy of the GNU Lesser General Public 00014 * License along with this library; if not, write to the Free Software 00015 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00016 * 00017 * Contact: thorsten.schindler@mytum.de 00018 */ 00019 00020 #ifndef MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_ 00021 #define MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_ 00022 00023 #include "mbsim/contours/circle.h" 00024 #include "mbsimFlexibleBody/contours/nurbs_disk_2s.h" 00025 //#include "mbsim/utils/function.h" 00026 #include "mbsim/functions_contact.h" 00027 00028 namespace MBSimFlexibleBody { 00029 00035 class FuncPairCircleNurbsDisk2s : public MBSim::DistanceFunction<double,double> { 00036 public: 00042 FuncPairCircleNurbsDisk2s(MBSim::Circle* circle_, NurbsDisk2s* nurbsdisk_) : nurbsdisk(nurbsdisk_), circle(circle_) {} 00043 00044 /* INHERITED INTERFACE OF DISTANCEFUNCTION */ 00045 double operator()(const double &alpha, const void * =NULL) { 00046 //Parameters of the AWK of the nurbs disk and the circle 00047 fmatvec::SqrMat3 AWK_disk = nurbsdisk->getFrame()->getOrientation(); 00048 fmatvec::SqrMat3 AWK_circle = circle->getFrame()->getOrientation(); 00049 00050 //Point on the Circle 00051 fmatvec::Vec3 WP_circle(fmatvec::INIT,0.); //world-coordinates of the point on the circle 00052 WP_circle(0) = cos(alpha); 00053 WP_circle(1) = sin(alpha); 00054 WP_circle = circle->getFrame()->getPosition() + circle->getRadius() * AWK_circle * WP_circle; 00055 00056 //derivatives of a point on the circle in world-coordinates with respect to the circle-parameter alpha 00057 fmatvec::Vec dWP_circle(3,fmatvec::INIT, 0.); 00058 dWP_circle(0) = -sin(alpha); 00059 dWP_circle(1) = cos(alpha); 00060 fmatvec::Vec circle_tangent = circle->getRadius() * AWK_circle * dWP_circle; //not normalised tangent on the circle 00061 00062 //compute radial and azimuthal nurbsdisk-coordinates out of alpha (saved in the LagrangeParameterPosition) 00063 MBSim::ContourPointData cp_nurbsdisk; 00064 cp_nurbsdisk.getLagrangeParameterPosition() = nurbsdisk->transformCW( AWK_disk.T() * (WP_circle - nurbsdisk->getFrame()->getPosition()) ); // position of the point in the cylinder-coordinates of the disk 00065 00066 //get the position and the derivatives on the disk 00067 nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::firstTangent); 00068 00069 //compute the derivates of the radial and the azimuthal coordinates with respect to alpha 00070 fmatvec::SqrMat A_inv(3,fmatvec::EYE); 00071 A_inv(0,0)= cos(cp_nurbsdisk.getLagrangeParameterPosition()(1)); 00072 A_inv(0,1)= sin(cp_nurbsdisk.getLagrangeParameterPosition()(1)); 00073 A_inv(1,0)= -sin(cp_nurbsdisk.getLagrangeParameterPosition()(1)) / cp_nurbsdisk.getLagrangeParameterPosition()(0); 00074 A_inv(1,1)= cos(cp_nurbsdisk.getLagrangeParameterPosition()(1)) / cp_nurbsdisk.getLagrangeParameterPosition()(0); 00075 fmatvec::Vec drphidalpha = A_inv * AWK_disk.T()* circle_tangent; // AWK_disk * A_inv * trans(AWK_disk)* circle_tangent CHANGED 00076 00077 //compution of the single elements in the function 00078 fmatvec::Vec nurbs_radial_tangent = cp_nurbsdisk.getFrameOfReference().getOrientation().col(1); 00079 fmatvec::Vec nurbs_azimuthal_tangent = cp_nurbsdisk.getFrameOfReference().getOrientation().col(2); 00080 00081 return nurbsdisk->getFrame()->getOrientation().col(2).T() * (circle_tangent - (nurbs_radial_tangent * drphidalpha(0)+ nurbs_azimuthal_tangent * drphidalpha(1))); 00082 } 00083 00084 fmatvec::Vec3 computeWrD(const double &alpha) { 00085 //point on the circle 00086 fmatvec::Vec WP_circle(3,fmatvec::INIT,0.); 00087 WP_circle(0) = cos(alpha); 00088 WP_circle(1) = sin(alpha); 00089 WP_circle = circle->getFrame()->getPosition() + circle->getRadius() * circle->getFrame()->getOrientation() * WP_circle; 00090 00091 //get the position on the nurbsdisk 00092 MBSim::ContourPointData cp_nurbsdisk; 00093 cp_nurbsdisk.getLagrangeParameterPosition() = nurbsdisk->transformCW(nurbsdisk->getFrame()->getOrientation().T()*(WP_circle - nurbsdisk->getFrame()->getPosition())); // position of the point in the cylinder-coordinates of the disk 00094 nurbsdisk->updateKinematicsForFrame(cp_nurbsdisk,MBSim::position); 00095 fmatvec::Vec WP_nurbsdisk = cp_nurbsdisk.getFrameOfReference().getPosition(); 00096 00097 return WP_circle - WP_nurbsdisk; 00098 } 00099 /***************************************************/ 00100 00101 private: 00105 NurbsDisk2s *nurbsdisk; 00106 MBSim::Circle *circle; 00107 }; 00108 00109 } 00110 00111 #endif /* MBSIMFLEXIBLEBODY_FUNCTIONS_CONTACT_H_ */ 00112
1.8.0