ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE-2.0/test/brains/RigidBody.cpp
(Generate patch)

Comparing trunk/OOPSE-2.0/test/brains/RigidBody.cpp (file contents):
Revision 1682 by tim, Thu Oct 28 22:34:01 2004 UTC vs.
Revision 1691 by tim, Mon Nov 1 19:57:07 2004 UTC

# Line 27 | Line 27 | RigidBody::RigidBody() : objType_(otRigidBody), storag
27  
28   namespace oopse {
29  
30 < RigidBody::RigidBody() : objType_(otRigidBody), storage_(&Snapshot::rigidbodyData){
30 > RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){
31  
32   }
33  
34   void RigidBody::setPrevA(const RotMat3x3d& a) {
35 <    (snapshotMan_->getPrevSnapshot())->storage_->aMat[localIndex_] = a;
36 <    (snapshotMan_->getPrevSnapshot())->storage_->unitVector[localIndex_] = a.inverse() * sU_.getColum(2);
35 >    ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
36 >    ((snapshotMan_->getPrevSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);
37 >
38 >    std::vector<Atom*>::iterator i;
39 >    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
40 >        if ((*i)->isDirectional()) {
41 >            (*i)->setPrevA(a * (*i)->getPrevA());
42 >        }
43 >    }
44 >
45   }
46  
47        
48   void RigidBody::setA(const RotMat3x3d& a) {
49 <    (snapshotMan_->getCurrentSnapshot())->storage_->aMat[localIndex_] = a;
50 <    (snapshotMan_->getCurrentSnapshot())->storage_->unitVector[localIndex_] = a.inverse() * sU_.getColum(2);
49 >    ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
50 >    ((snapshotMan_->getCurrentSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);
51 >
52 >    std::vector<Atom*>::iterator i;
53 >    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
54 >        if ((*i)->isDirectional()) {
55 >            (*i)->setA(a * (*i)->getA());
56 >        }
57 >    }
58   }    
59      
60   void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
61 <    (snapshotMan_->getSnapshot(snapshotNo))->storage_->aMat[localIndex_] = a;
62 <    (snapshotMan_->getSnapshot(snapshotNo))->storage_->unitVector[localIndex_] = a.inverse() * sU_.getColum(2);    
61 >    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
62 >    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);    
63 >
64 >    std::vector<Atom*>::iterator i;
65 >    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
66 >        if ((*i)->isDirectional()) {
67 >            (*i)->setA(a * (*i)->getA(snapshotNo), snapshotNo);
68 >        }
69 >    }
70 >
71   }  
72  
73 + void  DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) {
74 +    sU_.setupRotMat(phi,theta,psi);
75 + }
76 +
77   Mat3x3d RigidBody::getI() {
78      return inertiaTensor_;
79   }    
80  
54 – void RigidBody::setI(Mat3x3d& I) {
55 –    inertiaTensor_ = I;
56 – }    
57 –
81   std::vector<double> RigidBody::getGrad() {
82      vector<double> grad(6, 0.0);
83      Vector3d force;
# Line 112 | Line 135 | void RigidBody::accept(BaseVisitor* v) {
135      v->visit(this);
136   }    
137  
138 + void  RigidBody::calcRefCoords() {
139 +  /*
140 +  double mtmp;
141 +  vec3 apos;
142 +  double refCOM[3];
143 +  vec3 ptmp;
144 +  double Itmp[3][3];
145 +  double evals[3];
146 +  double evects[3][3];
147 +  double r, r2, len;
148 +
149 +  // First, find the center of mass:
150 +  
151 +  mass = 0.0;
152 +  for (j=0; j<3; j++)
153 +    refCOM[j] = 0.0;
154 +  
155 +  for (i = 0; i < atoms_.size(); i++) {
156 +    mtmp = atoms_[i]->getMass();
157 +    mass += mtmp;
158 +
159 +    apos = refCoords[i];
160 +    
161 +    for(j = 0; j < 3; j++) {
162 +      refCOM[j] += apos[j]*mtmp;    
163 +    }    
164 +  }
165 +  
166 +  for(j = 0; j < 3; j++)
167 +    refCOM[j] /= mass;
168 +
169 + // Next, move the origin of the reference coordinate system to the COM:
170 +
171 +  for (i = 0; i < atoms_.size(); i++) {
172 +    apos = refCoords[i];
173 +    for (j=0; j < 3; j++) {
174 +      apos[j] = apos[j] - refCOM[j];
175 +    }
176 +    refCoords[i] = apos;
177 +  }
178 +
179 + // Moment of Inertia calculation
180 +
181 +  for (i = 0; i < 3; i++)
182 +    for (j = 0; j < 3; j++)
183 +      Itmp[i][j] = 0.0;  
184 +  
185 +  for (it = 0; it < atoms_.size(); it++) {
186 +
187 +    mtmp = atoms_[it]->getMass();
188 +    ptmp = refCoords[it];
189 +    r= norm3(ptmp.vec);
190 +    r2 = r*r;
191 +    
192 +    for (i = 0; i < 3; i++) {
193 +      for (j = 0; j < 3; j++) {
194 +        
195 +        if (i==j) Itmp[i][j] += mtmp * r2;
196 +
197 +        Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
198 +      }
199 +    }
200 +  }
201 +  
202 +  diagonalize3x3(Itmp, evals, sU);
203 +  
204 +  // zero out I and then fill the diagonals with the moments of inertia:
205 +
206 +  n_linear_coords = 0;
207 +
208 +  for (i = 0; i < 3; i++) {
209 +    for (j = 0; j < 3; j++) {
210 +      I[i][j] = 0.0;  
211 +    }
212 +    I[i][i] = evals[i];
213 +
214 +    if (fabs(evals[i]) < momIntTol) {
215 +      is_linear = true;
216 +      n_linear_coords++;
217 +      linear_axis = i;
218 +    }
219 +  }
220 +
221 +  if (n_linear_coords > 1) {
222 +          sprintf( painCave.errMsg,
223 +               "RigidBody error.\n"
224 +               "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
225 +               "\tmoment of inertia.  This can happen in one of three ways:\n"
226 +               "\t 1) Only one atom was specified, or \n"
227 +               "\t 2) All atoms were specified at the same location, or\n"
228 +               "\t 3) The programmers did something stupid.\n"
229 +               "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
230 +               );
231 +      painCave.isFatal = 1;
232 +      simError();
233 +  }
234 +  
235 +  // renormalize column vectors:
236 +  
237 +  for (i=0; i < 3; i++) {
238 +    len = 0.0;
239 +    for (j = 0; j < 3; j++) {
240 +      len += sU[i][j]*sU[i][j];
241 +    }
242 +    len = sqrt(len);
243 +    for (j = 0; j < 3; j++) {
244 +      sU[i][j] /= len;
245 +    }
246 +  }
247 +  */
248   }
249  
250 + void  RigidBody::calcForcesAndTorques() {
251 +    unsigned int i;
252 +    unsigned int j;
253 +    //Vector3d apos;
254 +    Vector3d afrc;
255 +    Vector3d atrq;
256 +    Vector3d rpos;
257 +    Vector3d frc;
258 +    Vector3d trq;
259 +    //Vector3d pos;
260 +
261 +    zeroForces();
262 +    
263 +    //pos = getPos();
264 +    frc = getFrc();
265 +    trq = getTrq();
266 +
267 +    for (i = 0; i < atoms_.size(); i++) {
268 +
269 +        afrc = atoms_[i]->getFrc();
270 +
271 +        //apos = atoms_[i]->getPos(apos);
272 +        //rpos = apos - pos;
273 +        rpos = refCoords_[i];
274 +        
275 +        frc += afrc;
276 +
277 +        trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1];
278 +        trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2];
279 +        trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0];
280 +
281 +        // If the atom has a torque associated with it, then we also need to
282 +        // migrate the torques onto the center of mass:
283 +
284 +        if (atoms_[i]->isDirectional()) {
285 +            atrq = atoms_[i]->getTrq();
286 +            trq += atrq;
287 +        }
288 +        
289 +    }
290 +    
291 +    setFrc(frc);
292 +    setTrq(trq);
293 +    
294 + }
295 +
296 + void  RigidBody::updateAtoms() {
297 +    unsigned int i;
298 +    unsigned int j;
299 +    Vector3d ref;
300 +    Vector3d apos;
301 +    DirectionalAtom* dAtom;
302 +    Vector3d pos = getPos();
303 +    RotMat3x3d A = getA();
304 +    
305 +    for (i = 0; i < atoms_.size(); i++) {
306 +    
307 +        ref = body2Lab(refCoords_[i]);
308 +
309 +        apos = pos + ref;
310 +
311 +        atoms_[i]->setPos(apos);
312 +
313 +        if (atoms_[i]->isDirectional()) {
314 +          
315 +          dAtom = (DirectionalAtom *) atoms_[i];
316 +          dAtom->rotateBy( A );      
317 +        }
318 +
319 +    }
320 +  
321 + }
322 +
323 +
324 + bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
325 +    if (index < atoms_.size()) {
326 +
327 +        Vector3d ref = body2Lab(refCoords_[index]);
328 +        pos = getPos() + ref;
329 +        return true;
330 +    } else {
331 +        std::cerr << index << " is an invalid index, current rigid body contains "
332 +                      << atoms_.size() << "atoms" << std::endl;
333 +        return false;
334 +    }    
335 + }
336 +
337 + bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
338 +    std::vector<Atom*>::iterator i;
339 +    i = find(atoms_.begin(), atoms_.end(), atom);
340 +    if (i != atoms_.end()) {
341 +        //RigidBody class makes sure refCoords_ and atoms_ match each other
342 +        Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
343 +        pos = getPos() + ref;
344 +        return true;
345 +    } else {
346 +        std::cerr << "Atom " << atom->getGlobalIndex()
347 +                      <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
348 +        return false;
349 +    }
350 + }
351 + bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
352 +
353 +    //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
354 +
355 +    if (index < atoms_.size()) {
356 +
357 +        Vector3d velRot;
358 +        Mat3x3d skewMat;;
359 +        Vector3d ref = refCoords_[index];
360 +        Vector3d ji = getJ();
361 +        Mat3x3d I =  getI();
362 +
363 +        skewMat(0, 0) =0;
364 +        skewMat(0, 1) = ji[2] /I(2, 2);
365 +        skewMat(0, 2) = -ji[1] /I(1, 1);
366 +
367 +        skewMat(1, 0) = -ji[2] /I(2, 2);
368 +        skewMat(1, 1) = 0;
369 +        skewMat(1, 2) = ji[0]/I(0, 0);
370 +
371 +        skewMat(2, 0) =ji[1] /I(1, 1);
372 +        skewMat(2, 1) = -ji[0]/I(0, 0);
373 +        skewMat(2, 2) = 0;
374 +
375 +        velRot = (getA() * skewMat).transpose() * ref;
376 +
377 +        vel =getVel() + velRot;
378 +        return true;
379 +        
380 +    } else {
381 +        std::cerr << index << " is an invalid index, current rigid body contains "
382 +                      << atoms_.size() << "atoms" << std::endl;
383 +        return false;
384 +    }
385 + }
386 +
387 + bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
388 +
389 +    std::vector<Atom*>::iterator i;
390 +    i = find(atoms_.begin(), atoms_.end(), atom);
391 +    if (i != atoms_.end()) {
392 +        return getAtomVel(vel, i - atoms_.begin());
393 +    } else {
394 +        std::cerr << "Atom " << atom->getGlobalIndex()
395 +                      <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
396 +        return false;
397 +    }    
398 + }
399 +
400 + bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
401 +    if (index < atoms_.size()) {
402 +
403 +        coor = refCoords_[index];
404 +        return true;
405 +    } else {
406 +        std::cerr << index << " is an invalid index, current rigid body contains "
407 +                      << atoms_.size() << "atoms" << std::endl;
408 +        return false;
409 +    }
410 +
411 + }
412 +
413 + bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
414 +    std::vector<Atom*>::iterator i;
415 +    i = find(atoms_.begin(), atoms_.end(), atom);
416 +    if (i != atoms_.end()) {
417 +        //RigidBody class makes sure refCoords_ and atoms_ match each other
418 +        coor = refCoords_[i - atoms_.begin()];
419 +        return true;
420 +    } else {
421 +        std::cerr << "Atom " << atom->getGlobalIndex()
422 +                      <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
423 +        return false;
424 +    }
425 +
426 + }
427 +
428 + }
429 +

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines