void Transform3f::transformLocal (const Transform3f &transform)
{
   Transform3f tmp;

   tmp.composition(transform, *this);
   copy(tmp);
}

/// @brief Compute the motion bound for a triangle along a given direction n
/// according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity
/// and ci are the triangle vertex coordinates.
/// Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
{
  Transform3f tf;
  motion.getCurrentTransform(tf);

  const Vec3f& reference_p = motion.getReferencePoint();
  const Vec3f& angular_axis = motion.getAngularAxis();
  FCL_REAL angular_vel = motion.getAngularVelocity();
  const Vec3f& linear_vel = motion.getLinearVelocity();

  FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
  FCL_REAL tmp;
  tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
  if(tmp > proj_max) proj_max = tmp;
  tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
  if(tmp > proj_max) proj_max = tmp;

  proj_max = std::sqrt(proj_max);

  FCL_REAL v_dot_n = linear_vel.dot(n);
  FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
  FCL_REAL mu = v_dot_n + w_cross_n * proj_max;

  return mu;  
}

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

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

FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const InterpMotion& motion) const
{
  Transform3f tf;
  motion.getCurrentTransform(tf);

  const Vec3f& reference_p = motion.getReferencePoint();
  const Vec3f& angular_axis = motion.getAngularAxis();
  FCL_REAL angular_vel = motion.getAngularVelocity();
  const Vec3f& linear_vel = motion.getLinearVelocity();
  
  FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
  FCL_REAL tmp;
  tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
  if(tmp > c_proj_max) c_proj_max = tmp;
  tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
  if(tmp > c_proj_max) c_proj_max = tmp;
  tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
  if(tmp > c_proj_max) c_proj_max = tmp;

  c_proj_max = std::sqrt(c_proj_max);

  FCL_REAL v_dot_n = linear_vel.dot(n);
  FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
  FCL_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);

  return mu;  
}

void Transform3f::rotateZLocal (float angle)
{
   Transform3f rot;

   rot.rotationZ(angle);
   transformLocal(rot);
}

void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
                       Transform3f& tf)
{
  const Quaternion3f& q1inv = fcl::conj(tf1.getQuatRotation());
  const Quaternion3f& q2_q1inv = tf2.getQuatRotation() * q1inv;
  tf = Transform3f(q2_q1inv, tf2.getTranslation() - q2_q1inv.transform(tf1.getTranslation()));
}

void Transform3f::rotateY (float angle)
{
   Transform3f rot;

   rot.rotationY(angle);
   transform(rot);
}

/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
{
  const Quaternion3f& q = tf.getQuatRotation();
  const Vec3f& T = tf.getTranslation();
  ccdVec3Set(&o->pos, T[0], T[1], T[2]);
  ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
  ccdQuatInvert2(&o->rot_inv, &o->rot);
}

bool GraphAffineMatcher::match (float rms_threshold)
{
   if (cb_get_xyz == 0)
      throw Error("cb_get_xyz not set");

   int i;
   Transform3f matr;
   Vec3f pos;

   QS_DEF(Array<Vec3f>, points);
   QS_DEF(Array<Vec3f>, goals);

   points.clear();
   goals.clear();

   if (fixed_vertices != 0)
   {
      for (i = 0; i < fixed_vertices->size(); i++)
      {
         if (_mapping[fixed_vertices->at(i)] < 0)
            continue;
         cb_get_xyz(_subgraph, fixed_vertices->at(i), pos);
         points.push(pos);
         cb_get_xyz(_supergraph, _mapping[fixed_vertices->at(i)], pos);
         goals.push(pos);
      }
   }
   else for (i = _subgraph.vertexBegin(); i < _subgraph.vertexEnd(); i = _subgraph.vertexNext(i))
   {
      if (_mapping[i] < 0)
         continue;
      cb_get_xyz(_subgraph, i, pos);
      points.push(pos);
      cb_get_xyz(_supergraph, _mapping[i], pos);
      goals.push(pos);
   }

   if (points.size() < 1)
      return true;

   float sqsum;

   if (!matr.bestFit(points.size(), points.ptr(), goals.ptr(), &sqsum))
      return false;

   if (sqsum > rms_threshold * rms_threshold)
      return false;

   return true;
}

 void draw()
 {
   int end = mCenters.size();
   glEnable(GL_NORMALIZE);
   for (int i=0; i<end; ++i)
   {
     Transform3f t = Translation3f(mCenters[i]) * Scaling3f(mRadii[i]);
     gpu.pushMatrix(GL_MODELVIEW);
     gpu.multMatrix(t.matrix(),GL_MODELVIEW);
     mIcoSphere.draw(2);
     gpu.popMatrix(GL_MODELVIEW);
   }
   glDisable(GL_NORMALIZE);
 }

bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
                                   const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
                                   const Transform3f& tf,
                                   Vec3f* contact_points,
                                   unsigned int* num_contact_points,
                                   FCL_REAL* penetration_depth,
                                   Vec3f* normal)
{
  Vec3f Q1_ = tf.transform(Q1);
  Vec3f Q2_ = tf.transform(Q2);
  Vec3f Q3_ = tf.transform(Q3);

  return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal);
}

CMUK_ERROR_CODE cmuk::computeWorldPointFootDK( LegIndex leg,
                                               JointOffset link,
                                               const KState& state,
                                               const vec3f& pworld,
                                               mat3f* J_trans,
                                               mat3f* J_rot,
                                               float* det,
                                               float det_tol,
                                               float lambda ) const {

  if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
    return CMUK_BAD_LEG_INDEX;
  }
  if (!J_trans || !J_rot) {
    return CMUK_INSUFFICIENT_ARGUMENTS;
  }
    
  Transform3f xform = state.xform();
  vec3f pbody = xform.transformInv(pworld);
  vec3f fbody;

  mat3f J;

  CMUK_ERROR_CODE err = 
    computePointFootDK( leg, link, state.leg_rot[leg], pbody, &J,
                        &fbody, det, det_tol, lambda );

  if (err != CMUK_OKAY && err != CMUK_SINGULAR_MATRIX) {
    return err;
  }

  const mat3f& R = xform.rotFwd();
  const mat3f& Rinv = xform.rotInv();

  *J_trans = mat3f::identity() - R * J * Rinv;
  //*J_rot = R * (-mat3f::cross(pbody) + J * mat3f::cross(fbody));
  *J_rot = R * ( J * mat3f::cross(fbody) - mat3f::cross(pbody) ) * Rinv;

  return err;

}

FCL_REAL TBVMotionBoundVisitor<RSS>::visit(const ScrewMotion& motion) const
{
  Transform3f tf;
  motion.getCurrentTransform(tf);

  const Vec3f& axis = motion.getAxis();
  FCL_REAL linear_vel = motion.getLinearVelocity();
  FCL_REAL angular_vel = motion.getAngularVelocity();
  const Vec3f& p = motion.getAxisOrigin();
    
  FCL_REAL c_proj_max = ((tf.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
  FCL_REAL tmp;
  tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
  if(tmp > c_proj_max) c_proj_max = tmp;
  tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
  if(tmp > c_proj_max) c_proj_max = tmp;
  tmp = ((tf.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
  if(tmp > c_proj_max) c_proj_max = tmp;

  c_proj_max = sqrt(c_proj_max);

  FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
  FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
  FCL_REAL origin_proj = ((tf.getTranslation() - p).cross(axis)).length();

  FCL_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);

  return mu;  
}

void generateRandomTransform(FCL_REAL extents[6], Transform3f& transform)
{
  FCL_REAL x = rand_interval(extents[0], extents[3]);
  FCL_REAL y = rand_interval(extents[1], extents[4]);
  FCL_REAL z = rand_interval(extents[2], extents[5]);

  const FCL_REAL pi = 3.1415926;
  FCL_REAL a = rand_interval(0, 2 * pi);
  FCL_REAL b = rand_interval(0, 2 * pi);
  FCL_REAL c = rand_interval(0, 2 * pi);

  Matrix3f R;
  eulerToMatrix(a, b, c, R);
  Vec3f T(x, y, z);
  transform.setTransform(R, T);
}

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

  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";

CMUK_ERROR_CODE cmuk::getRelTransform( const cmuk::XformCache& cache,
					     FrameIndex f0,
					     FrameIndex f1,
					     Transform3f* xform ) const {

  int if0 = int(f0);
  int if1 = int(f1);

  if (if0 != FRAME_GROUND && (if0 < 0 || if0 >= NUM_FRAMES)) {
    return CMUK_BAD_FRAME_INDEX;
  }

  if (if1 !=FRAME_GROUND && (if1 < 0 || if1 >= NUM_FRAMES)) {
    return CMUK_BAD_FRAME_INDEX;
  }

  if (!xform) {
    return CMUK_INSUFFICIENT_ARGUMENTS;
  }

  int leg0 = -1;
  int leg1 = -1;
  if (if0 > int(FRAME_TRUNK)) { leg0 = (if0 - 1) / 4; }
  if (if1 > int(FRAME_TRUNK)) { leg1 = (if0 - 1) / 4; }

  if (leg0 >= 0 && leg1 >= 0 && leg0 != leg1) {

    Transform3f t1, t2;
    CMUK_ERROR_CODE status;

    status = getRelTransform(cache, f0, FRAME_TRUNK, &t1);
    if (status != CMUK_OKAY) { return status; }

    status = getRelTransform(cache, FRAME_TRUNK, f1, &t2);
    if (status != CMUK_OKAY) { return status; }

    *xform = t1 * t2;

    return CMUK_OKAY;

  }

  if (if0 == if1) {
    *xform = Transform3f();
    return CMUK_OKAY;
  }

  // if1 should be smaller than if0
  bool swap = if0 < if1;

  if (swap) {
    int tmp = if0;
    if0 = if1;
    if1 = tmp;
  }

  Transform3f rval = cache.relXforms[if0];
  if0 = parentFrame(if0);
  
  // now we are going to decrement if0
  while (if1 < if0 && if0 > 0) {
    rval = rval * cache.relXforms[if0];
    if0 = parentFrame(if0);
  }

  if (swap) {
    rval = rval.inverse();
  }

  *xform = rval;
  return CMUK_OKAY;

}

bool Transform3f::bestFit (int npoints, const Vec3f points[], const Vec3f goals[], float *sqsum_out)
{
   QS_DEF(Array<double>, X); //set of points
   QS_DEF(Array<double>, Y); //set of goals
   Matr3x3d R, RT, RTR, evectors_matrix;
   //
   Matr3x3d rotation;
   double scale;
   Vec3f translation;
   //

   bool res = 1;
   Vec3f vec, tmp;
   double cpoints[3] = {0.0}, cgoals[3] = {0.0}; // centroid of points, of goals
   int i, j, k;
   
   for (i = 0; i < npoints; i++)
   {
      cpoints[0] += points[i].x;
      cpoints[1] += points[i].y;
      cpoints[2] += points[i].z;
      cgoals[0] += goals[i].x;
      cgoals[1] += goals[i].y;
      cgoals[2] += goals[i].z;
   }
   for (i = 0; i < 3; i++)
   {
      cpoints[i] /= npoints; 
      cgoals[i] /= npoints;
   }
   X.resize(npoints * 3);
   Y.resize(npoints * 3);

   //move each set to origin
   for (i = 0; i < npoints; i++)
   {
      X[i * 3 + 0] = points[i].x - cpoints[0];
      X[i * 3 + 1] = points[i].y - cpoints[1];
      X[i * 3 + 2] = points[i].z - cpoints[2];
      Y[i * 3 + 0] = goals[i].x - cgoals[0];
      Y[i * 3 + 1] = goals[i].y - cgoals[1];
      Y[i * 3 + 2] = goals[i].z - cgoals[2];
   }

   if (npoints > 1)
   {
      /* compute R */
      for (i = 0; i < 3; i++) 
      {
         for (j = 0; j < 3; j++) 
         {
            R.elements[i * 3 + j] = 0.0;
            for (k = 0; k < npoints; k++)
            {
               R.elements[i * 3 + j] += Y[k * 3 + i] * X[k * 3 + j];
            }
         }
      }
      
      //Compute R^T * R

      R.getTransposed(RT);
      RT.matrixMatrixMultiply(R, RTR);

      RTR.eigenSystem(evectors_matrix);

      if (RTR.elements[0] > 2 * EPSILON)
      {
         float norm_b0,norm_b1,norm_b2;
         Vec3f a0, a1, a2;
         Vec3f b0, b1, b2;
         
         a0.set((float)evectors_matrix.elements[0], (float)evectors_matrix.elements[3], (float)evectors_matrix.elements[6]);
         a1.set((float)evectors_matrix.elements[1], (float)evectors_matrix.elements[4], (float)evectors_matrix.elements[7]);
         a2.cross(a0, a1);

         R.matrixVectorMultiply(a0, b0);
         R.matrixVectorMultiply(a1, b1);
         norm_b0 = b0.length();
         norm_b1 = b1.length();
         Line3f l1, l2;
         float sqs1, sqs2;
         l1.bestFit(npoints, points, &sqs1);
         l2.bestFit(npoints, goals, &sqs2);
         if( sqs1 < 2 * EPSILON && sqs2 < 2 * EPSILON)
         {
            Transform3f temp;
            temp.rotationVecVec(l1.dir, l2.dir);
            for (i = 0; i < 3; i++)
               for (j = 0; j < 3; j++)
                  rotation.elements[i * 3 + j] = temp.elements[j * 4 + i];
         }
         else
         {
            b0.normalize();
            b1.normalize();
            b2.cross(b0, b1);
            norm_b2 = b2.length();
            
            evectors_matrix.elements[2] = a2.x;
//.........这里部分代码省略.........

bool conservativeAdvancementMeshOriented(const BVHModel<BV>& o1,
                                         const MotionBase* motion1,
                                         const BVHModel<BV>& o2,
                                         const MotionBase* motion2,
                                         const CollisionRequest& request,
                                         CollisionResult& result,
                                         FCL_REAL& toc)
{
  Transform3f tf1, tf2;
  motion1->getCurrentTransform(tf1);
  motion2->getCurrentTransform(tf2);

  // whether the first start configuration is in collision
  if(collide(&o1, tf1, &o2, tf2, request, result))
  {
    toc = 0;
    return true;
  }
  
  
  ConservativeAdvancementOrientedNode node;

  initialize(node, o1, tf1, o2, tf2);

  node.motion1 = motion1;
  node.motion2 = motion2;

  do
  {
    node.motion1->getCurrentTransform(tf1);
    node.motion2->getCurrentTransform(tf2);

    // compute the transformation from 1 to 2
    Transform3f tf;
    relativeTransform(tf1, tf2, tf);
    node.R = tf.getRotation();
    node.T = tf.getTranslation();
    
    node.delta_t = 1;
    node.min_distance = std::numeric_limits<FCL_REAL>::max();

    distanceRecurse(&node, 0, 0, NULL);

    if(node.delta_t <= node.t_err)
    {
      // std::cout << node.delta_t << " " << node.t_err << std::endl;
      break;
    }

    node.toc += node.delta_t;
    if(node.toc > 1)
    {
      node.toc = 1;
      break;
    }

    node.motion1->integrate(node.toc);
    node.motion2->integrate(node.toc);
  }
  while(1);

  toc = node.toc;

  if(node.toc < 1)
    return true;

  return false;
}

void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
                       Transform3f& tf)
{
  const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation());
  tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
}

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