/**
* Generate 3 objects to work with - 1 sphere, 2 disks
*/
void generateObjects(CSetOfObjects::Ptr& world)
{
CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(0.5);
sph->setLocation(0, 0, 0);
sph->setColor(1, 0, 0);
world->insert(sph);
CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
pln->setDiskRadius(2);
pln->setPose(CPose3D(0, 0, 0, 0, DEG2RAD(5), DEG2RAD(5)));
pln->setColor(0.8, 0, 0);
world->insert(pln);
{
CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
pln->setDiskRadius(2);
pln->setPose(CPose3D(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(-2)));
pln->setColor(0.9, 0, 0);
world->insert(pln);
}
}
void display()
{
CDisplayWindow3D window("Ray trace demo", 640, 480);
window.setPos(10, 10);
std::this_thread::sleep_for(20ms);
COpenGLScene::Ptr scene1 = mrpt::make_aligned_shared<COpenGLScene>();
// COpenGLScene::Ptr &scene1=window.get3DSceneAndLock();
opengl::CGridPlaneXY::Ptr plane1 =
mrpt::make_aligned_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
plane1->setColor(GRID_R, GRID_G, GRID_B);
scene1->insert(plane1);
scene1->insert(
mrpt::make_aligned_shared<CAxis>(-5, -5, -5, 5, 5, 5, 2.5, 3, true));
CSetOfObjects::Ptr world = mrpt::make_aligned_shared<CSetOfObjects>();
generateObjects(world);
scene1->insert(world);
CPose3D basePose = randomPose();
CAngularObservationMesh::Ptr aom =
mrpt::make_aligned_shared<CAngularObservationMesh>();
CTicTac t;
t.Tic();
CAngularObservationMesh::trace2DSetOfRays(
scene1, basePose, aom,
CAngularObservationMesh::TDoubleRange::CreateFromAmount(
-M_PI / 2, 0, HOW_MANY_PITCHS),
CAngularObservationMesh::TDoubleRange::CreateFromAperture(
M_PI, HOW_MANY_YAWS));
cout << "Elapsed time: " << t.Tac() << " seconds.\n";
aom->setColor(0, 1, 0);
aom->setWireframe(true);
// Comment to stop showing traced rays and scan range guidelines.
CSetOfLines::Ptr traced = mrpt::make_aligned_shared<CSetOfLines>();
CSetOfLines::Ptr guides = mrpt::make_aligned_shared<CSetOfLines>();
aom->getTracedRays(traced);
traced->setLineWidth(1.5);
traced->setColor(1, 0, 0);
guideLines(basePose, guides, 10);
scene1->insert(traced);
scene1->insert(guides);
// Uncomment to show also traced rays who got lost.
/*
CSetOfLines::Ptr untraced=mrpt::make_aligned_shared<CSetOfLines>();
aom->getUntracedRays(untraced,20);
untraced->setLineWidth(1);
untraced->setColor(1,1,1,0.5);
scene1->insert(untraced);
*/
CSphere::Ptr point = mrpt::make_aligned_shared<CSphere>(0.2);
point->setColor(0, 1, 0);
point->setPose(basePose);
scene1->insert(point);
CDisplayWindow3D window2("Observed mesh", 640, 480);
window2.setPos(660, 10);
std::this_thread::sleep_for(20ms);
window.get3DSceneAndLock() = scene1;
window.unlockAccess3DScene();
window.setCameraElevationDeg(25.0f);
COpenGLScene::Ptr& scene2 = window2.get3DSceneAndLock();
scene2->insert(aom);
opengl::CGridPlaneXY::Ptr plane2 =
mrpt::make_aligned_shared<CGridPlaneXY>(-20, 20, -20, 20, 0, 1);
plane2->setColor(GRID_R, GRID_G, GRID_B);
scene2->insert(plane2);
scene2->insert(
mrpt::make_aligned_shared<CAxis>(-5, -5, -5, 5, 5, 5, 2.5, 3, true));
window2.unlockAccess3DScene();
window2.setCameraElevationDeg(25.0f);
window.waitForKey();
}
