Ctrl+K

Program listing for file mechanics/src/joints/PivotJointR.hpp

Program listing for file mechanics/src/joints/PivotJointR.hpp#

 1#ifndef PivotJointRELATION_H
 2#define PivotJointRELATION_H
 3
 4#include "KneeJointR.hpp"
 5
 6
 7class PivotJointR : public KneeJointR
 8{
 9protected:
10
11  ACCEPT_SERIALIZATION(PivotJointR);
12
13
14  SP::SiconosVector _A;
15  double _A1x, _A1y, _A1z;
16  double _A2x, _A2y, _A2z;
17
18
19  double _cq2q101, _cq2q102, _cq2q103, _cq2q104;
20  double _initial_AscalA, _initial_AscalA1, _initial_AscalA2;
21
22
23  int _twistCount;
24  int _previousAngle;
25
26  void buildA1A2();
27
28  virtual void Jd1d2(double X1, double Y1, double Z1,
29                     double q10, double q11, double q12, double q13,
30                     double X2, double Y2, double Z2,
31                     double q20, double q21, double q22, double q23);
32
33  virtual void Jd1(double X1, double Y1, double Z1,
34                   double q10, double q11, double q12, double q13);
35
36  void rot2to1(double q10, double q11, double q12, double q13,
37               double q20, double q21, double q22, double q23,
38               double *q2to1w, double *q2to1x,
39               double *q2to1y, double *q2to1z);
40
41  double AscalA1(double q2to1x, double q2to1y, double q2to1z);
42  double AscalA2(double q2to1x, double q2to1y, double q2to1z);
43  double AscalA(double q2to1x, double q2to1y, double q2to1z);
44
45
46  virtual void _normalDoF(SiconosVector& ans, const BlockVector& q0, int axis,
47                          bool absoluteRef=true);
48
49public:
50
51
52  PivotJointR();
53
54
55  PivotJointR(SP::SiconosVector P, SP::SiconosVector A, bool absoluteRef,
56              SP::NewtonEulerDS d1 = SP::NewtonEulerDS(),
57              SP::NewtonEulerDS d2 = SP::NewtonEulerDS());
58
59
60  virtual void setBasePositions(SP::SiconosVector q1,
61                                SP::SiconosVector q2 = SP::SiconosVector());
62
63  virtual ~PivotJointR() {};
64
65  SP::SiconosVector A() { return _A; }
66
67
68  virtual void computeh(double time, const BlockVector& q0, SiconosVector& y);
69
70
71  virtual void computehDoF(double time, const BlockVector& q0, SiconosVector& y,
72                           unsigned int axis);
73
74
75  virtual void computeJachqDoF(double time, Interaction& inter,
76                               SP::BlockVector q0, SimpleMatrix& jachq,
77                               unsigned int axis);
78
79
80  virtual unsigned int numberOfConstraints() { return 5; }
81
82
83  virtual unsigned int numberOfDoF() { return 1; }
84
85
86  virtual DoF_Type typeOfDoF(unsigned int axis) {
87    if (axis==0) return DOF_TYPE_ANGULAR;
88    else return DOF_TYPE_INVALID;
89  };
90};
91#endif