Build a Body Position, Velocity, and Acceleration Analysis
...
- An Analysis' record()method is the heart of the analysis. It collects and, if necessary, computes the data to output the results of an analysis. In this case, the analysis requires adding a calculation (call to the SimbodyEngine) to get the model accelerations.
| Code Block | ||
|---|---|---|
| ||
/*
* Compute and record the results.
*
* This method, for the purpose of example, records the position and
* orientation of each body in the model. You can customize it
* to perform your analysis.
*
* @param aState Current state of the system.
*/
record(const SimTK::State& aState)
{
…
// After setting the state of the model and applying forces
// Compute the derivative of the multibody system (speeds and
// accelerations)
// POSITION
const BodySet& bodySet = _model->getBodySet();
int numBodies = bodySet.getSize();
for(int i=0;i<numBodies;i++) {
const Body& body = bodySet.get;
SimTK::Vec3 com;
body.getMassCenter(com);
// GET POSITIONS AND EULER ANGLES
_model->getSimbodyEngine ().getPosition(body,com,vec);
_model->getSimbodyEngine ()
.getDirectionCosines(body,dirCos);
_model->getSimbodyEngine ()
.convertDirectionCosinesToAngles(dirCos,
&angVec[0],&angVec[1],&angVec[2]);
// CONVERT TO DEGREES?
if(getInDegrees()) {
angVec *= SimTK_RADIAN_TO_DEGREE;
}
// FILL KINEMATICS ARRAY
int I=6*i;
memcpy(&_bodypos[I],&vec[0],3*sizeof(double));
memcpy(&_bodypos[I+3],&angVec[0],3*sizeof(double));
}
_storePos.append(aT,_bodypos.getSize(),&_bodypos[0]);
// VELOCITY
…
|
...