// 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;
}
void resetClient()
{
int iSuccess;
printf("\n\nre-setting Client\n\n.");
iSuccess = theClient.Uninitialize();
if(iSuccess != 0)
printf("error un-initting Client\n");
iSuccess = theClient.Initialize(szMyIPAddress, szServerIPAddress);
if(iSuccess != 0)
printf("error re-initting Client\n");
}
// Initialize the NatNet client with client and server IP addresses.
bool InitNatNet(LPSTR szIPAddress, LPSTR szServerIPAddress)
{
unsigned char ver[4];
natnetClient.NatNetVersion(ver);
// Set callback handlers
// Callback for NatNet messages.
natnetClient.SetMessageCallback(MessageHandler);
// NatNet verbosity level. We want none.
natnetClient.SetVerbosityLevel(Verbosity_None);
// this function will receive data from the server
natnetClient.SetDataCallback( DataHandler );
int retCode = natnetClient.Initialize(szIPAddress, szServerIPAddress);
if (retCode != ErrorCode_OK)
{
//Unable to connect to server.
return false;
}
else
{
// Print server info
sServerDescription ServerDescription;
memset(&ServerDescription, 0, sizeof(ServerDescription));
natnetClient.GetServerDescription(&ServerDescription);
if(!ServerDescription.HostPresent)
{
//Unable to connect to server. Host not present
return false;
}
}
// Retrieve RigidBody description from server
sDataDescriptions* pDataDefs = NULL;
int nBodies = natnetClient.GetDataDescriptions(&pDataDefs);
if (ParseRigidBodyDescription(pDataDefs) == false)
{
//Unable to retrieve RigidBody description
//return false;
}
// example of NatNet general message passing. Set units to millimeters
// and get the multiplicative conversion factor in the response.
void* response;
int nBytes;
retCode = natnetClient.SendMessageAndWait("UnitsToMillimeters", &response, &nBytes);
if (retCode == ErrorCode_OK)
{
unitConversion = *(float*)response;
}
return true;
}
// Callback for the connect-to-NatNet dialog. Gets the server and local IP
// addresses and attempts to initialize the NatNet client.
LRESULT CALLBACK NatNetDlgProc(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
{
char szBuf[512];
switch (message)
{
case WM_INITDIALOG:
SetDlgItemText(hDlg, IDC_EDIT1, _itoa(IPAddress[0], szBuf,10));
SetDlgItemText(hDlg, IDC_EDIT2, _itoa(IPAddress[1], szBuf,10));
SetDlgItemText(hDlg, IDC_EDIT3, _itoa(IPAddress[2], szBuf,10));
SetDlgItemText(hDlg, IDC_EDIT4, _itoa(IPAddress[3], szBuf,10));
SetDlgItemText(hDlg, IDC_EDIT5, _itoa(IPAddress[0], szBuf,10));
SetDlgItemText(hDlg, IDC_EDIT6, _itoa(IPAddress[1], szBuf,10));
SetDlgItemText(hDlg, IDC_EDIT7, _itoa(IPAddress[2], szBuf,10));
SetDlgItemText(hDlg, IDC_EDIT8, _itoa(IPAddress[3], szBuf,10));
return true;
case WM_COMMAND:
switch (LOWORD(wParam))
{
case IDC_CONNECT:
{
char szMyIPAddress[30], szServerIPAddress[30];
char ip1[5], ip2[5], ip3[5], ip4[5];
GetDlgItemText(hDlg, IDC_EDIT1, ip1, 4);
GetDlgItemText(hDlg, IDC_EDIT2, ip2, 4);
GetDlgItemText(hDlg, IDC_EDIT3, ip3, 4);
GetDlgItemText(hDlg, IDC_EDIT4, ip4, 4);
sprintf_s(szMyIPAddress, 30, "%s.%s.%s.%s", ip1,ip2,ip3,ip4);
GetDlgItemText(hDlg, IDC_EDIT5, ip1, 4);
GetDlgItemText(hDlg, IDC_EDIT6, ip2, 4);
GetDlgItemText(hDlg, IDC_EDIT7, ip3, 4);
GetDlgItemText(hDlg, IDC_EDIT8, ip4, 4);
sprintf_s(szServerIPAddress, 30, "%s.%s.%s.%s", ip1,ip2,ip3,ip4);
// Try and intialize the NatNet client.
if (InitNatNet(szMyIPAddress, szServerIPAddress) == false)
{
natnetClient.Uninitialize();
MessageBox(hDlg, "Failed to connect", "", MB_OK);
}
}
case IDOK:
case IDCANCEL:
EndDialog(hDlg, LOWORD(wParam));
return true;
}
}
return false;
}
int _tmain(int argc, _TCHAR* argv[])
{
splash(argv[0]);
if((argc < 4) || ((argc > 5) && (strcmp("log", argv[5])))){
printf("Usage: %s <ServerIP> <ClientIP> <OSCIP> <OSCPort> [\"log\"]", argv[0]);
}else{
int retCode;
char* OSCIP = argv[3];
int OSCPort = atoi(argv[4]);
//EU
printf("\n\nBanana hammock!!\n\n");
// Set callback handlers
theClient.SetMessageCallback(MessageHandler);
theClient.SetVerbosityLevel(Verbosity_Debug);
theClient.SetDataCallback( DataHandler, &theClient ); // this function will receive data from the server
// Connect to NatNet server
strcpy(szServerIPAddress, argv[1]); // specified on command line
strcpy(szMyIPAddress, argv[2]); // specified on command line
printf("Connecting to server at %s from %s...\n", szServerIPAddress, szMyIPAddress);
// Connect to NatNet server
retCode = theClient.Initialize(szMyIPAddress, szServerIPAddress);
if (retCode != ErrorCode_OK)
{
printf("Unable to connect to server. Error code: %d. Exiting", retCode);
return 1;
}
else
{
// print server info
sServerDescription ServerDescription;
memset(&ServerDescription, 0, sizeof(ServerDescription));
theClient.GetServerDescription(&ServerDescription);
if(!ServerDescription.HostPresent)
{
printf("Unable to connect to server. Host not present. Exiting.");
return 1;
}
printf("[OSCNatNetClient] Server application info:\n");
printf("Application: %s (ver. %d.%d.%d.%d)\n",
ServerDescription.szHostApp,
ServerDescription.HostAppVersion[0],
ServerDescription.HostAppVersion[1],
ServerDescription.HostAppVersion[2],
ServerDescription.HostAppVersion[3]);
printf("NatNet Version: %d.%d.%d.%d\n",
ServerDescription.NatNetVersion[0],
ServerDescription.NatNetVersion[1],
ServerDescription.NatNetVersion[2],
ServerDescription.NatNetVersion[3]);
printf("Server IP:%s\n", szServerIPAddress);
printf("Server Name:%s\n\n", ServerDescription.szHostComputerName);
//EU - init UDP socket
transmitSocket = new UdpTransmitSocket(IpEndpointName(OSCIP, OSCPort));
printf("OSC IP:%s\n", OSCIP);
printf("OSC Port:%d\n", OSCPort);
}
// send/receive test request
printf("[OSCNatNetClient] Sending Test Request\n");
void* response;
int nBytes;
retCode = theClient.SendMessageAndWait("TestRequest", &response, &nBytes);
if (retCode == ErrorCode_OK)
{
printf("[OSCNatNetClient] Received: %s", (char*)response);
}
//Writing Session data to file
char szFile[MAX_PATH];
char szFolder[MAX_PATH];
#ifdef __linux__
sprintf(szFolder, "./");
#else
GetCurrentDirectory(MAX_PATH, szFolder);
#endif
char timeLabel[50];
time_t rawtime;
struct tm* timeinfo;
time(&rawtime);
timeinfo = localtime(&rawtime);
//create time label for files
strftime (timeLabel,80,"%d.%m.%y %H.%M.%S",timeinfo);
// Retrieve MarkerSets from server
printf("\n\n[OSCNatNetClient] Requesting all data definitions for current session...");
sDataDescriptions* pDataDefs = NULL;
theClient.GetDataDescriptions(&pDataDefs);
if(!pDataDefs){
printf("[OSCNatNetClient] Unable to retrieve current session.");
//return 1;
}else{
sprintf(szFile, "%s\\SessionData %s.txt", szFolder, timeLabel);
fps = fopen(szFile, "w");
if(!fps)
{
printf("error opening output file %s. Exiting.", szFile);
exit(1);
}
_WriteHeader(fps, pDataDefs);
fclose(fps);
}
// Prepare data file for frame info
if(argc > 5){
sprintf(szFile, "%s\\FrameData %s.txt", szFolder, timeLabel);
fpf = fopen(szFile, "w");
if(!fpf){
printf("error opening output file %s. Exiting.", szFile);
exit(1);
}
}
// Ready to receive marker stream!
printf("\nOSCNatNetClient is connected to server and listening for data... press 'q' for quitting\n");
int c;
bool bExit = false;
while(c =_getch()){
switch(c){
case 'q':
bExit = true;
printf("\nQuitting...\n\n");
break;
case 'r':
printf("\nReseting Client...\n\n");
resetClient();
break;
default:
printf("not an option\n\n");
break;
}
if(bExit)
break;
}
printf("clean up\n\n");
// Done - clean up.
theClient.Uninitialize();
if(fpf != NULL) fclose(fpf);
return ErrorCode_OK;
}
}
// 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;
}
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
{
int wmId, wmEvent;
PAINTSTRUCT ps;
HDC hdc;
switch (message)
{
case WM_COMMAND:
wmId = LOWORD(wParam);
wmEvent = HIWORD(wParam);
// Parse the menu selections:
switch (wmId)
{
case IDM_CONNECT:
DialogBox(g_hInst, (LPCTSTR)IDD_NATNET, hWnd, (DLGPROC)NatNetDlgProc);
break;
case IDM_EXIT:
DestroyWindow(hWnd);
break;
default:
return DefWindowProc(hWnd, message, wParam, lParam);
}
break;
case WM_PAINT:
hdc = BeginPaint(hWnd, &ps);
wglMakeCurrent(hdc, g_hRenderContext);
Render();
SwapBuffers(hdc);
wglMakeCurrent(0, 0);
EndPaint(hWnd, &ps);
break;
case WM_SIZE:
{
int cx = LOWORD(lParam), cy = HIWORD(lParam);
if(cx==0 || cy ==0 || hWnd==NULL)
break;
GLfloat fFovy = 40.0f; // Field-of-view
GLfloat fZNear = 1.0f; // Near clipping plane
GLfloat fZFar = 10000.0f; // Far clipping plane
HDC hDC = GetDC(hWnd);
wglMakeCurrent(hDC, g_hRenderContext);
// Calculate viewport aspect
RECT rv;
GetClientRect(hWnd, &rv);
GLfloat fAspect = (GLfloat)(rv.right-rv.left) / (GLfloat)(rv.bottom-rv.top);
// Define viewport
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(fFovy, fAspect, fZNear, fZFar);
glViewport(rv.left, rv.top, rv.right-rv.left, rv.bottom-rv.top);
glMatrixMode(GL_MODELVIEW);
wglMakeCurrent(0, 0);
ReleaseDC(hWnd, hDC);
}
break;
case WM_DESTROY:
{
HDC hDC = GetDC(hWnd);
wglMakeCurrent(hDC, g_hRenderContext);
natnetClient.Uninitialize();
wglMakeCurrent(0, 0);
wglDeleteContext(g_hRenderContext);
ReleaseDC(hWnd, hDC);
PostQuitMessage(0);
}
break;
default:
return DefWindowProc(hWnd, message, wParam, lParam);
}
return 0;
}
// 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);
}
}
