Revision: 7233
http://playerstage.svn.sourceforge.net/playerstage/?rev=7233&view=rev
Author: thjc
Date: 2009-01-04 01:37:56 +0000 (Sun, 04 Jan 2009)
Log Message:
-----------
applied 2150737 - bumper detects collisions incorrectly
Modified Paths:
--------------
code/stage/branches/release-2-1-patches/src/model_bumper.c
Modified: code/stage/branches/release-2-1-patches/src/model_bumper.c
===================================================================
--- code/stage/branches/release-2-1-patches/src/model_bumper.c 2009-01-04
01:34:05 UTC (rev 7232)
+++ code/stage/branches/release-2-1-patches/src/model_bumper.c 2009-01-04
01:37:56 UTC (rev 7233)
@@ -13,7 +13,7 @@
/**
@ingroup model
-...@defgroup model_bumper Bumper/Whisker model
+...@defgroup model_bumper Bumper/Whisker model
The bumper model simulates an array of binary touch sensors.
<h2>Worldfile properties</h2>
@@ -21,7 +21,7 @@
@par Summary and default values
@verbatim
-bumper
+bumper
(
# bumper properties
bcount 1
@@ -35,10 +35,10 @@
The bumper model allows configuration of the pose and length parameters of
each transducer seperately using bpose[index] and blength[index]. For
convenience, the length of all bumpers in the array can be set at once with the
blength property. If a blength with no index is specified, this global setting
is applied first, then specific blengh[index] properties are applied
afterwards. Note that the order in the worldfile is ignored.
@par Details
-- bcount int
+- bcount int
- the number of bumper transducers
- bpose[\<transducer index\>] [float float float]
- - [x y theta]
+ - [x y theta]
- pose of the center of the transducer relative to its parent.
- blength float
- sets the length in meters of all transducers in the array
@@ -74,14 +74,14 @@
mod->f_shutdown = bumper_shutdown;
mod->f_update = NULL; // installed on startup & shutdown
mod->f_load = bumper_load;
-
+
stg_model_set_data( mod, NULL, 0 );
-
+
// Set up sensible defaults
stg_geom_t geom;
memset( &geom, 0, sizeof(geom)); // no size
stg_model_set_geom( mod, &geom );
-
+
// remove the polygon: bumper has no body
stg_model_set_polygons( mod, NULL, 0 );
@@ -89,9 +89,9 @@
memset( &cfg, 0, sizeof(cfg));
cfg.length = 0.1;
stg_model_set_cfg( mod, &cfg, sizeof(cfg) );
-
+
// adds a menu item and associated on-and-off callbacks
- stg_model_add_property_toggles( mod,
+ stg_model_add_property_toggles( mod,
&mod->data,
bumper_render_data, // called when toggled on
NULL,
@@ -122,7 +122,7 @@
mod->f_update = NULL;
stg_model_set_watts( mod, 0 );
-
+
// clear the data - this will unrender it too.
stg_model_set_data( mod, NULL, 0 );
@@ -136,11 +136,11 @@
if (bcount > 0)
{
char key[256];
-
- size_t cfglen = bcount * sizeof(stg_bumper_config_t);
+
+ size_t cfglen = bcount * sizeof(stg_bumper_config_t);
stg_bumper_config_t* cfg = (stg_bumper_config_t*)
- calloc( sizeof(stg_bumper_config_t), bcount );
-
+ calloc( sizeof(stg_bumper_config_t), bcount );
+
double common_length = wf_read_length(mod->id, "blength", cfg[0].length
);
// set all transducers with the common setting
@@ -156,13 +156,13 @@
snprintf(key, sizeof(key), "bpose[%d]", i);
cfg[i].pose.x = wf_read_tuple_length(mod->id, key, 0, 0);
cfg[i].pose.y = wf_read_tuple_length(mod->id, key, 1, 0);
- cfg[i].pose.a = wf_read_tuple_angle(mod->id, key, 2, 0);
+ cfg[i].pose.a = wf_read_tuple_angle(mod->id, key, 2, 0);
snprintf(key, sizeof(key), "blength[%d]", i);
- cfg[i].length = wf_read_length(mod->id, key, common_length );
+ cfg[i].length = wf_read_length(mod->id, key, common_length );
}
-
- stg_model_set_cfg( mod, cfg, cfglen );
+
+ stg_model_set_cfg( mod, cfg, cfglen );
free( cfg );
}
}
@@ -171,11 +171,11 @@
{
// Ignore myself, my children, and my ancestors.
return( hitmod->obstacle_return && !stg_model_is_related(mod,hitmod) );
-}
+}
int bumper_update( stg_model_t* mod )
-{
+{
//if( mod->subs < 1 )
//return 0;
@@ -185,13 +185,13 @@
if( len < sizeof(stg_bumper_config_t) )
return 0; // nothing to see here
-
+
int rcount = len / sizeof(stg_bumper_config_t);
-
+
static stg_bumper_sample_t* data = NULL;
static size_t last_datalen = 0;
- size_t datalen = sizeof(stg_bumper_sample_t) * rcount;
+ size_t datalen = sizeof(stg_bumper_sample_t) * rcount;
if( datalen != last_datalen )
{
@@ -202,7 +202,7 @@
memset( data, 0, datalen );
if( fig_debug_rays ) stg_rtk_fig_clear( fig_debug_rays );
-
+
int t;
for( t=0; t<rcount; t++ )
{
@@ -212,18 +212,18 @@
pz.a = pz.a + M_PI/2.0;
pz.x -= cfg[t].length/2.0 * cos(pz.a);
- //pz.y -= cfg[t].length/2.0 * sin(cfg[t].pose.a );
-
+ pz.y -= cfg[t].length/2.0 * sin(pz.a);
+
stg_model_local_to_global( mod, &pz );
-
+
itl_t* itl = itl_create( pz.x, pz.y, pz.a,
cfg[t].length,
mod->world->matrix,
PointToBearingRange );
-
- stg_model_t * hitmod =
+
+ stg_model_t * hitmod =
itl_first_matching( itl, bumper_raytrace_match, mod );
-
+
if( hitmod )
{
data[t].hit = hitmod;
@@ -233,7 +233,7 @@
itl_destroy( itl );
}
-
+
stg_model_set_data( mod, data, datalen );
return 0;
@@ -248,7 +248,7 @@
int bumper_render_data( stg_model_t* mod, void* userp )
{
PRINT_DEBUG( "bumper render data" );
-
+
stg_rtk_fig_t* fig = stg_model_get_fig( mod, "bumper_data_fig" );
if( !fig )
@@ -257,15 +257,15 @@
}
stg_rtk_fig_clear(fig);
-
- stg_bumper_config_t* cfg = (stg_bumper_config_t*)mod->cfg;
+
+ stg_bumper_config_t* cfg = (stg_bumper_config_t*)mod->cfg;
int rcount = mod->cfg_len / sizeof( stg_bumper_config_t );
if( rcount < 1 ) // no samples
return 0;
-
+
size_t dlen = mod->data_len;
stg_bumper_sample_t *data = (stg_bumper_sample_t*)mod->data;
-
+
// iff we have the right amount of data
if( dlen == rcount * sizeof(stg_bumper_sample_t) )
{
@@ -274,14 +274,14 @@
{
stg_meters_t thick = STG_BUMPER_NOHIT_THICKNESS;
stg_color_t col = STG_BUMPER_NOHIT_RGB;
-
+
if( data[s].hit )
{
col = STG_BUMPER_HIT_RGB;
thick = STG_BUMPER_HIT_THICKNESS;
}
-
- stg_rtk_fig_color_rgb32(fig, col );
+
+ stg_rtk_fig_color_rgb32(fig, col );
stg_rtk_fig_rectangle( fig,
cfg[s].pose.x,
cfg[s].pose.y,
@@ -289,8 +289,8 @@
cfg[s].length,
thick,
1 );
-
- stg_rtk_fig_color_rgb32(fig, 0 ); //black
+
+ stg_rtk_fig_color_rgb32(fig, 0 ); //black
stg_rtk_fig_rectangle( fig,
cfg[s].pose.x,
cfg[s].pose.y,
@@ -298,14 +298,14 @@
cfg[s].length,
thick,
0 );
-
+
}
}
else
if( dlen > 0 )
PRINT_WARN2( "data size doesn't match configuation (%l/%l bytes)",
(int)dlen, (int)(rcount * sizeof(stg_bumper_sample_t) ));
-
+
return 0; // keep running
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
