geometry_msgs::Pose FootstepMarker::computeLegTransformation(uint8_t leg) { geometry_msgs::Pose new_pose; jsk_pcl_ros::CallSnapIt srv; srv.request.request.header.stamp = ros::Time::now(); srv.request.request.header.frame_id = marker_frame_id_; srv.request.request.target_plane.header.stamp = ros::Time::now(); srv.request.request.target_plane.header.frame_id = marker_frame_id_; srv.request.request.target_plane.polygon = computePolygon(leg); if (snapit_client_.call(srv)) { Eigen::Affine3d A, T, B, B_prime; tf::poseMsgToEigen(srv.response.transformation, T); tf::poseMsgToEigen(marker_pose_.pose, A); if (leg == jsk_footstep_msgs::Footstep::LEFT) { tf::poseMsgToEigen(lleg_pose_, B); } else if (leg == jsk_footstep_msgs::Footstep::RIGHT) { tf::poseMsgToEigen(rleg_pose_, B); } B_prime = A.inverse() * T * A * B; tf::poseEigenToMsg(B_prime, new_pose); } else { // throw exception ROS_ERROR("failed to call snapit"); } return new_pose; }
void ASprite::render() { m_program->bind(); shaderAction(); glDepthMask(GL_FALSE); computeMatrix(); computePolygon(); GLuint * vboId = m_geometric.getVboId(); glBindBuffer(GL_ARRAY_BUFFER,vboId[0]); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,vboId[1]); quintptr offset = 0; glBindTexture(GL_TEXTURE_2D, m_texId); // Tell OpenGL programmable pipeline how to locate vertex position data int vertexLocation = m_program->attributeLocation("a_position"); m_program->enableAttributeArray(vertexLocation); glVertexAttribPointer(vertexLocation,3, GL_FLOAT, GL_FALSE, sizeof(VertexData), (const void *)offset); offset += sizeof(AVector3D); // Tell OpenGL programmable pipeline how to locate vertex texture coordinate data int texcoordLocation = m_program->attributeLocation("a_texcoord"); m_program->enableAttributeArray(texcoordLocation); glVertexAttribPointer(texcoordLocation, 2, GL_FLOAT, GL_FALSE, sizeof(VertexData), (const void *)offset); m_program->setUniformValue("source",0); m_program->setUniformValue("age_Opacity",m_alpha); m_program->setUniformValue("red",m_color.red); m_program->setUniformValue("green",m_color.green); m_program->setUniformValue("blue",m_color.blue); glDrawElements(GL_TRIANGLE_FAN, 4, GL_UNSIGNED_SHORT, 0); glDepthMask(GL_TRUE); glBindBuffer(GL_ARRAY_BUFFER,0); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER,0); }
GlRegularPolygon::GlRegularPolygon(const Coord &position, const Size &size, unsigned int numberOfSides, const Color &fillColor, const Color &outlineColor, bool filled, bool outlined, const string &textureName, float outlineSize): position(position), size(size), numberOfSides(numberOfSides), startAngle(float(M_PI)/2.0f) { computePolygon(); invertYTexture=false; setFillColor(fillColor); setOutlineColor(outlineColor); setFillMode(filled); setOutlineMode(outlined); setTextureName(textureName); setOutlineSize(outlineSize); }
//===================================================== void GlRegularPolygon::setNumberOfSides(unsigned int number) { numberOfSides=number; computePolygon(); }
//===================================================== void GlRegularPolygon::setStartAngle(float angle) { startAngle=angle; computePolygon(); }