raytrace.cc

一个机器人平台

/*
 *  Stage : a multi-robot simulator.
 *  Copyright (C) 2001, 2002 Richard Vaughan, Andrew Howard and Brian Gerkey.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
// ==================================================================
// Filename:	raytrace.cc
// $Id: raytrace.cc,v 1.10 2002/11/01 19:12:30 rtv Exp $
// RTV
// ==================================================================

#include "raytrace.hh"

extern CWorld* world;

CEntity* g_nullp = 0;

CLineIterator::CLineIterator( double x, double y, double a, double b, 
			      double ppm, CMatrix* matrix, LineIteratorMode pmode )
{   
  m_matrix = matrix;
  m_ppm = ppm;       
  
  m_x = x * m_ppm;
  m_y = y * m_ppm;

  m_ent = 0;
  m_index = 0;

  switch( pmode )
  {
    case PointToBearingRange:
    {
      double range = b;
      double bearing = a;
	
      m_angle = bearing;
      m_max_range = m_remaining_range = range * m_ppm;
    }
    break;
    case PointToPoint:
    {
      double x1 = a;
      double y1 = b;
	
      //printf( "From %.2f,%.2f to %.2f,%.2f\n", x,y,x1,y1 );

      m_angle = atan2( y1-y, x1-x );
      m_max_range = m_remaining_range = hypot( x1-x, y1-y ) * m_ppm;
    }
    break;
    default:
      puts( "Stage Warning: unknown LineIterator mode" );
  }

  //printf( "angle = %.2f remaining_range = %.2f\n", m_angle, m_remaining_range );
  //fflush( stdout );
};


CEntity* CLineIterator::GetNextEntity( void )
{
  //PrintArray( m_ent );

  if( m_remaining_range <= 0 ) return 0;

  //printf( "current ptr: %p index: %d\n", m_ent[m_index], index );

  if( !(m_ent && m_ent[m_index]) ) // if the ent array is null or empty
    {
      //printf( "before %d,%d\n", (int)m_x, (int)m_y );
      // get a new array of pointers
      m_ent = RayTrace( m_x, m_y, m_angle, m_remaining_range );
      //PrintArray( m_ent );
      //printf( "after %d,%d\n", (int)m_x, (int)m_y );
      m_index = 0; // back to the start of the array
    }
  
  assert( m_ent != 0 ); // should be a valid array, even if empty
 
  //PrintArray( m_ent );
  //printf( "returning %p (index: %d) at: %d %d rng: %d\n",  
  //  m_ent[m_index], m_index, (int)m_x, (int)m_y, (int)m_remaining_range );

  return m_ent[m_index++]; // return the next CEntity* (it may be null)
}

inline CEntity** CLineIterator::RayTrace( double &px, double &py, double pth, 
				   double &remaining_range )
{
  int range = 0;
  
  //printf( "Ray from %.2f,%.2f angle: %.2f remaining_range %.2f\n", 
  //  px, py, RTOD(pth), remaining_range );
  
  // hops along each axis 1 cell at a time
  double cospth = cos( pth );
  double sinpth = sin( pth );
  
  CEntity** ent = 0;
  
  // Look along scan line for obstacles
  for( range = 0; range < remaining_range; range++ )
  {
    px += cospth;
    py += sinpth;
      
    // stop this ray if we're out of bounds
    if( px < 0 ) 
    { 
      px = 0; 
      remaining_range = 0.0;
    }
    if( px >= m_matrix->width ) 
    { 
      px = m_matrix->width-1; 
      remaining_range = 0.0;
    }
    if( py < 0 ) 
    { 
      py = 0; 
      remaining_range = 0.0;
    }
    if( py >= m_matrix->height ) 
    { 
      py = m_matrix->height-1; 
      remaining_range = 0.0;
    }
      
    //printf( "looking in %d,%d\n", (int)px, (int)py );
      
    ent = m_matrix->get_cell( (int)px,(int)py );
    if( !ent[0] && px+1 < m_matrix->width) 
      ent = m_matrix->get_cell( (int)px+1,(int)py );
      
	  if( ent[0] ) break;// we hit something!
  }
  
  remaining_range -= (double)range+1; // we have this much left to go
  
  //if( ent && ent[0] )
  //printf( "Hit %p (%d) at %.2f,%.2f with %.2f to go\n", 
  //    ent[0], ent[0]->type_num, px, py, remaining_range );
  //else
  //printf( "hit nothing. remaining range = %.2f\n", remaining_range );
  // fflush( stdout );
  
  return ent; // we hit these entities
}; 

void CLineIterator::PrintArray( CEntity** ent )
{
  printf( "Array: " );
  
  if( !ent ) 
  {
    printf( "(null)\n" ); 
    return;
  }
  
  //puts( "foo" );

  printf( "[ " );
  int p = 0;
  while( ent[p] ) printf( "%p ", (ent[p++]) );
  
  printf( " ]\n" );
  
  //fflush( stdout );
}

// an isoceles triangle is specified from it's point, by the bearing
// and length of it's left side, and by the angle between the equal
// sides. the triangle is scanned from point to base in increasingly
// long lines perpendicular to the base. skip give the distance
// increment between scan lines.
CTriangleAreaIterator::
CTriangleAreaIterator( double x, double y, 
		       double bearing, double range, double angle,
		       double skip,
		       double ppm, CMatrix* matrix )
{
  //printf( "new CTriangleAreaIterator( %.2f, %.2f, %.2f, %.2f, %.2f )\n",
  //  x, y, bearing, range, angle, skip ); 
  
  m_matrix = matrix;
  m_ppm = ppm;       
  
  m_x = x;
  m_y = y;
  m_angle = angle;

  m_bearing = bearing;

  m_skip = skip;
  m_max_range = range;
  m_range_so_far = 0; 


  // the angle of the lines to iterate over is 
  // perpendicular to the center line of the triangle.
  m_li_angle = (m_bearing -angle/2.0) - M_PI_2;

  li = 0; // initially null line iterator

  assert( m_skip > 0 );
}

CTriangleAreaIterator::~CTriangleAreaIterator( void )
{
  if( li ) delete li;
}

CEntity* CTriangleAreaIterator::GetNextEntity( void )
{
  CEntity* ent;

  while( m_range_so_far < m_max_range )
  {
    // if there's an iterator and it's still working, use it.
    if( li && (ent = li->GetNextEntity()))
      return ent;
      
    // otherwise we need to create a new iterator
    // after deleting the old one of course
    if( li ) delete li; 
      
    //printf( "x: %.2f y: %.2f + skip: %.2f = ", m_x, m_y, m_skip );

    // move from the current position down the left edge of the triangle
    m_x += m_skip * cos(m_bearing);
    m_y += m_skip * sin(m_bearing);

    //printf( "x: %.2f y: %.2f ", m_x, m_y );
      
    m_range_so_far += m_skip;
      
    //printf( "range so far: %.2f\n", m_range_so_far );
    
    // figure the length of the scan line
    double li_len = m_range_so_far * sin( m_angle );
      
    //printf( "new CLineIterator( %.2f, %.2f, %.2f, %.2f )\n",
    //      m_x, m_y, m_li_angle, li_len ); 
      

    li = new CLineIterator( m_x, m_y, m_li_angle, li_len, 
                            m_ppm, m_matrix, PointToBearingRange );
  }
  return 0;
}

double CTriangleAreaIterator::GetRange( void )
{
  // find the height of the triangle so far
  return( m_range_so_far * cos( m_angle/2.0 ) );
}


CRectangleIterator::CRectangleIterator( double x, double y, double th,
					double w, double h,  
					double ppm, CMatrix* matrix )
{
  // calculate the corners of our body

  double cx = (w/2.0) * cos(th);
  double cy = (h/2.0) * cos(th);
  double sx = (w/2.0) * sin(th);
  double sy = (h/2.0) * sin(th);
  
  corners[0][0] = x + cx - sy;
  corners[0][1] = y + sx + cy;
    
  corners[1][0] = x - cx - sy;
  corners[1][1] = y - sx + cy;
    
  corners[2][0] = x - cx + sy;
  corners[2][1] = y - sx - cy;
   
  corners[3][0] = x + cx + sy;
  corners[3][1] = y + sx - cy;
  
  
  lits[0] = new CLineIterator( corners[0][0], corners[0][1], 
                               corners[1][0], corners[1][1],
                               ppm, matrix, PointToPoint );
  
  lits[1] = new CLineIterator( corners[1][0], corners[1][1], 
                               corners[2][0], corners[2][1],
                               ppm, matrix, PointToPoint );

  lits[2] = new CLineIterator( corners[2][0], corners[2][1], 
                               corners[3][0], corners[3][1],
                               ppm, matrix, PointToPoint );
  
  lits[3] = new CLineIterator( corners[3][0], corners[3][1], 
                               corners[0][0], corners[0][1],
                               ppm, matrix, PointToPoint );
  
} 

CRectangleIterator::~CRectangleIterator( void )
{
  for( int f=0; f<4; f++ )
    if( lits[f] ) delete lits[f];
}

CEntity* CRectangleIterator::GetNextEntity( void )
{
  CEntity* ent = 0;
  
  for( int i=0; i<4; i++ )
  {
    if (!lits[i])
      continue;
    if( (ent = lits[i]->GetNextEntity()) == 0 )
    {
      delete lits[i]; // don't need that line any more
      lits[i] = 0; // mark it as deleted
    }
    else
      break; // we'll take this entity
  }
  
  return ent;
}


// a circle iterator is an arc iterator with a 2PI scan range
CCircleIterator::CCircleIterator( double x, double y, double r, 
				  double ppm, CMatrix* matrix )
  : CArcIterator( x, y, 0, r, M_PI*2.0, ppm, matrix )
{
}

CArcIterator::CArcIterator( double x, double y, double th,
			    double scan_r, double scan_th, 
			      double ppm, CMatrix* matrix )
{   
  m_matrix = matrix;
  m_ppm = ppm;       

  m_center_x = x * m_ppm;
  m_center_y = y * m_ppm;
  m_angle = th - scan_th/2.0;

  m_radius = scan_r * m_ppm;
  m_max_angle = m_angle + scan_th; // this is the angle length of the scan in radians

  m_ent = 0;
  m_index = 0;

  m_px = 0;
  m_py = 0;

  // the angle increment - moves us by 1 pixel
  m_dangle = atan2( 1, m_radius );

  //printf( "Construct circle iterator: x=%.2f y=%.2f r=%.2f a=%.2f dangle=%.2f \n",
  //  m_center_x, m_center_y, m_radius, m_angle, m_dangle );

};


CEntity* CArcIterator::GetNextEntity( void )
{
  if( m_angle >= m_max_angle ) return 0; // done!
  
  //printf( "current ptr: %p index: %d\n", m_ent[m_index], index );

  if( !(m_ent && m_ent[m_index]) ) // if the ent array is null or empty
  {
    //printf( "before %d,%d\n", (int)m_x, (int)m_y );
    // get a new array of pointers
    m_ent = ArcTrace( m_angle );
    //PrintArray( m_ent );
    //printf( "after %d,%d\n", (int)m_x, (int)m_y );
    m_index = 0; // back to the start of the array
  }
  
  assert( m_ent != 0 ); // should be a valid array, even if empty
 
  //PrintArray( m_ent );
  //printf( "returning %p (index: %d) at: %d %d rng: %d\n",  
  //m_ent[m_index], m_index, (int)m_x, (int)m_y, (int)m_angle );

  return m_ent[m_index++]; // return the next CEntity* (it may be null)  
}


CEntity** CArcIterator::ArcTrace( double &remaining_angle )
{
  CEntity** ent = 0;
   
  while( remaining_angle < m_max_angle )
  { 
    m_px = m_center_x + m_radius * cos( remaining_angle );
    m_py = m_center_y + m_radius * sin( remaining_angle );
     
    //printf( "looking in %d,%d  angle: %.2f\n", 
    //     (int)px, (int)py, remaining_angle );
     
    remaining_angle += m_dangle;
     
    // bounds check
    if( m_px < 0 || m_px >= m_matrix->width-1 || 
        m_py < 0 || m_py >= m_matrix->height ) 
    {
      // if we're out of bounds, then return a pointer to NULL,
      // indicating free space
      ent = &g_nullp;
    }
    else
    {
      ent = m_matrix->get_cell( (int)m_px,(int)m_py );
      if( !ent[0] )//&& (px+1 < m_matrix->width) ) 
        ent = m_matrix->get_cell( (int)m_px+1,(int)m_py );
    }

    if( ent[0] ) break;// we hit something!
  }
 
  //if( ent && ent[0] )
  //printf( "Hit %p at %.2f,%.2f with %.2f to go\n", 
  //   ent[0], px, py, remaining_angle );
  //else
  //printf( "hit nothing. remaining angle = %.2f\n", remaining_angle );
  //fflush( stdout );
 
  return ent; // we hit these entities
}

void CArcIterator::PrintArray( CEntity** ent )
{
  printf( "Array: " );
  
  if( !ent ) 
  {
    printf( "(null)\n" ); 
    return;
  }
  
  //puts( "foo" );
  
  printf( "[ " );
  int p = 0;
  while( ent[p] ) printf( "%p ", (ent[p++]) );
  
  printf( " ]\n" );
  
  //fflush( stdout );
}