MBSim

Project Page
rectangle_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 AREA_POLYNOMIALFRUSTUM_H_
00021 #define AREA_POLYNOMIALFRUSTUM_H_
00022 
00023 #include "contact_kinematics.h"
00024 
00025 #include <mbsim/contours/polynomial_frustum.h>
00026 #include <mbsim/contours/rectangle.h>
00027 #include <mbsim/numerics/nonlinear_algebra/multi_dimensional_newton_method.h>
00028 #include <mbsim/contact_kinematics/point_polynomialfrustum.h>
00029 
00030 namespace MBSim {
00031 
00035   class edgePolyFrustum : public fmatvec::Function<fmatvec::Vec(fmatvec::Vec)> {
00036     public:
00040       edgePolyFrustum(const PolynomialFrustum * frustum);
00041 
00045       virtual ~edgePolyFrustum();
00046 
00050       void setAdir(const fmatvec::Vec3 & A, const fmatvec::Vec3 & dir);
00051 
00052       fmatvec::Vec operator()(const fmatvec::Vec &x);
00053 
00054     protected:
00058       const PolynomialFrustum * frustum;
00059 
00063       fmatvec::Vec A;
00064 
00068       fmatvec::Vec dir;
00069   };
00070 
00071   class edgePolyFrustumCriteria : public CriteriaFunction {
00072     public:
00076       edgePolyFrustumCriteria(const double & tolerance_ = 1e-10);
00077 
00081       virtual ~edgePolyFrustumCriteria(){};
00082 
00086       void setFrustumHeight(const double & frustumHeight_) {
00087         frustumHeight = frustumHeight_;
00088       }
00089 
00093       void setStartingXCoordinate(const double & ax_) {
00094         ax = ax_;
00095       }
00096 
00100       void setdirectionXCoordinate(const double & dx_) {
00101         dx = dx_;
00102       }
00103 
00104       /* INHERITED INTERFACE */
00105       virtual int operator ()(const fmatvec::Vec &x);
00106       virtual bool isBetter(const fmatvec::Vec &x);
00107       virtual void clear(){criteriaResults.clear();}
00108       /*END - INHERITED INTERFACE*/
00109 
00110       const std::vector<double> & getResults() {
00111         return criteriaResults;
00112       }
00113 
00114     protected:
00118       bool inBounds(const double & t);
00122       double tolerance;
00123 
00127       double frustumHeight;
00128 
00132       std::vector<double> criteriaResults;
00133 
00137       double ax;
00138 
00142       double dx;
00143   };
00144 
00151   class ContactKinematicsRectanglePolynomialFrustum : public MBSim::ContactKinematics {
00152     public:
00153       ContactKinematicsRectanglePolynomialFrustum();
00154       virtual ~ContactKinematicsRectanglePolynomialFrustum();
00155 
00156       /* INHERITED INTERFACE OF CONTACTKINEAMTICS */
00157       void assignContours(const std::vector<Contour*> &contour);
00158       virtual void updateg(fmatvec::Vec& g, ContourPointData *cpData, int index = 0);
00159       virtual void updatewb(fmatvec::Vec& wb, const fmatvec::Vec &g, ContourPointData *cpData) {
00160         throw MBSimError("ERROR (ContactKinematicsRectanglePolynomialFrustum::updatewb): not implemented!");
00161       }
00162 
00163       void setGridSizeY(int gridSizeY_);
00164       void setGridSizeZ(int gridSizeZ_);
00165 
00166     protected:
00170       void setFrustumOrienationKinematics(const double & x, const double & phi, ContourPointData * cpData);
00171 
00172       /***************************************************/
00179       bool cpLocationInRectangle(fmatvec::Vec & g, ContourPointData * cpData);
00180 
00185       bool gridContact(fmatvec::Vec & g, ContourPointData * cpData);
00186 
00191       bool cornerContact(fmatvec::Vec & g, ContourPointData * cpData);
00192 
00199       bool edgeContact(fmatvec::Vec & g, ContourPointData * cpData);
00200 
00207       fmatvec::Vec3 computeContourPointFrustum(const double & x, const fmatvec::Vec3 & n);
00208 
00215       fmatvec::Vec3 computeContourPoint(const double & x, const fmatvec::Vec3 & n);
00216 
00222       int checkPossibleContactPoint(const double & x, const fmatvec::Vec3 & n);
00223 
00228       double distance2Rectangle(const fmatvec::Vec3 & point);
00229 
00233       void updateGrid();
00234 
00238       int irectangle;
00239 
00243       int ifrustum;
00244 
00248       Rectangle *rectangle;
00249 
00253       PolynomialFrustum *frustum;
00254 
00258       double signh;
00259 
00263       int gridSizeY;
00264 
00268       int gridSizeZ;
00269 
00273       double x1;
00274 
00278       double x2;
00279 
00283       fmatvec::Vec3 cornerPoints[4];
00284 
00288       std::vector<std::vector<fmatvec::Vec3> > gridPoints;
00289 
00293       projectPointAlongNormal * funcProjectAlongNormal;
00294 
00298       MultiDimensionalNewtonMethod newtonProjectAlongNormal;
00299 
00303       projectPointAlongNormalJacobian * jacobianProjectAlongNormal;
00304 
00308       GlobalResidualCriteriaFunction criteriaProjectAlongNormal;
00309 
00313       StandardDampingFunction dampingProjectAlongNormal;
00314 
00318       edgePolyFrustum * funcEdge;
00319 
00323       MultiDimensionalNewtonMethod newtonEdge;
00324 
00328       NumericalNewtonJacobianFunction jacobianEdge;
00329 
00333       edgePolyFrustumCriteria criteriaEdge;
00334 
00338       StandardDampingFunction dampingEdge;
00339 
00343       int ilast;
00344 
00348       fmatvec::Vec xi;
00349 
00350   };
00351 
00352 } /* namespace MBSim */
00353 #endif /* AREA_POLYNOMIALFRUSTUM_H_ */