src/Geometry.cpp

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007 
00008 1. Redistributions of source code must retain the above copyright notice, this
00009 list of conditions and the following disclaimer.
00010 
00011 2. Redistributions in binary form must reproduce the above copyright notice,
00012 this list of conditions and the following disclaimer in the documentation
00013 and/or other materials provided with the distribution.
00014 
00015 3. Neither the name of the University of Amsterdam nor the names of its
00016 contributors may be used to endorse or promote products derived from this
00017 software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
00023 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00024 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00025 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00027 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00028 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 */
00030 
00055 #include "Geometry.h"
00056 #include <stdio.h>    // needed for sprintf
00057 
00062 int sign( double d1 )
00063 {
00064   return (d1>0)?1:-1;
00065 }
00066 
00071 double max( double d1, double d2 )
00072 {
00073   return (d1>d2)?d1:d2;
00074 }
00075 
00080 double min( double d1, double d2 )
00081 {
00082   return (d1<d2)?d1:d2;
00083 }
00084 
00085 
00090 AngDeg Rad2Deg( AngRad x )
00091 {
00092   return ( x * 180 / M_PI );
00093 }
00094 
00099 AngRad Deg2Rad( AngDeg x )
00100 {
00101   return ( x * M_PI / 180 );
00102 }
00103 
00108 double cosDeg( AngDeg x )
00109 {
00110   return ( cos( Deg2Rad( x ) ) );
00111 }
00112 
00117 double sinDeg( AngDeg x )
00118 {
00119   return ( sin( Deg2Rad( x ) ) );
00120 }
00121 
00126 double tanDeg( AngDeg x )
00127 {
00128   return ( tan( Deg2Rad( x ) ) );
00129 }
00130 
00136 AngDeg atanDeg( double x )
00137 {
00138   return ( Rad2Deg( atan( x ) ) );
00139 }
00140 
00149 double atan2Deg( double x, double y )
00150 {
00151   if( fabs( x ) < EPSILON && fabs( y ) < EPSILON )
00152     return ( 0.0 );
00153 
00154   return ( Rad2Deg( atan2( x, y ) ) );
00155 }
00156 
00161 AngDeg acosDeg( double x )
00162 {
00163   if( x >= 1 )
00164     return ( 0.0 );
00165   else if( x <= -1 )
00166     return ( 180.0 );
00167 
00168   return ( Rad2Deg( acos( x ) ) );
00169 }
00170 
00175 AngDeg asinDeg( double x )
00176 {
00177   if( x >= 1 )
00178     return ( 90.0 );
00179   else if ( x <= -1 )
00180     return ( -90.0 );
00181 
00182   return ( Rad2Deg( asin( x ) ) );
00183 }
00184 
00193 bool isAngInInterval( AngDeg ang, AngDeg angMin, AngDeg angMax )
00194 {
00195   // convert all angles to interval 0..360
00196   if( ( ang    + 360 ) < 360 ) ang    += 360;
00197   if( ( angMin + 360 ) < 360 ) angMin += 360;
00198   if( ( angMax + 360 ) < 360 ) angMax += 360;
00199 
00200   if( angMin < angMax ) // 0 ---false-- angMin ---true-----angMax---false--360
00201     return angMin < ang && ang < angMax ;
00202   else                  // 0 ---true--- angMax ---false----angMin---true---360
00203     return !( angMax < ang && ang < angMin );
00204 }
00205 
00212 AngDeg getBisectorTwoAngles( AngDeg angMin, AngDeg angMax )
00213 {
00214   // separate sine and cosine part to circumvent boundary problem
00215   return VecPosition::normalizeAngle(
00216             atan2Deg( (sinDeg( angMin) + sinDeg( angMax ) )/2.0,
00217                       (cosDeg( angMin) + cosDeg( angMax ) )/2.0 ) );
00218 }
00219 
00220 /*****************************************************************************/
00221 /*******************   CLASS VECPOSITION   ***********************************/
00222 /*****************************************************************************/
00223 
00238 VecPosition::VecPosition( double x, double y, CoordSystemT cs )
00239 {
00240   setVecPosition( x, y, cs );
00241 }
00242 
00247 VecPosition VecPosition::operator - ( )
00248 {
00249   return ( VecPosition( -m_x, -m_y ) );
00250 }
00251 
00260 VecPosition VecPosition::operator + ( const double &d )
00261 {
00262   return ( VecPosition( m_x + d, m_y + d ) );
00263 }
00264 
00270 VecPosition VecPosition::operator + ( const VecPosition &p )
00271 {
00272   return ( VecPosition( m_x + p.m_x, m_y + p.m_y ) );
00273 }
00274 
00283 VecPosition VecPosition::operator - ( const double &d )
00284 {
00285   return ( VecPosition( m_x - d, m_y - d ) );
00286 }
00287 
00296 VecPosition VecPosition::operator - ( const VecPosition &p )
00297 {
00298   return ( VecPosition( m_x - p.m_x, m_y - p.m_y ) );
00299 }
00300 
00308 VecPosition VecPosition::operator * ( const double &d  )
00309 {
00310   return ( VecPosition( m_x * d, m_y * d  ) );
00311 }
00312 
00320 VecPosition VecPosition::operator * ( const VecPosition &p )
00321 {
00322   return ( VecPosition( m_x * p.m_x, m_y * p.m_y ) );
00323 }
00324 
00333 VecPosition VecPosition::operator / ( const double &d )
00334 {
00335   return ( VecPosition( m_x / d, m_y / d  ) );
00336 }
00337 
00345 VecPosition VecPosition::operator / ( const VecPosition &p )
00346 {
00347   return ( VecPosition( m_x / p.m_x, m_y / p.m_y ) );
00348 }
00349 
00355 void VecPosition::operator = ( const double &d )
00356 {
00357   m_x = d;
00358   m_y = d;
00359 }
00360 
00366 void VecPosition::operator +=( const VecPosition &p )
00367 {
00368   m_x += p.m_x;
00369   m_y += p.m_y;
00370 }
00371 
00378 void VecPosition::operator += ( const double &d )
00379 {
00380   m_x += d;
00381   m_y += d;
00382 }
00383 
00391 void VecPosition::operator -=( const VecPosition &p )
00392 {
00393   m_x -= p.m_x;
00394   m_y -= p.m_y;
00395 }
00396 
00404 void VecPosition::operator -=( const double &d )
00405 {
00406   m_x -= d;
00407   m_y -= d;
00408 }
00409 
00417 void VecPosition::operator *=( const VecPosition &p )
00418 {
00419   m_x *= p.m_x;
00420   m_y *= p.m_y;
00421 }
00422 
00430 void VecPosition::operator *=( const double &d )
00431 {
00432   m_x *= d;
00433   m_y *= d;
00434 }
00435 
00442 void VecPosition::operator /=( const VecPosition &p )
00443 {
00444   m_x /= p.m_x;
00445   m_y /= p.m_y;
00446 }
00447 
00455 void VecPosition::operator /=( const double &d )
00456 {
00457   m_x /= d;
00458   m_y /= d;
00459 }
00460 
00468 bool VecPosition::operator !=( const VecPosition &p )
00469 {
00470   return ( ( m_x != p.m_x ) || ( m_y != p.m_y ) );
00471 }
00472 
00482 bool VecPosition::operator !=( const double &d )
00483 {
00484   return ( ( m_x != d ) || ( m_y != d ) );
00485 }
00486 
00495 bool VecPosition::operator ==( const VecPosition &p )
00496 {
00497   return ( ( m_x == p.m_x ) && ( m_y == p.m_y ) );
00498 }
00499 
00509 bool VecPosition::operator ==( const double &d )
00510 {
00511   return ( ( m_x == d ) && ( m_y == d ) );
00512 }
00513 
00522 ostream& operator <<( ostream &os, VecPosition v )
00523 {
00524   return ( os << "( " << v.m_x << ", " << v.m_y << " )" );
00525 }
00526 
00532 void VecPosition::show( CoordSystemT cs )
00533 {
00534   if( cs == CARTESIAN )
00535     cout << *this << endl;
00536   else
00537     cout << "( r: " << getMagnitude( ) << ", phi: " << getDirection( ) << "  )";
00538 }
00539 
00547 string VecPosition::str( CoordSystemT cs )
00548 {
00549   char buf[ 1024 ];
00550 
00551   if( cs == CARTESIAN )
00552     sprintf( buf, "( %f, %f )", getX( ), getY( ) );
00553   else
00554     sprintf( buf, "( r: %f, phi: %f )", getMagnitude( ), getDirection( ) );
00555 
00556   string str( buf );
00557   return ( str );
00558 }
00559 
00564 bool VecPosition::setX( double dX )
00565 {
00566   m_x = dX;
00567   return ( true );
00568 }
00569 
00573 double VecPosition::getX( ) const
00574 {
00575   return ( m_x );
00576 }
00577 
00582 bool VecPosition::setY( double dY )
00583 {
00584   m_y = dY;
00585   return ( true );
00586 }
00587 
00591 double VecPosition::getY( ) const
00592 {
00593   return ( m_y );
00594 }
00595 
00611 void VecPosition::setVecPosition( double dX, double dY, CoordSystemT cs)
00612 {
00613   if( cs == CARTESIAN )
00614   {
00615     m_x = dX;
00616     m_y = dY;
00617   }
00618   else
00619     *this = getVecPositionFromPolar( dX, dY );
00620 }
00621 
00630 double VecPosition::getDistanceTo( const VecPosition p )
00631 {
00632   return getDistanceTo(p.getX(), p.getY());
00633 }
00634 
00635 double VecPosition::getDistanceTo(double x, double y)
00636 {
00637   return sqrt( (m_x - x)*(m_x - x) + (m_y - y)*(m_y - y) );
00638 }
00639 
00652 VecPosition VecPosition::setMagnitude( double d )
00653 {
00654   if( getMagnitude( ) > EPSILON )
00655      ( *this ) *= ( d / getMagnitude( ) );
00656 
00657   return ( *this );
00658 }
00659 
00666 double VecPosition::getMagnitude( ) const
00667 {
00668   return ( sqrt( m_x * m_x + m_y * m_y ) );
00669 }
00670 
00679 AngDeg VecPosition::getDirection( ) const
00680 {
00681   return ( atan2Deg( m_y, m_x ) );
00682 }
00683 
00692 bool VecPosition::isInFrontOf( const VecPosition &p )
00693 {
00694   return ( ( m_x > p.getX( ) ) ? true : false );
00695 }
00696 
00706 bool VecPosition::isInFrontOf( const double &d )
00707 {
00708   return ( ( m_x > d ) ? true : false );
00709 }
00710 
00721 bool VecPosition::isBehindOf( const VecPosition &p )
00722 {
00723   return ( ( m_x < p.getX( ) ) ? true : false );
00724 }
00725 
00734 bool VecPosition::isBehindOf( const double &d )
00735 {
00736   return ( ( m_x < d ) ? true : false );
00737 }
00738 
00749 bool VecPosition::isLeftOf( const VecPosition &p )
00750 {
00751   return ( ( m_y < p.getY( ) ) ? true : false );
00752 }
00753 
00763 bool VecPosition::isLeftOf( const double &d )
00764 {
00765   return ( ( m_y < d ) ? true : false );
00766 }
00767 
00778 bool VecPosition::isRightOf( const VecPosition &p )
00779 {
00780   return ( ( m_y > p.getY( ) ) ? true : false );
00781 }
00782 
00792 bool VecPosition::isRightOf( const double &d )
00793 {
00794   return ( ( m_y > d ) ? true : false );
00795 }
00796 
00811 bool VecPosition::isBetweenX( const VecPosition &p1, const VecPosition &p2 )
00812 {
00813   return ( ( isInFrontOf( p1 ) && isBehindOf( p2 ) ) ? true : false );
00814 }
00815 
00829 bool VecPosition::isBetweenX( const double &d1, const double &d2 )
00830 {
00831   return ( ( isInFrontOf( d1 ) && isBehindOf( d2 ) ) ? true : false );
00832 }
00833 
00848 bool VecPosition::isBetweenY( const VecPosition &p1, const VecPosition &p2 )
00849 {
00850   return ( ( isRightOf( p1 ) && isLeftOf( p2 ) ) ? true : false );
00851 }
00852 
00866 bool VecPosition::isBetweenY( const double &d1, const double &d2 )
00867 {
00868   return ( ( isRightOf( d1 ) && isLeftOf( d2 ) ) ? true : false );
00869 }
00870 
00877 VecPosition VecPosition::normalize( )
00878 {
00879   return ( setMagnitude( 1.0 ) );
00880 }
00881 
00896 VecPosition VecPosition::rotate( AngDeg angle )
00897 {
00898   // determine the polar representation of the current VecPosition
00899   double dMag    = this->getMagnitude( );
00900   double dNewDir = this->getDirection( ) + angle;  // add rotation angle to phi
00901   setVecPosition( dMag, dNewDir, POLAR );          // convert back to Cartesian
00902   return ( *this );
00903 }
00904 
00922 VecPosition VecPosition::globalToRelative( VecPosition origin, AngDeg ang )
00923 {
00924   // convert global coordinates into relative coordinates by aligning
00925   // relative frame and world frame. First perform translation to make
00926   // origins of both frames coincide. Then perform rotation to make
00927   // axes of both frames coincide (use negative angle since you rotate
00928   // relative frame to world frame).
00929   *this -= origin;
00930   return ( rotate( -ang ) );
00931 }
00932 
00948 VecPosition VecPosition::relativeToGlobal( VecPosition origin, AngDeg ang )
00949 {
00950   // convert relative coordinates into global coordinates by aligning
00951   // world frame and relative frame. First perform rotation to make
00952   // axes of both frames coincide (use positive angle since you rotate
00953   // world frame to relative frame). Then perform translation to make
00954   // origins of both frames coincide.
00955   rotate( ang );
00956   *this += origin;
00957   return ( *this );
00958 }
00959 
00974 VecPosition VecPosition::getVecPositionOnLineFraction( VecPosition &p,
00975                                                        double      dFrac )
00976 {
00977   // determine point on line that lies at fraction dFrac of whole line
00978   // example: this --- 0.25 ---------  p
00979   // formula: this + dFrac * ( p - this ) = this - dFrac * this + dFrac * p =
00980   //          ( 1 - dFrac ) * this + dFrac * p
00981   return ( ( *this ) * ( 1.0 - dFrac ) + ( p * dFrac ) );
00982 }
00983 
00996 VecPosition VecPosition::getVecPositionFromPolar( double dMag, AngDeg ang )
00997 {
00998   // cos(phi) = x/r <=> x = r*cos(phi); sin(phi) = y/r <=> y = r*sin(phi)
00999   return ( VecPosition( dMag * cosDeg( ang ), dMag * sinDeg( ang ) ) );
01000 }
01001 
01008 AngDeg VecPosition::normalizeAngle( AngDeg angle )
01009 {
01010   while( angle > 180.0  ) angle -= 360.0;
01011   while( angle < -180.0 ) angle += 360.0;
01012 
01013   return ( angle );
01014 }
01015 
01016 double VecPosition::pointsDistance(double x1, double y1, double x2, double y2)
01017 {
01018     return sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
01019 }
01020 
01021 
01022 /*****************************************************************************/
01023 /********************** CLASS GEOMETRY ***************************************/
01024 /*****************************************************************************/
01025 
01036 double Geometry::getLengthGeomSeries( double dFirst, double dRatio, double dSum )
01037 {
01038   if( dRatio < 0 )
01039     cerr << "(Geometry:getLengthGeomSeries): negative ratio" << endl;
01040 
01041   // s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
01042   // subtract: sr - s = - a + ar^n) =>  s(1-r)/a + 1 = r^n = temp
01043   // log r^n / n = n log r / log r = n = length
01044   double temp = (dSum * ( dRatio - 1 ) / dFirst) + 1;
01045   if( temp <= 0 )
01046     return -1.0;
01047   return log( temp ) / log( dRatio ) ;
01048 }
01049 
01060 double Geometry::getSumGeomSeries( double dFirst, double dRatio, double dLength)
01061 {
01062   // s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
01063   // subtract: s - sr = a - ar^n) =>  s = a(1-r^n)/(1-r)
01064   return dFirst * ( 1 - pow( dRatio, dLength ) ) / ( 1 - dRatio ) ;
01065 }
01066 
01077 double Geometry::getSumInfGeomSeries( double dFirst, double dRatio )
01078 {
01079   if( dRatio > 1 )
01080     cerr << "(Geometry:CalcLengthGeomSeries): series does not converge" <<endl;
01081 
01082   // s = a(1-r^n)/(1-r) with n->inf and 0<r<1 => r^n = 0
01083   return dFirst / ( 1 - dRatio );
01084 }
01085 
01096 double Geometry::getFirstGeomSeries( double dSum, double dRatio, double dLength)
01097 {
01098   // s = a + ar + ar^2 + .. + ar^n-1 and thus sr = ar + ar^2 + .. + ar^n
01099   // subtract: s - sr = a - ar^n) =>  s = a(1-r^n)/(1-r) => a = s*(1-r)/(1-r^n)
01100   return dSum *  ( 1 - dRatio )/( 1 - pow( dRatio, dLength ) ) ;
01101 }
01102 
01113 double Geometry::getFirstInfGeomSeries( double dSum, double dRatio )
01114 {
01115   if( dRatio > 1 )
01116     cerr << "(Geometry:getFirstInfGeomSeries):series does not converge" <<endl;
01117 
01118   // s = a(1-r^n)/(1-r) with r->inf and 0<r<1 => r^n = 0 => a = s ( 1 - r)
01119   return dSum * ( 1 - dRatio );
01120 }
01121 
01131 int Geometry::abcFormula(double a, double b, double c, double *s1, double *s2)
01132 {
01133   double dDiscr = b*b - 4*a*c;       // discriminant is b^2 - 4*a*c
01134   if (fabs(dDiscr) < EPSILON )       // if discriminant = 0
01135   {
01136     *s1 = -b / (2 * a);              //  only one solution
01137     return 1;
01138   }
01139   else if (dDiscr < 0)               // if discriminant < 0
01140     return 0;                        //  no solutions
01141   else                               // if discriminant > 0
01142   {
01143     dDiscr = sqrt(dDiscr);           //  two solutions
01144     *s1 = (-b + dDiscr ) / (2 * a);
01145     *s2 = (-b - dDiscr ) / (2 * a);
01146     return 2;
01147   }
01148 }
01149 
01150 /*****************************************************************************/
01151 /********************* CLASS CIRCLE ******************************************/
01152 /*****************************************************************************/
01153 
01158 Circle::Circle( VecPosition pos, double dR )
01159 {
01160   setCircle( pos, dR );
01161 }
01162 
01165 Circle::Circle( )
01166 {
01167   setCircle( VecPosition(-1000.0,-1000.0), 0);
01168 }
01169 
01174 void Circle::show( ostream& os)
01175 {
01176   os << "c:" << m_posCenter << ", r:" << m_dRadius;
01177 }
01178 
01184 bool Circle::setCircle( VecPosition pos, double dR )
01185 {
01186   setCenter( pos );
01187   return setRadius( dR  );
01188 }
01192 bool Circle::setRadius( double dR )
01193 {
01194   if( dR > 0 )
01195   {
01196     m_dRadius = dR;
01197     return true;
01198   }
01199   else
01200   {
01201     m_dRadius = 0.0;
01202     return false;
01203   }
01204 }
01205 
01208 double Circle::getRadius()
01209 {
01210   return m_dRadius;
01211 }
01212 
01216 bool Circle::setCenter( VecPosition pos )
01217 {
01218   m_posCenter = pos;
01219   return true;
01220 }
01221 
01224 VecPosition Circle::getCenter()
01225 {
01226   return m_posCenter;
01227 }
01228 
01231 double Circle::getCircumference()
01232 {
01233   return 2.0*M_PI*getRadius();
01234 }
01235 
01238 double Circle::getArea()
01239 {
01240   return M_PI*getRadius()*getRadius();
01241 }
01242 
01250 bool Circle::isInside( VecPosition pos )
01251 {
01252   return m_posCenter.getDistanceTo( pos ) < getRadius() ;
01253 }
01254 
01261 int Circle::getIntersectionPoints( Circle c, VecPosition *p1, VecPosition *p2)
01262 {
01263     double x0, y0, r0;
01264     double x1, y1, r1;
01265 
01266     x0 = getCenter( ).getX();
01267     y0 = getCenter( ).getY();
01268     r0 = getRadius( );
01269     x1 = c.getCenter( ).getX();
01270     y1 = c.getCenter( ).getY();
01271     r1 = c.getRadius( );
01272 
01273     double      d, dx, dy, h, a, x, y, p2_x, p2_y;
01274 
01275     // first calculate distance between two centers circles P0 and P1.
01276     dx = x1 - x0;
01277     dy = y1 - y0;
01278     d = sqrt(dx*dx + dy*dy);
01279 
01280     // normalize differences
01281     dx /= d; dy /= d;
01282 
01283     // a is distance between p0 and point that is the intersection point P2
01284     // that intersects P0-P1 and the line that crosses the two intersection
01285     // points P3 and P4.
01286     // Define two triangles: P0,P2,P3 and P1,P2,P3.
01287     // with distances a, h, r0 and b, h, r1 with d = a + b
01288     // We know a^2 + h^2 = r0^2 and b^2 + h^2 = r1^2 which then gives
01289     // a^2 + r1^2 - b^2 = r0^2 with d = a + b ==> a^2 + r1^2 - (d-a)^2 = r0^2
01290     // ==> r0^2 + d^2 - r1^2 / 2*d
01291     a = (r0*r0 + d*d - r1*r1) / (2.0 * d);
01292 
01293     // h is then a^2 + h^2 = r0^2 ==> h = sqrt( r0^2 - a^2 )
01294     double      arg = r0*r0 - a*a;
01295     h = (arg > 0.0) ? sqrt(arg) : 0.0;
01296 
01297     // First calculate P2
01298     p2_x = x0 + a * dx;
01299     p2_y = y0 + a * dy;
01300 
01301     // and finally the two intersection points
01302     x =  p2_x - h * dy;
01303     y =  p2_y + h * dx;
01304     p1->setVecPosition( x, y );
01305     x =  p2_x + h * dy;
01306     y =  p2_y - h * dx;
01307     p2->setVecPosition( x, y );
01308 
01309     return (arg < 0.0) ? 0 : ((arg == 0.0 ) ? 1 :  2);
01310 }
01311 
01315 double Circle::getIntersectionArea( Circle c )
01316 {
01317   VecPosition pos1, pos2, pos3;
01318   double d, h, dArea;
01319   AngDeg ang;
01320 
01321   d = getCenter().getDistanceTo( c.getCenter() ); // dist between two centers
01322   if( d > c.getRadius() + getRadius() )           // larger than sum radii
01323     return 0.0;                                   // circles do not intersect
01324   if( d <= fabs(c.getRadius() - getRadius() ) )   // one totally in the other
01325   {
01326     double dR = min( c.getRadius(), getRadius() );// return area smallest circ
01327     return M_PI*dR*dR;
01328   }
01329 
01330   int iNrSol = getIntersectionPoints( c, &pos1, &pos2 );
01331   if( iNrSol != 2 )
01332     return 0.0;
01333 
01334   // the intersection area of two circles can be divided into two segments:
01335   // left and right of the line between the two intersection points p1 and p2.
01336   // The outside area of each segment can be calculated by taking the part
01337   // of the circle pie excluding the triangle from the center to the
01338   // two intersection points.
01339   // The pie equals pi*r^2 * rad(2*ang) / 2*pi = 0.5*rad(2*ang)*r^2 with ang
01340   // the angle between the center c of the circle and one of the two
01341   // intersection points. Thus the angle between c and p1 and c and p3 where
01342   // p3 is the point that lies halfway between p1 and p2.
01343   // This can be calculated using ang = asin( d / r ) with d the distance
01344   // between p1 and p3 and r the radius of the circle.
01345   // The area of the triangle is 2*0.5*h*d.
01346 
01347   pos3 = pos1.getVecPositionOnLineFraction( pos2, 0.5 );
01348   d = pos1.getDistanceTo( pos3 );
01349   h = pos3.getDistanceTo( getCenter() );
01350   ang = asin( d / getRadius() );
01351 
01352   dArea = ang*getRadius()*getRadius();
01353   dArea = dArea - d*h;
01354 
01355   // and now for the other segment the same story
01356   h = pos3.getDistanceTo( c.getCenter() );
01357   ang = asin( d / c.getRadius() );
01358   dArea = dArea + ang*c.getRadius()*c.getRadius();
01359   dArea = dArea - d*h;
01360 
01361   return dArea;
01362 }
01363 
01364 
01365 /*****************************************************************************/
01366 /***********************  CLASS LINE *****************************************/
01367 /*****************************************************************************/
01368 
01374 Line::Line( double dA, double dB, double dC )
01375 {
01376   m_a = dA;
01377   m_b = dB;
01378   m_c = dC;
01379 }
01380 
01386 ostream& operator <<(ostream & os, Line l)
01387 {
01388   double a = l.getACoefficient();
01389   double b = l.getBCoefficient();
01390   double c = l.getCCoefficient();
01391 
01392   // ay + bx + c = 0 -> y = -b/a x - c/a
01393   if( a == 0 )
01394     os << "x = " << -c/b;
01395   else
01396   {
01397     os << "y = ";
01398     if( b != 0 )
01399       os << -b/a << "x ";
01400     if( c > 0 )
01401        os << "- " <<  fabs(c/a);
01402     else if( c < 0 )
01403        os << "+ " <<  fabs(c/a);
01404   }
01405   return os;
01406 }
01407 
01410 void Line::show( ostream& os)
01411 {
01412   os << *this;
01413 }
01414 
01419 VecPosition Line::getIntersection( Line line )
01420 {
01421   VecPosition pos;
01422   double x, y;
01423   if( ( m_a / m_b ) ==  (line.getACoefficient() / line.getBCoefficient() ))
01424     return pos; // lines are parallel, no intersection
01425   if( m_a == 0 )            // bx + c = 0 and a2*y + b2*x + c2 = 0 ==> x = -c/b
01426   {                          // calculate x using the current line
01427     x = -m_c/m_b;                // and calculate the y using the second line
01428     y = line.getYGivenX(x);
01429   }
01430   else if( line.getACoefficient() == 0 )
01431   {                         // ay + bx + c = 0 and b2*x + c2 = 0 ==> x = -c2/b2
01432    x = -line.getCCoefficient()/line.getBCoefficient(); // calculate x using
01433    y = getYGivenX(x);       // 2nd line and calculate y using current line
01434   }
01435   // ay + bx + c = 0 and a2y + b2*x + c2 = 0
01436   // y = (-b2/a2)x - c2/a2
01437   // bx = -a*y - c =>  bx = -a*(-b2/a2)x -a*(-c2/a2) - c ==>
01438   // ==> a2*bx = a*b2*x + a*c2 - a2*c ==> x = (a*c2 - a2*c)/(a2*b - a*b2)
01439   // calculate x using the above formula and the y using the current line
01440   else
01441   {
01442     x = (m_a*line.getCCoefficient() - line.getACoefficient()*m_c)/
01443                     (line.getACoefficient()*m_b - m_a*line.getBCoefficient());
01444     y = getYGivenX(x);
01445   }
01446 
01447   return VecPosition( x, y );
01448 }
01449 
01450 
01458 int Line::getCircleIntersectionPoints( Circle circle,
01459               VecPosition *posSolution1, VecPosition *posSolution2 )
01460 {
01461   int    iSol;
01462   double dSol1=0, dSol2=0;
01463   double h = circle.getCenter().getX();
01464   double k = circle.getCenter().getY();
01465 
01466   // line:   x = -c/b (if a = 0)
01467   // circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y
01468   // fill in:(-c/b-h)^2 + y^2 -2ky + k^2 - r^2 = 0
01469   //         y^2 -2ky + (-c/b-h)^2 + k^2 - r^2 = 0
01470   // and determine solutions for y using abc-formula
01471   if( fabs(m_a) < EPSILON )
01472   {
01473     iSol = Geometry::abcFormula( 1, -2*k, ((-m_c/m_b) - h)*((-m_c/m_b) - h)
01474               + k*k - circle.getRadius()*circle.getRadius(), &dSol1, &dSol2);
01475     posSolution1->setVecPosition( (-m_c/m_b), dSol1 );
01476     posSolution2->setVecPosition( (-m_c/m_b), dSol2 );
01477     return iSol;
01478   }
01479 
01480   // ay + bx + c = 0 => y = -b/a x - c/a, with da = -b/a and db = -c/a
01481   // circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y
01482   // fill in:x^2 -2hx + h^2 + (da*x-db)^2 -2k(da*x-db) + k^2 - r^2 = 0
01483   //         x^2 -2hx + h^2 + da^2*x^2 + 2da*db*x + db^2 -2k*da*x -2k*db
01484   //                                                         + k^2 - r^2 = 0
01485   //       (1+da^2)*x^2 + 2(da*db-h-k*da)*x + h2 + db^2  -2k*db + k^2 - r^2 = 0
01486   // and determine solutions for x using abc-formula
01487   // fill in x in original line equation to get y coordinate
01488   double da = -m_b/m_a;
01489   double db = -m_c/m_a;
01490 
01491   double dA = 1 + da*da;
01492   double dB = 2*( da*db - h - k*da );
01493   double dC = h*h + db*db-2*k*db + k*k - circle.getRadius()*circle.getRadius();
01494 
01495   iSol = Geometry::abcFormula( dA, dB, dC, &dSol1, &dSol2 );
01496 
01497   posSolution1->setVecPosition( dSol1, da*dSol1 + db );
01498   posSolution2->setVecPosition( dSol2, da*dSol2 + db );
01499   return iSol;
01500 
01501 }
01502 
01508 Line Line::getTangentLine( VecPosition pos )
01509 {
01510   // ay + bx + c = 0 -> y = (-b/a)x + (-c/a)
01511   // tangent: y = (a/b)*x + C1 -> by - ax + C2 = 0 => C2 = ax - by
01512   // with pos.y = y, pos.x = x
01513   return Line( m_b, -m_a, m_a*pos.getX() - m_b*pos.getY() );
01514 }
01515 
01519 VecPosition Line::getPointOnLineClosestTo( VecPosition pos )
01520 {
01521   Line l2 = getTangentLine( pos );  // get tangent line
01522   return getIntersection( l2 );     // and intersection between the two lines
01523 }
01524 
01529 double Line::getDistanceWithPoint( VecPosition pos )
01530 {
01531   return pos.getDistanceTo( getPointOnLineClosestTo( pos ) );
01532 }
01533 
01542 bool Line::isInBetween( VecPosition pos, VecPosition point1,VecPosition point2)
01543 {
01544   pos          = getPointOnLineClosestTo( pos ); // get closest point
01545   double dDist = point1.getDistanceTo( point2 ); // get distance between 2 pos
01546 
01547   // if the distance from both points to the projection is smaller than this
01548   // dist, the pos lies in between.
01549   return pos.getDistanceTo( point1 ) <= dDist &&
01550          pos.getDistanceTo( point2 ) <= dDist;
01551 }
01552 
01556 double Line::getYGivenX( double x )
01557 {
01558  if( m_a == 0 )
01559  {
01560    cerr << "(Line::getYGivenX) Cannot calculate Y coordinate: " ;
01561    show( cerr );
01562    cerr << endl;
01563    return 0;
01564  }
01565   // ay + bx + c = 0 ==> ay = -(b*x + c)/a
01566   return -(m_b*x+m_c)/m_a;
01567 }
01568 
01572 double Line::getXGivenY( double y )
01573 {
01574  if( m_b == 0 )
01575  {
01576    cerr << "(Line::getXGivenY) Cannot calculate X coordinate\n" ;
01577    return 0;
01578  }
01579   // ay + bx + c = 0 ==> bx = -(a*y + c)/a
01580   return -(m_a*y+m_c)/m_b;
01581 }
01582 
01587 Line Line::makeLineFromTwoPoints( VecPosition pos1, VecPosition pos2 )
01588 {
01589   // 1*y + bx + c = 0 => y = -bx - c
01590   // with -b the direction coefficient (or slope)
01591   // and c = - y - bx
01592   double dA, dB, dC;
01593   double dTemp = pos2.getX() - pos1.getX(); // determine the slope
01594   if( fabs(dTemp) < EPSILON )
01595   {
01596     // ay + bx + c = 0 with vertical slope=> a = 0, b = 1
01597     dA = 0.0;
01598     dB = 1.0;
01599   }
01600   else
01601   {
01602     // y = (-b)x -c with -b the slope of the line
01603     dA = 1.0;
01604     dB = -(pos2.getY() - pos1.getY())/dTemp;
01605   }
01606   // ay + bx + c = 0 ==> c = -a*y - b*x
01607   dC =  - dA*pos2.getY()  - dB * pos2.getX();
01608   return Line( dA, dB, dC );
01609 }
01610 
01615 Line Line::makeLineFromPositionAndAngle( VecPosition vec, AngDeg angle )
01616 {
01617   // calculate point somewhat further in direction 'angle' and make
01618   // line from these two points.
01619   return makeLineFromTwoPoints( vec, vec+VecPosition(1,angle,POLAR));
01620 }
01621 
01624 double Line::getACoefficient() const
01625 {
01626   return m_a;
01627 }
01628 
01631 double Line::getBCoefficient() const
01632 {
01633  return m_b;
01634 }
01635 
01638 double Line::getCCoefficient() const
01639 {
01640  return m_c;
01641 }
01642 
01643 /*****************************************************************************/
01644 /********************* CLASS RECTANGLE ***************************************/
01645 /*****************************************************************************/
01646 
01653 Rect::Rect( VecPosition pos, VecPosition pos2 )
01654 {
01655   setRectanglePoints( pos, pos2 );
01656 }
01657 
01662 void Rect::setRectanglePoints( VecPosition pos1, VecPosition pos2 )
01663 {
01664   m_posLeftTop.setX    ( max( pos1.getX(), pos2.getX() ) );
01665   m_posLeftTop.setY    ( min( pos1.getY(), pos2.getY() ) );
01666   m_posRightBottom.setX( min( pos1.getX(), pos2.getX() ) );
01667   m_posRightBottom.setY( max( pos1.getY(), pos2.getY() ) );
01668 }
01669 
01673 void Rect::show( ostream& os )
01674 {
01675   os << "rect(" << m_posLeftTop << " " << m_posRightBottom << ")";
01676 }
01677 
01682 bool Rect::isInside( VecPosition pos )
01683 {
01684   return pos.isBetweenX( m_posRightBottom.getX(), m_posLeftTop.getX() ) &&
01685          pos.isBetweenY( m_posLeftTop.getY(),     m_posRightBottom.getY() );
01686 
01687 }
01688 
01692 bool Rect::setPosLeftTop( VecPosition pos )
01693 {
01694   m_posLeftTop = pos;
01695   return true;
01696 }
01697 
01700 VecPosition Rect::getPosLeftTop(  )
01701 {
01702   return m_posLeftTop;
01703 }
01704 
01708 bool Rect::setPosRightBottom( VecPosition pos )
01709 {
01710   m_posRightBottom = pos;
01711   return true;
01712 }
01713 
01716 VecPosition Rect::getPosRightBottom(  )
01717 {
01718   return m_posRightBottom;
01719 }
01720 
01721 /*****************************************************************************/
01722 /********************** TESTING PURPOSES *************************************/
01723 /*****************************************************************************/
01724 
01725 /*
01726 #include<iostream.h>
01727 
01728 int main( void )
01729 {
01730   double dFirst = 1.0;
01731   double dRatio = 2.5;
01732   double dSum   = 63.4375;
01733   double dLength = 4.0;
01734 
01735   printf( "sum: %f\n", Geometry::getSumGeomSeries( dFirst, dRatio, dLength));
01736   printf( "length: %f\n", Geometry::getLengthGeomSeries( dFirst, dRatio, dSum));
01737 }
01738 
01739 int main( void )
01740 {
01741   Line l1(1,-1,3 );
01742   Line l2(1,-0.2,10 );
01743  Line l3 = Line::makeLineFromTwoPoints( VecPosition(1,-1), VecPosition(2,-2) );
01744  l3.show();
01745  cout << endl;
01746  l1.show();
01747  l2.show();
01748   l1.getIntersection( l2 ).show();
01749 }
01750 
01751 
01752 int main( void )
01753 {
01754   Line l( 1, -1, 0 );
01755   VecPosition s1, s2;
01756   int i = l.getCircleIntersectionPoints( Circle( VecPosition(1,1),1) &s1,&s2 );
01757   printf( "number of solutions: %d\n", i );
01758   if( i == 2 )
01759   {
01760     cout << s1 << " " << s2 ;
01761   }
01762   else if( i == 1 )
01763   {
01764     cout << s1;
01765   }
01766   cout << "line: " << l;
01767 }
01768 
01769 int main( void )
01770 {
01771   Circle c11( VecPosition( 10, 0 ), 10);
01772   Circle c12( VecPosition( 40, 3 ), 40 );
01773   Circle c21( VecPosition(  0,0 ), 5);
01774   Circle c22( VecPosition(  3,0 ), 40 );
01775 
01776   VecPosition p1, p2;
01777 
01778   cout << c11.getIntersectionArea( c21 ) << endl;
01779   cout << c12.getIntersectionArea( c21 ) << endl;
01780   cout << c22.getIntersectionArea( c11 ) << endl;
01781   cout << c12.getIntersectionArea( c22 ) << endl;
01782   return 0;
01783 }
01784 
01785 int main( void )
01786 {
01787   cout << getBisectorTwoAngles( -155.3, 179.0 ) << endl;
01788   cout << getBisectorTwoAngles( -179.3, 179.0 ) << endl;
01789 }
01790 */

Generated on Thu Apr 26 22:45:27 2007 for GangOfSix(GOS)-RoboCupTeamProject by  doxygen 1.5.1-p1