void Tracker::addMeasurementsToStream(autocal::SensorPlant& plant, std::string stream_name, autocal::TimeStamp t) {
for (int i = 0; i < m_count; i++) {
GLfloat* m = psmove_fusion_get_modelview_matrix(m_fusion, m_moves[i]);
Transform3D pose;
// units = decimeters
// HACK O.5 FOR HALF DISTANCE
pose << m[0] << m[4] << m[8] << unit_factor * m[12] * 0.5 << arma::endr << m[1] << m[5] << m[9]
<< unit_factor * m[13] * 0.5 << arma::endr << m[2] << m[6] << m[10] << unit_factor * m[14] * 0.5
<< arma::endr << m[3] << m[7] << m[11] << m[15] << arma::endr;
plant.mocapRecording.addMeasurement(stream_name, t, i, pose);
}
}
void Tracker::savePoses() {
std::vector<Transform3D> poses;
for (int i = 0; i < m_count; i++) {
GLfloat* m = psmove_fusion_get_modelview_matrix(m_fusion, m_moves[i]);
Transform3D pose;
// units = decimeters
pose << m[0] << m[4] << m[8] << unit_factor * m[12] << arma::endr << m[1] << m[5] << m[9] << unit_factor * m[13]
<< arma::endr << m[2] << m[6] << m[10] << unit_factor * m[14] << arma::endr << m[3] << m[7] << m[11]
<< m[15] << arma::endr;
// TODO:
poses.push_back(pose);
// std::cout << pose << std::endl;
}
mocapRecorder.saveFrame(poses);
}
void
Tracker::render()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
PSMoveTrackerRGBImage image = psmove_tracker_get_image(m_tracker);
glEnable(GL_TEXTURE_2D);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, image.width, image.height,
0, GL_RGB, GL_UNSIGNED_BYTE, image.data);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
/* Draw the camera image, filling the screen */
glColor3f(1., 1., 1.);
glBegin(GL_QUADS);
glTexCoord2f(0., 1.);
glVertex2f(-1., -1.);
glTexCoord2f(1., 1.);
glVertex2f(1., -1.);
glTexCoord2f(1., 0.);
glVertex2f(1., 1.);
glTexCoord2f(0., 0.);
glVertex2f(-1., 1.);
glEnd();
glDisable(GL_TEXTURE_2D);
/* Clear the depth buffer to allow overdraw */
glClear(GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadMatrixf(psmove_fusion_get_projection_matrix(m_fusion));
/* Render the trace */
if (m_trace.size()) {
Point3D center = m_trace[0];
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glTranslatef(center.x + m_offset.x, center.y + m_offset.y, center.z + m_offset.z);
glRotatef(m_rotation, 0., 1., 0.);
std::vector<Point3D>::iterator it;
glColor3f(1., 0., 0.);
glEnable(GL_LIGHTING);
//glBegin(GL_TRIANGLE_STRIP);
for (it=m_trace.begin(); it != m_trace.end(); ++it) {
Point3D point = *it;
Point3D moved(point.x - center.x,
point.y - center.y,
point.z - center.z);
//glVertex3f(moved.x, moved.y, moved.z);
glPushMatrix();
glTranslatef(moved.x, moved.y, moved.z);
glutSolidCube(.5);
glPopMatrix();
}
//glEnd();
glDisable(GL_LIGHTING);
}
for (int i=0; i<m_count; i++) {
glMatrixMode(GL_MODELVIEW);
glLoadMatrixf(psmove_fusion_get_modelview_matrix(m_fusion, m_moves[i]));
if (m_items[i] == WIRE_CUBE) {
glColor3f(1., 0., 0.);
glutWireCube(1.);
glColor3f(0., 1., 0.);
glPushMatrix();
glScalef(1., 1., 4.5);
glTranslatef(0., 0., -.5);
glutWireCube(1.);
glPopMatrix();
glColor3f(0., 0., 1.);
glutWireCube(3.);
} else if (m_items[i] == SOLID_CUBE) {
glEnable(GL_LIGHTING);
glutSolidCube(2.);
glDisable(GL_LIGHTING);
} else if (m_items[i] == SOLID_TEAPOT) {
glEnable(GL_LIGHTING);
glPushMatrix();
glRotatef(90., 1., 0., 0.);
glutSolidTeapot(1.);
glPopMatrix();
glDisable(GL_LIGHTING);
}
}
}
