mbsim 4.0.0 MBSim Kernel

sphere_polynomialfrustum.h
1 /* Copyright (C) 2004-2012 MBSim Development Team
2 *
3 * This library is free software; you can redistribute it and/or
4 * modify it under the terms of the GNU Lesser General Public
5 * License as published by the Free Software Foundation; either
6 * version 2.1 of the License, or (at your option) any later version.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 * Lesser General Public License for more details.
12 *
13 * You should have received a copy of the GNU Lesser General Public
14 * License along with this library; if not, write to the Free Software
15 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16 *
17 * Contact: martin.o.foerg@googlemail.com
18 */
19
20 #ifndef SPHERE_POLYNOMIALFRUSTUM_H_
21 #define SPHERE_POLYNOMIALFRUSTUM_H_
22
23 #include "contact_kinematics.h"
24
25 #include <mbsim/contours/polynomial_frustum.h>
26 #include <mbsim/contours/sphere.h>
27 #include <mbsim/numerics/nonlinear_algebra/multi_dimensional_newton_method.h>
28
29 namespace MBSim {
30
31 class PolyFurstumSphereContact : public Function<fmatvec::Vec(fmatvec::Vec)> {
32 public:
33 PolyFurstumSphereContact(const PolynomialFrustum * frustum) : sphereCenter(fmatvec::Vec3()), rS(0.), frustum(frustum) {
34 }
35
36 ~PolyFurstumSphereContact() override = default;
37
38 void setCenter(const fmatvec::Vec3 & sphereCenter_) {
39 sphereCenter = sphereCenter_;
40 rS = sqrt(sphereCenter(1) * sphereCenter(1) + sphereCenter(2) * sphereCenter(2));
41 }
42
43 fmatvec::Vec operator()(const fmatvec::Vec &x) override;
44
45 protected:
49 fmatvec::Vec3 sphereCenter;
50
54 double rS;
55
59 const PolynomialFrustum * frustum;
60 };
61
62 class PolyFurstumSphereContactJacobian : public NewtonJacobianFunction {
63 public:
64 PolyFurstumSphereContactJacobian(const PolynomialFrustum * frustum) : rS(0.), frustum(frustum) {
65 }
66
67 ~PolyFurstumSphereContactJacobian() override = default;
68
69 void setCenter(const fmatvec::Vec3 & sphereCenter_) {
70 sphereCenter = sphereCenter_;
71 rS = sqrt(sphereCenter(1) * sphereCenter(1) + sphereCenter(2) * sphereCenter(2));
72 }
73
74 fmatvec::SqrMat operator ()(const fmatvec::Vec & x) override;
75
76 protected:
80 fmatvec::Vec3 sphereCenter;
81
85 double rS;
86
90 const PolynomialFrustum * frustum;
91 };
92
98 class ContactKinematicsSpherePolynomialFrustum : public MBSim::ContactKinematics {
99 public:
100 ContactKinematicsSpherePolynomialFrustum();
101 ~ContactKinematicsSpherePolynomialFrustum() override;
102
103 /* INHERITED INTERFACE OF CONTACTKINEAMTICS */
104 void assignContours(const std::vector<Contour*> &contour) override;
105 void updateg(SingleContact &contact, int i=0) override;
106
107 protected:
108
112 int isphere{-1};
113
117 int ifrustum{-1};
118
122 Sphere *sphere{nullptr};
123
127 PolynomialFrustum *frustum{nullptr};
128
132 MBSim::MultiDimensionalNewtonMethod newton;
133
134 /*
135 * \brief function for contact search
136 */
137 PolyFurstumSphereContact * func{nullptr};
138
142 PolyFurstumSphereContactJacobian* jacobian{nullptr};
143
147 GlobalResidualCriteriaFunction* criteria;
148
152 MBSim::StandardDampingFunction* damping;
153
157 fmatvec::Vec x;
158
159
160
161 };
162
163 } /* namespace MBSim */
164 #endif /* SPHERE_POLYNOMIALFRUSTUM_H_ */
MBSim::PolyFurstumSphereContact::sphereCenter
fmatvec::Vec3 sphereCenter
center of the sphere in coordinates of the frustum
Definition: sphere_polynomialfrustum.h:49
MBSim::GlobalResidualCriteriaFunction
This criteria function class applies the infinity norm globally on the complete vector and compares i...
Definition: criteria_functions.h:170
MBSim::MultiDimensionalNewtonMethod
Newton method for multidimensional root finding.
Definition: multi_dimensional_newton_method.h:41
MBSim::ContactKinematicsSpherePolynomialFrustum::updateg
void updateg(SingleContact &contact, int i=0) override
compute contact kinematics for a single contact
Definition: sphere_polynomialfrustum.cc:86
MBSim::PolyFurstumSphereContactJacobian::sphereCenter
fmatvec::Vec3 sphereCenter
center of the sphere
Definition: sphere_polynomialfrustum.h:80
MBSim::ContactKinematicsSpherePolynomialFrustum::x
fmatvec::Vec x
solution vector of nonlinear system
Definition: sphere_polynomialfrustum.h:157
MBSim::ContactKinematicsSpherePolynomialFrustum::sphere
Sphere * sphere
pointer to the contour class for the sphere
Definition: sphere_polynomialfrustum.h:122
MBSim::PolyFurstumSphereContactJacobian
Definition: sphere_polynomialfrustum.h:62
MBSim::ContactKinematicsSpherePolynomialFrustum::criteria
GlobalResidualCriteriaFunction * criteria
residual function for newton
Definition: sphere_polynomialfrustum.h:147
MBSim::PolyFurstumSphereContactJacobian::frustum
const PolynomialFrustum * frustum
polynomial parameters of the frustum
Definition: sphere_polynomialfrustum.h:90
MBSim::NewtonJacobianFunction
base class for square Jacobians used for the newton method
Definition: newton_method_jacobian_functions.h:31
MBSim::Sphere
sphere
Definition: sphere.h:34
MBSim::PolynomialFrustum
Frustum contour with a polynomial radius over height course.
Definition: polynomial_frustum.h:38
MBSim::PolyFurstumSphereContact::frustum
const PolynomialFrustum * frustum
polynomial parameters of the frustum
Definition: sphere_polynomialfrustum.h:59
MBSim::ContactKinematicsSpherePolynomialFrustum::jacobian
PolyFurstumSphereContactJacobian * jacobian
jacobian function for solution
Definition: sphere_polynomialfrustum.h:142
MBSim::ContactKinematicsSpherePolynomialFrustum::damping
MBSim::StandardDampingFunction * damping
damping function for newton
Definition: sphere_polynomialfrustum.h:152
MBSim::ContactKinematicsSpherePolynomialFrustum::assignContours
void assignContours(const std::vector< Contour * > &contour) override
treats ordering of contours
Definition: sphere_polynomialfrustum.cc:58
MBSim::PolyFurstumSphereContact
Definition: sphere_polynomialfrustum.h:31
MBSim::ContactKinematicsSpherePolynomialFrustum::frustum
PolynomialFrustum * frustum
pointer to the contour class for the polynomial frustum
Definition: sphere_polynomialfrustum.h:127
MBSim::Function
Definition: contact_kinematics.h:32
MBSim::ContactKinematicsSpherePolynomialFrustum::isphere
int isphere
contour index of sphere (in cpData)
Definition: sphere_polynomialfrustum.h:112
MBSim::ContactKinematicsSpherePolynomialFrustum::newton
MBSim::MultiDimensionalNewtonMethod newton
Newton solver for nonlinear problem.
Definition: sphere_polynomialfrustum.h:132
MBSim::PolyFurstumSphereContact::rS
double rS
radial coordinate of the sphere center in the 2D description
Definition: sphere_polynomialfrustum.h:54
MBSim::StandardDampingFunction
Definition: damping_functions.h:63
MBSim::SingleContact
class for contacts
Definition: single_contact.h:46
MBSim::ContactKinematics
basic class for contact kinematical calculations
Definition: contact_kinematics.h:41
MBSim::ContactKinematicsSpherePolynomialFrustum
class for contact kinematics between convex frustum and a sphere
Definition: sphere_polynomialfrustum.h:98
MBSim::ContactKinematicsSpherePolynomialFrustum::ifrustum
int ifrustum
contour index of frustum (in cpData)
Definition: sphere_polynomialfrustum.h:117
MBSim::PolyFurstumSphereContactJacobian::rS
double rS
radial coordinate of the sphere center in the 2D description
Definition: sphere_polynomialfrustum.h:85
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30