441 lines
7.7 KiB
C++
441 lines
7.7 KiB
C++
/*
|
|
otp/transform.h
|
|
|
|
Copyright (c) 2021 Kevin Matz (kevin.matz@gmail.com)
|
|
|
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
of this software and associated documentation files (the "Software"), to deal
|
|
in the Software without restriction, including without limitation the rights
|
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
copies of the Software, and to permit persons to whom the Software is
|
|
furnished to do so, subject to the following conditions:
|
|
|
|
The above copyright notice and this permission notice shall be included in all
|
|
copies or substantial portions of the Software.
|
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
SOFTWARE.
|
|
*/
|
|
|
|
#include "otp.h"
|
|
#include "transform.h"
|
|
|
|
#include <chrono>
|
|
|
|
namespace OTP::Transform {
|
|
namespace Point {
|
|
|
|
/**
|
|
* @brief address::iStream
|
|
* @param stream
|
|
*/
|
|
void address::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> system;
|
|
*stream >> group;
|
|
*stream >> point;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief address::oStream
|
|
* @param stream
|
|
*/
|
|
void address::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << system;
|
|
*stream << group;
|
|
*stream << point;
|
|
}
|
|
|
|
|
|
namespace Module {
|
|
|
|
/**
|
|
* @brief Transform::Point::Module::pdu::iStream
|
|
* @param stream
|
|
*/
|
|
void pdu::iStream(PDU::Stream stream)
|
|
{
|
|
OTP::PDU::pdu::iStream(stream); // vector and length
|
|
createData<module_data>(); // data
|
|
|
|
auto d = static_cast<module_data*>(data);
|
|
|
|
module_id id;
|
|
id.manufacturer = vector;
|
|
id.number = d->number;
|
|
|
|
if (id.manufacturer == ESTA_MANUFACTURER_ID)
|
|
{
|
|
switch (id.number)
|
|
{
|
|
case Position:
|
|
d->mod = new position_module();
|
|
break;
|
|
case Position_VA:
|
|
d->mod = new position_va_module();
|
|
break;
|
|
case Rotation:
|
|
d->mod = new rotation_module();
|
|
break;
|
|
case Rotation_VA:
|
|
d->mod = new position_va_module();
|
|
break;
|
|
case Scale:
|
|
d->mod = new scale_module();
|
|
break;
|
|
case Reference_Frame:
|
|
d->mod = new reference_frame_module();
|
|
break;
|
|
default:
|
|
return;
|
|
}
|
|
*stream_ >> *d->mod;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief position_module::iStream
|
|
* @param stream
|
|
*/
|
|
void position_module::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> options;
|
|
*stream >> x;
|
|
*stream >> y;
|
|
*stream >> z;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief position_module::oStream
|
|
* @param stream
|
|
*/
|
|
void position_module::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << options;
|
|
*stream << x;
|
|
*stream << y;
|
|
*stream << z;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief position_va_module::iStream
|
|
* @param stream
|
|
*/
|
|
void position_va_module::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> Vx;
|
|
*stream >> Vy;
|
|
*stream >> Vz;
|
|
*stream >> Ax;
|
|
*stream >> Ay;
|
|
*stream >> Az;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief position_va_module::oStream
|
|
* @param stream
|
|
*/
|
|
void position_va_module::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << Vx;
|
|
*stream << Vy;
|
|
*stream << Vz;
|
|
*stream << Ax;
|
|
*stream << Ay;
|
|
*stream << Az;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief rotation_module::iStream
|
|
* @param stream
|
|
*/
|
|
void rotation_module::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> Rx;
|
|
*stream >> Ry;
|
|
*stream >> Rz;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief rotation_module::oStream
|
|
* @param stream
|
|
*/
|
|
void rotation_module::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << Rx;
|
|
*stream << Ry;
|
|
*stream << Rz;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief rotation_va_module::iStream
|
|
* @param stream
|
|
*/
|
|
void rotation_va_module::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> Vrx;
|
|
*stream >> Vry;
|
|
*stream >> Vrz;
|
|
*stream >> Arx;
|
|
*stream >> Ary;
|
|
*stream >> Arz;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief rotation_va_module::oStream
|
|
* @param stream
|
|
*/
|
|
void rotation_va_module::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << Vrx;
|
|
*stream << Vry;
|
|
*stream << Vrz;
|
|
*stream << Arx;
|
|
*stream << Ary;
|
|
*stream << Arz;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief scale_module::iStream
|
|
* @param stream
|
|
*/
|
|
void scale_module::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> scale_x;
|
|
*stream >> scale_y;
|
|
*stream >> scale_z;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief scale_module::oStream
|
|
* @param stream
|
|
*/
|
|
void scale_module::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << scale_x;
|
|
*stream << scale_y;
|
|
*stream << scale_z;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief reference_frame_module::iStream
|
|
* @param s
|
|
*/
|
|
void reference_frame_module::iStream(PDU::Stream s)
|
|
{
|
|
*s >> point;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief reference_frame_module::oStream
|
|
* @param s
|
|
*/
|
|
void reference_frame_module::oStream(PDU::Stream s) const
|
|
{
|
|
*s << point;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief module_data::~module_data
|
|
*/
|
|
module_data::~module_data()
|
|
{
|
|
delete mod;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief module_data::streamSize
|
|
* @return
|
|
*/
|
|
size_t module_data::streamSize() const
|
|
{
|
|
return 2; // + module values
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief module_data::iStream
|
|
* @param stream
|
|
*/
|
|
void module_data::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> number;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief module_data::oStream
|
|
* @param stream
|
|
*/
|
|
void module_data::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << number;
|
|
}
|
|
|
|
|
|
} // namespace Module
|
|
|
|
|
|
/**
|
|
* @brief Transform::Point::pdu::iStream
|
|
* @param stream
|
|
*/
|
|
void pdu::iStream(PDU::Stream stream)
|
|
{
|
|
OTP::PDU::pdu::iStream(stream); // vector and length
|
|
createData<point_data>(); // data
|
|
|
|
auto d = static_cast<point_data*>(data);
|
|
d->modules.setParent(shared_from_this());
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief point_data::streamSize
|
|
* @return
|
|
*/
|
|
size_t point_data::streamSize() const
|
|
{
|
|
return 20 + modules.streamSize();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief point_data::iStream
|
|
* @param stream
|
|
*/
|
|
void point_data::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> priority;
|
|
*stream >> group;
|
|
*stream >> point;
|
|
*stream >> timestamp;
|
|
*stream >> options;
|
|
*stream >> reserved;
|
|
*stream >> modules;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief point_data::oStream
|
|
* @param stream
|
|
*/
|
|
void point_data::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << priority;
|
|
*stream << group;
|
|
*stream << point;
|
|
*stream << timestamp;
|
|
*stream << options;
|
|
*stream << reserved;
|
|
*stream << modules;
|
|
}
|
|
|
|
} // namespace Point
|
|
|
|
|
|
/**
|
|
* @brief Transform::pdu::iStream
|
|
* @param stream
|
|
*/
|
|
void pdu::iStream(PDU::Stream stream)
|
|
{
|
|
OTP::PDU::pdu::iStream(stream); // vector and length
|
|
createData<transform_data>(); // data
|
|
|
|
auto d = static_cast<transform_data*>(data);
|
|
d->points.setParent(shared_from_this());
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief transform_data::streamSize
|
|
* @return
|
|
*/
|
|
size_t transform_data::streamSize() const
|
|
{
|
|
return 14 + points.streamSize();
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief transform_data::iStream
|
|
* @param stream
|
|
*/
|
|
void transform_data::iStream(PDU::Stream stream)
|
|
{
|
|
*stream >> system;
|
|
*stream >> timestamp;
|
|
*stream >> options;
|
|
*stream >> reserved;
|
|
*stream >> points;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief transform_data::oStream
|
|
* @param stream
|
|
*/
|
|
void transform_data::oStream(PDU::Stream stream) const
|
|
{
|
|
*stream << system;
|
|
*stream << std::chrono::system_clock::now();
|
|
*stream << options;
|
|
*stream << reserved;
|
|
*stream << points;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief operator >>
|
|
* @param s
|
|
* @param t
|
|
* @return
|
|
*/
|
|
ACN::PDU::pdu_stream& operator>>(ACN::PDU::pdu_stream& s, OTP::Transform::Timestamp& t)
|
|
{
|
|
const auto us = std::chrono::microseconds(s.readType<uint64_t>());
|
|
t = OTP::Transform::Timestamp(us);
|
|
return s;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief operator <<
|
|
* @param s
|
|
* @param t
|
|
* @return
|
|
*/
|
|
ACN::PDU::pdu_stream& operator<<(ACN::PDU::pdu_stream&s, const OTP::Transform::Timestamp& t)
|
|
{
|
|
const auto d = t.time_since_epoch();
|
|
const auto us = std::chrono::duration_cast<std::chrono::microseconds>(d);
|
|
uint64_t m = us.count();
|
|
s << m;
|
|
return s;
|
|
}
|
|
|
|
|
|
} // namespace OTP::Transform
|