69 const std::string& type ) :
70 Model( world, parent, type ),
72 max_range_anon( 8.0 ),
108 return( ! finder->
IsRelated( candidate ) );
131 double dx = hispose.
x - mypose.
x;
132 double dy = hispose.
y - mypose.
y;
133 double range = hypot( dy, dx );
147 double bearing = atan2( dy, dx );
148 double dtheta =
normalize(bearing - mypose.
a);
150 if( fabs(dtheta) >
fov/2.0 )
197 assert( range >= 0 );
207 fid.bearing = dtheta;
209 fid.geom.y = hisgeom.size.y;
210 fid.geom.z = hisgeom.size.z;
253 std::set<Model*,World::ltx>::iterator xmin =
257 const std::set<Model*,World::ltx>::iterator xmax =
261 std::set<Model*,World::lty>::iterator ymin =
265 const std::set<Model*,World::lty>::iterator ymax =
269 std::set<Model*> horiz, vert;
271 for( ; xmin != xmax; ++xmin)
272 horiz.insert( *xmin);
274 for( ; ymin != ymax; ++ymin )
275 vert.insert( *ymin );
278 std::vector<Model*> nearby;
279 std::set_intersection( horiz.begin(), horiz.end(),
280 vert.begin(), vert.end(),
281 std::inserter( nearby, nearby.end() ) );
322 GLUquadric* quadric = gluNewQuadric();
324 gluQuadricDrawStyle( quadric, GLU_SILHOUETTE );
326 gluPartialDisk( quadric,
334 gluDeleteQuadric( quadric );
345 glLineStipple( 1, 0x00FF );
355 glEnable(GL_LINE_STIPPLE);
358 glVertex2f( dx, dy );
360 glDisable(GL_LINE_STIPPLE);
365 glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
371 snprintf(idstr, 31,
"%d", fid.
id );
374 glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
radians_t fov
field of view
meters_t range
range to the target
virtual void PushColor(Color col)
meters_t max_range_anon
maximum detection range
void coord_shift(double x, double y, double z, double a)
int subs
the number of subscriptions to this model
The Stage library uses its own namespace.
bool ignore_zloc
Are we ignoring the Z-loc of the fiducials we detect compared to the fiducial detector?
virtual void DataVisualize(Camera *cam)
std::set< Model *, ltx > models_with_fiducials_byx
void AddModelIfVisible(Model *him)
std::vector< Fiducial > fiducials
RaytraceResult Raytrace(const Pose &pose, const meters_t range, const ray_test_func_t func, const void *arg, const bool ztest=true)
static bool fiducial_raytrace_match(Model *candidate, Model *finder, const void *dummy)
Model * mod
Pointer to the model (real fiducial detectors can't do this!)
bool IsRelated(const Model *testmod) const
double normalize(double a)
void RegisterOption(Option *opt)
Pose GetGlobalPose() const
radians_t bearing
bearing to the target
meters_t max_range_id
maximum range at which the ID can be read
double ReadLength(int entity, const char *name, double value)
void draw_string(float x, float y, float z, const char *string)
class Stg::Model::Visibility vis
Pose geom
size and relative angle of the target
std::set< Model *, lty > models_with_fiducials_byy
std::vector< Model * > models_with_fiducials
int ReadInt(int entity, const char *name, int value)
void SetGeom(const Geom &src)
ModelFiducial(World *world, Model *parent, const std::string &type)
radians_t a
rotation about the z axis.
meters_t min_range
minimum detection range
double ReadAngle(int entity, const char *name, double value)
Model * mod
the model struck by this beam
int id
the fiducial identifier of the target (i.e. its fiducial_return value), or -1 if none can be detected...