static void infoData (pScene sc, pMesh mesh, int k, int typel) {
pSolution ps;
if (!mesh->nbb) return;
ps = &mesh->sol[k];
if (mesh->nfield == 1) {
fprintf(stdout, " Data (scalar): %f\n", ps->bb);
} else if (mesh->nfield == mesh->dim) {
fprintf(stdout, " Data (vector): %f %f", ps->m[0], ps->m[1]);
if (mesh->dim == 3) fprintf(stdout, " %f", ps->m[2]);
fprintf(stdout, "\n");
} else if (mesh->dim == 2 && mesh->nfield == 3) {
fprintf(stdout, " Data (tensor): %f %f %f\n",
ps->m[0], ps->m[1], ps->m[2]);
drawEllipse(sc, mesh, typel, k);
} else if (mesh->dim == 3 && mesh->nfield == 6) {
if (mesh->ne)
fprintf(stdout, " Data (tensor): %f %f %f %f %f %f\n",
ps->m[0], ps->m[1], ps->m[2], ps->m[3], ps->m[4], ps->m[5]);
drawEllipsoid(sc, mesh, typel, k);
}
fflush(stdout); /* add J. Morice 12/2008 */
}
void AppStage_MagnetometerCalibration::render()
{
const float modelScale = 18.f;
glm::mat4 scaleAndRotateModelX90=
glm::rotate(
glm::scale(glm::mat4(1.f), glm::vec3(modelScale, modelScale, modelScale)),
90.f, glm::vec3(1.f, 0.f, 0.f));
PSMoveIntVector3 rawSampleExtents = (m_maxSampleExtent - m_minSampleExtent).unsafe_divide(2);
glm::vec3 boxMin = psmove_float_vector3_to_glm_vec3(m_minSampleExtent.castToFloatVector3());
glm::vec3 boxMax = psmove_float_vector3_to_glm_vec3(m_maxSampleExtent.castToFloatVector3());
glm::vec3 boxCenter = (boxMax + boxMin) * 0.5f;
glm::vec3 boxExtents = (boxMax - boxMin) * 0.5f;
glm::mat4 recenterMatrix =
glm::translate(glm::mat4(1.f), -eigen_vector3f_to_glm_vec3(m_sampleFitEllipsoid.center));
switch (m_menuState)
{
case eCalibrationMenuState::waitingForStreamStartResponse:
{
} break;
case eCalibrationMenuState::failedStreamStart:
case eCalibrationMenuState::failedBadCalibration:
{
} break;
case eCalibrationMenuState::measureBExtents:
{
float r= clampf01(static_cast<float>(m_led_color_r) / 255.f);
float g= clampf01(static_cast<float>(m_led_color_g) / 255.f);
float basis= clampf01(static_cast<float>(m_led_color_b) / 255.f);
// Draw the psmove model in the middle
drawPSMoveModel(scaleAndRotateModelX90, glm::vec3(r, g, basis));
// Draw the sample point cloud around the origin
drawPointCloud(
recenterMatrix,
glm::vec3(1.f, 1.f, 1.f),
reinterpret_cast<float *>(&m_alignedSamples->magnetometerEigenSamples[0]),
m_sampleCount);
// Draw the sample bounding box
// Label the min and max corners with the min and max magnetometer readings
drawTransformedBox(recenterMatrix, boxMin, boxMax, glm::vec3(1.f, 1.f, 1.f));
drawTextAtWorldPosition(recenterMatrix, boxMin, "%d,%d,%d",
m_minSampleExtent.i, m_minSampleExtent.j, m_minSampleExtent.k);
drawTextAtWorldPosition(recenterMatrix, boxMax, "%d,%d,%d",
m_maxSampleExtent.i, m_maxSampleExtent.j, m_maxSampleExtent.k);
// Draw and label the extent axes
drawTransformedAxes(glm::mat4(1.f), boxExtents.x, boxExtents.y, boxExtents.z);
drawTextAtWorldPosition(glm::mat4(1.f), glm::vec3(boxExtents.x, 0.f, 0.f), "%d", rawSampleExtents.i);
drawTextAtWorldPosition(glm::mat4(1.f), glm::vec3(0.f, boxExtents.y, 0.f), "%d", rawSampleExtents.j);
drawTextAtWorldPosition(glm::mat4(1.f), glm::vec3(0.f, 0.f, boxExtents.z), "%d", rawSampleExtents.k);
// Draw the best fit ellipsoid
{
glm::mat3 basis = eigen_matrix3f_to_glm_mat3(m_sampleFitEllipsoid.basis);
glm::vec3 center = eigen_vector3f_to_glm_vec3(m_sampleFitEllipsoid.center);
glm::vec3 extents = eigen_vector3f_to_glm_vec3(m_sampleFitEllipsoid.extents);
drawEllipsoid(
recenterMatrix,
glm::vec3(0.f, 0.4f, 1.f),
basis, center, extents);
drawTextAtWorldPosition(
recenterMatrix,
center - basis[0]*extents.x,
"E:%.1f", m_sampleFitEllipsoid.error);
}
// Draw the current magnetometer direction
{
glm::vec3 m_start= boxCenter;
glm::vec3 m_end= psmove_float_vector3_to_glm_vec3(m_lastMagnetometer.castToFloatVector3());
drawArrow(recenterMatrix, m_start, m_end, 0.1f, glm::vec3(1.f, 0.f, 0.f));
drawTextAtWorldPosition(recenterMatrix, m_end, "M");
}
} break;
case eCalibrationMenuState::waitForGravityAlignment:
{
drawPSMoveModel(scaleAndRotateModelX90, glm::vec3(1.f, 1.f, 1.f));
// Draw the current direction of gravity
{
const float renderScale = 200.f;
glm::mat4 renderScaleMatrix =
glm::scale(glm::mat4(1.f), glm::vec3(renderScale, renderScale, renderScale));
glm::vec3 g= psmove_float_vector3_to_glm_vec3(m_lastAccelerometer);
drawArrow(
renderScaleMatrix,
glm::vec3(), g,
0.1f,
glm::vec3(0.f, 1.f, 0.f));
drawTextAtWorldPosition(renderScaleMatrix, g, "G");
}
} break;
case eCalibrationMenuState::measureBDirection:
{
drawPSMoveModel(scaleAndRotateModelX90, glm::vec3(1.f, 1.f, 1.f));
// Draw the current magnetometer direction
{
glm::vec3 m_start = boxCenter;
glm::vec3 m_end = psmove_float_vector3_to_glm_vec3(m_lastMagnetometer.castToFloatVector3());
drawArrow(recenterMatrix, m_start, m_end, 0.1f, glm::vec3(1.f, 0.f, 0.f));
drawTextAtWorldPosition(recenterMatrix, m_end, "M");
}
} break;
case eCalibrationMenuState::waitForSetCalibrationResponse:
{
} break;
case eCalibrationMenuState::failedSetCalibration:
{
} break;
case eCalibrationMenuState::complete:
{
// Get the orientation of the controller in world space (OpenGL Coordinate System)
glm::quat q= psmove_quaternion_to_glm_quat(m_controllerView->GetPSMoveView().GetOrientation());
glm::mat4 worldSpaceOrientation= glm::mat4_cast(q);
glm::mat4 worldTransform = glm::scale(worldSpaceOrientation, glm::vec3(modelScale, modelScale, modelScale));
drawPSMoveModel(worldTransform, glm::vec3(1.f, 1.f, 1.f));
drawTransformedAxes(glm::mat4(1.f), 200.f);
} break;
case eCalibrationMenuState::pendingExit:
{
} break;
default:
assert(0 && "unreachable");
}
}
//FIXME: Refactor this to use polymorphism.
void OpenGLRenderInterface::draw(dynamics::Shape* _shape) {
if(_shape == 0)
return;
Eigen::Isometry3d pose = _shape->getLocalTransform();
Eigen::Vector3d color = _shape->getColor();
glPushMatrix();
glColorMaterial ( GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE );
glEnable ( GL_COLOR_MATERIAL );
glColor3d(color[0], color[1], color[2]);
glMultMatrixd(pose.data());
switch(_shape->getShapeType()) {
case dynamics::Shape::BOX: {
//FIXME: We are not in a glut instance
dynamics::BoxShape* box = static_cast<dynamics::BoxShape*>(_shape);
drawCube(box->getSize());
break;
}
case dynamics::Shape::CYLINDER: {
//FIXME: We are not in a glut instance
dynamics::CylinderShape* cylinder = static_cast<dynamics::CylinderShape*>(_shape);
drawCylinder(cylinder->getRadius(), cylinder->getHeight());
break;
}
case dynamics::Shape::ELLIPSOID: {
//FIXME: We are not in a glut instance
dynamics::EllipsoidShape* ellipsoid = static_cast<dynamics::EllipsoidShape*>(_shape);
drawEllipsoid(ellipsoid->getSize());
break;
}
case dynamics::Shape::PLANE: {
dterr << "PLANE shape is not supported yet." << std::endl;
break;
}
case dynamics::Shape::MESH: {
glDisable(GL_COLOR_MATERIAL); // Use mesh colors to draw
dynamics::MeshShape* mesh = static_cast<dynamics::MeshShape*>(_shape);
if(!mesh)
break;
else if(mesh->getDisplayList())
drawList(mesh->getDisplayList());
else
drawMesh(mesh->getScale(), mesh->getMesh());
break;
}
case dynamics::Shape::SOFT_MESH: {
// Do nothing
break;
}
case dynamics::Shape::LINE_SEGMENT: {
dynamics::LineSegmentShape* lineSegments =
static_cast<dynamics::LineSegmentShape*>(_shape);
drawLineSegments(lineSegments->getVertices(),
lineSegments->getConnections());
}
}
glDisable(GL_COLOR_MATERIAL);
glPopMatrix();
}
void display()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
gluLookAt(
0.0f, 0.0f, 6.0f,
0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f);
glRotatef(xrot, 1.0f, 0.0f, 0.0f);
glRotatef(yrot, 0.0f, 1.0f, 0.0f);
drawBox();
// glRotatef(120,0,0,1);
glTranslatef(0,0,Z0);
drawEllipsoid(A,B,C, 15,30);
glTranslatef(0,0,-Z0);
light();
static GLUquadricObj *q;
q = gluNewQuadric();
gluQuadricNormals (q,GLU_TRUE);
if(changingcolor==100) changingcolor=0;
changingcolor++;
float ecolor[] = { 0.1*(changingcolor/10), 0.0f,1-0.1*changingcolor/10, 0.1f };
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, ecolor);
glColor3f(0.2f, 0.1f, 0.6f);
float theta1;
float theta2;
glTranslated( x, y, z);
glBegin(GL_LINES);
glVertex3f(0,0,0);
//glVertex3f(x, y, z); // origin of the line
glVertex3f(fn[0]/10,fn[1]/10, fn[2]/10); // ending point of the line
glEnd( );
glBegin(GL_LINES);
glVertex3f(0,0,0);
//glVertex3f(x, y, z); // origin of the line
glVertex3f(ft[0]/10,ft[1]/10, ft[2]/10); // ending point of the line
glEnd( );
//if(fx>=0)*/ theta1=-(180/M_PI)*atan2(fz,fx);
/*else */theta1=180-(180/M_PI)*atan2(fz,fx);
theta2=(180/M_PI)*atan2(fy, sqrt(fx*fx+fz*fz));
glRotated( 90, 0, 1,0);
glRotated(theta1, 0,1 , 0);
glRotated(theta2 ,1, 0, 0);
//printf("theta1=%f\ntheta2=%f",(180/M_PI)*atan2(fz,fx),(180/M_PI)*atan2( fy,sqrt(fx*fx+fz*fz)));
if(Ftot>0.2) {
gluCylinder(q,0.01,0.01*Ftot,Ftot/10,30,20);
drawTorque(lt);
}
float mcolor[] = { 0.8f, 0.8f, 0.8f, 0.1f };
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, mcolor);
glFlush();
glutSwapBuffers();
//glutPostRedisplay();
//ros::spinOnce();
}
void Shapes::update()
{
Geometry::update();
for (unsigned int i=0; i<geom.size(); i++)
{
glNewList(displaylists[i], GL_COMPILE);
if (!drawable(i)) {glEndList(); continue;} ////
//Calibrate colour map (not yet available)
geom[i]->colourCalibrate();
//Set draw state
setState(i);
float scaling = geom[i]->draw->scaling;
//Load constant scaling factors from properties
float dims[3];
dims[0] = geom[i]->draw->props.Float("width", FLT_MIN);
dims[1] = geom[i]->draw->props.Float("height", FLT_MIN);
dims[2] = geom[i]->draw->props.Float("length", FLT_MIN);
int shape = geom[i]->draw->props.Int("shape");
if (scaling <= 0) scaling = 1.0;
for (int v=0; v < geom[i]->count; v++)
{
//Calculate colour
geom[i]->setColour(v);
//Scale the dimensions by variables (dynamic range options? by setting max/min?)
float sdims[3] = {dims[0], dims[1], dims[2]};
if (geom[i]->xWidths.size() > 0) sdims[0] = geom[i]->xWidths[v];
if (geom[i]->yHeights.size() > 0) sdims[1] = geom[i]->yHeights[v]; else sdims[1] = sdims[0];
if (geom[i]->zLengths.size() > 0) sdims[2] = geom[i]->zLengths[v]; else sdims[2] = sdims[1];
//Multiply by constant scaling factors if present
if (dims[0] != FLT_MIN) sdims[0] *= dims[0];
if (dims[1] != FLT_MIN) sdims[1] *= dims[1];
if (dims[2] != FLT_MIN) sdims[2] *= dims[2];
//Apply scaling
sdims[0] *= scaling * scale;
sdims[1] *= scaling * scale;
sdims[2] *= scaling * scale;
float pos[3];
memcpy(pos, geom[i]->vertices[v], 3 * sizeof(float));
//Scale manually (as global scaling is disabled to avoid distorting glyphs)
for (int d=0; d<3; d++)
pos[d] *= view->scale[d];
// Translate to centre position
glPushMatrix();
glTranslatef(pos[0], pos[1], pos[2]);
//Orient view to the alignment vector
if (geom[i]->vectors.size() > 0)
{
Vec3d vec(geom[i]->vectors[v]);
vec *= Vec3d(view->scale); //Scale
// Rotate to orient the cone
//...Want to align our z-axis to point along arrow vector:
// axis of rotation = (z x vec)
// cosine of angle between vector and z-axis = (z . vec) / |z|.|vec| */
vec.normalise();
float rangle = RAD2DEG * vec.angle(Vec3d(0.0, 0.0, 1.0));
//Axis of rotation = vec x [0,0,1] = -vec[1],vec[0],0
glRotatef(rangle, -vec[1], vec[0], 0);
}
//Draw shape
float zpos[3] = {0};
if (shape == 1)
//drawCuboid(min, max, true);
drawCuboid(zpos, sdims[0], sdims[1], sdims[2], true);
else
drawEllipsoid(zpos, sdims[0], sdims[1], sdims[2], 24, NULL);
//Restore model view
glPopMatrix();
}
glEndList();
}
}
