27 |
|
#include "utils/simError.h" |
28 |
|
namespace oopse { |
29 |
|
|
30 |
< |
RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){ |
30 |
> |
RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){ |
31 |
|
|
32 |
|
} |
33 |
|
|
137 |
|
|
138 |
|
/**@todo need modification */ |
139 |
|
void RigidBody::calcRefCoords() { |
140 |
< |
/* |
141 |
< |
double mtmp; |
142 |
< |
vec3 apos; |
143 |
< |
double refCOM[3]; |
144 |
< |
vec3 ptmp; |
145 |
< |
double Itmp[3][3]; |
146 |
< |
double evals[3]; |
147 |
< |
double evects[3][3]; |
148 |
< |
double r, r2, len; |
140 |
> |
double mtmp; |
141 |
> |
Vector3d refCOM(0.0); |
142 |
> |
mass_ = 0.0; |
143 |
> |
for (int i = 0; i < atoms_.size(); ++i) { |
144 |
> |
mtmp = atoms_[i]->getMass(); |
145 |
> |
mass_ += mtmp; |
146 |
> |
refCOM += refCoords_[i]*mtmp; |
147 |
> |
} |
148 |
> |
refCOM /= mass_; |
149 |
|
|
150 |
< |
// First, find the center of mass: |
151 |
< |
|
152 |
< |
mass = 0.0; |
153 |
< |
for (j=0; j<3; j++) |
154 |
< |
refCOM[j] = 0.0; |
155 |
< |
|
156 |
< |
for (i = 0; i < atoms_.size(); i++) { |
157 |
< |
mtmp = atoms_[i]->getMass(); |
158 |
< |
mass += mtmp; |
159 |
< |
|
160 |
< |
apos = refCoords[i]; |
161 |
< |
|
162 |
< |
for(j = 0; j < 3; j++) { |
163 |
< |
refCOM[j] += apos[j]*mtmp; |
164 |
< |
} |
165 |
< |
} |
166 |
< |
|
167 |
< |
for(j = 0; j < 3; j++) |
168 |
< |
refCOM[j] /= mass; |
169 |
< |
|
170 |
< |
// Next, move the origin of the reference coordinate system to the COM: |
171 |
< |
|
172 |
< |
for (i = 0; i < atoms_.size(); i++) { |
173 |
< |
apos = refCoords[i]; |
174 |
< |
for (j=0; j < 3; j++) { |
175 |
< |
apos[j] = apos[j] - refCOM[j]; |
150 |
> |
// Next, move the origin of the reference coordinate system to the COM: |
151 |
> |
for (int i = 0; i < atoms_.size(); ++i) { |
152 |
> |
refCoords_[i] -= refCOM; |
153 |
|
} |
177 |
– |
refCoords[i] = apos; |
178 |
– |
} |
154 |
|
|
155 |
|
// Moment of Inertia calculation |
156 |
< |
|
182 |
< |
for (i = 0; i < 3; i++) |
183 |
< |
for (j = 0; j < 3; j++) |
184 |
< |
Itmp[i][j] = 0.0; |
156 |
> |
Mat3x3d Itmp(0.0); |
157 |
|
|
158 |
< |
for (it = 0; it < atoms_.size(); it++) { |
158 |
> |
for (int i = 0; i < atoms_.size(); i++) { |
159 |
> |
mtmp = atoms_[i]->getMass(); |
160 |
> |
Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; |
161 |
> |
double r2 = refCoords_[i].lengthSquare(); |
162 |
> |
Itmp(0, 0) += mtmp * r2; |
163 |
> |
Itmp(1, 1) += mtmp * r2; |
164 |
> |
Itmp(2, 2) += mtmp * r2; |
165 |
> |
} |
166 |
|
|
167 |
< |
mtmp = atoms_[it]->getMass(); |
168 |
< |
ptmp = refCoords[it]; |
169 |
< |
r= norm3(ptmp.vec); |
191 |
< |
r2 = r*r; |
192 |
< |
|
193 |
< |
for (i = 0; i < 3; i++) { |
194 |
< |
for (j = 0; j < 3; j++) { |
195 |
< |
|
196 |
< |
if (i==j) Itmp[i][j] += mtmp * r2; |
167 |
> |
//diagonalize |
168 |
> |
Vector3d evals; |
169 |
> |
Mat3x3d::diagonalize(Itmp, evals, sU_) |
170 |
|
|
171 |
< |
Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j]; |
172 |
< |
} |
171 |
> |
// zero out I and then fill the diagonals with the moments of inertia: |
172 |
> |
inertiaTensor_(0, 0) = evals[0]; |
173 |
> |
inertiaTensor_(1, 1) = evals[1]; |
174 |
> |
inertiaTensor_(2, 2) = evals[2]; |
175 |
> |
|
176 |
> |
int nLinearAxis = 0; |
177 |
> |
for (int i = 0; i < 3; i++) { |
178 |
> |
if (fabs(evals[i]) < oopse::epsilon) { |
179 |
> |
linear_ = true; |
180 |
> |
linearAxis_ = i; |
181 |
> |
++ nLinearAxis; |
182 |
> |
} |
183 |
|
} |
201 |
– |
} |
202 |
– |
|
203 |
– |
diagonalize3x3(Itmp, evals, sU); |
204 |
– |
|
205 |
– |
// zero out I and then fill the diagonals with the moments of inertia: |
184 |
|
|
185 |
< |
n_linear_coords = 0; |
186 |
< |
|
187 |
< |
for (i = 0; i < 3; i++) { |
188 |
< |
for (j = 0; j < 3; j++) { |
189 |
< |
I[i][j] = 0.0; |
190 |
< |
} |
191 |
< |
I[i][i] = evals[i]; |
192 |
< |
|
193 |
< |
if (fabs(evals[i]) < momIntTol) { |
194 |
< |
is_linear = true; |
195 |
< |
n_linear_coords++; |
196 |
< |
linear_axis = i; |
185 |
> |
if (nLinearAxis > 1) { |
186 |
> |
sprintf( painCave.errMsg, |
187 |
> |
"RigidBody error.\n" |
188 |
> |
"\tOOPSE found more than one axis in this rigid body with a vanishing \n" |
189 |
> |
"\tmoment of inertia. This can happen in one of three ways:\n" |
190 |
> |
"\t 1) Only one atom was specified, or \n" |
191 |
> |
"\t 2) All atoms were specified at the same location, or\n" |
192 |
> |
"\t 3) The programmers did something stupid.\n" |
193 |
> |
"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
194 |
> |
); |
195 |
> |
painCave.isFatal = 1; |
196 |
> |
simError(); |
197 |
|
} |
220 |
– |
} |
221 |
– |
|
222 |
– |
if (n_linear_coords > 1) { |
223 |
– |
sprintf( painCave.errMsg, |
224 |
– |
"RigidBody error.\n" |
225 |
– |
"\tOOPSE found more than one axis in this rigid body with a vanishing \n" |
226 |
– |
"\tmoment of inertia. This can happen in one of three ways:\n" |
227 |
– |
"\t 1) Only one atom was specified, or \n" |
228 |
– |
"\t 2) All atoms were specified at the same location, or\n" |
229 |
– |
"\t 3) The programmers did something stupid.\n" |
230 |
– |
"\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
231 |
– |
); |
232 |
– |
painCave.isFatal = 1; |
233 |
– |
simError(); |
234 |
– |
} |
198 |
|
|
236 |
– |
// renormalize column vectors: |
237 |
– |
|
238 |
– |
for (i=0; i < 3; i++) { |
239 |
– |
len = 0.0; |
240 |
– |
for (j = 0; j < 3; j++) { |
241 |
– |
len += sU[i][j]*sU[i][j]; |
242 |
– |
} |
243 |
– |
len = sqrt(len); |
244 |
– |
for (j = 0; j < 3; j++) { |
245 |
– |
sU[i][j] /= len; |
246 |
– |
} |
247 |
– |
} |
248 |
– |
*/ |
199 |
|
} |
200 |
|
|
201 |
|
void RigidBody::calcForcesAndTorques() { |