C++ Transform3f::translation方法代码示例

//.........这里部分代码省略.........
    std::swap(hip_rx_angles[0], hip_rx_angles[1]);
    std::swap(badness[0], badness[1]);  
    std::swap(flags[0], flags[1]);
  }
  
  int hip_solution_cnt = 2;

  if (badness[0] == 0 && badness[1] != 0) {
    hip_solution_cnt = 1;
  } 

  debug << "hip_rx_angles[0]=" << hip_rx_angles[0] 
        << ", badness=" << badness[0]
        << ", flags=" << flags[0] << "\n";

  debug << "hip_rx_angles[1]=" << hip_rx_angles[1] 
        << ", badness=" << badness[1]
        << ", flags=" << flags[1] << "\n";
  
  debug << "hip_solution_cnt = " << hip_solution_cnt << "\n";

  vec3f qfwd[2], qrear[2];
  
  for (int i=0; i<hip_solution_cnt; ++i) {

    debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n";

    float hip_rx = hip_rx_angles[i];
    
    // now make inv. transform to get rid of hip rotation
    Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
    vec3f ptx = tx.transformInv(orig);

    debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n";
    
    // calculate lengths for cosine law
    float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
    float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK);
    float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
    float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK);
    
    float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2];
    float k = sqrt(ksqr);

    debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n";
    
    // check triangle inequality
    if (k > l1 + l2) { 
      debug << "oops, violated the triangle inequality for lower segments: "
            << "k = " << k << ", "
            << "l1 + l2 = " << l1 + l2 << "\n";
      if (k - (l1 + l2) > 1e-4) {
        flags[i] = flags[i] | IK_LOWER_DISTANCE;
      }
      k = l1 + l2;
      ksqr = k * k;
    }
    
    // 2*theta is the acute angle formed by the spread
    // of the two hip rotations... 
    float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k);
    if (fabs(costheta) > 1) {
      debug << "costheta = " << costheta << " > 1\n";
      if (fabs(costheta) - 1 > 1e-4) {
        flags[i] = flags[i] | IK_LOWER_DISTANCE;
      }

/** \reimpl
  */
void
FeatureLabelSetGeometry::render(RenderContext& rc, double /* clock */) const
{
    const float visibleSizeThreshold = 20.0f; // in pixels

    // No need to draw anything if the labels are turned off with an opacity
    // setting near 0.
    if (ms_globalOpacity <= 0.01f)
    {
        return;
    }

    // Render during the opaque pass if opaque or during the translucent pass if not.
    if (rc.pass() == RenderContext::TranslucentPass)
    {
        // Get the position of the camera in the body-fixed frame of the labeled object
        Transform3f inv = Transform3f(rc.modelview().inverse(Affine)); // Assuming an affine modelview matrix
        Vector3f cameraPosition = inv.translation();
        float overallPixelSize = boundingSphereRadius() / (rc.pixelSize() * cameraPosition.norm());

        // Only draw individual labels if the overall projected size of the set exceeds the threshold
        if (overallPixelSize > visibleSizeThreshold)
        {
            // Labels are treated as either completely visible or completely occluded. A label is
            // visible when the labeled point isn't blocked by the occluding ellipsoid.
            AlignedEllipsoid testEllipsoid(m_occludingEllipsoid.semiAxes() * 0.999);
            Vector3f ellipsoidSemiAxes = testEllipsoid.semiAxes().cast<float>();

            Vector3f viewDir = -cameraPosition.normalized();
            double distanceToEllipsoid = 0.0;

            // Instead of computing the ellipsoid intersection (as the line below), just treat the planet as a sphere
            //TestRayEllipsoidIntersection(cameraPosition, viewDir, ellipsoidSemiAxes, &distanceToEllipsoid);
            distanceToEllipsoid = (cameraPosition.norm() - ellipsoidSemiAxes.maxCoeff()) * 0.99f;

            // We don't want labels partially hidden by the planet ellipsoid, so we'll project them onto a
            // plane that lies just in front of the planet ellipsoid and which is parallel to the view plane
            Hyperplane<float, 3> labelPlane(viewDir, cameraPosition + viewDir * float(distanceToEllipsoid));

            for (vector<Feature, Eigen::aligned_allocator<Feature> >::const_iterator iter = m_features.begin(); iter != m_features.end(); ++iter)
            {
                Vector3f r = iter->position - cameraPosition;

                Vector3f labelPosition = labelPlane.projection(iter->position);
                float k = -(labelPlane.normal().dot(cameraPosition) + labelPlane.offset()) / (labelPlane.normal().dot(r));
                labelPosition = cameraPosition + k * r;

                rc.pushModelView();
                rc.translateModelView(labelPosition);
                float featureDistance = rc.modelview().translation().norm();
                float pixelSize = iter->size / (rc.pixelSize() * featureDistance);

                float d = r.norm();
                r /= d;
                double t = 0.0;
                TestRayEllipsoidIntersection(cameraPosition, r, ellipsoidSemiAxes, &t);

                if (pixelSize > visibleSizeThreshold && d < t)
                {
                    rc.drawEncodedText(Vector3f::Zero(), iter->label, m_font.ptr(), TextureFont::Utf8, iter->color, ms_globalOpacity);
                }

                rc.popModelView();
            }
        }
    }
}