//---------------------------------------------------------------------
// Where the actual computation is invoked. doForce is
// overloaded with specific calculation
//---------------------------------------------------------------------
void ComputePatchPair::doWork() {
#ifndef NAMD_CUDA
LdbCoordinator::Object()->startWork(ldObjHandle);
#endif
if ( ( computeType != computeNonbondedPairType ) ||
(!patch[0]->flags.doGBIS || gbisPhase == 1) ) {
// Open up positionBox, forceBox, and atomBox
for (int i=0; i<2; i++) {
p[i] = positionBox[i]->open();
r[i] = forceBox[i]->open();
pExt[i] = patch[i]->getCompAtomExtInfo();
}
}
doForce(p, pExt, r);
// Inform load balancer
#ifndef NAMD_CUDA
if (patch[0]->flags.doGBIS && (gbisPhase == 1 || gbisPhase == 2)) {
LdbCoordinator::Object()->pauseWork(ldObjHandle);
} else {
LdbCoordinator::Object()->endWork(ldObjHandle);
}
#endif
// Close up boxes
if ( ( computeType != computeNonbondedPairType ) ||
(!patch[0]->flags.doGBIS || gbisPhase == 3) ) {
for (int i=0; i<2; i++) {
positionBox[i]->close(&p[i]);
forceBox[i]->close(&r[i]);
}
}
}
void RK_CALL manipulator_dynamics_model::computeOutput(double aTime,const ReaK::vect_n<double>& aState, ReaK::vect_n<double>& aOutput) {
setJointStates(aState);
doMotion();
clearForce();
doForce();
aOutput.resize(getOutputsCount());
unsigned int i = 0;
for(std::vector< shared_ptr<system_output> >::iterator it = mOutputs.begin(); it != mOutputs.end(); ++it)
for(unsigned int k = 0; k < (*it)->getOutputCount(); ++k)
aOutput[i++] = (*it)->getOutput(k);
};
void RK_CALL manipulator_dynamics_model::computeStateRate(double aTime,const vect_n<double>& aState, vect_n<double>& aStateRate) {
setJointStates(aState);
doMotion();
clearForce();
doForce();
aStateRate.resize(getJointStatesCount());
unsigned int j = 0;
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->q_dot;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = -(*it)->Rotation[1] * (*it)->AngVelocity; ++j;
aStateRate[j] = (*it)->Rotation[0] * (*it)->AngVelocity; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = (*it)->Velocity[2]; ++j;
aStateRate[j] = (*it)->QuatDot[0]; ++j;
aStateRate[j] = (*it)->QuatDot[1]; ++j;
aStateRate[j] = (*it)->QuatDot[2]; ++j;
aStateRate[j] = (*it)->QuatDot[3]; ++j;
};
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->f;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Torque; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Force[2]; ++j;
aStateRate[j] = (*it)->Torque[0]; ++j;
aStateRate[j] = (*it)->Torque[1]; ++j;
aStateRate[j] = (*it)->Torque[2]; ++j;
};
mat<double,mat_structure::symmetric> Msys(getJointAccelerationsCount());
getMassMatrix(Msys);
try {
mat_vect_adaptor< vect_n<double> > acc_as_mat(aStateRate, getJointAccelerationsCount(), 1, getJointPositionsCount());
linsolve_Cholesky(Msys,acc_as_mat);
} catch(singularity_error& e) { RK_UNUSED(e);
std::stringstream ss; ss << "Mass matrix is singular in the manipulator model '" << getName() << "' at time " << aTime << " seconds.";
throw singularity_error(ss.str());
};
};
