{

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

// INITIALIZATION

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

printf ("\n");

printf ("-----------------------------------\n");

printf ("CHAI3D\n");

printf ("Demo: 10-oring\n");

printf ("Copyright 2003-2012\n");

printf ("-----------------------------------\n");

printf ("\n\n");

printf ("Keyboard Options:\n\n");

printf ("[1] - Texture (ON/OFF)\n");

printf ("[2] - Wireframe (ON/OFF)\n");

printf ("[x] - Exit application\n");

printf ("\n\n>\r");

// parse first arg to try and locate resources

string resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);

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

// WORLD - CAMERA - LIGHTING

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

// create a new world.

world = new cWorld();

// set the background color of the environment

world->m_backgroundColor.setBlack();

// create a camera and insert it into the virtual world

camera = new cCamera(world);

world->addChild(camera);

// position and oriente the camera

// origin***

camera->set( cVector3d (3.0, 0.0, 0.0), // 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

//MINE frame direction*******************************

/* camera->set( cVector3d (0.0, 0.0, -3.0), // camera position (eye)

cVector3d (0.0, 0.0, 0.0), // lookat position (target)

cVector3d (-1.0, 0.0, 0.0)); // direction of the (up) vector*/

// my dev camera

// camera->set( cVector3d (0.0, 0.0,-0.5), // camera position (eye)

// cVector3d (0.0, 0.0, 0.0), // lookat position (target)

// cVector3d (-1.0, 0.0, 0.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);

// enable shadow casting

camera->setUseShadowCasting(true);

// create a light source

light = new cSpotLight(world);

// attach light to camera

camera->addChild(light);

// enable light source

light->setEnabled(true);

// position the light source

light->setLocalPos( 0.0, 0.5, 0.0);

// define the direction of the light beam

light->setDir(-3.0,-0.5, 0.0);

// enable this light source to generate shadows

light->setShadowMapEnabled(true);

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

// 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 info = hapticDevice->getSpecifications();

// create a 3D tool and add it to the world

tool = new cToolCursor(world);

world->addChild(tool);

// connect the haptic device to the tool

tool->setHapticDevice(hapticDevice); // Á¬½Óhaptic deviceµ½tool ÖÐ

// initialize tool by connecting to haptic device

tool->start();

// map the physical workspace of the haptic device to a larger virtual workspace.

tool->setWorkspaceRadius(1.0);

// define a radius for the tool

tool->setRadius(0.03);

// read the scale factor between the physical workspace of the haptic

// device and the virtual workspace defined for the tool

double workspaceScaleFactor = tool->getWorkspaceScaleFactor();

// define a maximum stiffness that can be handled by the current

// haptic device. The value is scaled to take into account the

// workspace scale factor

//double stiffnessMax = info.m_maxLinearStiffness / workspaceScaleFactor;

// high stiffness MINE**********

double stiffnessMax =300;

// my device open **********************MINE

deltaCtrl.ConnectDevice();

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

// CREATING OBJECTS

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

// create a torus object (o-ring)

object = new cShapeTorus(0.12, 0.25);

// add object to world

world->addChild(object);

// set the position of the object at the center of the world

object->setLocalPos(1.4, 0.0, 0); // position

// rotate object of 90 degrees around its y-axis so that it

// stands vertically

object->rotateAboutGlobalAxisDeg( cVector3d(0, 1, 0), 90);

// set the material stiffness to 100% of the maximum permitted stiffness

// of the haptic device

object->m_material->setStiffness(1.00 * stiffnessMax);

// set some environmental texture and material properties

object->m_texture = new cTexture2d();

// load texture file

bool fileload = object->m_texture->loadFromFile(RESOURCE_PATH("resources/images/spheremap-6.jpg"));

if (!fileload)

{

//fileload = object->m_texture->loadFromFile("../../../bin/resources/images/spheremap-6.jpg");

fileload = object->m_texture->loadFromFile("resources/images/spheremap-6.jpg");

}

if (!fileload)

{

printf("Error - Texture image failed to load correctly.\n");

close();

return (-1);

}

// setup texture and material properties

object->setUseTexture(true);

object->m_texture->setSphericalMappingEnabled(true);

object->m_material->m_ambient.set(0.9, 0.9, 0.9);

object->m_material->m_diffuse.set(0.9, 0.9, 0.9);

object->m_material->m_specular.set(1.0, 1.0, 1.0);

// create a haptic effect for this object so that the operator can

// feel its surface

cEffectSurface* newEffect = new cEffectSurface(object);

object->addEffect(newEffect);

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

// WIDGETS

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

// create a font

cFont *font = NEW_CFONTCALIBRI20();

// create a label to display the haptic rate of the simulation

labelHapticRate = new cLabel(font);

camera->m_frontLayer->addChild(labelHapticRate);

level = new cLevel();

camera->m_frontLayer->addChild(level);

level->setLocalPos(100,100,0);

level->setRange(0.0, 25.0);

dial = new cDial();

camera->m_frontLayer->addChild(dial);

dial->setSingleIncrementDisplay(false);

dial->setLocalPos(600,100,0);

dial->setRange(0.0, 5.0);

dial->setSize(60);

dial->setValue0(0.0);

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

// OPEN GL - WINDOW DISPLAY

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

// simulation in now running!

simulationRunning = true;

// initialize GLUT

glutInit(&argc, argv);

// retrieve the resolution of the computer display and estimate the position

// of the GLUT window so that it is located at the center of the screen

int screenW = glutGet(GLUT_SCREEN_WIDTH);

int screenH = glutGet(GLUT_SCREEN_HEIGHT);

int windowPosX = (screenW - WINDOW_SIZE_W) / 2;

int windowPosY = (screenH - WINDOW_SIZE_H) / 2;

// initialize the OpenGL GLUT window

glutInitWindowPosition(windowPosX, windowPosY);

glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);

if (USE_STEREO_DISPLAY)

{

glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE | GLUT_STEREO);

camera->setUseStereo(true);

}

else

{

glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);

camera->setUseStereo(false);

}

glutCreateWindow(argv[0]);

glutDisplayFunc(updateGraphics);

glutKeyboardFunc(keySelect);

glutReshapeFunc(resizeWindow);

glutSetWindowTitle("CHAI3D");

// create a mouse menu (right button)

glutCreateMenu(menuSelect);

glutAddMenuEntry("full screen", OPTION_FULLSCREEN);

glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);

glutAttachMenu(GLUT_RIGHT_BUTTON);

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

// START SIMULATION

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

// 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(30, graphicsTimer, 0);

glutMainLoop();

// close everything

close();

// exit

return (0);

{

// update the size of the viewport

displayW = w;

displayH = h;

glViewport(0, 0, displayW, displayH);

{

// escape key

if ((key == 27) || (key == 'x'))

{

// close everything

close();

// exit application

exit(0);

}

// option 1:

if (key == '1')

{

bool useTexture = !object->getUseTexture();

object->setUseTexture(useTexture);

if (useTexture)

printf ("> Enable texture \r");

else

printf ("> Disable texture \r");

}

// option 2:

if (key == '2')

{

bool useWireMode = !object->getWireMode();

object->setWireMode(useWireMode);

if (useWireMode)

printf ("> Wire mode enabled \r");

else

printf ("> Wire mode disabled \r");

}

{

static int _wx, _wy, _ww, _wh;

switch (value)

{

// enable full screen display

case OPTION_FULLSCREEN:

_wx = glutGet(GLUT_WINDOW_X);

_wy = glutGet(GLUT_WINDOW_Y);

_ww = glutGet(GLUT_WINDOW_WIDTH);

_wh = glutGet(GLUT_WINDOW_HEIGHT);

glutFullScreen();

break;

// reshape window to original size

case OPTION_WINDOWDISPLAY:

glutPositionWindow(_wx, _wy);

glutReshapeWindow(_ww, _wh);

break;

}

{

// stop the simulation

simulationRunning = false;

// wait for graphics and haptics loops to terminate

while (!simulationFinished) { cSleepMs(100); }

// close haptic device

tool->stop();

// my device close**************************MINE

memset(dF,0,sizeof(dF));

memset(dT,0,sizeof(dT));

//deltaCtrl.SetFGetPosStatues(dF,dPos,statu,dT,blOutRange); // clear force

deltaCtrl.SetTorqueGetPosStatus(dT,nErrCode); // clear the force

deltaCtrl.ReleaseDev();

{

// inform the GLUT window to call updateGraphics again (next frame)

if (simulationRunning)

{

glutPostRedisplay();

}

glutTimerFunc(30, graphicsTimer, 0);

{

int px, py;

// update haptic rate label

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

px = (int)(0.5 * (displayW - labelHapticRate->getWidth()));

labelHapticRate->setLocalPos(px, 15);

// update other widgets

py = (int)(0.5 * (displayH - level->getHeight()));

level->setLocalPos(50, py);

level->setValue(tool->m_lastComputedGlobalForce.length());

px = displayW - 80;

py = (int)(0.5 * displayH);

dial->setLocalPos(px, py);

dial->setValue1(angVel.length());

// render world

camera->renderView(displayW, displayH);

// swap buffers

glutSwapBuffers();

// check for any OpenGL errors

GLenum err;

err = glGetError();

if (err != GL_NO_ERROR) printf("Error: %s\n", gluErrorString(err));

{

// reset clock

cPrecisionClock clock;

clock.reset();

// main haptic simulation loop

while(simulationRunning)

{

/////////////////////////////////////////////////////////////////////

// SIMULATION TIME

/////////////////////////////////////////////////////////////////////

// stop the simulation clock

clock.stop();

// read the time increment in seconds

double timeInterval = clock.getCurrentTimeSeconds();

// restart the simulation clock

clock.reset();

clock.start();

// update frequency counter

frequencyCounter.signal(1);

/////////////////////////////////////////////////////////////////////

// HAPTIC FORCE COMPUTATION

/////////////////////////////////////////////////////////////////////

// bool SetFGetPosStatues(double dF[3],double dPos[3], BYTE & statu,double dT[3],bool & blOutRange);

// MINE ***********************IO process

bool blIO = false;

//blIO = deltaCtrl.SetFGetPosStatues(dF,dPos,statu,dT, blOutRange); // changing point

blIO = deltaCtrl.SetFTGetPosStatus(dF,dT,nErrCode);

deltaCtrl.GetPos(dPos);

deltaCtrl.GetRad(dRad);

deltaCtrl.GetState(statu);

// compute global reference frames for each object

world->computeGlobalPositions(true);

// update position and orientation of tool

// tool->updatePose();

// update my pose************MINE***************** we get the force in

tool->updateMyPose(dPos);

//cVector3d posT;

// posT = tool->getDeviceLocalPos();

// compute interaction forces

tool->computeInteractionForces();

// we need to get the force out to our device**************** MINE

tool->getLastComputedForce(dF);

// send forces to device

tool->applyForces(); // ´Ë´¦Ó¦ÓÃÁ¦£¿

/////////////////////////////////////////////////////////////////////

// HAPTIC SIMULATION

/////////////////////////////////////////////////////////////////////

// get position of cursor in global coordinates

cVector3d toolPos = tool->getDeviceGlobalPos();

// get position of object in global coordinates

cVector3d objectPos = object->getGlobalPos();

// compute a vector from the center of mass of the object (point of rotation) to the tool

cVector3d v = cSub(toolPos, objectPos);

// compute angular acceleration based on the interaction forces

// between the tool and the object

cVector3d angAcc(0,0,0);

if (v.length() > 0.0)

{

// get the last force applied to the cursor in global coordinates

// we negate the result to obtain the opposite force that is applied on the

// object

cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);

// compute the effective force that contributes to rotating the object.

cVector3d force = toolForce - cProject(toolForce, v);

// compute the resulting torque

cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force));

// update rotational acceleration

const double INERTIA = 0.4;

angAcc = (1.0 / INERTIA) * torque;

}

// update rotational velocity

angVel.add(timeInterval * angAcc);

// set a threshold on the rotational velocity term

const double MAX_ANG_VEL = 10.0;

double vel = angVel.length();

if (vel > MAX_ANG_VEL)

{

angAcc.mul(MAX_ANG_VEL / vel);

}

// add some damping too

const double DAMPING = 0.1;

angVel.mul(1.0 - DAMPING * timeInterval);

// if user switch is pressed, set velocity to zero

if (tool->getUserSwitch(0) == 1)

{

angVel.zero();

}

// compute the next rotation configuration of the object

if (angVel.length() > C_SMALL)

{

object->rotateAboutGlobalAxisRad(cNormalize(angVel), timeInterval * angVel.length());

}

}

// exit haptics thread

simulationFinished = true;