ParticleList& BeamEmitter::emit(Scalar _t, Scalar dt, Species* species_to_push)
{
t = _t;
if (species==species_to_push){
dt *= species->get_subcycleIndex()/(Scalar)species->get_supercycleIndex(); // expand dt for subcycled particles
if (init) initialize();
if (points!=0) return obliqueEmit(t, dt);
Scalar nPer_dt;
switch (get_xtFlag()) {
case 0:
case 1:
fAvg = get_time_value(t);
// FTotal = get_time_value(t);
break;
case 2:
break;
case 3:
case 4:
if(!Farray_compute) computeLocationArray();
Farray_compute = FALSE;
break;
}
nPer_dt = fAvg*emissionRate*dt;
// nPer_dt = FTotal*emissionRate*dt;
extra += nPer_dt;
if (extra < 1) return particleList; // < 1 particle to emit
Scalar del_t = dt/nPer_dt;
Vector2 xMKS;
while (extra >= 1){
extra -= 1;
xMKS = computeLocation();
/* if (alongx1()||!rweight)
xMKS = p1 + frand()*(p2 - p1);
else {
xMKS = Vector2(0.5*(p1.e1() + p2.e1()),
sqrt(rMin*rMin + (rMax*rMax - rMin*rMin)*frand()));
}
*/
Vector2 x = fields->getGridCoords(xMKS);
x+=delta; //make sure the particle isn't on the boundary
Vector3 u;
if (thetaDot.e3()){
// only correct if r*thetaDot << vz
Vector3 v = maxwellian->get_V(normal);
if (rweight) v+=xMKS.e2()*thetaDot;
u = v/sqrt(1-v*v*iSPEED_OF_LIGHT_SQ);
}
else
u = maxwellian->get_U(normal);
Particle* p = new Particle(x, u, species, np2c);
Boundary* bPtr = initialPush(del_t*extra, dt, *p);
if (!bPtr) particleList.add(p);
}
}
return particleList;
}
void *thread_Bluetooth(void *arg)
{
initLocationComputation();
while(1)
{
// Blocking function, cannot go faster than 500/8 ms
computeLocation();
}
}
void VectorLocalization2D::initialize(int _numParticles, const char* mapName, vector2f loc, float angle, float locationUncertainty, float angleUncertainty)
{
static const bool debug = false;
currentMap = &maps[0];
bool found = false;
for(unsigned int i=0; i<maps.size(); i++){
if(maps[i].mapName.compare(mapName)==0){
found = true;
currentMap = &maps[i];
}
}
if(!found){
char buf[2048];
snprintf(buf,2047,"Map %s not found in Atlas! Reverting to map %s.",mapName,maps[0].mapName.c_str());
TerminalWarning(buf);
}
numParticles = _numParticles;
particles.resize(numParticles);
if(debug) printf(" Initializing particles: 0.0%%\r");
fflush(stdout);
for(int i=0; i<numParticles; i++){
particles[i] = Particle2D(DBL_MAX,DBL_MAX,0.0,0.0);
}
for(int i=0; i<numParticles; i++){
if(debug && (i%10)==0){
printf(" Initializing particles: %5.1f%%\r",float(i)/float(numParticles)*100.0);
fflush(stdout);
}
particles[i] = createParticle(loc, angle, locationUncertainty, angleUncertainty);
}
computeLocation(loc, angle);
laserEval.numCorrespondences = 0;
laserEval.numObservedPoints = 0;
laserEval.runTime = 0;
laserEval.stage0Weights = 0;
laserEval.stageRWeights = 0;
pointCloudEval.numCorrespondences = 0;
pointCloudEval.numObservedPoints = 0;
pointCloudEval.runTime = 0;
pointCloudEval.stage0Weights = 0;
pointCloudEval.stageRWeights = 0;
if(debug) printf("\nDone.\n");
}
/* private */
void
PointLocator::computeLocation(const Coordinate& p, const Geometry *geom)
{
if (const Point *pt=dynamic_cast<const Point*>(geom))
{
updateLocationInfo(locate(p, pt));
}
else if (const LineString *ls=dynamic_cast<const LineString*>(geom))
{
updateLocationInfo(locate(p, ls));
}
else if (const Polygon *po=dynamic_cast<const Polygon*>(geom))
{
updateLocationInfo(locate(p, po));
}
else if (const MultiLineString *mls=dynamic_cast<const MultiLineString *>(geom))
{
for(std::size_t i=0, n=mls->getNumGeometries(); i<n; ++i)
{
const LineString *l=dynamic_cast<const LineString *>(mls->getGeometryN(i));
updateLocationInfo(locate(p,l));
}
}
else if (const MultiPolygon *mpo=dynamic_cast<const MultiPolygon *>(geom))
{
for(std::size_t i=0, n=mpo->getNumGeometries(); i<n; ++i)
{
const Polygon *p_po=dynamic_cast<const Polygon *>(mpo->getGeometryN(i));
updateLocationInfo(locate(p, p_po));
}
}
else if (const GeometryCollection *col=dynamic_cast<const GeometryCollection *>(geom))
{
for (GeometryCollection::const_iterator
it=col->begin(), endIt=col->end();
it != endIt;
++it)
{
const Geometry *g2=*it;
assert (g2!=geom); // is this check really needed ?
computeLocation(p, g2);
}
}
}
int
PointLocator::locate(const Coordinate& p, const Geometry *geom)
{
if (geom->isEmpty()) return Location::EXTERIOR;
const LineString *ls_geom = dynamic_cast<const LineString *>(geom);
if (ls_geom) return locate(p, ls_geom);
const Polygon *poly_geom = dynamic_cast<const Polygon *>(geom);
if (poly_geom) return locate(p, poly_geom);
isIn=false;
numBoundaries=0;
computeLocation(p,geom);
if (geomgraph::GeometryGraph::isInBoundary(numBoundaries)) return Location::BOUNDARY;
if (numBoundaries>0 || isIn) return Location::INTERIOR;
return Location::EXTERIOR;
}
void init() {
GLfloat sun_direction[] = { 0.0, 0.0, 0.0, 0.0 };
GLfloat sun_intensity[] = { 1.0, 1.0, 1.0, 1.0 };
GLfloat ambient_intensity[] = { 0.5, 0.5, 0.5, 1.0 };
glClearColor(1.0, 1.0, 1.0, 0.0);
computeLocation();
glEnable(GL_DEPTH_TEST);
glEnable(GL_LIGHTING);
glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambient_intensity);
glEnable(GL_LIGHT0);
glLightfv(GL_LIGHT0, GL_POSITION, sun_direction);
glLightfv(GL_LIGHT0, GL_AMBIENT, sun_intensity);
glEnable(GL_COLOR_MATERIAL);
glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
glDepthFunc(GL_LEQUAL);
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
}
void draw()
{
nodeDefense *d;
nodeTiro *t;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); /*Limpa tudo*/
glLoadIdentity();
computeLocation();
gluLookAt(
0.0, 0.0, -5.0, /*Olho*/
0.0, 0.0, 0.0, /*Centro*/
0.0, 1.0, 0.0); /*Orientação (topo na direção Y)*/
glColor4f(1.0, 1.0, 1.0, 1.0);
glEnable(GL_NORMALIZE);
glEnable(GL_TEXTURE_2D);
glShadeModel(GL_SMOOTH);
glColor4f(1.0, .85, 0.0,1.0);
glDisable(GL_LIGHTING);
glDisable(GL_LIGHT0);
glPushMatrix();
glTranslatef(0., -10., -30.);
/*~Rio~*/
glColor4f(0.0, 0.0, 0.5, 1.0);
glBegin(GL_TRIANGLES);
{
glVertex3f(-60., 0., 0.);
glVertex3f(-60., 0., 500.);
glVertex3f(60., 0., 0.);
glVertex3f(60., 0., 0.);
glVertex3f(-60., 0., 500.);
glVertex3f(60., 0., 500.);
}
glEnd();
/*Terra*/
glColor4f(0.48, 0.25, 0.01, 1.0);
glBegin(GL_TRIANGLES);
{
glVertex3f(60., 0., 0.);
glVertex3f(60., 0., 500.);
glVertex3f(220., 0., 0.);
glVertex3f(220., 0., 0.);
glVertex3f(60., 0., 500.);
glVertex3f(220., 0., 500.);
}
glEnd();
glBegin(GL_TRIANGLES);
{
glVertex3f(-60., 0., 0.);
glVertex3f(-60., 0., 500.);
glVertex3f(-220., 0., 0.);
glVertex3f(-220., 0., 0.);
glVertex3f(-60., 0., 500.);
glVertex3f(-220., 0., 500.);
}
glEnd();
/*Céu*/
glColor4f(0.6,0.75, 1. ,1.);
glBegin(GL_TRIANGLES);
{
glVertex3f(-220., 0., 500.);
glVertex3f(220., 0., 500.);
glVertex3f(-220., 200., 500.);
glVertex3f(220., 200., 500.);
glVertex3f(220., 0., 500.);
glVertex3f(-220., 200., 500.);
}
glEnd();
glPopMatrix();
glBegin(GL_QUADS);
{
glVertex3f( 21., 26., -19);
glVertex3f( 5.5, 26., -19);
glVertex3f( 5.5, 23., -19);
glVertex3f( 21., 23., -19);
}
glEnd();
glColor3f(0.0, 0.0, 0.0);
sprintf(jogador, "HP: %d Vidas: %d Pontos: %d", ship->hp, ship->vidas, ship->score);
glRasterPos3f(16., 23.,-20);
glutBitmapString(GLUT_BITMAP_TIMES_ROMAN_24, jogador);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
for (d = defenseList->next; d != NULL; d = d->next)
{
glPushMatrix();
glTranslatef(d->defense->position->x, d->defense->position->y, d->defense->position->z);
glBegin(GL_QUADS);
{
glColor3f(0.5f, 0.5f, 0.5f);
glVertex3f( 0.8f, 0.8f, -0.8f);
glVertex3f(-0.8f, 0.8f, -0.8f);
glColor3f(0.3f, 0.3f, 0.3f);
glVertex3f(-0.8f, 0.8f, 0.8f);
glVertex3f( 0.8f, 0.8f, 0.8f);
/*Bottom face (y = -0.8f)*/
glColor3f(0.5f, 0.5f, 0.5f);
glVertex3f( 0.8f, -3.0f, 0.8f);
glVertex3f(-0.8f, -3.0f, 0.8f);
glColor3f(0.3f, 0.3f, 0.3f);
glVertex3f(-0.8f, -3.0f, -0.8f);
glVertex3f( 0.8f, -3.0f, -0.8f);
/* Front face (z = 0.8f)*/
glColor3f(0.5f, 0.5f, 0.5f);
glVertex3f( 0.8f, 0.8f, 0.8f);
glVertex3f(-0.8f, 0.8f, 0.8f);
glColor3f(0.3f, 0.3f, 0.3f);
glVertex3f(-0.8f, -3.0f, 0.8f);
glVertex3f( 0.8f, -3.0f, 0.8f);
/* Back face (z = -0.8f)*/
glColor3f(0.5f, 0.5f, 0.5f);
glVertex3f( 0.8f, -3.0f, -0.8f);
glVertex3f(-0.8f, -3.0f, -0.8f);
glColor3f(0.3f, 0.3f, 0.3f);
glVertex3f(-0.8f, 0.8f, -0.8f);
glVertex3f( 0.8f, 0.8f, -0.8f);
/* Left face (x = -0.8f)*/
glColor3f(0.5f, 0.5f, 0.5f);
glVertex3f(-0.8f, 0.8f, 0.8f);
glVertex3f(-0.8f, 0.8f, -0.8f);
glColor3f(0.3f, 0.3f, 0.3f);
glVertex3f(-0.8f, -3.0f, -0.8f);
glVertex3f(-0.8f, -3.0f, 0.8f);
/* Right face (x = 0.8f)*/
glColor3f(0.5f, 0.5f, 0.5f);
glVertex3f(0.8f, 0.8f, -0.8f);
glVertex3f(0.8f, 0.8f, 0.8f);
glColor3f(0.3f, 0.3f, 0.3f);
glVertex3f(0.8f, -3.0f, 0.8f);
glVertex3f(0.8f, -3.0f, -0.8f);
}
glEnd();
glPopMatrix();
}
for (t = tiroList->next; t != NULL; t = t->next)
{
glPushMatrix();
glTranslatef(t->tiro->position->x, t->tiro->position->y, t->tiro->position->z);
if (t->tiro->source == NAVE)
glColor3f(0.0f, 0.0f, 0.0f);
else glColor3f(0.5f, 0.27f, 0.0f);
glutSolidSphere(0.15, 60, 60);
glPopMatrix();
}
drawShip();
glPushMatrix();
glTranslatef(ship->orientation->x, ship->orientation->y, -.5);
glColor3f(0.0, 1.0, 0.0);
glBegin(GL_LINE_LOOP);
glVertex3f(-0.5, 1.0, 0.0);
glVertex3f(-0.5, 3.0, 0.0);
glVertex3f(1.5, 3.0, 0.0);
glVertex3f(1.5, 1.0, 0.0);
glEnd();
glPopMatrix();
glPushMatrix();
glTranslatef(ship->orientation->x, ship->orientation->y, 0.0);
glBegin(GL_LINE_LOOP);
glVertex3f(0.0, 1.3, 0.0);
glVertex3f(0.0, 2.6, 0.0);
glVertex3f(1.0, 2.6, 0.0);
glVertex3f(1.0, 1.3, 0.0);
glEnd();
glPopMatrix();
glutSwapBuffers();
glFlush();
}
void VectorLocalization2D::drawDisplay(vector<float> &lines_p1x, vector<float> &lines_p1y, vector<float> &lines_p2x, vector<float> &lines_p2y, vector<uint32_t> &lines_color,
vector<float> &points_x, vector<float> &points_y, vector<uint32_t> &points_color,
vector<float> &circles_x, vector<float> &circles_y, vector<uint32_t> &circles_color, float scale)
{
static const bool debug = false;
static const bool drawParticles = true;
static const bool drawLidarPoints = true;
static const bool drawKinectPoints = true;
static const bool drawCorrections = true;
static const bool drawSceneRender = false;
static const float GradientsScale = 1.0;
static const int ParticleCloudColor = 0xF77274;
static const int LidarPointColor = 0xF0761F;
static const int LidarGradientColor = 0x2F95ED;
static const int CloudPointColor = 0xDE2352;
static const int CloudGradientColor = 0x8A23DE;
static const int LineCorrespondenceColor = 0x93FA70;
static const int LineExtentColor = 0xFFD659;
static const int DebugColour = 0xFF0000;
for(int i=0; i<debugLines.size(); i++){
lines_p1x.push_back(scale*debugLines[i].P0().x);
lines_p1y.push_back(scale*debugLines[i].P0().y);
lines_p2x.push_back(scale*debugLines[i].P1().x);
lines_p2y.push_back(scale*debugLines[i].P1().y);
lines_color.push_back(DebugColour);
}
debugLines.clear();
if(drawCorrections){
assert(locCorrectionP0.size() == locCorrectionP1.size());
for(int i=0; i<int(locCorrectionP0.size()); i++){
vector2f p2 = locCorrectionP1[i] + 20.0*(locCorrectionP1[i] - locCorrectionP0[i]);
lines_p1x.push_back(scale*locCorrectionP0[i].x);
lines_p1y.push_back(scale*locCorrectionP0[i].y);
lines_p2x.push_back(scale*p2.x);
lines_p2y.push_back(scale*p2.y);
lines_color.push_back(0x1BE042);
}
locCorrectionP0.clear();
locCorrectionP1.clear();
}
if(drawParticles){
//Draw all (unrefined) particles
for(int i=0; i<particles.size(); i++){
circles_x.push_back(scale*particles[i].loc.x);
circles_y.push_back(scale*particles[i].loc.y);
circles_color.push_back(ParticleCloudColor);
vector2f h;
h.heading(particles[i].angle);
vector2f p = particles[i].loc + 0.3*h;
lines_p1x.push_back(scale*particles[i].loc.x);
lines_p1y.push_back(scale*particles[i].loc.y);
lines_p2x.push_back(scale*p.x);
lines_p2y.push_back(scale*p.y);
lines_color.push_back(ParticleCloudColor);
}
}
if(drawSceneRender){
vector2f loc;
float angle;
computeLocation(loc,angle);
vector<line2f> lines = currentMap->sceneRender(loc,0.0,RAD(359.9));
vector<int> locVisibilityList;
if(currentMap->preRenderExists)
locVisibilityList = *currentMap->getVisibilityList(loc.x, loc.y);
else
locVisibilityList = currentMap->getSceneLines(loc, 6.5);
for(unsigned int i=0; i<locVisibilityList.size(); i++){
lines_p1x.push_back(scale*currentMap->lines[locVisibilityList[i]].P0().x);
lines_p1y.push_back(scale*currentMap->lines[locVisibilityList[i]].P0().y);
lines_p2x.push_back(scale*currentMap->lines[locVisibilityList[i]].P1().x);
lines_p2y.push_back(scale*currentMap->lines[locVisibilityList[i]].P1().y);
lines_color.push_back(0x00DE07);
}
vector<vector2f> rays;
vector2f ray;
bool duplicate;
for(unsigned int i=0; i<lines.size(); i++){
lines_p1x.push_back(scale*lines[i].P0().x);
lines_p1y.push_back(scale*lines[i].P0().y);
lines_p2x.push_back(scale*lines[i].P1().x);
lines_p2y.push_back(scale*lines[i].P1().y);
lines_color.push_back(DebugColour);
duplicate = false;
ray = lines[i].P0()-loc;
for(int j=0; j<rays.size()&&!duplicate; j++){
if(ray.norm().dot(rays[j].norm())>cos(RAD(0.1)) && fabs(ray.length()-rays[j].length())<0.01){
duplicate = true;
j = rays.erase(rays.begin()+j) - rays.begin();
}
}
if(!duplicate)
rays.push_back(ray);
duplicate = false;
ray = lines[i].P1()-loc;
for(int j=0; j<rays.size()&&!duplicate; j++){
if(ray.norm().dot(rays[j].norm())>cos(RAD(0.1)) && fabs(ray.length()-rays[j].length())<0.01){
duplicate = true;
j = rays.erase(rays.begin()+j) - rays.begin();
}
}
if(!duplicate)
rays.push_back(ray);
}
for(int i=0; i<rays.size(); i++){
vector2f p0 = loc;
vector2f p1 = rays[i]+loc;
lines_p1x.push_back(scale*p0.x);
lines_p1y.push_back(scale*p0.y);
lines_p2x.push_back(scale*p1.x);
lines_p2y.push_back(scale*p1.y);
lines_color.push_back(LineExtentColor);
}
}
if(drawLidarPoints){
// Draw all LIDAR points considered for gradient descent, as well as their gradients
for(int i=0; i<points.size(); i++){
points_x.push_back(scale*points[i].x());
points_y.push_back(scale*points[i].y());
points_color.push_back(LidarPointColor);
Vector2f p = points[i] - GradientsScale*gradients[i];
lines_p1x.push_back(scale*points[i].x());
lines_p1y.push_back(scale*points[i].y());
lines_p2x.push_back(scale*p.x());
lines_p2y.push_back(scale*p.y());
lines_color.push_back(LidarGradientColor);
}
}
if(drawKinectPoints){
// Draw all point cloud points considered for gradient descent, as well as their gradients
for(int i=0; i<points2.size(); i++){
points_x.push_back(scale*points2[i].x());
points_y.push_back(scale*points2[i].y());
points_color.push_back(CloudPointColor);
Vector2f p = points2[i] - GradientsScale*gradients2[i];
lines_p1x.push_back(scale*points2[i].x());
lines_p1y.push_back(scale*points2[i].y());
lines_p2x.push_back(scale*p.x());
lines_p2y.push_back(scale*p.y());
lines_color.push_back(CloudGradientColor);
}
}
}
