/** * 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()); }; };