C++ cFrequencyCounter::signal方法代码示例

void updateHaptics(void)
{
    // simulation in now running
    simulationRunning  = true;
    simulationFinished = false;

    // main haptic simulation loop
    while(simulationRunning)
    {
        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

        // send forces to haptic device
        tool->applyForces();

        // update frequency counter
        frequencyCounter.signal(1);
    }
    
    // exit haptics thread
    simulationFinished = true;
}

void updateHaptics(void)
{
	// precion clock
	cPrecisionClock clock;
	int i = 0;

    // main haptic simulation loop
    while(simulationRunning)
    {
		bool blIO = false;
		blIO = deltaCtrl.SetFGetPosStatues(dF,dPos,statu,dT, blOutRange);				// changing point

		dPos[2]+=0.1;

        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

		// update my pose************MINE***************** we get the force in
		tool->updateMyPose(dPos);

        // compute interaction forces
        tool->computeInteractionForces();

		// we need to get the force out to our device****************	MINE
		tool->getLastComputedForce(dF);

        // send forces to device
        tool->applyForces();

		// stop clock
		double time = clock.stop();
		
		// check if contact occured with cylinder
		if(tool->isInContact(cylinder))
		{
			// get force applied on the y axis
			double force = tool->m_lastComputedGlobalForce.y();

			// shift cylinderaccording to force
            const double K = 0.5;
			double pos = cylinder->getLocalPos().y();
			pos = cClamp(pos - K * time * force,-0.5, 0.6);
			cylinder->setLocalPos(0.0, pos, 0.0);
		}

		// restart clock
		clock.start(true);

        // update frequency counter
        frequencyCounter.signal(1);
    }
    
    // exit haptics thread
    simulationFinished = true;
}

void updateHaptics(void)
{
    // initialize frequency counter
    frequencyCounter.reset();

	// initialize precision clock
	cPrecisionClock clock;
    clock.reset();

    // simulation in now running
    simulationRunning  = true;
    simulationFinished = false;

    // main haptic simulation loop
    while(simulationRunning)
    {
        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

        // send forces to haptic device
        tool->applyForces();

        // stop clock
        double time = clock.stop();

        // scroll text according to the haptic device position
        const double K = 4000;
        double command = tool->m_hapticPoint->getGlobalPosGoal().y();
        double pos = textPosition + K * time * command;
        textPosition = cClamp(pos, -text->getWidth(), (double)(windowW));

        // restart clock
        clock.start(true);

        // update frequency counter
        frequencyCounter.signal(1);
    }
    
    // exit haptics thread
    simulationFinished = true;
}

void updateHaptics(void)
{
    cPrecisionClock clock;
    double ang;

    // simulation in now running
    simulationRunning  = true;
    simulationFinished = false;

    double t0 = clock.getCPUTimeSeconds();
    double rate = 2*M_PI/videoEven->getDuration();

    // main haptic simulation loop
    while(simulationRunning)
    {
        // rotate movie (5 deg per second)
        ang = -rate*(clock.getCPUTimeSeconds()-t0);
        anchor->setLocalRot (cMatrix3d(cVector3d(0,1,0),ang));

        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

        // send forces to haptic device
        tool->applyForces();

        // update frequency counter
        frequencyCounter.signal(1);
    }

    // exit haptics thread
    simulationFinished = true;
}

//.........这里部分代码省略.........

        // send forces to haptic device
        tool->applyForces();


        /////////////////////////////////////////////////////////////////////
        // INTERACTION WITH PALETTE
        /////////////////////////////////////////////////////////////////////

        if (tool->isInContact(palette))
        {
            cCollisionEvent* contact = tool->m_hapticPoint->getCollisionEvent(0);
            if (contact != NULL)
            {
                // retrieve contact information
                cVector3d localPos = contact->m_localPos;
                unsigned int triangleIndex = contact->m_triangleIndex;
                cTriangleArrayPtr triangles = contact->m_triangles;

                // retrieve texture coordinate
                cVector3d texCoord = triangles->getTexCoordAtPosition(triangleIndex, localPos);

                // retrieve pixel information
                int px, py;
                palette->m_texture->m_image->getPixelLocation(texCoord, px, py);

                // retrieve color information at pixel
                palette->m_texture->m_image->getPixelColor(px, py, paintColor);

                // update color of tool
                tool->m_hapticPoint->m_sphereProxy->m_material->setColor(paintColor);
            }
        }


        /////////////////////////////////////////////////////////////////////
        // INTERACTION WITH CANVAS
        /////////////////////////////////////////////////////////////////////

        if (tool->isInContact(canvas))
        {
            cCollisionEvent* contact = tool->m_hapticPoint->getCollisionEvent(0);
            if (contact != NULL)
            {
                // retrieve contact information
                cVector3d localPos = contact->m_localPos;
                unsigned int triangleIndex = contact->m_triangleIndex;
                cTriangleArrayPtr triangles = contact->m_triangles;

                // retrieve texture coordinate
                cVector3d texCoord = triangles->getTexCoordAtPosition(triangleIndex, localPos);

                // retrieve pixel information
                int px, py;
                canvas->m_texture->m_image->getPixelLocation(texCoord, px, py);

                // paint color at tool position
                const double K_INK = 20;
                const double K_SIZE = 10;
                const int BRUSH_SIZE = 25;

                double size = cClamp((K_SIZE * force), 0.0, (double)(BRUSH_SIZE));
                for (int x=-BRUSH_SIZE; x<BRUSH_SIZE; x++)
                {
                    for (int y=-BRUSH_SIZE; y<BRUSH_SIZE; y++)
                    {                        
                        // compute new color percentage
                        double distance = sqrt((double)(x*x+y*y));
                        if (distance <= size)
                        {
                            // get color at location
                            cColorb color, newColor;
                            canvas->m_texture->m_image->getPixelColor(px+x, py+y, color);
                            
                            // compute color factor based of pixel position and force interaction 
                            double factor = cClamp(K_INK * timeInterval * cClamp(force, 0.0, 10.0) * cClamp(1 - distance/size, 0.0, 1.0), 0.0, 1.0);

                            // compute new color
                            newColor.setR((1.0 - factor) * color.getR() + factor * paintColor.getR());
                            newColor.setG((1.0 - factor) * color.getG() + factor * paintColor.getG());
                            newColor.setB((1.0 - factor) * color.getB() + factor * paintColor.getB());

                            // assign new color to pixel
                            canvas->m_texture->m_image->setPixelColor(px+x, py+y, newColor);
                        }              
                    }
                }

                // update texture
                canvas->m_texture->markForUpdate();
            }
        }

        // update frequency counter
        frequencyCounter.signal(1);
    }
    
    // exit haptics thread
    simulationFinished = true;
}

void updateHaptics(void)
{
    // reset clock
    cPrecisionClock clock;
    clock.reset();

    // main haptic simulation loop
    while(simulationRunning)
    {
        /////////////////////////////////////////////////////////////////////
        // SIMULATION TIME    
        /////////////////////////////////////////////////////////////////////

        // stop the simulation clock
        clock.stop();

        // read the time increment in seconds
        double timeInterval = clock.getCurrentTimeSeconds();

        // restart the simulation clock
        clock.reset();
        clock.start();

        // update frequency counter
        frequencyCounter.signal(1);


        /////////////////////////////////////////////////////////////////////
        // HAPTIC FORCE COMPUTATION
        /////////////////////////////////////////////////////////////////////

        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

        // send forces to device
        tool->applyForces();


        /////////////////////////////////////////////////////////////////////
        // HAPTIC SIMULATION
        /////////////////////////////////////////////////////////////////////

        // get position of cursor in global coordinates
        cVector3d toolPos = tool->getDeviceGlobalPos();

        // get position of object in global coordinates
        cVector3d objectPos = object->getGlobalPos();

        // compute a vector from the center of mass of the object (point of rotation) to the tool
        cVector3d v = cSub(toolPos, objectPos);

        // compute angular acceleration based on the interaction forces
        // between the tool and the object
        cVector3d angAcc(0,0,0);
        if (v.length() > 0.0)
        {
            // get the last force applied to the cursor in global coordinates
            // we negate the result to obtain the opposite force that is applied on the
            // object
            cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);

            // compute the effective force that contributes to rotating the object.
            cVector3d force = toolForce - cProject(toolForce, v);

            // compute the resulting torque
            cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force));

            // update rotational acceleration
            const double INERTIA = 0.4;
            angAcc = (1.0 / INERTIA) * torque;
        }

        // update rotational velocity
        angVel.add(timeInterval * angAcc);

        // set a threshold on the rotational velocity term
        const double MAX_ANG_VEL = 10.0;
        double vel = angVel.length();
        if (vel > MAX_ANG_VEL)
        {
            angAcc.mul(MAX_ANG_VEL / vel);
        }

        // add some damping too
        const double DAMPING = 0.1;
        angVel.mul(1.0 - DAMPING * timeInterval);

        // if user switch is pressed, set velocity to zero
        if (tool->getUserSwitch(0) == 1)
        {
            angVel.zero();
        }

        // compute the next rotation configuration of the object
//.........这里部分代码省略.........

void updateHaptics(void)
{
    cMode state = IDLE;
    cGenericObject* object;
    cTransform tool_T_object;

    // main haptic simulation loop
    while(simulationRunning)
    {
        /////////////////////////////////////////////////////////////////////////
        // HAPTIC RENDERING
        /////////////////////////////////////////////////////////////////////////

        // update frequency counter
        frequencyCounter.signal(1);

        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

 
        /////////////////////////////////////////////////////////////////////////
        // MANIPULATION
        /////////////////////////////////////////////////////////////////////////

        // compute transformation from world to tool (haptic device)
        cTransform world_T_tool = tool->getDeviceGlobalTransform();

        // get status of user switch
        bool button = tool->getUserSwitch(0);

        //
        // MODE 1:
        // Idle mode - user presses the user switch
        //
        if ((state == IDLE) && (button == true))
        {
            // check if at least one contact has occured
            if (tool->m_hapticPoint->getNumCollisionEvents() > 0)
            {
                // get contact event
                cCollisionEvent* collisionEvent = tool->m_hapticPoint->getCollisionEvent(0);

                // get object from contact event
                object = collisionEvent->m_object;

                // get transformation from object
                cTransform world_T_object = object->getGlobalTransform();

                // compute inverse transformation from contact point to object 
                cTransform tool_T_world = world_T_tool;
                tool_T_world.invert();

                // store current transformation tool
                tool_T_object = tool_T_world * world_T_object;

                // update state
                state = SELECTION;
            }
        }


        //
        // MODE 2:
        // Selection mode - operator maintains user switch enabled and moves object
        //
        else if ((state == SELECTION) && (button == true))
        {
            // compute new tranformation of object in global coordinates
            cTransform world_T_object = world_T_tool * tool_T_object;

            // compute new tranformation of object in local coordinates
            cTransform parent_T_world = object->getParent()->getLocalTransform();
            parent_T_world.invert();
            cTransform parent_T_object = parent_T_world * world_T_object;

            // assign new local transformation to object
            object->setLocalTransform(parent_T_object);

            // set zero forces when manipulating objects
            tool->m_lastComputedGlobalForce.zero();
        }

        //
        // MODE 3:
        // Finalize Selection mode - operator releases user switch.
        //
        else
        {
            state = IDLE;
        }


        /////////////////////////////////////////////////////////////////////////
        // FINALIZE
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        button3 = false;

        hapticDevice->getUserSwitch(0, button0);
        hapticDevice->getUserSwitch(1, button1);
        hapticDevice->getUserSwitch(2, button2);
        hapticDevice->getUserSwitch(3, button3);

        /////////////////////////////////////////////////////////////////////
        // UPDATE 3D MODELS
        /////////////////////////////////////////////////////////////////////

        // update arrow
        velocity->m_pointA = position;
        velocity->m_pointB = cAdd(position, linearVelocity);

        // update position and orientation of cursor
        cursor->setLocalPos(position);
        cursor->setLocalRot(rotation);

        // adjust the  color of the cursor according to the status of
        // the user switch (ON = TRUE / OFF = FALSE)
        if (button0)
        {
            cursor->m_material->setGreenMediumAquamarine();
        }
        else if (button1)
        {
            cursor->m_material->setYellowGold();
        }
        else if (button2)
        {
            cursor->m_material->setOrangeCoral();
        }
        else if (button3)
        {
            cursor->m_material->setPurpleLavender();
        }
        else
        {
            cursor->m_material->setBlueRoyal();
        }

        // update global variable for graphic display update
        hapticDevicePosition = position;


        /////////////////////////////////////////////////////////////////////
        // COMPUTE AND SEND FORCE AND TORQUE
        /////////////////////////////////////////////////////////////////////

        cVector3d force (0,0,0);
        cVector3d torque (0,0,0);
        double gripperForce = 0.0;

        // apply force field
        if (useForceField)
        {
            // compute linear force
            double Kp = 20; // [N/m]
            cVector3d forceField = -Kp * position;
            force.add(forceField);

            // compute angular torque
            double Kr = 0.05; // [N/m.rad]
            cVector3d axis;
            double angle;
            rotation.toAngleAxis(angle, axis);
            torque = (-Kr * angle) * axis;
        }

        // apply damping term
        if (useDamping)
        {
            cHapticDeviceInfo info = hapticDevice->getSpecifications();

            // compute linear damping force
            double Kv = 1.0 * info.m_maxLinearDamping;
            cVector3d forceDamping = -Kv * linearVelocity;
            force.add(forceDamping);

            // compute angluar damping force
            double Kvr = 1.0 * info.m_maxAngularDamping;
            cVector3d torqueDamping = -Kvr * angularVelocity;
            torque.add(torqueDamping);

            // compute gripper angular damping force
            double Kvg = 1.0 * info.m_maxGripperAngularDamping;
            gripperForce = gripperForce - Kvg * gripperAngularVelocity;
        }

        // send computed force, torque and gripper force to haptic device
        hapticDevice->setForceAndTorqueAndGripperForce(force, torque, gripperForce);

        // update frequency counter
        frequencyCounter.signal(1);
    }

    // exit haptics thread
    simulationFinished = true;
}

void updateHaptics( void )
{

    // simulation in now running
    simulationRunning  = true;
    simulationFinished = false;

    // main haptic simulation loop
    while( simulationRunning )
    {

        /////////////////////////////////////////////////////////////////////////
        // HAPTIC RENDERING
        /////////////////////////////////////////////////////////////////////////

        // update frequency counter
        frequencyCounter.signal( 1 );

        // compute global reference frames for each object
        world->computeGlobalPositions( true );

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

        // update selected particle to cursor
        cVector3d proxy_pos, tool_pos;
        tool_pos = tool->getDeviceLocalPos();
        proxy_pos.x( tool_pos.x() - SELECTED_PARTICLE_RADIUS );
        proxy_pos.y( tool_pos.y() - SELECTED_PARTICLE_RADIUS );
        proxy_pos.z( tool_pos.z() + SELECTED_PARTICLE_RADIUS );
        selected_particle->setLocalPos( proxy_pos );

        cVector3d x_vector( 0, 0, 0 );
        x_vector.x( proxy_pos.x() );

        // temp_vector is on the y-z plane
        cVector3d temp_vector = proxy_pos - x_vector;
        cVector3d unit_vector = temp_vector;
        unit_vector.normalize();

        cVector3d force( 0, 0, 0 );
        
        // If user has picked up a electron, set magnetical effect around shells
        if ( ELECTRON == is_selected )
        {
            for ( int i = 0; i < NUM_SHELLS; i++ )
            {
                // Set force field depending on the number of electrons placed
                if ( particles_left[ELECTRON] == 0 )
                {
                    break;
                }
                // If less than two electrons are placed, skip force for outer shells
                // If at least two electrons are placed, skip first shell
                if ( current_atom_num - particles_left[ELECTRON] < 2 && i > 0 ||
                     current_atom_num - particles_left[ELECTRON] >= 2 && i == 0 )
                {
                    continue;
                }
                // If we are within the force field
                if( temp_vector.length() < ( TORUS_OUTER + OUTER_THRESHOLD + TORUS_DISTANCE*i ) &&
                   temp_vector.length() > ( TORUS_OUTER - OUTER_THRESHOLD + TORUS_DISTANCE*i ) &&
                   proxy_pos.x() < 0.07 )
                {
                    in_ok_position = ELECTRON;
                    // If we are close to the center of the torus
                    if ( temp_vector.length() >= ( TORUS_OUTER + INNER_THRESHOLD + TORUS_DISTANCE*i ) ||
                        temp_vector.length() <= ( TORUS_OUTER - INNER_THRESHOLD + TORUS_DISTANCE*i ) )
                    {
                        // We are either inside the inner or outer force field
                        if ( temp_vector.length() > ( TORUS_OUTER - OUTER_THRESHOLD + TORUS_DISTANCE*i ) &&
                            temp_vector.length() <= ( TORUS_OUTER - INNER_THRESHOLD + TORUS_DISTANCE*i ) )
                        {
                            // When on the inside force field for the torus
                            force = SPRING_CONSTANT * ( unit_vector*( TORUS_OUTER - INNER_THRESHOLD + TORUS_DISTANCE*i ) - temp_vector );
                        }
                        else {
                            // When on the outer force field for the torus
                            force = SPRING_CONSTANT * ( unit_vector*( TORUS_OUTER + INNER_THRESHOLD + TORUS_DISTANCE*i ) - temp_vector );
                        }
                    }
                    break;
                }
                else {
                    in_ok_position = -1;
                }
            }

            // Prevent the user place an electron in the nucleus
            if ( temp_vector.length() <= NUCLEUS_RADIUS )
            {
                force = SPRING_CONSTANT * ( unit_vector*NUCLEUS_RADIUS - temp_vector );
            }
        }
        else if ( PROTON == is_selected || NEUTRON == is_selected )
        {
            // If cursor is close to the nucleus in the y-z plane, set force to centrum
//.........这里部分代码省略.........

void updateHaptics(void)
{
    // precision clock
    cPrecisionClock clock;
    clock.reset();

    // simulation in now running
    simulationRunning  = true;
    simulationFinished = false;

    // main haptic simulation loop
    while(simulationRunning)
    {
        /////////////////////////////////////////////////////////////////////
        // SIMULATION TIME    
        /////////////////////////////////////////////////////////////////////

        // stop the simulation clock
        clock.stop();

        // read the time increment in seconds
        double timeInterval = clock.getCurrentTimeSeconds();

        // restart the simulation clock
        clock.reset();
        clock.start();

        // update frequency counter
        frequencyCounter.signal(1);


        /////////////////////////////////////////////////////////////////////
        // HAPTIC FORCE COMPUTATION
        /////////////////////////////////////////////////////////////////////

        // compute global reference frames for each object
        world->computeGlobalPositions(true);

        // update position and orientation of tool
        tool->updatePose();

        // compute interaction forces
        tool->computeInteractionForces();

        // send forces to haptic device
        tool->applyForces();


        /////////////////////////////////////////////////////////////////////
        // HAPTIC SIMULATION
        /////////////////////////////////////////////////////////////////////
        
        // check if contact occurred with cylinder
        if(tool->isInContact(cylinder))
        {
            // get force applied on the y axis
            double force = tool->m_lastComputedGlobalForce.y();

            // shift cylinder according to force
            const double K = 0.5;
            double pos = cylinder->getLocalPos().y();
            pos = cClamp(pos - K * timeInterval * force,-0.5, 0.6);
            cylinder->setLocalPos(0.0, pos, 0.0);
        }
    }
    
    // exit haptics thread
    simulationFinished = true;
}