14 #include <boost/cstdint.hpp> 26 positions.open(QIODevice::ReadOnly);
27 stream.setDevice(&positions);
28 stream.setByteOrder(QDataStream::LittleEndian);
31 memset(header, 0,
sizeof(header));
32 stream.readRawData(header, 6);
36 if (!positions.exists())
38 std::cout <<
"Error: File [" << filename.toStdString() <<
"] does not exist." << std::endl;
41 if (QString(header)!=
"SNWPOS" || mVersion<1)
44 std::cout <<
"Error in header for file [" << filename.toStdString() <<
"]" << std::endl;
76 Frame3D frame = this->frameFromStream();
82 return (type==1) && (size==15);
102 stream.readBytes(data, isize);
103 mCurrentToolUid = QString(QByteArray(data, size));
125 Frame3D frame = this->frameFromStream();
129 *toolUid = QString::number(tool);
143 Frame3D frame = this->frameFromStream();
150 *toolUid = mCurrentToolUid;
158 Frame3D PositionStorageReader::frameFromStream()
160 boost::array<double, 6> rep;
161 stream >> rep[0] >> rep[1] >> rep[2] >> rep[3] >> rep[4] >> rep[5];
168 return !positions.isReadable() || stream.atEnd() || mError;
177 boost::uint64_t ts =
static_cast<boost::uint64_t
>(timestamp);
178 retval.setTime_t(ts/1000);
179 retval = retval.addMSecs(ts%1000);
191 positions.open(QIODevice::Append);
192 stream.setDevice(&positions);
193 stream.setByteOrder(QDataStream::LittleEndian);
194 if (positions.size() == 0)
196 stream.writeRawData(
"SNWPOS", 6);
212 stream << (quint8)(8+1+6*10);
213 stream << (quint64)timestamp;
214 stream << (quint8)toolIndex;
221 stream << (double)frame.
mPos[0];
222 stream << (
double)frame.
mPos[1];
223 stream << (double)frame.
mPos[2];
228 if (toolUid!=mCurrentToolUid)
230 QByteArray name = toolUid.toLatin1();
233 stream << (quint8)(name.size()+4);
234 stream.writeBytes(name.data(), name.size());
235 mCurrentToolUid = toolUid;
243 stream << (quint8)(8+6*10);
244 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.