35 #include <boost/cstdint.hpp> 47 positions.open(QIODevice::ReadOnly);
48 stream.setDevice(&positions);
49 stream.setByteOrder(QDataStream::LittleEndian);
52 memset(header, 0,
sizeof(header));
53 stream.readRawData(header, 6);
57 if (!positions.exists())
59 std::cout <<
"Error: File [" << filename.toStdString() <<
"] does not exist." << std::endl;
62 if (QString(header)!=
"SNWPOS" || mVersion<1)
65 std::cout <<
"Error in header for file [" << filename.toStdString() <<
"]" << std::endl;
97 Frame3D frame = this->frameFromStream();
103 return (type==1) && (size==15);
123 stream.readBytes(data, isize);
124 mCurrentToolUid = QString(QByteArray(data, size));
146 Frame3D frame = this->frameFromStream();
150 *toolUid = QString::number(tool);
164 Frame3D frame = this->frameFromStream();
171 *toolUid = mCurrentToolUid;
179 Frame3D PositionStorageReader::frameFromStream()
181 boost::array<double, 6> rep;
182 stream >> rep[0] >> rep[1] >> rep[2] >> rep[3] >> rep[4] >> rep[5];
189 return !positions.isReadable() || stream.atEnd() || mError;
198 boost::uint64_t ts =
static_cast<boost::uint64_t
>(timestamp);
199 retval.setTime_t(ts/1000);
200 retval = retval.addMSecs(ts%1000);
212 positions.open(QIODevice::Append);
213 stream.setDevice(&positions);
214 stream.setByteOrder(QDataStream::LittleEndian);
215 if (positions.size() == 0)
217 stream.writeRawData(
"SNWPOS", 6);
233 stream << (quint8)(8+1+6*10);
234 stream << (quint64)timestamp;
235 stream << (quint8)toolIndex;
242 stream << (double)frame.
mPos[0];
243 stream << (
double)frame.
mPos[1];
244 stream << (double)frame.
mPos[2];
249 if (toolUid!=mCurrentToolUid)
251 QByteArray name = toolUid.toLatin1();
254 stream << (quint8)(name.size()+4);
255 stream.writeBytes(name.data(), name.size());
256 mCurrentToolUid = toolUid;
264 stream << (quint8)(8+6*10);
265 stream << (quint64)timestamp;
static QString timestampToString(double timestamp)
PositionStorageReader(QString filename)
static Frame3D fromCompactAxisAngleRep(const boost::array< double, 6 > &rep)
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Transform3D transform() const
Defines an axis-angle representation of a position+orientation in 3D space.
double getThetaXY(Vector3D k)
get thetaXY, meaning the angle of v projected onto the xy plane
boost::array< double, 6 > getCompactAxisAngleRep() const
PositionStorageWriter(QString filename)
double getThetaZ(Vector3D k)
get thetaZ, z meaning the elevation from the xy plane
static Frame3D create(const Transform3D &transform)
bool read(Transform3D *matrix, double *timestamp, int *toolIndex)
void write(Transform3D matrix, uint64_t timestamp, int toolIndex)
Eigen::AngleAxisd mAngleAxis
angle-axis representation of rotation
QString timestampMilliSecondsFormatNice()
Namespace for all CustusX production code.