/* Fill v[] with axis (from origin), return angle for quaternion style rotation */
double GetAngleAxis(double hpb[3],double v[3])
{
Quat out;
double ang=0,s;
EulerAngles inAngs = {0,0,0,EulHPB};
inAngs.x = (float)hpb[0];
inAngs.y = (float)hpb[1];
inAngs.z = (float)hpb[2];
out = Eul_ToQuat(inAngs);
ang = acos(out.w);
s=sin(ang);
ang *= 2/RADperDEG;
if(s) /* Is this valid???!!! */
{
v[0] = out.x/s;
v[1] = out.y/s;
v[2] = out.z/s;
}
else if(!ang) v[0] = v[1] = v[2] = 0.0;
return(ang);
}
// Build frame of MocapData
void BuildFrame(long FrameNumber, sDataDescriptions* pModels, sFrameOfMocapData* pOutFrame)
{
if(!pModels)
{
printf("No models defined - nothing to send.\n");
return;
}
ZeroMemory(pOutFrame, sizeof(sFrameOfMocapData));
pOutFrame->iFrame = FrameNumber;
pOutFrame->fLatency = (float)GetTickCount();
pOutFrame->nOtherMarkers = 0;
pOutFrame->nMarkerSets = 0;
pOutFrame->nRigidBodies = 0;
pOutFrame->nLabeledMarkers = 0;
for(int i=0; i < pModels->nDataDescriptions; i++)
{
#if STREAM_MARKERS
// MarkerSet data
if(pModels->arrDataDescriptions[i].type == Descriptor_MarkerSet)
{
// add marker data
int index = pOutFrame->nMarkerSets;
sMarkerSetDescription* pMS = pModels->arrDataDescriptions[i].Data.MarkerSetDescription;
strcpy(pOutFrame->MocapData[index].szName, pMS->szName);
pOutFrame->MocapData[index].nMarkers = pMS->nMarkers;
pOutFrame->MocapData[index].Markers = new MarkerData[pOutFrame->MocapData[0].nMarkers];
for(int iMarker=0; iMarker < pOutFrame->MocapData[index].nMarkers; iMarker++)
{
double rads = (double)counter * 3.14159265358979 / 180.0f;
pOutFrame->MocapData[index].Markers[iMarker][0] = (float) sin(rads) + (10*iMarker); // x
pOutFrame->MocapData[index].Markers[iMarker][1] = (float) cos(rads) + (20*iMarker); // y
pOutFrame->MocapData[index].Markers[iMarker][2] = (float) tan(rads) + (30*iMarker); // z
counter++;
counter %= 360;
}
pOutFrame->nMarkerSets++;
}
#endif
#if STREAM_RBS
// RigidBody data
if(pModels->arrDataDescriptions[i].type == Descriptor_RigidBody)
{
sRigidBodyDescription* pMS = pModels->arrDataDescriptions[i].Data.RigidBodyDescription;
int index = pOutFrame->nRigidBodies;
sRigidBodyData* pRB = &pOutFrame->RigidBodies[index];
pRB->ID = pMS->ID;
double rads = (double)counter * 3.14159265358979 / 180.0f;
pRB->x = (float) sin(rads);
pRB->y = (float) cos(rads);
pRB->z = (float) tan(rads);
EulerAngles ea;
ea.x = (float) sin(rads);
ea.y = (float) cos(rads);
ea.z = (float) tan(rads);
ea.w = 0.0f;
Quat q = Eul_ToQuat(ea);
pRB->qx = q.x;
pRB->qy = q.y;
pRB->qz = q.z;
pRB->qw = q.w;
pRB->nMarkers = 3;
pRB->Markers = new MarkerData[pRB->nMarkers];
pRB->MarkerIDs = new int[pRB->nMarkers];
pRB->MarkerSizes = new float[pRB->nMarkers];
pRB->MeanError = 0.0f;
for(int iMarker=0; iMarker < pRB->nMarkers; iMarker++)
{
pRB->Markers[iMarker][0] = iMarker + 0.1f; // x
pRB->Markers[iMarker][1] = iMarker + 0.2f; // y
pRB->Markers[iMarker][2] = iMarker + 0.3f; // z
pRB->MarkerIDs[iMarker] = iMarker + 200;
pRB->MarkerSizes[iMarker] = 77.0f;
}
pOutFrame->nRigidBodies++;
counter++;
}
#endif
#if STREAM_SKELETONS
// Skeleton data
if(pModels->arrDataDescriptions[i].type == Descriptor_Skeleton)
{
sSkeletonDescription* pSK = pModels->arrDataDescriptions[i].Data.SkeletonDescription;
int index = pOutFrame->nSkeletons;
pOutFrame->Skeletons[index].skeletonID = pSK->skeletonID;
pOutFrame->Skeletons[index].nRigidBodies = pSK->nRigidBodies;
// RigidBody data
pOutFrame->Skeletons[index].RigidBodyData = new sRigidBodyData[pSK->nRigidBodies];
for(int i=0; i<pSK->nRigidBodies; i++)
{
sRigidBodyData* pRB = &pOutFrame->Skeletons[index].RigidBodyData[i];
pRB->ID = pOutFrame->Skeletons[index].skeletonID + i;
double rads = (double)counter * 3.14159265358979 / 180.0f;
pRB->x = (float) sin(rads);
pRB->y = (float) cos(rads);
pRB->z = (float) tan(rads);
fCounter += 0.1f;
if(fCounter > 1.0f)
fCounter = 0.1f;
pRB->qx = fCounter;
pRB->qy = fCounter + 0.1f;
pRB->qz = fCounter + 0.2f;
pRB->qw = 1.0f;
pRB->nMarkers = 3;
pRB->Markers = new MarkerData[pRB->nMarkers];
pRB->MarkerIDs = new int[pRB->nMarkers];
pRB->MarkerSizes = new float[pRB->nMarkers];
pRB->MeanError = 0.0f;
for(int iMarker=0; iMarker < pRB->nMarkers; iMarker++)
{
pRB->Markers[iMarker][0] = iMarker + 0.1f; // x
pRB->Markers[iMarker][1] = iMarker + 0.2f; // y
pRB->Markers[iMarker][2] = iMarker + 0.3f; // z
pRB->MarkerIDs[iMarker] = iMarker + 200;
pRB->MarkerSizes[iMarker] = 77.0f;
}
counter++;
}
pOutFrame->nSkeletons++;
}
#endif
}
#if STREAM_LABELED_MARKERS
// add marker data
pOutFrame->nLabeledMarkers = 10;
for(int iMarker=0; iMarker < 10; iMarker++)
{
sMarker* pMarker = &pOutFrame->LabeledMarkers[iMarker];
pMarker->ID = iMarker+100;
pMarker->x = (float) iMarker;
pMarker->y = (float) (counter2 * iMarker);
pMarker->z = (float) iMarker;
pMarker->size = 5.0f;
}
counter2++;
counter2 %= 100;
#endif
}