📄 path_8cpp-source.html
字号:
<a name="l00644"></a>00644 e = e->getPrev();<a name="l00645"></a>00645 <span class="keywordflow">if</span>( !e ) <span class="keywordflow">break</span>;<a name="l00646"></a>00646 <a name="l00647"></a>00647 <span class="keywordflow">if</span>( !e->adjustEndVel(v) )<a name="l00648"></a>00648 <span class="keywordflow">break</span>;<a name="l00649"></a>00649 }<a name="l00650"></a>00650 <a name="l00651"></a>00651 <span class="comment">// Handle internal calculations for all segments</span><a name="l00652"></a>00652 <span class="comment">// that have been fully added.</span><a name="l00653"></a>00653 <span class="keywordflow">while</span>( e )<a name="l00654"></a>00654 {<a name="l00655"></a>00655 <span class="keywordflow">if</span>( e->Calculate() )<a name="l00656"></a>00656 <span class="keywordflow">break</span>;<a name="l00657"></a>00657 e = e->getPrev();<a name="l00658"></a>00658 }<a name="l00659"></a>00659 <a name="l00660"></a>00660 <span class="keywordflow">return</span> 0;<a name="l00661"></a>00661 }<a name="l00662"></a>00662 <a name="l00663"></a>00663 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a7">Path::AddLine</a>( <a class="code" href="classPointN.html">PointN</a> &p )<a name="l00664"></a>00664 {<a name="l00665"></a>00665 <span class="comment">// Make sure the passed point is of the correct dimension</span><a name="l00666"></a>00666 <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( p.<a class="code" href="classPointN.html#a0">getDim</a>() == <a class="code" href="classPath.html#a12">GetDim</a>() );<a name="l00667"></a>00667 <span class="keywordflow">if</span>( p.<a class="code" href="classPointN.html#a0">getDim</a>() != <a class="code" href="classPath.html#a12">GetDim</a>() )<a name="l00668"></a>00668 <span class="keywordflow">return</span> &<a class="code" href="classPathError.html#s4">PathError::BadPoint</a>;<a name="l00669"></a>00669 <a name="l00670"></a>00670 <span class="comment">// Adjust the passed point to find a position relative to </span><a name="l00671"></a>00671 <span class="comment">// the starting position.</span><a name="l00672"></a>00672 p -= posStart;<a name="l00673"></a>00673 <a name="l00674"></a>00674 <span class="comment">// Find the direction of travel between the current position</span><a name="l00675"></a>00675 <span class="comment">// and the passed point.</span><a name="l00676"></a>00676 <span class="keywordtype">double</span> dx = p[0] - posEnd[0];<a name="l00677"></a>00677 <span class="keywordtype">double</span> dy = 0.0;<a name="l00678"></a>00678 <a name="l00679"></a>00679 <span class="keywordflow">if</span>( <a class="code" href="classPath.html#a12">GetDim</a>() > 1 )<a name="l00680"></a>00680 dy = p[1] - posEnd[1];<a name="l00681"></a>00681 <a name="l00682"></a>00682 <span class="keywordtype">double</span> dirMove = atan2( dy, dx );<a name="l00683"></a>00683 <a name="l00684"></a>00684 <span class="comment">// If the move direction isn't in line with my current direction,</span><a name="l00685"></a>00685 <span class="comment">// then I'll have to come to a halt before adding the line segment.</span><a name="l00686"></a>00686 <span class="comment">// Otherwise, I would have an infinite acceleration during the </span><a name="l00687"></a>00687 <span class="comment">// direction change.</span><a name="l00688"></a>00688 <span class="keywordflow">if</span>( fabs(dirMove - dirEnd) > MAX_ANGLE_ERROR )<a name="l00689"></a>00689 <a class="code" href="classPath.html#a11">Pause</a>(0);<a name="l00690"></a>00690 <a name="l00691"></a>00691 <span class="keywordtype">double</span> len = posEnd.distance( p );<a name="l00692"></a>00692 <a name="l00693"></a>00693 <span class="comment">// Now, add the line segment to my path</span><a name="l00694"></a>00694 LineSeg *seg = <span class="keyword">new</span> LineSeg( posEnd, dirMove, len );<a name="l00695"></a>00695 <span class="keywordflow">if</span>( !seg )<a name="l00696"></a>00696 <span class="keywordflow">return</span> &<a class="code" href="classPathError.html#s5">PathError::Alloc</a>;<a name="l00697"></a>00697 <a name="l00698"></a>00698 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *err = AddSegment( seg );<a name="l00699"></a>00699 <span class="keywordflow">if</span>( err ) <span class="keywordflow">return</span> err;<a name="l00700"></a>00700 <a name="l00701"></a>00701 <span class="comment">// Update my ending position & direction</span><a name="l00702"></a>00702 posEnd = p;<a name="l00703"></a>00703 dirEnd = dirMove;<a name="l00704"></a>00704 <span class="keywordflow">return</span> 0;<a name="l00705"></a>00705 }<a name="l00706"></a>00706 <a name="l00707"></a>00707 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a7">Path::AddLine</a>( <a class="code" href="CML__Utils_8h.html#a11">uunit</a> length )<a name="l00708"></a>00708 {<a name="l00709"></a>00709 <span class="comment">// Length must be a positive value.</span><a name="l00710"></a>00710 <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( length >= 0 );<a name="l00711"></a>00711 <a name="l00712"></a>00712 <span class="comment">// Calculate the ending position based on the</span><a name="l00713"></a>00713 <span class="comment">// current position and direction of travel</span><a name="l00714"></a>00714 <a class="code" href="classPoint.html">Point<PATH_MAX_DIMENSIONS></a> p;<a name="l00715"></a>00715 p.<a class="code" href="classPoint.html#a4">setDim</a>( <a class="code" href="classPath.html#a12">GetDim</a>() );<a name="l00716"></a>00716 <a name="l00717"></a>00717 p = posEnd;<a name="l00718"></a>00718 p[0] += cos(dirEnd) * length;<a name="l00719"></a>00719 <span class="keywordflow">if</span>( <a class="code" href="classPath.html#a12">GetDim</a>() > 1 )<a name="l00720"></a>00720 p[1] += sin(dirEnd) * length;<a name="l00721"></a>00721 <a name="l00722"></a>00722 <span class="comment">// add the line segment to my path</span><a name="l00723"></a>00723 LineSeg *seg = <span class="keyword">new</span> LineSeg( posEnd, dirEnd, length );<a name="l00724"></a>00724 <span class="keywordflow">if</span>( !seg ) <span class="keywordflow">return</span> &<a class="code" href="classPathError.html#s5">PathError::Alloc</a>;<a name="l00725"></a>00725 <a name="l00726"></a>00726 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *err = AddSegment( seg );<a name="l00727"></a>00727 <span class="keywordflow">if</span>( err ) <span class="keywordflow">return</span> err;<a name="l00728"></a>00728 <a name="l00729"></a>00729 <span class="comment">// Update my ending position & direction</span><a name="l00730"></a>00730 posEnd = p;<a name="l00731"></a>00731 <span class="keywordflow">return</span> 0;<a name="l00732"></a>00732 }<a name="l00733"></a>00733 <a name="l00734"></a>00734 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a9">Path::AddArc</a>( <a class="code" href="classPointN.html">PointN</a> &center, <span class="keywordtype">double</span> angle )<a name="l00735"></a>00735 {<a name="l00736"></a>00736 <span class="comment">// Can't add an arc to a one dimensional path</span><a name="l00737"></a>00737 <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( <a class="code" href="classPath.html#a12">GetDim</a>() > 1 );<a name="l00738"></a>00738 <a name="l00739"></a>00739 <span class="comment">// Make sure the center position has the right number of dimensions</span><a name="l00740"></a>00740 <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( <a class="code" href="classPath.html#a12">GetDim</a>() == center.<a class="code" href="classPointN.html#a0">getDim</a>() );<a name="l00741"></a>00741 <a name="l00742"></a>00742 <span class="comment">// Adjust the center to find a position relative to </span><a name="l00743"></a>00743 <span class="comment">// the starting position.</span><a name="l00744"></a>00744 center -= posStart;<a name="l00745"></a>00745 <a name="l00746"></a>00746 <span class="comment">// Find the starting angle on the arc</span><a name="l00747"></a>00747 <span class="keywordtype">double</span> deltaX = center[0] - posEnd[0];<a name="l00748"></a>00748 <span class="keywordtype">double</span> deltaY = center[1] - posEnd[1];<a name="l00749"></a>00749 <span class="keywordtype">double</span> startAng = PI + atan2( deltaY, deltaX );<a name="l00750"></a>00750 <a name="l00751"></a>00751 <span class="comment">// See if there will be an abrupt change in direction when we start</span><a name="l00752"></a>00752 <span class="comment">// this path. If so, then I'll need to come to a halt first.</span><a name="l00753"></a>00753 <span class="keywordtype">double</span> deltaDir;<a name="l00754"></a>00754 <span class="keywordflow">if</span>( angle < 0 )<a name="l00755"></a>00755 deltaDir = startAng - dirEnd + PI_by_2;<a name="l00756"></a>00756 <span class="keywordflow">else</span><a name="l00757"></a>00757 deltaDir = startAng - dirEnd - PI_by_2;<a name="l00758"></a>00758 <a name="l00759"></a>00759 <span class="keywordflow">if</span>( fabs(deltaDir) > MAX_ANGLE_ERROR )<a name="l00760"></a>00760 <a class="code" href="classPath.html#a11">Pause</a>(0);<a name="l00761"></a>00761 <a name="l00762"></a>00762 <span class="keywordtype">double</span> radius = center.<a class="code" href="classPointN.html#a5">distance</a>( posEnd );<a name="l00763"></a>00763 ArcSeg *seg = <span class="keyword">new</span> ArcSeg( center, radius, startAng, angle );<a name="l00764"></a>00764 <span class="keywordflow">if</span>( !seg ) <span class="keywordflow">return</span> &<a class="code" href="classPathError.html#s5">PathError::Alloc</a>;<a name="l00765"></a>00765 <a name="l00766"></a>00766 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *err = AddSegment( seg );<a name="l00767"></a>00767 <span class="keywordflow">if</span>( err ) <span class="keywordflow">return</span> err;<a name="l00768"></a>00768 <a name="l00769"></a>00769 <span class="comment">// Update the ending position and direction</span><a name="l00770"></a>00770 <span class="keywordtype">double</span> ang = startAng - angle;<a name="l00771"></a>00771 posEnd[0] = center[0] + cos(ang) * radius;<a name="l00772"></a>00772 posEnd[1] = center[1] + sin(ang) * radius;<a name="l00773"></a>00773 <a name="l00774"></a>00774 <span class="keywordflow">if</span>( angle < 0 ) <a name="l00775"></a>00775 dirEnd = ang + PI_by_2;<a name="l00776"></a>00776 <span class="keywordflow">else</span> <a name="l00777"></a>00777 dirEnd = ang - PI_by_2;<a name="l00778"></a>00778 <a name="l00779"></a>00779 <span class="keywordflow">return</span> 0;<a name="l00780"></a>00780 }<a name="l00781"></a>00781 <a name="l00782"></a>00782 <span class="keyword">const</span> <a class="code" href="classError.html">Error</a> *<a class="code" href="classPath.html#a9">Path::AddArc</a>( <span class="keywordtype">double</span> radius, <span class="keywordtype">double</span> angle )<a name="l00783"></a>00783 {<a name="l00784"></a>00784 <span class="comment">// radius must be a positive value.</span><a name="l00785"></a>00785 <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( radius >= 0 );<a name="l00786"></a>00786 <a name="l00787"></a>00787 <span class="comment">// Can't add an arc to a one dimensional path</span><a name="l00788"></a>00788 <a class="code" href="CML__Settings_8h.html#a14">CML_ASSERT</a>( <a class="code" href="classPath.html#a12">GetDim</a>() > 1 );<a name="l00789"></a>00789 <a name="l00790"></a>00790 <span class="comment">// Find the center. This will be a point on the line that passes</span><a name="l00791"></a>00791 <span class="comment">// through the current end position and is orthogonal to the current</span><a name="l00792"></a>00792 <span class="comment">// direction of motion.</span><a name="l00793"></a>00793 <a class="code" href="classPoint.html">Point<PATH_MAX_DIMENSIONS></a> center;<a name="l00794"></a>00794 center.<a class="code" href="classPoint.html#a4">setDim</a>( <a class="code" href="classPath.html#a12">GetDim</a>() );<a name="l00795"></a>00795 center = posEnd;<a name="l00796"></a>00796 <a name="l00797"></a>00797 <span class="keywordtype">double</span> startAng;<a name="l00798"></a>00798 <a name="l00799"></a>00799 <span class="comment">// Positive angles are clockwise rotation</span><a name="l00800"></a>00800 <span class="keywordflow">if</span>( angle < 0 )<a name="l00801"></a>00801 {<a name="l00802"></a>00802 center[0] -= radius * sin(dirEnd);<a name="l00803"></a>00803 center[1] += radius * cos(dirEnd);<a name="l00804"></a>00804 startAng = dirEnd - PI_by_2;<a name="l00805"></a>00805 ⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -