public static double Distance(vec2 first, double east, double north)
{
return(Math.Sqrt(
Math.Pow(first.easting - east, 2)
+ Math.Pow(first.northing - north, 2)));
}
//calculate the extreme tool left, right velocities, each section lookahead, and whether or not its going backwards
public void CalculateSectionLookAhead(double northing, double easting, double cosHeading, double sinHeading)
{
//calculate left side of section 1
vec2 left = new vec2(0, 0);
vec2 right = left;
double leftSpeed = 0, rightSpeed = 0, leftLook = 0, rightLook = 0;
//now loop all the section rights and the one extreme left
for (int j = 0; j < vehicle.numOfSections; j++)
{
if (j == 0)
{
//only one first left point, the rest are all rights moved over to left
section[j].leftPoint = new vec2(cosHeading * (section[j].positionLeft) + easting,
sinHeading * (section[j].positionLeft) + northing);
left = section[j].leftPoint - section[j].lastLeftPoint;
//save a copy for next time
section[j].lastLeftPoint = section[j].leftPoint;
//get the speed for left side only once
leftSpeed = left.GetLength() / fixUpdateTime * 10;
leftLook = leftSpeed * vehicle.toolLookAhead;
//save the far left speed
vehicle.toolFarLeftSpeed = leftSpeed;
}
else
{
//right point from last section becomes this left one
section[j].leftPoint = section[j-1].rightPoint;
left = section[j].leftPoint - section[j].lastLeftPoint;
//save a copy for next time
section[j].lastLeftPoint = section[j].leftPoint;
leftSpeed = rightSpeed;
}
section[j].rightPoint = new vec2(cosHeading * (section[j].positionRight) + easting,
sinHeading * (section[j].positionRight) + northing);
//now we have left and right for this section
right = section[j].rightPoint - section[j].lastRightPoint;
//save a copy for next time
section[j].lastRightPoint = section[j].rightPoint;
//grab vector length and convert to meters/sec/10 pixels per meter
rightSpeed = right.GetLength() / fixUpdateTime * 10;
rightLook = rightSpeed * vehicle.toolLookAhead;
//Is section outer going forward or backward
double head = left.HeadingXZ();
if (Math.PI - Math.Abs(Math.Abs(head - toolPos.heading) - Math.PI) > glm.PIBy2) leftLook *= -1;
head = right.HeadingXZ();
if (Math.PI - Math.Abs(Math.Abs(head - toolPos.heading) - Math.PI) > glm.PIBy2) rightLook *= -1;
//choose fastest speed
if (leftLook > rightLook) section[j].sectionLookAhead = leftLook;
else section[j].sectionLookAhead = rightLook;
if (section[j].sectionLookAhead > 190) section[j].sectionLookAhead = 190;
//Doing the slow mo, exceeding buffer so just set as minimum 0.5 meter
if (currentStepFix >= totalFixSteps - 1) section[j].sectionLookAhead = 5;
}//endfor
////set up the super for youturn
//section[vehicle.numOfSections].isInsideBoundary = true;
////determine if section is in boundary using the section left/right positions
//for (int j = 0; j < vehicle.numOfSections; j++)
//{
// {
// section[j].isInsideBoundary = true;
// section[vehicle.numOfSections].isInsideBoundary = false;
// }
//}
//with left and right tool velocity to determine rate of triangle generation, corners are more
//save far right speed, 0 if going backwards, in meters/sec
if (section[vehicle.numOfSections - 1].sectionLookAhead > 0) vehicle.toolFarRightSpeed = rightSpeed * 0.05;
else vehicle.toolFarRightSpeed = 0;
//save left side, 0 if going backwards, in meters/sec convert back from pixels/m
if (section[0].sectionLookAhead > 0) vehicle.toolFarLeftSpeed = vehicle.toolFarLeftSpeed * 0.05;
else vehicle.toolFarLeftSpeed = 0;
}
public static double Distance(vec2 first, vec3 second)
{
return(Math.Sqrt(
Math.Pow(first.easting - second.easting, 2)
+ Math.Pow(first.northing - second.northing, 2)));
}
