无奈，想问问题，但是问题页的字数有限，所以在此贴上代码！

#include<fstream>
#include"time.h"
#include "chai3d.h"
//---------------------------------------------------------------------------
using namespace chai3d;
using namespace std;
//---------------------------------------------------------------------------
#include "CODE.h"
//---------------------------------------------------------------------------
#ifndef MACOSX
#include "GL/glut.h"
#else
#include "GLUT/glut.h"
#endif
//---------------------------------------------------------------------------

cStereoMode stereoMode = C_STEREO_DISABLED;

// fullscreen mode
long mytime1;
long mytime2=clock();

bool zero=true; //是否过原点
bool jilu=false;//是否记录

bool fullscreen = false;
bool replace1=false;
int count1=0;
bool first1=false;
bool second1=false;
bool third1=false;

double count2 = 0; // 记个数
double reactTime =0; // 反应时间
double railroad =0; // 轨迹差值
double totalcount=0; // 更新总数
bool done = false; //任务完成
int time1 = 0;

//计时
time_t globaltime = 0;
struct tm *globaltmtime = NULL;
time_t tmpTime = 0;
struct tm * tmpTimep = NULL;

//---------------------------------------------------------------------------
// CHAI3D VARIABLES
//---------------------------------------------------------------------------

// a world that contains all objects of the virtual environment
cWorld* world;

// a camera to render the world in the window display
cCamera* camera;

// a light source to illuminate the objects in the world
cSpotLight *light;

// a haptic device handler
cHapticDeviceHandler* handler;

// a global variable to store the position [m] of the haptic device
cVector3d hapticDevicePosition;

// a global variable to store the velocity [m/s] of the haptic device
cVector3d hapticDeviceVelocity;

// a line representing the velocity vector of the haptic device
cShapeLine* velocity;

// a pointer to the current haptic device
shared_ptr<cGenericHapticDevice> hapticDevice;

// workspace scale factor
double workspaceScaleFactor = 30.0;

// a label to display the rate [Hz] at which the simulation is running
cLabel* labelHapticRate;

cLabel* labelHapticRate2;

cLabel* labelHapticRate3;

cLabel* labelreacttime;
cLabel* labeltotalcount;
cLabel* labelrailroad;

cShapeLine* line1;
cShapeLine* line2;
cShapeLine* line3;
cShapeLine* line4;

cShapeLine* line5;
cShapeLine* line6;
cShapeLine* line7;
cShapeLine* line8;

cShapeLine* line9;
cShapeLine* line10;
cShapeLine* line11;
cShapeLine* line12;

cShapeLine* line13;
cShapeLine* line14;
cShapeLine* line15;


// stiffness of virtual spring
double linG ;
double angG ;
double linStiffness = 800;
double angStiffness = 30;


//---------------------------------------------------------------------------
// ODE MODULE VARIABLES
//---------------------------------------------------------------------------

// ODE world
cODEWorld* ODEWorld;

// ODE objects
cODEGenericBody* ODEBody0;
cODEGenericBody* ODETool;

cODEGenericBody* ODEGPlane0;
cODEGenericBody* ODEGPlane1;
cODEGenericBody* ODEGPlane2;
cODEGenericBody* ODEGPlane3;
cODEGenericBody* ODEGPlane4;
cODEGenericBody* ODEGPlane5;


//---------------------------------------------------------------------------
// GENERAL VARIABLES
//---------------------------------------------------------------------------

// flag to indicate if the haptic simulation currently running
bool simulationRunning = false;

// flag to indicate if the haptic simulation has terminated
bool simulationFinished = true;

// frequency counter to measure the simulation haptic rate
cFrequencyCounter frequencyCounter;

// information about computer screen and GLUT display window
int screenW;
int screenH;
int windowW;
int windowH;
int windowPosX;
int windowPosY;



//---------------------------------------------------------------------------
// DECLARED FUNCTIONS
//---------------------------------------------------------------------------

// callback when the window display is resized
void resizeWindow(int w, int h);

// callback when a key is pressed
void keySelect(unsigned char key, int x, int y);

// callback to render graphic scene
void updateGraphics(void);

// callback of GLUT timer
void graphicsTimer(int data);

// function that closes the application
void close(void);

// main haptics simulation loop
void updateHaptics(void);

ofstream myFile1;
ofstream myFile2;
ofstream myFile3;
ofstream myFile4;
           


//===========================================================================
/*
    DEMO:    ODE_cubic.cpp

    This example illustrates the use of the ODE framework for simulating
    haptic interaction with dynamic bodies. In this scene we create 3
	cubic meshes that we individually attach to ODE bodies. Haptic interactions
	are computer by using the finger-proxy haptic model and forces are
	propagated to the ODE representation.
 */
//===========================================================================

int main(int argc, char* argv[])
{
    //-----------------------------------------------------------------------
    // INITIALIZATION
    //------------
	globaltime = time(NULL);
	globaltmtime = localtime(&globaltime);
			    
    myFile1.open("向上运动的轨迹.txt",ios::app); //myfile.bat是存放数据的文件名
	myFile2.open("完成任务时间.txt",  ios::app);   //myfile.bat是存放数据的文件名
	myFile3.open("左下运动的轨迹.txt",ios::app); //myfile.bat是存放数据的文件名
	myFile4.open("右下运动的轨迹.txt",ios::app); //myfile.bat是存放数据的文件名
         
   // cout << endl;
    cout << "-----------------------------------" << endl;
    cout << "CHAI3D" << endl;
    cout << "[x] - Exit application\n" << endl;
    //-----------------------------------------------------------------------
    // OPEN GL - WINDOW DISPLAY
    //-----------------------------------------------------------------------
    // initialize GLUT
    glutInit(&argc, argv);
    // retrieve  resolution of computer display and position window accordingly
    screenW = glutGet(GLUT_SCREEN_WIDTH);
    screenH = glutGet(GLUT_SCREEN_HEIGHT);
    windowW = 0.8 * screenH;
    windowH = 0.5 * screenH;
    windowPosY = (screenH - windowH) / 2;
    windowPosX = windowPosY; 

    // initialize the OpenGL GLUT window
    glutInitWindowPosition(windowPosX, windowPosY);
    glutInitWindowSize(windowW, windowH);
    if (stereoMode == C_STEREO_ACTIVE)
    {
        glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE | GLUT_STEREO);
    }
    else
    {
        glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
    }

    // create display context and initialize GLEW library
    glutCreateWindow(argv[0]);
    glewInit();

    // setup GLUT options
    glutDisplayFunc(updateGraphics);
    glutKeyboardFunc(keySelect);
    glutReshapeFunc(resizeWindow);
    glutSetWindowTitle("虚拟康复实验");

    // set fullscreen mode
    if (fullscreen)
    {
        glutFullScreen();
    }

	first1=true;
    //-----------------------------------------------------------------------
    // 3D - SCENEGRAPH
    //-----------------------------------------------------------------------
    // create a new world.
    world = new cWorld();

    // set the background color of the environment
    world->m_backgroundColor.setWhite();

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

    // position and orient the camera
    camera->set( cVector3d (3.0, 0.0, 2.5),    // camera position (eye)
                 cVector3d (0.0, 0.0,-0.5),    // 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
    camera->setClippingPlanes(0.01, 10.0);

    // set stereo mode
    camera->setStereoMode(stereoMode);

    // set stereo eye separation and focal length (applies only if stereo is enabled)
    camera->setStereoEyeSeparation(0.02);
    camera->setStereoFocalLength(2.0);

    // enable shadow casting
    camera->setUseShadowCasting(true);

    // create a light source
    light = new cSpotLight(world);

    // attach light to camera
    world->addChild(light);    

    // enable light source
    light->setEnabled(true);                   

    // position the light source
    light->setLocalPos( 0, 0, 1.2);             

    // define the direction of the light beam
    light->setDir(0,0,-1.0);             

    // set uniform concentration level of light 
    light->setSpotExponent(0.0);

    // enable this light source to generate shadows
    light->setShadowMapEnabled(true);

    // set the resolution of the shadow map
    light->m_shadowMap->setResolutionLow();
    //light->m_shadowMap->setResolutionMedium();

    // set light cone half angle
    light->setCutOffAngleDeg(45);
    //-----------------------------------------------------------------------
    // HAPTIC DEVICES / TOOLS
    //-----------------------------------------------------------------------
    // create a haptic device handler
    handler = new cHapticDeviceHandler();

    // get access to the first available haptic device
    handler->getDevice(hapticDevice, 0);

    // retrieve information about the current haptic device
    cHapticDeviceInfo hapticDeviceInfo = hapticDevice->getSpecifications();

    //--------------------------------------------------------------------------
    // WIDGETS   标志区域、划红线
    //--------------------------------------------------------------------------
    // create a font
    cFont *font = NEW_CFONTCALIBRI20();

    // create a label to display the haptic rate of the simulation
    labelHapticRate = new cLabel(font);
    labelHapticRate->m_fontColor.setBlack();
    camera->m_frontLayer->addChild(labelHapticRate);

	//计时
	labelHapticRate2 = new cLabel(font);
    labelHapticRate2->m_fontColor.setBlack();
    camera->m_frontLayer->addChild(labelHapticRate2);

	//计数
	labelHapticRate3 = new cLabel(font);
    labelHapticRate3->m_fontColor.setBlack();
    camera->m_frontLayer->addChild(labelHapticRate3);

	//反应时间
	//labelreacttime = new cLabel(font);
    //labelreacttime->m_fontColor.setBlack();
    //camera->m_frontLayer->addChild(labelreacttime);

	//总数
	labeltotalcount = new cLabel(font);
    labeltotalcount->m_fontColor.setBlack();
    camera->m_frontLayer->addChild(labeltotalcount);
	//轨迹
	labelrailroad= new cLabel(font);
    labelrailroad->m_fontColor.setBlack();
    camera->m_frontLayer->addChild(labelrailroad);

	line1 = new cShapeLine(cVector3d(-0.65,-0.15, -0.9),cVector3d(-0.65,0.15, -0.9));
    world->addChild(line1);
	line1->m_colorPointA.set(1.0, 0.0, 0.0);
    line1->m_colorPointB.set(1.0, 0.0, 0.0);

	line2 = new cShapeLine(cVector3d(-0.95, -0.15, -0.9),cVector3d(-0.95,0.15, -0.9));
    world->addChild(line2);
	line2->m_colorPointA.set(1.0, 0.0, 0.0);
    line2->m_colorPointB.set(1.0, 0.0, 0.0);

	line3 = new cShapeLine(cVector3d(-0.65,-0.15,-0.9),cVector3d(-0.95,-0.15, -0.9));
    world->addChild(line3);
	line3->m_colorPointA.set(1.0, 0.0, 0.0);
    line3->m_colorPointB.set(1.0, 0.0, 0.0);

	line4 = new cShapeLine(cVector3d(-0.95,0.15,-0.9),cVector3d(-0.65, 0.15, -0.9));
    world->addChild(line4);
	line4->m_colorPointA.set(1.0, 0.0, 0.0);
    line4->m_colorPointB.set(1.0, 0.0, 0.0);

	line5 = new cShapeLine(cVector3d(0.25,-0.8428, -0.9),cVector3d(0.25,-0.5428, -0.9));
    world->addChild(line5);
	line5->m_colorPointA.set(1.0, 0.0, 0.0);
    line5->m_colorPointB.set(1.0, 0.0, 0.0);

	line6 = new cShapeLine(cVector3d(0.25, -0.8428, -0.9),cVector3d(0.55,-0.8428, -0.9));
    world->addChild(line6);
	line6->m_colorPointA.set(1.0, 0.0, 0.0);
    line6->m_colorPointB.set(1.0, 0.0, 0.0);

	line7 = new cShapeLine(cVector3d(0.55,-0.8428,-0.9),cVector3d(0.55,-0.5428, -0.9));
    world->addChild(line7);
	line7->m_colorPointA.set(1.0, 0.0, 0.0);
    line7->m_colorPointB.set(1.0, 0.0, 0.0);

	line8 = new cShapeLine(cVector3d(0.55,-0.5428,-0.9),cVector3d(0.25,-0.5428, -0.9));
    world->addChild(line8);
	line8->m_colorPointA.set(1.0, 0.0, 0.0);
    line8->m_colorPointB.set(1.0, 0.0, 0.0);

	line9 = new cShapeLine(cVector3d(0.25,0.5428, -0.9),cVector3d(0.25,0.8428, -0.9));
    world->addChild(line9);
	line9->m_colorPointA.set(1.0, 0.0, 0.0);
    line9->m_colorPointB.set(1.0, 0.0, 0.0);

	line10 = new cShapeLine(cVector3d(0.25,0.8428, -0.9),cVector3d(0.55,0.8428, -0.9));
    world->addChild(line10);
	line10->m_colorPointA.set(1.0, 0.0, 0.0);
    line10->m_colorPointB.set(1.0, 0.0, 0.0);

	line11 = new cShapeLine(cVector3d(0.55,0.8428,-0.9),cVector3d(0.55,0.5428, -0.9));
    world->addChild(line11);
	line11->m_colorPointA.set(1.0, 0.0, 0.0);
    line11->m_colorPointB.set(1.0, 0.0, 0.0);

	line12 = new cShapeLine(cVector3d(0.55,0.5428,-0.9),cVector3d(0.25,0.5428, -0.9));
    world->addChild(line12);
	line12->m_colorPointA.set(1.0, 0.0, 0.0);
    line12->m_colorPointB.set(1.0, 0.0, 0.0);

	line13 = new cShapeLine(cVector3d(-0.8,0.0,-0.9),cVector3d(0.0,0.0, -0.9));
    world->addChild(line13);
	line13->m_colorPointA.set(1.0, 0.0, 0.0);
    line13->m_colorPointB.set(1.0, 0.0, 0.0);

	line14 = new cShapeLine(cVector3d(0.0,0.0,-0.9),cVector3d(0.4,0.6928, -0.9));
    world->addChild(line14);
	line14->m_colorPointA.set(1.0, 0.0, 0.0);
    line14->m_colorPointB.set(1.0, 0.0, 0.0);

	line15 = new cShapeLine(cVector3d(0.0,0.0,-0.9),cVector3d(0.4,-0.6928,-0.9));
    world->addChild(line15);
	line15->m_colorPointA.set(1.0, 0.0, 0.0);
    line15->m_colorPointB.set(1.0, 0.0, 0.0);

    //////////////////////////////////////////////////////////////////////////
    // ODE WORLD
    //////////////////////////////////////////////////////////////////////////
    // stiffness properties
    double maxStiffness	= hapticDeviceInfo.m_maxLinearStiffness / workspaceScaleFactor;
    // create an ODE world to simulate dynamic bodies
    ODEWorld = new cODEWorld(world);

    // add ODE world as a node inside world
    world->addChild(ODEWorld);

    // set some gravity
    ODEWorld->setGravity(cVector3d(0.0, 0.0, -10));

    // define damping properties
    ODEWorld->setAngularDamping(0.00002);  //0.00002  //是否可以调试下！！
    ODEWorld->setLinearDamping(0.00002); //0.00002

    // create a new ODE object that is automatically added to the ODE world
    ODEBody0 = new cODEGenericBody(ODEWorld);

    // create a virtual mesh  that will be used for the geometry representation of the dynamic body
    cMesh* object0 = new cMesh();
    // create a cube mesh
    double size = 0.15;
    cCreateBox(object0, size, size, size);

    // define some material properties for each cube
    cMaterial mat0;
    mat0.setRedIndian();
	mat0.setDynamicFriction(1.0);  //0.8
    mat0.setStaticFriction(1.0);  //0.8

    object0->setMaterial(mat0);
    // add mesh to ODE object
    ODEBody0->setImageModel(object0);

    // create a dynamic model of the ODE object. Here we decide to use a box just like
    // the object mesh we just defined
    ODEBody0->createDynamicBox(size, size, size);

    // define some mass properties for each cube
    ODEBody0->setMass(5.0);

    // set position of each cube
    ODEBody0->setLocalPos(-0.8,0.0,-0.925);

    // rotate central cube 45 degrees around z-axis
    ODEBody0->rotateAboutGlobalAxisDeg(0,0,1, 45);

    //////////////////////////////////////////////////////////////////////////
    // TOOL
    //////////////////////////////////////////////////////////////////////////
    // create a virtual tool
    ODETool = new cODEGenericBody(ODEWorld);

    cMesh* objectTool = new cMesh();
    cCreateBox(objectTool, size, size, size);

    // define some material properties for each cube
    cMaterial matTool;
    matTool.m_ambient.set(0.4, 0.4, 0.4);
    matTool.m_diffuse.set(0.8, 0.8, 0.8);
    matTool.m_specular.set(1.0, 1.0, 1.0);
    matTool.setDynamicFriction(0.8);  //0.8
    matTool.setStaticFriction(0.8);  //0.8
    objectTool->setMaterial(matTool);

    // add mesh to ODE object
    ODETool->setImageModel(objectTool);
    ODETool->createDynamicBox(size, size, size);

    // define some mass properties for each cube
    ODETool->setMass(0.01);    //手柄的质量
	ODETool->setLocalPos(0.0,0.0,-0.8);
    dBodySetAngularDamping(ODETool->m_ode_body, 0.02);//0.02
    dBodySetLinearDamping(ODETool->m_ode_body, 0.02);//0.02

	 //////////////////////////////////////////////////////////////////////////
    // 6 ODE INVISIBLE WALLS
    //////////////////////////////////////////////////////////////////////////
    // we create 6 static walls to contains the 3 cubes within a limited workspace

    ODEGPlane0 = new cODEGenericBody(ODEWorld);
    ODEGPlane1 = new cODEGenericBody(ODEWorld);
    ODEGPlane2 = new cODEGenericBody(ODEWorld);
    ODEGPlane3 = new cODEGenericBody(ODEWorld);
    ODEGPlane4 = new cODEGenericBody(ODEWorld);
    ODEGPlane5 = new cODEGenericBody(ODEWorld);

    double width = 1.0;

    ODEGPlane0->createStaticPlane(cVector3d(0.0, 0.0,  2.0 *width), cVector3d(0.0, 0.0 ,-1.0));
    ODEGPlane1->createStaticPlane(cVector3d(0.0, 0.0, -width), cVector3d(0.0, 0.0 , 1.0));
    ODEGPlane2->createStaticPlane(cVector3d(0.0,  1.5*width, 0.0), cVector3d(0.0,-1.0, 0.0));
    ODEGPlane3->createStaticPlane(cVector3d(0.0, -1.5*width, 0.0), cVector3d(0.0, 1.0, 0.0));
    ODEGPlane4->createStaticPlane(cVector3d( 1.5 * width, 0.0, 0.0), cVector3d(-1.0,0.0, 0.0));
    ODEGPlane5->createStaticPlane(cVector3d( -1.2*width, 0.0, 0.0), cVector3d( 1.0,0.0, 0.0));


    //////////////////////////////////////////////////////////////////////////
    // GROUND
    //////////////////////////////////////////////////////////////////////////
    // create a mesh that represents the ground
    cMesh* ground = new cMesh();
    world->addChild(ground);

    // create a plane
    double groundSize = 3.0;
    cCreatePlane(ground, groundSize, groundSize);

    // position ground in world where the invisible ODE plane is located (ODEGPlane1)
    ground->setLocalPos(0.0, 0.0, -1.0);

    // define some material properties and apply to mesh
    cMaterial matGround;
	matGround.setStiffness(0);
	matGround.setDynamicFriction(1.0);
    matGround.setStaticFriction(1.0);
    //matGround.setStiffness(0.3 * maxStiffness);//0.3
    //matGround.setDynamicFriction(0.8); //0.8
    //matGround.setStaticFriction(0.2); //0.2
	matGround.setBlue();
    matGround.m_emission.setGrayLevel(0.3);
    ground->setMaterial(matGround);
    //-----------------------------------------------------------------------
    // START SIMULATION
    //-----------------------------------------------------------------------
    // simulation in now running
    simulationRunning = true;

    // create a thread which starts the main haptics rendering loop
    cThread* hapticsThread = new cThread();
    hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);

    // start the main graphics rendering loop
    glutTimerFunc(50, graphicsTimer, 0);
    glutMainLoop();

    // close everything
    close();

    // exit
    return (0);
 }

//---------------------------------------------------------------------------
void resizeWindow(int w, int h)
{
    windowW = w;
    windowH = h;
}

//---------------------------------------------------------------------------

void keySelect(unsigned char key, int x, int y)
{
    // escape key
    if ((key == 27) || (key == 'x'))
    {
        // close everything
        close();

        // exit application
        exit(0);
    }
    // option f: toggle fullscreen
    if (key == 'f')
    {
        if (fullscreen)
        {
            windowPosX = glutGet(GLUT_INIT_WINDOW_X);
            windowPosY = glutGet(GLUT_INIT_WINDOW_Y);
            windowW = glutGet(GLUT_INIT_WINDOW_WIDTH);
            windowH = glutGet(GLUT_INIT_WINDOW_HEIGHT);
            glutPositionWindow(windowPosX, windowPosY);
            glutReshapeWindow(windowW, windowH);
            fullscreen = false;
        }
        else
        {
            glutFullScreen();
            fullscreen = true;
        }
    }
}

//---------------------------------------------------------------------------

void close(void)
{
    // stop the simulation
    simulationRunning = false;

    // wait for graphics and haptics loops to terminate
    while (!simulationFinished) { cSleepMs(100); }

    // close haptic device
    hapticDevice->close();
}

//---------------------------------------------------------------------------

void graphicsTimer(int data)
{
    // inform the GLUT window to call updateGraphics again (next frame)
    if (simulationRunning)
    {
        glutPostRedisplay();
    }

    glutTimerFunc(50, graphicsTimer, 0);
}

//---------------------------------------------------------------------------

void updateGraphics(void)
{
    /////////////////////////////////////////////////////////////////////
    // UPDATE WIDGETS
    /////////////////////////////////////////////////////////////////////

    // update haptic rate label
    labelHapticRate->setString ("haptic rate: "+cStr(frequencyCounter.getFrequency(), 0) + " [Hz]");

    // update position of label
    labelHapticRate->setLocalPos((int)(0.5 * (windowW - labelHapticRate->getWidth())), 15);

	char tmpchar[100]= {0};
	time_t tmptime = tmpTime - globaltime;
	struct tm *tmpP = localtime(&tmptime);
	sprintf(tmpchar, "Time : %d min : %d sec", tmpP->tm_min, tmpP->tm_sec);

	labelHapticRate2->setString (tmpchar);
    labelHapticRate2->setLocalPos(10, windowH - 50);


	labelHapticRate3->setString("Count: "+ cStr(count2,0));
	labelHapticRate3->setLocalPos(10, windowH - 70);

	//labelreacttime->setString("ReactTime: "+ cStr(reactTime,0));
	//labelreacttime->setLocalPos(10, windowH - 90);

	labelrailroad->setString("Road: "+ cStr(railroad,5));
	labelrailroad->setLocalPos(10, windowH - 90);

	labeltotalcount->setString("TotalCount: "+ cStr(totalcount,0));
	labeltotalcount->setLocalPos(10, windowH - 110);

    /////////////////////////////////////////////////////////////////////
    // RENDER SCENE
    /////////////////////////////////////////////////////////////////////

    // render world
    camera->renderView(windowW, windowH);

    // swap buffers
    glutSwapBuffers();

    // check for any OpenGL errors
    GLenum err = glGetError();
    if (err != GL_NO_ERROR) printf("Error:  %s\n", gluErrorString(err));
}

//---------------------------------------------------------------------------

void updateHaptics(void)
{
	// start haptic device
	
	hapticDevice->open();

	// simulation clock
    cPrecisionClock simClock;
    simClock.start(true);

	double kp, kr;
	kp = 0.0;
	kr = 0.0;

	cMatrix3d prevRotTool;
	prevRotTool.identity();

    // main haptic simulation loop
    while(simulationRunning)
    {
        // update frequency counter
        frequencyCounter.signal(1);

		tmpTime = time(NULL);
		tmpTimep = localtime(&tmpTime);

        // retrieve simulation time and compute next interval
        double nextSimInterval = 0.0005;//cClamp(time, 0.00001, 0.0002);
		count1=count1+1;
		if (count1==1000)
		{
			replace1=true;
			count1=0;
		}
        
		// reset clock
        simClock.reset();
        simClock.start();

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

        // update position and orientation of tool
		cVector3d posDevice;
		cMatrix3d rotDevice;

		hapticDevice->getPosition(posDevice);
		hapticDevice->getRotation(rotDevice);
		// scale position of device
		posDevice.mul(workspaceScaleFactor);
		// read position of tool
		cVector3d posTool = ODETool->getLocalPos();
		cMatrix3d rotTool = ODETool->getLocalRot();

		cVector3d odebody = ODEBody0->getGlobalPos();
		double ballx=odebody.x();
		double bally=odebody.y();
	
		if (((sqrt(posTool.x()*posTool.x()+posTool.y()*posTool.y())) < 0.2))
		{
			zero=true;
		}
		if (zero==true&&replace1==false)
		{
			jilu=true;
				
		}
		if (jilu)
		{

		   if (posTool.x() < 0)
		   {
				railroad=(posTool.y() - 0.0)*1.0;
				myFile1 << (posTool.y() - 0.0 ) << endl; //message是程序中处理的数据  outfile<<ODEBody0->getGlobalPos()<<endl;tool->m_lastComputedGlobalForce.length();outfile<<clock.getCurrentTimeSeconds()<<endl;
	    	}
		else if (posTool.y() < 0)
		{
				railroad=(-1.0*posTool.y() - (7*posTool.x()/3.0) );
				myFile3 << (-1.0*posTool.y() - (7*posTool.x()/3.0) )<< endl;
		}
		else
		{
				railroad=(posTool.y() - (7*posTool.x()/3.0) );
				myFile4 << (posTool.y() - (7*posTool.x()/3.0) )<< endl; 
		}
		}
		
	if ( replace1 || ((sqrt((ballx -posTool.x())*(ballx-posTool.x())+(bally - posTool.y())*(bally-posTool.y())) < 0.2)))
	  { 
		  if(((sqrt((ballx -posTool.x())*(ballx-posTool.x())+(bally - posTool.y())*(bally-posTool.y())) < 0.2)))
		  {
			 //记录下是否完成标志,反应时间
			   mytime1=mytime2;
			   mytime2=clock();
			   reactTime=mytime2-mytime1;
			   if (posTool.x() < 0)
		       {
				myFile1 << "正确" << endl; //message是程序中处理的数据  outfile<<ODEBody0->getGlobalPos()<<endl;tool->m_lastComputedGlobalForce.length();outfile<<clock.getCurrentTimeSeconds()<<endl;
	    	   }
		       else if (posTool.y() < 0)
		      {
				myFile3 <<"正确"<< endl;
		      }
		     else
		     {
				myFile4 << "正确"<< endl; 
		      }
			  myFile2 << mytime2-mytime1<< endl; //message是程序中处理的数据  outfile<<ODEBody0->getGlobalPos()<<endl;tool->m_lastComputedGlobalForce.length();outfile<<clock.getCurrentTimeSeconds()<<endl;
		  }
		  totalcount++;
		  if((!replace1)&&zero)
		  {
	        count2++ ;
		  }
		  if (replace1)
		  {
			  if (posTool.x() < 0)
		       {
				myFile1 << "错误" << endl; //message是程序中处理的数据  outfile<<ODEBody0->getGlobalPos()<<endl;tool->m_lastComputedGlobalForce.length();outfile<<clock.getCurrentTimeSeconds()<<endl;
	    	   }
		       else if (posTool.y() < 0)
		      {
				myFile3 <<"错误"<< endl;
		      }
		       else
		      {
				myFile4 << "错误"<< endl; 
		      }
		  }
     	  long mytime=clock();
		  int tt=mytime%2;
		  float zuobiao = -0.925;    //用于修改正方体B更新位置，尤其是Z轴上的数值！
	      if (first1)
		  {
			if (tt==0)
			{	
				first1=false;
				second1=true;
				third1=false;
				ODEBody0->setLocalPos(-0.8,0.0,zuobiao);
			}
			else if(tt==1)
			{
				first1=false;
				second1=false;
				third1=true;
				ODEBody0->setLocalPos(0.4,-0.6928,zuobiao);
			}
		}
		else if (second1)
		{
			if (tt==0)
			{
				first1=false;
				second1=false;
				third1=true;
				ODEBody0->setLocalPos(0.4,-0.6928,zuobiao);
			}
			else if(tt==1)
			{
				first1=true;
				second1=false;
				third1=false;
				ODEBody0->setLocalPos(0.4,0.6928,zuobiao);
			}
		}
		else if (third1)
		{
			if (tt==0)
			{ 
				first1=true;
				second1=false;
				third1=false;
				ODEBody0->setLocalPos(0.4,0.6928,zuobiao);
			}
			else if (tt==1)
			{
				
				first1=false;
				second1=true;
				third1=false;
				ODEBody0->setLocalPos(-0.8,0.0,zuobiao);
			}
		 }
		   replace1 = false;
		   count1 = 0;
		   zero=false;
		   jilu =false;	     
	   	}

		// compute position and angular error between tool and haptic device
		cVector3d deltaPos = (posDevice - posTool);
		cMatrix3d deltaRot = cMul(cTranspose(rotTool), rotDevice);
		double angle;
		cVector3d axis;
		deltaRot.toAxisAngle(axis, angle);	
		// compute force and torque to apply to tool
		cVector3d force, torque;
		force = linStiffness * deltaPos;
		ODETool->addExternalForce(force);

		torque = cMul((angStiffness * angle), axis);
		rotTool.mul(torque);	
		ODETool->addExternalTorque(torque);

		linG = 0.005;
		angG = 0.0005;
		//linG = 0.010;
		//angG = 0.001;
	
		// compute force and torque to apply to haptic device
		force = -linG * force;
		torque = -angG * torque;

        // send forces to device
		hapticDevice->setForceAndTorqueAndGripperForce(force, torque, 0.0);

        // update simulation
        ODEWorld->updateDynamics(nextSimInterval);  
    }
    // exit haptics thread
    simulationFinished = true;
}