void SweepAndPrune::AddObject(const Eigen::AlignedBox3f& box, void* obj) { auto maxvec = box.max(); auto minvec = box.min(); BBox toAdd = { 0 }; toAdd.userRef = obj; _objects.push_back(toAdd); unsigned int boxIdx = static_cast<unsigned int>(_objects.size() - 1); _objectMap[obj] = boxIdx; for (unsigned int i = 0; i < 3; ++i) { EndPoint min; EndPoint max; //make sure we are not screwing around with the high bit assert(boxIdx <= (std::numeric_limits<unsigned int>::max() & ~EndPoint::max)); min.box = boxIdx; max.box = boxIdx | EndPoint::max; min.value = std::numeric_limits<float>::max(); max.value = std::numeric_limits<float>::max(); //this stuff is slow as balls _axis[i]->insert(_axis[i]->end() - 1, min); _objects[boxIdx].min[i] = static_cast<unsigned int>(_axis[i]->size()) - 2; updateAxis(i, static_cast<int>(_axis[i]->size()) - 2, minvec(i)); _axis[i]->insert(_axis[i]->end() - 1, max); _objects[boxIdx].max[i] = static_cast<unsigned int>(_axis[i]->size()) - 2; updateAxis(i, static_cast<int>(_axis[i]->size()) - 2, maxvec(i)); } }
void SweepAndPrune::UpdateObject(const Eigen::AlignedBox3f& aabb, void* obj) { auto min = aabb.min(); auto max = aabb.max(); auto& box = _objects[_objectMap.at(obj)]; for (unsigned int i = 0; i < 3; ++i) { updateAxis(i, box.min[i], min(i)); updateAxis(i, box.max[i], max(i)); } }
//---------------------------------------- void ofNode::createMatrix() { localTransformMatrix = glm::translate(glm::mat4(1.0), toGlm(position)); localTransformMatrix = localTransformMatrix * glm::toMat4((const glm::quat&)orientation); localTransformMatrix = glm::scale(localTransformMatrix, toGlm(scale)); updateAxis(); }
void RigidAlignment::saveSphere(const char *dir) { for (int i = 0; i < m_nSubj; i++) { const float *axis = &faxis[i * 3]; float axis2[3]; updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2); Vector ax(axis), ax2(axis2); float inner = ax * ax2; if (inner > 1) inner = 1; else if (inner < -1) inner = -1; float deg = acos(inner); Mesh *sphere = new Mesh(); sphere->openFile(m_spherename); // matrix Vector ax3 = ax.cross(ax2); ax3.unit(); if (ax3.norm() != 0) sphere->rotation(ax3.fv(), deg); sphere->rotation(axis2, m_rot[i * 3]); // output char filename[1024]; sprintf(filename, "%s/%s.vtk", dir, m_filename[i]); sphere->saveFile(filename, "vtk"); delete sphere; } }
// Update the scale after there is a zoom in or out void SimpleChart::updateScale() { if (scale <= 9) pixelsByGroup = 1; else if (scale == 10) pixelsByGroup = 10; pixelsByUnit = AXIS_SIZE / ((int)scale * 2); updateAxis(); }
//---------------------------------------- void ofNode::createMatrix() { //if(isMatrixDirty) { // isMatrixDirty = false; localTransformMatrix.makeScaleMatrix(scale); localTransformMatrix.rotate(orientation); localTransformMatrix.setTranslation(position); updateAxis(); }
//---------------------------------------- void ofNode::setTransformMatrix(const ofMatrix4x4 &m44) { localTransformMatrix = m44; ofQuaternion so; localTransformMatrix.decompose(position, orientation, scale, so); updateAxis(); onPositionChanged(); onOrientationChanged(); onScaleChanged(); }
void ComparativeBarChart::update() { updateSizes(); osg::Vec3 scale(_width,1.0,_height); osg::Matrix scaleMat; scaleMat.makeScale(scale); _bgScaleMT->setMatrix(scaleMat); updateAxis(); updateGraph(); }
void UpdateAxisActionJob::run() { // Note: we assume axis/action are not really shared: // there's no benefit in sharing those when it comes to computing LogicalDevice *device = m_handler->logicalDeviceManager()->data(m_handle); if (!device->isEnabled()) return; updateAction(device); updateAxis(device); }
//---------------------------------------- void ofNode::setTransformMatrix(const ofMatrix4x4 &m44) { localTransformMatrix = m44; ofVec3f position; ofQuaternion orientation; ofVec3f scale; ofQuaternion so; localTransformMatrix.decompose(position, orientation, scale, so); this->position = position; this->orientation = orientation; this->scale = scale; updateAxis(); onPositionChanged(); onOrientationChanged(); onScaleChanged(); }
//---------------------------------------- void ofNode::setTransformMatrix(const glm::mat4 &m44) { localTransformMatrix = m44; glm::vec3 scale; glm::quat orientation; glm::vec3 translation; glm::vec3 skew; glm::vec4 perspective; glm::decompose(m44, scale, orientation, translation, skew, perspective); this->position = translation; this->scale = scale; this->orientation = orientation; updateAxis(); onPositionChanged(); onOrientationChanged(); onScaleChanged(); }
void DiagnosticsDisplay::onInitialize() { static int counter = 0; scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); orbit_node_ = scene_node_->createChildSceneNode(); // ?? line_ = new rviz::BillboardLine(context_->getSceneManager(), scene_node_); msg_ = new rviz::MovableText("not initialized", "Liberation Sans", 0.05); msg_->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_ABOVE); frame_id_property_->setFrameManager(context_->getFrameManager()); orbit_node_->attachObject(msg_); msg_->setVisible(false); orbit_theta_ = M_PI * 2.0 / 6 * counter++; updateLineWidth(); updateAxis(); updateDiagnosticsNamespace(); updateRadius(); updateRosTopic(); updateFontSize(); }
DiagnosticsDisplay::DiagnosticsDisplay() : rviz::Display(), msg_(0) { ros_topic_property_ = new rviz::RosTopicProperty( "Topic", "/diagnostics_agg", ros::message_traits::datatype<diagnostic_msgs::DiagnosticArray>(), "diagnostic_msgs::DiagnosticArray topic to subscribe to.", this, SLOT( updateRosTopic() )); frame_id_property_ = new rviz::TfFrameProperty( "frame_id", rviz::TfFrameProperty::FIXED_FRAME_STRING, "the parent frame_id to visualize diagnostics", this, 0, true); diagnostics_namespace_property_ = new rviz::EditableEnumProperty( "diagnostics namespace", "/", "diagnostics namespace to visualize diagnostics", this, SLOT(updateDiagnosticsNamespace())); radius_property_ = new rviz::FloatProperty( "radius", 1.0, "radius of diagnostics circle", this, SLOT(updateRadius())); line_width_property_ = new rviz::FloatProperty( "line width", 0.03, "line width", this, SLOT(updateLineWidth())); axis_property_ = new rviz::EnumProperty( "axis", "x", "axis", this, SLOT(updateAxis())); axis_property_->addOption("x", 0); axis_property_->addOption("y", 1); axis_property_->addOption("z", 2); font_size_property_ = new rviz::FloatProperty( "font size", 0.05, "font size", this, SLOT(updateFontSize())); }
// Update the filter based on most recent values // dT is in milliseconds // g* is gyro rotation rate in degrees/s // a* is current angle in degrees // heading is magnetometer heading in degrees void IMU::update(int dT, float gx, float gy, float gz, float ax, float ay, float az, float heading){ updateAxis(ROLL, dT, gx, ax); updateAxis(PITCH, dT, gy, ay); updateAxis(YAW, dT, gz, heading); }
void RigidAlignment::update(void) { // new point for (int i = 0; i < m_nSubj; i++) { const float *axis = &faxis[i * 3]; // new axis float axis2[3]; updateAxis(m_rot[i * 3 + 1], m_rot[i * 3 + 2], axis, axis2); Vector ax(axis), ax2(axis2); float inner = ax * ax2; if (inner > 1) inner = 1; else if (inner < -1) inner = -1; float deg = acos(inner); // matrix float mat[9]; Vector ax3 = ax.cross(ax2); ax3.unit(); if (ax3.norm() != 0) { Coordinate::rotation(ax3.fv(), deg, mat); // axis rotation for (int j = 0; j < m_nLM; j++) { float newp[3]; float *p = &fpoint[(i * m_nLM + j) * 3]; int id = m_point[i][j]; memcpy(p, m_sphere->vertex(id)->fv(), sizeof(float) * 3); Coordinate::rotPoint(p, mat, newp); memcpy(p, newp, sizeof(float) * 3); } } // matrix Coordinate::rotation(axis2, m_rot[i * 3], mat); // rotation for (int j = 0; j < m_nLM; j++) { float newp[3]; float *p = &fpoint[(i * m_nLM + j) * 3]; Coordinate::rotPoint(p, mat, newp); memcpy(p, newp, sizeof(float) * 3); } } // mean memset(fmean, 0, sizeof(float) * m_nLM * 3); for (int i = 0; i < m_nLM; i++) for (int j = 0; j < m_nSubj; j++) for (int k = 0; k < 3; k++) fmean[i * 3 + k] += fpoint[(j * m_nLM + i) * 3 + k] / m_nSubj; for (int i = 0; i < m_nLM; i++) { float norm = fmean[i * 3] * fmean[i * 3] + fmean[i * 3 + 1] * fmean[i * 3 + 1] + fmean[i * 3 + 2] * fmean[i * 3 + 2]; norm = sqrt(norm); for (int j = 0; j < 3; j++) fmean[i * 3 + j] /= norm; } /*for (int i = 0; i < m_nLM; i++) cout << fmean[i * 3] << " " << fmean[i * 3 + 1] << " " << fmean[i * 3 + 2] << endl;*/ }