void paint (Graphics& g) override
{
g.fillAll (Colour::greyLevel (0.4f));
g.setColour (Colours::lightgrey);
g.setFont (14.0f);
g.drawFittedText ("Drag here with as many fingers as you have!",
getLocalBounds().reduced (30), Justification::centred, 4);
for (int i = 0; i < trails.size(); ++i)
drawTrail (*trails.getUnchecked(i), g);
}
void Helper::paint(QPainter *painter, QPaintEvent *event, int elapsed, bool timerStopped)
{
painter->fillRect(event->rect(), background);
for (int i = 0; i < b->lights.length(); i++) {
glm::vec2 position = b->lights.at(i)->getPosition();
drawLight(painter, position.x, position.y, b->lights.at(i)->radius);
drawTrail(painter, b->lights.at(i)->trail, b->lights.at(i)->trailColor);
}
for (int j = 0; j < b->mosquitoes.length(); j++) {
if (!b->mosquitoes.at(j)->isEaten) {
drawMosquito(painter, b->mosquitoes.at(j)->position.x, b->mosquitoes.at(j)->position.y);
}
}
for (int k = 0; k < b->walls.length(); k++) {
drawWall(painter, b->walls.at(k)->point1.x, b->walls.at(k)->point1.y, b->walls.at(k)->point2.x, b->walls.at(k)->point2.y);
}
drawFrog(painter, b->frog->position.x, b->frog->position.y, b->frog->radius);
}
//-----------------------------------------------------------------------------
// draw this pedestrian into scene
void Pedestrian::draw( const float currentTime, const float elapsedTime )
{
SLBaseClass::draw( currentTime, elapsedTime );
#if 0
Color kColor;
Vec3 kPosition = position();
bool bGotParentColor = false;
AbstractPlugin* parentPlugin = dynamic_cast<AbstractPlugin*>(getParentEntity());
ZonePlugin* zonePlugin = NULL;
if( NULL != parentPlugin )
{
zonePlugin = dynamic_cast<ZonePlugin*>(parentPlugin->getParentPlugin());
bGotParentColor = parentPlugin->queryVehicleColor( *this, kColor ) ;
if( true == bGotParentColor )
{
}
else
{
if( true == isRemoteObject() )
{
kColor = gGreen;
Vec3 kTempPosition = kPosition;
kTempPosition.y += 0.05f;
setPosition( kTempPosition );
}
else
{
kColor = gRed;
}
kColor.setA( 0.5f );
}
}
drawBasic2dCircularVehicle (*this, kColor);
setPosition( kPosition );
kColor.setA( 1.0f );
EAnnotationMode eMode = getAnnotationMode();
setAnnotationMode( EAnnotationMode_local );
drawTrail( kColor, gWhite );
setAnnotationMode( eMode );
if( NULL != zonePlugin )
{
// check for zone memberships
// textual annotation
std::ostringstream annote;
annote << std::setprecision (2) << std::setiosflags (std::ios::fixed);
annote << "z[";
for( size_t i = 0; i < 4; ++i )
{
if( true == getIsZoneMember(i) )
{
annote << i;
}
else
{
annote << " ";
}
if( i < 3 )
{
annote << "-";
}
}
annote << "]\n";
//draw borders
annote << "b[";
for( size_t i = 0; i < 4; ++i )
{
if( true == getIsZoneBorderMember(i) )
{
annote << i;
}
else
{
annote << " ";
}
if( i < 3 )
{
annote << "-";
}
}
annote << "]";
draw2dTextAt3dLocation (annote, position(), gWhite, drawGetWindowWidth(), drawGetWindowHeight());
}
#endif
}
void MapView::Render()
{
// get new pose.
pthread_mutex_lock(&filter->filter_CS);
lastFramePoseSpeed = filter->getCurrentPoseSpeedAsVec(); // Note: this is maybe an old pose, but max. one frame old = 50ms = not noticable.
pthread_mutex_unlock(&filter->filter_CS);
if(clearTrail)
{
trailPoints.clear();
clearTrail = false;
}
// render
bool addTrail;
if(trailPoints.size() == 0)
addTrail = true;
else
{
TooN::Vector<3> distToLast = lastFramePoseSpeed.slice<0,3>() - trailPoints[trailPoints.size()-1].pointFilter;
double d = distToLast[0]*distToLast[0] + distToLast[1]*distToLast[1]+distToLast[2]*distToLast[2];
addTrail = d > 0.1*0.1;
}
// the following complicated code is to save trail-points in ptam-scale, such that scale-reestimation will re-scale the drawn path.
if(addTrail)
{
if(ptamWrapper->PTAMStatus == ptamWrapper->PTAM_BEST ||
ptamWrapper->PTAMStatus == ptamWrapper->PTAM_TOOKKF ||
ptamWrapper->PTAMStatus == ptamWrapper->PTAM_GOOD)
{
if(ptamWrapper->PTAMInitializedClock != 0 && getMS() - ptamWrapper->PTAMInitializedClock > 200)
{
TooN::Vector<3> PTAMScales = filter->getCurrentScales();
TooN::Vector<3> PTAMOffsets = filter->getCurrentOffsets().slice<0,3>();
TooN::Vector<3> ptamPointPos = lastFramePoseSpeed.slice<0,3>();
ptamPointPos -= PTAMOffsets;
ptamPointPos /= PTAMScales[0];
trailPoints.push_back(TrailPoint(
lastFramePoseSpeed.slice<0,3>(),
ptamPointPos
));
}
}
else if(ptamWrapper->PTAMStatus == ptamWrapper->PTAM_LOST ||
ptamWrapper->PTAMStatus == ptamWrapper->PTAM_FALSEPOSITIVE)
{
if(ptamWrapper->PTAMInitializedClock != 0 && getMS() - ptamWrapper->PTAMInitializedClock > 200)
{
TooN::Vector<3> PTAMScales = filter->getCurrentScales();
TooN::Vector<3> PTAMOffsets = filter->getCurrentOffsets().slice<0,3>();
TooN::Vector<3> ptamPointPos = lastFramePoseSpeed.slice<0,3>();
ptamPointPos -= PTAMOffsets;
ptamPointPos /= PTAMScales[0];
trailPoints.push_back(TrailPoint(
lastFramePoseSpeed.slice<0,3>(),
ptamPointPos
));
}
}
else
{
trailPoints.push_back(TrailPoint(
lastFramePoseSpeed.slice<0,3>()
));
}
}
if(resetMapViewFlag)
{
resetMapView();
resetMapViewFlag = false;
}
// get lineWidthFactor
lineWidthFactor = sqrt((float)(myGLWindow->size()[0] * myGLWindow->size()[1] / (640*480)));
plotGrid();
pthread_mutex_lock(&ptamWrapper->shallowMapCS);
std::vector<tse3>* kfl = &(ptamWrapper->keyFramesTransformed);
// draw keyframes
for(unsigned int i=0;i<kfl->size();i++)
{
plotCam((*kfl)[i],false,2,0.04f,1);
}
// draw trail
drawTrail();
// draw keypoints
plotMapPoints();
pthread_mutex_unlock(&ptamWrapper->shallowMapCS);
// draw predicted cam
// real in opaque
predConvert->setPosRPY(lastFramePoseSpeed[0], lastFramePoseSpeed[1], lastFramePoseSpeed[2], lastFramePoseSpeed[3], lastFramePoseSpeed[4], lastFramePoseSpeed[5]);
plotCam(predConvert->droneToGlobal,true,5.0f,0.2f,1);
// --------------------- make msg ------------------------------
msg = "";
TooN::Vector<6> of = filter->getCurrentOffsets();
TooN::Vector<3> sc = filter->getCurrentScales();
if(drawUI == UI_DEBUG)
{
snprintf(charBuf,1000,"Pose: ");
snprintf(charBuf+10,800, "x: %.2f ",lastFramePoseSpeed[0]);
snprintf(charBuf+20,800, "y: %.2f ",lastFramePoseSpeed[1]);
snprintf(charBuf+30,800, "z: %.2f ",lastFramePoseSpeed[2]);
snprintf(charBuf+40,800, "r: %.2f ",lastFramePoseSpeed[3]);
snprintf(charBuf+50,800, "p: %.2f ",lastFramePoseSpeed[4]);
snprintf(charBuf+60,800, "y: %.2f ",lastFramePoseSpeed[5]);
snprintf(charBuf+70,800, "vx: %.2f ",lastFramePoseSpeed[6]);
snprintf(charBuf+80,800, "vy: %.2f ",lastFramePoseSpeed[7]);
snprintf(charBuf+90,800, "vz: %.2f ",lastFramePoseSpeed[8]);
snprintf(charBuf+100,800, "vy: %.2f",lastFramePoseSpeed[9]);
msg += charBuf;
snprintf(charBuf,1000,"\nSync: ");
snprintf(charBuf+10,800, "ox: %.2f ",of[0]);
snprintf(charBuf+20,800, "oy: %.2f ",of[1]);
snprintf(charBuf+30,800, "oz: %.2f ",of[2]);
snprintf(charBuf+40,800, "or: %.2f ",of[3]);
snprintf(charBuf+50,800, "op: %.2f ",of[4]);
snprintf(charBuf+60,800, "oy: %.2f ",of[5]);
snprintf(charBuf+70,800, "Sx: %.2f ",sc[0]);
snprintf(charBuf+80,800, "Sy: %.2f ",sc[1]);
snprintf(charBuf+90,800, "Sz: %.2f",sc[2]);
msg += charBuf;
snprintf(charBuf,1000,"\nStDvs: ");
snprintf(charBuf+10,800, "x: %.2f ",std::sqrt((double)lastFramePoseSpeed[0]));
snprintf(charBuf+20,800, "y: %.2f ",std::sqrt((double)lastFramePoseSpeed[1]));
snprintf(charBuf+30,800, "z: %.2f ",std::sqrt((double)lastFramePoseSpeed[2]));
snprintf(charBuf+40,800, "r: %.2f ",std::sqrt((double)lastFramePoseSpeed[3]));
snprintf(charBuf+50,800, "p: %.2f ",std::sqrt((double)lastFramePoseSpeed[4]));
snprintf(charBuf+60,800, "y: %.2f ",std::sqrt((double)lastFramePoseSpeed[5]));
snprintf(charBuf+70,800, "vx: %.2f ",std::sqrt((double)lastFramePoseSpeed[6]));
snprintf(charBuf+80,800, "vy: %.2f ",std::sqrt((double)lastFramePoseSpeed[7]));
snprintf(charBuf+90,800, "vz: %.2f ",std::sqrt((double)lastFramePoseSpeed[8]));
snprintf(charBuf+100,800, "vy: %.2f",std::sqrt((double)lastFramePoseSpeed[9]));
msg += charBuf;
}
else
{
snprintf(charBuf,1000,"Drone Pose: ");
snprintf(charBuf+13,800, "xyz=(%.2f, ",lastFramePoseSpeed[0]);
snprintf(charBuf+25,800, "%.2f, ",lastFramePoseSpeed[1]);
snprintf(charBuf+32,800, "%.2f), ",lastFramePoseSpeed[2]);
snprintf(charBuf+42,800, "rpy=(%.2f, ",lastFramePoseSpeed[3]);
snprintf(charBuf+54,800, "%.2f, ",lastFramePoseSpeed[4]);
snprintf(charBuf+61,800, "%.2f) ",lastFramePoseSpeed[5]);
msg += charBuf;
}
myGLWindow->GetMousePoseUpdate();
CVD::glSetFont("sans");
if(drawUI != UI_NONE)
myGLWindow->DrawCaption(msg);
if(drawUI == UI_DEBUG)
{
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glTranslatef((float)0, (float)100, 0.0);
glScalef(45,-45,1);
snprintf(charBuf,1000,"xyz: %.2f %.2f %.2f",lastFramePoseSpeed[0],lastFramePoseSpeed[1],lastFramePoseSpeed[2]);
CVD::glDrawText(charBuf, CVD::NICE, 1.6, 0.1);
glPopMatrix();
}
myGLWindow->swap_buffers();
myGLWindow->HandlePendingEvents();
}
//------------------------------------------------------------
void particle::draw(){
glColor4f(red,green,blue,alpha);
//float avg_vel = abs((int)vel.x)+abs((int)vel.y)/2;
//ofCircle(pos.x, pos.y, avg_vel);
drawTrail();
}
