C++ Vector3::length方法代码示例

void HapticsPSM::compute_average_normal(std::vector<tf::Vector3> &v_arr, tf::Vector3 &v){
    tf::Vector3 new_n;
    new_n.setValue(0,0,0);
    if(v_arr.size() == 1){
        new_n = v_arr.at(0);
    }
    else{
        for(size_t i=0 ; i < v_arr.size() ; i++){
            new_n = new_n + v_arr.at(i);
        }
    }
    new_n.normalize();

    if(v.length() == 0){
        v = new_n; // This is for the first contact, since the current normal is zero, so assign the new value of the normal computed form collision checking
    }
    else{
        //Check if new_n = 0 or in the negative direction as curr n, if it is, leave n unchanged
        if(v.dot(new_n) == 0){ // If the dot product of the last and the current normal in 0, this could be due to the bug of getting negative normals
            //v remains unchanged
            ROS_INFO("New Normal Cancels out the previous one");
        }
        else if(v.dot(new_n) < 0 ){ //If the dot product of the last and the current normal in a negative number, this could be due to the bug of getting negative normals
            v = -new_n;
            ROS_INFO("New normal lies on the opposite plane");
        }
        else{
            v = new_n;
        }
    }
}

void HapticsPSM::compute_deflection_along_normal(tf::Vector3 &n, tf::Vector3 &d, tf::Vector3 &d_along_n){
    d_along_n = (d.dot(n)/n.length()) * n; //This gives us the dot product of current deflection in the direction of current normal
}

 tf::Vector3 sat(const tf::Vector3 v, double val)
 {
     return v*val/std::max(val,v.length());
 }

void speedsForObstacles(
  mnm::RouteSpeedArray &speeds,
  std::vector<DistanceReport> &reports,
  const Route &route,
  const mnm::RoutePosition &route_position,
  const std::vector<ObstacleData> &obstacles,
  const SpeedForObstaclesParameters &p)
{
  tf::Vector3 local_fl(p.origin_to_left_m_, p.origin_to_left_m_, 0.0);
  tf::Vector3 local_fr(p.origin_to_left_m_, -p.origin_to_right_m_, 0.0);
  tf::Vector3 local_br(-p.origin_to_right_m_, -p.origin_to_right_m_, 0.0);
  tf::Vector3 local_bl(-p.origin_to_right_m_, p.origin_to_left_m_, 0.0);
  double car_r = 0.0;
  car_r = std::max(car_r, local_fl.length());
  car_r = std::max(car_r, local_fr.length());
  car_r = std::max(car_r, local_br.length());
  car_r = std::max(car_r, local_bl.length());

  bool skip_point = true;

  for (size_t route_index = 0; route_index < route.points.size(); route_index++) {
    const RoutePoint &point = route.points[route_index];

    if (skip_point) {
      if (point.id() == route_position.id) {
        skip_point = false;
      }
      continue;
    }
    
    // Use half of the vehicle width override as the car radius when smaller
    // This is used for tight routes that are known to be safe such as when going
    // though cones. It prevents objects close to the vehicle from causing the vehicle
    // to stop/slow down too much unnecessarily.
    double veh_r = car_r;
    if (point.hasProperty("vehicle_width_override"))
    {
      ROS_DEBUG("Speeds for obstacle found vehicle_width_override property");
      double width = point.getTypedProperty<double>("vehicle_width_override");

      // Pick the smaller of the radii
      if (veh_r >= width/2.0)
      {
        veh_r = width/2.0;
        ROS_WARN_THROTTLE(1.0, "Vehicle width being overriden to %0.2f", (float)veh_r);
      }
    }

    for (const auto& obstacle: obstacles) {
      const tf::Vector3 v = obstacle.center - point.position();
      const double d = v.length() - veh_r - obstacle.radius;
      if (d > p.max_distance_m_) {
        // The obstacle is too far away from this point to be a concern
        continue;
      }

      tf::Vector3 closest_point = obstacle.center;

      double distance = std::numeric_limits<double>::max();
      for (size_t i = 1; i < obstacle.polygon.size(); i++) {
        double dist = swri_geometry_util::DistanceFromLineSegment(
          obstacle.polygon[i - 1],
          obstacle.polygon[i],
          point.position()) - veh_r;

        if (dist < distance) {
          distance = dist;
          closest_point = swri_geometry_util::ProjectToLineSegment(
            obstacle.polygon[i - 1],
            obstacle.polygon[i],
            point.position());
        }
      }

      if (obstacle.polygon.size() > 1) {
        double dist = swri_geometry_util::DistanceFromLineSegment(
          obstacle.polygon.back(),
          obstacle.polygon.front(),
          point.position()) - veh_r;

        if (dist < distance) {
          distance = dist;
          closest_point = swri_geometry_util::ProjectToLineSegment(
            obstacle.polygon.back(),
            obstacle.polygon.front(),
            point.position());
        }
      }

      if (distance > p.max_distance_m_) {
        // The obstacle is too far away from this point to be a concern
        continue;
      }

      // This obstacle is close enough to be a concern.  If the bounding
      // circles are still not touching, we apply a speed limit based on the
      // distance.
      if (distance > 0.0) {
        DistanceReport report;
        report.near = false;
//.........这里部分代码省略.........