示例#1
0
CMUK_ERROR_CODE cmuk::computeFootIK( LegIndex leg,
                                     const vec3f& pos,
                                     vec3f* q_bent_forward,
                                     vec3f* q_bent_rearward ) const {

  if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
    return CMUK_BAD_LEG_INDEX;
  } else if (!q_bent_forward || !q_bent_rearward) {
    return CMUK_INSUFFICIENT_ARGUMENTS;
  }

  debug << "*** computing IK...\n";

  int hipflags = 0;

  // subtract off hip position
  vec3f p = pos - jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK); 
  vec3f orig = pos;

  // get dist from hip rx joint to y rotation plane
  const float& d = jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK)[1]; 

  // get the squared length of the distance on the plane
  float yz = p[1]*p[1] + p[2]*p[2];

  // alpha is the angle of the foot in the YZ plane with respect to the Y axis
  float alpha = atan2(p[2], p[1]);

  // h is the distance of foot from hip in YZ plane
  float h = sqrt(yz);

  // beta is the angle between the foot-hip vector (projected in YZ
  // plane) and the top hip link.
  float cosbeta = d / h;

  debug << "p = " << p << ", d = " << d << ", yz = " << yz << "\nalpha = " << alpha << ", h = " << h << ", cosbeta=" << cosbeta << "\n";

  if (fabs(cosbeta) > 1) {
    debug << "violated triangle inequality when calculating hip_rx_angle!\n" ;
    if (fabs(cosbeta) - 1 > 1e-4) {
      hipflags = hipflags | IK_UPPER_DISTANCE;
    }
    cosbeta = (cosbeta < 0) ? -1 : 1;
    if (yz < 1e-4) {
      p[1] = d;
      p[2] = 0;
    } else {
      float scl = fabs(d) / h;
      p[1] *= scl;
      p[2] *= scl;
      orig = p + jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK);
    }
  }

  float beta = acos(cosbeta);

  // Now compute the two possible hip angles
  float hip_rx_angles[2], badness[2];
  int flags[2];

  flags[0] = hipflags;
  flags[1] = hipflags;

  hip_rx_angles[0] = fix_angle(alpha - beta, -M_PI, M_PI);
  hip_rx_angles[1] = fix_angle(alpha + beta, -M_PI, M_PI);

  const float& min = jl(_kc, leg, HIP_RX, 0);
  const float& max = jl(_kc, leg, HIP_RX, 1);

  // See how badly we violate the joint limits for this hip angles
  for (int i=0; i<2; ++i) {
    float& angle = hip_rx_angles[i];
    badness[i] = fabs(compute_badness(angle, min, max));
    if (badness[i]) { flags[i] = flags[i] | IK_UPPER_ANGLE_RANGE; }
  }
  
  // Put the least bad (and smallest) hip angle first
  bool swap = false;

  if ( badness[1] <= badness[0] ) {
    // We want the less bad solution for hip angle
    swap = true;
  } else if (badness[0] == 0 && badness[1] == 0) {
    // We want the solution for hip angle that leaves the hip up.
    if ((leg == FL || leg == HL) && hip_rx_angles[0] > hip_rx_angles[1]) {
      swap = true;
    } else if ((leg == FR || leg == HR) && hip_rx_angles[0] < hip_rx_angles[1]) {
      swap = true;
    }
  } 

  if (swap) {
    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;
      }
      costheta = (costheta < 0) ? -1 : 1;
    }
    float theta = acos(costheta);
    
    // gamma is the angle of the foot with respect to the z axis
    float gamma = atan2(-ptx[0], -ptx[2]);
    
    // hip angles are just offsets off of gamma now
    float hip_ry_1 = gamma - theta;
    float hip_ry_2 = gamma + theta;
    
    // phi is the obtuse angle of the parallelogram
    float cosphi = (l1sqr + l2sqr - ksqr) / (2 * l1 * l2);
    if (fabs(cosphi) > 1) {
      debug << "cosphi = " << cosphi << " > 1\n";
      if (fabs(cosphi) - 1 > 1e-4) {
        flags[i] = flags[i] | IK_LOWER_DISTANCE;
      }
      cosphi = (cosphi < 0) ? -1 : 1;
    }
    float phi = acos(cosphi);
    
    // epsilon is the "error" caused by not having feet offset directly
    // along the z-axis (if they were, epsilon would equal zero)
    float epsilon = le(_kc, leg, _centeredFootIK);
    
    // now we can directly solve for knee angles
    float knee_ry_1 =  M_PI - phi - epsilon;
    float knee_ry_2 =  -M_PI + phi - epsilon;

    // now fill out angle structs and check limits
    qfwd[i] = vec3f(hip_rx, hip_ry_1, knee_ry_1);
    qrear[i] = vec3f(hip_rx, hip_ry_2, knee_ry_2);
    
    debug << "before wrap, qfwd =  " << qfwd[i] << "\n";
    debug << "before wrap, qrear = " << qrear[i] << "\n";

    check_wrap(_kc, qfwd[i], leg);
    check_wrap(_kc, qrear[i], leg);

    debug << "after wrap, qfwd =  " << qfwd[i] << "\n";
    debug << "after wrap, qrear = " << qrear[i] << "\n";
    
    if (!check_limits(_kc, qfwd[i], leg)) {
      debug << "violated limits forward!\n";
      flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_FWD;
    }
    if (!check_limits(_kc, qrear[i], leg)) {
      debug << "violated limits rearward!\n";
      flags[i] = flags[i] | IK_LOWER_ANGLE_RANGE_REAR;
    }
    
  } // for each viable hip solution

  int best = 0;

  if (hip_solution_cnt == 2) {
    if (howbad(flags[0]) > howbad(flags[1]))  {
      best = 1;
    }
    debug << "best overall solution is " << (best+1) << "\n";
  }


  *q_bent_forward = qfwd[best];
  *q_bent_rearward = qrear[best];
  return flags_to_errcode(flags[best]);

}
/** \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();
            }
        }
    }
}