//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; }