MBSim |
Project Page |
00001 /* Copyright (C) 2004-2012 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: martin.o.foerg@googlemail.com 00018 */ 00019 00020 #ifndef SPHERE_POLYNOMIALFRUSTUM_H_ 00021 #define SPHERE_POLYNOMIALFRUSTUM_H_ 00022 00023 #include "contact_kinematics.h" 00024 00025 #include <mbsim/contours/polynomial_frustum.h> 00026 #include <mbsim/contours/sphere.h> 00027 #include <mbsim/numerics/nonlinear_algebra/multi_dimensional_newton_method.h> 00028 00029 namespace MBSim { 00030 00031 class PolyFurstumSphereContact : public fmatvec::Function<fmatvec::Vec(fmatvec::Vec)> { 00032 public: 00033 PolyFurstumSphereContact(const PolynomialFrustum * frustum) : sphereCenter(fmatvec::Vec3()), rS(0.), frustum(frustum) { 00034 } 00035 00036 virtual ~PolyFurstumSphereContact() { 00037 } 00038 00039 void setCenter(const fmatvec::Vec3 & sphereCenter_) { 00040 sphereCenter = sphereCenter_; 00041 rS = sqrt(sphereCenter(1) * sphereCenter(1) + sphereCenter(2) * sphereCenter(2)); 00042 } 00043 00044 fmatvec::Vec operator()(const fmatvec::Vec &x); 00045 00046 protected: 00050 fmatvec::Vec3 sphereCenter; 00051 00055 double rS; 00056 00060 const PolynomialFrustum * frustum; 00061 }; 00062 00063 class PolyFurstumSphereContactJacobian : public NewtonJacobianFunction { 00064 public: 00065 PolyFurstumSphereContactJacobian(const PolynomialFrustum * frustum) : rS(0.), frustum(frustum) { 00066 } 00067 00068 virtual ~PolyFurstumSphereContactJacobian() { 00069 } 00070 00071 void setCenter(const fmatvec::Vec3 & sphereCenter_) { 00072 sphereCenter = sphereCenter_; 00073 rS = sqrt(sphereCenter(1) * sphereCenter(1) + sphereCenter(2) * sphereCenter(2)); 00074 } 00075 00076 virtual fmatvec::SqrMat operator ()(const fmatvec::Vec & x); 00077 00078 protected: 00082 fmatvec::Vec3 sphereCenter; 00083 00087 double rS; 00088 00092 const PolynomialFrustum * frustum; 00093 }; 00094 00100 class ContactKinematicsSpherePolynomialFrustum : public MBSim::ContactKinematics { 00101 public: 00102 ContactKinematicsSpherePolynomialFrustum(); 00103 virtual ~ContactKinematicsSpherePolynomialFrustum(); 00104 00105 /* INHERITED INTERFACE OF CONTACTKINEAMTICS */ 00106 void assignContours(const std::vector<Contour*> &contour); 00107 virtual void updateg(fmatvec::Vec& g, ContourPointData *cpData, int index = 0); 00108 virtual void updatewb(fmatvec::Vec& wb, const fmatvec::Vec &g, ContourPointData *cpData) { 00109 throw MBSimError("ERROR (ContactKinematicsAreaPolynomialFrustum::updatewb): not implemented!"); 00110 } 00111 00112 protected: 00113 00117 int isphere; 00118 00122 int ifrustum; 00123 00127 Sphere *sphere; 00128 00132 PolynomialFrustum *frustum; 00133 00137 MultiDimensionalNewtonMethod newton; 00138 00139 /* 00140 * \brief function for contact search 00141 */ 00142 PolyFurstumSphereContact * func; 00143 00147 PolyFurstumSphereContactJacobian* jacobian; 00148 00152 GlobalResidualCriteriaFunction* criteria; 00153 00157 StandardDampingFunction* damping; 00158 00162 fmatvec::Vec x; 00163 00164 00165 00166 }; 00167 00168 } /* namespace MBSim */ 00169 #endif /* SPHERE_POLYNOMIALFRUSTUM_H_ */
1.8.0