00001
00002
00003
00004
00005
00006
00007
00008
00010
00011 #define CAMERA_HEIGHT 0.5
00012 #define CAMERA_NEAR_CLIP 0.2
00013 #define CAMERA_FAR_CLIP 8.0
00014
00015
00016 #include "canvas.hh"
00017 #include "worldfile.hh"
00018
00019 using namespace Stg;
00020
00021 #include <sstream>
00022 #include <iomanip>
00023
00024
00025 Option ModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true, NULL );
00026
00027 static const Stg::Size DEFAULT_SIZE( 0.1, 0.07, 0.05 );
00028 static const char DEFAULT_GEOM_COLOR[] = "black";
00029 static const float DEFAULT_HFOV = 70;
00030 static const float DEFAULT_VFOV = 40;
00031
00072
00073 void cross( float& x1, float& y1, float& z1, float x2, float y2, float z2 )
00074 {
00075 float x3, y3, z3;
00076
00077 x3 = y1 * z2 - z1 * y2;
00078 y3 = z1 * x2 - x1 * z2;
00079 z3 = x1 * y2 - y1 * x2;
00080
00081 x1 = x3;
00082 y1 = y3;
00083 z1 = z3;
00084 }
00085
00086
00087 ModelCamera::ModelCamera( World* world,
00088 Model* parent,
00089 const std::string& type ) :
00090 Model( world, parent, type ),
00091 _canvas( NULL ),
00092 _frame_data( NULL ),
00093 _frame_color_data( NULL ),
00094 _valid_vertexbuf_cache( false ),
00095 _vertexbuf_cache( NULL ),
00096 _width( 32 ),
00097 _height( 32 ),
00098 _camera_quads_size( 0 ),
00099 _camera_quads( NULL ),
00100 _camera_colors( NULL ),
00101 _camera(),
00102 _yaw_offset( 0.0 ),
00103 _pitch_offset( 0.0 )
00104 {
00105 PRINT_DEBUG2( "Constructing ModelCamera %d (%s)\n",
00106 id, typestr );
00107
00108 WorldGui* world_gui = dynamic_cast< WorldGui* >( world );
00109
00110 if( world_gui == NULL ) {
00111 printf( "Unable to use Camera Model - it must be run with a GUI world\n" );
00112 assert( 0 );
00113 }
00114 _canvas = world_gui->GetCanvas();
00115
00116 _camera.setPitch( 90.0 );
00117
00118 Geom geom;
00119 memset( &geom, 0, sizeof(geom));
00120 geom.size = DEFAULT_SIZE;
00121 SetGeom( geom );
00122
00123
00124 SetColor( Color( DEFAULT_GEOM_COLOR) );
00125
00126 RegisterOption( &showCameraData );
00127
00128 Startup();
00129 }
00130
00131 ModelCamera::~ModelCamera()
00132 {
00133 if( _frame_data != NULL ) {
00134
00135 delete[] _frame_data;
00136 delete[] _frame_color_data;
00137 delete[] _vertexbuf_cache;
00138 delete[] _camera_quads;
00139 delete[] _camera_colors;
00140 }
00141 }
00142
00143 void ModelCamera::Load( void )
00144 {
00145 Model::Load();
00146
00147 double horizFov = DEFAULT_HFOV;
00148 double vertFov = DEFAULT_VFOV;
00149 wf->ReadTuple( wf_entity, "fov", 0, 2, "ff", &horizFov, &vertFov );
00150 _camera.setFov( horizFov, vertFov );
00151
00152 double range_min = CAMERA_NEAR_CLIP;
00153 double range_max = CAMERA_FAR_CLIP;
00154 wf->ReadTuple( wf_entity, "range", 0, 2, "ll", &range_min, &range_max );
00155 _camera.setClip( range_min, range_max );
00156
00157
00158 wf->ReadTuple( wf_entity, "pantilt", 0, 2, "ff", &_yaw_offset, &_pitch_offset );
00159
00160 wf->ReadTuple( wf_entity, "resolution", 0, 2, "ii", &_width, &_height );
00161 }
00162
00163
00164 void ModelCamera::Update( void )
00165 {
00166 GetFrame();
00167 Model::Update();
00168 }
00169
00170 bool ModelCamera::GetFrame( void )
00171 {
00172 if( _width == 0 || _height == 0 )
00173 return false;
00174
00175 if( _frame_data == NULL ) {
00176 if( _frame_data != NULL ) {
00177
00178 delete[] _frame_data;
00179 delete[] _frame_color_data;
00180 delete[] _vertexbuf_cache;
00181 delete[] _camera_quads;
00182 delete[] _camera_colors;
00183 }
00184 _frame_data = new GLfloat[ _width * _height ];
00185 _frame_color_data = new GLubyte[ 4 * _width * _height ];
00186
00187 _vertexbuf_cache = new ColoredVertex[ _width * _height ];
00188
00189 _camera_quads_size = _height * _width * 4 * 3;
00190 _camera_quads = new GLfloat[ _camera_quads_size ];
00191 _camera_colors = new GLubyte[ _camera_quads_size ];
00192 }
00193
00194
00195
00196 if( _width > _canvas->w() )
00197 _width = _canvas->w();
00198 if( _height > _canvas->h() )
00199 _height = _canvas->h();
00200
00201 GLint viewport[4];
00202 glGetIntegerv(GL_VIEWPORT,viewport);
00203
00204 glViewport( 0, 0, _width, _height );
00205 _camera.update();
00206 _camera.SetProjection();
00207 float height = GetGlobalPose().z;
00208
00209 _camera.setPose( parent->GetGlobalPose().x, parent->GetGlobalPose().y, height );
00210 _camera.setYaw( rtod( parent->GetGlobalPose().a ) - 90.0 - _yaw_offset );
00211 _camera.setPitch( 90.0 - _pitch_offset );
00212 _camera.Draw();
00213
00214
00215 glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
00216 _canvas->DrawFloor();
00217 _canvas->DrawBlocks();
00218
00219
00220 glReadPixels(0, 0, _width, _height,
00221 GL_DEPTH_COMPONENT,
00222 GL_FLOAT,
00223 _frame_data );
00224
00225 float* data = ( float* )( _frame_data );
00226 int buf_size = _width * _height;
00227 for( int i = 0; i < buf_size; i++ )
00228 data[ i ] = _camera.realDistance( data[ i ] );
00229
00230
00231 glReadPixels(0, 0, _width, _height,
00232 GL_RGBA,
00233 GL_UNSIGNED_BYTE,
00234 _frame_color_data );
00235
00236
00237 glViewport( viewport[0], viewport[1], viewport[2], viewport[3] );
00238 _canvas->invalidate();
00239 _canvas->setDirtyBuffer();
00240 return true;
00241 }
00242
00243
00244 void ModelCamera::DataVisualize( Camera* cam )
00245 {
00246 if( _frame_data == NULL || !showCameraData )
00247 return;
00248
00249 float w_fov = _camera.horizFov();
00250 float h_fov = _camera.vertFov();
00251
00252 float start_fov = w_fov / 2.0 + 180.0;
00253 float start_vert_fov = h_fov / 2.0 + 90.0;
00254
00255 int w = _width;
00256 int h = _height;
00257 float a_space = w_fov / w;
00258 float vert_a_space = h_fov / h;
00259
00260
00261 if( _valid_vertexbuf_cache == false ) {
00262 for( int j = 0; j < h; j++ ) {
00263 for( int i = 0; i < w; i++ ) {
00264
00265 float a = start_fov - static_cast< float >( i ) * a_space;
00266 float vert_a = start_vert_fov - static_cast< float >( h - j - 1 ) * vert_a_space;
00267
00268 int index = i + j * w;
00269 ColoredVertex* vertex = _vertexbuf_cache + index;
00270
00271
00272 float x = -sin( dtor( vert_a ) ) * cos( dtor( a ) );
00273 float y = -sin( dtor( vert_a ) ) * sin( dtor( a ) );
00274 float z = -cos( dtor( vert_a ) );
00275
00276
00277 a = dtor( - _yaw_offset );
00278 vertex->x = x * cos( a ) - y * sin( a );
00279 vertex->y = x * sin( a ) + y * cos( a );
00280 vertex->z = z;
00281
00282 x = vertex->x; y = vertex->y; z = vertex->z;
00283 a = dtor( _pitch_offset );
00284 vertex->x = z * sin( a ) + x * cos( a );
00285 vertex->y = y;
00286 vertex->z = z * cos( a ) - x * sin( a );
00287 }
00288 }
00289 _valid_vertexbuf_cache = true;
00290 }
00291
00292
00293
00294
00295
00296 float* depth_data = ( float* )( _frame_data );
00297 for( int j = 0; j < h; j++ ) {
00298 for( int i = 0; i < w; i++ ) {
00299 int index = i + j * w;
00300 const ColoredVertex* unit_vertex = _vertexbuf_cache + index;
00301 const float length = depth_data[ index ];
00302
00303
00304 float sx = unit_vertex->x * length;
00305 float sy = unit_vertex->y * length;
00306 float sz = unit_vertex->z * length;
00307
00308
00309
00310 float x, y, z;
00311 x = 0; y = 0; z = length * M_PI * a_space / 360.0;
00312 cross( x, y, z, unit_vertex->x, unit_vertex->y, unit_vertex->z );
00313
00314 z = length * M_PI * vert_a_space / 360.0;
00315
00316 GLfloat* p = _camera_quads + index * 4 * 3;
00317 p[ 0 ] = sx - x; p[ 1 ] = sy - y; p[ 2 ] = sz - z;
00318 p[ 3 ] = sx - x; p[ 4 ] = sy - y; p[ 5 ] = sz + z;
00319 p[ 6 ] = sx + x; p[ 7 ] = sy + y; p[ 8 ] = sz + z;
00320 p[ 9 ] = sx + x; p[ 10 ] = sy + y; p[ 11 ] = sz - z;
00321
00322
00323
00324 const GLubyte* color = _frame_color_data + index * 4;
00325 for( int i = 0; i < 4; i++ ) {
00326 GLubyte* cp = _camera_colors + index * 4 * 3 + i * 3;
00327 memcpy( cp, color, sizeof( GLubyte ) * 3 );
00328 }
00329 }
00330 }
00331
00332
00333 glEnableClientState( GL_COLOR_ARRAY );
00334
00335 glVertexPointer( 3, GL_FLOAT, 0, _camera_quads );
00336 glColorPointer( 3, GL_UNSIGNED_BYTE, 0, _camera_colors );
00337 glDrawArrays( GL_QUADS, 0, w * h * 4 );
00338
00339 glDisableClientState( GL_COLOR_ARRAY );
00340 }
00341