Update of /cvsroot/playerstage/code/stage/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7705/src
Modified Files:
Tag: opengl
Makefile.am gui_gl.c model.c model_laser.c model_load.c
raytrace.c stage_internal.h
Log Message:
new raytrace model in progress
Index: model_load.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/model_load.c,v
retrieving revision 1.8.4.6
retrieving revision 1.8.4.7
diff -C2 -d -r1.8.4.6 -r1.8.4.7
*** model_load.c 27 Feb 2007 02:54:08 -0000 1.8.4.6
--- model_load.c 18 May 2007 05:21:07 -0000 1.8.4.7
***************
*** 227,230 ****
--- 227,233 ----
printf( "found %d rects\n", rect_count );
+ //mod->verts_count = rect_count * 4;
+ //mod->verts = g_new( float, mod->verts_count * 3);
+
if( rects && (rect_count > 0) )
{
***************
*** 285,293 ****
// pointcount, l );
stg_point_t* pts = stg_points_create( pointcount );
int p;
! for( p=0; p<pointcount; p++ )
! {
snprintf(key, sizeof(key), "polygon[%d].point[%d]", l, p );
--- 288,298 ----
// pointcount, l );
+ mod->verts_count = pointcount * 2;
+ mod->verts = g_new( float, mod->verts_count * 3);
+
stg_point_t* pts = stg_points_create( pointcount );
int p;
! for( p=0; p<pointcount; p++ ) {
snprintf(key, sizeof(key), "polygon[%d].point[%d]", l, p );
***************
*** 297,301 ****
//printf( "key %s x: %.2f y: %.2f\n",
// key, pt.x, pt.y );
!
}
--- 302,315 ----
//printf( "key %s x: %.2f y: %.2f\n",
// key, pt.x, pt.y );
!
! // original point
! mod->verts[6*p+0] = pts[p].x;
! mod->verts[6*p+1] = pts[p].y;
! mod->verts[6*p+2] = 0;
!
! // extruded point
! mod->verts[6*p+3] = pts[p].x;
! mod->verts[6*p+4] = pts[p].y;
! mod->verts[6*p+5] = 1;
}
Index: raytrace.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/raytrace.c,v
retrieving revision 1.17.4.2
retrieving revision 1.17.4.3
diff -C2 -d -r1.17.4.2 -r1.17.4.3
*** raytrace.c 20 Dec 2006 03:01:13 -0000 1.17.4.2
--- raytrace.c 18 May 2007 05:21:07 -0000 1.17.4.3
***************
*** 181,197 ****
// find all the models that overlap with this bbox
! GList* candidates = mod->intersectors;
itl->hits = NULL;
! printf( "ray from %.2f,%.2f hitlist:\n", x, y );
GList* it;
for( it=candidates; it; it=it->next )
{
! stg_model_t* him = (stg_model_t*)it->data;
! printf( " %s\t", him->token );
! if( (*func)( him, mod ) )
{
double xs,ys;
--- 181,198 ----
// find all the models that overlap with this bbox
! GList* candidates = mod->sense_poly->intersectors;
itl->hits = NULL;
! //printf( "ray from %.2f,%.2f hitlist:\n", x, y );
GList* it;
for( it=candidates; it; it=it->next )
{
! stg_polygon_t* his_poly = (stg_polygon_t*)it->data;
! stg_model_t* him = his_poly->mod;
! //printf( " %s\t", him->token );
! if( (him != mod) && (*func)( him, mod ) )
{
double xs,ys;
***************
*** 209,219 ****
itl->hits = g_list_prepend( itl->hits, hit );
! printf( " \n" );
}
}
! else
! printf( " (failed the filter)\n" );
}
! puts("");
itl->hits = g_list_sort( itl->hits, hit_range_compare );
--- 210,220 ----
itl->hits = g_list_prepend( itl->hits, hit );
! // printf( " \n" );
}
}
! //else
! //printf( " (failed the filter)\n" );
}
! //puts("");
itl->hits = g_list_sort( itl->hits, hit_range_compare );
***************
*** 221,230 ****
//GList* it;
! printf( "Sorted hit list:\n" );
! for( it=itl->current; it; it=it->next )
! {
! stg_hit_t* hit = (stg_hit_t*)it->data;
! printf( "%s @ %.2f\n", hit->mod->token, hit->range );
! }
itl->cosa = cos( itl->a );
--- 222,231 ----
//GList* it;
! //printf( "Sorted hit list:\n" );
! //for( it=itl->current; it; it=it->next )
! //{
! // stg_hit_t* hit = (stg_hit_t*)it->data;
! // printf( "%s @ %.2f\n", hit->mod->token, hit->range );
! // }
itl->cosa = cos( itl->a );
***************
*** 232,237 ****
itl->tana = tan( itl->a );
! puts("");
! puts("");
return itl;
--- 233,238 ----
itl->tana = tan( itl->a );
! // puts("");
! //puts("");
return itl;
Index: stage_internal.h
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/stage_internal.h,v
retrieving revision 1.58.2.9
retrieving revision 1.58.2.10
diff -C2 -d -r1.58.2.9 -r1.58.2.10
*** stage_internal.h 2 Mar 2007 04:09:50 -0000 1.58.2.9
--- stage_internal.h 18 May 2007 05:21:07 -0000 1.58.2.10
***************
*** 165,168 ****
--- 165,171 ----
GList* polys;
+
+ float* verts;
+ int verts_count;
stg_watts_t watts; //< power consumed by this model
Index: gui_gl.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Attic/gui_gl.c,v
retrieving revision 1.1.2.14
retrieving revision 1.1.2.15
diff -C2 -d -r1.1.2.14 -r1.1.2.15
*** gui_gl.c 23 Mar 2007 02:19:36 -0000 1.1.2.14
--- gui_gl.c 18 May 2007 05:21:07 -0000 1.1.2.15
***************
*** 357,360 ****
--- 357,361 ----
// construct a strip that wraps around the polygon
+
glBegin(GL_QUAD_STRIP);
int p;
***************
*** 381,385 ****
else
glEnable(GL_CULL_FACE);
-
}
--- 382,385 ----
***************
*** 388,400 ****
static void polygon2d( double* pts, size_t pt_count, stg_color_t color )
{
! //push_color_stgcolor( color );
!
! glBegin(GL_POLYGON);
! int p;
! for( p=0; p<pt_count; p++)
! glVertex2f( pts[p*2], pts[p*2+1] );
! glEnd();
!
! //pop_color();
}
--- 388,394 ----
static void polygon2d( double* pts, size_t pt_count, stg_color_t color )
{
! glEnableClientState( GL_VERTEX_ARRAY );
! glVertexPointer( 2, GL_DOUBLE, 0, pts );
! glDrawArrays( GL_POLYGON, 0, pt_count );
}
***************
*** 421,424 ****
--- 415,422 ----
glNewList( list, GL_COMPILE );
+
+
+ // end exp
+
stg_laser_sample_t* samples = (stg_laser_sample_t*)mod->data;
size_t sample_count = mod->data_len / sizeof(stg_laser_sample_t);
***************
*** 430,435 ****
stg_pose_t gpose;
stg_model_get_global_pose( mod, &gpose );
!
! // do alpha properly
glDepthMask( GL_FALSE );
--- 428,433 ----
stg_pose_t gpose;
stg_model_get_global_pose( mod, &gpose );
!
! // do alpha properly
glDepthMask( GL_FALSE );
***************
*** 469,472 ****
--- 467,493 ----
}
+ // outline the polgons that the selected robot intersects with
+ push_color_stgcolor( stg_lookup_color( "orange" ));
+
+ GList *b;
+ for( b = mod->sense_poly->intersectors; b; b=b->next )
+ {
+ stg_polygon_t* p = (stg_polygon_t*)b->data;
+
+ glPushMatrix();
+
+ stg_pose_t pose;
+ stg_model_get_global_pose( p->mod, &pose );
+
+ // move into this model's coordinate frame
+ glTranslatef( pose.x, pose.y, pose.z );
+ glRotatef( RTOD(pose.a), 0,0,1 );
+ gl_draw_polygon3d( p );
+
+ glPopMatrix();
+ }
+
+ pop_color();
+
glEndList();
***************
*** 913,917 ****
g_list_foreach( mod->polys, (GFunc)gl_draw_polygon3d_cb, NULL );
!
//if( 1 )// mod->boundary )
// box3d_wireframe( &mod->bbox );
--- 934,951 ----
g_list_foreach( mod->polys, (GFunc)gl_draw_polygon3d_cb, NULL );
! // experimental
! /* glDisable (GL_CULL_FACE); */
! /* glEnableClientState( GL_VERTEX_ARRAY ); */
! /* glVertexPointer( 3, GL_FLOAT, 0, mod->verts ); */
! /* glDrawArrays( GL_QUAD_STRIP, 0, mod->verts_count ); */
! /* glEnable(GL_CULL_FACE); */
!
!
! /* glBegin(GL_POLYGON); */
! /* int i; */
! /* for( i=0; i<mod->verts_count/2; i++ ) */
! /* glArrayElement( 2*i+1 ); */
! /* glEnd(); */
!
//if( 1 )// mod->boundary )
// box3d_wireframe( &mod->bbox );
***************
*** 1114,1134 ****
glEnable (GL_DEPTH_TEST);
glDepthFunc (GL_LESS);
! //glClearColor ( 0.7, 0.7, 0.8, 1.0);
! glClearColor ( 0,0,0, 1.0);
gdk_gl_glPolygonOffsetEXT (proc, 1.0, 1.0);
glEnable (GL_CULL_FACE);
- //glDisable (GL_CULL_FACE);
glHint (GL_LINE_SMOOTH_HINT, GL_NICEST);
glHint (GL_POLYGON_SMOOTH_HINT, GL_NICEST);
- //glHint (GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
glDisable (GL_LIGHTING);
-
- //glEnable (GL_COLOR_MATERIAL);
- //glColorMaterial (GL_FRONT, GL_DIFFUSE);
- //glLightfv (GL_LIGHT0, GL_POSITION, lightPosition);
- //glEnable (GL_LIGHT0);
- //glShadeModel (GL_FLAT);
- //glEnable(GL_LIGHTING);
gdk_gl_drawable_gl_end (gldrawable);
--- 1148,1159 ----
glEnable (GL_DEPTH_TEST);
glDepthFunc (GL_LESS);
! glClearColor ( 0.7, 0.7, 0.8, 1.0);
! //glClearColor ( 0,0,0, 1.0);
gdk_gl_glPolygonOffsetEXT (proc, 1.0, 1.0);
glEnable (GL_CULL_FACE);
glHint (GL_LINE_SMOOTH_HINT, GL_NICEST);
glHint (GL_POLYGON_SMOOTH_HINT, GL_NICEST);
glDisable (GL_LIGHTING);
gdk_gl_drawable_gl_end (gldrawable);
***************
*** 1394,1423 ****
static GLuint VertShader;
! static char * VertSrc = \
! /* "!!ARBvp1.0 \n\ */
! /* PARAM CTM[4] = { state.matrix.mvp }; \n\ */
! /* ## Standard transform \n\ */
! /* DP4 result.position.x, CTM[0], vertex.position; \n\ */
! /* DP4 result.position.y, CTM[1], vertex.position; \n\ */
! /* DP4 result.position.z, CTM[2], vertex.position; \n\ */
! /* DP4 result.position.w, CTM[3], vertex.position; \n\ */
! /* # Rotate color values \n\ */
! /* MOV result.color, vertex.color.zxyw; \n\ */
! /* END\n" */
! /* ; */
"!!ARBvp1.0 \n\
- #This vp is used in zprecision demo for solving the z-precision problem\n\
- #in a perspective projection.\n\
- #AUTHOR: Vrej Melkonian\n\
- #DATE: Monday, June 23, 2003\n\
- PARAM m[4]={state.matrix.modelview}; \n\
PARAM mvp[4]={state.matrix.mvp}; \n\
! ATTRIB iPos=vertex.position; \n\
! OUTPUT oPos=result.position;\n\
! #Transform the vertex to clip coordinates.\n\
! DP4 r0.x, iPos, mvp[0];\n\
! DP4 r0.y, iPos, mvp[1];\n\
! DP4 r0.z, iPos, mvp[2];\n\
! DP4 r0.w, iPos, mvp[3];\n\
END\n ";
--- 1419,1437 ----
static GLuint VertShader;
! static char * VertSrc =
"!!ARBvp1.0 \n\
PARAM mvp[4]={state.matrix.mvp}; \n\
! TEMP r0,tmp; \n\
! DP4 r0.x, mvp[0], vertex.position; \n\
! DP4 r0.y, mvp[1], vertex.position; \n\
! DP4 r0.z, mvp[2], vertex.position; \n\
! DP4 r0.w, mvp[3], vertex.position; \n\
! #RCP tmp.w, r0.w; \n\
! RCP tmp, 8; \n\
! MUL tmp.w, r0.w, tmp.w; \n\
! #MUL r0.z, r0.z, r0.w; \n\
! MUL r0.z, r0.z, tmp.w; \n\
! MOV result.position, r0; \n\
! MOV result.color, vertex.color; \n\
END\n ";
***************
*** 1620,1630 ****
gl_draw_polygon_bbox( mod->sense_poly );
pop_color();
!
! /* // outline the polgons that the selected robot intersects with */
/* push_color_stgcolor( stg_lookup_color( "blue" )); */
! /* GList *a, *b; */
! /* //for( a=mod->polys ; a; a=a->next ) */
! /* //for( b = ((stg_polygon_t*)a->data)->intersectors; b; b=b->next ) */
/* for( b = mod->sense_poly->intersectors; b; b=b->next ) */
/* { */
--- 1634,1642 ----
gl_draw_polygon_bbox( mod->sense_poly );
pop_color();
! //}
! /* // outline the polgons that the selected robot intersects with */
/* push_color_stgcolor( stg_lookup_color( "blue" )); */
! /* GList *b; */
/* for( b = mod->sense_poly->intersectors; b; b=b->next ) */
/* { */
***************
*** 1639,1782 ****
/* glTranslatef( pose.x, pose.y, pose.z ); */
/* glRotatef( RTOD(pose.a), 0,0,1 ); */
-
- /* //gl_draw_polygon_bbox( p ); */
/* gl_draw_polygon3d( p ); */
/* glPopMatrix(); */
! /* } */
!
! int err;
! //if (! glutExtensionSupported("GL_ARB_vertex_program"))
! //Fail("GL_ARB_vertex_program not available on this machine");
! /* Vertex shader */
! glEnable(GL_VERTEX_PROGRAM_ARB);
! glGenProgramsARB(1, &VertShader);
! glBindProgramARB(GL_VERTEX_PROGRAM_ARB, VertShader);
! glProgramStringARB(GL_VERTEX_PROGRAM_ARB, GL_PROGRAM_FORMAT_ASCII_ARB,
! strlen(VertSrc), VertSrc);
! glGetIntegerv(GL_PROGRAM_ERROR_POSITION_ARB, &err);
! if (err >= 0)
! printf("Error in vertex shader %s\n",
! glGetString(GL_PROGRAM_ERROR_STRING_ARB));
! glPushAttrib( GL_ALL_ATTRIB_BITS );
! // these need to be set
! glEnable(GL_DEPTH_TEST);
! glDepthMask(GL_TRUE);
!
! glViewport( 0,0, 361, 100 );
- glMatrixMode (GL_PROJECTION);
- glPushMatrix();
- glLoadIdentity ();
- //glScalef( win->scale, win->scale, win->scale );
! double far = 8.0;
! double near = 0.1;
! gluPerspective( 45, 361/100, near, far );
! GLdouble projection[16];
! glGetDoublev(GL_PROJECTION_MATRIX, projection);
- // hack the projection matric to simulate a w-buffer
- projection[10] = -2.0/(far-near);
- projection[11] = (-far-near) / (far-near);
- glLoadMatrixd( projection );
! glMatrixMode( GL_MODELVIEW );
! glPushMatrix();
! glLoadIdentity();
- //glScalef( win->scale, win->scale, 1/100.0 );
! stg_pose_t gpose;
! stg_model_get_global_pose( world->win->selection_last, &gpose );
!
! //glRotatef( -90, 1,0,0 );
! //glRotatef( 90, 0,0,1 );
! gluLookAt( gpose.x, gpose.y, 0.4, // eye point
! gpose.x + cos( gpose.a), // view direction
! gpose.y + sin( gpose.a),
! 0.4,
! 0,0,1 );// Z is up
! push_color_stgcolor( stg_lookup_color( "gray" ));
! glTranslatef( gpose.x, gpose.y, 0 );
! GLUquadricObj* q = gluNewQuadric();
! gluQuadricDrawStyle( q, GLU_LINE );
! gluSphere( q, 8.0, 20, 20 );
! gluDeleteQuadric( q );
! glTranslatef( -gpose.x, -gpose.y, 0 );
! //glRotatef( RTOD( -gpose.a ), 0,0,1 );
! for( GList* b = mod->sense_poly->intersectors; b; b=b->next )
! {
! stg_polygon_t* p = (stg_polygon_t*)b->data;
! if( p->mod == world->win->selection_last )
! continue;
! push_color_stgcolor( p->mod->color );
! glPushMatrix();
! stg_pose_t pose;
! stg_model_get_global_pose( p->mod, &pose );
! // move into this model's coordinate frame
! glTranslatef( pose.x, pose.y, pose.z );
! glRotatef( RTOD(pose.a), 0,0,1 );
! //gl_draw_polygon_bbox( p );
! gl_draw_polygon3d( p );
! glPopMatrix();
! pop_color();
! }
! //glFlush();
! pop_color();
! //pop_color();
! glPopMatrix();
! glPopAttrib();
! glMatrixMode( GL_PROJECTION );
! glPopMatrix();
! GLfloat* depths = calloc( sizeof(GLfloat), 361 );
! glReadPixels( 0,50, 361, 1, GL_DEPTH_COMPONENT, GL_FLOAT, depths );
! puts("");
! for( int c=0; c<361; c++ )
! printf( "%.3f ", depths[c] );
! puts("");
- puts("");
- for( int c=0; c<361; c++ )
- printf( "%.3f ", near + depths[c] * (far-near) );
- puts("");
- }
/* glFrustum( 0, 361, */
--- 1651,1787 ----
/* glTranslatef( pose.x, pose.y, pose.z ); */
/* glRotatef( RTOD(pose.a), 0,0,1 ); */
/* gl_draw_polygon3d( p ); */
/* glPopMatrix(); */
! /* } */
! /* pop_color(); */
! }
!
! /* int err; */
! /* //if (! glutExtensionSupported("GL_ARB_vertex_program")) */
! /* //Fail("GL_ARB_vertex_program not available on this machine"); */
! /* /\* Vertex shader *\/ */
! /* glEnable(GL_VERTEX_PROGRAM_ARB); */
! /* glGenProgramsARB(1, &VertShader); */
! /* glBindProgramARB(GL_VERTEX_PROGRAM_ARB, VertShader); */
! /* glProgramStringARB(GL_VERTEX_PROGRAM_ARB,
GL_PROGRAM_FORMAT_ASCII_ARB, */
! /* strlen(VertSrc), VertSrc); */
! /* glGetIntegerv(GL_PROGRAM_ERROR_POSITION_ARB, &err); */
! /* if (err >= 0) */
! /* printf("Error in vertex shader %s\n", */
! /* glGetString(GL_PROGRAM_ERROR_STRING_ARB)); */
! /* glPushAttrib( GL_ALL_ATTRIB_BITS ); */
! /* // these need to be set */
! /* glEnable(GL_DEPTH_TEST); */
! /* glDepthMask(GL_TRUE); */
+ /* glViewport( 0,0, 361, 100 ); */
! /* glMatrixMode (GL_PROJECTION); */
! /* glPushMatrix(); */
! /* glLoadIdentity (); */
! /* //glScalef( win->scale, win->scale, win->scale ); */
! /* double far = 8.0; */
! /* double near = 0.1; */
! /* //glScalef( 1,1,1.0/far ); */
! /* gluPerspective( 45, 361/100, near, far ); */
! /* glMatrixMode( GL_MODELVIEW ); */
! /* glPushMatrix(); */
! /* glLoadIdentity(); */
! /* stg_pose_t gpose; */
! /* stg_model_get_global_pose( world->win->selection_last, &gpose ); */
! /* gluLookAt( gpose.x, gpose.y, 0.4, // eye point */
! /* gpose.x + cos( gpose.a), // view direction */
! /* gpose.y + sin( gpose.a), */
! /* 0.4, */
! /* 0,0,1 );// Z is up */
! /* push_color_stgcolor( stg_lookup_color( "gray" )); */
! /* glTranslatef( gpose.x, gpose.y, 0 ); */
! /* GLUquadricObj* q = gluNewQuadric(); */
! /* gluQuadricDrawStyle( q, GLU_LINE ); */
! /* gluSphere( q, 8.0, 20, 20 ); */
! /* gluDeleteQuadric( q ); */
! /* glTranslatef( -gpose.x, -gpose.y, 0 ); */
! /* //glRotatef( RTOD( -gpose.a ), 0,0,1 ); */
! /* for( GList* b = mod->sense_poly->intersectors; b; b=b->next )
*/
! /* { */
! /* stg_polygon_t* p = (stg_polygon_t*)b->data; */
! /* if( p->mod == world->win->selection_last ) */
! /* continue; */
! /* push_color_stgcolor( p->mod->color ); */
! /* glPushMatrix(); */
! /* stg_pose_t pose; */
! /* stg_model_get_global_pose( p->mod, &pose ); */
! /* // move into this model's coordinate frame */
! /* glTranslatef( pose.x, pose.y, pose.z ); */
! /* glRotatef( RTOD(pose.a), 0,0,1 ); */
! /* //gl_draw_polygon_bbox( p ); */
! /* gl_draw_polygon3d( p ); */
! /* glPopMatrix(); */
! /* pop_color(); */
! /* } */
! /* //glFlush(); */
! /* pop_color(); */
! /* //pop_color(); */
! /* glPopMatrix(); */
! /* glPopAttrib(); */
! /* glMatrixMode( GL_PROJECTION ); */
! /* glPopMatrix(); */
! /* GLfloat* depths = calloc( sizeof(GLfloat), 361 ); */
! /* glReadPixels( 0,50, 361, 1, GL_DEPTH_COMPONENT, GL_FLOAT, depths );
*/
! /* /\* puts(""); *\/ */
! /* /\* for( int c=0; c<361; c++ ) *\/ */
! /* /\* printf( "%.3f ", depths[c] ); *\/ */
! /* /\* puts(""); *\/ */
!
! /* // HACK alert. woop woop. */
! /* puts(""); */
! /* for( int c=0; c<361; c++ ) */
! /* printf( "%.2f ", 2 * ((depths[c] * (far-near)) -far/2.0) ); */
! /* puts(""); */
!
! /* } */
/* glFrustum( 0, 361, */
Index: Makefile.am
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/Makefile.am,v
retrieving revision 1.140.2.4
retrieving revision 1.140.2.5
diff -C2 -d -r1.140.2.4 -r1.140.2.5
*** Makefile.am 3 Mar 2007 02:20:52 -0000 1.140.2.4
--- Makefile.am 18 May 2007 05:21:07 -0000 1.140.2.5
***************
*** 20,24 ****
# TODO put this back in before release
# build stest, a stand-alone test binary using the stage library,
! #bin_PROGRAMS = stest
# The stage library contains everything you need to build a robot
--- 20,24 ----
# TODO put this back in before release
# build stest, a stand-alone test binary using the stage library,
! bin_PROGRAMS = stest
# The stage library contains everything you need to build a robot
***************
*** 115,119 ****
stest_DEPENDENCIES = libstage.la
stest_SOURCES = stest.c
! stest_LDADD = libstage.la
#stest_LDFLAGS =
--- 115,119 ----
stest_DEPENDENCIES = libstage.la
stest_SOURCES = stest.c
! stest_LDADD = libstage.la @GUI_LIBS@
#stest_LDFLAGS =
Index: model_laser.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/model_laser.c,v
retrieving revision 1.89.2.6
retrieving revision 1.89.2.7
diff -C2 -d -r1.89.2.6 -r1.89.2.7
*** model_laser.c 5 Mar 2007 00:42:29 -0000 1.89.2.6
--- model_laser.c 18 May 2007 05:21:07 -0000 1.89.2.7
***************
*** 192,197 ****
// get the sensor's pose in global coords
stg_pose_t pz;
! memcpy( &pz, &geom.pose, sizeof(pz) );
! stg_model_local_to_global( mod, &pz );
PRINT_DEBUG3( "laser origin %.2f %.2f %.2f", pz.x, pz.y, pz.a );
--- 192,196 ----
// get the sensor's pose in global coords
stg_pose_t pz;
! stg_model_get_global_pose( mod, &pz );
PRINT_DEBUG3( "laser origin %.2f %.2f %.2f", pz.x, pz.y, pz.a );
***************
*** 210,290 ****
// when the number of samples changes between calls - probably
// unusual.
! static stg_laser_sample_t* scan = 0;
! static size_t old_len = 0;
!
! /* size_t data_len = sizeof(stg_laser_sample_t)*cfg->samples; */
!
! /* if( old_len != data_len ) */
! /* { */
! /* scan = realloc( scan, data_len ); */
! /* old_len = data_len; */
! /* } */
!
scan = g_renew( stg_laser_sample_t, scan, cfg->samples );
- //memset( scan, 0, data_len );
! for( int t=0; t<cfg->samples; t += cfg->resolution )
! {
! double bearing = pz.a - cfg->fov/2.0 + sample_incr * t;
!
! itl_t* itl = itl_create2( mod, pz.x, pz.y,
! bearing,
! cfg->range_max,
! mod->world->matrix,
! PointToBearingRange,
! laser_raytrace_match );
! //double range = cfg->range_max;
! stg_model_t* hitmod = itl_next( itl );
!
! if( hitmod )
! {
! scan[t].range = MAX( itl->range, cfg->range_min );
! scan[t].hitpoint.x = itl->x;
! scan[t].hitpoint.y = itl->y;
! }
! else
{
! scan[t].range = cfg->range_max;
! scan[t].hitpoint.x = pz.x + scan[t].range * cos(bearing);
! scan[t].hitpoint.y = pz.y + scan[t].range * sin(bearing);
}
! itl_destroy( itl );
! // lower bound on range
! // if the object is bright, it has a non-zero reflectance
! if( hitmod )
{
! scan[t].reflectance =
! (hitmod->laser_return >= LaserBright) ? 1.0 : 0.0;
}
! else
! scan[t].reflectance = 0.0;
}
! /* we may need to resolution the samples we skipped */
! if( cfg->resolution > 1 )
! {
! for( int t=cfg->resolution; t<cfg->samples; t+=cfg->resolution )
! for( int g=1; g<cfg->resolution; g++ )
! {
! if( t >= cfg->samples )
! break;
! // copy the rightmost sample data into this point
! memcpy( &scan[t-g],
! &scan[t-cfg->resolution],
! sizeof(stg_laser_sample_t));
! double left = scan[t].range;
! double right = scan[t-cfg->resolution].range;
! // linear range interpolation between the left and right samples
! scan[t-g].range = (left-g*(left-right)/cfg->resolution);
! }
! }
stg_model_set_data( mod, scan, sizeof(stg_laser_sample_t) * cfg->samples);
--- 209,343 ----
// when the number of samples changes between calls - probably
// unusual.
! static stg_laser_sample_t* scan = NULL;
scan = g_renew( stg_laser_sample_t, scan, cfg->samples );
!
! stg_endpoint_t* ep = mod->world->endpts.x;
! assert( ep );
! // wind along to the first point to the right of the model
! //while( ep->value <= pz.x )
! // ep = ep->next;
!
! GList* hitlist = NULL;
!
! while( ep )
! {
! //printf( "pz x: %.2f\n", pz.x );
! //printf( "ep: %s %.2f\n", ep->polygon->mod->token, ep->value );
! //printf( "ep next: %s %.2f\n", ep->next->polygon->mod->token,
ep->next->value );
! double xmin=0,ymin=0;
! if( ep->type == STG_BEGIN )
{
! xmin = ep->polygon->epts[0].value;
! ymin = ep->polygon->epts[2].value;
!
! printf( "see polygon of %s starting at (%.2f,%.2f) ",
! ep->polygon->mod->token,
! xmin, ymin );
!
! hitlist = g_list_prepend( hitlist, ep->polygon );
}
! if( ep->type == STG_END )
! {
! xmin = ep->polygon->epts[1].value;
! ymin = ep->polygon->epts[3].value;
!
! printf( "see polygon of %s ending at (%.2f,%.2f) ",
! ep->polygon->mod->token,
! xmin, ymin );
! hitlist = g_list_remove( hitlist, ep->polygon );
! }
! double angle = RTOD(atan2( ymin - pz.y, xmin - pz.x ));
!
! //if( angle > -90 && angle < 180 )
{
! //ranges[(int)angle] = hypot( ymin - pz.y, xmin - pz.x );
! //ranges[(int)angle+1] = 4.0;
! //ranges[(int)angle+2] = 4.0;
}
!
!
! if( xmin )
! {
! printf( "angle %.2f hits", angle );
! GList* it;
! for( it=hitlist; it; it=it->next )
! printf( " %s", ((stg_polygon_t*)it->data)->mod->token );
! puts("");
! }
!
! ep = ep->next;
}
+
+
+
+ /* for( int t=0; t<cfg->samples; t += cfg->resolution ) */
+ /* { */
+ /* double bearing = pz.a - cfg->fov/2.0 + sample_incr * t; */
+
+ /* itl_t* itl = itl_create2( mod, pz.x, pz.y, */
+ /* bearing, */
+ /* cfg->range_max, */
+ /* mod->world->matrix, */
+ /* PointToBearingRange, */
+ /* laser_raytrace_match ); */
+
+ /* //double range = cfg->range_max; */
+
+ /* stg_model_t* hitmod = itl_next( itl ); */
! /* if( hitmod ) */
! /* { */
! /* scan[t].range = MAX( itl->range, cfg->range_min ); */
! /* scan[t].hitpoint.x = itl->x; */
! /* scan[t].hitpoint.y = itl->y; */
! /* } */
! /* else */
! /* { */
! /* scan[t].range = cfg->range_max; */
! /* scan[t].hitpoint.x = pz.x + scan[t].range * cos(bearing); */
! /* scan[t].hitpoint.y = pz.y + scan[t].range * sin(bearing); */
! /* } */
!
! /* itl_destroy( itl ); */
!
! /* // lower bound on range */
!
! /* // if the object is bright, it has a non-zero reflectance */
! /* if( hitmod ) */
! /* { */
! /* scan[t].reflectance = */
! /* (hitmod->laser_return >= LaserBright) ? 1.0 : 0.0; */
! /* } */
! /* else */
! /* scan[t].reflectance = 0.0; */
! /* } */
!
! /* /\* we may need to resolution the samples we skipped *\/ */
! /* if( cfg->resolution > 1 ) */
! /* { */
! /* for( int t=cfg->resolution; t<cfg->samples; t+=cfg->resolution ) */
! /* for( int g=1; g<cfg->resolution; g++ ) */
! /* { */
! /* if( t >= cfg->samples ) */
! /* break; */
! /* // copy the rightmost sample data into this point */
! /* memcpy( &scan[t-g], */
! /* &scan[t-cfg->resolution], */
! /* sizeof(stg_laser_sample_t)); */
! /* double left = scan[t].range; */
! /* double right = scan[t-cfg->resolution].range; */
! /* // linear range interpolation between the left and right samples */
! /* scan[t-g].range = (left-g*(left-right)/cfg->resolution); */
! /* } */
! /* } */
stg_model_set_data( mod, scan, sizeof(stg_laser_sample_t) * cfg->samples);
Index: model.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/model.c,v
retrieving revision 1.153.2.12
retrieving revision 1.153.2.13
diff -C2 -d -r1.153.2.12 -r1.153.2.13
*** model.c 2 Mar 2007 04:09:50 -0000 1.153.2.12
--- model.c 18 May 2007 05:21:07 -0000 1.153.2.13
***************
*** 362,365 ****
--- 362,366 ----
mod->sense_poly->intersectors = NULL;
mod->sense_poly->accum = g_hash_table_new( NULL, NULL );
+ //mod->sense_poly->accum = g_tree_new( accum_cmp );
mod->sense_poly->points = g_array_new( FALSE, TRUE, sizeof(stg_point_t));
g_array_append_vals( mod->sense_poly->points, pts, 4 );
***************
*** 370,374 ****
{
mod->sense_poly->epts[e].polygon = mod->sense_poly;
! mod->sense_poly->epts[e].type = (e % 2);// + 2; // STG_SENSOR_BEGIN or
STG_SENSOR_END
mod->sense_poly->epts[e].value = 0;
mod->sense_poly->epts[e].next = NULL;
--- 371,375 ----
{
mod->sense_poly->epts[e].polygon = mod->sense_poly;
! mod->sense_poly->epts[e].type = (e % 2) + 2; // STG_SENSOR_BEGIN or
STG_SENSOR_END
mod->sense_poly->epts[e].value = 0;
mod->sense_poly->epts[e].next = NULL;
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
