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

void Vec3f::rotate (const Vec3f &around, float angle)
{
  Transform3f matr;

  matr.rotation(around.x, around.y, around.z, angle);
  transformVector(matr);
}

//.........这里部分代码省略.........
    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;
      }

//.........这里部分代码省略.........

         // 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