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

最新推荐文章于 2024-09-09 22:12:12 发布
最新推荐文章于 2024-09-09 22:12:12 发布
阅读量661 收藏
点赞数
分类专栏： 问题页的代码数有限！ 文章标签： c++ OpenGL
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/baidu_23319009/article/details/49783243
版权 
#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;
}

味全好喝
关注
专栏目录
DirectX 11 学习笔记-Part2-4【Cubemap/贴图置换/阴影/屏幕空间环境光遮蔽】【原理篇】
Zealot_Alie的博客
01-27 1483
代码已上传至GitHub，这部分对应的项目为NormalDisplacementSsao。运行项目时可以使用数字键1-3切换渲染的shader，另外按下z键可以看到SSAO生成的遮罩贴图。 这部分内容较多，所以以介绍原理为主。另外在阅读源码时，为了剔除枝干让整体的代码更加一目了然，我用了模板和SFINAE（主要在SceneObject.h），如果看不懂的话也没有关系，不重要，关键是了解这些技术的...
数学建模优化模型简单例题_10次数学建模积累下的经验，希望能对你有所帮助！...
weixin_34108829的博客
12-30 7105
我，今年大四，经管学院(偏文科)，从大一上学期开始参加数学建模，前前后后参加了10次建模比赛，但是第10次才拿到了全国一等奖(有点丢人)，期间没有看过任何建模相关的课程或培训(绝对没有吹牛逼)，所分享的经验全部是我在一次次的建模过程中悟出来的，所以如果不适合勿喷。以下文章是我在自己公众号上的原创内容，虽然粉丝不多，但还是有四百多的阅读量，一定程度上得到了老师和学弟学妹的认可。所以在这里想跟其他学弟...
【cesium】可视域分析
weixin_39823367的博客
03-24 4627
// 创建相机 createLightCamera () { this.lightCamera = new Cesium.Camera(this.viewer.scene); this.lightCamera.position = this.viewPosition; // if (this.viewPositionEnd) { // let direction = Cesium.Cartesian3.normalize(Cesium.Cartesian3.subtract(this.viewPos
Three.js - 使用阴影效果 shadowMap 和 设置阴影分辨率
ithanmang
06-24 1万+
开启阴影效果需要渲染器支持，如果使用的WebGLRender 渲染器。 需要renderer.shadowMap.enabled = true;来开启渲染气阴影效果。 示例浏览地址：http://www.ithanmang.com/threejshome/20180624/01-shadow.html 可以看待阴影效果比较突出，并且阴影的分辨率也比较高 步骤 1 首先需要渲染器支持阴...
Cesium深入浅出之可视域分析
Helsing的专栏
09-05 1万+
引子 万众瞩目的可视域分析功能终于来了！上一篇做这个预告的时候，压根还没开始碰这块东西，稍微有一点小担心，万一弄不出来不就打脸了么。不过好在功夫不负有心人，赶紧把热乎的饭给您盛上。 预期效果 细心的小盆友一定发现了，怎么你的效果跟别人的不一样啊。嗯，这是因为我没开深度检测的缘故，开了之后地面都绿洼洼的，我嫌弃它难看！ 实现原理 ShadowMap 顾名思义，ShadowMap就是阴影贴图。看Cesium的API，有一句话“Do not construct this directly”，看来官
STM32CUBE使用中的小问题
qq_34735854的博客
10-08 836
目的 （1）串口使用 1： 在函数HAL_UART_MspInit中添加，开启接收中断 __HAL_UART_ENABLE_IT(&huart1,UART_IT_RXNE); //开启接收中断 2 ： 为支持printf函数功能，在USART.C中添加如下部分 typedef struct __FILE FILE; #ifdef __GNUC__ /* With GCC/RAIS...
Android百度面试一面问题,百度面试篇——百度的一面会遇到怎样的题？
weixin_35592186的博客
05-27 194
前言金三银四，这个春天，比冬天更寒冷，不是因为别的，而是当下严峻的就业形势，希望这篇文章能给各位读者严冬中送去一些温暖。本文将讲述一次面试百度的惨痛经历，包含我的一些思考。题目本文将按照顺序将题目分为三类，分别按照题目、答案、思考的顺序去书写。话不多说，正文开始。CSS类题目面试官：例如qq聊天界面(此处图省略)，左边一个头像右边一个三角，三角右边是聊天内容，如图：答：定位，三角形，xxxx。面试...
记录第一次上线(阿里云为例 springboot+vue)
qq_43567814的博客
02-29 659
记录第一次上线的过程 因为是个小白，又因为疫情的原因，并且阿里大大很大气的送了一个服务器，正好临近毕业毕业设计快要完成，就想着能够上线试试看，然后学着配置服务器，买了一个域名！后面就讲接下来的遇见的很多很多的错误 1: 首先领取了阿里大大的免费赠送的服务器嘿嘿，配置还好把，自己选了windows(傻了吧唧的)然后内存总共32G其他配置不多说了，都能再阿里云搜到 2: 之后兴冲冲的开始研究了，首先服...
简明python指南（预览版）
johngogogo的博客
03-21 1446
本文作者：NTFS 因为，全文比较长，没有经过完整的校对，纰漏难免。 当前最新版本的 pdf 在此 -> 关注公众号 “ 无知红 ” 回复 “简明python指南 ” 即可获取下载链接 以下内容是为了方便搜索引擎索引特别加上的，不是给人看的。因此不进行频繁修补，只做积累更新（最后更新于2015年10月14日）。 PS：全文两万六千余字，目前32页，因为字数限制，不能完整复制进来。...
2018清北学堂普及组冲刺班6连考总结反思
c_uizrp_dzjopkl的博客
10-01 1386
DAY 1 Problem 1 A(A.cpp/c/pas) 【题目描述】 给出一个n*m的网格，其中小明初始站在（x，y）这个位置，网格内无障碍，小明可以向上下左右四个方向一步走一格，问小明最少走几步可以走出网格。 【输入格式】 输入文件A .in 第一行四个整数n，m，x，y。 【输出格式】 输出文件A.out 一行一个整数表示答案。 【样例输入】 3 3 2 2 【样例输出】 2 【数据范围...
2019保研Tips：双非推免南京大学
henussw的博客
10-21 7556
前言 感谢我的母校，给我这样一个平台，让我能够走的更快飞的更高~ 个人简介 母校：如博客名 排名：1/528 六级：486 竞赛：数学建模国赛国二、mathorcup国二、泰迪杯数据挖掘国三、数创杯二等、蓝桥杯省二、等等 项目：1个自己做的，2个合伙做的（我是负责人），1个校创（非负责人） 软件著作权：2个（其中1个是第一申请人） 论文：没有 保研前准备 成绩 成绩排名很重...
threejs中让物体围绕指定轴旋转
热门推荐
weixin_42390878的博客
08-31 2万+
转自：https://my.oschina.net/u/3407699/blog/1619536 第一点 物体的rotation的旋转属性是相对与该物体的本身坐标系而言的，物体的rotation就是Euler欧拉角，但是会存在万向节死锁的问题。 第二点 three.js中提供了相关的方法rotateOnAxis(axis,angle),该方法需要注意的是angle是相对于物体之前状态的旋转角度...
DirectX 11 学习笔记-Part2-5【Cubemap/贴图置换/阴影/屏幕空间环境光遮蔽】【代码篇】
Zealot_Alie的博客
01-27 932
代码已上传至GitHub，这部分对应的项目为NormalDisplacementSsao。运行项目时可以使用数字键1-3切换渲染的shader，另外按下z键可以看到SSAO生成的遮罩贴图。 这篇文章主要结合代码介绍渲染的流程（对应代码中的NDSsaoApp::DrawScene）。 Shadow Map 首先我们要渲染阴影深度图，看ShadowMap的定义 class ShadowMap...
UnityShader如何写宏之六——疯狂测试
大话程序员
11-26 258
长江后浪推前浪 不知道你碰到， #if (defined(SHADER_API_D3D11) && !defined(SHADER_API_XBOXONE)) // Real-support for depth-format cube shadow map. #define SHADOWS_CUBE_IN_DEPTH_TEX #endif 会不会觉得很烦。。。。。 宏就是这么个东西，自从当年被cocos2dx的宏（c++)，虐过后，就有了免疫力 再来，UNITY_EDITOR,U
【c++23种设计模式概述】
最新发布
m0_45463480的博客
09-09 834
c++23种设计模式概述。
C语言作用域
天道酬勤
09-07 1040
作用域 (scope) 是描述程序可以访问标识符的区域。块是一对花括号 {} 括起来的区域 或 函数体内任意复合语句定义范围内的区域。定义在块中的变量具有块作用域。块作用域的变量的可见范围从定义处开始到该块定义范围的末尾为止。函数作用域是在整个函数体内可见的意思，在 C 程序中，函数形参和 goto 语句的标签具有函数作用域，在整个函数体内可见。在所有函数外定义的变量具有全局作用域，在整个程序都可见。在数据类型前加 static 关键字定义的全局变量具有文件作用域。
上一个问题字数不够，多说点
09-20
### 回答1： 好的，我来补充一下上一个问题的回答。 如果你想要在 Python 中操作文件和目录，可以使用 Python 内置的 os 模块。os 模块提供了许多用于操作文件和目录的函数。 例如，可以使用 os.rename() 函数修改文件的名称，使用 os.remove() 函数删除文件，使用 os.mkdir() 函数创建目录，使用 os.rmdir() 函数删除目录等。 还可以使用 os.listdir() 函数获取目录中的文件列表，使用 os.path 模块中的函数判断文件或目录是否存在，获取文件或目录的信息等。 另外，如果你想要更简单地操作文件和目录，可以使用 Python 的 shutil 模块。shutil 模块提供了简单易用的文件操作函数，如复制文件、移动文件、删除目录及其内容等。 总的来说，通过使用 os 模块和 shutil 模块，可以很方便地在 Python 中操作文件和目录。 ### 回答2： 好的，我可以多说一些关于上一个问题的内容。 上一个问题是有关观点和理由的问题，所以我想进一步解释一下。当我们被要求提出一个观点时，我们需要给出支持这个观点的理由。这些理由可以是基于事实、经验或逻辑推理。 比如，如果我问你是否认为运动对身体健康有好处，你可以选择赞同或反对。如果你选择赞同，你可以给出一些理由，比如运动可以增强心肺功能、提高代谢率、减少患病风险等等。这些理由都是通过科学研究得出的结论，所以是有充分依据的。 如果你选择反对，你也需要给出一些理由。你可以提出一些不常见的观点，比如过度运动可能对身体健康不利。然后你可以通过一些案例或研究来支持你的观点。 无论你是赞同还是反对一个观点，关键是要给出充分的理由，而不是随便说几句。这样可以让我们的观点更有说服力，同时也能提高我们的表达能力和思维深度。 希望这个补充能够对你有所帮助。如果你对其他问题有任何疑问，我很愿意继续帮助你。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值