basic/geometry/geo_rangesensor/Range_Sensor.cpp

Go to the documentation of this file.

// Range_Sensor
#include "Range_Sensor.h"

// Ray Tracing Specific Headers
#include "raytrace.h"
#include "MgcCylinder.h"
#include "MgcDistLin3Lin3.h"
#include "MgcIntrLin3Cyln.h"
#include "MgcLine3.h"
#include "MgcMath.h"
#include "MgcRay3.h"
#include "MgcSegment3.h"
#include "MgcVector3.h"
#include "MgcRTLib.h"
#include "MgcBox3.h"
#include "MgcFrustum.h"
#include "MgcIntrBox3Frustum.h"
#include "MgcIntrLin3Box3.h"
#include "MgcContBox.h"
#include "MgcGaussPointsFit.h"
#include "MgcEigen.h"

// Class Range_Sensor 

Range_Sensor::Range_Sensor (FrameManager* frameManager)
: ObjectBase( frameManager )
{
  //TRACE("\nRange_Sensor::Range_Sensor");
  this->angle = 0.785;
  this->maxrange = 125.0;
  this->maxerror = 0;
  this->user_ray_range_y = 256;
  this->user_ray_range_z = 256;
  this->DisplayingBitmapEnabled = true;
  
  //Initially camera is at theFrame, and theFrame is identity matrix
  
  bp = NULL;
  amDisplayingBitmap = false;

  //set these basic vars at construction time
  lower_ray_lim_y = RAY_ARRAY_LIMIT/2-user_ray_range_y/2+1;
  upper_ray_lim_y = RAY_ARRAY_LIMIT/2+user_ray_range_y/2;

  lower_ray_lim_z = RAY_ARRAY_LIMIT/2-user_ray_range_z/2+1;
  upper_ray_lim_z = RAY_ARRAY_LIMIT/2+user_ray_range_z/2;

  ray_half_limit = RAY_ARRAY_LIMIT/2;

  //theFrame is pos of camera relative to link frame in GetTransform()
  absFrame = GetTransform() * theFrame;

}

Range_Sensor::Range_Sensor (const Range_Sensor& right)
:       ObjectBase( right )
{
  //TRACE("\nRange_Sensor::Range_Sensor (const Range_Sensor& right)???");
  this->angle = right.angle;
  this->maxrange = right.maxrange;
  this->maxerror = right.maxerror;
  this->user_ray_range_y = right.user_ray_range_y;
  this->user_ray_range_z = right.user_ray_range_z;
  this->DisplayingBitmapEnabled = right.DisplayingBitmapEnabled;
  
  //new cameras won't get a copy of the 2D window until it is regenerated by this camera object
  amDisplayingBitmap = false;
  
  //set these basic vars at construction time
  lower_ray_lim_y = RAY_ARRAY_LIMIT/2-user_ray_range_y/2+1;
  upper_ray_lim_y = RAY_ARRAY_LIMIT/2+user_ray_range_y/2;

  lower_ray_lim_z = RAY_ARRAY_LIMIT/2-user_ray_range_z/2+1;
  upper_ray_lim_z = RAY_ARRAY_LIMIT/2+user_ray_range_z/2;

  ray_half_limit = RAY_ARRAY_LIMIT/2;

  //theFrame is already copied by the ObjectBase constructor.
  //this->theFrame = right.theFrame;

  //theFrame is pos of camera relative to link frame in GetTransform()
  absFrame = GetTransform() * theFrame;
}


Range_Sensor::~Range_Sensor()
{
  //TRACE("\nRange_Sensor::~Range_Sensor");
  if (amDisplayingBitmap)
  {
        //Calling EndOutput here seems to cause a crash sometimes, sometimes not
    //bp->EndOutput();
    delete bp;
  }
}

Entity* Range_Sensor::Clone () const
{
  //TRACE("\nRange_Sensor::Clone ()");
  //shallow copy, good enough for now
  return new Range_Sensor( *this ) ;
}

bool Range_Sensor::IsInterfering (const Entity* entity) const
{
  //TRACE("\nRange_Sensor::IsInterfering");
  //assert( false ) ;
  return false ;
}

bool Range_Sensor::CanCheckInterference (const Entity* entity) const
{
  //TRACE("\nRange_Sensor::CanCheckInterference");
  //assert( false ) ;
  return false ;
}

void Range_Sensor::Serialize (ostream& os) const
{  
  //TRACE("\nRange_Sensor::Serialize");
  using std::endl ;
  os << "#camera_jonathan" << endl ;
  
  os << "#name " << GetName() << endl;
  os << "#linknum " << BaseFrame() << endl;
  os << "#resolution-y " << user_ray_range_y << endl;
  os << "#resolution-z " << user_ray_range_z << endl;
  os << "#angle " << angle << endl;
  os << "#maxrange " << maxrange << endl;
  os << "#maxerror " << maxerror << endl;
  os << "#depthmapwindow " << DisplayingBitmapEnabled << endl;
  os << "#transform ";
  
  int tempi, tempj;
  for (tempi = 0; tempi < 4; tempi++)
  {
    for (tempj = 0; tempj < 4; tempj++)
    {
      os << this->theFrame.values[tempi][tempj] << " ";
    }
  }
  os << endl;
  os << "#end_camera_jonathan" << endl;
}

void Range_Sensor::Deserialize (istream& is)
{
  
  //TRACE("\nRange_Sensor::Deserialize");
  char marker[ 300 ] = "" ;
  int linknum;
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#name" ) == 0 ) ;
  is.getline(marker, 300, '\n') ;
  SetName (marker);
  //TRACE ("\nname is %s",marker);
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#linknum" ) == 0 ) ;
  linknum = -1 ;
  is >> linknum ;
  SetBaseFrame (linknum);
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#resolution-y" ) == 0 ) ;
  is >> user_ray_range_y ;
  assert (user_ray_range_y <= RAY_ARRAY_LIMIT);
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#resolution-z" ) == 0 ) ;
  is >> user_ray_range_z ;
  assert (user_ray_range_z <= RAY_ARRAY_LIMIT);
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#angle" ) == 0 ) ;
  angle = -1 ;
  is >> angle ;
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#maxrange" ) == 0 ) ;
  maxrange = -1 ;
  is >> maxrange ;
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#maxerror" ) == 0 ) ;
  maxerror = -1 ;
  is >> maxerror ;
  
  eatwhite( is ) ;
  is.getline( marker, 300, ' ' ) ;
  assert( strcmp( marker, "#depthmapwindow" ) == 0 ) ;
  is >> DisplayingBitmapEnabled;

  
  eatwhite( is ) ;
  is.getline( marker, 300, ' '  ) ;
  assert( strcmp( marker, "#transform" ) == 0 ) ;
  
  for (int tempi = 0; tempi < 4; tempi++)
  {
    for (int tempj = 0; tempj < 4; tempj++)
    {
      is >> this->theFrame.values[tempi][tempj] ;
      //TRACE ("\nCamera theFrame:%f", this->theFrame.values[tempi][tempj]);
    }
  }
  
  //TRACE("\nRange_Sensor::Deserialize l%d, res-y%d, res-z%d, an%f, mr%f, ec%f, bme%d",BaseFrame(),user_ray_range_y,user_ray_range_z,angle,maxrange,maxerror,DisplayingBitmapEnabled);
  
  eatwhite( is ) ;
  is.getline( marker, 300 ) ;
  assert( strcmp( marker, "#end_camera_jonathan" ) == 0 ) ;

  //set these basic vars at construction time
  lower_ray_lim_y = RAY_ARRAY_LIMIT/2-user_ray_range_y/2+1;
  upper_ray_lim_y = RAY_ARRAY_LIMIT/2+user_ray_range_y/2;

  lower_ray_lim_z = RAY_ARRAY_LIMIT/2-user_ray_range_z/2+1;
  upper_ray_lim_z = RAY_ARRAY_LIMIT/2+user_ray_range_z/2;

  ray_half_limit = RAY_ARRAY_LIMIT/2;

  //theFrame is pos of camera relative to link frame in GetTransform()
  absFrame = GetTransform() * theFrame;

}

void Range_Sensor::SetAngle (double angle)
{
  //TRACE("\nRange_Sensor::SetAngle");
  this->angle = angle ;
}

void Range_Sensor::SetMaxRange (double maxrange)
{
  //TRACE("\nRange_Sensor::SetMaxRange");
  this->maxrange = maxrange ;
}

void Range_Sensor::SetMaxError (double maxerror)
{
  //TRACE("\nRange_Sensor::SetMaxRange");
  this->maxerror = maxerror ;
}

double Range_Sensor::GetAngle () const
{
  //TRACE("\nRange_Sensor::GetAngle");
  return angle ;
}

double Range_Sensor::GetMaxRange () const
{
  //TRACE("\nRange_Sensor::GetMaxRange");
  return maxrange ;
}

double Range_Sensor::GetMaxError () const
{
  //TRACE("\nRange_Sensor::GetMaxError");
  return maxerror ;
}

void Range_Sensor::AddEntityToDepthMap (const Entity* entity)
{
  //TRACE("\nRange_Sensor::AddEntityToDepthMap");
  const Mesh* mesh = dynamic_cast< const Mesh* >( entity ) ;
  if( mesh != NULL )
  {
    AddMeshToDepthMap( *mesh ) ;
    return;
  }
  const MPK_Cylinder* mpk_cylinder = dynamic_cast< const MPK_Cylinder * >( entity ) ;
  if( mpk_cylinder != NULL )
  {
    AddCylinderToDepthMap( *mpk_cylinder ) ;
    return;
  }
  const MPK_Sphere* mpk_sphere = dynamic_cast< const MPK_Sphere * >( entity ) ;
  if( mpk_sphere != NULL )
  {
    AddSphereToDepthMap( *mpk_sphere );
    return;
  }
  const MPK_Box* mpk_box = dynamic_cast< const MPK_Box * >( entity ) ;
  if( mpk_box != NULL )
  {
    AddBoxToDepthMap( *mpk_box );
    return;
  }
  
  const ObjectGroup* group = dynamic_cast< const ObjectGroup* >( entity ) ;
  if( group != NULL )
  {
    //TRACE("\nObjectGroup Detected");
    for( int i = 0; i < group->Size(); i++ )
    {
      const ObjectBase* addme = ( *group )[ i ] ;
      AddEntityToDepthMap( addme ) ;
    }
    return;
  }
}

void Range_Sensor::AddMeshToDepthMap (const Mesh& mesh)
{
  //TRACE("\nRange_Sensor::AddMeshToDepthMap");
  double t,u,v;
  double v1[3],v2[3],v3[3],edge1[3],edge2[3];
  int i,j,k,l,temp;
  std::vector<Vector4> absvertexes = mesh.GetVertexes();
  
  MgcVector3* pointslist = new MgcVector3[absvertexes.size()];
  
  //pre-transform the vertex list to get absolute distance from (0,0,0)
  for (i = 0; i < absvertexes.size(); i++)
  {
    absvertexes[i] = mesh.GetTransform() * absvertexes[i];
    //convert from Vector4 to MgcVector3
    pointslist[i][0] = absvertexes[i][0];
    pointslist[i][1] = absvertexes[i][1];
    pointslist[i][2] = absvertexes[i][2];
  }
  
  MgcBox3 orientedbox;
  orientedbox = MgcContOrientedBox (absvertexes.size(), pointslist);
  delete [] pointslist;
  
  MgcRay3 rkRay;
  
  //TRACE ("\nvals are %f,%f,%f  lb %f,  ub %f",theFrustum.DVector().x, theFrustum.DVector().y,theFrustum.DVector().z,theFrustum.LBound(),theFrustum.UBound());
  //TRACE ("\nvals are %f,%f,%f  lb %f,  ub %f",theFrustum.LVector().x, theFrustum.LVector().y,theFrustum.LVector().z,theFrustum.LBound(),theFrustum.UBound());
  //TRACE ("\nvals are %f,%f,%f  lb %f,  ub %f",theFrustum.UVector().x, theFrustum.UVector().y,theFrustum.UVector().z,theFrustum.LBound(),theFrustum.UBound());
  //TRACE ("\nOrigin:(%f,%f,%f)", theFrustum.Origin().x, theFrustum.Origin().y, theFrustum.Origin().z);
    
  //for (i = 0; i < absvertexes.size(); i++)
  //{
  //  TRACE ("\npoints:(%f,%f,%f)", pointslist[i][0],pointslist[i][1],pointslist[i][2]);
  //}
  
  //if we miss the frustum, then quit
  if (MgcTestIntersection (orientedbox, theFrustum) == 0)
  {
    //TRACE("\nFrustum missed, skipping");
    return;
  }
  
  bool okaytotest[RAY_ARRAY_LIMIT][RAY_ARRAY_LIMIT];
  rkRay.Origin() = theFrustum.Origin();
  
  //Note: I've tried doing collsion detection with bounding boxes inside the ray loop,
  //but precomputing an array of bools like this seems fastest
  for (k=lower_ray_lim_y; k < upper_ray_lim_y; k++)
  {
    for (l=lower_ray_lim_z; l < upper_ray_lim_z; l++)
    {
      rkRay.Direction()[0] = ray_array->values[k][l][0];
      rkRay.Direction()[1] = ray_array->values[k][l][1];
      rkRay.Direction()[2] = ray_array->values[k][l][2];
      okaytotest[k][l] = MgcTestIntersection (rkRay, orientedbox);
    }
  }
  
  //TRACE("\nAttempting mesh coll-det");
  //need to add each triangle to ray tracing algorithim individually
  const FACETVECTOR& f = mesh.GetFacets();
  for( i = 0; i < f.size(); i++ )
  {
    Facet face = f[ i ] ;
    int edges = face.vertexNumbers.size() ;
    assert( edges == 3 ) ;
    
    //copy the vertex points of the mesh into the arrays above
    for( j = 0; j < 3; j++ )
    {
      v1[ j ] = absvertexes[ face.vertexNumbers[ 0 ] ][ j ] ;
      v2[ j ] = absvertexes[ face.vertexNumbers[ 1 ] ][ j ] ;
      v3[ j ] = absvertexes[ face.vertexNumbers[ 2 ] ][ j ] ;
          // find vectors for two edges sharing v1
      SUB(edge1, v2, v1);
      SUB(edge2, v3, v1);
    }
    //bool once = false;
    //TRACE("\nTri is at (%f,%f,%f)(%f,%f,%f)(%f,%f,%f)",v1[0],v1[1],v1[2],v2[0],v2[1],v2[2],v3[0],v3[1],v3[2]);
    for (k=lower_ray_lim_y; k < upper_ray_lim_y; k++)
    {
      for (l=lower_ray_lim_z; l < upper_ray_lim_z; l++)
      {
        //if bounding box hit is closer than current closest, continue
        if (okaytotest[k][l])
        {
          temp = intersect_triangle( ray_source, ray_array->values[k][l], v1,v2,v3, edge1,edge2, &t,&u,&v);
          if ((temp != 0) && (t < ray_depth_map[k][l]) && (t > 0))
          {
            ray_depth_map[k][l] = t;
            //if (once == false)
            //{
            //  TRACE("\n **hit ray_depth_map %f",t);
            //  once = true;
            //}
          }
        }
      }
    }
  }
  //TRACE ("\n  ray source: (%f,%f,%f)", ray_source[0], ray_source[1], ray_source[2]);
  //TRACE ("\n  ray was pointing at: (%f,%f,%f)", ray_array->values[128][128][0], ray_array->values[128][128][1], ray_array->values[128][128][2]);
  
}

void Range_Sensor::AddCylinderToDepthMap (const MPK_Cylinder& mpk_cylinder)
{
  //TRACE("\nRange_Sensor::AddCylinderToDepthMap");
  Vector4 updir;
  MgcRay3 rkRay;
  MgcCylinder rkCylinder;
  double t;
  int i,j;
  bool intersect_val;
  //TRACE("\nCylinder Detected");
  
  //homogeneous coord for the original position of cylinder before transforming
  updir[0] = 0;
  updir[1] = 1;
  updir[2] = 0;
  updir[3] = 0;
  
  Matrix4x4 absCylFrame;
  //note: the cylinder may be associted with any link (not neccescarily that of the camera)
  // so is this how I access the full postion of any cylinder in the world?
  absCylFrame = mpk_cylinder.GetTransform()*mpk_cylinder.theFrame ;

  //updir holds the upward dir for the cylinder
  updir = absCylFrame * updir;
  
  normalize (updir[0],updir[1],updir[2]);
  //TRACE("\n3updir %f,%f,%f",updir[0],updir[1],updir[2]);
  
  MgcBox3 boundingbox;
  MgcVector3 tempdir;
  
  //set up cylinder bounding box
  tempdir.x = absCylFrame.values[0][0]; tempdir.y = absCylFrame.values[1][0]; tempdir.z = absCylFrame.values[2][0];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(0) = tempdir;
  
  tempdir.x = absCylFrame.values[0][1]; tempdir.y = absCylFrame.values[1][1]; tempdir.z = absCylFrame.values[2][1];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(1) = tempdir;
  
  tempdir.x = absCylFrame.values[0][2]; tempdir.y = absCylFrame.values[1][2]; tempdir.z = absCylFrame.values[2][2];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(2) = tempdir;
  
  boundingbox.Extent(0) = mpk_cylinder.Radius();  
  boundingbox.Extent(1) = mpk_cylinder.Height()*0.5;
  boundingbox.Extent(2) = mpk_cylinder.Radius(); 
  
  tempdir.x = absCylFrame.values[0][3]; tempdir.y = absCylFrame.values[1][3]; tempdir.z = absCylFrame.values[2][3];
  boundingbox.Center() = tempdir;

  //if we miss the frustum, then quit
  if (MgcTestIntersection (boundingbox, theFrustum) == 0)
  {
    return;
  }
  //TRACE("\nAttempting cylinder coll-det");
  
  //convert from MPK cylinder to Magic library cylinder
  rkCylinder.Center()[0] = absCylFrame.values[0][3];
  rkCylinder.Center()[1] = absCylFrame.values[1][3];
  rkCylinder.Center()[2] = absCylFrame.values[2][3];
  rkCylinder.Direction()[0] = updir[0];
  rkCylinder.Direction()[1] = updir[1];
  rkCylinder.Direction()[2] = updir[2];
  rkCylinder.Height() = mpk_cylinder.Height();
  rkCylinder.Radius() = mpk_cylinder.Radius();
  rkRay.Origin()[0] = ray_source[0];
  rkRay.Origin()[1] = ray_source[1];
  rkRay.Origin()[2] = ray_source[2];
  
  //for (i = 0; i < 4; i++)
  //{
//              TRACE("\n");
//              for (j = 0; j < 4; j++)
//              {
//                      TRACE (" absCylFrame %f",absCylFrame.values[i][j]);
//              }
//  }

//  TRACE("\nradius %f, height %f",rkCylinder.Radius(),rkCylinder.Height());
//  TRACE("\nCenter %f,%f,%f",rkCylinder.Center()[0],rkCylinder.Center()[1],rkCylinder.Center()[2]);
//  TRACE("\nRay Source %f,%f,%f",rkRay.Origin()[0],rkRay.Origin()[1],rkRay.Origin()[2]);
//  TRACE("\nRay Dir %f,%f,%f",ray_array->values[128][128][0],ray_array->values[128][128][1],ray_array->values[128][128][2]);
  
  //test for if and where intersection between cylinder and rays
  for (i=lower_ray_lim_y; i< upper_ray_lim_y; i++)
  {
    for (j=lower_ray_lim_z; j < upper_ray_lim_z; j++)
    {
      rkRay.Direction()[0] = ray_array->values[i][j][0];
      rkRay.Direction()[1] = ray_array->values[i][j][1];
      rkRay.Direction()[2] = ray_array->values[i][j][2];

      intersect_val = MgcFindIntersection (rkRay,rkCylinder,t);
      if ((intersect_val == true) && (t < ray_depth_map[i][j]) && (t > 0))
      {
        ray_depth_map[i][j] = t;
      }
    }
  }
}

void Range_Sensor::AddSphereToDepthMap (const MPK_Sphere& mpk_sphere)
{
  //TRACE("\nRange_Sensor::AddSphereToDepthMap");
  double center[3];
  double t;
  int i,j,intersect_val;
  //TRACE("\nSphere Detected");
  
  Vector4 absSphFrame;
  //note: the sphere may be associted with any link (not neccescarily that of the camera)
  // so is this how I access the full postion of any sphere in the world?
  absSphFrame = mpk_sphere.GetTransform() * mpk_sphere.Position();

  //for (i = 0; i < 4; i++)
  //{
//              for (j = 0; j < 4; j++)
//              {
//                      TRACE ("\nabsSphFrame%f",absSphFrame[j]);
//              }
//  }
  
  center[0] = absSphFrame[0];
  center[1] = absSphFrame[1];
  center[2] = absSphFrame[2];

  MgcBox3 boundingbox;
  MgcVector3 tempdir;
  
  //set up bounding box for sphere
  //default sphere orientation (arbitrary)
  tempdir.x = 1; tempdir.y = 0; tempdir.z = 0;
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(0) = tempdir;
  
  tempdir.x = 0; tempdir.y = 1; tempdir.z = 0;
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(1) = tempdir;
  
  tempdir.x = 0; tempdir.y = 0; tempdir.z = 1;
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(2) = tempdir;
  
  boundingbox.Extent(0) = mpk_sphere.Radius();
  boundingbox.Extent(1) = mpk_sphere.Radius();
  boundingbox.Extent(2) = mpk_sphere.Radius();
  
  tempdir.x = absSphFrame[0]; tempdir.y = absSphFrame[1]; tempdir.z = absSphFrame[2];
  boundingbox.Center() = tempdir;

  //if we miss the frustum, then quit
  if (MgcTestIntersection (boundingbox, theFrustum) == 0)
  {
    return;
  }
  //TRACE("\nAttempting sphere coll-det");
  
  //test for if and where intersection between sphere and rays
  for (i=lower_ray_lim_y; i< upper_ray_lim_y; i++)
  {
    for (j=lower_ray_lim_z; j < upper_ray_lim_z; j++)
    {
      intersect_val = intersect_sphere(ray_source, ray_array->values[i][j], 
        center, mpk_sphere.Radius(), t, this->maxrange);
      if ((intersect_val != 0) && (t < ray_depth_map[i][j]) && (t > 0))
      {
        ray_depth_map[i][j] = t;
      }
    }
  }
}

void Range_Sensor::AddBoxToDepthMap (const MPK_Box& mpk_box)
{
  //TRACE("\nRange_Sensor::AddBoxToDepthMap");
  
  int i,j;
  Matrix4x4 absBoxFrame;
  //note: the sphere may be associted with any link (not neccescarily that of the camera)
  // so is this how I access the full postion of any sphere in the world?
  absBoxFrame = mpk_box.GetTransform()*mpk_box.theFrame ;
  
  //the bounding box will be exactly defined by an oriented bounding box
  MgcBox3 boundingbox;
  MgcVector3 tempdir;
  
  tempdir.x = absBoxFrame.values[0][0]; tempdir.y = absBoxFrame.values[1][0]; tempdir.z = absBoxFrame.values[2][0];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(0) = tempdir;
  
  tempdir.x = absBoxFrame.values[0][1]; tempdir.y = absBoxFrame.values[1][1]; tempdir.z = absBoxFrame.values[2][1];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(1) = tempdir;
  
  tempdir.x = absBoxFrame.values[0][2]; tempdir.y = absBoxFrame.values[1][2]; tempdir.z = absBoxFrame.values[2][2];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  boundingbox.Axis(2) = tempdir;
  
  boundingbox.Extent(0) = mpk_box.Length()*0.5;  
  boundingbox.Extent(1) = mpk_box.Width()*0.5;
  boundingbox.Extent(2) = mpk_box.Height()*0.5; 
  
  tempdir.x = absBoxFrame.values[0][3]; tempdir.y = absBoxFrame.values[1][3]; tempdir.z = absBoxFrame.values[2][3];
  boundingbox.Center() = tempdir;

  //if we miss the frustum, then quit
  if (MgcTestIntersection (boundingbox, theFrustum) == 0)
  {
    return;
  }
  //TRACE("\nAttempting box coll-det");
  
  MgcRay3 rkRay;
  bool intersect_val;
  double t;

  //for (i = 0; i < 4; i++)
  //{
  //  TRACE("\n");
//              for (j = 0; j < 4; j++)
//              {
//                      TRACE ("absSphFrame%f",absBoxFrame.values[i][j]);
//              }
//  }

//  TRACE ("\naxis(0) are: %f,%f,%f",boundingbox.Axis(0).x,boundingbox.Axis(0).y,boundingbox.Axis(0).z);
//  TRACE ("\naxis(1) are: %f,%f,%f",boundingbox.Axis(1).x,boundingbox.Axis(1).y,boundingbox.Axis(1).z);
//  TRACE ("\naxis(2) are: %f,%f,%f",boundingbox.Axis(2).x,boundingbox.Axis(2).y,boundingbox.Axis(2).z);
//  TRACE ("\nextents are: %f,%f,%f",boundingbox.Extent(0),boundingbox.Extent(1),boundingbox.Extent(2));
//  TRACE ("\ncenter is: %f,%f,%f",boundingbox.Center().x,boundingbox.Center().y,boundingbox.Center().z);

  rkRay.Origin()[0] = ray_source[0];
  rkRay.Origin()[1] = ray_source[1];
  rkRay.Origin()[2] = ray_source[2];
  
  //test for if and where intersection between box and rays
  for (i=lower_ray_lim_y; i < upper_ray_lim_y; i++)
  {
    for (j=lower_ray_lim_z; j < upper_ray_lim_z; j++)
    {
      rkRay.Direction()[0] = ray_array->values[i][j][0];
      rkRay.Direction()[1] = ray_array->values[i][j][1];
      rkRay.Direction()[2] = ray_array->values[i][j][2];
      intersect_val = MgcFindIntersection (rkRay, boundingbox, t);
      if ((intersect_val == true) && (t < ray_depth_map[i][j]) && (t > 0))
      {
        ray_depth_map[i][j] = t;
      }
    }
  }
}

void Range_Sensor::Generate_Ray_Array ()
{
  //TRACE("\nRange_Sensor::Generate_Ray_Array");
  int i,j;
  
  //generate inital ray array
  
  double newdir[3], temp[3];
  
  //tempvalues stores normalized rotation matrix needing scaling according to this->angle
  double tempvalues[3][3]; 
  
  //update the abs frame (relative to (0,0,0)) before taking a picture.
  //theFrame is pos of camera relative to link frame in GetTransform()
  absFrame = GetTransform() * theFrame;

  ray_source[0] = this->absFrame.values[0][3];
  ray_source[1] = this->absFrame.values[1][3];
  ray_source[2] = this->absFrame.values[2][3];
  
  //shoot rays in x direction from 0,0,0
  newdir[0] = ray_half_limit*this->absFrame.values[0][0];
  newdir[1] = ray_half_limit*this->absFrame.values[1][0];
  newdir[2] = ray_half_limit*this->absFrame.values[2][0];
  
  //calculate maximum extents for pre-normalized y,z directions
  angleratio = tan((this->angle)/2);
  for (i=0; i< 3; i++)
  {
    for (j=0; j < 3; j++)
    {
      tempvalues[i][j] = angleratio * this->absFrame.values[i][j];
    }
  }
  
  //define ray targets in y direction
  for (i = 0; i < RAY_ARRAY_LIMIT; i++)
  {
    temp[0] = newdir[0] + (i - ray_half_limit)*tempvalues[0][1];
    temp[1] = newdir[1] + (i - ray_half_limit)*tempvalues[1][1];
    temp[2] = newdir[2] + (i - ray_half_limit)*tempvalues[2][1];
    //define ray targets in z direction
    for (j = 0; j < RAY_ARRAY_LIMIT; j++)
    {
      ray_array->values[i][j][0] =  temp[0] + (j - ray_half_limit)*tempvalues[0][2];
      ray_array->values[i][j][1] =  temp[1] + (j - ray_half_limit)*tempvalues[1][2];
      ray_array->values[i][j][2] =  temp[2] + (j - ray_half_limit)*tempvalues[2][2];
      normalize ( ray_array->values[i][j][0],ray_array->values[i][j][1], ray_array->values[i][j][2] );
    }
  }
  // now, ray array has been generated
  // make no-intersection case ray_depth_map
  for (i=lower_ray_lim_y; i < upper_ray_lim_y; i++)
  {
    for (j=lower_ray_lim_z; j < upper_ray_lim_z; j++)
    {
      ray_depth_map[i][j] = maxrange;
    }
  }
  //TRACE("\nmaxrange is: %f,%f",this->maxrange, ray_depth_map[128][128]);

  MgcVector3 tempdir;
  
  //generate view frustum from camera location variables.
  tempdir.x = absFrame.values[0][3]; tempdir.y = absFrame.values[1][3]; tempdir.z = absFrame.values[2][3];
  theFrustum.Origin() = tempdir;
  
  tempdir.x = absFrame.values[0][0]; tempdir.y = absFrame.values[1][0]; tempdir.z = absFrame.values[2][0];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  theFrustum.DVector() = tempdir;
  
  tempdir.x = absFrame.values[0][1]; tempdir.y = absFrame.values[1][1]; tempdir.z = absFrame.values[2][1];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  theFrustum.LVector() = tempdir;
  
  tempdir.x = absFrame.values[0][2]; tempdir.y = absFrame.values[1][2]; tempdir.z = absFrame.values[2][2];
  //normalize(tempdir.x,tempdir.y,tempdir.z);
  theFrustum.UVector() = tempdir;
  
  theFrustum.LBound() = minrange * angleratio * user_ray_range_y / RAY_ARRAY_LIMIT;
  theFrustum.UBound() = minrange * angleratio * user_ray_range_z / RAY_ARRAY_LIMIT; 
  //to change sensor shape, change resolution instead of angleratio

  theFrustum.DMin() = minrange;
  theFrustum.DMax() = maxrange;
  theFrustum.Update(); //always call Update() after setting up MgcFrustum

}


void Range_Sensor::Generate_Coord_Map ()
{
  //TRACE("\nRange_Sensor::Generate_Coord_Map");
  int i,j;
  if (maxerror > 0)
  {
    //TRACE ("\nGenerating Error in ray_depth_map");
  }

  //generate ray_coord_map using depths in ray_depth_map and dirs in ray_array->values
  for (i = 0; i < RAY_ARRAY_LIMIT; i++)
  {
    for (j = 0; j < RAY_ARRAY_LIMIT; j++)
    {
      //introduce error if necesscary
          if (maxerror > 0)
      {
        double temperror = (2.0*double( rand() ) / RAND_MAX-1.0);
        ray_depth_map[i][j] = ray_depth_map[i][j] * (1.0 + maxerror * temperror);
      }
      ray_coord_map[i][j][0]=ray_source[0]+ray_depth_map[i][j]*ray_array->values[i][j][0];
      ray_coord_map[i][j][1]=ray_source[1]+ray_depth_map[i][j]*ray_array->values[i][j][1];
      ray_coord_map[i][j][2]=ray_source[2]+ray_depth_map[i][j]*ray_array->values[i][j][2];
    }
  }
}

void Range_Sensor::Take_Picture (const std::vector< Entity* > allEntities, bool display2DMap)
{
  //TRACE("\nRange_Sensor::Take_Picture");
  int i;  
    
  memset (ray_source,0,sizeof(ray_source));
  memset (ray_depth_map,0,sizeof(ray_depth_map));
  memset (ray_coord_map,0,sizeof(ray_coord_map));  
  
  ray_array = new ray_array_space;
  
  //generate the normalized ray vector map and initial, worst case ray_depth_map;
  Generate_Ray_Array ();
  
  //now, scan through all objects to find the nearest intersection ray.
  int size = allEntities.size() ;
  for( i = 0; i < size; i++ )
  {
    AddEntityToDepthMap (allEntities[ i ]);      
  }
  
  //convert ray_depth_map to ray_coord_map.
  Generate_Coord_Map ();
  
  delete ray_array;
  
  //if user has enabled the 2D display of ray_depth_map and we aren't in multithreaded mode
  if (DisplayingBitmapEnabled && display2DMap)
  {
    if (amDisplayingBitmap)
    {
      //Calling EndOutput here seems to cause a crash sometimes, sometimes not
      //bp->EndOutput();
      delete bp;
    }
    bp = new O_Bitmap;
    assert (bp != NULL);
    
    amDisplayingBitmap = true;
 
    bp->StartOutput() ;         //IMPROVE: remember to end output somewhere
    bp->SetResolution( user_ray_range_z, user_ray_range_y ) ;
    bp->NameDialog( GetName() ) ;
    
        int y,z;
        //display depth map pixels to screen in proper order
    for ( y=lower_ray_lim_y; y < upper_ray_lim_y; y++)
        {
      for (z=lower_ray_lim_z; z < upper_ray_lim_z; z++)
          {
        bp->PutPixel( z-lower_ray_lim_z, y-lower_ray_lim_y, 1.0 - ray_depth_map[y][z]/maxrange ) ;
      }
    }
    
    bp->Display() ;
  }
}

bool Range_Sensor::Verify() const
{
        return true;
}

---