1 |
tim |
1254 |
#ifndef _ROLL_H_ |
2 |
|
|
#define _ROLL_H_ |
3 |
|
|
#include "Rattle.hpp" |
4 |
|
|
#include "Vector3d.hpp" |
5 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
6 |
|
|
//Declaration of DCRollAFunctor |
7 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
class DCRollAFunctor : public DCRattleAFunctor{ |
9 |
|
|
public: |
10 |
|
|
DCRollAFunctor(SimInfo* info) : DCRattleAFunctor(info) {} |
11 |
|
|
//virtual int operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2); |
12 |
|
|
//virtual int operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB); |
13 |
|
|
virtual int operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2); |
14 |
|
|
private: |
15 |
|
|
void getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec); |
16 |
|
|
void integrate(ConstraintRigidBody* consRB, const Vector3d& force); |
17 |
|
|
void rotationPropagation(StuntDouble* sd, double ji[3]); |
18 |
|
|
void rotate(int axes1, int axes2, double angle, double ji[3], double A[3][3]); |
19 |
|
|
}; |
20 |
|
|
|
21 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
22 |
|
|
//Declaration of JCRollAFunctor |
23 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
24 |
|
|
class JCRollAFunctor : public JCRattleAFunctor{ |
25 |
|
|
public: |
26 |
|
|
JCRollAFunctor(SimInfo* info) : JCRattleAFunctor(info) {} |
27 |
|
|
//virtual int operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2); |
28 |
|
|
//virtual int operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB); |
29 |
|
|
//virtual int operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2); |
30 |
|
|
}; |
31 |
|
|
|
32 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
33 |
|
|
//Declaration of RollA |
34 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
35 |
|
|
class RollA : public ConstraintAlgorithm{ |
36 |
|
|
public: |
37 |
|
|
RollA(SimInfo* rhs) : ConstraintAlgorithm(rhs){ |
38 |
|
|
registerCallback(typeid(DistanceConstraintPair), new DCRollAFunctor(rhs)); |
39 |
|
|
registerCallback(typeid(JointConstraintPair), new JCRollAFunctor(rhs)); |
40 |
|
|
} |
41 |
|
|
}; |
42 |
|
|
|
43 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
44 |
|
|
//Declaration of DCRollBFunctor |
45 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
46 |
|
|
class DCRollBFunctor : public DCRattleBFunctor{ |
47 |
|
|
public: |
48 |
|
|
DCRollBFunctor(SimInfo* info) : DCRattleBFunctor(info) {} |
49 |
|
|
//virtual int operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2); |
50 |
|
|
//virtual int operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB); |
51 |
|
|
virtual int operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2); |
52 |
|
|
private: |
53 |
|
|
void getEffInvMassVec(ConstraintRigidBody* consRB, const Vector3d& bondDir, Vector3d& invMassVec); |
54 |
|
|
void integrate(ConstraintRigidBody* consRB, const Vector3d& force); |
55 |
|
|
}; |
56 |
|
|
|
57 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
58 |
|
|
//Declaration of JCRollBFunctor |
59 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
60 |
|
|
class JCRollBFunctor : public JCRattleBFunctor{ |
61 |
|
|
public: |
62 |
|
|
JCRollBFunctor(SimInfo* info) : JCRattleBFunctor(info) {} |
63 |
|
|
//virtual int operator()(ConstraintAtom* consAtom1, ConstraintAtom* consAtom2); |
64 |
|
|
//virtual int operator()(ConstraintAtom* consAtom,ConstraintRigidBody* consRB); |
65 |
|
|
//virtual int operator()(ConstraintRigidBody* consRB1, ConstraintRigidBody* consRB2); |
66 |
|
|
}; |
67 |
|
|
|
68 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
69 |
|
|
//Declaration of RollB |
70 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
71 |
|
|
class RollB : public ConstraintAlgorithm{ |
72 |
|
|
public: |
73 |
|
|
RollB(SimInfo* rhs) : ConstraintAlgorithm(rhs){ |
74 |
|
|
registerCallback(typeid(DistanceConstraintPair), new DCRollBFunctor(rhs)); |
75 |
|
|
registerCallback(typeid(JointConstraintPair), new JCRollBFunctor(rhs)); |
76 |
|
|
} |
77 |
|
|
}; |
78 |
|
|
|
79 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
80 |
|
|
//Declaration of RollFramework |
81 |
|
|
//////////////////////////////////////////////////////////////////////////////// |
82 |
|
|
//class RattleAlgorithm will encapsulate preConstraint, RattleA and RattleB |
83 |
|
|
//actually, we could use factory pattern to seperate the creation process |
84 |
|
|
class RollFramework : public ConsAlgoFramework{ |
85 |
|
|
public: |
86 |
|
|
RollFramework(SimInfo* rhs) : ConsAlgoFramework(rhs){ |
87 |
|
|
raAlgo = new RollA(rhs); |
88 |
|
|
rbAlgo = new RollB(rhs); |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
~RollFramework(){ |
92 |
|
|
delete raAlgo; |
93 |
|
|
delete rbAlgo; |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
int doRollA(){ |
97 |
|
|
raAlgo->doConstrain(); |
98 |
|
|
return raAlgo->haveError()? -1 : 1; |
99 |
|
|
|
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
int doRollB(){ |
103 |
|
|
rbAlgo->doConstrain(); |
104 |
|
|
return rbAlgo->haveError()? -1 : 1; |
105 |
|
|
|
106 |
|
|
} |
107 |
|
|
private: |
108 |
|
|
RollA* raAlgo; |
109 |
|
|
RollB* rbAlgo; |
110 |
|
|
}; |
111 |
|
|
#endif //end ifndef _ROLL_H_ |