Examples with CurvaturePoint2f com.robo4j.math.geometry.CurvaturePoint2f used on opensource projects

Example 1 with CurvaturePoint2f

use of com.robo4j.math.geometry.CurvaturePoint2f in project robo4j by Robo4J.

the class FeatureExtraction method extractCorners.

private static Collection<? extends CurvaturePoint2f> extractCorners(List<Point2f> Point2fs, float[] deltaAngles) {
    List<CurvaturePoint2f> corners = new ArrayList<>();
    for (int i = 0; i < deltaAngles.length; i++) {
        if (Math.abs(deltaAngles[i]) > CURVATURE_THRESHOLD) {
            int maxIndex = i;
            float maxPhi = deltaAngles[i];
            float totalPhi = maxPhi;
            int last = Math.min(i + 4, deltaAngles.length);
            for (int k = i + 1; k < last; k++) {
                totalPhi += deltaAngles[k];
                if (deltaAngles[k] > maxPhi) {
                    maxPhi = deltaAngles[k];
                    maxIndex = k;
                }
                i = k;
            }
            if (Math.abs(totalPhi) > CORNER_THRESHOLD && Math.signum(totalPhi) == Math.signum(maxPhi) && maxIndex - 3 >= 0 && maxIndex + 4 < deltaAngles.length) {
                Point2f p = Point2fs.get(maxIndex);
                Point2f b = Point2fs.get(maxIndex - 3);
                Point2f f = Point2fs.get(maxIndex + 3);
                float cornerAlpha = calculateVectorAngle(b, p, f);
                if (cornerAlpha > CORNER_THRESHOLD) {
                    corners.add(CurvaturePoint2f.fromPoint(p, cornerAlpha));
                }
            }
        }
    }
    return corners;
}

Example 2 with CurvaturePoint2f

use of com.robo4j.math.geometry.CurvaturePoint2f in project robo4j by Robo4J.

the class Raycast method raycastSingle.

/**
 * Casts a single ray at the specified angle, and returns the distance at
 * which it hit something, or {@link Float#MAX_VALUE} if it did not hit
 * anything.
 *
 * @param points
 *            the points to check against.
 * @param corners
 *            the points known to be corners.
 * @param rayAlpha
 *            the angle to emit the ray at.
 * @param defaultNoGoRadius
 *            the radius around a known point to avoid.
 * @return the distance at which the ray hit something.
 */
public static float raycastSingle(List<Point2f> points, List<CurvaturePoint2f> corners, float rayAlpha, float defaultNoGoRadius) {
    float minIntersectionRange = Float.MAX_VALUE;
    float currentNoGoRadius = defaultNoGoRadius;
    for (Point2f p : points) {
        CurvaturePoint2f cornerPoint = getMatchingCornerPoint(corners, p);
        if (cornerPoint != null) {
            currentNoGoRadius = calculateNoGoRadius(cornerPoint, defaultNoGoRadius);
        } else {
            currentNoGoRadius = defaultNoGoRadius;
        }
        float tangentDistance = calculateTangentDistance(rayAlpha, p);
        // Fast rejection
        if (Math.abs(tangentDistance) >= currentNoGoRadius) {
            continue;
        }
        float intersectionRange = calculateIntersectionRange(rayAlpha, currentNoGoRadius, tangentDistance, p);
        if (!Float.isNaN(intersectionRange)) {
            minIntersectionRange = Math.min(minIntersectionRange, intersectionRange);
        }
    }
    return minIntersectionRange;
}