C++ cPrecisionClock::reset方法代码示例

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

    // main haptic simulation loop
    while(simulationRunning)
    {
        // stop the simulation clock
        simClock.stop();

        // read the time increment in seconds
        double timeInterval = simClock.getCurrentTimeSeconds();
        if (timeInterval > 0.001) { timeInterval = 0.001; }

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

        // read position from haptic device
        cVector3d pos;
        hapticDevice->getPosition(pos);
        pos.mul(workspaceScaleFactor);
        device->setPos(pos);

        // init temp variable
        cVector3d force;
        force.zero();

        // compute reaction forces
        for (int y=0; y<10; y++)
        {
            for (int x=0; x<10; x++)
            {
               cVector3d nodePos = nodes[x][y]->m_pos;
               cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
               cVector3d tmpfrc = cNegate(f);
               nodes[x][y]->setExternalForce(tmpfrc);
               force.add(f);
            }
        }

        // integrate dynamics
        defWorld->updateDynamics(timeInterval);

        // scale force
        force.mul(deviceForceScale);

        // send forces to haptic device
        hapticDevice->setForce(force);
    }

    // exit haptics thread
    simulationFinished = true;
}

void updateHaptics(void)
{
	// reset simulation clock
	simClock.reset();
	simClock.start();

    // main haptic simulation loop
    while(simulationRunning)
    {
        // read position from haptic device
        cVector3d pos;
        hapticDevice->getPosition(pos);
        pos.mul(workspaceScaleFactor);
        device->setPos(pos);

        // init temp variable
        cVector3d force;
        force.zero();

        // compute reaction forces
        list<cGELMesh*>::iterator i;

        // model water level
        for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
        {
            cGELMesh *nextItem = *i;

            if (nextItem->m_useMassParticleModel)
            {
                int numVertices = nextItem->m_gelVertices.size();
                for (int i=0; i<numVertices; i++)
                {
                   cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
                   cVector3d force = cVector3d(-0.002 * nodePos.x, -0.002 * nodePos.y, 0.0);
                   if (nodePos.z < level)
                   {
                        double depth = nodePos.z - level;
                        force.add(cVector3d(0,0,-100*depth));
                   }
                   nextItem->m_gelVertices[i].m_massParticle->setExternalForce(force);
                }
            }

            if (nextItem->m_useSkeletonModel)
            {
                list<cGELSkeletonNode*>::iterator i;
                for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
                {
                    cGELSkeletonNode* node = *i;
                    cVector3d nodePos = node->m_pos;
                    double radius = node->m_radius;
                    cVector3d force = cVector3d(-0.01 * nodePos.x, -0.01 * (nodePos.y), 0.0);
                    if ((nodePos.z-radius) < level)
                    {
                        double depth = (nodePos.z-radius) - level;
                        force.add(cVector3d(0,0,-1.0 * depth));
                        node->m_vel.mul(0.95);
                    }
                    node->setExternalForce(force);
                }
            }
        }


        // compute haptic feedback
		for(i = defWorld->m_gelMeshes.begin(); i != defWorld->m_gelMeshes.end(); ++i)
		{
			cGELMesh *nextItem = *i;

			if (nextItem->m_useMassParticleModel)
			{
				int numVertices = nextItem->m_gelVertices.size();
				for (int i=0; i<numVertices; i++)
				{
				cVector3d nodePos = nextItem->m_gelVertices[i].m_massParticle->m_pos;
				cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
				if (f.lengthsq() > 0)
				{
					cVector3d tmpfrc = cNegate(f);
					nextItem->m_gelVertices[i].m_massParticle->setExternalForce(tmpfrc);
				}
				force.add(cMul(1.0, f));
				}
			}

			if (nextItem->m_useSkeletonModel)
			{
				list<cGELSkeletonNode*>::iterator i;
				for(i = nextItem->m_nodes.begin(); i != nextItem->m_nodes.end(); ++i)
				{
					cGELSkeletonNode* node = *i;
					cVector3d nodePos = node->m_pos;
					double radius = node->m_radius;
					cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffness);
					if (f.lengthsq() > 0)
					{
						cVector3d tmpfrc = cNegate(f);
						node->setExternalForce(tmpfrc);
					}
					force.add(cMul(4.0, f));
//.........这里部分代码省略.........

void updateHaptics(void)
{
    // simulation clock
    cPrecisionClock simClock;
    simClock.start(true);

    // reset haptics activation clock
    startHapticsClock.reset();
	startHapticsClock.start();
	bool hapticsReady = false;

    // main haptic simulation loop
    while(simulationRunning)
    {
        // wait for some time before enabling haptics
		if (!hapticsReady)
		{
			if (startHapticsClock.getCurrentTimeSeconds() > 3.0)
			{
				hapticsReady = true;
			}
		}

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

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

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

        // check if the tool is touching an object
        cGenericObject* object = tool->m_proxyPointForceModel->m_contactPoint0->m_object;

        // read user switch status
        bool userSwitch = tool->getUserSwitch(0);

        // if the tool is currently grasping an object we simply update the interaction grasp force
        // between the tool and the object (modeled by a virtual spring)
        if (graspActive && userSwitch)
        {
            // retrieve latest position and orientation of grasped ODE object in world coordinates
            cMatrix3d globalGraspObjectRot = graspObject->getGlobalRot();
            cVector3d globalGraspObjectPos = graspObject->getGlobalPos();

            // compute the position of the grasp point on object in global coordinates
            cVector3d globalGraspPos = globalGraspObjectPos + cMul(globalGraspObjectRot, graspPos);

            // retrieve the position of the tool in global coordinates
            cVector3d globalToolPos  = tool->getProxyGlobalPos();

            // compute the offset between the tool and grasp point on the object
            cVector3d offset = globalToolPos - globalGraspPos;

            // model a spring between both points
            double STIFFNESS = 4;
            cVector3d force = STIFFNESS * offset;

            // apply attraction force (grasp) onto object
            graspObject->addGlobalForceAtGlobalPos(force, globalGraspPos);

            // scale magnitude and apply opposite force to haptic device
            tool->m_lastComputedGlobalForce.add(cMul(-1.0, force));

            // update both end points of the line which is used for display purposes only
            graspLine->m_pointA = globalGraspPos;
            graspLine->m_pointB = globalToolPos;
        }

        // the user is not or no longer currently grasping the object
        else
        {
            // was the user grasping the object at the previous simulation loop
            if (graspActive)
            {
                // we disable grasping
                graspActive = false;

                // we hide the virtual line between the tool and the grasp point
                graspLine->setShowEnabled(false);

                // we enable haptics interaction between the tool and the previously grasped object
                if (graspObject != NULL)
                {
                    graspObject->m_imageModel->setHapticEnabled(true, true);
                }
            }

            // the user is touching an object
            if (object != NULL)
            {
                // check if object is attached to an external ODE parent
                cGenericType* externalParent = object->getExternalParent();
                cODEGenericBody* ODEobject = dynamic_cast<cODEGenericBody*>(externalParent);
                if (ODEobject != NULL)
                {
                    // get position of tool
                    cVector3d pos = tool->m_proxyPointForceModel->m_contactPoint0->m_globalPos;

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

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

    // 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 device
        tool->applyForces();

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

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

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

        // temp variable to compute rotational acceleration
        cVector3d rotAcc(0,0,0);

        // check if tool is touching an object
        cGenericObject* objectContact = tool->m_proxyPointForceModel->m_contactPoint0->m_object;
        if (objectContact != NULL)
        {
            // retrieve the root of the object mesh
            cGenericObject* obj = objectContact->getSuperParent();

            // get position of cursor in global coordinates
            cVector3d toolPos = tool->m_deviceGlobalPos;

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

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

            // compute acceleration based on the interaction forces
            // between the tool and the object
            if (vObjectCMToTool.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 effective force to take into account the fact the object
                // can only rotate around a its center mass and not translate
                cVector3d effectiveForce = toolForce - cProject(toolForce, vObjectCMToTool);

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

                // update rotational acceleration
                const double OBJECT_INERTIA = 0.4;
                rotAcc = (1.0 / OBJECT_INERTIA) * torque;
            }
        }

        // update rotational velocity
        rotVel.add(timeInterval * rotAcc);

        // set a threshold on the rotational velocity term
        const double ROT_VEL_MAX = 10.0;
        double velMag = rotVel.length();
        if (velMag > ROT_VEL_MAX)
        {
            rotVel.mul(ROT_VEL_MAX / velMag);
        }

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

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

        // compute the next rotation configuration of the object
        if (rotVel.length() > CHAI_SMALL)
        {
            object->rotate(cNormalize(rotVel), timeInterval * rotVel.length());
        }
    }
    
    // exit haptics thread
//.........这里部分代码省略.........