int create_msg(DIME_MSG* msg, int msg_type, char* payload, int msg_size)
{
if(!is_valid_msg(msg_type)) { fprintf(stderr, "Invalid Message Type\n"); return 0; }
if(!payload && msg_size > 0) { msg_size = 0; }
//printf("[%s %d]\n", payload, msg_size);
memset(msg, 0, sizeof(DIME_MSG));
msg->msg_type = msg_type;
strncpy(msg->payload, payload, msg_size);
msg->payload_size = msg_size;
// print_msg(msg);
return 1;
}
fsMsgPtr fsBinaryStream::get_message() {
BlockHeader super_block;
if( !headerAvailable(super_block, m_buffer, m_start, m_end) ) return fsMsgPtr();
if (!is_valid_msg(super_block.id)) { LOG_RELEASE_ERROR("Invalid superblock id"); m_valid = false; return fsMsgPtr(); }
if( !blockAvailable( m_buffer, m_start, m_end) ) return fsMsgPtr();
skipHeader(m_start);
long super_block_data_start = m_start;
switch (super_block.id) {
case fsMsg::MSG_IN_START_TRACKING: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgStartCapturing() );
}; break;
case fsMsg::MSG_IN_STOP_TRACKING: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgStopCapturing() );
}; break;
case fsMsg::MSG_IN_CALIBRATE_NEUTRAL: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgCalibrateNeutral() );
}; break;
case fsMsg::MSG_IN_SEND_MARKER_NAMES: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgSendMarkerNames() );
}; break;
case fsMsg::MSG_IN_SEND_BLENDSHAPE_NAMES: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgSendBlendshapeNames() );
}; break;
case fsMsg::MSG_IN_SEND_RIG: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgSendRig() );
}; break;
case fsMsg::MSG_IN_HEADPOSE_RELATIVE: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgHeadPoseRelative() );
}; break;
case fsMsg::MSG_IN_HEADPOSE_ABSOLUTE: {
if (super_block.size > 0) { LOG_RELEASE_ERROR("Expected Size to be 0, not %d", super_block.size); m_valid = false; return fsMsgPtr(); }
return fsMsgPtr(new fsMsgHeadPoseAbsolute() );
}; break;
case fsMsg::MSG_OUT_MARKER_NAMES: {
std::tr1::shared_ptr< fsMsgMarkerNames > msg(new fsMsgMarkerNames());
if( !decodeMarkerNames(*msg, m_buffer, m_start )) { LOG_RELEASE_ERROR("Could not decode marker names"); m_valid = false; return fsMsgPtr(); }
uint64_t actual_size = m_start-super_block_data_start;
if( actual_size != super_block.size ) { LOG_RELEASE_ERROR("Block was promised to be of size %d, not %d", super_block.size, actual_size); m_valid = false; return fsMsgPtr(); }
return msg;
}; break;
case fsMsg::MSG_OUT_BLENDSHAPE_NAMES: {
std::tr1::shared_ptr< fsMsgBlendshapeNames > msg(new fsMsgBlendshapeNames() );
if( !decodeBlendshapeNames(*msg, m_buffer, m_start) ) { LOG_RELEASE_ERROR("Could not decode blendshape names"); m_valid = false; return fsMsgPtr(); }
uint64_t actual_size = m_start-super_block_data_start;
if( actual_size != super_block.size ) { LOG_RELEASE_ERROR("Block was promised to be of size %d, not %d", super_block.size, actual_size); m_valid = false; return fsMsgPtr(); }
return msg;
}; break;
case fsMsg::MSG_OUT_TRACKING_STATE: {
BlockHeader sub_block;
uint16_t num_blocks = 0;
if( !read_pod(num_blocks, m_buffer, m_start) ) { LOG_RELEASE_ERROR("Could not read num_blocks"); m_valid = false; return fsMsgPtr(); }
std::tr1::shared_ptr<fsMsgTrackingState> msg = std::tr1::shared_ptr<fsMsgTrackingState>(new fsMsgTrackingState());
for(int i = 0; i < num_blocks; i++) {
if( !headerAvailable(sub_block, m_buffer, m_start, m_end) ) { LOG_RELEASE_ERROR("could not read sub-header %d", i); m_valid = false; return fsMsgPtr(); }
if( !blockAvailable( m_buffer, m_start, m_end) ) { LOG_RELEASE_ERROR("could not read sub-block %d", i); m_valid = false; return fsMsgPtr(); }
skipHeader(m_start);
long sub_block_data_start = m_start;
bool success = true;
switch(sub_block.id) {
case BLOCKID_INFO: success &= decodeInfo( msg->tracking_data(), m_buffer, m_start); break;
case BLOCKID_POSE: success &= decodePose( msg->tracking_data(), m_buffer, m_start); break;
case BLOCKID_BLENDSHAPES: success &= decodeBlendshapes(msg->tracking_data(), m_buffer, m_start); break;
case BLOCKID_EYES: success &= decodeEyeGaze( msg->tracking_data(), m_buffer, m_start); break;
case BLOCKID_MARKERS: success &= decodeMarkers( msg->tracking_data(), m_buffer, m_start); break;
default:
LOG_RELEASE_ERROR("Unexpected subblock id %d", sub_block.id);
m_valid = false; return msg;
break;
}
if(!success) {
LOG_RELEASE_ERROR("Could not decode subblock with id %d", sub_block.id);
m_valid = false; return fsMsgPtr();
}
uint64_t actual_size = m_start-sub_block_data_start;
if( actual_size != sub_block.size ) {
LOG_RELEASE_ERROR("Unexpected number of bytes consumed %d instead of %d for subblock %d id:%d", actual_size, sub_block.size, i, sub_block.id);
m_valid = false; return fsMsgPtr();
}
}
uint64_t actual_size = m_start-super_block_data_start;
if( actual_size != super_block.size ) {
LOG_RELEASE_ERROR("Unexpected number of bytes consumed %d instead of %d", actual_size, super_block.size);
m_valid = false; return fsMsgPtr();
}
return msg;
}; break;
case fsMsg::MSG_OUT_RIG: {
std::tr1::shared_ptr< fsMsgRig > msg(new fsMsgRig() );
if( !decodeRig(*msg, m_buffer, m_start) ) { LOG_RELEASE_ERROR("Could not decode rig"); m_valid = false; return fsMsgPtr(); }
if( m_start-super_block_data_start != super_block.size ) { LOG_RELEASE_ERROR("Could not decode rig unexpected size"); m_valid = false; return fsMsgPtr(); }
return msg;
}; break;
default: {
LOG_RELEASE_ERROR("Unexpected superblock id %d", super_block.id);
m_valid = false; return fsMsgPtr();
}; break;
}
return fsMsgPtr();
}
