| 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() { |
