/**
* Load and read file
*/
void BVHReader::readData()
{
ifstream *datafile = new ifstream(this->filename);
if (datafile->is_open())
{
string line;
while (!datafile->eof())
{
getline (*datafile, line, '\n');
// Hierarchy
if(line.compare("HIERARCHY") == 0) {
doHierarchy(datafile);
}
// Animation data
else if(line.compare("MOTION") == 0) {
doMotion(datafile);
}
}
datafile->close();
//
for (int i=0;i<objectsNumber;i++) {
BVHObject *bvh = (*sequence)[i];
}
} else {
cout << "Unable to open file";
}
}
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());
};
};
