00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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 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
00105 virtual int operator ()(const fmatvec::Vec &x);
00106 virtual bool isBetter(const fmatvec::Vec &x, const fmatvec::Vec & fVal = fmatvec::Vec(0,fmatvec::NONINIT));
00107 virtual void clear(){criteriaResults.clear();}
00108
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
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("(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 }
00353 #endif