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

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

FCL_REAL TriangleMotionBoundVisitor::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 proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
  FCL_REAL tmp;
  tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
  if(tmp > proj_max) proj_max = tmp;
  tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
  if(tmp > proj_max) proj_max = tmp;

  proj_max = std::sqrt(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 mu = v_dot_n + w_cross_n * proj_max;

  return mu;
}

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

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