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 Function1<fmatvec::Vec, fmatvec::Vec> { 00032 public: 00033 PolyFurstumSphereContact(const PolynomialFrustum * frustum) : 00034 frustum(frustum) { 00035 } 00036 00037 virtual ~PolyFurstumSphereContact() { 00038 } 00039 00040 void setCenter(const fmatvec::Vec3 & sphereCenter_) { 00041 sphereCenter = sphereCenter_; 00042 } 00043 00044 fmatvec::Vec operator()(const fmatvec::Vec &x, const void* = NULL); 00045 00046 protected: 00050 fmatvec::Vec3 sphereCenter; 00051 00055 const PolynomialFrustum * frustum; 00056 }; 00057 00058 class PolyFurstumSphereContactJacobian : public NewtonJacobianFunction { 00059 public: 00060 PolyFurstumSphereContactJacobian(const PolynomialFrustum * frustum) : 00061 frustum(frustum) { 00062 } 00063 00064 virtual ~PolyFurstumSphereContactJacobian() { 00065 } 00066 00067 void setCenter(const fmatvec::Vec3 & sphereCenter_) { 00068 sphereCenter = sphereCenter_; 00069 } 00070 00071 virtual fmatvec::SqrMat operator ()(const fmatvec::Vec & x, const void* = NULL); 00072 00073 protected: 00077 fmatvec::Vec3 sphereCenter; 00078 00082 const PolynomialFrustum * frustum; 00083 }; 00084 00090 class ContactKinematicsSpherePolynomialFrustum : public MBSim::ContactKinematics { 00091 public: 00092 ContactKinematicsSpherePolynomialFrustum(); 00093 virtual ~ContactKinematicsSpherePolynomialFrustum(); 00094 00095 /* INHERITED INTERFACE OF CONTACTKINEAMTICS */ 00096 void assignContours(const std::vector<Contour*> &contour); 00097 virtual void updateg(fmatvec::Vec& g, ContourPointData *cpData, int index = 0); 00098 virtual void updatewb(fmatvec::Vec& wb, const fmatvec::Vec &g, ContourPointData *cpData) { 00099 throw MBSimError("ERROR (ContactKinematicsAreaPolynomialFrustum::updatewb): not implemented!"); 00100 } 00101 00102 protected: 00103 00107 int isphere; 00108 00112 int ifrustum; 00113 00117 Sphere *sphere; 00118 00122 PolynomialFrustum *frustum; 00123 00127 MultiDimensionalNewtonMethod newton; 00128 00129 /* 00130 * \brief function for contact search 00131 */ 00132 PolyFurstumSphereContact * func; 00133 00137 PolyFurstumSphereContactJacobian* jacobian; 00138 00142 GlobalResidualCriteriaFunction* criteria; 00143 00147 StandardDampingFunction* damping; 00148 00152 fmatvec::Vec x; 00153 00154 00155 00156 }; 00157 00158 } /* namespace MBSim */ 00159 #endif /* SPHERE_POLYNOMIALFRUSTUM_H_ */
1.8.0