Example #1
0
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;
}
Example #2
0
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;
}
Example #3
0
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;
}
Example #4
0
/*
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;
}
Example #5
0
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);
}
Example #6
0
// 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;
	
}
Example #7
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";
    }
}
Example #8
0
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;
 }