//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 - fixHeadingSection) - Math.PI) > glm.PIBy2) leftLook *= -1;
head = right.HeadingXZ();
if (Math.PI - Math.Abs(Math.Abs(head - fixHeadingSection) - 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++)
{
if (boundz.isSet)
{
bool isLeftIn = true, isRightIn = true;
if (j == 0)
{
//only one first left point, the rest are all rights moved over to left
isLeftIn = boundz.IsPointInsideBoundary(section[j].leftPoint);
isRightIn = boundz.IsPointInsideBoundary(section[j].rightPoint);
if (isLeftIn && isRightIn) section[j].isInsideBoundary = true;
else section[j].isInsideBoundary = false;
}
else
{
//grab the right of previous section, its the left of this section
isLeftIn = isRightIn;
isRightIn = boundz.IsPointInsideBoundary(section[j].rightPoint);
if (isLeftIn && isRightIn) section[j].isInsideBoundary = true;
else section[j].isInsideBoundary = false;
}
section[vehicle.numOfSections].isInsideBoundary &= section[j].isInsideBoundary;
}
//no boundary created so always inside
else
{
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;
}
//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, rightSpeed = 0;
//now loop all the section rights and the one extreme left
for (int j = 0; j < vehicle.numberOfSections; 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()* vehicle.toolLookAhead / fixUpdateTime * 10;
//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()* vehicle.toolLookAhead / fixUpdateTime * 10;
//choose fastest speed
if (leftSpeed > rightSpeed) section[j].sectionLookAhead = leftSpeed;
else section[j].sectionLookAhead = rightSpeed;
double head = left.HeadingXZ();
if (head < 0) head += glm.twoPI;
double head2 = right.HeadingXZ();
if (head2 < 0) head2 += glm.twoPI;
//if both left and right sides of section are going backwards turn off section, negative lookahead
if (Math.PI - Math.Abs(Math.Abs(head - fixHeadingSection) - Math.PI) > glm.PIBy2 &&
Math.PI - Math.Abs(Math.Abs(head2 - fixHeadingSection) - Math.PI) > glm.PIBy2)
section[j].sectionLookAhead = Math.Abs(section[j].sectionLookAhead) * -1;
}//endfor
//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.numberOfSections - 1].sectionLookAhead > 0) vehicle.toolFarRightSpeed = rightSpeed * 0.05;
else vehicle.toolFarRightSpeed = 0;
//safe 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;
}
