1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
|
#ifndef BT_REDUCED_SOFT_BODY_H
#define BT_REDUCED_SOFT_BODY_H
#include "../btSoftBody.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
// Reduced deformable body is a simplified deformable object embedded in a rigid frame.
class btReducedDeformableBody : public btSoftBody
{
public:
//
// Typedefs
//
typedef btAlignedObjectArray<btVector3> TVStack;
// typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
typedef btAlignedObjectArray<btScalar> tDenseArray;
typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
private:
// flag to turn off the reduced modes
bool m_rigidOnly;
// Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass.
bool m_transform_lock;
// scaling factors
btScalar m_rhoScale; // mass density scale
btScalar m_ksScale; // stiffness scale
// projection matrix
tDenseMatrix m_projPA; // Eqn. 4.11 from Rahul Sheth's thesis
tDenseMatrix m_projCq;
tDenseArray m_STP;
tDenseArray m_MrInvSTP;
TVStack m_localMomentArm; // Sq + x0
btVector3 m_internalDeltaLinearVelocity;
btVector3 m_internalDeltaAngularVelocity;
tDenseArray m_internalDeltaReducedVelocity;
btVector3 m_linearVelocityFromReduced; // contribution to the linear velocity from reduced velocity
btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity
btVector3 m_internalDeltaAngularVelocityFromReduced;
protected:
// rigid frame
btScalar m_mass; // total mass of the rigid frame
btScalar m_inverseMass; // inverse of the total mass of the rigid frame
btVector3 m_linearVelocity;
btVector3 m_angularVelocity;
btScalar m_linearDamping; // linear damping coefficient
btScalar m_angularDamping; // angular damping coefficient
btVector3 m_linearFactor;
btVector3 m_angularFactor;
// btVector3 m_invInertiaLocal;
btMatrix3x3 m_invInertiaLocal;
btTransform m_rigidTransformWorld;
btMatrix3x3 m_invInertiaTensorWorldInitial;
btMatrix3x3 m_invInertiaTensorWorld;
btMatrix3x3 m_interpolateInvInertiaTensorWorld;
btVector3 m_initialCoM; // initial center of mass (original of the m_rigidTransformWorld)
// damping
btScalar m_dampingAlpha;
btScalar m_dampingBeta;
public:
//
// Fields
//
// reduced space
int m_nReduced;
int m_nFull;
tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
tDenseArray m_reducedDofs; // Reduced degree of freedom
tDenseArray m_reducedDofsBuffer; // Reduced degree of freedom at t^n
tDenseArray m_reducedVelocity; // Reduced velocity array
tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n
tDenseArray m_reducedForceExternal; // reduced external force
tDenseArray m_reducedForceElastic; // reduced internal elastic force
tDenseArray m_reducedForceDamping; // reduced internal damping force
tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model
tDenseArray m_Kr; // reduced stiffness matrix
// full space
TVStack m_x0; // Rest position
tDenseArray m_nodalMass; // Mass on each node
btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes
int m_nodeIndexOffset; // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world.
// contacts
btAlignedObjectArray<int> m_contactNodesList;
//
// Api
//
btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
~btReducedDeformableBody() {}
//
// initializing helpers
//
void internalInitialization();
void setReducedModes(int num_modes, int full_size);
void setMassProps(const tDenseArray& mass_array);
void setInertiaProps();
void setRigidVelocity(const btVector3& v);
void setRigidAngularVelocity(const btVector3& omega);
void setStiffnessScale(const btScalar ks);
void setMassScale(const btScalar rho);
void setFixedNodes(const int n_node);
void setDamping(const btScalar alpha, const btScalar beta);
void disableReducedModes(const bool rigid_only);
virtual void setTotalMass(btScalar mass, bool fromfaces = false);
//
// various internal updates
//
virtual void transformTo(const btTransform& trs);
virtual void transform(const btTransform& trs);
// caution:
// need to use scale before using transform, because the scale is performed in the local frame
// (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info)
virtual void scale(const btVector3& scl);
private:
void updateRestNodalPositions();
void updateInitialInertiaTensor(const btMatrix3x3& rotation);
void updateLocalInertiaTensorFromNodes();
void updateInertiaTensor();
void updateModesByRotation(const btMatrix3x3& rotation);
public:
void updateLocalMomentArm();
void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform);
// update the external force projection matrix
void updateExternalForceProjectMatrix(bool initialized);
void endOfTimeStepZeroing();
void applyInternalVelocityChanges();
//
// position and velocity update related
//
// compute reduced degree of freedoms
void updateReducedDofs(btScalar solverdt);
// compute reduced velocity update (for explicit time stepping)
void updateReducedVelocity(btScalar solverdt);
// map to full degree of freedoms
void mapToFullPosition(const btTransform& ref_trans);
// compute full space velocity from the reduced velocity
void mapToFullVelocity(const btTransform& ref_trans);
// compute total angular momentum
const btVector3 computeTotalAngularMomentum() const;
// get a single node's full space velocity from the reduced velocity
const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;
// get a single node's all delta velocity
const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const;
//
// rigid motion related
//
void applyDamping(btScalar timeStep);
void applyCentralImpulse(const btVector3& impulse);
void applyTorqueImpulse(const btVector3& torque);
void proceedToTransform(btScalar dt, bool end_of_time_step);
//
// force related
//
// apply impulse to the rigid frame
void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
// apply impulse to nodes in the full space
void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
// apply nodal external force in the full space
void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node);
// apply gravity to the rigid frame
void applyRigidGravity(const btVector3& gravity, btScalar dt);
// apply reduced elastic force
void applyReducedElasticForce(const tDenseArray& reduce_dofs);
// apply reduced damping force
void applyReducedDampingForce(const tDenseArray& reduce_vel);
// calculate the impulse factor
virtual btMatrix3x3 getImpulseFactor(int n_node);
// get relative position from a node to the CoM of the rigid frame
btVector3 getRelativePos(int n_node);
//
// accessors
//
bool isReducedModesOFF() const;
btScalar getTotalMass() const;
btTransform& getRigidTransform();
const btVector3& getLinearVelocity() const;
const btVector3& getAngularVelocity() const;
#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
void clampVelocity(btVector3& v) const {
v.setX(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
);
v.setY(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
);
v.setZ(
fmax(-BT_CLAMP_VELOCITY_TO,
fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
);
}
#endif
};
#endif // BT_REDUCED_SOFT_BODY_H
|