00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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 Function1<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, const void* = NULL);
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, const void* = NULL);
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
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 }
00169 #endif