void Render()
{
GLfloat m[9];
GLfloat v[3];
float fRadius = 5.0f;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear buffers
glLoadIdentity(); // Load identity matrix
GLfloat glfLight[] = {-4.0f, 4.0f, 4.0f, 0.0f};
glLightfv(GL_LIGHT0, GL_POSITION, glfLight);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glPushMatrix();
// draw timecode
glPushMatrix();
glTranslatef(2400.f, -1750.f, -5000.0f);
glPrint(0.0f,0.0f,szTimecode);
glPopMatrix();
// Position and rotate the camera
#if 1
glTranslatef(0.0f, -1000.0f, -5000.0f);
#else
// ori
float quat[4];
quat[0] = rigidbody[3];
quat[1] = rigidbody[4];
quat[2] = rigidbody[5];
quat[3] = rigidbody[6];
float yaw, pitch, roll;
GetEulers(quat[0], quat[1], quat[2], quat[3], &yaw, &pitch, &roll);
glRotatef(-roll, 1.0f, 0.0f, 0.0f);
glRotatef(-yaw, 0.0f, 1.0f, 0.0f);
glRotatef(-pitch, 0.0f, 0.0f, 1.0f);
/*
EulerAngles eulers = Eul_FromQuat(q, order);
eulers.x = RadiansToDegrees(eulers.x);
eulers.y = RadiansToDegrees(eulers.y);
eulers.z = RadiansToDegrees(eulers.z);
*/
// pos
float x = rigidbody[0];
float y = rigidbody[1];
float z = rigidbody[2];
x *= g_unitConversion; // convert to mm
y *= g_unitConversion;
z *= g_unitConversion;
//glTranslatef(0.0f, -100.0f, -5000.0f); // origin offset
glTranslatef(-x, -y, -z);
#endif
//draw axis
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3f(0,0,0);
glVertex3f(300,0,0);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3f(0,0,0);
glVertex3f(0,300,0);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3f(0,0,0);
glVertex3f(0,0,300);
glEnd();
// draw grid
DrawGrid();
int i;
#if 1
// draw rigid bodies
for (i=0;i<rigidbodyCount;i++)
{
int index = 7*i; // a rigid body pos/ori consists of 7 values : [x,y,x,qx,qy,qz,qw]
GLfloat x = rigidbody[index], y = rigidbody[index+1], z = rigidbody[index+2];
x *= g_unitConversion; // convert to mm
y *= g_unitConversion;
z *= g_unitConversion;
GLfloat qx = rigidbody[index+3], qy = rigidbody[index+4], qz = rigidbody[index+5], qw = rigidbody[index+6];
glColor4f(0.0f,0.0f,1.0f,1.0f);
drawbox(x,y,z,qx,qy,qz,qw);
glColor4f(0.0f,0.0f,0.0f,1.0f);
// Convert quaternion to eulers. Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
Quat q;
q.x = qx; q.y = qy; q.z = qz; q.w = qw;
int order = EulOrdXYZr;
EulerAngles ea = Eul_FromQuat(q, order);
ea.x = RadiansToDegrees(ea.x);
ea.y = RadiansToDegrees(ea.y);
ea.z = RadiansToDegrees(ea.z);
float angle1, angle2, angle3;
GetEulers(qx, qy, qz, qw, &angle1, &angle2, &angle3);
glPrint(x,y,"RB %d (yaw:%3.1f, pitch:%3.1f, roll:%3.1f) (yaw:%3.1f, pitch:%3.1f, roll:%3.1f)", rigidbodyids[i], ea.x, ea.y, ea.z, angle1, angle2, angle3 );
}
#endif
#if 1
// draw markers
// [optional] local coordinate support : get first rb's pos/ori (face support only- assume 1st is root)
if(rigidbodyCount==1)
{
GLfloat q[4] = {rigidbody[3], rigidbody[4], rigidbody[5], rigidbody[6]};
quatToMatrix(q, m);
}
for (i=0;i<markerCount;i++)
{
int index = 3*i;
v[0] = markerPos[index];
v[1] = markerPos[index+1];
v[2] = markerPos[index+2];
// [optional] local coordinate support : inherit (accumulate) parent's RB pos/ori ("root") if using local marker position
if(rigidbodyCount==1)
{
vec3MatrixMult(v,m);
v[0] += rigidbody[0];
v[1] += rigidbody[1];
v[2] += rigidbody[2];
}
glPushMatrix();
glTranslatef(v[0],v[1],v[2]);
drawsphere(1,fRadius);
glPopMatrix();
}
#endif
#if 1 //draw labeled markers
for (i=0;i<nLabeledMarkers;i++)
{
v[0] = labeledMarkers[i].x * g_unitConversion;
v[1] = labeledMarkers[i].y * g_unitConversion;
v[2] = labeledMarkers[i].z * g_unitConversion;
fRadius = labeledMarkers[i].size * g_unitConversion;
glPushMatrix();
glTranslatef(v[0],v[1],v[2]);
glColor4f(1.0f,0.0f,0.0f,1.0f);
drawsphere(1,fRadius);
/*
glPrint(v[0],v[1], "Marker: %d (%3.1f, %3.1f, %3.1f)",
labeledMarkers[i].ID, labeledMarkers[i].x,labeledMarkers[i].y,labeledMarkers[i].z );
*/
glPopMatrix();
}
#endif
glPopMatrix();
glFlush();
render = false;
}
// Main OpenGL rendering function.
void Render()
{
GLfloat m[9];
GLfloat v[3];
float fRadius = 5.0f;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear buffers
glLoadIdentity(); // Load identity matrix
GLfloat glfLight[] = {-4.0f, 4.0f, 4.0f, 0.0f};
glLightfv(GL_LIGHT0, GL_POSITION, glfLight);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glPushMatrix();
// draw timecode
glPushMatrix();
glTranslatef(2400.f, -1750.f, -5000.0f);
glPrinter.Print(0.0f,0.0f,szTimecode);
glPopMatrix();
// Position and rotate the camera
glTranslatef(0.0f, -1000.0f, -5000.0f);
//draw axis
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3f(0,0,0);
glVertex3f(300,0,0);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3f(0,0,0);
glVertex3f(0,300,0);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3f(0,0,0);
glVertex3f(0,0,300);
glEnd();
// draw grid
OpenGLDrawingFunctions::DrawGrid();
// Draw the rigid bodies
for (size_t i = 0; i < rigidBodies.Count(); i++)
{
GLfloat x, y, z;
std::tie(x, y, z) = rigidBodies.GetCoordinates(i);
x *= unitConversion; // convert to mm
y *= unitConversion;
z *= unitConversion;
GLfloat qx, qy, qz, qw;
std::tie(qx, qy, qz, qw) = rigidBodies.GetQuaternion(i);
glColor4f(0.0f,0.0f,1.0f,1.0f);
OpenGLDrawingFunctions::DrawBox(x,y,z,qx,qy,qz,qw);
glColor4f(0.0f,0.0f,0.0f,1.0f);
// Convert quaternion to eulers. Motive coordinate conventions: X(Pitch), Y(Yaw), Z(Roll), Relative, RHS
Quat q;
q.x = qx; q.y = qy; q.z = qz; q.w = qw;
int order = EulOrdXYZr;
EulerAngles ea = Eul_FromQuat(q, order);
ea.x = NATUtils::RadiansToDegrees(ea.x);
ea.y = NATUtils::RadiansToDegrees(ea.y);
ea.z = NATUtils::RadiansToDegrees(ea.z);
glPrinter.Print(x,y,"RB %d (Pitch: %3.1f, Yaw: %3.1f, Roll: %3.1f)", rigidBodies.ID(i), ea.x, ea.y, ea.z);
}
// draw markers
// [optional] local coordinate support : get first rb's pos/ori (face support only- assume 1st is root)
if(rigidBodies.Count()==1)
{
GLfloat q[4];
std::tie(q[0], q[1], q[2], q[3]) = rigidBodies.GetQuaternion(0);
NATUtils::QaternionToRotationMatrix(q, m);
}
for (size_t i = 0; i < markerPositions.MarkerPositionCount(); i++)
{
std::tie(v[0], v[1], v[2]) = markerPositions.GetMarkerPosition(i);
// [optional] local coordinate support : inherit (accumulate) parent's RB pos/ori ("root") if using local marker position
if(rigidBodies.Count()==1)
{
NATUtils::Vec3MatrixMult(v,m);
v[0] += std::get<0>(rigidBodies.GetCoordinates(0));
v[1] += std::get<1>(rigidBodies.GetCoordinates(0));
v[2] += std::get<2>(rigidBodies.GetCoordinates(0));
}
glPushMatrix();
glTranslatef(v[0] * unitConversion, v[1] * unitConversion, v[2] * unitConversion);
OpenGLDrawingFunctions::DrawSphere(1,fRadius);
glPopMatrix();
}
//draw labeled markers
for (size_t i = 0; i < markerPositions.LabeledMarkerPositionCount(); i++)
{
const sMarker& markerData = markerPositions.GetLabeledMarker(i);
v[0] = markerData.x * unitConversion;
v[1] = markerData.y * unitConversion;
v[2] = markerData.z * unitConversion;
fRadius = markerData.size * unitConversion;
glPushMatrix();
glTranslatef(v[0],v[1],v[2]);
glColor4f(1.0f,0.0f,0.0f,1.0f);
OpenGLDrawingFunctions::DrawSphere(1,fRadius);
glPopMatrix();
}
glPopMatrix();
glFlush();
// Done rendering a frame. The NatNet callback function DataHandler
// will set render to true when it receives another frame of data.
render = false;
}
