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;

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

  /*!
  * \brief Callback for receiving a point cloud.
  *
  * This description is displayed lower in the doxygen as an extended description along with
  * the above brief description.
  */
  void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& input)
  {

//    const std::string& publisher_name = event.getPublisherName();
//    ros::M_string connection_header = event.getConnectionHeader();
//    ros::Time receipt_time = event.getReceiptTime();
//    const std::string topic = connection_header["topic"];

//    const sensor_msgs::PointCloud2& input = event.getMessage();

    std::string topic = "none";

    int cloud_size = input->width*input->height;
    ROS_DEBUG_NAMED("haptics", "Got a cloud on topic %s in frame %s with %d points!",
               topic.c_str(),
               input->header.frame_id.c_str(),
               cloud_size);

    if(cloud_size == 0) return;

    pcl::fromROSMsg(*input, last_cloud_);
    
    if(last_cloud_.header.frame_id.compare("/tool_frame"))
    {
//        ROS_INFO("Transforming cloud with %d points from %s to %s.",
//                 (int)last_cloud_.points.size(), last_cloud_.header.frame_id.c_str(),
//                 "/world");
        if(!tfl_.waitForTransform("/tool_frame", last_cloud_.header.frame_id,
                                  last_cloud_.header.stamp, ros::Duration(2.0)) )
        {
            ROS_ERROR("Couldn't get transform for cloud, returning FAILURE!");
            return;
        }
        pcl_ros::transformPointCloud("/tool_frame", last_cloud_, last_cloud_, tfl_);
    }

    //ROS_INFO("m_shape is %d", m_shape);
    //if(new_object->m_shape == hviz::Haptics_CLOUD)
    {
      boost::mutex::scoped_lock lock(mutex_);
      if(object->m_shape == hviz::Haptics_CLOUD)
        object->createFromCloud(last_cloud_);
    }

    if(!got_first_cloud_){
      got_first_cloud_ = true;
      ROS_INFO("Got first cloud!");
    }

    static bool firstTime = true;
    static int counter = 0;
    static cPrecisionClock pclock;
    float sample_period = 2.0;

    if(firstTime) // start a clock to estimate the rate
    {
        pclock.setTimeoutPeriodSeconds(sample_period);
        pclock.start(true);
        firstTime = false;
    }

    // estimate the refresh rate and publish
    ++counter;
    if (pclock.timeoutOccurred()) {
        pclock.stop();
        float rate = counter/sample_period;
        counter = 0;
        pclock.start(true);
        std_msgs::String msg;
        char status_string[256];
        sprintf(status_string, "Cloud rate: %.3f",
                rate );
        msg.data = status_string;
        pub_status_.publish(msg);
    }

  }

  /*!
  * \brief The timer callback sends graphical output to rviz.
  *
  * This description is displayed lower in the doxygen as an extended description along with
  * the above brief description.
  */
  void timerCallback()
  {
    if(!tool) return;

    ros::Time time_now = ros::Time::now(); // use single time for all output

    static bool firstTime = true;
    static int counter = 0;
    static cPrecisionClock pclock;

    if(firstTime) // start a clock to estimate the rate
    {
        pclock.setTimeoutPeriodSeconds(1.0);
        pclock.start(true);
        firstTime = false;
    }

    // estimate the refresh rate and publish
    ++counter;
    if (pclock.timeoutOccurred()) {
        pclock.stop();
        graphicRateEstimate = counter;
        counter = 0;
        pclock.start(true);
        std_msgs::String msg;
        char status_string[256];
        sprintf(status_string, "Haptic rate: %.3f Graphics Rate: %.3f",
                rateEstimate, graphicRateEstimate);
        msg.data = status_string;
        pub_status_.publish(msg);
    }

    // Transmit the visualizations of the tool/proxy
    float proxy_radius = config_.tool_radius;
    cVector3d pos =  object->m_interactionProjectedPoint; //tool->getDeviceGlobalPos();
    cVector3d HIP = tool->getDeviceGlobalPos();
    cMatrix3d tool_rotation = tool->m_deviceGlobalRot;

    if(false && device_info.m_sensedRotation)
    {
      object_manipulator::shapes::Mesh mesh;
      mesh.dims = tf::Vector3(0.5, 0.5, 0.5);
      mesh.frame.setRotation(chai_tools::cMatrixToTFQuaternion(tool_rotation));
      mesh.frame.setOrigin(tf::Vector3(pos.x, pos.y, pos.z));
      mesh.header.stamp = time_now;
      mesh.header.frame_id = "/tool_frame";
      mesh.use_embedded_materials = true;
      mesh.mesh_resource = std::string("package://pr2_description/meshes/gripper_v0/gripper_palm.dae");
//      std::string proximal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger.dae");
//      std::string distal_finger_string("package://pr2_description/meshes/gripper_v0/l_finger_tip.dae");
      object_manipulator::drawMesh(pub_marker_, mesh, "gripper", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.3, 0.7, 1.0));
    }
    else
    {
      object_manipulator::shapes::Sphere sphere;
      sphere.dims = tf::Vector3(2*proxy_radius, 2*proxy_radius, 2*proxy_radius);
      sphere.frame = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(pos.x, pos.y, pos.z));
      sphere.header.frame_id = "/tool_frame";
      sphere.header.stamp = time_now;
      object_manipulator::drawSphere(pub_marker_, sphere, "proxy", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.3, 0.7, 1.0));

      //object_manipulator::shapes::Sphere sphere;
      sphere.dims = tf::Vector3(1.9*proxy_radius, 1.9*proxy_radius, 1.9*proxy_radius);
      sphere.frame = tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(HIP.x, HIP.y, HIP.z));
      sphere.header.frame_id = "/tool_frame";
      sphere.header.stamp = time_now;
      object_manipulator::drawSphere(pub_marker_, sphere, "HIP", 0, ros::Duration(), object_manipulator::msg::createColorMsg(1.0, 0.0, 0.0, 0.6));
    }


    object_manipulator::shapes::Cylinder box;
    tf::Quaternion quat = chai_tools::cMatrixToTFQuaternion(object->tPlane->getRot());
    box.frame.setRotation(quat);
    box.frame.setOrigin(chai_tools::cVectorToTF(object->tPlane->getPos()) - 0.5*proxy_radius*box.frame.getBasis().getColumn(2));
    box.dims = tf::Vector3(5*proxy_radius, 5*proxy_radius, 0.0015);
    box.header.frame_id = "/tool_frame";
    box.header.stamp = time_now;
    if(object->m_interactionInside){
      object_manipulator::drawCylinder(pub_marker_, box, "tPlane", 0, ros::Duration(), object_manipulator::msg::createColorMsg(0.2, 0.6, 1.0, 0.8), false);
    }
    else
      object_manipulator::drawCylinder(pub_marker_, box, "tPlane", 0, ros::Duration(), object_manipulator::msg::createColorMsg(0.2, 0.6, 1.0, 0.8), true);


    boost::mutex::scoped_lock lock(mutex_);
    if(config_.publish_cloud)
    {
//      if(object->last_normals->points.size() == object->last_points->points.size())
//      {
//        visualization_msgs::Marker marker;
//        marker.header = object->last_points->header;
//        marker.ns = "cloud";
//        marker.id = 0;
//        marker.type = visualization_msgs::Marker::SPHERE_LIST; // CUBE, SPHERE, ARROW, CYLINDER
//.........这里部分代码省略.........

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)
{
    // 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
//.........这里部分代码省略.........

void updateHaptics(void)
{
    double timeV = 0.0;

    cLabel* label = new cLabel();
    cLabel* label2 = new cLabel();
    rootLabels->addChild(label);
    rootLabels->addChild(label2);
    label->setPos(0, 0, 0);
    label2->setPos(0, -20, 0);
    label->m_fontColor.set(1.0, 1.0, 1.0);
    label2->m_fontColor.set(1.0, 1.0, 1.0);

    // main haptic simulation loop
    while(simulationRunning)
    {
        float deltaTime = pClock.getCurrentTimeSeconds() - lastTime;
        lastTime = pClock.getCurrentTimeSeconds();

        cVector3d equilibrium (0.0, 0.0, 0.0);
        cVector3d pos0 (0.0, 0.0, 0.0), pos1 (0.0, 0.0, 0.0);

        // for each device
        int i=0;
        while (i < numHapticDevices)
        {
            // read position of haptic device
            cVector3d newPosition;
            hapticDevices[i]->getPosition(newPosition);
            newPosition = deviceToWorld(newPosition, i);

            ((i == 0)? pos0 : pos1) = newPosition;

            // read orientation of haptic device
            cMatrix3d newRotation;
            hapticDevices[i]->getRotation(newRotation);

            // update position and orientation of cursor
            cursors[i]->setPos(newPosition);
            cursors[i]->setRot(newRotation);

            // read linear velocity from device
            cVector3d linearVelocity;
            hapticDevices[i]->getLinearVelocity(linearVelocity);

            // update arrow
//            velocityVectors[i]->m_pointA = newPosition;
//            velocityVectors[i]->m_pointB = cAdd(newPosition, linearVelocity);

            // read user button status
            bool buttonStatus;
            hapticDevices[i]->getUserSwitch(0, buttonStatus);

            // adjustthe  color of the cursor according to the status of
            // the user switch (ON = TRUE / OFF = FALSE)
            if (i == 0)
                cursors[i]->m_material = matCursor2;
            else
                cursors[i]->m_material = matCursor1;

            // increment counter
            i++;
        }

        double f0, f1;
        f0 = pos0.length();
        f1 = pos1.length();

        f0 = f0/(f0 + f1);
        f1 = 1.0 - f0;

        equilibrium = pos1 + (pos0 - pos1)*f0;

        // Update the position of the sun
        cVector3d dir = pos1 - pos0;
        double dist = dir.length();
        dir.normalize();

        double vibrationSpeed = 20.0;
        double vibrationAmount = 0.02;

        //sun->setPos(sun->getPos()*(1.0 - deltaTime*speed) + equilibrium*(deltaTime*speed));
        //timeV += deltaTime*vibrationSpeed*(0.7 - cAbs(f0 - 0.5)*2.0);
        sun->setPos(equilibrium /*+ vibrationAmount*dir*cSinRad(timeV)*/);

        // Update logic
        if (!calibrationFinished) {
            label->m_string = "Calibrating, please move the haptic devices around in order to determine their limitations in movement.";
            label2->m_string = "Press 'c' to finish calibration.";

            if (sun->getPos().x < min.x)
                min.x = sun->getPos().x;
            if (sun->getPos().y < min.y)
                min.y = sun->getPos().y;
            if (sun->getPos().z < min.z)
                min.z = sun->getPos().z;

            if (sun->getPos().x > max.x)
                max.x = sun->getPos().x;
            if (sun->getPos().y > max.y)
//.........这里部分代码省略.........

int main(int argc, char* argv[])
{
    //-----------------------------------------------------------------------
    // INITIALIZATION
    //-----------------------------------------------------------------------

    printf ("\n");
    printf ("-----------------------------------\n");
    printf ("Be God\n");
    printf ("DH2660 Haptics\n");
    printf ("-----------------------------------\n");
    printf ("\n\n");

    // parse first arg to try and locate resources
    resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);

    pClock.start(true);
    lastTime = pClock.getCurrentTimeSeconds();

    //-----------------------------------------------------------------------
    // 3D - SCENEGRAPH
    //-----------------------------------------------------------------------

    // create a new world.
    world = new cWorld();

    // set the background color of the environment
    // the color is defined by its (R,G,B) components.
    world->setBackgroundColor(0.25, 0.55, 0.8);

    // create a camera and insert it into the virtual world
    camera = new cCamera(world);
    world->addChild(camera);

    // position and oriente the camera
    camera->set( cVector3d (-1.0, 0.0, 0.3),    // camera position (eye)
                 cVector3d (0.0, 0.0, 0.0),    // lookat position (target)
                 cVector3d (0.0, 0.0, 1.0));   // direction of the "up" vector

    // set the near and far clipping planes of the camera
    // anything in front/behind these clipping planes will not be rendered
    camera->setClippingPlanes(0.01, 10.0);
    camera->enableMultipassTransparency(true);

    // create a light source and attach it to the camera
    light = new cLight(world);
    camera->addChild(light);                   // attach light to camera
    light->setEnabled(true);                   // enable light source
    light->setPos(cVector3d( 2.0, 0.5, 1.0));  // position the light source
    light->setDir(cVector3d(-2.0, 0.5, 1.0));  // define the direction of the light beam


    //-----------------------------------------------------------------------
    // HAPTIC DEVICES / TOOLS
    //-----------------------------------------------------------------------

    // create a haptic device handler
    handler = new cHapticDeviceHandler();

    // read the number of haptic devices currently connected to the computer
    numHapticDevices = handler->getNumDevices();

    // Check if two devices are plugged in.
    if(numHapticDevices != 2) {
        std::cout << "Application shut down: Two falcon devices are needed." << std::endl;
        exit(0);
    }

    // create a node on which we will attach small labels that display the
    // position of each haptic device
    rootLabels = new cGenericObject();
    camera->m_front_2Dscene.addChild(rootLabels);

    // create a small label as title
//    cLabel* titleLabel = new cLabel();
//    rootLabels->addChild(titleLabel);

//    // define its position, color and string message
//    titleLabel->setPos(0, 30, 0);
//    titleLabel->m_fontColor.set(1.0, 1.0, 1.0);
//    titleLabel->m_string = "Haptic Device Pos [mm]:";

    // for each available haptic device, create a 3D cursor
    // and a small line to show velocity
    int i = 0;
    while (i < numHapticDevices)
    {
        // get a handle to the next haptic device
        cGenericHapticDevice* newHapticDevice;
        handler->getDevice(newHapticDevice, i);

        // open connection to haptic device
        newHapticDevice->open();

		// initialize haptic device
        newHapticDevice->initialize();

        // store the handle in the haptic device table
        hapticDevices[i] = newHapticDevice;

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

void hapticsLoop(void* a_pUserData)
{
    // read the position of the haptic device
    cursor->updatePose();

    // compute forces between the cursor and the environment
    cursor->computeForces();

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

    // read the time increment in seconds
    double increment = g_clock.getCurrentTime() / 1000000.0;

    // restart the simulation clock
    g_clock.initialize();
    g_clock.start();

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

    // compute velocity of cursor;
    timeCounter = timeCounter + increment;
    if (timeCounter > 0.01)
    {
        cursorVel = (cursorPos - lastCursorPos) / timeCounter;
        lastCursorPos = cursorPos;
        timeCounter = 0;
    }

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

    // compute the velocity of the sphere at the contact point
    cVector3d contactVel = cVector3d(0.0, 0.0, 0.0);
    if (rotVelocity.length() > CHAI_SMALL)
    {
        cVector3d projection = cProjectPointOnLine(cursorPos, objectPos, rotVelocity);
        cVector3d vpc = cursorPos - projection;
        if (vpc.length() > CHAI_SMALL)
        {
            contactVel = vpc.length() * rotVelocity.length() * cNormalize(cCross(rotVelocity, vpc));
        }
    }

    // get the last force applied to the cursor in global coordinates
    cVector3d cursorForce = cursor->m_lastComputedGlobalForce;

    // compute friction force
    cVector3d friction = -40.0 * cursorForce.length() * cProjectPointOnPlane((cursorVel - contactVel), cVector3d(0.0, 0.0, 0.0), (cursorPos - objectPos));

    // add friction force to cursor
    cursor->m_lastComputedGlobalForce.add(friction);

    // update rotational velocity
    if (friction.length() > CHAI_SMALL)
    {
        rotVelocity.add( cMul(-10.0 * increment, cCross(cSub(cursorPos, objectPos), friction)));
    }

    // add some damping...
    //rotVelocity.mul(1.0 - increment);
    
    // compute the next rotation of the torus
    if (rotVelocity.length() > CHAI_SMALL)
    {
        object->rotate(cNormalize(rotVelocity), increment * rotVelocity.length());
    }

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