diff --git a/CloudManager.cpp b/CloudManager.cpp index 360b211..d770920 100644 --- a/CloudManager.cpp +++ b/CloudManager.cpp @@ -15,9 +15,8 @@ CloudManager::CloudManager() { useKColor = true; should_quit = false; - //initialPointScale = 0.001; - initialPointScale = ConfigManager::getFloat("Plugin.KinectDemo.KinectDefaultOn.KinectPointSize",0.0f); + initialPointScale = ConfigManager::getFloat("Plugin.KinectDemo.KinectDefaultOn.KinectPointSize", 0.0f); pgm1 = new osg::Program; pgm1->setName("Sphere"); std::string shaderPath = ConfigManager::getEntry("Plugin.Points.ShaderPath"); @@ -29,7 +28,6 @@ CloudManager::CloudManager() pgm1->setParameter(GL_GEOMETRY_OUTPUT_TYPE_EXT, GL_TRIANGLE_STRIP); minDistHSV = 700; maxDistHSV = 5000; - kinectgrp = new osg::Group(); osg::StateSet* state = kinectgrp->getOrCreateStateSet(); state->setAttribute(pgm1); @@ -38,11 +36,9 @@ CloudManager::CloudManager() float pscale = 1.0; osg::Uniform* _scaleUni = new osg::Uniform("pointScale", 1.0f * pscale); kinectgrp->getOrCreateStateSet()->addUniform(_scaleUni); - _root = new osg::MatrixTransform(); SceneManager::instance()->getObjectsRoot()->addChild(_root); _root->addChild(kinectgrp); - } CloudManager::~CloudManager() @@ -57,164 +53,156 @@ bool CloudManager::isCacheDone() void CloudManager::update() { static bool frameLoading = false; - bool cDone; -if(true) -{ - if(cvr::ComController::instance()->isMaster()) + bool cDone; + + if (true) + { + if (cvr::ComController::instance()->isMaster()) { - cDone = _cacheDone; - int numSlaves = cvr::ComController::instance()->getNumSlaves(); - bool sDone[numSlaves]; - cvr::ComController::instance()->readSlaves(sDone,sizeof(bool)); - for(int i = 0; i < numSlaves; i++) - { - cDone = cDone && sDone[i]; - } - cvr::ComController::instance()->sendSlaves(&cDone,sizeof(bool)); + cDone = _cacheDone; + int numSlaves = cvr::ComController::instance()->getNumSlaves(); + bool sDone[numSlaves]; + cvr::ComController::instance()->readSlaves(sDone, sizeof(bool)); + + for (int i = 0; i < numSlaves; i++) + { + cDone = cDone && sDone[i]; + } + + cvr::ComController::instance()->sendSlaves(&cDone, sizeof(bool)); } else { - cDone = _cacheDone; - cvr::ComController::instance()->sendMaster(&cDone,sizeof(bool)); - cvr::ComController::instance()->readMaster(&cDone,sizeof(bool)); + cDone = _cacheDone; + cvr::ComController::instance()->sendMaster(&cDone, sizeof(bool)); + cvr::ComController::instance()->readMaster(&cDone, sizeof(bool)); } - if(!cDone) + if (!cDone) { - //std::cerr << "Waiting for load to finish." << std::endl; - return; + //std::cerr << "Waiting for load to finish." << std::endl; + return; } cvr::ComController::instance()->sync(); -//Add not here? - + //Add not here? + return; + } - return; -} - if(cDone) + if (cDone) { - std::cerr << "Load Finished." << std::endl; - //Add loaded node to root - + std::cerr << "Load Finished." << std::endl; + //Add loaded node to root } else { -// std::cerr << "Waiting for GPU load finish." << std::endl; + // std::cerr << "Waiting for GPU load finish." << std::endl; } - } void CloudManager::run() { -//Do functions -cerr << "."; -if(kinectgrp != NULL) -{ + //Do functions + cerr << "."; - zmq::context_t context2(1); - cloudT_socket = NULL; - cloudT_socket = new SubSocket (context2, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.PointCloud")); - - packet = new RemoteKinect::PointCloud(); - while(!should_quit) + if (kinectgrp != NULL) { - - //printf("."); - if(cloudT_socket != NULL) -{ -// printf("NotNull"); + zmq::context_t context2(1); + cloudT_socket = NULL; + cloudT_socket = new SubSocket (context2, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.PointCloud")); + packet = new RemoteKinect::PointCloud(); - if (cloudT_socket->recv(*packet)) - { - float r, g, b, a; - osg::Vec3Array* kinectVertices = new osg::Vec3Array; -// kinectVertices->empty(); - osg::Vec3Array* normals = new osg::Vec3Array; - osg::Vec4Array* kinectColours = new osg::Vec4Array; - // kinectColours->empty(); - // cerr << "."; - //int size = packet->points_size(); - //printf("Points %i\n",size); - if(true) + while (!should_quit) { - for (int i = 0; i < packet->points_size(); i++) + //printf("."); + if (cloudT_socket != NULL) { -/* - osg::Vec3f ppos((packet->points(i).x() / 1000) + Skeleton::camPos.x(), - (packet->points(i).z() / -1000) + Skeleton::camPos.y(), - (packet->points(i).y() / 1000) + Skeleton::camPos.z()); -*/ - osg::Vec3f ppos((packet->points(i).x() / 1000), - (packet->points(i).z() / -1000), - (packet->points(i).y() / 1000)); - kinectVertices->push_back(ppos); - //useKColor - if (useKColor) + // printf("NotNull"); + if (cloudT_socket->recv(*packet)) { - r = (packet->points(i).r() / 255.); - g = (packet->points(i).g() / 255.); - b = (packet->points(i).b() / 255.); - a = 1; - kinectColours->push_back(osg::Vec4f(r, g, b, a)); + float r, g, b, a; + osg::Vec3Array* kinectVertices = new osg::Vec3Array; + // kinectVertices->empty(); + osg::Vec3Array* normals = new osg::Vec3Array; + osg::Vec4Array* kinectColours = new osg::Vec4Array; + + // kinectColours->empty(); + // cerr << "."; + //int size = packet->points_size(); + //printf("Points %i\n",size); + if (true) + { + for (int i = 0; i < packet->points_size(); i++) + { + /* + osg::Vec3f ppos((packet->points(i).x() / 1000) + Skeleton::camPos.x(), + (packet->points(i).z() / -1000) + Skeleton::camPos.y(), + (packet->points(i).y() / 1000) + Skeleton::camPos.z()); + */ + osg::Vec3f ppos((packet->points(i).x() / 1000), + (packet->points(i).z() / -1000), + (packet->points(i).y() / 1000)); + kinectVertices->push_back(ppos); + + //useKColor + if (useKColor) + { + r = (packet->points(i).r() / 255.); + g = (packet->points(i).g() / 255.); + b = (packet->points(i).b() / 255.); + a = 1; + kinectColours->push_back(osg::Vec4f(r, g, b, a)); + } + else + { + kinectColours->push_back(getColorRGB(packet->points(i).z())); + } + } + + //cerr << kinectVertices->size(); + osg::Geode* kgeode = new osg::Geode(); + kgeode->setCullingActive(false); + osg::Geometry* nodeGeom = new osg::Geometry(); + osg::StateSet* state = nodeGeom->getOrCreateStateSet(); + nodeGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, 0, kinectVertices->size())); + osg::VertexBufferObject* vboP = nodeGeom->getOrCreateVertexBufferObject(); + vboP->setUsage(GL_STREAM_DRAW); + nodeGeom->setUseDisplayList(true); + nodeGeom->setUseVertexBufferObjects(true); + nodeGeom->setVertexArray(kinectVertices); + nodeGeom->setColorArray(kinectColours); + nodeGeom->setColorBinding(osg::Geometry::BIND_PER_VERTEX); + kgeode->addDrawable(nodeGeom); + kgeode->dirtyBound(); + //if (kinectgrp != NULL) _root->removeChild(kinectgrp); + kinectgrp->removeChild(0, 1); + kinectgrp->addChild(kgeode); + } } - else + + if (should_quit) { - kinectColours->push_back(getColorRGB(packet->points(i).z())); + pgm1->ref(); + kinectgrp->removeChild(0, 1); + _root->removeChild(kinectgrp); + kinectgrp = NULL; } } -//cerr << kinectVertices->size(); - - osg::Geode* kgeode = new osg::Geode(); - kgeode->setCullingActive(false); - osg::Geometry* nodeGeom = new osg::Geometry(); - osg::StateSet* state = nodeGeom->getOrCreateStateSet(); - nodeGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, 0, kinectVertices->size())); - osg::VertexBufferObject* vboP = nodeGeom->getOrCreateVertexBufferObject(); - vboP->setUsage(GL_STREAM_DRAW); - nodeGeom->setUseDisplayList(true); - nodeGeom->setUseVertexBufferObjects(true); - nodeGeom->setVertexArray(kinectVertices); - nodeGeom->setColorArray(kinectColours); - nodeGeom->setColorBinding(osg::Geometry::BIND_PER_VERTEX); - kgeode->addDrawable(nodeGeom); - kgeode->dirtyBound(); - //if (kinectgrp != NULL) _root->removeChild(kinectgrp); - kinectgrp->removeChild(0, 1); - kinectgrp->addChild(kgeode); - - - } - } -if(should_quit) -{ - pgm1->ref(); - kinectgrp->removeChild(0, 1); - _root->removeChild(kinectgrp); - kinectgrp = NULL; - -} -} -} -} -//When Finished - _cacheDone = true; - - - - - std::cerr << "All frames loaded." << std::endl; + } + } + //When Finished + _cacheDone = true; + std::cerr << "All frames loaded." << std::endl; } void CloudManager::quit() { - - should_quit = true; - // pgm1->ref(); - // kinectgrp->removeChild(0, 1); - // _root->removeChild(kinectgrp); - // kinectgrp = NULL; - + should_quit = true; + // pgm1->ref(); + // kinectgrp->removeChild(0, 1); + // _root->removeChild(kinectgrp); + // kinectgrp = NULL; } osg::Vec4f CloudManager::getColorRGB(int dist) diff --git a/CloudManager.h b/CloudManager.h index e4e5ed4..3e69d14 100644 --- a/CloudManager.h +++ b/CloudManager.h @@ -29,27 +29,27 @@ #include -// zmq::context_t contextCloud(1); +// zmq::context_t contextCloud(1); class CloudManager : public OpenThreads::Thread { - public: +public: - CloudManager(); - ~CloudManager(); - //contextCloud(1); - SubSocket* cloudT_socket; - void update(); - bool isCacheDone(); - RemoteKinect::PointCloud* packet; + CloudManager(); + ~CloudManager(); + //contextCloud(1); + SubSocket* cloudT_socket; + void update(); + bool isCacheDone(); + RemoteKinect::PointCloud* packet; - virtual void run(); - void quit(); - protected: + virtual void run(); + void quit(); +protected: - // osg::ref_ptr kinectColours; - // osg::ref_ptr kinectVertices; - bool _cacheDone; + // osg::ref_ptr kinectColours; + // osg::ref_ptr kinectVertices; + bool _cacheDone; bool useKColor; bool should_quit; osg::Program* pgm1; diff --git a/KinectDemo.cpp b/KinectDemo.cpp index 3aab86b..0a7aac5 100755 --- a/KinectDemo.cpp +++ b/KinectDemo.cpp @@ -50,7 +50,7 @@ bool KinectDemo::init() pointGeode = new osg::Geode(); useKinect = true; kShowPCloud = ConfigManager::getBool("Plugin.KinectDemo.KinectDefaultOn.ShowPCloud"); - // kShowPCloud = false; + // kShowPCloud = false; //kinectThreaded = false; useKColor = ConfigManager::getBool("Plugin.KinectDemo.KinectDefaultOn.ShowColor"); kShowDepth = false; @@ -82,15 +82,12 @@ bool KinectDemo::init() _kColorFPS->setCallback(this); _kShowPCloud = new MenuCheckbox("Show Point Cloud", kShowPCloud); //new _kShowPCloud->setCallback(this); - - _kShowInfoPanel = new MenuCheckbox("Show Info Panel", kShowInfoPanel); //new _kShowInfoPanel->setCallback(this); _infoPanel = new TabbedDialogPanel(500, 20, 4, "Info Panel", "Plugin.KinectDemo.InfoPanel"); _infoPanel->addTextTab("Info", ""); _infoPanel->setVisible(false); _infoPanel->setActiveTab("Info"); - _kColorOn = new MenuCheckbox("Show Real Colors on Point Cloud", useKColor); //new _kColorOn->setCallback(this); _kNavSpheres = new MenuCheckbox("Navigation Spheres", kNavSpheres); @@ -143,6 +140,7 @@ void KinectDemo::menuCallback(MenuItem* menuItem) kMoveWithCam = false; } } + if (menuItem == _kFreezeCloud) { if (_kFreezeCloud->getValue()) @@ -231,31 +229,33 @@ void KinectDemo::menuCallback(MenuItem* menuItem) Quat camQuad = camMat.getRotate(); cerr << "Saved camera position: " << endl; cerr << cscale << ", " << (camTrans.x() / cscale) << ", " << (camTrans.y() / cscale) << ", " << (camTrans.z() / cscale) << ", " << camQuad.x() << ", " << camQuad.y() << ", " << camQuad.z() << ", " << camQuad.w() << endl; - bool savePointCloud = true; - if(savePointCloud && cloud_socket) + + if (savePointCloud && cloud_socket) { - ExportPointCloud(); + ExportPointCloud(); } - } + if (menuItem == _testInteract) { - sendEvents(); + sendEvents(); } - if(menuItem == sliderX) + + if (menuItem == sliderX) { kinectX = sliderX->getValue(); } - if(menuItem == sliderY) + + if (menuItem == sliderY) { kinectY = sliderY->getValue(); } - if(menuItem == sliderZ) + + if (menuItem == sliderZ) { kinectZ = sliderZ->getValue(); } - } void KinectDemo::preFrame() @@ -264,8 +264,8 @@ void KinectDemo::preFrame() // loop getting new data from Kinect server ThirdLoop(); - updateInfoPanel(); + // show image from Kinect camera every ... frames if (kShowColor || kShowDepth) { @@ -278,417 +278,414 @@ void KinectDemo::preFrame() if (kShowDepth) showDepthImage(); } } -if(!skeletonThreaded) -{ - // for every skeleton in mapIdSkel - draw, navigation spheres, check intersection with objects - std::map< osg::ref_ptr, int >::iterator iter; - for (std::map::iterator it = mapIdSkel.begin(); it != mapIdSkel.end(); ++it) + if (!skeletonThreaded) { - int sk_id = it->first; - Skeleton* sk = &(it->second); + // for every skeleton in mapIdSkel - draw, navigation spheres, check intersection with objects + std::map< osg::ref_ptr, int >::iterator iter; - if (kNavSpheres) + for (std::map::iterator it = mapIdSkel.begin(); it != mapIdSkel.end(); ++it) { - if (sk->navSphere.lock == -1) + int sk_id = it->first; + Skeleton* sk = &(it->second); + + if (kNavSpheres) { - Vec3 navPos = sk->joints[3].position; - navPos.y() += 0.3; - sk->navSphere.update(navPos, Vec4(0, 0, 0, 1)); + if (sk->navSphere.lock == -1) + { + Vec3 navPos = sk->joints[3].position; + navPos.y() += 0.3; + sk->navSphere.update(navPos, Vec4(0, 0, 0, 1)); + } } - } - - /******One hand selection ********/ - if (sk->cylinder.attached == false) - checkHandsIntersections(sk_id); - // cout << sk->joints[M_LHAND].position.x() << " lbusy " << sk->leftHandBusy << endl; - Vec3 StartPoint = sk->joints[M_LHAND].position; - Vec3 EndPoint = sk->joints[M_RHAND].position; - // if cylinder would be >distanceMIN && cylinder.attached == false) + checkHandsIntersections(sk_id); - //printf("Distance Cylinder: %g \n",distance); - if (((sk->joints[9].position.z() - sk->joints[7].position.z() > HAND_ELBOW_OFFSET) && (sk->joints[15].position.z() - sk->joints[13].position.z() > HAND_ELBOW_OFFSET))) sk->cylinder.handsBeenAboveElbows = true; + // cout << sk->joints[M_LHAND].position.x() << " lbusy " << sk->leftHandBusy << endl; + Vec3 StartPoint = sk->joints[M_LHAND].position; + Vec3 EndPoint = sk->joints[M_RHAND].position; + // if cylinder would be >distanceMIN && cylinder.update(StartPoint, EndPoint); - - if (sk->cylinder.attached) - { - bool detachC = false; + //printf("Distance Cylinder: %g \n",distance); + if (((sk->joints[9].position.z() - sk->joints[7].position.z() > HAND_ELBOW_OFFSET) && (sk->joints[15].position.z() - sk->joints[13].position.z() > HAND_ELBOW_OFFSET))) sk->cylinder.handsBeenAboveElbows = true; - if (sk->cylinder.locked && ((sk->joints[9].position.z() - sk->joints[7].position.z() < HAND_ELBOW_OFFSET) && (sk->joints[15].position.z() - sk->joints[13].position.z() < HAND_ELBOW_OFFSET))) - detachC = true; + sk->cylinder.update(StartPoint, EndPoint); - if (sk->cylinder.handsBeenAboveElbows && (distance > distanceMAX || distance < distanceMIN || detachC)) + if (sk->cylinder.attached) { - sk->cylinder.detach(_pointClouds[0]->switchNode); + bool detachC = false; - // unlock all the objects that were locked by this cylinder - for (int j = 0; j < selectableItems.size(); j++) - { - if (selectableItems[j].lock == sk_id) selectableItems[j].unlock(); - } + if (sk->cylinder.locked && ((sk->joints[9].position.z() - sk->joints[7].position.z() < HAND_ELBOW_OFFSET) && (sk->joints[15].position.z() - sk->joints[13].position.z() < HAND_ELBOW_OFFSET))) + detachC = true; - // and NavSpheres - sk->navSphere.lock = -1; - navLock = -1; - sk->navSphere.activated = false; - sk->cylinder.locked = false; - } - else - { - // if sk->leton's cylinder is locked to some object, do not lock any more - if (sk->cylinder.locked == false) + if (sk->cylinder.handsBeenAboveElbows && (distance > distanceMAX || distance < distanceMIN || detachC)) { - // for every selectable item, check if it intersects with the current cylinder - const osg::BoundingBox& bboxCyl = sk->cylinder.geode->getDrawable(0)->getBound(); + sk->cylinder.detach(_pointClouds[0]->switchNode); - // NavSphere - if (kNavSpheres) + // unlock all the objects that were locked by this cylinder + for (int j = 0; j < selectableItems.size(); j++) { - Sphere* tempNav = new Sphere(sk->navSphere.position, 0.03 * 2); - ShapeDrawable* ggg3 = new ShapeDrawable(tempNav); - const osg::BoundingBox& fakenav = ggg3->getBound(); - - if (bboxCyl.intersects(fakenav) && sk->navSphere.lock == -1) - { - sk->navSphere.lock = sk_id; - navLock = sk_id; - sk->cylinder.locked = true; - sk->cylinder.prevVec = (sk->joints[M_LHAND].position - osg::Vec3d((StartPoint.x() + EndPoint.x()) / 2, (StartPoint.y() + EndPoint.y()) / 2, (StartPoint.z() + EndPoint.z()) / 2)); - sk->cylinder.handsBeenAboveElbows = true; - break; - } + if (selectableItems[j].lock == sk_id) selectableItems[j].unlock(); } - for (int j = 0; j < selectableItems.size(); j++) + // and NavSpheres + sk->navSphere.lock = -1; + navLock = -1; + sk->navSphere.activated = false; + sk->cylinder.locked = false; + } + else + { + // if sk->leton's cylinder is locked to some object, do not lock any more + if (sk->cylinder.locked == false) { - // fake sphere to easily calculate boundary - Vec3 center2 = Vec3(0, 0, 0) * (selectableItems[j].mt->getMatrix()); - Box* fakeSphere = new Box(center2, selectableItems[j].scale); - ShapeDrawable* ggg2 = new ShapeDrawable(fakeSphere); - const osg::BoundingBox& fakeBbox = ggg2->getBound(); + // for every selectable item, check if it intersects with the current cylinder + const osg::BoundingBox& bboxCyl = sk->cylinder.geode->getDrawable(0)->getBound(); + + // NavSphere + if (kNavSpheres) + { + Sphere* tempNav = new Sphere(sk->navSphere.position, 0.03 * 2); + ShapeDrawable* ggg3 = new ShapeDrawable(tempNav); + const osg::BoundingBox& fakenav = ggg3->getBound(); + + if (bboxCyl.intersects(fakenav) && sk->navSphere.lock == -1) + { + sk->navSphere.lock = sk_id; + navLock = sk_id; + sk->cylinder.locked = true; + sk->cylinder.prevVec = (sk->joints[M_LHAND].position - osg::Vec3d((StartPoint.x() + EndPoint.x()) / 2, (StartPoint.y() + EndPoint.y()) / 2, (StartPoint.z() + EndPoint.z()) / 2)); + sk->cylinder.handsBeenAboveElbows = true; + break; + } + } - if (bboxCyl.intersects(fakeBbox) && selectableItems[j].lock == -1) + for (int j = 0; j < selectableItems.size(); j++) { - sk->cylinder.locked = true; - selectableItems[j].lockType = 0; - selectableItems[j].lockTo(sk_id); - sk->cylinder.prevVec = (sk->joints[M_LHAND].position - osg::Vec3d((StartPoint.x() + EndPoint.x()) / 2, (StartPoint.y() + EndPoint.y()) / 2, (StartPoint.z() + EndPoint.z()) / 2)); - sk->cylinder.handsBeenAboveElbows = false; - break; // lock only one object + // fake sphere to easily calculate boundary + Vec3 center2 = Vec3(0, 0, 0) * (selectableItems[j].mt->getMatrix()); + Box* fakeSphere = new Box(center2, selectableItems[j].scale); + ShapeDrawable* ggg2 = new ShapeDrawable(fakeSphere); + const osg::BoundingBox& fakeBbox = ggg2->getBound(); + + if (bboxCyl.intersects(fakeBbox) && selectableItems[j].lock == -1) + { + sk->cylinder.locked = true; + selectableItems[j].lockType = 0; + selectableItems[j].lockTo(sk_id); + sk->cylinder.prevVec = (sk->joints[M_LHAND].position - osg::Vec3d((StartPoint.x() + EndPoint.x()) / 2, (StartPoint.y() + EndPoint.y()) / 2, (StartPoint.z() + EndPoint.z()) / 2)); + sk->cylinder.handsBeenAboveElbows = false; + break; // lock only one object + } } } } } - } - else - { - // Cylinder is not attached - // Don't create a cylinder between hands if any of them is holding an object - if (sk->leftHandBusy == false && sk->rightHandBusy == false) + else { - // CONDITIONS TO CREATE CYLINDER - if (distance < distanceMAX / 2 && distance > distanceMIN && ((sk->joints[9].position.z() - sk->joints[7].position.z() > HAND_ELBOW_OFFSET) && (sk->joints[15].position.z() - sk->joints[13].position.z() > HAND_ELBOW_OFFSET))) + // Cylinder is not attached + // Don't create a cylinder between hands if any of them is holding an object + if (sk->leftHandBusy == false && sk->rightHandBusy == false) { - sk->cylinder.attach(_pointClouds[0]->switchNode); + // CONDITIONS TO CREATE CYLINDER + if (distance < distanceMAX / 2 && distance > distanceMIN && ((sk->joints[9].position.z() - sk->joints[7].position.z() > HAND_ELBOW_OFFSET) && (sk->joints[15].position.z() - sk->joints[13].position.z() > HAND_ELBOW_OFFSET))) + { + sk->cylinder.attach(_pointClouds[0]->switchNode); + } } } } - } - - // move all the objects that are locked to centers and rotate to centers rotation - for (int j = 0; j < selectableItems.size(); j++) - { - SelectableItem sel = selectableItems[j]; - - if (sel.lock == -1) continue; + // move all the objects that are locked to centers and rotate to centers rotation - if (sel.lockType == 0) + for (int j = 0; j < selectableItems.size(); j++) { - int cylinderId = sel.lock; - Matrix rotMat0; - rotMat0.makeRotate(mapIdSkel[cylinderId].cylinder.prevVec, mapIdSkel[cylinderId].cylinder.currVec); - selectableItems[j].rt->postMult(rotMat0); - Matrix posMat; - posMat.setTrans(mapIdSkel[cylinderId].cylinder.center); - selectableItems[j].mt->setMatrix(posMat); - double newscale = selectableItems[j].scale; - newscale *= (1 + ((mapIdSkel[cylinderId].cylinder.length - mapIdSkel[cylinderId].cylinder.prevLength) / (500 / 1000.0))); + SelectableItem sel = selectableItems[j]; - if (newscale < 1 / 1000.0) newscale = 1 / 1000.0; + if (sel.lock == -1) continue; - selectableItems[j].setScale(newscale); - } - else if (sel.lockType != -1) - { - // moving artifact by one hand - Matrix posMat; + if (sel.lockType == 0) + { + int cylinderId = sel.lock; + Matrix rotMat0; + rotMat0.makeRotate(mapIdSkel[cylinderId].cylinder.prevVec, mapIdSkel[cylinderId].cylinder.currVec); + selectableItems[j].rt->postMult(rotMat0); + Matrix posMat; + posMat.setTrans(mapIdSkel[cylinderId].cylinder.center); + selectableItems[j].mt->setMatrix(posMat); + double newscale = selectableItems[j].scale; + newscale *= (1 + ((mapIdSkel[cylinderId].cylinder.length - mapIdSkel[cylinderId].cylinder.prevLength) / (500 / 1000.0))); + + if (newscale < 1 / 1000.0) newscale = 1 / 1000.0; + + selectableItems[j].setScale(newscale); + } + else if (sel.lockType != -1) + { + // moving artifact by one hand + Matrix posMat; - if (sel.lockType == 1) posMat.setTrans(mapIdSkel[sel.lock].joints[M_LHAND].position); - else if (sel.lockType == 2) posMat.setTrans(mapIdSkel[sel.lock].joints[M_RHAND].position); - else cout << "Error - unknown type of a lock (" << sel.lockType << ") on an object" << endl; + if (sel.lockType == 1) posMat.setTrans(mapIdSkel[sel.lock].joints[M_LHAND].position); + else if (sel.lockType == 2) posMat.setTrans(mapIdSkel[sel.lock].joints[M_RHAND].position); + else cout << "Error - unknown type of a lock (" << sel.lockType << ") on an object" << endl; - selectableItems[j].mt->setMatrix(posMat); + selectableItems[j].mt->setMatrix(posMat); + } } - } - - if (kNavSpheres && navLock != -1) - { - int cylinderId = navLock; - Vec3 diff = mapIdSkel[cylinderId].cylinder.center - mapIdSkel[cylinderId].navSphere.position; - bool navSphereActivated = mapIdSkel[cylinderId].navSphere.activated; - if (!navSphereActivated) + if (kNavSpheres && navLock != -1) { - const osg::BoundingBox& bboxCyl = mapIdSkel[cylinderId].cylinder.geode->getDrawable(0)->getBound(); - //Vec3 navPos = mapIdSkel[cylinderId].joints[3].position; navbang - Vec3 center2 = mapIdSkel[cylinderId].joints[3].position; - center2.y() += 0.4; - //center2.z() += 0.2; - Box* fakeSphere = new Box(center2, _sphereRadius * 1); - ShapeDrawable* ggg2 = new ShapeDrawable(fakeSphere); - const osg::BoundingBox& fakeBbox = ggg2->getBound(); - - if (bboxCyl.intersects(fakeBbox)) - { - //printf("NavSphere Activated\n"); - navSphereActivated = true; - mapIdSkel[cylinderId].navSphere.activated = true; - mapIdSkel[cylinderId].navSphere.update(mapIdSkel[cylinderId].navSphere.position, Vec4(0, 0, 0, 1)); - } + int cylinderId = navLock; + Vec3 diff = mapIdSkel[cylinderId].cylinder.center - mapIdSkel[cylinderId].navSphere.position; + bool navSphereActivated = mapIdSkel[cylinderId].navSphere.activated; if (!navSphereActivated) { - mapIdSkel[cylinderId].navSphere.activated = false; - mapIdSkel[cylinderId].navSphere.update(mapIdSkel[cylinderId].cylinder.center, Vec4(0, 0, 0, 1)); - } - } - else - { - Vec3 center2 = mapIdSkel[cylinderId].joints[3].position; - center2.y() += 0.4; - mapIdSkel[cylinderId].navSphere.update(center2, Vec4(0, 0, 0, 1)); - } - - Matrix rotMat0; - rotMat0.makeRotate(mapIdSkel[cylinderId].cylinder.prevVec, mapIdSkel[cylinderId].cylinder.currVec); + const osg::BoundingBox& bboxCyl = mapIdSkel[cylinderId].cylinder.geode->getDrawable(0)->getBound(); + //Vec3 navPos = mapIdSkel[cylinderId].joints[3].position; navbang + Vec3 center2 = mapIdSkel[cylinderId].joints[3].position; + center2.y() += 0.4; + //center2.z() += 0.2; + Box* fakeSphere = new Box(center2, _sphereRadius * 1); + ShapeDrawable* ggg2 = new ShapeDrawable(fakeSphere); + const osg::BoundingBox& fakeBbox = ggg2->getBound(); + + if (bboxCyl.intersects(fakeBbox)) + { + //printf("NavSphere Activated\n"); + navSphereActivated = true; + mapIdSkel[cylinderId].navSphere.activated = true; + mapIdSkel[cylinderId].navSphere.update(mapIdSkel[cylinderId].navSphere.position, Vec4(0, 0, 0, 1)); + } - if (navSphereActivated) - { - double x, y, z; - x = y = z = 0.0; - double rx, ry, rz; - rx = ry = rz = 0.0; - double tranScale = -75; - bool moved = false; - - if (diff.x() > 0.1 || diff.x() < -0.1) - { - x = diff.x() * tranScale; - moved = true; + if (!navSphereActivated) + { + mapIdSkel[cylinderId].navSphere.activated = false; + mapIdSkel[cylinderId].navSphere.update(mapIdSkel[cylinderId].cylinder.center, Vec4(0, 0, 0, 1)); + } } - - if (diff.y() > 0.1 || diff.y() < -0.1) + else { - y = diff.y() * tranScale; - moved = true; + Vec3 center2 = mapIdSkel[cylinderId].joints[3].position; + center2.y() += 0.4; + mapIdSkel[cylinderId].navSphere.update(center2, Vec4(0, 0, 0, 1)); } - if (diff.z() > 0.1 || diff.z() < -0.1) - { - z = diff.z() * tranScale; - moved = true; - } + Matrix rotMat0; + rotMat0.makeRotate(mapIdSkel[cylinderId].cylinder.prevVec, mapIdSkel[cylinderId].cylinder.currVec); - if (moved) + if (navSphereActivated) { - Quat handQuad = rotMat0.getRotate(); - double rscale = 1; - rx = 0.0; //handQuad.x(); - ry = 0.0; //handQuad.y(); - rz = handQuad.z(); + double x, y, z; + x = y = z = 0.0; + double rx, ry, rz; + rx = ry = rz = 0.0; + double tranScale = -75; + bool moved = false; + + if (diff.x() > 0.1 || diff.x() < -0.1) + { + x = diff.x() * tranScale; + moved = true; + } - if (rz > 0.03 || rz < -0.03) + if (diff.y() > 0.1 || diff.y() < -0.1) { - rz = 0.0; + y = diff.y() * tranScale; + moved = true; } - else + + if (diff.z() > 0.1 || diff.z() < -0.1) { - rz = 0.0; + z = diff.z() * tranScale; + moved = true; } - Matrixd finalmat; - Matrix view = PluginHelper::getHeadMat(); - Vec3 campos = view.getTrans(); - Vec3 trans = Vec3(x, y, z); - trans = (trans * view) - campos; - Matrix tmat; - tmat.makeTranslate(trans); - Vec3 xa = Vec3(1.0, 0.0, 0.0); - Vec3 ya = Vec3(0.0, 1.0, 0.0); - Vec3 za = Vec3(0.0, 0.0, 1.0); - xa = (xa * view) - campos; - ya = (ya * view) - campos; - za = (za * view) - campos; - Matrix rot; - rot.makeRotate(rx, xa, ry, ya, rz, za); - Matrix ctrans, nctrans; - ctrans.makeTranslate(campos); - nctrans.makeTranslate(-campos); - finalmat = PluginHelper::getObjectMatrix() * nctrans * rot * tmat * ctrans; - PluginHelper::setObjectMatrix(finalmat); + if (moved) + { + Quat handQuad = rotMat0.getRotate(); + double rscale = 1; + rx = 0.0; //handQuad.x(); + ry = 0.0; //handQuad.y(); + rz = handQuad.z(); + + if (rz > 0.03 || rz < -0.03) + { + rz = 0.0; + } + else + { + rz = 0.0; + } + + Matrixd finalmat; + Matrix view = PluginHelper::getHeadMat(); + Vec3 campos = view.getTrans(); + Vec3 trans = Vec3(x, y, z); + trans = (trans * view) - campos; + Matrix tmat; + tmat.makeTranslate(trans); + Vec3 xa = Vec3(1.0, 0.0, 0.0); + Vec3 ya = Vec3(0.0, 1.0, 0.0); + Vec3 za = Vec3(0.0, 0.0, 1.0); + xa = (xa * view) - campos; + ya = (ya * view) - campos; + za = (za * view) - campos; + Matrix rot; + rot.makeRotate(rx, xa, ry, ya, rz, za); + Matrix ctrans, nctrans; + ctrans.makeTranslate(campos); + nctrans.makeTranslate(-campos); + finalmat = PluginHelper::getObjectMatrix() * nctrans * rot * tmat * ctrans; + PluginHelper::setObjectMatrix(finalmat); + } } } } } -} void KinectDemo::kinectInit() { // moving from points to spheres in kinect point cloud -//bang + //bang kinectThreaded = ConfigManager::getBool("Plugin.KinectDemo.KinectDefaultOn.KinectThreaded"); skeletonThreaded = false; -if(!kinectThreaded) -{ - initialPointScale = ConfigManager::getFloat("Plugin.KinectDemo.KinectDefaultOn.KinectPointSize"); - cerr << "PointScale=" << initialPointScale << "\n"; - pgm1 = new osg::Program; - pgm1->setName("Sphere"); - std::string shaderPath = ConfigManager::getEntry("Plugin.Points.ShaderPath"); - pgm1->addShader(osg::Shader::readShaderFile(osg::Shader::VERTEX, osgDB::findDataFile(shaderPath + "/Sphere.vert"))); - pgm1->addShader(osg::Shader::readShaderFile(osg::Shader::FRAGMENT, osgDB::findDataFile(shaderPath + "/Sphere.frag"))); - pgm1->addShader(osg::Shader::readShaderFile(osg::Shader::GEOMETRY, osgDB::findDataFile(shaderPath + "/Sphere.geom"))); - pgm1->setParameter(GL_GEOMETRY_VERTICES_OUT_EXT, 4); - pgm1->setParameter(GL_GEOMETRY_INPUT_TYPE_EXT, GL_POINTS); - pgm1->setParameter(GL_GEOMETRY_OUTPUT_TYPE_EXT, GL_TRIANGLE_STRIP); - // move camera to the kinect-person -} -//Get KinectSkeleton Offset - - kinectX = ConfigManager::getFloat("x","Plugin.KinectDemo.KinectSkeleton",0.0f); - kinectY = ConfigManager::getFloat("y","Plugin.KinectDemo.KinectSkeleton",0.0f); - kinectZ = ConfigManager::getFloat("z","Plugin.KinectDemo.KinectSkeleton",0.0f); - kinectRX = ConfigManager::getFloat("rx","Plugin.KinectDemo.KinectSkeleton",0.0f); - kinectRY = ConfigManager::getFloat("ry","Plugin.KinectDemo.KinectSkeleton",0.0f); - kinectRZ = ConfigManager::getFloat("rz","Plugin.KinectDemo.KinectSkeleton",0.0f); - kinectRW = ConfigManager::getFloat("rw","Plugin.KinectDemo.KinectSkeleton",0.0f); - -//Show info Panel + if (!kinectThreaded) + { + initialPointScale = ConfigManager::getFloat("Plugin.KinectDemo.KinectDefaultOn.KinectPointSize"); + cerr << "PointScale=" << initialPointScale << "\n"; + pgm1 = new osg::Program; + pgm1->setName("Sphere"); + std::string shaderPath = ConfigManager::getEntry("Plugin.Points.ShaderPath"); + pgm1->addShader(osg::Shader::readShaderFile(osg::Shader::VERTEX, osgDB::findDataFile(shaderPath + "/Sphere.vert"))); + pgm1->addShader(osg::Shader::readShaderFile(osg::Shader::FRAGMENT, osgDB::findDataFile(shaderPath + "/Sphere.frag"))); + pgm1->addShader(osg::Shader::readShaderFile(osg::Shader::GEOMETRY, osgDB::findDataFile(shaderPath + "/Sphere.geom"))); + pgm1->setParameter(GL_GEOMETRY_VERTICES_OUT_EXT, 4); + pgm1->setParameter(GL_GEOMETRY_INPUT_TYPE_EXT, GL_POINTS); + pgm1->setParameter(GL_GEOMETRY_OUTPUT_TYPE_EXT, GL_TRIANGLE_STRIP); + // move camera to the kinect-person + } + //Get KinectSkeleton Offset + kinectX = ConfigManager::getFloat("x", "Plugin.KinectDemo.KinectSkeleton", 0.0f); + kinectY = ConfigManager::getFloat("y", "Plugin.KinectDemo.KinectSkeleton", 0.0f); + kinectZ = ConfigManager::getFloat("z", "Plugin.KinectDemo.KinectSkeleton", 0.0f); + kinectRX = ConfigManager::getFloat("rx", "Plugin.KinectDemo.KinectSkeleton", 0.0f); + kinectRY = ConfigManager::getFloat("ry", "Plugin.KinectDemo.KinectSkeleton", 0.0f); + kinectRZ = ConfigManager::getFloat("rz", "Plugin.KinectDemo.KinectSkeleton", 0.0f); + kinectRW = ConfigManager::getFloat("rw", "Plugin.KinectDemo.KinectSkeleton", 0.0f); + //Show info Panel kShowInfoPanel = ConfigManager::getBool("Plugin.KinectDemo.KinectDefaultOn.ShowInfoPanel"); - if(kShowInfoPanel) + + if (kShowInfoPanel) { - _kShowInfoPanel->setValue(true); - _infoPanel->setVisible(true); - - sliderX = new MenuRangeValue("X",-1000.0,1000.0,kinectX,0.01); - sliderX->setCallback(this); - _infoPanel->addMenuItem(sliderX); - - sliderY = new MenuRangeValue("Y",-1000.0,1000.0,kinectY,0.01); - sliderY->setCallback(this); - _infoPanel->addMenuItem(sliderY); - - sliderZ = new MenuRangeValue("Z",-1000.0,1000.0,kinectZ,0.01); - sliderZ->setCallback(this); - _infoPanel->addMenuItem(sliderZ); -/* - sliderRX = new MenuRangeValue("RX",-1000.0,1000.0,kinectRX,0.01); - sliderRX->setCallback(this); - _infoPanel->addMenuItem(sliderRX); - - sliderRY = new MenuRangeValue("RY",-1000.0,1000.0,kinectRY,0.01); - sliderRY->setCallback(this); - _infoPanel->addMenuItem(sliderRY); - - sliderRZ = new MenuRangeValue("RZ",-1000.0,1000.0,kinectRZ,0.01); - sliderRZ->setCallback(this); - _infoPanel->addMenuItem(sliderRZ); - - sliderRW = new MenuRangeValue("RW",-1000.0,1000.0,kinectRW,0.01); - sliderRW->setCallback(this); - _infoPanel->addMenuItem(sliderRW); -*/ + _kShowInfoPanel->setValue(true); + _infoPanel->setVisible(true); + sliderX = new MenuRangeValue("X", -1000.0, 1000.0, kinectX, 0.01); + sliderX->setCallback(this); + _infoPanel->addMenuItem(sliderX); + sliderY = new MenuRangeValue("Y", -1000.0, 1000.0, kinectY, 0.01); + sliderY->setCallback(this); + _infoPanel->addMenuItem(sliderY); + sliderZ = new MenuRangeValue("Z", -1000.0, 1000.0, kinectZ, 0.01); + sliderZ->setCallback(this); + _infoPanel->addMenuItem(sliderZ); + /* + sliderRX = new MenuRangeValue("RX",-1000.0,1000.0,kinectRX,0.01); + sliderRX->setCallback(this); + _infoPanel->addMenuItem(sliderRX); + + sliderRY = new MenuRangeValue("RY",-1000.0,1000.0,kinectRY,0.01); + sliderRY->setCallback(this); + _infoPanel->addMenuItem(sliderRY); + + sliderRZ = new MenuRangeValue("RZ",-1000.0,1000.0,kinectRZ,0.01); + sliderRZ->setCallback(this); + _infoPanel->addMenuItem(sliderRZ); + + sliderRW = new MenuRangeValue("RW",-1000.0,1000.0,kinectRW,0.01); + sliderRW->setCallback(this); + _infoPanel->addMenuItem(sliderRW); + */ } - float camX = ConfigManager::getFloat("x","Plugin.KinectDemo.CamStart",0.0f); - float camY = ConfigManager::getFloat("y","Plugin.KinectDemo.CamStart",0.0f); - float camZ = ConfigManager::getFloat("z","Plugin.KinectDemo.CamStart",0.0f); - float camS = ConfigManager::getFloat("s","Plugin.KinectDemo.CamStart",0.0f); - float camRX = ConfigManager::getFloat("rx","Plugin.KinectDemo.CamStart",0.0f); - float camRY = ConfigManager::getFloat("ry","Plugin.KinectDemo.CamStart",0.0f); - float camRZ = ConfigManager::getFloat("rz","Plugin.KinectDemo.CamStart",0.0f); - float camRW = ConfigManager::getFloat("rw","Plugin.KinectDemo.CamStart",0.0f); + float camX = ConfigManager::getFloat("x", "Plugin.KinectDemo.CamStart", 0.0f); + float camY = ConfigManager::getFloat("y", "Plugin.KinectDemo.CamStart", 0.0f); + float camZ = ConfigManager::getFloat("z", "Plugin.KinectDemo.CamStart", 0.0f); + float camS = ConfigManager::getFloat("s", "Plugin.KinectDemo.CamStart", 0.0f); + float camRX = ConfigManager::getFloat("rx", "Plugin.KinectDemo.CamStart", 0.0f); + float camRY = ConfigManager::getFloat("ry", "Plugin.KinectDemo.CamStart", 0.0f); + float camRZ = ConfigManager::getFloat("rz", "Plugin.KinectDemo.CamStart", 0.0f); + float camRW = ConfigManager::getFloat("rw", "Plugin.KinectDemo.CamStart", 0.0f); + //moveCam(273.923, 0.178878, -1.27967, 1.64388, -0.0247491, 0.294768, 0.952783, 0.0685912); //moveCam(camS, camX, camY, camZ, camRX, camRY, camRZ, camRW); -if(!skeletonThreaded) -{ - distanceMAX = ConfigManager::getFloat("Plugin.KinectDemo.Cylinder.Max"); - distanceMIN = ConfigManager::getFloat("Plugin.KinectDemo.Cylinder.Min"); - Skeleton::navSpheres = false; - bcounter = 0; - _modelFileNode1 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("kinect_mm.obj")); - // _modelFileNode2 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("dumptruck.osg")); - // _modelFileNode5 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("cessna.osg")); - // _modelFileNode4 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("cow.osg")); - // _modelFileNode3 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("robot.osg")); - _sphereRadius = 0.07; - // Group* kinectgrp = new Group(); - // kinectgrp->addChild(_modelFileNode3); - // createSceneObject(kinectgrp); - - // createSelObj(Vec3(-0.70, -2.0, 0.15), string("DD"), 0.002, _modelFileNode1); - // createSelObj(Vec3(-0.40, -2.0, 0.15), string("FD"), 0.007, _modelFileNode2); - // createSelObj(Vec3(0, -2.0, 0.15), string("GD"), 0.02, _modelFileNode3); - // createSelObj(Vec3(0.40, -2.0, 0.15), string("ED"), 0.02, _modelFileNode4); - // createSelObj(Vec3(0.70, -2.0, 0.15), string("ZD"), 0.002, _modelFileNode5); - -//CreateSceneObject for all Kinect Data - - createSceneObject(); + if (!skeletonThreaded) + { + distanceMAX = ConfigManager::getFloat("Plugin.KinectDemo.Cylinder.Max"); + distanceMIN = ConfigManager::getFloat("Plugin.KinectDemo.Cylinder.Min"); + Skeleton::navSpheres = false; + bcounter = 0; + _modelFileNode1 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("kinect_mm.obj")); + // _modelFileNode2 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("dumptruck.osg")); + // _modelFileNode5 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("cessna.osg")); + // _modelFileNode4 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("cow.osg")); + // _modelFileNode3 = osgDB::readNodeFile(ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("robot.osg")); + _sphereRadius = 0.07; + // Group* kinectgrp = new Group(); + // kinectgrp->addChild(_modelFileNode3); + // createSceneObject(kinectgrp); + // createSelObj(Vec3(-0.70, -2.0, 0.15), string("DD"), 0.002, _modelFileNode1); + // createSelObj(Vec3(-0.40, -2.0, 0.15), string("FD"), 0.007, _modelFileNode2); + // createSelObj(Vec3(0, -2.0, 0.15), string("GD"), 0.02, _modelFileNode3); + // createSelObj(Vec3(0.40, -2.0, 0.15), string("ED"), 0.02, _modelFileNode4); + // createSelObj(Vec3(0.70, -2.0, 0.15), string("ZD"), 0.002, _modelFileNode5); + //CreateSceneObject for all Kinect Data + createSceneObject(); + } -} minDistHSV = 700; maxDistHSV = 5000; minDistHSVDepth = 300; maxDistHSVDepth = 6000; -if(!kinectThreaded) -{ - // precompute colors for ... mm distances - for (int i = 0; i < 10001; i++) getColorRGB(i); - - for (int i = 0; i < 15001; i++) getColorRGBDepth(i); - // precompute packed values for colors on depthmap - for (int i = 0; i < 15000; i++) + if (!kinectThreaded) { - // XXX - // - // TODO SEE IF THERE IS RED/BLUE MAP ONCE CONVERTING TO MM - // TODO ADD MORE POINTS SEE IF LOOKS BETTER - // TODO CHECK IF O3 MAKES A DIFFERENCE - // - // XXX - //http://graphics.stanford.edu/~mdfisher/Kinect.html - // float valf = 0; - // if (i < 2047) { valf = float(1000.0 / (double(i) * -0.0030711016 + 3.3309495161)); } - // int val = valf; - //int val = float(1000.0 / (double(i) * -0.0030711016 + 3.3309495161)); // kinect data to milimeters - //cout << val << " "; - osg::Vec4 color = getColorRGBDepth(i); - char rrr = (char)((float)color.r() * 255.0); - char ggg = (char)((float)color.g() * 255.0); - char bbb = (char)((float)color.b() * 255.0); - uint32_t packed = (((rrr << 0) | (ggg << 8) | (bbb << 16)) & 0x00FFFFFF); - dpmap[i] = packed; + // precompute colors for ... mm distances + for (int i = 0; i < 10001; i++) getColorRGB(i); + + for (int i = 0; i < 15001; i++) getColorRGBDepth(i); + + // precompute packed values for colors on depthmap + for (int i = 0; i < 15000; i++) + { + // XXX + // + // TODO SEE IF THERE IS RED/BLUE MAP ONCE CONVERTING TO MM + // TODO ADD MORE POINTS SEE IF LOOKS BETTER + // TODO CHECK IF O3 MAKES A DIFFERENCE + // + // XXX + //http://graphics.stanford.edu/~mdfisher/Kinect.html + // float valf = 0; + // if (i < 2047) { valf = float(1000.0 / (double(i) * -0.0030711016 + 3.3309495161)); } + // int val = valf; + //int val = float(1000.0 / (double(i) * -0.0030711016 + 3.3309495161)); // kinect data to milimeters + //cout << val << " "; + osg::Vec4 color = getColorRGBDepth(i); + char rrr = (char)((float)color.r() * 255.0); + char ggg = (char)((float)color.g() * 255.0); + char bbb = (char)((float)color.b() * 255.0); + uint32_t packed = (((rrr << 0) | (ggg << 8) | (bbb << 16)) & 0x00FFFFFF); + dpmap[i] = packed; + } } -} + cout << endl << endl; ThirdInit(); kinectInitialized = true; @@ -727,97 +724,99 @@ void KinectDemo::createSelObj(osg::Vec3 pos, string color, float scalenumber, os void KinectDemo::ThirdLoop() { -if(!skeletonThreaded) -{ - // if skeletons are to be displayed in coorinates relative to the camera, position of the camera is saved in Skeleton::camPos,camRot (one for all skeletons) - Skeleton::camPos = Vec3(kinectX, kinectY, kinectZ); - // Skeleton::camPos = Vec3d(0, kinectY, 0); - float rotDegrees[3]; - rotDegrees[0] = 0; - rotDegrees[1] = 0; - rotDegrees[2] = 0; - rotDegrees[0] = DegreesToRadians(rotDegrees[0]); - rotDegrees[1] = DegreesToRadians(rotDegrees[1]); - rotDegrees[2] = DegreesToRadians(rotDegrees[2]); -Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1,0,0),rotDegrees[1], osg::Vec3d(0,1,0),rotDegrees[2], osg::Vec3d(0,0,1)); - // Skeleton::camRot = rot; - - if (Skeleton::moveWithCam) - { - //Matrixd camMat = PluginHelper::getWorldToObjectTransform(); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) - Matrixd camMat = PluginHelper::getHeadMat(0); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) - float cscale = 1; //Want to keep scale to actual Kinect which is is meters - Vec3 camTrans = camMat.getTrans(); - Quat camQuad = camMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! - //double xOffset = (camTrans.x() / cscale); - //double yOffset = (camTrans.y() / cscale) + 3; //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) - //double zOffset = (camTrans.z() / cscale); - double xOffset = camTrans.x(); - double yOffset = camTrans.y(); //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) - double zOffset = camTrans.z(); - Skeleton::camPos = Vec3d(xOffset, yOffset, zOffset); - Skeleton::camRot = camQuad; - } -if(!kFreezeCloud) -{ - while (skel_socket->recv(*sf)) + if (!skeletonThreaded) { - //return; - // remove all the skeletons that are no longer reported by the server - for (std::map::iterator it2 = mapIdSkel.begin(); it2 != mapIdSkel.end(); ++it2) + // if skeletons are to be displayed in coorinates relative to the camera, position of the camera is saved in Skeleton::camPos,camRot (one for all skeletons) + Skeleton::camPos = Vec3(kinectX, kinectY, kinectZ); + // Skeleton::camPos = Vec3d(0, kinectY, 0); + float rotDegrees[3]; + rotDegrees[0] = 0; + rotDegrees[1] = 0; + rotDegrees[2] = 0; + rotDegrees[0] = DegreesToRadians(rotDegrees[0]); + rotDegrees[1] = DegreesToRadians(rotDegrees[1]); + rotDegrees[2] = DegreesToRadians(rotDegrees[2]); + Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1, 0, 0), rotDegrees[1], osg::Vec3d(0, 1, 0), rotDegrees[2], osg::Vec3d(0, 0, 1)); + // Skeleton::camRot = rot; + + if (Skeleton::moveWithCam) { - bool found = false; + //Matrixd camMat = PluginHelper::getWorldToObjectTransform(); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) + Matrixd camMat = PluginHelper::getHeadMat(0); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) + float cscale = 1; //Want to keep scale to actual Kinect which is is meters + Vec3 camTrans = camMat.getTrans(); + Quat camQuad = camMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! + //double xOffset = (camTrans.x() / cscale); + //double yOffset = (camTrans.y() / cscale) + 3; //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) + //double zOffset = (camTrans.z() / cscale); + double xOffset = camTrans.x(); + double yOffset = camTrans.y(); //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) + double zOffset = camTrans.z(); + Skeleton::camPos = Vec3d(xOffset, yOffset, zOffset); + Skeleton::camRot = camQuad; + } - for (int i = 0; i < sf->skeletons_size(); i++) + if (!kFreezeCloud) + { + while (skel_socket->recv(*sf)) { - if (sf->skeletons(i).skeleton_id() == it2->first) + //return; + // remove all the skeletons that are no longer reported by the server + for (std::map::iterator it2 = mapIdSkel.begin(); it2 != mapIdSkel.end(); ++it2) { - found = true; - break; - } - } + bool found = false; - if (!found) - { - mapIdSkel[it2->first].detach(_pointClouds[0]->switchNode); - } - } + for (int i = 0; i < sf->skeletons_size(); i++) + { + if (sf->skeletons(i).skeleton_id() == it2->first) + { + found = true; + break; + } + } - // update all skeletons' joints' positions - for (int i = 0; i < sf->skeletons_size(); i++) - { - // Skeleton reported but not in the map -> create a new one - if (mapIdSkel.count(sf->skeletons(i).skeleton_id()) == 0) - { - mapIdSkel[sf->skeletons(i).skeleton_id()] = Skeleton(); ///XXX remove Skeleton(); part - // mapIdSkel[sf->skeletons(i).skeleton_id()].attach(_root); - cerr << "Found Skeleton\n"; - mapIdSkel[sf->skeletons(i).skeleton_id()].attach(_pointClouds[0]->switchNode); - } + if (!found) + { + mapIdSkel[it2->first].detach(_pointClouds[0]->switchNode); + } + } - // Skeleton previously detached (stopped being reported), but is again reported -> reattach - if (mapIdSkel[sf->skeletons(i).skeleton_id()].attached == false) - mapIdSkel[sf->skeletons(i).skeleton_id()].attach(_pointClouds[0]->switchNode); + // update all skeletons' joints' positions + for (int i = 0; i < sf->skeletons_size(); i++) + { + // Skeleton reported but not in the map -> create a new one + if (mapIdSkel.count(sf->skeletons(i).skeleton_id()) == 0) + { + mapIdSkel[sf->skeletons(i).skeleton_id()] = Skeleton(); ///XXX remove Skeleton(); part + // mapIdSkel[sf->skeletons(i).skeleton_id()].attach(_root); + cerr << "Found Skeleton\n"; + mapIdSkel[sf->skeletons(i).skeleton_id()].attach(_pointClouds[0]->switchNode); + } - for (int j = 0; j < sf->skeletons(i).joints_size(); j++) - { - mapIdSkel[sf->skeletons(i).skeleton_id()].update( - sf->skeletons(i).joints(j).type(), - sf->skeletons(i).joints(j).x(), - sf->skeletons(i).joints(j).z(), - sf->skeletons(i).joints(j).y(), - sf->skeletons(i).joints(j).qx(), - sf->skeletons(i).joints(j).qz(), - sf->skeletons(i).joints(j).qy(), - sf->skeletons(i).joints(j).qw()); + // Skeleton previously detached (stopped being reported), but is again reported -> reattach + if (mapIdSkel[sf->skeletons(i).skeleton_id()].attached == false) + mapIdSkel[sf->skeletons(i).skeleton_id()].attach(_pointClouds[0]->switchNode); + + for (int j = 0; j < sf->skeletons(i).joints_size(); j++) + { + mapIdSkel[sf->skeletons(i).skeleton_id()].update( + sf->skeletons(i).joints(j).type(), + sf->skeletons(i).joints(j).x(), + sf->skeletons(i).joints(j).z(), + sf->skeletons(i).joints(j).y(), + sf->skeletons(i).joints(j).qx(), + sf->skeletons(i).joints(j).qz(), + sf->skeletons(i).joints(j).qy(), + sf->skeletons(i).joints(j).qw()); + } + } } } } -} -} + if (kShowPCloud && cloud_socket != NULL && !kinectThreaded && !kFreezeCloud) { -// cerr << "."; + // cerr << "."; float r, g, b, a; kinectVertices = new osg::Vec3Array; kinectVertices->empty(); @@ -830,13 +829,15 @@ if(!kFreezeCloud) for (int i = 0; i < packet->points_size(); i++) { osg::Vec3 ppos(packet->points(i).x() + Skeleton::camPos.x(), - packet->points(i).z() + Skeleton::camPos.y(), - packet->points(i).y() + Skeleton::camPos.z()); - // ppos *= 0.0001; - if(i == 200000) - { - // cerr << "Y=" << ppos.y() << endl; - } + packet->points(i).z() + Skeleton::camPos.y(), + packet->points(i).y() + Skeleton::camPos.z()); + + // ppos *= 0.0001; + if (i == 200000) + { + // cerr << "Y=" << ppos.y() << endl; + } + kinectVertices->push_back(ppos); if (useKColor) @@ -845,7 +846,7 @@ if(!kFreezeCloud) g = (packet->points(i).g() / 255.); b = (packet->points(i).b() / 255.); a = 1; - kinectColours->push_back(osg::Vec4f(r, g, b, a)); + kinectColours->push_back(osg::Vec4f(r, g, b, a)); //kinectColours->push_back(osg::Vec4(1.0f,1.0f,0.0f,1.0f)); } else @@ -853,51 +854,51 @@ if(!kFreezeCloud) kinectColours->push_back(getColorRGB(packet->points(i).z())); } } - if(true) - { - osg::Geode* kgeode = new osg::Geode(); - kgeode->setCullingActive(false); - osg::Geometry* nodeGeom = new osg::Geometry(); - osg::StateSet* state = nodeGeom->getOrCreateStateSet(); - state->setMode(GL_LIGHTING, StateAttribute::OFF | osg::StateAttribute::OVERRIDE); - nodeGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, 0, kinectVertices->size())); - osg::VertexBufferObject* vboP = nodeGeom->getOrCreateVertexBufferObject(); - vboP->setUsage(GL_STREAM_DRAW); - nodeGeom->setUseDisplayList(false); - nodeGeom->setUseVertexBufferObjects(true); - nodeGeom->setVertexArray(kinectVertices); - nodeGeom->setColorArray(kinectColours); - nodeGeom->setColorBinding(osg::Geometry::BIND_PER_VERTEX); - kgeode->addDrawable(nodeGeom); - kgeode->dirtyBound(); - //if (kinectgrp != NULL) _root->removeChild(kinectgrp); - kinectgrp->removeChild(0, 1); - kinectgrp->addChild(kgeode); - } - else + + if (true) + { + osg::Geode* kgeode = new osg::Geode(); + kgeode->setCullingActive(false); + osg::Geometry* nodeGeom = new osg::Geometry(); + osg::StateSet* state = nodeGeom->getOrCreateStateSet(); + state->setMode(GL_LIGHTING, StateAttribute::OFF | osg::StateAttribute::OVERRIDE); + nodeGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, 0, kinectVertices->size())); + osg::VertexBufferObject* vboP = nodeGeom->getOrCreateVertexBufferObject(); + vboP->setUsage(GL_STREAM_DRAW); + nodeGeom->setUseDisplayList(false); + nodeGeom->setUseVertexBufferObjects(true); + nodeGeom->setVertexArray(kinectVertices); + nodeGeom->setColorArray(kinectColours); + nodeGeom->setColorBinding(osg::Geometry::BIND_PER_VERTEX); + kgeode->addDrawable(nodeGeom); + kgeode->dirtyBound(); + //if (kinectgrp != NULL) _root->removeChild(kinectgrp); + kinectgrp->removeChild(0, 1); + kinectgrp->addChild(kgeode); + } + else { - osg::Geode* kgeode = new osg::Geode(); - kgeode->setCullingActive(false); - osg::Geometry* geometry = new osg::Geometry(); - geometry->setUseDisplayList(false); - geometry->setUseVertexBufferObjects(true); - // geometry->setVertexArray(kinectVertices); - geometry->setVertexArray(kinectVertices); - geometry->setNormalArray(normals); - geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX); - geometry->setColorArray(kinectColours); - geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX); - geometry->addPrimitiveSet(new osg::DrawArrays(GL_POINTS, 0, kinectVertices->size())); - //_root->removeChild(pointGeode); - //pointGeode = new Geode(); - StateSet* ss = kgeode->getOrCreateStateSet(); - ss->setMode(GL_LIGHTING, StateAttribute::OFF | osg::StateAttribute::OVERRIDE); - kgeode->addDrawable(geometry); - kinectgrp->removeChild(0, 1); - kinectgrp->addChild(kgeode); - //_root->addChild(pointGeode); - - } + osg::Geode* kgeode = new osg::Geode(); + kgeode->setCullingActive(false); + osg::Geometry* geometry = new osg::Geometry(); + geometry->setUseDisplayList(false); + geometry->setUseVertexBufferObjects(true); + // geometry->setVertexArray(kinectVertices); + geometry->setVertexArray(kinectVertices); + geometry->setNormalArray(normals); + geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX); + geometry->setColorArray(kinectColours); + geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX); + geometry->addPrimitiveSet(new osg::DrawArrays(GL_POINTS, 0, kinectVertices->size())); + //_root->removeChild(pointGeode); + //pointGeode = new Geode(); + StateSet* ss = kgeode->getOrCreateStateSet(); + ss->setMode(GL_LIGHTING, StateAttribute::OFF | osg::StateAttribute::OVERRIDE); + kgeode->addDrawable(geometry); + kinectgrp->removeChild(0, 1); + kinectgrp->addChild(kgeode); + //_root->addChild(pointGeode); + } } } @@ -943,60 +944,58 @@ if(!kFreezeCloud) void KinectDemo::cloudOff() { -if(!kinectThreaded) -{ - if (cloud_socket) { - delete cloud_socket; - cloud_socket = NULL; - } - - if (kinectgrp) + if (!kinectThreaded) { - pgm1->ref(); - kinectgrp->removeChild(0, 1); - // _root->removeChild(kinectgrp); - SceneManager::instance()->getScene()->removeChild(kinectgrp); - kinectgrp = NULL; + if (cloud_socket) { + delete cloud_socket; + cloud_socket = NULL; + } + + if (kinectgrp) + { + pgm1->ref(); + kinectgrp->removeChild(0, 1); + // _root->removeChild(kinectgrp); + SceneManager::instance()->getScene()->removeChild(kinectgrp); + kinectgrp = NULL; + } } -} -else -{ + else + { _cloudThread->quit(); _cloudThread->cancel(); -} + } } void KinectDemo::cloudOn() { - -if(kShowPCloud) -{ - if(!kinectThreaded) - { - if (!cloud_socket) cloud_socket = new SubSocket (context, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.PointCloud")); - - kinectgrp = new osg::Group(); - osg::StateSet* state = kinectgrp->getOrCreateStateSet(); - state->setAttribute(pgm1); - state->addUniform(new osg::Uniform("pointScale", initialPointScale)); - state->addUniform(new osg::Uniform("globalAlpha", 1.0f)); - float pscale = initialPointScale; - osg::Uniform* _scaleUni = new osg::Uniform("pointScale", 1.0f * pscale); - kinectgrp->getOrCreateStateSet()->addUniform(_scaleUni); - _pointClouds[0]->switchNode->addChild(kinectgrp); -//bangKN - - // SceneManager::instance()->getScene()->addChild(kinectgrp); - // _root->addChild(kinectgrp); - } - else - { - cout << "Starting Thread\n"; - _cloudThread = new CloudManager(); - cout <<"Started\n"; - _cloudThread->start(); - } -} + if (kShowPCloud) + { + if (!kinectThreaded) + { + if (!cloud_socket) cloud_socket = new SubSocket (context, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.PointCloud")); + + kinectgrp = new osg::Group(); + osg::StateSet* state = kinectgrp->getOrCreateStateSet(); + state->setAttribute(pgm1); + state->addUniform(new osg::Uniform("pointScale", initialPointScale)); + state->addUniform(new osg::Uniform("globalAlpha", 1.0f)); + float pscale = initialPointScale; + osg::Uniform* _scaleUni = new osg::Uniform("pointScale", 1.0f * pscale); + kinectgrp->getOrCreateStateSet()->addUniform(_scaleUni); + _pointClouds[0]->switchNode->addChild(kinectgrp); + //bangKN + // SceneManager::instance()->getScene()->addChild(kinectgrp); + // _root->addChild(kinectgrp); + } + else + { + cout << "Starting Thread\n"; + _cloudThread = new CloudManager(); + cout << "Started\n"; + _cloudThread->start(); + } + } } void KinectDemo::colorOff() @@ -1194,7 +1193,7 @@ void KinectDemo::kinectOff() skel_socket = NULL; } -// _kShowColor->setValue(false); + // _kShowColor->setValue(false); _kShowDepth->setValue(false); _kShowPCloud->setValue(false); _kNavSpheres->setValue(false); @@ -1213,29 +1212,28 @@ void KinectDemo::kinectOff() } void KinectDemo::ThirdInit() { -if(!skeletonThreaded) -{ - sf = new RemoteKinect::SkeletonFrame(); - skel_socket = new SubSocket (context, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.Skeleton")); + if (!skeletonThreaded) + { + sf = new RemoteKinect::SkeletonFrame(); + skel_socket = new SubSocket (context, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.Skeleton")); + } + else + { + cout << "Starting Skeleton Thread\n"; + // _skeletonThread = new SkeletonManager(); + cout << "Started\n"; + // _skeletonThread->start(); + } -} -else -{ - cout << "Starting Skeleton Thread\n"; - // _skeletonThread = new SkeletonManager(); - cout <<"Started\n"; - // _skeletonThread->start(); -} -if(!kinectThreaded) -{ - packet = new RemoteKinect::PointCloud(); + if (!kinectThreaded) + { + packet = new RemoteKinect::PointCloud(); + } -} //cm = new RemoteKinect::ColorMap(); //dm = new RemoteKinect::DepthMap(); //cloud_socket = NULL; cloudOn(); - } osg::Vec4f KinectDemo::getColorRGB(int dist) @@ -1319,54 +1317,43 @@ void KinectDemo::checkHandsIntersections(int skel_id) void KinectDemo::updateInfoPanel() { - std::stringstream ss; -//kinectUsers - -if(cloud_socket) -{ - if(cloud_socket->recv(*packet)) - { - - - - } - + //kinectUsers + if (cloud_socket) + { + if (cloud_socket->recv(*packet)) + { + } + } -} - // ss << "FPS: " << navSphereTimer << endl; - // ss << "X: " << kinectX << endl; - Matrixd camMat = PluginHelper::getHeadMat(0); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) - float cscale = 1; //Want to keep scale to actual Kinect which is is meters - Vec3 camTrans = camMat.getTrans(); - Quat camQuad = camMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! - double xOffset = camTrans.x(); - double yOffset = camTrans.y(); //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) - double zOffset = camTrans.z(); - double rxOffset = camQuad.x(); - double ryOffset = camQuad.y(); - double rzOffset = camQuad.z(); - double rwOffset = camQuad.w(); - // ss << "HeadMat X:" << xOffset << " Y:" << yOffset << " Z:" << zOffset << " RX:" << rxOffset << " RY:" << ryOffset << " RZ:" << rzOffset << " RW:" << rwOffset << endl; - - Matrixd handMat = PluginHelper::getHandMat(0); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) - Vec3 handTrans = handMat.getTrans(); - Quat handQuad = handMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! - xOffset = handTrans.x(); - yOffset = handTrans.y(); //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) - zOffset = handTrans.z(); - rxOffset = handQuad.x(); - ryOffset = handQuad.y(); - rzOffset = handQuad.z(); - rwOffset = handQuad.w(); - - // ss << "HandMat X:" << xOffset << " Y:" << yOffset << " Z:" << zOffset << " RX:" << rxOffset << " RY:" << ryOffset << " RZ:" << rzOffset << " RW:" << rwOffset << endl; - - -//GetSkel -ss << "Skeletons Tracked: " << sf->skeletons_size() << endl; - + // ss << "FPS: " << navSphereTimer << endl; + // ss << "X: " << kinectX << endl; + Matrixd camMat = PluginHelper::getHeadMat(0); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) + float cscale = 1; //Want to keep scale to actual Kinect which is is meters + Vec3 camTrans = camMat.getTrans(); + Quat camQuad = camMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! + double xOffset = camTrans.x(); + double yOffset = camTrans.y(); //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) + double zOffset = camTrans.z(); + double rxOffset = camQuad.x(); + double ryOffset = camQuad.y(); + double rzOffset = camQuad.z(); + double rwOffset = camQuad.w(); + // ss << "HeadMat X:" << xOffset << " Y:" << yOffset << " Z:" << zOffset << " RX:" << rxOffset << " RY:" << ryOffset << " RZ:" << rzOffset << " RW:" << rwOffset << endl; + Matrixd handMat = PluginHelper::getHandMat(0); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) + Vec3 handTrans = handMat.getTrans(); + Quat handQuad = handMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! + xOffset = handTrans.x(); + yOffset = handTrans.y(); //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) + zOffset = handTrans.z(); + rxOffset = handQuad.x(); + ryOffset = handQuad.y(); + rzOffset = handQuad.z(); + rwOffset = handQuad.w(); + // ss << "HandMat X:" << xOffset << " Y:" << yOffset << " Z:" << zOffset << " RX:" << rxOffset << " RY:" << ryOffset << " RZ:" << rzOffset << " RW:" << rwOffset << endl; + //GetSkel + ss << "Skeletons Tracked: " << sf->skeletons_size() << endl; std::map< osg::ref_ptr, int >::iterator iter; for (std::map::iterator it = mapIdSkel.begin(); it != mapIdSkel.end(); ++it) @@ -1383,513 +1370,458 @@ ss << "Skeletons Tracked: " << sf->skeletons_size() << endl; break; } - - - - - - _infoPanel->updateTabWithText("Info", ss.str()); } void KinectDemo::ExportPointCloud() { + printf("Triggered\n"); + osg::Vec3Array* vertices = kinectVertices; + osg::Vec4Array* colours = kinectColours; + FILE* pFile; + string file = ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("kinectDump.ply"); + pFile = fopen(file.c_str(), "w"); + fprintf(pFile, "ply\nformat ascii 1.0\ncomment VCGLIB generated\n"); + fprintf(pFile, "element vertex %i\n", vertices->size()); + fprintf(pFile, "property float x\nproperty float y\nproperty float z\nproperty uchar red\nproperty uchar green\nproperty uchar blue\nproperty uchar alpha\nelement face 0\nproperty list uchar int vertex_indices\nend_header\n"); + + for (int i = 0; i < vertices->size() ; i++) + { + double x = vertices->at(i).x(); // * 1000; + double y = vertices->at(i).y();// * 1000; + double z = vertices->at(i).z();//* 1000; + double r = colours->at(i).r() * 255; + double g = colours->at(i).g() * 255; + double b = colours->at(i).b() * 255; + double a = colours->at(i).a(); + int ri = int (r); + int gi = int (g); + int bi = int (b); + int ai = int (a); + fprintf(pFile, "%f %f %f %i %i %i %i\n", x, y, z, ri, gi, bi, ai); + } - printf("Triggered\n"); - osg::Vec3Array* vertices = kinectVertices; - osg::Vec4Array* colours = kinectColours; - FILE* pFile; - string file = ConfigManager::getEntry("Plugin.KinectDemo.3DModelFolder").append("kinectDump.ply"); - pFile = fopen(file.c_str(), "w"); - fprintf(pFile, "ply\nformat ascii 1.0\ncomment VCGLIB generated\n"); - fprintf(pFile, "element vertex %i\n", vertices->size()); - fprintf(pFile, "property float x\nproperty float y\nproperty float z\nproperty uchar red\nproperty uchar green\nproperty uchar blue\nproperty uchar alpha\nelement face 0\nproperty list uchar int vertex_indices\nend_header\n"); - - for (int i = 0; i < vertices->size() ; i++) - { - double x = vertices->at(i).x(); // * 1000; - double y = vertices->at(i).y();// * 1000; - double z = vertices->at(i).z();//* 1000; - double r = colours->at(i).r() * 255; - double g = colours->at(i).g() * 255; - double b = colours->at(i).b() * 255; - double a = colours->at(i).a(); - int ri = int (r); - int gi = int (g); - int bi = int (b); - int ai = int (a); - fprintf(pFile, "%f %f %f %i %i %i %i\n", x, y, z, ri, gi, bi, ai); - - } - - fclose(pFile); - + fclose(pFile); } void KinectDemo::createSceneObject() { -cerr << "Creating SceneObject\n"; - PointCloud* newModel = new PointCloud(); - _pointClouds.push_back(newModel); - string name = "test"; - int i = _pointClouds.size() -1; - if(i == -1) return; - // float currentScale = _pointClouds[i]->scale; - float currentScale = 1; - - SceneObject * so; - so = new SceneObject(name, false, false, false, true, false); - osg::Switch* switchNode = new osg::Switch(); - so->addChild(switchNode); - PluginHelper::registerSceneObject(so,"Test"); - so->attachToScene(); -//Add currentNode to switchNode - // _models3d[i]->currentModelNode = modelNode; - cerr << "here\n"; -// switchNode->addChild(kinectgrp); - if(i == 0) + cerr << "Creating SceneObject\n"; + PointCloud* newModel = new PointCloud(); + _pointClouds.push_back(newModel); + string name = "test"; + int i = _pointClouds.size() - 1; + + if (i == -1) return; + + // float currentScale = _pointClouds[i]->scale; + float currentScale = 1; + SceneObject* so; + so = new SceneObject(name, false, false, false, true, false); + osg::Switch* switchNode = new osg::Switch(); + so->addChild(switchNode); + PluginHelper::registerSceneObject(so, "Test"); + so->attachToScene(); + //Add currentNode to switchNode + // _models3d[i]->currentModelNode = modelNode; + cerr << "here\n"; + + // switchNode->addChild(kinectgrp); + if (i == 0) + { + // switchNode->addChild(_modelFileNode4); + if (false) { -// switchNode->addChild(_modelFileNode4); - - if(false) - { - Vec3d poz0(kinectX, kinectY, kinectZ); - Box* sphereShape = new Box(poz0, 50.0); - ShapeDrawable* ggg2 = new ShapeDrawable(sphereShape); - ggg2->setColor(Vec4(1,1,1,1)); - osg::Geode* boxGeode = new osg::Geode; - boxGeode->addDrawable(ggg2); - - switchNode->addChild(boxGeode); - } - - if(ConfigManager::getBool("Plugin.KinectDemo.ShowKinectModel")) - { - //Loads Kinect Obj file - Matrixd scale; - double snum = 1; - scale.makeScale(snum, snum, snum); - MatrixTransform* modelScaleTrans = new MatrixTransform(); - modelScaleTrans->setMatrix(scale); - modelScaleTrans->addChild(_modelFileNode1); - MatrixTransform* rotate = new osg::MatrixTransform(); - - - float rotDegrees[3]; - rotDegrees[0] = -90; - rotDegrees[1] = 0; - rotDegrees[2] = 180; - rotDegrees[0] = DegreesToRadians(rotDegrees[0]); - rotDegrees[1] = DegreesToRadians(rotDegrees[1]); - rotDegrees[2] = DegreesToRadians(rotDegrees[2]); - Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1,0,0),rotDegrees[1], osg::Vec3d(0,1,0),rotDegrees[2], osg::Vec3d(0,0,1)); - - Matrix rotMat; - rotMat.makeRotate(rot); - rotate->setMatrix(rotMat); - rotate->addChild(modelScaleTrans); - MatrixTransform* translate = new osg::MatrixTransform(); - osg::Matrixd tmat; - Vec3 pos = Vec3(kinectX,kinectY,kinectZ); - tmat.makeTranslate(pos); - translate->setMatrix(tmat); - translate->addChild(rotate); - switchNode->addChild(translate); - + Vec3d poz0(kinectX, kinectY, kinectZ); + Box* sphereShape = new Box(poz0, 50.0); + ShapeDrawable* ggg2 = new ShapeDrawable(sphereShape); + ggg2->setColor(Vec4(1, 1, 1, 1)); + osg::Geode* boxGeode = new osg::Geode; + boxGeode->addDrawable(ggg2); + switchNode->addChild(boxGeode); + } + if (ConfigManager::getBool("Plugin.KinectDemo.ShowKinectModel")) + { + //Loads Kinect Obj file + Matrixd scale; + double snum = 1; + scale.makeScale(snum, snum, snum); + MatrixTransform* modelScaleTrans = new MatrixTransform(); + modelScaleTrans->setMatrix(scale); + modelScaleTrans->addChild(_modelFileNode1); + MatrixTransform* rotate = new osg::MatrixTransform(); + float rotDegrees[3]; + rotDegrees[0] = -90; + rotDegrees[1] = 0; + rotDegrees[2] = 180; + rotDegrees[0] = DegreesToRadians(rotDegrees[0]); + rotDegrees[1] = DegreesToRadians(rotDegrees[1]); + rotDegrees[2] = DegreesToRadians(rotDegrees[2]); + Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1, 0, 0), rotDegrees[1], osg::Vec3d(0, 1, 0), rotDegrees[2], osg::Vec3d(0, 0, 1)); + Matrix rotMat; + rotMat.makeRotate(rot); + rotate->setMatrix(rotMat); + rotate->addChild(modelScaleTrans); + MatrixTransform* translate = new osg::MatrixTransform(); + osg::Matrixd tmat; + Vec3 pos = Vec3(kinectX, kinectY, kinectZ); + tmat.makeTranslate(pos); + translate->setMatrix(tmat); + translate->addChild(rotate); + switchNode->addChild(translate); + } - } - if(ConfigManager::getBool("Plugin.KinectDemo.ShowScreenFrames")) + if (ConfigManager::getBool("Plugin.KinectDemo.ShowScreenFrames")) + { + //Draw Configured Screens + int numWindows = ScreenConfig::instance()->getNumWindows(); + float width; + float height; + float h; + float p; + float r; + Vec3 offsetScreen; + cerr << "NumWindows: " << numWindows << endl; + + //TODO:Get Screen Info from Config file + for (int j = 0; j < numWindows; j++) { - //Draw Configured Screens - int numWindows =ScreenConfig::instance()->getNumWindows(); - float width; - float height; - float h; - float p; - float r; - Vec3 offsetScreen; - cerr << "NumWindows: " << numWindows << endl; - //TODO:Get Screen Info from Config file - for (int j = 0; j < numWindows; j++) - { ScreenInfo* si = ScreenConfig::instance()->getScreenInfo(j); - width = si->width; - height = si->height; - h = si->h; - p = si->p; - r = si->r; - offsetScreen = si->xyz; - - //Create Quad Face - // float width = 300; - // float height = 500; - Vec3 pos = Vec3(-(width/2),0,-(height/2)); - Vec4f color = Vec4f(0, 0.42, 0.92, 1); - //Ofset Pos - pos += offsetScreen; - osg::Geometry * geo = new osg::Geometry(); - osg::Vec3Array* verts = new osg::Vec3Array(); - verts->push_back(pos); - verts->push_back(pos + osg::Vec3(width,0,0)); - verts->push_back(pos + osg::Vec3(width,0,height)); - verts->push_back(pos + osg::Vec3(0,0,height)); - - geo->setVertexArray(verts); - - osg::DrawElementsUInt * ele = new osg::DrawElementsUInt( - osg::PrimitiveSet::QUADS,0); - - ele->push_back(0); - ele->push_back(1); - ele->push_back(2); - ele->push_back(3); - geo->addPrimitiveSet(ele); - - Geode* fgeode = new Geode(); - StateSet* state(fgeode->getOrCreateStateSet()); - Material* mat(new Material); - - mat->setColorMode(Material::DIFFUSE); - mat->setDiffuse(Material::FRONT_AND_BACK, color); - state->setAttribute(mat); - state->setRenderingHint(StateSet::TRANSPARENT_BIN); - state->setMode(GL_BLEND, StateAttribute::ON); - state->setMode(GL_LIGHTING, StateAttribute::OFF); - osg::PolygonMode* polymode = new osg::PolygonMode; - polymode->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); - state->setAttributeAndModes(polymode, osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON); - fgeode->setStateSet(state); - - // _annotations[inc]->geo = geo; - fgeode->addDrawable(geo); - float rotDegrees[3]; - rotDegrees[0] = h; - rotDegrees[1] = p; - rotDegrees[2] = r; - rotDegrees[0] = DegreesToRadians(rotDegrees[0]); - rotDegrees[1] = DegreesToRadians(rotDegrees[1]); - rotDegrees[2] = DegreesToRadians(rotDegrees[2]); - Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1,0,0),rotDegrees[1], osg::Vec3d(0,1,0),rotDegrees[2], osg::Vec3d(0,0,1)); - - MatrixTransform* rotate = new osg::MatrixTransform(); - Matrix rotMat; - rotMat.makeRotate(rot); - rotate->setMatrix(rotMat); - rotate->addChild(fgeode); - - switchNode->addChild(rotate); - } - if(ConfigManager::getBool("Plugin.KinectDemo.ShowKinectFOV")) - { - //Draw Kinect FOV - float width; - float height; - Vec3 offsetScreen = Vec3(0,500,0); - Vec3 pos; - Vec4f color = Vec4f(0, 0.42, 0.92, 1); - //Create Quad Face - width = 543; - height = 394; - pos = Vec3(-(width/2),0,-(height/2)); - pos += Vec3(kinectX,kinectY,kinectZ); - pos += offsetScreen; - osg::Geometry * geo = new osg::Geometry(); - osg::Vec3Array* verts = new osg::Vec3Array(); - verts->push_back(pos); - verts->push_back(pos + osg::Vec3(width,0,0)); - verts->push_back(pos + osg::Vec3(width,0,height)); - verts->push_back(pos + osg::Vec3(0,0,height)); - //do it Again - width = 3800.6; - height = 2756; - offsetScreen = Vec3(0,3500,0); - pos = Vec3(-(width/2),0,-(height/2)); - pos += Vec3(kinectX,kinectY,kinectZ); - pos += offsetScreen; - verts->push_back(pos); - verts->push_back(pos + osg::Vec3(width,0,0)); - verts->push_back(pos + osg::Vec3(width,0,height)); - verts->push_back(pos + osg::Vec3(0,0,height)); - //.................................... - - - int size = verts->size() / 2; - - Geometry* geom = new Geometry(); - Geometry* tgeom = new Geometry(); - Geode* fgeode = new Geode(); - Geode* lgeode = new Geode(); - geom->setVertexArray(verts); - tgeom->setVertexArray(verts); - - for (int n = 0; n < size; n++) - { - DrawElementsUInt* face = new DrawElementsUInt(PrimitiveSet::QUADS, 0); - face->push_back(n); - face->push_back(n + size); - face->push_back(((n + 1) % size) + size); - face->push_back((n + 1) % size); - geom->addPrimitiveSet(face); - } - - StateSet* state(fgeode->getOrCreateStateSet()); - Material* mat(new Material); - mat->setColorMode(Material::DIFFUSE); - mat->setDiffuse(Material::FRONT_AND_BACK, color); - state->setAttribute(mat); - state->setRenderingHint(StateSet::OPAQUE_BIN); - state->setMode(GL_BLEND, StateAttribute::ON); - state->setMode(GL_LIGHTING, StateAttribute::OFF); - osg::PolygonMode* polymode = new osg::PolygonMode; - polymode->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); - state->setAttributeAndModes(polymode, osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON); - fgeode->setStateSet(state); - fgeode->addDrawable(geom); - if(false) - { - geo->setVertexArray(verts); - - osg::DrawElementsUInt * ele = new osg::DrawElementsUInt( - osg::PrimitiveSet::QUADS,0); - - ele->push_back(0); - ele->push_back(1); - ele->push_back(2); - ele->push_back(3); - - - ele->push_back(4); - ele->push_back(5); - ele->push_back(6); - ele->push_back(7); - geo->addPrimitiveSet(ele); - - Geode* fgeode = new Geode(); - StateSet* state(fgeode->getOrCreateStateSet()); - Material* mat(new Material); - - mat->setColorMode(Material::DIFFUSE); - mat->setDiffuse(Material::FRONT_AND_BACK, color); - state->setAttribute(mat); - state->setRenderingHint(StateSet::TRANSPARENT_BIN); - state->setMode(GL_BLEND, StateAttribute::ON); - state->setMode(GL_LIGHTING, StateAttribute::OFF); - osg::PolygonMode* polymode = new osg::PolygonMode; - polymode->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); - state->setAttributeAndModes(polymode, osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON); - fgeode->setStateSet(state); - - // _annotations[inc]->geo = geo; - fgeode->addDrawable(geo); - } - float rotDegrees[3]; - rotDegrees[0] = 0; - rotDegrees[1] = 0; - rotDegrees[2] = 0; - rotDegrees[0] = DegreesToRadians(rotDegrees[0]); - rotDegrees[1] = DegreesToRadians(rotDegrees[1]); - rotDegrees[2] = DegreesToRadians(rotDegrees[2]); - Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1,0,0),rotDegrees[1], osg::Vec3d(0,1,0),rotDegrees[2], osg::Vec3d(0,0,1)); - - MatrixTransform* rotate = new osg::MatrixTransform(); - Matrix rotMat; - rotMat.makeRotate(rot); - rotate->setMatrix(rotMat); - rotate->addChild(fgeode); - - switchNode->addChild(rotate); - } + width = si->width; + height = si->height; + h = si->h; + p = si->p; + r = si->r; + offsetScreen = si->xyz; + //Create Quad Face + // float width = 300; + // float height = 500; + Vec3 pos = Vec3(-(width / 2), 0, -(height / 2)); + Vec4f color = Vec4f(0, 0.42, 0.92, 1); + //Ofset Pos + pos += offsetScreen; + osg::Geometry* geo = new osg::Geometry(); + osg::Vec3Array* verts = new osg::Vec3Array(); + verts->push_back(pos); + verts->push_back(pos + osg::Vec3(width, 0, 0)); + verts->push_back(pos + osg::Vec3(width, 0, height)); + verts->push_back(pos + osg::Vec3(0, 0, height)); + geo->setVertexArray(verts); + osg::DrawElementsUInt* ele = new osg::DrawElementsUInt( + osg::PrimitiveSet::QUADS, 0); + ele->push_back(0); + ele->push_back(1); + ele->push_back(2); + ele->push_back(3); + geo->addPrimitiveSet(ele); + Geode* fgeode = new Geode(); + StateSet* state(fgeode->getOrCreateStateSet()); + Material* mat(new Material); + mat->setColorMode(Material::DIFFUSE); + mat->setDiffuse(Material::FRONT_AND_BACK, color); + state->setAttribute(mat); + state->setRenderingHint(StateSet::TRANSPARENT_BIN); + state->setMode(GL_BLEND, StateAttribute::ON); + state->setMode(GL_LIGHTING, StateAttribute::OFF); + osg::PolygonMode* polymode = new osg::PolygonMode; + polymode->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); + state->setAttributeAndModes(polymode, osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON); + fgeode->setStateSet(state); + // _annotations[inc]->geo = geo; + fgeode->addDrawable(geo); + float rotDegrees[3]; + rotDegrees[0] = h; + rotDegrees[1] = p; + rotDegrees[2] = r; + rotDegrees[0] = DegreesToRadians(rotDegrees[0]); + rotDegrees[1] = DegreesToRadians(rotDegrees[1]); + rotDegrees[2] = DegreesToRadians(rotDegrees[2]); + Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1, 0, 0), rotDegrees[1], osg::Vec3d(0, 1, 0), rotDegrees[2], osg::Vec3d(0, 0, 1)); + MatrixTransform* rotate = new osg::MatrixTransform(); + Matrix rotMat; + rotMat.makeRotate(rot); + rotate->setMatrix(rotMat); + rotate->addChild(fgeode); + switchNode->addChild(rotate); } + if (ConfigManager::getBool("Plugin.KinectDemo.ShowKinectFOV")) + { + //Draw Kinect FOV + float width; + float height; + Vec3 offsetScreen = Vec3(0, 500, 0); + Vec3 pos; + Vec4f color = Vec4f(0, 0.42, 0.92, 1); + //Create Quad Face + width = 543; + height = 394; + pos = Vec3(-(width / 2), 0, -(height / 2)); + pos += Vec3(kinectX, kinectY, kinectZ); + pos += offsetScreen; + osg::Geometry* geo = new osg::Geometry(); + osg::Vec3Array* verts = new osg::Vec3Array(); + verts->push_back(pos); + verts->push_back(pos + osg::Vec3(width, 0, 0)); + verts->push_back(pos + osg::Vec3(width, 0, height)); + verts->push_back(pos + osg::Vec3(0, 0, height)); + //do it Again + width = 3800.6; + height = 2756; + offsetScreen = Vec3(0, 3500, 0); + pos = Vec3(-(width / 2), 0, -(height / 2)); + pos += Vec3(kinectX, kinectY, kinectZ); + pos += offsetScreen; + verts->push_back(pos); + verts->push_back(pos + osg::Vec3(width, 0, 0)); + verts->push_back(pos + osg::Vec3(width, 0, height)); + verts->push_back(pos + osg::Vec3(0, 0, height)); + //.................................... + int size = verts->size() / 2; + Geometry* geom = new Geometry(); + Geometry* tgeom = new Geometry(); + Geode* fgeode = new Geode(); + Geode* lgeode = new Geode(); + geom->setVertexArray(verts); + tgeom->setVertexArray(verts); + + for (int n = 0; n < size; n++) + { + DrawElementsUInt* face = new DrawElementsUInt(PrimitiveSet::QUADS, 0); + face->push_back(n); + face->push_back(n + size); + face->push_back(((n + 1) % size) + size); + face->push_back((n + 1) % size); + geom->addPrimitiveSet(face); + } + StateSet* state(fgeode->getOrCreateStateSet()); + Material* mat(new Material); + mat->setColorMode(Material::DIFFUSE); + mat->setDiffuse(Material::FRONT_AND_BACK, color); + state->setAttribute(mat); + state->setRenderingHint(StateSet::OPAQUE_BIN); + state->setMode(GL_BLEND, StateAttribute::ON); + state->setMode(GL_LIGHTING, StateAttribute::OFF); + osg::PolygonMode* polymode = new osg::PolygonMode; + polymode->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); + state->setAttributeAndModes(polymode, osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON); + fgeode->setStateSet(state); + fgeode->addDrawable(geom); + + if (false) + { + geo->setVertexArray(verts); + osg::DrawElementsUInt* ele = new osg::DrawElementsUInt( + osg::PrimitiveSet::QUADS, 0); + ele->push_back(0); + ele->push_back(1); + ele->push_back(2); + ele->push_back(3); + ele->push_back(4); + ele->push_back(5); + ele->push_back(6); + ele->push_back(7); + geo->addPrimitiveSet(ele); + Geode* fgeode = new Geode(); + StateSet* state(fgeode->getOrCreateStateSet()); + Material* mat(new Material); + mat->setColorMode(Material::DIFFUSE); + mat->setDiffuse(Material::FRONT_AND_BACK, color); + state->setAttribute(mat); + state->setRenderingHint(StateSet::TRANSPARENT_BIN); + state->setMode(GL_BLEND, StateAttribute::ON); + state->setMode(GL_LIGHTING, StateAttribute::OFF); + osg::PolygonMode* polymode = new osg::PolygonMode; + polymode->setMode(osg::PolygonMode::FRONT_AND_BACK, osg::PolygonMode::LINE); + state->setAttributeAndModes(polymode, osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON); + fgeode->setStateSet(state); + // _annotations[inc]->geo = geo; + fgeode->addDrawable(geo); + } + float rotDegrees[3]; + rotDegrees[0] = 0; + rotDegrees[1] = 0; + rotDegrees[2] = 0; + rotDegrees[0] = DegreesToRadians(rotDegrees[0]); + rotDegrees[1] = DegreesToRadians(rotDegrees[1]); + rotDegrees[2] = DegreesToRadians(rotDegrees[2]); + Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1, 0, 0), rotDegrees[1], osg::Vec3d(0, 1, 0), rotDegrees[2], osg::Vec3d(0, 0, 1)); + MatrixTransform* rotate = new osg::MatrixTransform(); + Matrix rotMat; + rotMat.makeRotate(rot); + rotate->setMatrix(rotMat); + rotate->addChild(fgeode); + switchNode->addChild(rotate); + } } - _pointClouds[i]->switchNode = switchNode; - -//Add menu system - so->setNavigationOn(true); - so->setMovable(false); - so->addMoveMenuItem(); - so->addNavigationMenuItem(); - float min = 0.0001; - float max = 1; - so->addScaleMenuItem("Scale",min,max,currentScale); - SubMenu * sm = new SubMenu("Position"); - so->addMenuItem(sm); - - MenuButton * mb; - mb = new MenuButton("Load"); - mb->setCallback(this); - sm->addItem(mb); - - SubMenu * savemenu = new SubMenu("Save"); - sm->addItem(savemenu); - - mb = new MenuButton("Save"); - mb->setCallback(this); - savemenu->addItem(mb); - _pointClouds[i]->saveMap = mb; - - mb = new MenuButton("Save New Kml"); - mb->setCallback(this); - savemenu->addItem(mb); - _pointClouds[i]->saveNewMap = mb; - - mb = new MenuButton("Reset to Origin"); - mb->setCallback(this); - so->addMenuItem(mb); - _pointClouds[i]->resetMap = mb; - - MenuCheckbox * mc; - mc = new MenuCheckbox("Active",false); - mc->setCallback(this); - so->addMenuItem(mc); - _pointClouds[i]->activeMap = mc; - - - mc = new MenuCheckbox("Visible",true); - mc->setCallback(this); - so->addMenuItem(mc); - _pointClouds[i]->visibleMap = mc; - _pointClouds[i]->visible = true; - - float rValue = 0; - min = -1; - max = 1; - MenuRangeValue* rt = new MenuRangeValue("rx",min,max,rValue); - rt->setCallback(this); - so->addMenuItem(rt); - _pointClouds[i]->rxMap = rt; - - rt = new MenuRangeValue("ry",min,max,rValue); - rt->setCallback(this); - so->addMenuItem(rt); - _pointClouds[i]->ryMap = rt; - - rt = new MenuRangeValue("rz",min,max,rValue); - rt->setCallback(this); - so->addMenuItem(rt); - _pointClouds[i]->rzMap = rt; -/* - mc = new MenuCheckbox("Panel Visible",true); - mc->setCallback(this); - so->addMenuItem(mc); - // _query[q]->artifacts[inc]->model->pVisibleMap = mc; - // _query[q]->artifacts[inc]->model->pVisible = true; -*/ -//Quat currentRot = _pointClouds[i]->rot; -//Vec3 currentPos = _pointClouds[i]->pos; -//Vec3 orig = currentPos; -//cerr << "Pos: " << orig.x() << " " << orig.y() << " " << orig.z() << "\n"; - -// so->setPosition(currentPos); - so->setScale(1); - if(i == 0) - { - - - - - - - - - Vec3 currentPos = Vec3(0,0,0); - float rotDegrees[3]; - rotDegrees[0] = 0; - rotDegrees[1] = 0; - rotDegrees[2] = 180; - rotDegrees[0] = DegreesToRadians(rotDegrees[0]); - rotDegrees[1] = DegreesToRadians(rotDegrees[1]); - rotDegrees[2] = DegreesToRadians(rotDegrees[2]); -Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1,0,0),rotDegrees[1], osg::Vec3d(0,1,0),rotDegrees[2], osg::Vec3d(0,0,1)); - //so->setRotation(rot); -//so->setPosition(currentPos); - } + } + + _pointClouds[i]->switchNode = switchNode; + //Add menu system + so->setNavigationOn(true); + so->setMovable(false); + so->addMoveMenuItem(); + so->addNavigationMenuItem(); + float min = 0.0001; + float max = 1; + so->addScaleMenuItem("Scale", min, max, currentScale); + SubMenu* sm = new SubMenu("Position"); + so->addMenuItem(sm); + MenuButton* mb; + mb = new MenuButton("Load"); + mb->setCallback(this); + sm->addItem(mb); + SubMenu* savemenu = new SubMenu("Save"); + sm->addItem(savemenu); + mb = new MenuButton("Save"); + mb->setCallback(this); + savemenu->addItem(mb); + _pointClouds[i]->saveMap = mb; + mb = new MenuButton("Save New Kml"); + mb->setCallback(this); + savemenu->addItem(mb); + _pointClouds[i]->saveNewMap = mb; + mb = new MenuButton("Reset to Origin"); + mb->setCallback(this); + so->addMenuItem(mb); + _pointClouds[i]->resetMap = mb; + MenuCheckbox* mc; + mc = new MenuCheckbox("Active", false); + mc->setCallback(this); + so->addMenuItem(mc); + _pointClouds[i]->activeMap = mc; + mc = new MenuCheckbox("Visible", true); + mc->setCallback(this); + so->addMenuItem(mc); + _pointClouds[i]->visibleMap = mc; + _pointClouds[i]->visible = true; + float rValue = 0; + min = -1; + max = 1; + MenuRangeValue* rt = new MenuRangeValue("rx", min, max, rValue); + rt->setCallback(this); + so->addMenuItem(rt); + _pointClouds[i]->rxMap = rt; + rt = new MenuRangeValue("ry", min, max, rValue); + rt->setCallback(this); + so->addMenuItem(rt); + _pointClouds[i]->ryMap = rt; + rt = new MenuRangeValue("rz", min, max, rValue); + rt->setCallback(this); + so->addMenuItem(rt); + _pointClouds[i]->rzMap = rt; + /* + mc = new MenuCheckbox("Panel Visible",true); + mc->setCallback(this); + so->addMenuItem(mc); + // _query[q]->artifacts[inc]->model->pVisibleMap = mc; + // _query[q]->artifacts[inc]->model->pVisible = true; + */ + //Quat currentRot = _pointClouds[i]->rot; + //Vec3 currentPos = _pointClouds[i]->pos; + //Vec3 orig = currentPos; + //cerr << "Pos: " << orig.x() << " " << orig.y() << " " << orig.z() << "\n"; + // so->setPosition(currentPos); + so->setScale(1); + + if (i == 0) + { + Vec3 currentPos = Vec3(0, 0, 0); + float rotDegrees[3]; + rotDegrees[0] = 0; + rotDegrees[1] = 0; + rotDegrees[2] = 180; + rotDegrees[0] = DegreesToRadians(rotDegrees[0]); + rotDegrees[1] = DegreesToRadians(rotDegrees[1]); + rotDegrees[2] = DegreesToRadians(rotDegrees[2]); + Quat rot = osg::Quat(rotDegrees[0], osg::Vec3d(1, 0, 0), rotDegrees[1], osg::Vec3d(0, 1, 0), rotDegrees[2], osg::Vec3d(0, 0, 1)); + //so->setRotation(rot); + //so->setPosition(currentPos); + } + _pointClouds[i]->so = so; _pointClouds[i]->pos = so->getPosition(); _pointClouds[i]->rot = so->getRotation(); _pointClouds[i]->active = false; _pointClouds[i]->loaded = true; - - - } void KinectDemo::sendEvents() { cerr << "Sending Event\n"; - /* TrackingManager * tConfig = TrackingManager::instance(); - TrackerPlugin::TrackerPlugin* _trackerSystem; - //helo - cerr << "Total Tracking Systems=" << tConfig->getNumTrackingSystems() << "\n"; - for (int i=1; i < tConfig->getNumTrackingSystems(); i++) - { - //TrackerBase - //Currently testing with first found tracking system - _trackerSystem = dynamic_cast (tConfig->getTrackingSystem(i)); - if (_trackerSystem != NULL) - break; - } - if (_trackerSystem == NULL) - { - std::cerr<<"Cannot initialize tracker.\n"; - - } - else - { - int numButtons = _trackerSystem->getNumButtons(); - int numBodies = _trackerSystem->getNumBodies(); - cerr << "Buttons=" << numButtons << " Bodies=" << numBodies << endl; - - //Test updating Body - if(true) - { - TrackerBase::TrackedBody* body = _trackerSystem->getBody(0); - - float x; ///< position x - float y; ///< position y - float z; ///< position z - float qx; ///< rotation x (quat) - float qy; ///< rotation y (quat) - float qz; ///< rotation z (quat) - float qw; ///< rotation w (quat) - x = body->x; - y = body->y; - z = body->z; - cerr << "Body:" << x << "," << y << "," << z << endl; - float rotDegrees[3]; - rotDegrees[0] = 0; - rotDegrees[1] = 0; - rotDegrees[2] = 5; - //Convert Degrees to Radians - rotDegrees[0] = DegreesToRadians(rotDegrees[0]); - rotDegrees[1] = DegreesToRadians(rotDegrees[1]); - rotDegrees[2] = DegreesToRadians(rotDegrees[2]); - osg::Quat q = osg::Quat(rotDegrees[0], osg::Vec3d(1,0,0),rotDegrees[1], osg::Vec3d(0,1,0),rotDegrees[2], osg::Vec3d(0,0,1)); - //Get old Bodies transform into quat - osg::Quat qOld = osg::Quat(body->qx,body->qy,body->qz,body->qw); - //Add new update to old Quat - qOld *= q; - //Set new quat for creating new TrackedBody - q = qOld; - //Setup tracked body TODO:Use a KinectSensor for trackedBody; - TrackerBase::TrackedBody * tb = new TrackerBase::TrackedBody; - tb->x = tb->y = tb->z = 0.0; - tb->qx = q.x(); - tb->qy = q.y(); - tb->qz = q.z(); - tb->qw = q.w(); - //Set Current Tracker System Body to new tb - _trackerSystem->setBody(0,tb); - delete tb; - } - //Test Button Interaction - if(true) - { - _trackerSystem->setButton(0,true); - // _trackerSystem->setButton(0,false); - } - } -*/ + /* TrackingManager * tConfig = TrackingManager::instance(); + TrackerPlugin::TrackerPlugin* _trackerSystem; + //helo + cerr << "Total Tracking Systems=" << tConfig->getNumTrackingSystems() << "\n"; + for (int i=1; i < tConfig->getNumTrackingSystems(); i++) + { + //TrackerBase + //Currently testing with first found tracking system + _trackerSystem = dynamic_cast (tConfig->getTrackingSystem(i)); + if (_trackerSystem != NULL) + break; + } + if (_trackerSystem == NULL) + { + std::cerr<<"Cannot initialize tracker.\n"; + + } + else + { + int numButtons = _trackerSystem->getNumButtons(); + int numBodies = _trackerSystem->getNumBodies(); + cerr << "Buttons=" << numButtons << " Bodies=" << numBodies << endl; + + //Test updating Body + if(true) + { + TrackerBase::TrackedBody* body = _trackerSystem->getBody(0); + + float x; ///< position x + float y; ///< position y + float z; ///< position z + float qx; ///< rotation x (quat) + float qy; ///< rotation y (quat) + float qz; ///< rotation z (quat) + float qw; ///< rotation w (quat) + x = body->x; + y = body->y; + z = body->z; + cerr << "Body:" << x << "," << y << "," << z << endl; + float rotDegrees[3]; + rotDegrees[0] = 0; + rotDegrees[1] = 0; + rotDegrees[2] = 5; + //Convert Degrees to Radians + rotDegrees[0] = DegreesToRadians(rotDegrees[0]); + rotDegrees[1] = DegreesToRadians(rotDegrees[1]); + rotDegrees[2] = DegreesToRadians(rotDegrees[2]); + osg::Quat q = osg::Quat(rotDegrees[0], osg::Vec3d(1,0,0),rotDegrees[1], osg::Vec3d(0,1,0),rotDegrees[2], osg::Vec3d(0,0,1)); + //Get old Bodies transform into quat + osg::Quat qOld = osg::Quat(body->qx,body->qy,body->qz,body->qw); + //Add new update to old Quat + qOld *= q; + //Set new quat for creating new TrackedBody + q = qOld; + //Setup tracked body TODO:Use a KinectSensor for trackedBody; + TrackerBase::TrackedBody * tb = new TrackerBase::TrackedBody; + tb->x = tb->y = tb->z = 0.0; + tb->qx = q.x(); + tb->qy = q.y(); + tb->qz = q.z(); + tb->qw = q.w(); + //Set Current Tracker System Body to new tb + _trackerSystem->setBody(0,tb); + delete tb; + } + //Test Button Interaction + if(true) + { + _trackerSystem->setButton(0,true); + // _trackerSystem->setButton(0,false); + } + } + */ cerr << "Finised\n"; } @@ -1901,8 +1833,8 @@ bool KinectDemo::processEvent(InteractionEvent* event) { //For Testing Simulated Hand Interaction cerr << "Hand 4 Button " << tie->getButton() << "\n"; - } + if ((event->getInteraction() == BUTTON_DOWN) && tie->getHand() == 0) { cerr << "Hand 1 Button 0\n"; diff --git a/KinectDemo.h b/KinectDemo.h index 4d587e3..e1232be 100755 --- a/KinectDemo.h +++ b/KinectDemo.h @@ -102,22 +102,22 @@ class KinectDemo : public cvr::MenuCallback, public cvr::CVRPlugin osg::Group* kinectgrp; float initialPointScale; -struct PointCloud -{ - string name; - string filename; + struct PointCloud + { + string name; + string filename; string fullpath; string filetype; string modelType; string group; - float scale; + float scale; osg::Vec3 pos; osg::Quat rot; osg::Vec3 origPos; osg::Quat origRot; float origScale; - cvr::SceneObject * so; - bool loaded; + cvr::SceneObject* so; + bool loaded; bool active; bool visible; bool selected; @@ -125,8 +125,8 @@ struct PointCloud bool lockRot; bool lockScale; bool lockGraph; - int lockedTo; - int lockedType; + int lockedTo; + int lockedType; cvr::MenuButton* saveMap; cvr::MenuButton* saveNewMap; cvr::MenuButton* resetMap; @@ -135,13 +135,13 @@ struct PointCloud cvr::MenuRangeValue* rxMap; cvr::MenuRangeValue* ryMap; cvr::MenuRangeValue* rzMap; - //Store Different Model Type Transforms + //Store Different Model Type Transforms osg::Node* currentModelNode; - osg::Switch* switchNode; + osg::Switch* switchNode; -}; - + }; + std::vector _pointClouds; std::vector selectableItems; void createSceneObject(); @@ -204,8 +204,8 @@ struct PointCloud osg::ref_ptr kinectColours; osg::ref_ptr kinectVertices; - CloudManager * _cloudThread; - // SkeletonManager * _skeletonThread; + CloudManager* _cloudThread; + // SkeletonManager * _skeletonThread; protected: static KinectDemo* _kinectDemo; diff --git a/SkeletonManager.cpp b/SkeletonManager.cpp index 2dc831a..14d3cd1 100644 --- a/SkeletonManager.cpp +++ b/SkeletonManager.cpp @@ -17,13 +17,11 @@ SkeletonManager::SkeletonManager() { should_quit = false; sf2 = new RemoteKinect::SkeletonFrame(); - kNavSpheres = false; kMoveWithCam = false; _root = new osg::MatrixTransform(); SceneManager::instance()->getObjectsRoot()->addChild(_root); - // _root->addChild(kinectgrp); - + // _root->addChild(kinectgrp); } SkeletonManager::~SkeletonManager() @@ -38,176 +36,158 @@ bool SkeletonManager::isCacheDone() void SkeletonManager::update() { static bool frameLoading = false; - bool cDone; -if(true) -{ - if(cvr::ComController::instance()->isMaster()) + bool cDone; + + if (true) + { + if (cvr::ComController::instance()->isMaster()) { - cDone = _cacheDone; - int numSlaves = cvr::ComController::instance()->getNumSlaves(); - bool sDone[numSlaves]; - cvr::ComController::instance()->readSlaves(sDone,sizeof(bool)); - for(int i = 0; i < numSlaves; i++) - { - cDone = cDone && sDone[i]; - } - cvr::ComController::instance()->sendSlaves(&cDone,sizeof(bool)); + cDone = _cacheDone; + int numSlaves = cvr::ComController::instance()->getNumSlaves(); + bool sDone[numSlaves]; + cvr::ComController::instance()->readSlaves(sDone, sizeof(bool)); + + for (int i = 0; i < numSlaves; i++) + { + cDone = cDone && sDone[i]; + } + + cvr::ComController::instance()->sendSlaves(&cDone, sizeof(bool)); } else { - cDone = _cacheDone; - cvr::ComController::instance()->sendMaster(&cDone,sizeof(bool)); - cvr::ComController::instance()->readMaster(&cDone,sizeof(bool)); + cDone = _cacheDone; + cvr::ComController::instance()->sendMaster(&cDone, sizeof(bool)); + cvr::ComController::instance()->readMaster(&cDone, sizeof(bool)); } - if(!cDone) + if (!cDone) { - //std::cerr << "Waiting for load to finish." << std::endl; - return; + //std::cerr << "Waiting for load to finish." << std::endl; + return; } cvr::ComController::instance()->sync(); -//Add not here? - + //Add not here? + return; + } - return; -} - if(cDone) + if (cDone) { - std::cerr << "Load Finished." << std::endl; - //Add loaded node to root - + std::cerr << "Load Finished." << std::endl; + //Add loaded node to root } else { -// std::cerr << "Waiting for GPU load finish." << std::endl; + // std::cerr << "Waiting for GPU load finish." << std::endl; } - } void SkeletonManager::run() { -//Do functions -//cerr << "."; -if(!should_quit) -{ - - zmq::context_t context3(1); - -skelT_socket = NULL; -skelT_socket = new SubSocket (context3, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.Skeleton")); - while(!should_quit) - { - - if (Skeleton::moveWithCam) + //Do functions + //cerr << "."; + if (!should_quit) { + zmq::context_t context3(1); + skelT_socket = NULL; + skelT_socket = new SubSocket (context3, ConfigManager::getEntry("Plugin.KinectDemo.KinectServer.Skeleton")); - Matrix camMat = PluginHelper::getWorldToObjectTransform(); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) - float cscale = 1; //Want to keep scale to actual Kinect which is is meters - Vec3 camTrans = camMat.getTrans(); - Quat camQuad = camMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! - double xOffset = (camTrans.x() / cscale); - double yOffset = (camTrans.y() / cscale) + 3; //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) - double zOffset = (camTrans.z() / cscale); - Skeleton::camPos = Vec3d(xOffset, yOffset, zOffset); - Skeleton::camRot = camQuad; - } - - while (skelT_socket->recv(*sf2)) - { -// cerr << "."; - - //return; - // remove all the skeletons that are no longer reported by the server - for (std::map::iterator it2 = mapIdSkel.begin(); it2 != mapIdSkel.end(); ++it2) + while (!should_quit) { - - bool found = false; - - for (int i = 0; i < sf2->skeletons_size(); i++) + if (Skeleton::moveWithCam) { - if (sf2->skeletons(i).skeleton_id() == it2->first) - { - found = true; - break; - } + Matrix camMat = PluginHelper::getWorldToObjectTransform(); //This will get us actual real world coordinates that the camera is at (not sure about how it does rotation) + float cscale = 1; //Want to keep scale to actual Kinect which is is meters + Vec3 camTrans = camMat.getTrans(); + Quat camQuad = camMat.getRotate(); //Rotation of cam will cause skeleton to be off center--need Fix!! + double xOffset = (camTrans.x() / cscale); + double yOffset = (camTrans.y() / cscale) + 3; //Added Offset of Skeleton so see a little ways from camera (i.e. 5 meters, works at this scale,only) + double zOffset = (camTrans.z() / cscale); + Skeleton::camPos = Vec3d(xOffset, yOffset, zOffset); + Skeleton::camRot = camQuad; } - if (!found) + while (skelT_socket->recv(*sf2)) { - mapIdSkel[it2->first].detach(_root); - } - } -//cerr << sf->skeletons_size(); + // cerr << "."; - // update all skeletons' joints' positions - for (int i = 0; i < sf2->skeletons_size(); i++) - { - // Skeleton reported but not in the map -> create a new one - if (mapIdSkel.count(sf2->skeletons(i).skeleton_id()) == 0) - { - mapIdSkel[sf2->skeletons(i).skeleton_id()] = Skeleton(); ///XXX remove Skeleton(); part - mapIdSkel[sf2->skeletons(i).skeleton_id()].attach(_root); - } + //return; + // remove all the skeletons that are no longer reported by the server + for (std::map::iterator it2 = mapIdSkel.begin(); it2 != mapIdSkel.end(); ++it2) + { + bool found = false; + + for (int i = 0; i < sf2->skeletons_size(); i++) + { + if (sf2->skeletons(i).skeleton_id() == it2->first) + { + found = true; + break; + } + } + + if (!found) + { + mapIdSkel[it2->first].detach(_root); + } + } - // Skeleton previously detached (stopped being reported), but is again reported -> reattach - if (mapIdSkel[sf2->skeletons(i).skeleton_id()].attached == false) - mapIdSkel[sf2->skeletons(i).skeleton_id()].attach(_root); + //cerr << sf->skeletons_size(); - for (int j = 0; j < sf2->skeletons(i).joints_size(); j++) - { -float test; - test = sf2->skeletons(i).joints(j).type(); - test = sf2->skeletons(i).joints(j).x() / 1000; - test = sf2->skeletons(i).joints(j).z() / -1000; - test = sf2->skeletons(i).joints(j).y() / 1000; - test = sf2->skeletons(i).joints(j).qx(); - test = sf2->skeletons(i).joints(j).qz(); - test = sf2->skeletons(i).joints(j).qy(); - test = sf2->skeletons(i).joints(j).qw(); -/* - mapIdSkel[sf2->skeletons(i).skeleton_id()].update( - sf2->skeletons(i).joints(j).type(), - sf2->skeletons(i).joints(j).x() / 1000, - sf2->skeletons(i).joints(j).z() / -1000, - sf2->skeletons(i).joints(j).y() / 1000, - sf2->skeletons(i).joints(j).qx(), - sf2->skeletons(i).joints(j).qz(), - sf2->skeletons(i).joints(j).qy(), - sf2->skeletons(i).joints(j).qw()); -*/ + // update all skeletons' joints' positions + for (int i = 0; i < sf2->skeletons_size(); i++) + { + // Skeleton reported but not in the map -> create a new one + if (mapIdSkel.count(sf2->skeletons(i).skeleton_id()) == 0) + { + mapIdSkel[sf2->skeletons(i).skeleton_id()] = Skeleton(); ///XXX remove Skeleton(); part + mapIdSkel[sf2->skeletons(i).skeleton_id()].attach(_root); + } + + // Skeleton previously detached (stopped being reported), but is again reported -> reattach + if (mapIdSkel[sf2->skeletons(i).skeleton_id()].attached == false) + mapIdSkel[sf2->skeletons(i).skeleton_id()].attach(_root); + + for (int j = 0; j < sf2->skeletons(i).joints_size(); j++) + { + float test; + test = sf2->skeletons(i).joints(j).type(); + test = sf2->skeletons(i).joints(j).x() / 1000; + test = sf2->skeletons(i).joints(j).z() / -1000; + test = sf2->skeletons(i).joints(j).y() / 1000; + test = sf2->skeletons(i).joints(j).qx(); + test = sf2->skeletons(i).joints(j).qz(); + test = sf2->skeletons(i).joints(j).qy(); + test = sf2->skeletons(i).joints(j).qw(); + /* + mapIdSkel[sf2->skeletons(i).skeleton_id()].update( + sf2->skeletons(i).joints(j).type(), + sf2->skeletons(i).joints(j).x() / 1000, + sf2->skeletons(i).joints(j).z() / -1000, + sf2->skeletons(i).joints(j).y() / 1000, + sf2->skeletons(i).joints(j).qx(), + sf2->skeletons(i).joints(j).qz(), + sf2->skeletons(i).joints(j).qy(), + sf2->skeletons(i).joints(j).qw()); + */ + } + } } - } + if (should_quit) + { + } } - - - - - } -if(should_quit) -{ - -} - - -} -//When Finished - _cacheDone = true; - - - - - std::cerr << "All frames loaded." << std::endl; - + //When Finished + _cacheDone = true; + std::cerr << "All frames loaded." << std::endl; } void SkeletonManager::quit() { - - should_quit = true; - + should_quit = true; } diff --git a/SkeletonManager.h b/SkeletonManager.h index 2aec319..707b01b 100644 --- a/SkeletonManager.h +++ b/SkeletonManager.h @@ -29,28 +29,28 @@ #include -// zmq::context_t contextCloud(1); +// zmq::context_t contextCloud(1); class SkeletonManager : public OpenThreads::Thread { - public: +public: - SkeletonManager(); - ~SkeletonManager(); - //contextCloud(1); -SubSocket* skelT_socket; - void update(); - bool isCacheDone(); + SkeletonManager(); + ~SkeletonManager(); + //contextCloud(1); + SubSocket* skelT_socket; + void update(); + bool isCacheDone(); - RemoteKinect::SkeletonFrame* sf2; - virtual void run(); - void quit(); - protected: + RemoteKinect::SkeletonFrame* sf2; + virtual void run(); + void quit(); +protected: std::map mapIdSkel; bool kNavSpheres; bool kMoveWithCam; - bool _cacheDone; + bool _cacheDone; bool should_quit; osg::MatrixTransform* _root; }; diff --git a/TrackerPlugin.cpp b/TrackerPlugin.cpp index 034f701..e2a9126 100644 --- a/TrackerPlugin.cpp +++ b/TrackerPlugin.cpp @@ -18,8 +18,6 @@ TrackerPlugin::TrackerPlugin() _numBodies = 0; _numButtons = 0; _numVal = 0; - - _buttonMask = 0; } @@ -29,56 +27,52 @@ TrackerPlugin::~TrackerPlugin() bool TrackerPlugin::init(std::string tag) { - _numBodies = 1; - for(int i = 0; i < _numBodies; i++) - { - TrackedBody * tb = new TrackedBody; - tb->x = tb->y = tb->z = 0.0; - osg::Quat q; - tb->qx = q.x(); - tb->qy = q.y(); - tb->qz = q.z(); - tb->qw = q.w(); - _bodyList.push_back(tb); - _bodyListMem.push_back(tb); - } - - - _numButtons = 2; - - _numVal = 0; - for(int i = 0; i < _numVal; i++) - { - _valList.push_back(0.0); - _valListMem.push_back(0.0); - } - + _numBodies = 1; + + for (int i = 0; i < _numBodies; i++) + { + TrackedBody* tb = new TrackedBody; + tb->x = tb->y = tb->z = 0.0; + osg::Quat q; + tb->qx = q.x(); + tb->qy = q.y(); + tb->qz = q.z(); + tb->qw = q.w(); + _bodyList.push_back(tb); + _bodyListMem.push_back(tb); + } + + _numButtons = 2; + _numVal = 0; + + for (int i = 0; i < _numVal; i++) + { + _valList.push_back(0.0); + _valListMem.push_back(0.0); + } _buttonMask = 0; _buttonMaskMem = 0; - return true; } -TrackerBase::TrackedBody * TrackerPlugin::getBody(int index) +TrackerBase::TrackedBody* TrackerPlugin::getBody(int index) { - if(index < 0 || index >= _numBodies) + if (index < 0 || index >= _numBodies) { return NULL; } return _bodyList[index]; } -void TrackerPlugin::setBody(int index, TrackedBody * tb) +void TrackerPlugin::setBody(int index, TrackedBody* tb) { - if(index < 0 || index >= _numBodies) + if (index < 0 || index >= _numBodies) { - } else { - - _bodyListMem[index] = tb; + _bodyListMem[index] = tb; } } @@ -89,12 +83,12 @@ unsigned int TrackerPlugin::getButtonMask() void TrackerPlugin::setButtonMask(int buttonMask) { - _buttonMaskMem = buttonMask; + _buttonMaskMem = buttonMask; } float TrackerPlugin::getValuator(int index) { - if(index < 0 || index >= _numVal) + if (index < 0 || index >= _numVal) { return 0.0; } @@ -102,10 +96,9 @@ float TrackerPlugin::getValuator(int index) return _valList[index]; } -void TrackerPlugin::setValuator(int index,float val) +void TrackerPlugin::setValuator(int index, float val) { - _valListMem[index] = val; - + _valListMem[index] = val; } int TrackerPlugin::getNumBodies() @@ -124,37 +117,34 @@ int TrackerPlugin::getNumButtons() } void TrackerPlugin::update( - std::map > & eventMap) + std::map >& eventMap) { - if(_numButtons) + if (_numButtons) { _buttonMask = 0; - for(int i = 0; i < _numButtons; i++) + + for (int i = 0; i < _numButtons; i++) { _buttonMask = _buttonMask | _buttonMaskMem; } } - if(_numVal) + if (_numVal) { - for(int i = 0; i < _numVal; i++) + for (int i = 0; i < _numVal; i++) { _valList[i] = _valListMem[i]; } } - - for(int i = 0; i < _numBodies; i++) + for (int i = 0; i < _numBodies; i++) { _bodyList[i]->x = _bodyListMem[i]->x; _bodyList[i]->y = _bodyListMem[i]->y; _bodyList[i]->z = _bodyListMem[i]->z; - - _bodyList[i]->qx = _bodyListMem[i]->qx; _bodyList[i]->qy = _bodyListMem[i]->qy; _bodyList[i]->qz = _bodyListMem[i]->qz; _bodyList[i]->qw = _bodyListMem[i]->qw; - } } diff --git a/TrackerPlugin.h b/TrackerPlugin.h index ef6133e..838f222 100644 --- a/TrackerPlugin.h +++ b/TrackerPlugin.h @@ -25,47 +25,47 @@ namespace cvr */ class TrackerPlugin : public TrackerBase { - public: - TrackerPlugin(); - virtual ~TrackerPlugin(); - - virtual bool init(std::string tag); - - virtual TrackedBody * getBody(int index); - virtual void setBody(int index, TrackedBody * tb); - virtual unsigned int getButtonMask(); - virtual void setButtonMask(int buttonMask); - virtual float getValuator(int index); - virtual void setValuator(int index,float val); - - virtual int getNumBodies(); - virtual int getNumValuators(); - virtual int getNumButtons(); - - virtual void update( - std::map > & eventMap); - protected: - - - struct sensor - { - float p[3]; - float r[3]; - uint32_t t[2]; - uint32_t calib; - uint32_t frame; - }; - - int _numBodies; ///< number of bodies in block - int _numVal; ///< number of valuators in block - int _numButtons; ///< number of buttons in block - - std::vector _bodyList; ///< list of all tracked body info - std::vector _bodyListMem; ///< list of all tracked body info - unsigned int _buttonMask; ///< current button mask - unsigned int _buttonMaskMem; ///< current button mask - std::vector _valList; ///< list of all valuator values - std::vector _valListMem; ///< list of all valuator values +public: + TrackerPlugin(); + virtual ~TrackerPlugin(); + + virtual bool init(std::string tag); + + virtual TrackedBody* getBody(int index); + virtual void setBody(int index, TrackedBody* tb); + virtual unsigned int getButtonMask(); + virtual void setButtonMask(int buttonMask); + virtual float getValuator(int index); + virtual void setValuator(int index, float val); + + virtual int getNumBodies(); + virtual int getNumValuators(); + virtual int getNumButtons(); + + virtual void update( + std::map >& eventMap); +protected: + + + struct sensor + { + float p[3]; + float r[3]; + uint32_t t[2]; + uint32_t calib; + uint32_t frame; + }; + + int _numBodies; ///< number of bodies in block + int _numVal; ///< number of valuators in block + int _numButtons; ///< number of buttons in block + + std::vector _bodyList; ///< list of all tracked body info + std::vector _bodyListMem; ///< list of all tracked body info + unsigned int _buttonMask; ///< current button mask + unsigned int _buttonMaskMem; ///< current button mask + std::vector _valList; ///< list of all valuator values + std::vector _valListMem; ///< list of all valuator values }; /**