public class Helpers {

// Checks if the SocketIO event has JSON data.
// If there is data the JSON object in string format will be returned,
//   else the empty string "" will be returned.
public static String hasData(String s) {
    int found_null = s.indexOf("null");
    int b1 = s.indexOf("[");
    int b2 = s.indexOf("}");
    if (found_null != -1) {
        return "";
    } else if (b1 != -1 && b2 != -1) {
        return s.substring(b1, b2 + 1);
    }
    return "";
}

//
// Helper functions related to waypoints and converting from XY to Frenet
//   or vice versa
//

// For converting back and forth between radians and degrees.
public static double pi() {
    return Math.PI;
}

public static double deg2rad(double x) {
    return x * pi() / 180;
}

public static double rad2deg(double x) {
    return x * 180 / pi();
}

// Calculate distance between two points
public static double distance(double x1, double y1, double x2, double y2) {
    return Math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}

// Calculate closest waypoint to current x, y position
public static int closestWaypoint(double x, double y, final List<Double> maps_x, final List<Double> maps_y) {
    double closestLen = 100000; //large number
    int closestWaypoint = 0;

    for (int i = 0; i < maps_x.size(); ++i) {
        double map_x = maps_x.get(i);
        double map_y = maps_y.get(i);
        double dist = distance(x, y, map_x, map_y);
        if (dist < closestLen) {
            closestLen = dist;
            closestWaypoint = i;
        }
    }

    return closestWaypoint;
}

// Returns next waypoint of the closest waypoint
public static int nextWaypoint(double x, double y, double theta, final List<Double> maps_x,
                               final List<Double> maps_y) {
    int closestWaypoint = closestWaypoint(x, y, maps_x, maps_y);

    double map_x = maps_x.get(closestWaypoint);
    double map_y = maps_y.get(closestWaypoint);

    double heading = Math.atan2((map_y - y), (map_x - x));

    double angle = Math.abs(theta - heading);
    angle = Math.min(2 * pi() - angle, angle);

    if (angle > pi() / 2) {
        ++closestWaypoint;
        if (closestWaypoint == maps_x.size()) {
            closestWaypoint = 0;
        }
    }

    return closestWaypoint;
}

// Transform from Cartesian x,y coordinates to Frenet s,d coordinates
public static List<Double> getFrenet(double x, double y, double theta,
                                     final List<Double> maps_x,
                                     final List<Double> maps_y) {
    int next_wp = nextWaypoint(x, y, theta, maps_x, maps_y);

    int prev_wp;
    prev_wp = next_wp - 1;
    if (next_wp == 0) {
        prev_wp = maps_x.size() - 1;
    }

    double n_x = maps_x.get(next_wp) - maps_x.get(prev_wp);
    double n_y = maps_y.get(next_wp) - maps_y.get(prev_wp);
    double x_x = x - maps_x.get(prev_wp);
    double x_y = y - maps_y.get(prev_wp);

    // find the projection of x onto n
    double proj_norm = (x_x * n_x + x_y * n_y) / (n_x * n_x + n_y * n_y);
    double proj_x = proj_norm * n_x;
    double proj_y = proj_norm * n_y;

    double frenet_d = distance(x_x, x_y, proj_x, proj_y);

    //see if d value is positive or negative by comparing it to a center point
    double center_x = 1000 - maps_x.get(prev_wp);
    double center_y = 2000 - maps_y.get(prev_wp);
    double centerToPos = distance(center_x, center_y, x_x, x_y);
    double centerToRef = distance(center_x, center_y, proj_x, proj_y);

    if (centerToPos <= centerToRef) {
        frenet_d *= -1;
    }

    // calculate s value
    double frenet_s = 0;
    for (int i = 0; i < prev_wp; ++i) {
        frenet_s += distance(maps_x.get(i), maps_y.get(i), maps_x.get(i + 1), maps_y.get(i + 1));
    }

    frenet_s += distance(0, 0, proj_x, proj_y);

    return Arrays.asList(frenet_s, frenet_d);
}

// Transform from Frenet s,d coordinates to Cartesian x,y
public static List<Double> getXY(double s, double d, final List<Double> maps_s,
                                 final List<Double> maps_x,
                                 final List<Double> maps_y) {
    int prev_wp = -1;

    while (s > maps_s.get(prev_wp + 1) && (prev_wp < (int) (maps_s.size() - 1))) {
        ++prev_wp;
    }

    int wp2 = (prev_wp + 1) % maps_x.size();

    double heading = Math.atan2((maps_y.get(wp2) - maps_y.get(prev_wp)),
            (maps_x.get(wp2) - maps_x.get(prev_wp)));
    // the x,y,s along the segment
    double seg_s = (s - maps_s.get(prev_wp));

    double seg_x = maps_x.get(prev_wp) + seg_s * Math.cos(heading);
    double seg_y = maps_y.get(prev_wp) + seg_s * Math.sin(heading);

    double perp_heading = heading - pi() / 2;

    double x = seg_x + d * Math.cos(perp_heading);
    double y = seg_y + d * Math.sin(perp_heading);

    return Arrays.asList(x, y);
}

// Transform from Frenet d coordinate to lane number
public static int getLane(float d, float lane_width) {
    // get the lane of a car from it's distance from the middle of the road
    int lane = (int) (d / lane_width);
    if (lane < 0) {
        return -1;
    } else {
        return lane;
    }
}

}

convert c++ to java code below#ifndef HELPERS_H#define HELPERS_H#include mathh#include string#include vector for convenienceusing stdstring;using stdvector; Checks if the SocketIO event has JSON data

原文地址: https://www.cveoy.top/t/topic/jaH6 著作权归作者所有。请勿转载和采集!

免费AI点我,无需注册和登录