// NatNet data callback function. Stores rigid body and marker data in the file level
// variables markerPositions, and rigidBodies and sets the file level variable render
// to true. This signals that we have a frame ready to render.
void DataHandler(sFrameOfMocapData* data, void* pUserData)
{
int mcount = min(MarkerPositionCollection::MAX_MARKER_COUNT,data->MocapData->nMarkers);
markerPositions.SetMarkerPositions(data->MocapData->Markers, mcount);
// unidentified markers
mcount = min(MarkerPositionCollection::MAX_MARKER_COUNT,data->nOtherMarkers);
markerPositions.AppendMarkerPositions(data->OtherMarkers, mcount);
int rbcount = min(RigidBodyCollection::MAX_RIGIDBODY_COUNT, data->nRigidBodies);
rigidBodies.SetRigidBodyData(data->RigidBodies, rbcount);
for (int s = 0; s < data->nSkeletons; s++)
{
rigidBodies.AppendRigidBodyData(data->Skeletons[s].RigidBodyData, data->Skeletons[s].nRigidBodies);
}
markerPositions.SetLabledMarkers(data->LabeledMarkers, data->nLabeledMarkers);
// timecode
NatNetClient* pClient = (NatNetClient*)pUserData;
// decode to values
int hour, minute, second, frame, subframe;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
render = true;
}
// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData)
{
NatNetClient* pClient = (NatNetClient*)pUserData;
int i = 0;
printf("FrameID : %d\n", data->iFrame);
printf("Timestamp : %3.2lf\n", data->fTimestamp);
printf("Latency : %3.2lf\n", data->fLatency);
// FrameOfMocapData params
bool bIsRecording = data->params & 0x01;
bool bTrackedModelsChanged = data->params & 0x02;
if (bIsRecording)
printf("RECORDING\n");
if (bTrackedModelsChanged)
printf("Models Changed.\n");
// timecode - for systems with an eSync and SMPTE timecode generator - decode to values
int hour, minute, second, frame, subframe;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
char szTimecode[128] = "";
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
printf("Timecode : %s\n", szTimecode);
// Rigid Bodies
printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies);
std::stringstream ss;
bool any = false;
for (i = 0; i < data->nRigidBodies; i++)
{
// params
// 0x01 : bool, rigid body was successfully tracked in this frame
bool bTrackingValid = data->RigidBodies[i].params & 0x01;
if (bTrackingValid)
{
auto &rb = data->RigidBodies[i];
float
x = rb.x,
y = rb.y,
z = rb.z,
qw = rb.qw,
qx = rb.qx,
qy = rb.qy,
qz = rb.qz;
#ifdef UNREAL_TRANSFORM
#define PI 3.141592
{
typedef Eigen::AngleAxisd Axis;
typedef Eigen::Vector3d Vec;
auto UX = Vec::UnitX();
auto UY = Vec::UnitY();
auto UZ = Vec::UnitZ();
auto q = Eigen::Quaterniond(qw, qx, qy, qz);
/* Here we 'add' an angular offset */{
Eigen::Quaterniond
a(Eigen::AngleAxisd(PI, UX)),
b(Eigen::AngleAxisd(PI, UY)),
c(Eigen::AngleAxisd(PI, UZ));
q = q * a;
}
/* Here we invert the rotation axes */{
auto euler = q.toRotationMatrix().eulerAngles(2, 0, 1);
auto flip =
Axis((euler[0]), UX) *
Axis(-(euler[1]), UY) *
Axis(-(euler[2]), UZ);
q = Eigen::Quaterniond(flip);
}
float
tx = x,
ty = y,
tz = z;
x = -tz;
y = tx;
z = ty;
qw = q.w();
qx = q.x();
qy = q.y();
qz = q.z();
}
#endif
ss
<< x << " " << y << " " << z << " "
<< qw << " " << qx << " " << qy << " " << qz << " "
<< data->fTimestamp << " " << rb.ID << "\n";
any = true;
}
printf("Rigid Body [ID=%d Error=%3.2f Valid=%d]\n", data->RigidBodies[i].ID, data->RigidBodies[i].MeanError, bTrackingValid);
//printf("\tx\ty\tz\tqx\tqy\tqz\tqw\n");
printf("\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",
data->RigidBodies[i].x,
data->RigidBodies[i].y,
data->RigidBodies[i].z,
data->RigidBodies[i].qx,
data->RigidBodies[i].qy,
data->RigidBodies[i].qz,
data->RigidBodies[i].qw);
}
printf("Sending data...\n");
if (any && tcpClient)
{
if (tcpClient->write(ss.str()))
printf("Sending data... OK\n");
else
printf("Sending data... SOCKET CLOSED\n");
}
else
printf("Sending data... NOPE\n");
}
// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData)
{
if (acommand.packet_counter != 4) {
acommand.packet_counter++;
return;
}
acommand.packet_counter = 0;
NatNetClient* pClient = (NatNetClient*) pUserData;
printf("Received frame %d\n", data->iFrame);
if(fp)
_WriteFrame(fp,data);
int i=0;
// same system latency test
float fThisTick = (float)GetTickCount();
float fDiff = fThisTick - data->fLatency;
double dDuration = fDiff;
printf("Latency (same system) (msecs): %3.2lf\n", dDuration);
// timecode
// decode to values
int hour, minute, second, frame, subframe;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
char szTimecode[128] = "";
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
printf("Timecode : %s\n", szTimecode);
// Other Markers
printf("Other Markers [Count=%d]\n", data->nOtherMarkers);
for(i=0; i < data->nOtherMarkers; i++)
{
printf("Other Marker %d : %3.2f\t%3.2f\t%3.2f\n",
i,
data->OtherMarkers[i][0],
data->OtherMarkers[i][1],
data->OtherMarkers[i][2]);
}
// Rigid Bodies
printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies);
double theta = calculate_angle(data->RigidBodies[0], data->RigidBodies[1], data->RigidBodies[2]);
double dist_calc = calculate_distance(data->RigidBodies[0], data->RigidBodies[3]);
printf("\n\n\nAngle is %Lf and distance is %Lf\n\n\n", theta*180/3.14, dist_calc);
for(i=0; i < data->nRigidBodies; i++)
{
//location_data[i] = data->RigidBodies[i];
printf("Rigid Body [ID=%d Error=%3.2f]\n", data->RigidBodies[i].ID, data->RigidBodies[i].MeanError);
printf("\tx\ty\tz\tqx\tqy\tqz\tqw\n");
printf("\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",
data->RigidBodies[i].x,
data->RigidBodies[i].y,
data->RigidBodies[i].z,
data->RigidBodies[i].qx,
data->RigidBodies[i].qy,
data->RigidBodies[i].qz,
data->RigidBodies[i].qw);
printf("\tRigid body markers [Count=%d]\n", data->RigidBodies[i].nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(data->RigidBodies[i].MarkerIDs)
printf("MarkerID:%d", data->RigidBodies[i].MarkerIDs[iMarker]);
if(data->RigidBodies[i].MarkerSizes)
printf("\tMarkerSize:%3.2f", data->RigidBodies[i].MarkerSizes[iMarker]);
if(data->RigidBodies[i].Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
// skeletons
printf("Skeletons [Count=%d]\n", data->nSkeletons);
for(i=0; i < data->nSkeletons; i++)
{
sSkeletonData skData = data->Skeletons[i];
printf("Skeleton [ID=%d Bone count=%d]\n", skData.skeletonID, skData.nRigidBodies);
for(int j=0; j< skData.nRigidBodies; j++)
{
sRigidBodyData rbData = skData.RigidBodyData[j];
printf("Bone %d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",
rbData.ID, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw );
printf("\tRigid body markers [Count=%d]\n", rbData.nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(rbData.MarkerIDs)
printf("MarkerID:%d", rbData.MarkerIDs[iMarker]);
if(rbData.MarkerSizes)
printf("\tMarkerSize:%3.2f", rbData.MarkerSizes[iMarker]);
if(rbData.Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
}
// labeled markers
printf("Labeled Markers [Count=%d]\n", data->nLabeledMarkers);
for(i=0; i < data->nLabeledMarkers; i++)
{
sMarker marker = data->LabeledMarkers[i];
printf("Labeled Marker [ID=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n", marker.ID, marker.size, marker.x, marker.y, marker.z);
}
if (!init_called) {
//init();
init_called = true;
}
EnterCriticalSection(&dest_cont.lock);
int destination = dest_cont.dests[dest_cont.step_num];
int stn = dest_cont.step_num;
int nsc = num_same_cmds;
int num_dests = dest_cont.num_dests;
LeaveCriticalSection(&dest_cont.lock);
if (destination >0) {
double angle_to_dest = calculate_angle(data->RigidBodies[0], data->RigidBodies[1], data->RigidBodies[destination]);
double dist_to_dest = calculate_distance(data->RigidBodies[0], data->RigidBodies[destination]);
printf("==================================== %f", dist_to_dest);
printf("angle is %f", angle_to_dest);
char return_val[1];
if (dist_to_dest < .4) {
if (acommand.state != '0' || last_dest != destination) {
acommand.cmd = '0';
send_cmd(NULL);
last_dest = destination;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
//state = '0';
EnterCriticalSection(&dest_cont.lock);
dest_cont.step_num += 1;
//dest_cont.num_dests -= 1;
LeaveCriticalSection(&dest_cont.lock);
num_same_cmds = 0;
} else {
num_same_cmds += 1;
if (num_same_cmds > 20) {
acommand.cmd = '0';
send_cmd(NULL);
num_same_cmds = 0;
}
}
//dest_cont.step_num += 1;
return;
}
if ( angle_to_dest <165 ) {
if (acommand.state != 'R') {
acommand.cmd = 'R';
send_cmd(NULL);
num_same_cmds = 0;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
} else {
num_same_cmds += 1;
if (num_same_cmds > 20) {
acommand.cmd = 'R';
send_cmd(NULL);
num_same_cmds = 0;
}
}
state = 'R';
return;
} else if (angle_to_dest > 170) {
if (acommand.state == 'R' || acommand.state == 'L') {
acommand.cmd = '0';
send_cmd(NULL);
num_same_cmds = 0;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
//state = '0';
} else {
if (acommand.state != 'D') {
acommand.cmd = 'D';
send_cmd(NULL);
num_same_cmds = 0;
//HANDLE thread = (HANDLE)::_beginthreadex(NULL, 0, send_cmd, &(acommand.cmd), 0, NULL);
} else {
num_same_cmds += 1;
if (num_same_cmds > 20) {
acommand.cmd = 'D';
send_cmd(NULL);
num_same_cmds = 0;
}
}
//state = 'D';
}
}
}
}
void DataHandler(sFrameOfMocapData* data, void* pUserData)
{
int i;
int mcount = min(MAX_MARKER_COUNT,data->MocapData->nMarkers);
for(i=0;i<mcount;i++)
{
markerPos[3*i] = data->MocapData->Markers[i][0]; //x pos
markerPos[3*i+1] = data->MocapData->Markers[i][1]; //y pos
markerPos[3*i+2] = data->MocapData->Markers[i][2]; //z pos
}
markerCount = mcount;
// unidentified markers
mcount = min(MAX_MARKER_COUNT,data->nOtherMarkers);
for(i=0;i<mcount;i++)
{
markerPos[markerCount + (3*i)] = data->OtherMarkers[i][0] * g_unitConversion; //x pos
markerPos[markerCount + (3*i)+1] = data->OtherMarkers[i][1] * g_unitConversion; //y pos
markerPos[markerCount + (3*i)+2] = data->OtherMarkers[i][2] * g_unitConversion; //z pos
}
markerCount += mcount;
int rbcount = min(MAX_RIGIDBODY_COUNT, data->nRigidBodies);
for (i=0;i<rbcount;i++)
{
rigidbodyids[i] = data->RigidBodies[i].ID;
rigidbody[7*i] = data->RigidBodies[i].x; //x pos
rigidbody[7*i+1] = data->RigidBodies[i].y; //y pos
rigidbody[7*i+2] = data->RigidBodies[i].z; //z pos
rigidbody[7*i+3] = data->RigidBodies[i].qx; //quaternion x
rigidbody[7*i+4] = data->RigidBodies[i].qy; //quaternion y
rigidbody[7*i+5] = data->RigidBodies[i].qz; //quaternion z
rigidbody[7*i+6] = data->RigidBodies[i].qw; //quaternion w
}
rigidbodyCount = rbcount;
int sklRigidbodyCount = 0;
for (int s=0;s<data->nSkeletons;s++)
{
for (int r=0; r<data->Skeletons[s].nRigidBodies; r++)
{
rigidbodyids[i] = data->Skeletons[s].RigidBodyData[r].ID;
rigidbody[7*i] = data->Skeletons[s].RigidBodyData[r].x; //x pos
rigidbody[7*i+1] = data->Skeletons[s].RigidBodyData[r].y; //y pos
rigidbody[7*i+2] = data->Skeletons[s].RigidBodyData[r].z; //z pos
rigidbody[7*i+3] = data->Skeletons[s].RigidBodyData[r].qx; //quaternion x
rigidbody[7*i+4] = data->Skeletons[s].RigidBodyData[r].qy; //quaternion y
rigidbody[7*i+5] = data->Skeletons[s].RigidBodyData[r].qz; //quaternion z
rigidbody[7*i+6] = data->Skeletons[s].RigidBodyData[r].qw; //quaternion w
i++;
sklRigidbodyCount++;
}
}
rigidbodyCount += sklRigidbodyCount;
nLabeledMarkers = data->nLabeledMarkers;
for(int i=0;i<nLabeledMarkers;i++)
{
labeledMarkers[i].ID = data->LabeledMarkers[i].ID;
labeledMarkers[i].x = data->LabeledMarkers[i].x;
labeledMarkers[i].y = data->LabeledMarkers[i].y;
labeledMarkers[i].z = data->LabeledMarkers[i].z;
labeledMarkers[i].size = data->LabeledMarkers[i].size;
}
// timecode
NatNetClient* pClient = (NatNetClient*)pUserData;
// decode to values
int hour, minute, second, frame, subframe;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
render = true;
}
// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData)
{
NatNetClient* pClient = (NatNetClient*) pUserData;
printf("Received frame %d\n", data->iFrame);
int i=0;
// same system latency test
float fThisTick = (float)GetTickCount();
float fDiff = fThisTick - data->fLatency;
double dDuration = fDiff;
printf("Latency (same system) (msecs): %3.2lf\n", dDuration);
// timecode
// decode to values
int hour, minute, second, frame, subframe;
bool bValid = pClient->DecodeTimecode(data->Timecode, data->TimecodeSubframe, &hour, &minute, &second, &frame, &subframe);
// decode to friendly string
char szTimecode[128] = "";
pClient->TimecodeStringify(data->Timecode, data->TimecodeSubframe, szTimecode, 128);
printf("Timecode : %s\n", szTimecode);
// Other Markers
printf("Other Markers [Count=%d]\n", data->nOtherMarkers);
for(i=0; i < data->nOtherMarkers; i++)
{
printf("Other Marker %d : %3.2f\t%3.2f\t%3.2f\n",
i,
data->OtherMarkers[i][0],
data->OtherMarkers[i][1],
data->OtherMarkers[i][2]);
}
// Rigid Bodies
printf("Rigid Bodies [Count=%d]\n", data->nRigidBodies);
for(i=0; i < data->nRigidBodies; i++)
{
printf("Rigid Body [ID=%d Error=%3.2f]\n", data->RigidBodies[i].ID, data->RigidBodies[i].MeanError);
printf("\tx\ty\tz\tqx\tqy\tqz\tqw\n");
printf("\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",
data->RigidBodies[i].x,
data->RigidBodies[i].y,
data->RigidBodies[i].z,
data->RigidBodies[i].qx,
data->RigidBodies[i].qy,
data->RigidBodies[i].qz,
data->RigidBodies[i].qw);
printf("\tRigid body markers [Count=%d]\n", data->RigidBodies[i].nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(data->RigidBodies[i].MarkerIDs)
printf("MarkerID:%d", data->RigidBodies[i].MarkerIDs[iMarker]);
if(data->RigidBodies[i].MarkerSizes)
printf("\tMarkerSize:%3.2f", data->RigidBodies[i].MarkerSizes[iMarker]);
if(data->RigidBodies[i].Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
// skeletons
printf("Skeletons [Count=%d]\n", data->nSkeletons);
for(i=0; i < data->nSkeletons; i++)
{
sSkeletonData skData = data->Skeletons[i];
printf("Skeleton [ID=%d Bone count=%d]\n", skData.skeletonID, skData.nRigidBodies);
for(int j=0; j< skData.nRigidBodies; j++)
{
sRigidBodyData rbData = skData.RigidBodyData[j];
printf("Bone %d\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\t%3.2f\n",
rbData.ID, rbData.x, rbData.y, rbData.z, rbData.qx, rbData.qy, rbData.qz, rbData.qw );
printf("\tRigid body markers [Count=%d]\n", rbData.nMarkers);
for(int iMarker=0; iMarker < data->RigidBodies[i].nMarkers; iMarker++)
{
printf("\t\t");
if(rbData.MarkerIDs)
printf("MarkerID:%d", rbData.MarkerIDs[iMarker]);
if(rbData.MarkerSizes)
printf("\tMarkerSize:%3.2f", rbData.MarkerSizes[iMarker]);
if(rbData.Markers)
printf("\tMarkerPos:%3.2f,%3.2f,%3.2f\n" ,
data->RigidBodies[i].Markers[iMarker][0],
data->RigidBodies[i].Markers[iMarker][1],
data->RigidBodies[i].Markers[iMarker][2]);
}
}
}
// labeled markers
printf("Labeled Markers [Count=%d]\n", data->nLabeledMarkers);
for(i=0; i < data->nLabeledMarkers; i++)
{
sMarker marker = data->LabeledMarkers[i];
printf("Labeled Marker [ID=%d] [size=%3.2f] [pos=%3.2f,%3.2f,%3.2f]\n", marker.ID, marker.size, marker.x, marker.y, marker.z);
}
}
