MBSim

Project Page
sphere_polynomialfrustum.h
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_ */