C++ Coord::norm方法代码示例

void ClosestPointRegistrationForceFieldCam<DataTypes, DepthTypes>::addSpringForce(double& potentialEnergy, VecDeriv& f,const  VecCoord& p,const VecDeriv& v,int i, const Spring& spring)
{
    int a = spring.m1;
    Coord u = this->closestPos[i]-p[a];
    Real d = u.norm();
    if( d>1.0e-4 )
    {
        Real inverseLength = 1.0f/d;
        u *= inverseLength;
        Real elongation = (Real)d;
        potentialEnergy += elongation * elongation * spring.ks / 2;
        /*          serr<<"addSpringForce, p = "<<p<<sendl;
        serr<<"addSpringForce, new potential energy = "<<potentialEnergy<<sendl;*/
        Deriv relativeVelocity = -v[a];
        Real elongationVelocity = dot(u,relativeVelocity);
        Real forceIntensity;
        if(theCloserTheStiffer.getValue())
        {
            Real ks_max=spring.ks;
            Real ks_min=spring.ks/10;
            Real ks_mod = ks_min*(max-elongation)/(max-min)+ks_max*(elongation-min)/(max-min);
            forceIntensity = (Real)(ks_mod*elongation+spring.kd*elongationVelocity);
        }
        else forceIntensity = (Real)(spring.ks*elongation+spring.kd*elongationVelocity);
        Deriv force = u*forceIntensity;
        f[a]+=force;
        Mat& m = this->dfdx[i];
        Real tgt = forceIntensity * inverseLength;
        for( int j=0; j<N; ++j )
        {
            // anisotropic
            //for( int k=0; k<N; ++k ) m[j][k] = tgt * u[j] * u[k];

            // isotropic
            for( int k=0; k<N; ++k ) m[j][k] = ((Real)spring.ks-tgt) * u[j] * u[k];
            m[j][j] += tgt;
        }
    }
    else // null length, no force and no stiffness
    {
        Mat& m = this->dfdx[i];
        for( int j=0; j<N; ++j )
        {
            for( int k=0; k<N; ++k )
            {
                m[j][k] = 0;
            }
        }
    }
}

const std::vector<tlp::Coord> &Glyph::getGlyphNormals() {
  if (_normals.empty() && !glyph2D()) {
    _normals.resize(_vertices.size(), Coord(0,0,0));
    for (size_t i = 0 ; i < _indices.size() ; i += 3) {
      Coord v1 = _vertices[_indices[i]], v2 = _vertices[_indices[i+1]], v3 = _vertices[_indices[i+2]];
      Coord normal = (v2 - v1) ^ (v3 - v1);
      //            if (v1[2] <= 0 && v2[2] <= 0 && v3[2] <= 0)
      //              normal = (v3 - v1) ^ (v2 - v1);
      if (normal.norm() != 0) {
        normal /= normal.norm();
      }
      _normals[_indices[i]] += normal;
      _normals[_indices[i+1]] += normal;
      _normals[_indices[i+2]] += normal;
    }
    for (size_t i = 0 ; i < _normals.size() ; ++i) {
      if (_normals[i].norm() != 0) {
        _normals[i] /= _normals[i].norm();
      }
    }
  }
  return _normals;
}

tlp::Coord ConeGlyph::getAnchor(const tlp::Coord &vector) const {
  Coord anchor = vector;

  float x, y, z, n;
  anchor.get(x, y, z);
  n = sqrt(x * x + y * y);
  float vx0, vy0, vx1, vy1, x0, y0, x1, y1, px, py;
  x0 = 0;
  y0 = 0.5;
  vx0 = 0.5;
  vy0 = -1.0;
  x1 = 0;
  y1 = 0;
  vx1 = sqrt(x * x + y * y);
  vy1 = z;
  py = -1.0 * (vy1 * (vx0 / vy0 * y0 + x0 - x1) - vx1 * y1) / (vx1 - vy1 / vy0 * vx0);
  px = vx0 * (py - y0) / vy0 + x0;

  if (fabsf(py) > 0.5) {
    n = anchor.norm();
    py = n * 0.5 / fabsf(z);
    anchor.setX(x * py / n);
    anchor.setY(y * py / n);
    anchor.setZ(z * py / n);
  }
  else {
    anchor.setX(x * px / n);
    anchor.setY(y * px / n);
    anchor.setZ(py);
  }

  if (_eeGlyph) {
    swap(anchor[1], anchor[2]);
  }

  return anchor;
}

//========================================================================================
void MouseSelectionEditor::mMouseRotate(double newX, double newY, GlMainWidget *glMainWidget) {
  //  qWarning() << __PRETTY_FUNCTION__ << endl;
  if (operation == ROTATE_Z) {
    Coord curPos(newX, newY, 0);

    Coord vCS = glMainWidget->screenToViewport(editPosition) - editCenter;
    vCS /= vCS.norm();
    Coord vCP = glMainWidget->screenToViewport(curPos) - editCenter;
    vCP /= vCP.norm();

    float sign = (vCS ^ vCP)[2];
    sign /= fabs(sign);
    double cosalpha = vCS.dotProduct(vCP);
    double deltaAngle = sign * acos(cosalpha);

    Observable::holdObservers();

    initProxies(glMainWidget);
    _graph->pop();
    _graph->push();

    double degAngle = (deltaAngle * 180.0 / M_PI);

    // rotate layout
    if (mode == COORD_AND_SIZE || mode == COORD) {
      Coord center(editLayoutCenter);
      center *= -1.;
      Iterator<node> *itN = _selection->getNodesEqualTo(true, _graph);
      Iterator<edge> *itE = _selection->getEdgesEqualTo(true, _graph);
      _layout->translate(center, itN, itE);
      delete itN;
      delete itE;
      itN = _selection->getNodesEqualTo(true, _graph);
      itE = _selection->getEdgesEqualTo(true, _graph);
      _layout->rotateZ(-degAngle, itN, itE);
      delete itN;
      delete itE;
      center *= -1.;
      itN = _selection->getNodesEqualTo(true, _graph);
      itE = _selection->getEdgesEqualTo(true, _graph);
      _layout->translate(center, itN, itE);
      delete itN;
      delete itE;
    }

    if (mode == COORD_AND_SIZE || mode == SIZE) {
      for (auto n : _selection->getNodesEqualTo(true, _graph)) {
        double rotation = _rotation->getNodeValue(n);
        _rotation->setNodeValue(n, rotation - degAngle);
      }
    }

    Observable::unholdObservers();
  } else {
    double initDelta, delta, cosa;
    double xAngle = 0, yAngle = 0;
    double nbPI = 0;

    delta = abs(glMainWidget->screenToViewport(newX - editPosition[0]));

    if (delta > abs(glMainWidget->screenToViewport(newY - editPosition[1]))) {
      initDelta = abs(editCenter[0] - glMainWidget->screenToViewport(editPosition[0]));
      nbPI = floor(delta / (2. * initDelta));
      delta -= nbPI * 2. * initDelta;
      cosa = (initDelta - delta) / initDelta;

      yAngle = (acos(cosa) + (nbPI * M_PI)) * 180.0 / M_PI;
    } else {
      delta = abs(glMainWidget->screenToViewport(newY - editPosition[1]));
      initDelta = abs(editCenter[1] - glMainWidget->screenToViewport(editPosition[1]));
      nbPI = floor(delta / (2. * initDelta));
      delta -= nbPI * 2. * initDelta;
      cosa = (initDelta - delta) / initDelta;

      xAngle = (acos(cosa) + (nbPI * M_PI)) * 180.0 / M_PI;
    }

    Observable::holdObservers();

    initProxies(glMainWidget);
    _graph->pop();
    _graph->push();

    Coord center(editLayoutCenter);
    center *= -1.;
    Iterator<node> *itN = _selection->getNodesEqualTo(true, _graph);
    Iterator<edge> *itE = _selection->getEdgesEqualTo(true, _graph);
    _layout->translate(center, itN, itE);
    delete itN;
    delete itE;
    itN = _selection->getNodesEqualTo(true, _graph);
    itE = _selection->getEdgesEqualTo(true, _graph);

    if (yAngle > xAngle)
      _layout->rotateY(yAngle, itN, itE);
    else
      _layout->rotateX(xAngle, itN, itE);

    delete itN;
//.........这里部分代码省略.........

void
vnode_impl::update_coords (Coord uc, float ud)
{
  //  warn << myID << " --- starting update -----\n";
  Coord v = uc;
  float rmt_err = uc.err ();

  float actual = ud;
  float expect = me_->coords ().distance_f (uc);

  if (actual >= 0 && actual < 1000000) { //ignore timeouts
    //track our prediction error
    update_error (actual, expect, rmt_err);
    // Work on a copy (including the updated error).
    Coord coords = me_->coords ();

    // force magnitude: > 0 --> stretched
    float grad = expect - actual;
    v.vector_sub (coords);

    float len = v.plane_norm ();
    while (len < 0.0001) {
      for (unsigned int i = 0; i < Coord::NCOORD; i++)
	v.coords[i] = (double)(random () % 400 - 200) / 1.0;
      //if (USING_HT) v.ht += fabs((double)(random () % 10 - 5) / 10.0);
      len = v.plane_norm ();
    }
    len = v.norm ();

    float unit = 1.0 / sqrtf (len);

    // scalar_mult(v, unit) is unit force vector
    // times grad gives the scaled force vector
    v.scalar_mult (unit*grad);

    //timestep is the scaled ratio of our prediction error
    // and the remote node's prediction error
    float pred_err = coords.err ();
    float timestep;
    if (pred_err > 0 && rmt_err > 0)
      timestep = 0.1 * (pred_err)/(pred_err + rmt_err);
    else if (pred_err > 0)
      timestep = 0.0;
    else
      timestep = 1.0;

    v.scalar_mult (timestep);
    //flip sign on height
    v.ht = -v.ht;

    coords.vector_add (v);

#ifdef VIVALDI_DEBUG
    char b[1024];
    snprintf (b, 1024, "coord hop: %f - %f = %f ; len=%f ts=%f (%f %f)\n",
	      expect, actual, grad, len, timestep, pred_err, rmt_err);
    warn << b;
    coords.print ("coords ");
    uc.print ("uc ");
    warn << "----------------\n";
#endif

    if (coords.ht <= 100) coords.ht = 100;
    me_->set_coords (coords);
  } else {
    char b[32];
    snprintf (b, 32, "%f", actual);
    trace << "COORD: ignored actual of " << b << "\n";
  }
}

//=================================================================
Coord Glyph::getAnchor(const Coord &vector) const {
  Coord anchor = vector;
  /* anchor must be on the surface of a sphere centered on nodecenter, radius is 0.5 */
  anchor *= 0.5f / anchor.norm();
  return anchor;
}

void GlMetaNodeRenderer::render(node n, float, Camera *camera) {

  bool viewMeta = _inputData->renderingParameters()
                      ->isDisplayMetaNodes(); // Checks if user wants to see metanode content
  bool viewMetaLabels =
      _inputData->renderingParameters()
          ->isViewMetaLabel(); // Checks if user wants to see metanode content labels

  if (!viewMeta && !viewMetaLabels) {
    return;
  }

  GLint renderMode;
  glGetIntegerv(GL_RENDER_MODE, &renderMode);

  if (renderMode == GL_SELECT)
    return;

  Graph *metaGraph = _inputData->getGraph()->getNodeMetaInfo(n);
  GlScene *scene = nullptr;

  if (_metaGraphToSceneMap.count(metaGraph) != 0) {
    scene = _metaGraphToSceneMap[metaGraph];
  } else {
    scene = createScene(metaGraph);
    assert(scene != nullptr);
    _metaGraphToSceneMap[metaGraph] = scene;
    metaGraph->addListener(this);
  }

  scene->getGlGraphComposite()->setRenderingParameters(*(_inputData->renderingParameters()));
  int metaStencil = _inputData->renderingParameters()->getMetaNodesStencil();
  int metaSelectedStencil = _inputData->renderingParameters()->getSelectedMetaNodesStencil();
  int metaLabelStencil = _inputData->renderingParameters()->getMetaNodesLabelStencil();
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setDisplayNodes(viewMeta);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setDisplayEdges(viewMeta);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setViewEdgeLabel(viewMetaLabels);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setViewNodeLabel(viewMetaLabels);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setNodesStencil(metaStencil);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setEdgesStencil(metaStencil);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setSelectedNodesStencil(
      metaSelectedStencil);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setSelectedEdgesStencil(
      metaSelectedStencil);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setNodesLabelStencil(
      metaLabelStencil);
  scene->getGlGraphComposite()->getRenderingParametersPointer()->setEdgesLabelStencil(
      metaLabelStencil);

  GlNode glNode(n.id);

  BoundingBox includeBB;
  _inputData->glyphs.get(_inputData->getElementShape()->getNodeValue(n))
      ->getIncludeBoundingBox(includeBB, n);
  BoundingBox &&bbTmp = glNode.getBoundingBox(_inputData);
  BoundingBox bb(bbTmp.center() - Coord((bbTmp.width() / 2.f) * (includeBB[0][0] * -2.f),
                                        (bbTmp.height() / 2.f) * (includeBB[0][1] * -2.f),
                                        (bbTmp.depth() / 2.f) * (includeBB[0][2] * -2.f)),
                 bbTmp.center() + Coord((bbTmp.width() / 2.f) * (includeBB[1][0] * 2.f),
                                        (bbTmp.height() / 2.f) * (includeBB[1][1] * 2.f),
                                        (bbTmp.depth() / 2.f) * (includeBB[1][2] * 2.f)));

  Coord eyeDirection = camera->getEyes() - camera->getCenter();
  eyeDirection = eyeDirection / eyeDirection.norm();

  Camera newCamera2(*camera);
  newCamera2.setEyes(newCamera2.getCenter() +
                     Coord(0, 0, 1) * (newCamera2.getEyes() - newCamera2.getCenter()).norm());
  newCamera2.setUp(Coord(0, 1, 0));

  Coord center = camera->worldTo2DViewport((bb[0] + bb[1]) / 2.f);
  Coord &&first = newCamera2.worldTo2DViewport(bb[0]);
  Coord &&second = newCamera2.worldTo2DViewport(bb[1]);

  Coord &&size = second - first;

  Vector<int, 4> viewport;
  viewport[0] = center[0] - size[0] / 2;
  viewport[1] = center[1] - size[1] / 2;
  viewport[2] = size[0];
  viewport[3] = size[1];

  viewport[0] = camera->getViewport()[0] + viewport[0] - viewport[2] / 2;
  viewport[1] = camera->getViewport()[1] + viewport[1] - viewport[3] / 2;
  viewport[2] *= 2;
  viewport[3] *= 2;

  if (viewport[2] == 0 || viewport[3] == 0)
    return;

  scene->setViewport(viewport[0], viewport[1], viewport[2], viewport[3]);
  scene->setClearBufferAtDraw(false);
  scene->setClearDepthBufferAtDraw(false);
  scene->setClearStencilBufferAtDraw(false);
  scene->centerScene();

  float baseNorm = (scene->getGraphLayer()->getCamera().getEyes() -
                    scene->getGraphLayer()->getCamera().getCenter())
                       .norm();
  Camera newCamera = scene->getGraphLayer()->getCamera();
//.........这里部分代码省略.........