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 POINT_POLYNOMIALFRUSTUM_H_ 00021 #define POINT_POLYNOMIALFRUSTUM_H_ 00022 00023 #include "contact_kinematics.h" 00024 00025 #include <mbsim/contours/polynomial_frustum.h> 00026 #include <mbsim/contours/point.h> 00027 #include <mbsim/numerics/nonlinear_algebra/multi_dimensional_newton_method.h> 00028 00029 namespace MBSim { 00030 00034 class projectPointAlongNormal : public fmatvec::Function<fmatvec::Vec(fmatvec::Vec)> { 00035 public: 00036 projectPointAlongNormal(PolynomialFrustum * frustum); 00037 00038 virtual ~projectPointAlongNormal(); 00039 00040 void setUpSystemParamters(const fmatvec::Vec3 & referencePoint, const double & phi); 00041 00042 fmatvec::Vec operator()(const fmatvec::Vec &x); 00043 00044 protected: 00048 PolynomialFrustum * frustum; 00049 00053 fmatvec::Vec3 referencePoint; 00054 00058 double phi; 00059 }; 00060 00064 class projectPointAlongNormalJacobian : public NewtonJacobianFunction { 00065 public: 00066 projectPointAlongNormalJacobian(PolynomialFrustum * frustum); 00067 00068 virtual ~projectPointAlongNormalJacobian(); 00069 00070 void setUpSystemParamters(const fmatvec::Vec3 & referencePoint, const double & phi); 00071 00072 fmatvec::SqrMat operator()(const fmatvec::Vec &x); 00073 00074 protected: 00078 PolynomialFrustum * frustum; 00079 00083 fmatvec::Vec3 referencePoint; 00084 00088 double phi; 00089 }; 00090 00091 00098 class ContactKinematicsPointPolynomialFrustum : public MBSim::ContactKinematics { 00099 public: 00100 ContactKinematicsPointPolynomialFrustum(); 00101 virtual ~ContactKinematicsPointPolynomialFrustum(); 00102 00103 /* INHERITED INTERFACE OF CONTACTKINEAMTICS */ 00104 void assignContours(const std::vector<Contour*> &contour); 00105 virtual void updateg(fmatvec::Vec& g, ContourPointData *cpData, int index = 0); 00106 virtual void updatewb(fmatvec::Vec& wb, const fmatvec::Vec &g, ContourPointData *cpData) { 00107 throw MBSimError("ERROR (ContactKinematicsRectanglePolynomialFrustum::updatewb): not implemented!"); 00108 } 00109 00110 protected: 00114 void setFrustumOrienationKinematics(const double & x, const double & phi, ContourPointData * cpData); 00115 00119 int ipoint; 00120 00124 int ifrustum; 00125 00129 Point *point; 00130 00134 PolynomialFrustum *frustum; 00135 00139 double signh; 00140 00144 projectPointAlongNormal * funcProjectAlongNormal; 00145 00149 MultiDimensionalNewtonMethod newtonProjectAlongNormal; 00150 00154 projectPointAlongNormalJacobian * jacobianProjectAlongNormal; 00155 00159 GlobalResidualCriteriaFunction criteriaProjectAlongNormal; 00160 00164 StandardDampingFunction dampingProjectAlongNormal; 00165 00166 }; 00167 00168 } /* namespace MBSim */ 00169 #endif /* POINT_POLYNOMIALFRUSTUM_H_ */
1.8.0