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 Function1<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, const void* = NULL);
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, const void * = NULL);
00106 virtual bool isBetter(const fmatvec::Vec & x);
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("ERROR (ContactKinematicsRectanglePolynomialFrustum::updatewb): not implemented!");
00161 }
00162
00163 protected:
00167 void setFrustumOrienationKinematics(const double & x, const double & phi, ContourPointData * cpData);
00168
00169
00176 bool cpLocationInRectangle(fmatvec::Vec & g, ContourPointData * cpData);
00177
00182 bool cornerContact(fmatvec::Vec & g, ContourPointData * cpData);
00183
00190 bool edgeContact(fmatvec::Vec & g, ContourPointData * cpData);
00191
00198 fmatvec::Vec3 computeContourPointFrustum(const double & x, const fmatvec::Vec3 & n);
00199
00206 fmatvec::Vec3 computeContourPoint(const double & x, const fmatvec::Vec3 & n);
00207
00213 int checkPossibleContactPoint(const double & x, const fmatvec::Vec3 & n);
00214
00219 double distance2Rectangle(const fmatvec::Vec3 & point);
00220
00224 int irectangle;
00225
00229 int ifrustum;
00230
00234 Rectangle *rectangle;
00235
00239 PolynomialFrustum *frustum;
00240
00244 double signh;
00245
00249 double x1;
00250
00254 double x2;
00255
00259 fmatvec::Vec3 cornerPoints[4];
00260
00264 projectPointAlongNormal * funcProjectAlongNormal;
00265
00269 MultiDimensionalNewtonMethod newtonProjectAlongNormal;
00270
00274 projectPointAlongNormalJacobian * jacobianProjectAlongNormal;
00275
00279 GlobalResidualCriteriaFunction criteriaProjectAlongNormal;
00280
00284 StandardDampingFunction dampingProjectAlongNormal;
00285
00289 edgePolyFrustum * funcEdge;
00290
00294 MultiDimensionalNewtonMethod newtonEdge;
00295
00299 NumericalNewtonJacobianFunction jacobianEdge;
00300
00304 edgePolyFrustumCriteria criteriaEdge;
00305
00309 StandardDampingFunction dampingEdge;
00310
00314 int ilast;
00315
00319 fmatvec::Vec xi;
00320
00321 };
00322
00323 }
00324 #endif