FrameData H5RandomReader::getFrame(unsigned long step) const {
FrameData fData;
for (std::map< std::string, H5::DataSet > ::const_iterator im (matrices.begin());
im != matrices.end();
im++) {
H5::DataSet dSet(im->second);
double buff[16];
H5::DataSpace dSpace(dSet.getSpace());
hsize_t count[3] = {1, 4, 4};
hsize_t offset[3] = {step, 0, 0};
dSpace.selectHyperslab(H5S_SELECT_SET, count, offset);
hsize_t dims[2] = {4, 4};
H5::DataSpace memSpace(2, dims);
dSet.read(buff, H5::PredType::NATIVE_DOUBLE, memSpace, dSpace);
fData.matrices[im->first] = Matrix(buff);
}
for (std::map< std::string, H5::DataSet > ::const_iterator im(translates.begin());
im != translates.end();
im++) {
H5::DataSet dSet(im->second);
double buff[3];
H5::DataSpace dSpace(dSet.getSpace());
hsize_t count[2] = {1, 3};
hsize_t offset[2] = {step, 0};
dSpace.selectHyperslab(H5S_SELECT_SET, count, offset);
hsize_t dims[1] = {3};
H5::DataSpace memSpace(1, dims);
dSet.read(buff, H5::PredType::NATIVE_DOUBLE, memSpace, dSpace);
fData.translates[im->first] = Translate(buff);
}
for (std::map< std::string, H5::DataSet > ::const_iterator im(wrenches.begin());
im != wrenches.end();
im++) {
H5::DataSet dSet(im->second);
double buff[6];
H5::DataSpace dSpace(dSet.getSpace());
hsize_t count[2] = {1, 6};
hsize_t offset[2] = {step, 0};
dSpace.selectHyperslab(H5S_SELECT_SET, count, offset);
hsize_t dims[1] = {6};
H5::DataSpace memSpace(1, dims);
dSet.read(buff, H5::PredType::NATIVE_DOUBLE, memSpace, dSpace);
fData.wrenches[im->first] = Wrench(buff);
}
return fData;
};
Wrench operator*(const RigidBodyInertia& I,const Twist& t){
return Wrench(I.m*t.vel-I.h*t.rot,I.I*t.rot+I.h*t.vel);
}
Wrench wrenchft( const Vector& force, const Vector& torque )
{
return Wrench( force, torque );
};
