Esempio n. 1
0
void Vec3f::rotate (const Vec3f &around, float angle)
{
  Transform3f matr;

  matr.rotation(around.x, around.y, around.z, angle);
  transformVector(matr);
}
Esempio n. 2
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]);

}
Esempio n. 3
0
bool EdgeRotationMatcher::match (float rms_threshold, float eps)
{
   if (cb_get_xyz == 0)
      throw Error("cb_get_xyz not specified");

   if (_subgraph.vertexCount() < 2 || _subgraph.edgeCount() < 1)
      return true;

   QS_DEF(Array<int>, in_cycle);
   QS_DEF(Array<_DirEdge>, edge_queue);
   QS_DEF(Array<int>, vertex_queue);
   QS_DEF(Array<int>, states);

   in_cycle.clear_resize(_subgraph.edgeEnd());
   edge_queue.clear();
   states.clear_resize(__max(_subgraph.edgeEnd(), _subgraph.vertexEnd() + 1));
   
   int i, j, k, bottom;

   // Find all subgraph bridges
   SpanningTree spt(_subgraph, 0);

   in_cycle.zerofill();

   spt.markAllEdgesInCycles(in_cycle.ptr(), 1);

   // Find the first bridge, put it to the queue 2 times
   for (i = _subgraph.edgeBegin(); i < _subgraph.edgeEnd(); i = _subgraph.edgeNext(i))
      if (!in_cycle[i] && (cb_can_rotate == 0 || cb_can_rotate(_subgraph, i)))
      {
         const Edge &edge = _subgraph.getEdge(i);

         if (_mapping[edge.beg] < 0 || _mapping[edge.end] < 0)
            continue;

         edge_queue.push();
         edge_queue.top().idx = i;
         edge_queue.top().beg = edge.beg;
         edge_queue.top().end = edge.end;

         edge_queue.push();
         edge_queue.top().idx = i;
         edge_queue.top().beg = edge.end;
         edge_queue.top().end = edge.beg;
         break;
      }

   // If the queue is empty, then we have no bridge
   if (edge_queue.size() == 0)
   {
      GraphAffineMatcher afm(_subgraph, _supergraph, _mapping);

      afm.cb_get_xyz = cb_get_xyz;
      return afm.match(rms_threshold);
   }

   float scale = 1.f;

   // detect scaling factor by average bond length
   if (equalize_edges)
   {
      float sum_sub = 0.f, sum_super = 0.f;
      
      for (i = _subgraph.edgeBegin(); i < _subgraph.edgeEnd(); i = _subgraph.edgeNext(i))
      {
         const Edge &edge = _subgraph.getEdge(i);
         Vec3f beg, end;

         cb_get_xyz(_subgraph, edge.beg, beg);
         cb_get_xyz(_subgraph, edge.end, end);

         sum_sub += Vec3f::dist(beg, end);
      }

      for (i = _supergraph.edgeBegin(); i < _supergraph.edgeEnd(); i = _supergraph.edgeNext(i))
      {
         const Edge &edge = _supergraph.getEdge(i);
         Vec3f beg, end;

         cb_get_xyz(_supergraph, edge.beg, beg);
         cb_get_xyz(_supergraph, edge.end, end);

         sum_super += Vec3f::dist(beg, end);
      }

      if (sum_sub > EPSILON && sum_super > EPSILON)
      {
         sum_sub /= _subgraph.edgeCount();
         sum_super /= _supergraph.edgeCount();
         scale = sum_super / sum_sub;
      }
   }

   // save vertex positions

   QS_DEF(Array<Vec3f>, xyz_sub);
   QS_DEF(Array<Vec3f>, xyz_super);
   QS_DEF(Array<int>, xyzmap);

   xyzmap.clear_resize(_supergraph.vertexEnd());
   xyz_sub.clear();
   xyz_super.clear();

   for (i = _subgraph.vertexBegin(); i != _subgraph.vertexEnd();
        i = _subgraph.vertexNext(i))
   {
      if (_mapping[i] < 0)
         continue;

      Vec3f &pos_sub   = xyz_sub.push();
      Vec3f &pos_super = xyz_super.push();

      cb_get_xyz(_subgraph, i, pos_sub);
      cb_get_xyz(_supergraph, _mapping[i], pos_super);

      pos_sub.scale(scale);

      xyzmap[_mapping[i]] = xyz_sub.size() - 1;
   }

   // Make queue of edges
   states.zerofill();
   bottom = 0;

   while (edge_queue.size() != bottom)
   {
      // extract edge from queue
      int edge_end = edge_queue[bottom].end;
      int edge_idx = edge_queue[bottom].idx;
      bottom++;
      
      // mark it as 'completed'
      states[edge_idx] = 2;

      // look for neighbors
      const Vertex &end_vertex = _subgraph.getVertex(edge_end);

      for (i = end_vertex.neiBegin(); i != end_vertex.neiEnd(); i = end_vertex.neiNext(i))
      {
         int nei_edge_idx = end_vertex.neiEdge(i);

         // check that neighbor have 'untouched' status
         if (states[nei_edge_idx] != 0)
            continue;

         const Edge &nei_edge = _subgraph.getEdge(nei_edge_idx);
         int other_end = nei_edge.findOtherEnd(edge_end);

         if (_mapping[other_end] < 0)
            continue;

         // set status 'in process'
         states[nei_edge_idx] = 1;

         // push the neighbor edge to the queue
         edge_queue.push();
         edge_queue.top().idx = nei_edge_idx;
         edge_queue.top().beg = edge_end;
         edge_queue.top().end = other_end;
      }
   }

   // do initial transform (impose first subgraph edge in the queue on corresponding one in the graph)
   int beg2 = edge_queue[0].beg;
   int end2 = edge_queue[0].end;
   int beg1 = _mapping[beg2];
   int end1 = _mapping[end2];
   Vec3f g1_v1, g1_v2, g2_v1, g2_v2, diff1, diff2;
   Transform3f matr;

   cb_get_xyz(_supergraph, beg1, g1_v1);
   cb_get_xyz(_supergraph, end1, g1_v2);

   cb_get_xyz(_subgraph, beg2, g2_v1);
   cb_get_xyz(_subgraph, end2, g2_v2);

   g2_v1.scale(scale);
   g2_v2.scale(scale);

   diff1.diff(g1_v2, g1_v1);
   diff2.diff(g2_v2, g2_v1);

   matr.identity();
   if (!matr.rotationVecVec(diff2, diff1))
      throw Error("error calling RotationVecVec()");

   matr.translateLocal(-g2_v1.x, -g2_v1.y, -g2_v1.z);
   matr.translate(g1_v1);

   for (k = 0; k < xyz_sub.size(); k++)
      xyz_sub[k].transformPoint(matr);

   // for all edges in queue that are subject to rotate...
   for (i = 0; i < edge_queue.size(); i++)
   {
      int edge_beg = edge_queue[i].beg;
      int edge_end = edge_queue[i].end;
      int edge_idx = edge_queue[i].idx;

      if (in_cycle[edge_idx])
         continue;

      if (cb_can_rotate != 0 && !cb_can_rotate(_subgraph, edge_idx))
         continue;

      // start BFS from the end of the edge
      states.zerofill();
      states[edge_end] = 1;

      vertex_queue.clear();
      vertex_queue.push(edge_end);
      bottom = 0;

      while (vertex_queue.size() != bottom)
      {
         // extract vertex from queue
         const Vertex &vertex = _subgraph.getVertex(vertex_queue[bottom]);

         states[vertex_queue[bottom]] = 2;
         bottom++;

         // look over neighbors
         for (int j = vertex.neiBegin(); j != vertex.neiEnd(); j = vertex.neiNext(j))
         {
            int nei_idx = vertex.neiVertex(j);

            if (nei_idx == edge_beg)
               continue;
            if (states[nei_idx] != 0)
               continue;
            
            states[nei_idx] = 1;
            vertex_queue.push(nei_idx);
         }
      }

      // now states[j] == 0 if j-th vertex shound not be moved

      Vec3f edge_beg_pos, edge_end_pos, rot_axis;

      // get rotation axis
      edge_beg_pos.copy(xyz_sub[xyzmap[_mapping[edge_beg]]]);
      edge_end_pos.copy(xyz_sub[xyzmap[_mapping[edge_end]]]);

      rot_axis.diff(edge_end_pos, edge_beg_pos);
      if (!rot_axis.normalize())
         continue;

      const Vertex &edge_end_vertex = _subgraph.getVertex(edge_end);

      float max_sum_len = -1;

      for (j = edge_end_vertex.neiBegin(); j != edge_end_vertex.neiEnd();
           j = edge_end_vertex.neiNext(j))
      {
         int nei_idx_2 = edge_end_vertex.neiVertex(j);
         int nei_idx_1 = _mapping[nei_idx_2];

         if (nei_idx_2 == edge_beg)
            continue;

         if (nei_idx_1 == -1)
            continue;

         Vec3f nei1_pos;
         Vec3f nei2_pos;

         nei1_pos.copy(xyz_super[xyzmap[nei_idx_1]]);
         nei2_pos.copy(xyz_sub[xyzmap[_mapping[nei_idx_2]]]);

         nei1_pos.sub(edge_end_pos);
         nei2_pos.sub(edge_end_pos);

         float dot1 = Vec3f::dot(nei1_pos, rot_axis);
         float dot2 = Vec3f::dot(nei2_pos, rot_axis);

         nei1_pos.addScaled(rot_axis, -dot1);
         nei2_pos.addScaled(rot_axis, -dot2);

         if (max_sum_len > nei1_pos.length() + nei1_pos.length())
            continue;

         max_sum_len = nei1_pos.length() + nei1_pos.length();

         if (!nei1_pos.normalize() || !nei2_pos.normalize())
            continue;

         double dp = Vec3f::dot(nei1_pos, nei2_pos);

         if (dp > 1 - EPSILON)
            dp = 1 - EPSILON;
         if (dp < -1 + EPSILON)
            dp = -1 + EPSILON;

         double ang = acos(dp);

         Vec3f cross;

         cross.cross(nei1_pos, nei2_pos);

         if (Vec3f::dot(cross, rot_axis) < 0)
            ang = -ang;

         matr.rotation(rot_axis.x, rot_axis.y, rot_axis.z, (float)ang);
         matr.translateLocalInv(edge_end_pos);
         matr.translate(edge_end_pos);
      }

      if (max_sum_len > 0)
      {
         for (j = _subgraph.vertexBegin(); j < _subgraph.vertexEnd(); j = _subgraph.vertexNext(j))
            if (_mapping[j] >= 0 && states[j] != 0)
               xyz_sub[xyzmap[_mapping[j]]].transformPoint(matr);
      }
   }

   float sqsum = 0;

   for (k = 0; k < xyz_sub.size(); k++)
      sqsum += Vec3f::distSqr(xyz_sub[k], xyz_super[k]);

   sqsum = sqrt(sqsum / xyz_sub.size());

   if (sqsum > rms_threshold + eps)
      return false;

   return true;
}