void Reactable2OPC::addTuioObject(TuioObject *tobj) { std::cout << "add obj " << tobj->getSymbolID() << " (" << tobj->getSessionID() << ") "<< tobj->getX() << " " << tobj->getY() << " " << tobj->getAngle() << std::endl; if (idMap.find(tobj->getSymbolID()) == idMap.end() ) { cout<<"Unknown object with ID "<<tobj->getSymbolID()<<" added"<<endl; return; } if (!w->isConnected()) { cout<<"Not connected to OPC"<<endl; return; } RTObject* o = w->addOrRetrieveEntity<RTObject>(idMap[tobj->getSymbolID()]); //w->update(o); Vector rtPosition(4); rtPosition(0) = XaxisFactor * tobj->getX(); rtPosition(1) = YaxisFactor * tobj->getY(); rtPosition(2) = 0; rtPosition(3) = 1; o->m_rt_position[0] = rtPosition[0]; o->m_rt_position[1] = rtPosition[1]; o->m_rt_position[2] = rtPosition[2]; cout<<o->toString()<<endl; if ( checkCalibration() ) { Vector icubPos(4); icubPos = H2ICUB*rtPosition; o->m_ego_position[0] = icubPos[0] + idOffsets[tobj->getSymbolID()][0]; o->m_ego_position[1] = icubPos[1] + idOffsets[tobj->getSymbolID()][1]; o->m_ego_position[2] = icubPos[2] + idOffsets[tobj->getSymbolID()][2]; } o->m_present = 1.0; o->m_ego_orientation[0] = 0.0; o->m_ego_orientation[1] = 0.0; o->m_ego_orientation[2] = tobj->getAngle(); w->isVerbose = true; w->commit(o); w->isVerbose = false; //std::cout <<o->toString() << std::endl; }
bool PasarModule::updateModule() { opc->update(); list<Entity*> entities = opc->EntitiesCache(); presentObjects.clear(); presentLastSpeed = presentCurrentSpeed; presentCurrentSpeed.clear(); for(list<Entity*>::iterator it=entities.begin(); it !=entities.end(); it++) { if ((*it)->name() != "icub") { //!!! ONLY RT_OBJECT and AGENTS ARE TRACKED !!! if ( ( (*it)->isType(EFAA_OPC_ENTITY_OBJECT) )) { if ((*it)->isType(EFAA_OPC_ENTITY_RTOBJECT)) { RTObject * rto = opc->addRTObject((*it)->name()); presentObjects[ (*it)->name() ].o.fromBottle( rto->asBottle() ); presentObjects[ (*it)->name() ].o.m_saliency = rto->m_saliency; presentObjects[ (*it)->name() ].speed = 0.0; presentObjects[ (*it)->name() ].acceleration = 0.0; presentObjects[ (*it)->name() ].restingSteps = 0; } if ((*it)->isType(EFAA_OPC_ENTITY_AGENT)) { Agent *ag = opc->addAgent((*it)->name()); presentObjects[ (*it)->name() ].o.fromBottle( ag->asBottle() ); presentObjects[ (*it)->name() ].o.m_saliency = ag->m_saliency; presentObjects[ (*it)->name() ].speed = 0.0; presentObjects[ (*it)->name() ].acceleration = 0.0; presentObjects[ (*it)->name() ].restingSteps = 0; }/* presentObjects[ (*it)->name() ].o.fromBottle( (*it)->asBottle() ); presentObjects[ (*it)->name() ].o.m_saliency = 0.0; presentObjects[ (*it)->name() ].speed = 0.0; presentObjects[ (*it)->name() ].acceleration = 0.0; presentObjects[ (*it)->name() ].restingSteps = 0;*/ } } //if (presentObjects[ (*it)->name() ].o.m_saliency > 0) // cout<<" salience : " << (*it)->name() << " " << presentObjects[ (*it)->name() ].o.m_saliency << endl; } if (presentObjectsLastStep.size() > 0) { //Retrieve the bottom-up saliency (vision based) and attribute it to objects //saliencyBottomUp(); //Compute top down saliency (concept based) saliencyTopDown(); //Normalize //saliencyNormalize(); //Inhinbition of return // if(trackedObject!= "") // presentObjects[trackedObject].o.m_saliency = max(0.0, presentObjects[trackedObject].o.m_saliency - pTopDownInhibitionReturn); //Leaky integrate saliencyLeakyIntegration(); //Get the most salient object and track it map< string, ObjectModel >::iterator mostSalientObject = presentObjects.begin(); for(map< string, ObjectModel >::iterator it=presentObjects.begin(); it!=presentObjects.end();it++) { // cout<<"Saliency ("<<it->second.o.name()<<") = "<<it->second.o.m_saliency<<endl; if (it->second.o.m_saliency > mostSalientObject->second.o.m_saliency) mostSalientObject = it; } trackedObject = mostSalientObject->first; if (presentObjects[trackedObject].o.m_saliency >0.0) { cout << "Tracking : " << trackedObject << " Salience : " << presentObjects[trackedObject].o.m_saliency << endl; } if (isControllingMotors && isFixationPointSafe(presentObjects[trackedObject].o.m_ego_position)) igaze->lookAtFixationPoint(presentObjects[trackedObject].o.m_ego_position); //Prepare the output img ImageOf<PixelRgb> &img=saliencyOutput.prepare(); img.copy(imageOut); saliencyOutput.write(); //Update the OPC values for(list<Entity*>::iterator it=entities.begin(); it !=entities.end(); it++) { if (presentObjects.find( (*it)->name()) != presentObjects.end()) { ((Object*)(*it))->m_saliency = presentObjects[(*it)->name()].o.m_saliency; } } opc->commit(); } presentObjectsLastStep = presentObjects; return true; }
bool PasarModule::updateModule() { opc->update(); entities = opc->EntitiesCache(); presentObjects.clear(); presentLastSpeed = presentCurrentSpeed; presentCurrentSpeed.clear(); for (list<Entity*>::iterator it = entities.begin(); it != entities.end(); it++) { if ((*it)->name() != "icub") { //!!! ONLY OBJECTS, RT_OBJECT and AGENTS ARE TRACKED !!! if ((*it)->isType(EFAA_OPC_ENTITY_RTOBJECT) || (*it)->isType(EFAA_OPC_ENTITY_AGENT) || (*it)->isType(EFAA_OPC_ENTITY_OBJECT)) { if ((*it)->isType(EFAA_OPC_ENTITY_RTOBJECT)) { RTObject * rto = dynamic_cast<RTObject*>(*it); presentObjects[(*it)->name()].o.fromBottle(rto->asBottle()); presentObjects[(*it)->name()].o.m_saliency = rto->m_saliency; } if ((*it)->isType(EFAA_OPC_ENTITY_OBJECT)) { Object * ob = dynamic_cast<Object*>(*it); presentObjects[(*it)->name()].o.fromBottle(ob->asBottle()); presentObjects[(*it)->name()].o.m_saliency = ob->m_saliency; } if ((*it)->isType(EFAA_OPC_ENTITY_AGENT)) { Agent *ag = dynamic_cast<Agent*>(*it); presentObjects[(*it)->name()].o.fromBottle(ag->asBottle()); presentObjects[(*it)->name()].o.m_saliency = ag->m_saliency; } } } } //if (presentObjects[ (*it)->name() ].o.m_saliency > 0) // cout<<" salience : " << (*it)->name() << " " << presentObjects[ (*it)->name() ].o.m_saliency << endl; if (presentObjectsLastStep.size() > 0) { //Compute top down saliency (concept based) saliencyTopDown(); if (isPointing) saliencyPointing(); if (isWaving) saliencyWaving(); //Normalize //saliencyNormalize(); //Inhinbition of return // if(trackedObject!= "") // presentObjects[trackedObject].o.m_saliency = max(0.0, presentObjects[trackedObject].o.m_saliency - pTopDownInhibitionReturn); //Leaky integrate saliencyLeakyIntegration(); //Get the most salient object and track it map< string, ObjectModel >::iterator mostSalientObject = presentObjects.begin(); for (map< string, ObjectModel >::iterator it = presentObjects.begin(); it != presentObjects.end(); it++) { // cout<<"Saliency ("<<it->second.o.name()<<") = "<<it->second.o.m_saliency<<endl; if (it->second.o.m_saliency > mostSalientObject->second.o.m_saliency) mostSalientObject = it; } trackedObject = mostSalientObject->first; if (presentObjects[trackedObject].o.m_saliency > 0.0) { cout << "Tracking : " << trackedObject << " Salience : " << presentObjects[trackedObject].o.m_saliency << endl; } //Update the OPC values for (list<Entity*>::iterator it = entities.begin(); it != entities.end(); it++) { if (presentObjects.find((*it)->name()) != presentObjects.end()) { (dynamic_cast<Object*>(*it))->m_saliency = presentObjects[(*it)->name()].o.m_saliency; } } opc->commit(); } presentObjectsLastStep = presentObjects; return true; }
/* Synchonise the content of the OPC in the mentalOPC */ Bottle opcManager::synchoniseOPCs() { cout << endl << "Begining of the synchronisation of the OPCs" << endl; Bottle bOutput; if (!(realOPC->isConnected() && mentalOPC->isConnected())) { cout << "Error in opcManager::synchroniseOPCs : OPC not connected" << endl; bOutput.addString("Error in opcManager::synchroniseOPCs : OPC not connected"); return bOutput; } realOPC->checkout(); mentalOPC->checkout(); list<Entity*> lEntities = realOPC->EntitiesCache(); list<Relation> lRelations = realOPC->getRelations(); //clean GUI : list<Entity*> lMental = mentalOPC->EntitiesCache(); for (list<Entity*>::iterator it_E = lMental.begin(); it_E != lMental.end(); it_E++) { if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_OBJECT) { Object *Ob = dynamic_cast<Object*>(*it_E); Ob->m_present = 0; } if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_AGENT) { Agent *Ag = dynamic_cast<Agent*>(*it_E); Ag->m_present = 0; } if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_RTOBJECT) { RTObject *Rt = dynamic_cast<RTObject*>(*it_E); Rt->m_present = 0; } } mentalOPC->commit(); Time::delay(time_action); mentalOPC->clear(); mentalOPC->checkout(); for (list<Entity*>::iterator it_E = lEntities.begin(); it_E != lEntities.end(); it_E++) { if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_OBJECT) { Object *Ob = dynamic_cast<Object*>(*it_E); Ob->fromBottle((*it_E)->asBottle()); } if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_AGENT) { Agent *Ag = dynamic_cast<Agent*>(*it_E); Ag->fromBottle((*it_E)->asBottle()); } if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_RTOBJECT) { RTObject *Rt = dynamic_cast<RTObject*>(*it_E); Rt->fromBottle((*it_E)->asBottle()); } if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_ADJECTIVE) { Adjective *Ad = dynamic_cast<Adjective*>(*it_E); Ad->fromBottle((*it_E)->asBottle()); } if ((*it_E)->entity_type() == EFAA_OPC_ENTITY_ACTION) { Action *Ac = dynamic_cast<Action*>(*it_E); Ac->fromBottle((*it_E)->asBottle()); } } mentalOPC->commit(); for (list<Relation>::iterator it_R = lRelations.begin(); it_R != lRelations.end(); it_R++) { mentalOPC->addRelation(*it_R); } mentalOPC->update(); cout << "Synchronisation done" << endl; bOutput.addString("synchronisation done."); return bOutput; }
void initScene() { /************************************* ****** CREATE OBJECTS HERE!!!!! ***** *************************************/ // bear skin RTObject *oHead, *oLeftEar, *oRightEar, *oBeard;; // bear features RTObject *oLeftEarFace, *oRightEarFace, *oLeftEye, *oRightEye, *oLeftPupil, *oRightPupil, *oNose, *tMouth; // environment triangles RTObject *groundLeft, *groundCenter, *groundRight; // am i doing it right triangle RTObject *tri1, *tri2, *tri3; // refractive crystal ball RTObject *oCrystalBall; // create a new rto oHead = new RTSphere(-0.2f, 0.0f, 0.0f, 0.3f, "Bear Head"); // RTSphere(x, y, z, radius) oLeftEar = new RTSphere(-0.42f, 0.22f, 0.0f, 0.1f, "Bear Left Ear"); oRightEar = new RTSphere(0.02f, 0.22f, 0.0f, 0.1f, " Bear Right Ear"); oBeard = new RTSphere(-0.2f, -0.05f, 0.2f, 0.15f, "Bear Beard"); oLeftEarFace = new RTSphere(-0.42f, 0.22f, 0.1f, 0.05f, "Bear Left Ear patch"); oRightEarFace = new RTSphere(0.02f, 0.22f, 0.1f, 0.05f, "bear right ear patch"); oLeftEye = new RTSphere(-0.23f, 0.14f, 0.27f, 0.03f, "bear left eye white"); oRightEye = new RTSphere(-0.17f, 0.14f, 0.27f, 0.03f, "bear right eye white"); oLeftPupil = new RTSphere(-0.215f, 0.143f, 0.30f, 0.02, "bear left eye pupil"); oRightPupil = new RTSphere(-0.155f, 0.143f, 0.30f, 0.02, "bear right eye pupil"); oNose = new RTSphere(-0.2f, -0.02f, 0.4f, 0.1f, "bear nose"); // bear-chan's mouth point p0 = {-0.218f, -0.13f, 0.5f, 1.0f}; point p1 = {-0.22f, -0.14f, 0.5f, 1.0f}; point p2 = {-0.131f, -0.127f, 0.5f, 1.0f}; tMouth = new RTTriangle(p0, p1, p2, "bear mouth"); // create ground left set(p0, -1.0f, 0.0f, 0.0f, 1.0f); set(p1, -1.0f, -1.0f, 0.0f, 1.0f); set(p2, 0.0f, 0.2f, 0.0f, 1.0f); groundLeft = new RTTriangle(p0, p1, p2, "left ground triangle"); // create ground center set(p0, 0.0f, 0.2f, 0.0f, 1.0f); set(p1, -1.0f, -1.0f, 0.0f, 1.0f); set(p2, 1.0f, -1.0f, 0.0f, 1.0f); groundCenter = new RTTriangle(p0, p1, p2, "middle ground triangle"); // create ground right set(p0, 0.0f, 0.2f, 0.0f, 1.0f); set(p1, 1.0f, -1.0f, 0.0f, 1.0f); set(p2, 1.0f, -0.5f, 0.0f, 1.0f); groundRight = new RTTriangle(p0, p1, p2, "right ground triangle"); // create "am i doing it right?" triangle set(p0, 0.40f, 0.20f, -1.0f, 1.0f); set(p1, 0.60f, 0.20f, -1.0f, 1.0f); set(p2, 0.50f, 0.40f, -1.0f, 1.0f); tri1 = new RTTriangle(p0, p1, p2, "bot left triangle"); set(p0, 0.60f, 0.20f, -1.0f, 1.0f); set(p1, 0.80f, 0.20f, -1.0f, 1.0f); set(p2, 0.70f, 0.40f, -1.0f, 1.0f); tri2 = new RTTriangle(p0, p1, p2, "bot right triangle"); set(p0, 0.40f, 0.40f, -1.0f, 1.0f); set(p1, 0.60f, 0.40f, -1.0f, 1.0f); set(p2, 0.50f, 0.60f, -1.0f, 1.0f); tri3 = new RTTriangle(p0, p1, p2, "top left triangle"); // refractive crystal ball oCrystalBall = new RTSphere(0.3f, 0.2f, -0.5f, 0.1f, "CrystalBall"); /************************************* ****** SET MATERIAL PROPERTIES ***** *************************************/ // material properties Material mBearSkin, mBearFace, mBearBeard, mBearPupil, mBearNose, mBearMouth, mGround; Material mTriangles; Material mCrystalBall; // set new rto material properties mBearSkin.set(0.99f, 0.5f, 0.05f, 0.0f, 0.7f, 1.0f, 0.4f, 0.0f); // set(r, g, b, ambientRef, diffusePer, specPer, reflPer, refrInd) mBearFace.set(1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f); mBearBeard.set(0.99f, 0.5f, 0.05f, 0.3f, 0.7f, 1.0f, 0.4f, 0.0f); // set(r, g, b, ambientRef, diffusePer, specPer, reflPer, refrInd) mBearPupil.set(0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 0.0f); mBearNose.set(0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f); mBearMouth.set(1.0f, 0.1, 0.1f, 0.44f, 0.6f, 0.0f, 0.0f, 0.0f); // other materials mGround.set(0.1f, 0.6f, 0.15f, 0.3f, 0.6f, 0.0f, 0.0f, 0.0f); // triangles mTriangles.set(1.0f, 1.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f); // crystal ball mCrystalBall.set(0.2f, 0.2f, 0.2f, 0.2f, 0.2f, 0.0f, 0.0f, 1.33f); // refractive index of water ~1.33 oHead->setMaterial(mBearSkin); oLeftEar->setMaterial(mBearSkin); oRightEar->setMaterial(mBearSkin); oBeard->setMaterial(mBearBeard); oLeftEarFace->setMaterial(mBearFace); oRightEarFace->setMaterial(mBearFace); oLeftEye->setMaterial(mBearFace); oRightEye->setMaterial(mBearFace); oLeftPupil->setMaterial(mBearPupil); oRightPupil->setMaterial(mBearPupil); oNose->setMaterial(mBearNose); tMouth->setMaterial(mBearMouth); groundLeft->setMaterial(mGround); groundCenter->setMaterial(mGround); groundRight->setMaterial(mGround); tri1->setMaterial(mTriangles); tri2->setMaterial(mTriangles); tri3->setMaterial(mTriangles); oCrystalBall->setMaterial(mCrystalBall); /************************************* ****** PUSH OBJECTS TO VECTOR ***** *************************************/ // push to back of std vector sceneObjects (global variable) sceneObjects.push_back(oHead); sceneObjects.push_back(oLeftEar); sceneObjects.push_back(oRightEar); sceneObjects.push_back(oBeard); sceneObjects.push_back(oLeftEarFace); sceneObjects.push_back(oRightEarFace); sceneObjects.push_back(oLeftEye); sceneObjects.push_back(oRightEye); sceneObjects.push_back(oLeftPupil); sceneObjects.push_back(oRightPupil); sceneObjects.push_back(oNose); sceneObjects.push_back(tMouth); sceneObjects.push_back(groundLeft); sceneObjects.push_back(groundCenter); sceneObjects.push_back(groundRight); sceneObjects.push_back(tri1); sceneObjects.push_back(tri2); sceneObjects.push_back(tri3); sceneObjects.push_back(oCrystalBall); }
// Color of point p on object rto returned in c // p: point where hit rto object, rto: hit object, r: ray with startpoint worldPix and vector direction, color: will be based on material properties // SHADE ONLY CALLED IF RAY FROM CAMERA HIT AN RTO OBJECT! void shade(point &p, RTObject *rto, const ray &r, color* c, GLint nColorFilter, int depth /* = 2 */) // depth used for reflections (2 bounces) { // Get n and material from rto vector n = rto->normalAt(p, *(r.start)); // gimme the normal vector from the point this object was hit yo! Material *m = rto->getMaterialPointer(); // m just needs to be set by void setMaterial(const Material &mat) { m = mat; } // reset ray color c->r = 0.0f; c->g = 0.0f; c->b = 0.0f; /* material default: c.r = 1.0; c.g = 1.0; c.b = 1.0; amb = 1.0; dif = 1.0; Camera placement: viewpoint.x = 0.0; viewpoint.y = 0.0; viewpoint.z = 2.0; viewpoint.w = 1.0; pnear = 1.0; */ /************************************ * Ambient part (ambient color = c.light * c.material) ************************************/ color cAmb; cAmb.r = m->amb * m->c.r; cAmb.g = m->amb * m->c.g; cAmb.b = m->amb * m->c.b; /************************************ * Diffuse part ************************************/ // can the hit point on object p see light source? if so, at what angle? cos(theta) between p to camera and p to light should give us amount // also check normal ... should have something to do with seeing the light? vector vPToEye = eye - p; vector vPToLight = light1.p - p; // normalize vectors to make them unit vectors normalize(&vPToEye); normalize(&vPToLight); // cosTheta gives us how much light gets reflected on the first bounce of light source and surface normal where rto hit GLfloat cosTheta = dotproduct(n, vPToLight); // cDif will be the diffuse color part color cDif = {0.0f, 0.0f, 0.0f}; // if cosTheta is negative, then the hit point p cannot see the light! if ( cosTheta > 0.0f ) { // apply the diffuse color values cDif.r = m->dif * m->c.r * cosTheta; cDif.g = m->dif * m->c.g * cosTheta; cDif.b = m->dif * m->c.b * cosTheta; } /************************************ * Specular part ************************************/ // v is the view direction vector v = *(r.end); // l is the vector from light source to point vector l = p - light1.p; // r is l reflected in the surface vector ref = l - 2.0f*dotproduct(l, n)*n; normalize(&ref); float dot = dotproduct(v, ref); color cSpec = {0.0f, 0.0f, 0.0f}; if ( dot < 0.0f ) // correcting for specular that appears from back surfaces { float spec = powf(dot, 20.0f); cSpec.r += spec * m->spec; cSpec.g += spec * m->spec; cSpec.b += spec * m->spec; } /************************************ * Calculate reflections ************************************/ // calculate reflected ray ray reflected; // move start point back along direction a little to account for rays reflected on their own objects point refStart = p; refStart = p + (-INTERSECTION_TOLERANCE * (*r.end)); reflected.start = &refStart; vector refl = *r.end - 2.0f * ( dotproduct(*r.end, n) ) * n; reflected.end = &refl; point reflectedHitPoint; // test if there is an object in the reflected direction RTObject *rtoReflected = trace(reflected, reflectedHitPoint); // reflectedHitPoint.w = 1.0 if hit; rto = closest hit object, null if no hit // declare reflected color properties color cRef = {0.0f, 0.0f, 0.0f}; // Do the reflection calculations only if something was hit! if ( reflectedHitPoint.w != 0.0 && depth-- > 0 && m->refl ) { // reflected hit point, rto hit object, reflected ray, color, num bounces shade(reflectedHitPoint, rtoReflected, reflected, &cRef, nColorFilter, depth); } /************************************ * Calculate refractions ************************************/ color cRefr = { 0.0f, 0.0f, 0.0f }; GLfloat air = 1.0003f; // if hit object material has a refractive index, let's do refraction! if ( m->refr ) { // get refracted inside ray direction vector refractInside = refract(n, *r.end, air, m->refr); // test if vector is zero vector, if so we quit vector zero = {0.0f, 0.0f, 0.0f, 0.0f}; if ( refractInside == zero ) std::cout << "ZERO!" << std::endl; if ( refractInside != zero ) { // move refracted ray start point along vector direction to account for objects refracting from same surface point refrStart = p + INTERSECTION_TOLERANCE*(*r.end); // setup refract inside ray ray refractInsideRay; refractInsideRay.start = &refrStart; refractInsideRay.end = &refractInside; // assign point where refracted inside ray hits other side of object (that goes to air) here point refractedHitInside; // trace for other side of object RTObject *rtoRefracted = trace(refractInsideRay, refractedHitInside); // now refrace outside of the object switching the n values (negative normal since going out back face) if ( rtoRefracted ) { vector nOut = rtoRefracted->normalAt(refractedHitInside, *refractInsideRay.start); Material *mOut = rtoRefracted->getMaterialPointer(); // if other side has refractive index (it definitely should), let's refract outside of this object if ( mOut->refr ) { // get refract to outside ray direction and test if it is valid (not 0.0f, 0.0f, 0.0f) vector refractOutside = refract(nOut, *refractInsideRay.end, mOut->refr, air); // notice we're going from material to air now if ( refractOutside == zero ) std::cout << "ZERO!" << std::endl; if ( refractOutside != zero ) { // move refracted ray start point along vector direction to account for objects refracting from same surface point refrOutStart = refractedHitInside + INTERSECTION_TOLERANCE*(*refractInsideRay.end); // setup refract outside ray ray refractOutsideRay; refractOutsideRay.start = &refrOutStart; refractOutsideRay.end = &refractOutside; // assign point where refracted outside ray hits a new object here point refractOutHit; // trace for an object hit with the outside refracted ray RTObject *rtoRefractOut = trace(refractOutsideRay, refractOutHit); // if we hit something, let's get the hit object's colors and add it to our refractive color! if ( rtoRefractOut ) { // p: point where hit rto object, rto: hit object, r: ray with startpoint worldPix and vector direction, color: will be based on material properties shade(refractOutHit, rtoRefractOut, refractOutsideRay, &cRefr, nColorFilter); } } } } } } /************************************ * Calculate shadows ************************************/ // create a new ray from point where hit to light ray shadowRay; shadowRay.start = &p; vector shadowDir = light1.p - p; shadowRay.end = &shadowDir; point shadowHitPoint; // test if there are any objects inbetween p and light RTObject *shadower = trace(shadowRay, shadowHitPoint); // declare shadow boolean bool pixShadowed = false; // if there's an object between our start point and light, then shade this pixel! TEST ALL LIGHTS! if ( shadower ) { pixShadowed = true; } /************************************ * Calculate distance attenuation factor (maximum distance is 99999) ************************************/ GLfloat da = (::MAX_DISTANCE_ATTENUATION - length(light1.p - p))/::MAX_DISTANCE_ATTENUATION; /************************************ * Apply final color formula! ************************************/ // color += ambient + (distance attenuation)*(diffuse + specular) + reflective c->r += light1.amb.r*cAmb.r + da*(light1.dif.r*cDif.r + light1.spec.r*cSpec.r) + cRef.r*m->refl + cRefr.r*m->c.r; c->g += light1.amb.g*cAmb.g + da*(light1.dif.g*cDif.g + light1.spec.g*cSpec.g) + cRef.g*m->refl + cRefr.g*m->c.g; c->b += light1.amb.b*cAmb.b + da*(light1.dif.b*cDif.b + light1.spec.b*cSpec.b) + cRef.b*m->refl + cRefr.b*m->c.b; /************************************ * Apply color filter ************************************/ switch ( nColorFilter ) { case COLOR_FILTER_RED: c->g = 0.0f; c->b = 0.0f; break; case COLOR_FILTER_GREEN: c->r = 0.0f; c->b = 0.0f; break; case COLOR_FILTER_BLUE: c->r = 0.0f; c->g = 0.0f; break; case COLOR_FILTER_NONE: default: break; } /************************************ * Apply shadow filter ************************************/ if ( pixShadowed ) { c->r *= 0.8f; // 80% to apply a soft shadow c->g *= 0.8f; c->b *= 0.8f; } /************************************ * Clamp extreme color values to 1.0 ************************************/ if (c->r > 1.0) c->r = 1.0; if (c->g > 1.0) c->g = 1.0; if (c->b > 1.0) c->b = 1.0; }
void OscThread::ProcessMessage( const osc::ReceivedMessage& m, const IpEndpointName& remoteEndpoint ) { (void) remoteEndpoint; // suppress unused parameter warning try{ string keyword = m.AddressPattern(); if( keyword == "/object") { cout<<"Received virtual object from osc"<<endl; //if (H2ICUB != NULL) osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin(); const char *type = (arg++)->AsString(); const char *name = (arg++)->AsString(); string typeword = type; if (typeword == "add") { float x = (arg++)->AsFloat(); float y = (arg++)->AsFloat(); float dimx = (arg++)->AsFloat(); float dimy = (arg++)->AsFloat(); int r = (arg++)->AsInt32(); int g = (arg++)->AsInt32(); int b = (arg++)->AsInt32(); cout<<"Adding object "<<name<<endl; if (!opc->isConnected()) { cout<<"Not connected to OPC"<<endl; return; } if (!isCalibrated) { cout<<"Not calibrated"<<endl; return; } RTObject* o = opc->addRTObject(name); opc->update(o); Vector rtPosition(4); rtPosition(0) = XaxisFactor * x; rtPosition(1) = YaxisFactor * y; rtPosition(2) = 0; rtPosition(3) = 1; o->m_rt_position[0] = rtPosition[0]; o->m_rt_position[1] = rtPosition[1]; o->m_rt_position[2] = rtPosition[2]; cout<<o->toString()<<endl; Vector icubPos(4); icubPos = H2ICUB * rtPosition; o->m_ego_position[0] = icubPos[0];//+ idOffsets[tobj->getSymbolID()][0]; o->m_ego_position[1] = icubPos[1];//+ idOffsets[tobj->getSymbolID()][1]; o->m_ego_position[2] = icubPos[2];//+ idOffsets[tobj->getSymbolID()][2]; o->m_dimensions[0] = dimx; o->m_dimensions[1] = dimy; o->m_dimensions[2] = 0.01; o->m_present = true; o->m_ego_orientation[0] = 0.0; o->m_ego_orientation[1] = 0.0; o->m_ego_orientation[2] = 0.0;//tobj->getAngle(); o->m_color[0] = r; o->m_color[1] = g; o->m_color[2] = b;//tobj->getAngle(); //opc->isVerbose = true; opc->commit(o); //opc->isVerbose = false; cout<<"Added"<<endl; } if (typeword == "remove") { if (!opc->isConnected()) { cout<<"Not connected to OPC"<<endl; return; } cout<<"Removing object"<<endl; RTObject* o = opc->addRTObject(name); o->m_present = false; //opc->isVerbose = true; opc->commit(o); //opc->isVerbose = false; cout<<"Removed"<<endl; } } else if( keyword == "/event") { osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin(); const char *subject = (arg++)->AsString(); const char *verb = (arg++)->AsString(); const char *cobj = "none"; const char *cplace = "none"; const char *ctime = "none"; const char *cmanner = "none"; double timeLife = 5.0; if (arg != m.ArgumentsEnd()) cobj = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) cplace = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) ctime = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) cmanner = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) timeLife = (arg++)->AsFloat(); Relation r(subject,verb,cobj,cplace,ctime,cmanner); cout<<"Received relation (/Event) from OSC "<<r.toString()<< "with a lifetime of "<<timeLife<< "Trying to add : "; opc->addRelation(r, timeLife); cout<<endl; } else if( keyword == "/revent") { osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin(); const char *subject = (arg++)->AsString(); const char *verb = (arg++)->AsString(); const char *cobj = "none"; const char *cplace = "none"; const char *ctime = "none"; const char *cmanner = "none"; double timeLife = 5.0; if (arg != m.ArgumentsEnd()) cobj = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) cplace = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) ctime = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) cmanner = (arg++)->AsString(); if (arg != m.ArgumentsEnd()) timeLife = (arg++)->AsFloat(); Relation r(subject,verb,cobj,cplace,ctime,cmanner); cout<<"Received relation remove request (/revent) from OSC "<<r.toString()<< "Trying to remove : "; opc->removeRelation(r); cout<<endl; } else if ( keyword == "/bottle") { Bottle bFwd; osc::ReceivedMessage::const_iterator arg = m.ArgumentsBegin(); const char *fwdMsg = (arg++)->AsString(); cout<<"OSC input (bottle forwarding) : "<<fwdMsg<<endl; bFwd.addString(fwdMsg); oscFwding.write(bFwd); } else { cout<<"OSC input : Unknown format"<<endl; } }catch( osc::Exception& e ){ std::cout << "error while parsing message: " << m.AddressPattern() << ": " << e.what() << "\n"; } }
bool reservoirHandler::nodeTrainSD() { sCurrentNode = "nodeTrainSD"; sCurrentGrammarFile = nameGrammarNodeTrainSD; ostringstream osError; // Error message osError << "Error in reservoirHandler | "<< sCurrentNode << " :: "; cout << endl << "In " << sCurrentNode << endl << endl; Bottle bOutput; bool fGetaReply = false; Bottle bSpeechRecognized, //recceived FROM speech recog with transfer information (1/0 (bAnswer) ACK/NACK) bMessenger, //to be send TO speech recog bAnswer, //response from speech recog without transfer information, including raw sentence bSemantic, // semantic information of the content of the recognition bSendReasoning, // send the information of recall to the abmReasoning bSpeak, // bottle for tts bTemp; bMessenger.addString("recog"); bMessenger.addString("grammarXML"); bMessenger.addString(grammarToString(sCurrentGrammarFile).c_str()); //to be replace by Say while (!fGetaReply) { Port2SpeechRecog.write(bMessenger,bSpeechRecognized); cout << "Reply from Speech Recog : " << bSpeechRecognized.toString() << endl; if (bSpeechRecognized.toString() == "NACK" || bSpeechRecognized.size() != 3) { osError << "Check " << sCurrentGrammarFile; bOutput.addString(osError.str()); cout << osError.str() << endl; return false; } if (bSpeechRecognized.get(0).toString() == "0") { osError << "Grammar not recognized"; bOutput.addString(osError.str()); cout << osError.str() << endl; return false; } bAnswer = *bSpeechRecognized.get(1).asList(); if (bAnswer.toString() != "" && !bAnswer.isNull()) { fGetaReply = true; } } bSemantic = *bAnswer.get(1).asList()->get(1).asList(); string sQuestionKind = bAnswer.get(1).asList()->get(0).toString(); if(sCurrentType == "train") { // Do you know any ... if (sQuestionKind == "sentence") { cout << "Human : 'Let me see, I have finished...'" << endl; cout << "iCub says : 'What is the corresponding sentence '" << bAnswer.get(0).asString() << " " << bAnswer.toString() << endl ; /* * bAnswer.get(0).asString() => the circle is to the left of of the cross * bAnswer.toString() => * "the circle is to the left of of the cross" (sentence (sentence1 ((object "the circle") (relative_complete ((spatial_relative ((relative to) (spatial "the left"))) (object "the cross")))))) */ cout << "iCub says : 'I have recognized '" << bAnswer.get(0).asString() << endl ; cout << "iCub says : 'Is it ok ? ... 'continue or exit" << endl; string sSentence = bAnswer.get(0).asString(); nodeTrainSD(); } else if (sQuestionKind == "follow") { string continueExit = bSemantic.check("mode", Value("none")).asString(); if (continueExit == "continue the interaction") { cout << "lMeaningsSentences " << endl; lMeaningsSentences.push_back(sSentence+ sSentence_type); nodeTrainSD(); } else if (continueExit == "exit") { trainSaveMeaningSentence(fileSRinputM.c_str()); nodeType(); } } } else if (sCurrentType == "test") { iCub->say("Ok, Set your initial situation, and show me your object of focus !", false); while (!nodeYesNo()) {} iCub->opc->update(); std::list<Entity*> PresentObjects = iCub->opc->EntitiesCache(); std::vector<RTObject> PresentRtoBefore; for(std::list<Entity*>::iterator itE = PresentObjects.begin() ; itE != PresentObjects.end(); itE++) { if ((*itE)->isType(EFAA_OPC_ENTITY_RTOBJECT)) { RTObject rto; rto.fromBottle((*itE)->asBottle()); if (rto.m_present)PresentRtoBefore.push_back(rto) ; } } if (PresentObjects.size() < 2 && PresentObjects.size() > 3) { iCub->say("Dude, I was expecting 2 or 3 objects... Star again !",false); return nodeTrainSD(); } //get the focus object double maxSalience = 0; string sObjectFocus = "none"; for (std::vector<RTObject>::iterator itRTO = PresentRtoBefore.begin() ; itRTO != PresentRtoBefore.end() ; itRTO++) { if (itRTO->m_saliency > maxSalience) { maxSalience = itRTO->m_saliency; sObjectFocus = itRTO->name(); } } if (maxSalience == 0.) { iCub->say("I think I didn't get your focus object dude...",false); return nodeTrainSD(); } string sSentence = "Ok, so you decided to focus on " + sObjectFocus; iCub->say(sSentence,false); if (PresentRtoBefore.size()==2) { double deltaX = 0.0; double deltaY = 0.0; int iFactor; (PresentRtoBefore[0].name() == sObjectFocus) ? iFactor = 1 : iFactor = -1; deltaX = iFactor*(PresentRtoBefore[1].m_ego_position[0] - PresentRtoBefore[0].m_ego_position[0]); deltaY = iFactor*(PresentRtoBefore[1].m_ego_position[1] - PresentRtoBefore[0].m_ego_position[1]); string sLocation; (deltaY>0)? sLocation = "right" : sLocation = "left"; string sRelative; (iFactor==1)? sRelative = (PresentRtoBefore[1].name()) : sRelative =(PresentRtoBefore[0].name()); cout << "I understood :" << endl << sObjectFocus << "\t" << sLocation << "\t" << sRelative << endl ; //TODO send to xavier get response } else // case of 3 objects { RTObject rtFocus, rtRelative1, rtRelative2; bool bFirstRelative = true; for (unsigned int i = 0 ; i < 3 ; i++) { if (PresentRtoBefore[i].name() != sObjectFocus ) { bFirstRelative? rtRelative1 = PresentRtoBefore[i] : rtRelative2 =PresentRtoBefore[i]; bFirstRelative = false; } else { rtFocus = PresentRtoBefore[i]; } } iCub->say("Thinking of the situation",false); double deltaX1 ; // difference btw focus and relative1 double deltaX2 ; // difference btw focus and relative2 deltaX1 = rtRelative1.m_ego_position[1] - rtFocus.m_ego_position[1]; deltaX2 = rtRelative2.m_ego_position[1] - rtFocus.m_ego_position[1]; string sLocation1; string sLocation2; string sRelative1 = rtRelative1.name(); string sRelative2 = rtRelative2.name(); (deltaX1>0)? sLocation1 = "right" : sLocation1 = "left"; (deltaX2>0)? sLocation2 = "right" : sLocation2 = "left"; cout << "I understood : " << sLocation1 << "\t" << sObjectFocus << "\t" << sRelative1 << endl; cout << "and : " << sLocation2 << "\t" << sObjectFocus << "\t" << sRelative2 << endl; if (sSentence_type.size()==0) { sSentence_type = " :C"; } sdataTestSD = sLocation1 + " " + sObjectFocus + " " + sRelative1 + ", " + sLocation2 + " " + sObjectFocus + " " + sRelative2 + sSentence_type; // TO SEND TO XAVIER copyPastFile(fileXavierTrain.c_str(), fileSRinputM.c_str()); cout << fileXavierTrain << endl; cout << fileSRinputM << endl; createTestwithTrainData(fileSRinputM.c_str(), sdataTestSD); callReservoir(pythonPath + fileSD); string result = openResult(fileSRoutputS.c_str()); iCub->say(result,false); cout << "iCub says : 'I have understood '" << result << endl ; } return nodeType(); } if (bAnswer.get(0).asString() == "return the interaction") { nodeModality(); } return true; }