I have a KML file , coming from a tractor that contains the worked area of a field. It divides the field in allot of polygons, each containing coordinates to shape that polygon. I want to use that data for guiding a robot that detects and removes weed from the corn field. Since i have this data, i don't need complex systems to guide the robot since this high accuracy data is already available (2cm).
I need to calculate a centerline from a those polygons in the KML file, for use as waypoints for the robot. Since the workedarea polygons are always 3m wide (corn planter is 3m) , and my robot will be 1.5m wide, i should prefer to have 2 guidlines per . But i'd be happy to get one decent centerline. i can work from there to add more.
This is one example placemark , with its coordinates of the polygon :
<Placemark>
<name>WorkedArea</name>
<Polygon>
<outerBoundaryIs>
<LinearRing>
<coordinates>
3.503930725,51.106383112,0
3.503908586,51.106378907,0
3.503672366,51.106360783,0
3.503569505,51.106356148,0
3.503519129,51.106356295,0
3.503467953,51.10636004,0
3.503347383,51.106378471,0
3.503297128,51.106384066,0
3.503124894,51.106407317,0
3.502982908,51.106418658,0
3.50289805,51.106422361,0
3.502897037,51.10644914,0
3.502985213,51.106445405,0
3.503130216,51.106433893,0
3.503355477,51.106404768,0
3.503478017,51.106386066,0
3.503525883,51.106382742,0
3.503572303,51.106382876,0
3.503669887,51.106387524,0
3.503902831,51.106405447,0
3.503922577,51.106409402,0
</coordinates>
</LinearRing>
</outerBoundaryIs>
</Polygon>
</Placemark>
this code refers to the followng polygon : Polygon image example
The coordinates are in order from one side to the other Polygon image example waypoints order
I was able with some PHP code to get centers between two side by side coordinates ,but for that to work i need an even number of opposite coordinates, and that is not always the case in highly bended polygons like the example
function getCenterLatLng($coordinates)
{
$x = $y = $z = 0;
$n = count($coordinates);
foreach ($coordinates as $point)
{
$lt = $point[0] * pi() / 180;
$lg = $point[1] * pi() / 180;
$x += cos($lt) * cos($lg);
$y += cos($lt) * sin($lg);
$z += sin($lt);
}
$x /= $n;
$y /= $n;
return [atan2(($z / $n), sqrt($x * $x + $y * $y)) * 180 / pi(), atan2($y, $x) * 180 / pi()];
}
Is there anyone who can point my in a direction to get a solid centerline? Prefered PHP, but any code example will do. Thank you.
/* SOLUTION */
I came to the following script : It detects one long side of the polygon, and draws a point at 1.5m at a 90° bearing from the point compared to the previous point. That gives me a pretty accurate waypoint in the middle of the polygon. The script is still not perfect , but it's a good way to start from here.
function calculatePoint($point1, $point2, $distance)
{
$aroundCorner = false;
// Convert latitude and longitude to radians
$lat1 = deg2rad($point1[1]);
$lon1 = deg2rad($point1[0]);
$lat2 = deg2rad($point2[1]);
$lon2 = deg2rad($point2[0]);
// Earth radius in meters
$earthRadius = 6371000;
// Calculate bearing from point1 to point2
$deltaLon = $lon2 - $lon1;
$y = sin($deltaLon) * cos($lat2);
$x = cos($lat1) * sin($lat2) - sin($lat1) * cos($lat2) * cos($deltaLon);
$bearing = atan2($y, $x);
echo $bearing.'<br/>';
if ($bearing>0) {
$aroundCorner=true;
}
// Convert bearing to initial azimuth
$azimuth = ($bearing + 2 * M_PI) % (2 * M_PI);
// Convert distance to radians
$distanceRad = $distance / $earthRadius;
// Calculate latitude and longitude of the new point
$newLat = asin(sin($lat1) * cos($distanceRad) + cos($lat1) * sin($distanceRad) * cos($azimuth));
$newLon = $lon1 + atan2(sin($azimuth) * sin($distanceRad) * cos($lat1), cos($distanceRad) - sin($lat1) * sin($newLat));
// Convert latitude and longitude back to degrees
$newLat = rad2deg($newLat);
$newLon = rad2deg($newLon);
// Determine if the new point is on the correct side of the waypoint
$crossProduct = sin($lon2 - $lon1) * cos($newLat);
if ($crossProduct < 0) {
$newBearing = ($azimuth - M_PI) % (2 * M_PI);
$newLat = asin(sin($lat1) * cos($distanceRad) + cos($lat1) * sin($distanceRad) * cos($newBearing));
$newLon = $lon1 + atan2(sin($newBearing) * sin($distanceRad) * cos($lat1), cos($distanceRad) - sin($lat1) * sin($newLat));
$newLat = rad2deg($newLat);
$newLon = rad2deg($newLon);
}
return [$newLat, $newLon,$aroundCorner];
}
// Polygon sample
$wayline = [
[ 51.106383112, 3.503930725],
[ 51.106378907, 3.503908586],
[ 51.106360783, 3.503672366],
[ 51.106356148, 3.503569505],
[ 51.106356295, 3.503519129],
[ 51.10636004, 3.503467953],
[ 51.106378471, 3.503347383],
[ 51.106384066, 3.503297128],
[ 51.106407317, 3.503124894],
[ 51.106418658, 3.502982908],
[ 51.106422361, 3.50289805],
[ 51.10644914, 3.502897037],
[ 51.106445405, 3.502985213],
[ 51.106433893, 3.503130216],
[ 51.106386066, 3.503478017],
[ 51.106382742, 3.503525883],
[ 51.106382876, 3.503572303],
[ 51.106387524, 3.503669887],
[ 51.106405447, 3.503902831],
[ 51.106409402, 3.503922577]
];
$distance = 1.5; // Distance in meters
$numPoints = count($wayline);
$newPoints = [];
for ($i = 0; $i < $numPoints - 1; $i++) {
$point1 = $wayline[$i];
$point2 = $wayline[$i+1];
$newPoint = calculatePoint($point1, $point2, $distance);
$newPoints[] = $newPoint;
}
// Output the new points
foreach ($newPoints as $point) {
if ($point[2]==false)
{
echo $point[1] . ',' . $point[0] . ',0<br/>';
}
}
outputs the following : KML imported into google maps
