//===========================================================================
void cODEGenericBody::createStaticPlane(const cVector3d& a_position,
const cVector3d& a_normal)
{
// object is static by default
m_static = true;
// temp variables
cVector3d normal = a_normal;
cVector3d offset(0,0,0);
// check normal
if (normal.length() == 0) { return; }
// compute parameters
normal.normalize();
double a = normal.x;
double b = normal.y;
double c = normal.z;
offset = cProject(a_position, normal);
double d = offset.length();
if (d > 0)
{
if (cAngle(offset, normal) > cDegToRad(90))
{
d = -d;
}
}
// build fixed plane
m_ode_geom = dCreatePlane(m_ODEWorld->m_ode_space, a, b, c, d);
// store dynamic model type
m_typeDynamicCollisionModel = ODE_MODEL_PLANE;
// store dynamic model parameters
m_paramDynColModel0 = normal.x;
m_paramDynColModel1 = normal.y;
m_paramDynColModel2 = normal.z;
m_posOffsetDynColModel = a_position;
m_rotOffsetDynColModel.identity();
}
void __fastcall TForm1::MassSpringFormCreate(TObject *Sender)
{
m_floor = 0;
m_floor_spring_constant = DEFAULT_FLOOR_SPRING_CONSTANT;
// init world scale
scale = 2.0;
// create a world
world = new cWorld();
// set background properties
world->setBackgroundColor(0.2,0.2,0.2);
// create a camera in world
camera = new cCamera(world);
world->addChild(camera);
// set camera position and orientation
camera->set( cVector3d(0.0, 0.0, 7.0), // position of camera.
cVector3d(0.0, 0.0, 0.0), // camera looking at origin.
cVector3d(0.0, 1.0, 0.0)); // orientation of camera. (standing up.)
// Create a light source and attach it to camera
light = new cLight(world);
camera->addChild(light);
light->setEnabled(true);
light->setPos(cVector3d(0,2,4));
light->rotate(cVector3d(0,0,1), cDegToRad(180));
// create a display for graphic rendering
viewport = new cViewport(Panel1->Handle, camera, false);
// create a tool
tool = new cMeta3dofPointer(world, 0, true);
tool->initialize();
tool->setRenderingMode(RENDER_DEVICE);
// Rotate the tool so its axes align with our opengl-like axes
tool->rotate(cVector3d(0,0,1),-90.0*M_PI/180.0);
tool->rotate(cVector3d(1,0,0),-90.0*M_PI/180.0);
tool->computeGlobalPositions();
tool->setWorkspace(3.5*scale, 3.5*scale, 3.5*scale);
tool->setRadius(scale / 70.0);
// tool becomes a child of the camera which we can control from
world->addChild(tool);
// Create a mesh to represent the floor
m_floor = new cMesh(world);
world->addChild(m_floor);
// Fill in meaningful vertex positions
m_floor->newVertex(-FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, -FLOOR_Z_SIZE/2.0);
m_floor->newVertex(-FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, FLOOR_Z_SIZE/2.0);
m_floor->newVertex( FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, FLOOR_Z_SIZE/2.0);
m_floor->newTriangle(0,1,2);
m_floor->newVertex( FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, -FLOOR_Z_SIZE/2.0);
m_floor->newVertex(-FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, -FLOOR_Z_SIZE/2.0);
m_floor->newVertex( FLOOR_X_SIZE/2.0, FLOOR_Y_POSITION, FLOOR_Z_SIZE/2.0);
m_floor->newTriangle(3,4,5);
for(int n=0; n<6; n++)
{
cVertex* curVertex = m_floor->getVertex(n);
curVertex->setNormal(0,1,0);
}
// Give him some material properties...
cMaterial material;
material.m_ambient.set( 0.2, 0.2, 0.2, 1.0 );
material.m_diffuse.set( 0.6, 0.6, 0.6, 1.0 );
material.m_specular.set( 0.9, 0.9, 0.9, 1.0 );
material.setShininess(100);
m_floor->m_material = material;
// Create an initial ball
CBall* b = new CBall();
m_active_balls.push_back(b);
cVector3d pos(-1.0,0,1.0);
b->setPos(pos);
world->addChild(b);
// Create a series of masses connected by springs
for(int i=1; i<INITIAL_NUM_BALLS; i++) {
add_ball();
}
simulationOn = false;
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI 3D\n");
printf ("Demo: 52-GEL-duck\n");
printf ("Copyright 2003-2010\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Show GEL Skeleton\n");
printf ("[2] - Hide GEL Skeleton\n");
printf ("[x] - Exit application\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);
//-----------------------------------------------------------------------
// 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.0, 0.0, 0.0);
world->setBackgroundColor(1.0, 1.0, 1.0);
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
// define a default position of the camera (described in spherical coordinates)
cameraAngleH = 0;
cameraAngleV = 45;
cameraDistance = 2.5;
updateCameraPosition();
// 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 higher rendering quality because we are displaying transparent objects
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
light->setDirectionalLight(false);
light->m_ambient.set(0.5, 0.5, 0.5);
light->m_diffuse.set(0.7, 0.7, 0.7);
light->m_specular.set(0.9, 0.9, 0.9);
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_front_2Dscene.addChild(logo);
// load a "chai3d" bitmap image file
bool fileload;
fileload = logo->m_image.loadFromFile(RESOURCE_PATH("resources/images/chai3d.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = logo->m_image.loadFromFile("../../../bin/resources/images/chai3d.bmp");
#endif
}
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoomHV(0.4, 0.4);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image.replace(
cColorb(0, 0, 0), // original RGB color
cColorb(0, 0, 0, 0) // new RGBA color
);
// enable transparency
logo->enableTransparency(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;
if (hapticDevice)
{
hapticDevice->open();
info = hapticDevice->getSpecifications();
}
// desired workspace radius of the cursor
cursorWorkspaceRadius = 0.8;
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
workspaceScaleFactor = cursorWorkspaceRadius / info.m_workspaceRadius;
// define a scale factor between the force perceived at the cursor and the
// forces actually sent to the haptic device
deviceForceScale = 0.005 * (info.m_maxForceStiffness / workspaceScaleFactor);
// define a force scale factor for the current haptic device.
const double FORCE_REFERENCE = 10.0;
deviceForceScale = info.m_maxForce / FORCE_REFERENCE;
// create a large sphere that represents the haptic device
deviceRadius = 0.12;
device = new cShapeSphere(deviceRadius);
world->addChild(device);
device->m_material.m_ambient.set(0.4, 0.4, 0.4, 0.7);
device->m_material.m_diffuse.set(0.7, 0.7, 0.7, 0.7);
device->m_material.m_specular.set(1.0, 1.0, 1.0, 0.7);
device->m_material.setShininess(100);
stiffness = 40;
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create a world which supports deformable object
defWorld = new cGELWorld();
world->addChild(defWorld);
world->setPos(-1.5, 0.0, -0.1);
world->rotate(cVector3d(0,1,0), cDegToRad(30));
//-----------------------------------------------------------------------
// COMPOSE THE WATER
//-----------------------------------------------------------------------
ground = new cGELMesh(world);
ground->m_useMassParticleModel = true;
defWorld->m_gelMeshes.push_back(ground);
cGELMassParticle::default_mass = 0.010;
cGELMassParticle::default_kDampingPos = 0.4;
cGELMassParticle::default_gravity.set(0,0,0);
int u,v;
int RESOLUTION = 15;
double SIZE = 3.5;
for (v=0; v<RESOLUTION; v++)
{
for (u=0; u<RESOLUTION; u++)
{
double px, py, tu, tv;
// compute the position of the vertex
px = SIZE / (double)RESOLUTION * (double)u - (SIZE/2.0);
py = SIZE / (double)RESOLUTION * (double)v - (SIZE/2.0);
// create new vertex
unsigned int index = ground->newVertex(px, py, level);
cVertex* vertex = ground->getVertex(index);
// compute texture coordinate
tu = (double)u / (double)RESOLUTION;
tv = (double)v / (double)RESOLUTION;
vertex->setTexCoord(tu, tv);
vertex->setColor(cColorf(1.0, 0.0, 0.1));
}
}
ground->buildVertices();
for (v=0; v<RESOLUTION; v++)
{
for (u=0; u<RESOLUTION; u++)
{
if ((u == 0) || (v == 0) || (u == (RESOLUTION-1)) || (v == (RESOLUTION-1)))
{
// create new vertex
unsigned int index = ((v + 0) * RESOLUTION) + (u + 0);
ground->m_gelVertices[index].m_massParticle->m_fixed = true;
}
}
}
cGELLinearSpring::default_kSpringElongation = 10.0; // [N/m]
// Create a triangle based map using the above pixels
for (v=0; v<(RESOLUTION-1); v++)
{
for (u=0; u<(RESOLUTION-1); u++)
{
// get the indexing numbers of the next four vertices
unsigned int index00 = ((v + 0) * RESOLUTION) + (u + 0);
unsigned int index01 = ((v + 0) * RESOLUTION) + (u + 1);
unsigned int index10 = ((v + 1) * RESOLUTION) + (u + 0);
unsigned int index11 = ((v + 1) * RESOLUTION) + (u + 1);
// create two new triangles
ground->newTriangle(index00, index01, index10);
ground->newTriangle(index10, index01, index11);
cGELMassParticle* m0 = ground->m_gelVertices[index00].m_massParticle;
cGELMassParticle* m1 = ground->m_gelVertices[index01].m_massParticle;
cGELMassParticle* m2 = ground->m_gelVertices[index10].m_massParticle;
cGELLinearSpring* spring0 = new cGELLinearSpring(m0, m1);
cGELLinearSpring* spring1 = new cGELLinearSpring(m0, m2);
ground->m_linearSprings.push_back(spring0);
ground->m_linearSprings.push_back(spring1);
}
}
double transparencyLevel = 0.7;
ground->setUseMaterial(true);
ground->m_material.m_ambient.set(0.6, 0.6, 0.6, transparencyLevel);
ground->m_material.m_diffuse.set(0.8, 0.8, 0.8, transparencyLevel);
ground->m_material.m_specular.set(0.9, 0.9, 0.9, transparencyLevel);
ground->setTransparencyLevel(transparencyLevel);
ground->setUseTransparency(true);
cTexture2D* textureGround = new cTexture2D();
ground->setTexture(textureGround);
ground->setUseTexture(true, true);
fileload = textureGround->loadFromFile(RESOURCE_PATH("resources/images/aqua.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = textureGround->loadFromFile("../../../bin/resources/images/aqua.bmp" );
#endif
if (!fileload)
{
printf("Error - 3D Texture failed to load correctly.\n");
close();
return (-1);
}
}
//-----------------------------------------------------------------------
// COMPOSE SOME REFLEXION
//-----------------------------------------------------------------------
cGenericObject* reflexion = new cGenericObject();
world->addChild(reflexion);
cMatrix3d rot;
rot.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
reflexion->setRot(rot);
reflexion->addChild(defWorld);
reflexion->setPos(0.0, 0.0, 2.0 * level);
cGenericObject* reflexionTool = new cGenericObject();
reflexion->addChild(reflexionTool);
reflexionTool->addChild(device);
reflexionTool->setPos(0.0, 0.0, -0.5 * level);
// Play with thse numbers carefully!
defWorld->m_integrationTime = 0.001;
// create a deformable mesh
defObject = new cGELMesh(world);
defWorld->m_gelMeshes.push_front(defObject);
//-----------------------------------------------------------------------
// BUILD THE DUCK!
//-----------------------------------------------------------------------
if (USE_SKELETON_MODEL)
{
fileload = createSkeletonMesh(defObject, RESOURCE_PATH("resources/models/ducky/duck-200.off"), RESOURCE_PATH("resources/models/ducky/duck-full.obj"));
if (!fileload)
{
#if defined(_MSVC)
fileload = createSkeletonMesh(defObject, "../../../bin/resources/models/ducky/duck-200.off", "../../../bin/resources/models/ducky/duck-full.obj");
#endif
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
}
}
else
{
fileload = createTetGenMesh(defObject, RESOURCE_PATH("resources/models/ducky/duck-1200.off"), RESOURCE_PATH("resources/models/ducky/duck-green.obj"));
if (!fileload)
{
#if defined(_MSVC)
fileload = createTetGenMesh(defObject, "../../../bin/resources/models/ducky/duck-1200.off", "../../../bin/resources/models/ducky/duck-green.obj");
#endif
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
}
}
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// 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);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutMouseFunc(mouseClick);
glutMotionFunc(mouseMove);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CS 268 - Sonny Chan & Francois Conti - March 2009");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAddMenuEntry("show skeleton", OPTION_SHOWSKELETON);
glutAddMenuEntry("hide skeleton", OPTION_HIDESKELETON);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI3D\n");
printf ("Demo: 40-ODE-cubic\n");
printf ("Copyright 2003-2011\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Instructions:\n\n");
printf ("- Use haptic device and user switch to manipulate cubes \n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Enable gravity\n");
printf ("[2] - Disable gravity\n");
printf ("[x] - Exit application\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);
//-----------------------------------------------------------------------
// 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.0, 0.0, 0.0);
world->m_backgroundColor.setWhite();
// 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 (2.5, 0.0, 0.3), // 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
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.1, 4.0);
camera->setUseShadowCasting(true);
camera->setUseMultipassTransparency(true);
// create a light source and attach it to the camera
light = new cSpotLight(world);
world->addChild(light); // attach light to camera
light->setEnabled(true); // enable light source
light->setLocalPos(cVector3d( 1.5, 0.5, 1.5)); // position the light source
light->setDir(cVector3d(-1.0, -0.5, -1.0)); // define the direction of the light beam
light->m_ambient.set(0.6, 0.6, 0.6);
light->m_diffuse.set(0.8, 0.8, 0.8);
light->m_specular.set(0.8, 0.8, 0.8);
light->m_shadowMap->setEnabled(true);
light->setCutOffAngleDeg(40);
// light->setDisplaySettings(0.05, 4.0, true);
//light->setDisplayEnabled(true);
//light->setShowFrame(true);
//light->setShowEnabled(true);
cDirectionalLight* light2 = new cDirectionalLight(world);
world->addChild(light2); // attach light to camera
light2->setEnabled(true); // enable light source
light2->setDir(cVector3d(-1.0, 0.0, -1.0)); // define the direction of the light beam
light2->m_ambient.set(0.3, 0.3, 0.3);
light2->m_diffuse.set(0.6, 0.6, 0.6);
light2->m_specular.set(0.0, 0.0, 0.0);
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
/*
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_frontLayer->addChild(logo);
// load a "chai3d" bitmap image file
logo->setImage (NEW_CHAI3D_LOGO());
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setLocalPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoom(0.25, 0.25);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image->setTransparentColor(0x00, 0x00, 0x00, 0x00);
// enable transparency
logo->setUseTransparency(true);
*/
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
cGenericHapticDevice* hapticDevice;
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info;
if (hapticDevice)
{
info = hapticDevice->getSpecifications();
}
// create a 3D tool and add it to the world
tool = new cToolGripper(world);
// tool = new cToolCursor(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// 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.3);
// define a radius for the tool (graphical display)
tool->setRadius(0.03);
// hide the device sphere. only show proxy.
tool->setShowContactPoints(true, false);
// set the physical readius of the proxy.
proxyRadius = 0.03;
tool->setRadiusContact(proxyRadius);
// tool->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
// enable if objects in the scene are going to rotate of translate
// or possibly collide against the tool. If the environment
// is entirely static, you can set this parameter to "false"
tool->enableDynamicObjects(true);
// ajust the color of the tool
// tool->m_materialProxy = tool->m_materialProxyButtonPressed;
// 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
stiffnessMax = info.m_maxLinearStiffness / workspaceScaleFactor;
// create a small white line that will be enabled every time the operator
// grasps an object. The line indicated the connection between the
// position of the tool and the grasp position on the object
graspLine = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));
world->addChild(graspLine);
graspLine->m_colorPointA.set(1.0, 1.0, 1.0);
graspLine->m_colorPointB.set(1.0, 1.0, 1.0);
graspLine->setShowEnabled(false);
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// 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, -9.81));
// create a new ODE object that is automatically added to the ODE world
ODEBody0 = new cODEGenericBody(ODEWorld);
ODEBody1 = new cODEGenericBody(ODEWorld);
ODEBody2 = new cODEGenericBody(ODEWorld);
// create a virtual mesh that will be used for the geometry
// representation of the dynamic body
cMesh* object0 = new cMesh();
cMesh* object1 = new cMesh();
cMesh* object2 = new cMesh();
// crate a cube mesh
double boxSize = 0.2;
createCube(object0, boxSize);
createCube(object1, boxSize);
createCube(object2, boxSize);
// define some material properties for each cube
cMaterial mat0, mat1, mat2;
mat0.m_ambient.set(0.8, 0.1, 0.4);
mat0.m_diffuse.set(1.0, 0.15, 0.5);
mat0.m_specular.set(1.0, 0.2, 0.8);
mat0.setRedIndian();
mat0.m_specular.set(0.0, 0.0, 0.0);
mat0.setStiffness(0.5 * stiffnessMax);
mat0.setDynamicFriction(0.6);
mat0.setStaticFriction(0.6);
object0->setMaterial(mat0);
mat1.m_ambient.set(0.2, 0.6, 0.0);
mat1.m_diffuse.set(0.2, 0.8, 0.0);
mat1.m_specular.set(0.2, 1.0, 0.0);
mat1.setBlueRoyal();
mat1.m_specular.set(0.0, 0.0, 0.0);
mat1.setStiffness(0.5 * stiffnessMax);
mat1.setDynamicFriction(0.6);
mat1.setStaticFriction(0.6);
object1->setMaterial(mat1);
mat2.m_ambient.set(0.0, 0.2, 0.6);
mat2.m_diffuse.set(0.0, 0.2, 0.8);
mat2.m_specular.set(0.0, 0.2, 1.0);
mat2.setGreenDarkSea();
mat2.m_specular.set(0.0, 0.0, 0.0);
mat2.setStiffness(0.5 * stiffnessMax);
mat2.setDynamicFriction(0.6);
mat2.setStaticFriction(0.6);
object2->setMaterial(mat2);
// add mesh to ODE object
ODEBody0->setImageModel(object0);
ODEBody1->setImageModel(object1);
ODEBody2->setImageModel(object2);
// 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(boxSize, boxSize, boxSize);
ODEBody1->createDynamicBox(boxSize, boxSize, boxSize, false, cVector3d(1,1,1));
ODEBody2->createDynamicBox(boxSize, boxSize, boxSize);
// define some mass properties for each cube
ODEBody0->setMass(0.05);
ODEBody1->setMass(0.05);
ODEBody2->setMass(0.05);
// set position of each cube
cVector3d tmpvct;
tmpvct = cVector3d(0.0,-0.6, -0.5);
ODEBody0->setPosition(tmpvct);
tmpvct = cVector3d(0.0, 0.6, -0.5);
ODEBody1->setPosition(tmpvct);
tmpvct = cVector3d(0.0, 0.0, -0.5);
ODEBody2->setPosition(tmpvct);
// rotate central cube of 45 degrees (just to show hoe this works!)
cMatrix3d rot;
rot.identity();
rot.rotateAboutGlobalAxisRad(cVector3d(0,0,1), cDegToRad(45));
ODEBody0->setRotation(rot);
// 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 size = 1.0;
ODEGPlane0->createStaticPlane(cVector3d(0.0, 0.0, 2.0 *size), cVector3d(0.0, 0.0 ,-1.0));
ODEGPlane1->createStaticPlane(cVector3d(0.0, 0.0, -size), cVector3d(0.0, 0.0 , 1.0));
ODEGPlane2->createStaticPlane(cVector3d(0.0, size, 0.0), cVector3d(0.0,-1.0, 0.0));
ODEGPlane3->createStaticPlane(cVector3d(0.0, -size, 0.0), cVector3d(0.0, 1.0, 0.0));
ODEGPlane4->createStaticPlane(cVector3d( size, 0.0, 0.0), cVector3d(-1.0,0.0, 0.0));
ODEGPlane5->createStaticPlane(cVector3d(-0.8 * size, 0.0, 0.0), cVector3d( 1.0,0.0, 0.0));
//////////////////////////////////////////////////////////////////////////
// Create some reflexion
//////////////////////////////////////////////////////////////////////////
/*
// we create an intermediate node to which we will attach
// a copy of the object located inside the world
cGenericObject* reflexion = new cGenericObject();
// set this object as a ghost node so that no haptic interactions or
// collision detecting take place within the child nodes added to the
// reflexion node.
reflexion->setGhostEnabled(true);
// add reflexion node to world
world->addChild(reflexion);
// turn off culling on each object (objects now get rendered on both sides)
object0->setUseCulling(false, true);
object1->setUseCulling(false, true);
object2->setUseCulling(false, true);
// create a symmetry rotation matrix (z-plane)
cMatrix3d rotRefexion;
rotRefexion.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
reflexion->setLocalRot(rotRefexion);
reflexion->setLocalPos(0.0, 0.0, -2.005);
// add objects to the world
reflexion->addChild(ODEWorld);
reflexion->addChild(tool);
*/
//////////////////////////////////////////////////////////////////////////
// Create a Ground
//////////////////////////////////////////////////////////////////////////
// create mesh to model ground surface
cMesh* ground = new cMesh();
world->addChild(ground);
//cCreateCylinder(ground, 0.01, 2.0, 36, 1, true, true);
// create 4 vertices (one at each corner)
double groundSize = 1.3;
int vertices0 = ground->newVertex(-groundSize, -1.0 *groundSize, 0.0);
int vertices1 = ground->newVertex( groundSize, -1.0 *groundSize, 0.0);
int vertices2 = ground->newVertex( groundSize, 1.0 *groundSize, 0.0);
int vertices3 = ground->newVertex(-groundSize, 1.0 *groundSize, 0.0);
// compose surface with 2 triangles
ground->newTriangle(vertices0, vertices1, vertices2);
ground->newTriangle(vertices0, vertices2, vertices3);
// compute surface normals
ground->computeAllNormals();
// position ground at the right level
ground->setLocalPos(0.0, 0.0, -1.0);
// define some material properties and apply to mesh
cMaterial matGround;
matGround.setStiffness(0.5 * stiffnessMax);
matGround.setDynamicFriction(0.2);
matGround.setStaticFriction(0.0);
// matGround.m_ambient.set(0.0, 0.0, 0.0);
// matGround.m_diffuse.set(0.0, 0.0, 0.0);
// matGround.m_specular.set(0.0, 0.0, 0.0);
matGround.setWhite();
matGround.m_emission.setGrayLevel(0.2);
ground->setMaterial(matGround);
// enable and set transparency level of ground
// ground->setTransparencyLevel(0.7);
// ground->setUseTransparency(true);
// setup collision detector
ground->createAABBCollisionDetector(proxyRadius);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// 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);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
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
//-----------------------------------------------------------------------
// 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(30, graphicsTimer, 0);
glutMainLoop();
// close everything
close();
// exit
return (0);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI 3D\n");
printf ("Demo: 23-tooth\n");
printf ("Copyright 2003-2010\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Instructions:\n\n");
printf ("- Use haptic device and user switch to rotate \n");
printf (" rotate and translate tooth. \n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Wireframe (ON/OFF)\n");
printf ("[2] - Normals (ON/OFF)\n");
printf ("[+] - Increase Opacity\n");
printf ("[-] - Reduce Opacity\n");
printf ("[x] - Exit application\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);
sp = new Serial("COM6");
if (sp->IsConnected())
printf("We're connected");
//-----------------------------------------------------------------------
// 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.0, 0.0, 0.0);
// 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 (3.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);
// enable high quality rendering when tooth becomes transparent
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
light->m_ambient.set(0.6, 0.6, 0.6);
light->m_diffuse.set(0.8, 0.8, 0.8);
light->m_specular.set(0.8, 0.8, 0.8);
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_front_2Dscene.addChild(logo);
// load a "chai3d" bitmap image file
bool fileload;
fileload = logo->m_image.loadFromFile(RESOURCE_PATH("resources/images/chai3d.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = logo->m_image.loadFromFile("../../../bin/resources/images/chai3d.bmp");
#endif
}
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoomHV(0.4, 0.4);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image.replace(
cColorb(0, 0, 0), // original RGB color
cColorb(0, 0, 0, 0) // new RGBA color
);
// enable transparency
logo->enableTransparency(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
cGenericHapticDevice* hapticDevice;
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
if (hapticDevice)
{
info = hapticDevice->getSpecifications();
}
// create a 3D tool and add it to the world
tool = new cGeneric3dofPointer(world);
//-----------------Jake--------------//
index_finger = new cGeneric3dofPointer(world);
thumb = new cGeneric3dofPointer(world);
world->addChild(tool);
world->addChild(index_finger);
world->addChild(thumb);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// 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);
index_finger->setWorkspaceRadius(1.0);
thumb->setWorkspaceRadius(1.0);
// define a radius for the tool (graphical display)
tool->setRadius(0.01);
index_finger->setRadius(0.01);
thumb->setRadius(0.01);
// hide the device sphere. only show proxy.
tool->m_deviceSphere->setShowEnabled(false);
index_finger->setShowEnabled(false);
thumb->setShowEnabled(false);
// set the physical radius of the proxy to be equal to the radius
// of the tip of the mesh drill (see drill in the virtual scene section)
proxyRadius = 0.03;
tool->m_proxyPointForceModel->setProxyRadius(proxyRadius);
index_finger->m_proxyPointForceModel->setProxyRadius(proxyRadius);
thumb->m_proxyPointForceModel->setProxyRadius(proxyRadius);
// informe the finger-proxy force renderer to only check one side of triangles
tool->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
index_finger->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
thumb->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
// the environmeny is static, you can set this parameter to "false"
tool->m_proxyPointForceModel->m_useDynamicProxy = false;
index_finger->m_proxyPointForceModel->m_useDynamicProxy = false;
thumb->m_proxyPointForceModel->m_useDynamicProxy = false;
tool->m_proxyPointForceModel->m_useForceShading = true;
index_finger->m_proxyPointForceModel->m_useForceShading = true;
thumb->m_proxyPointForceModel->m_useForceShading = true;
// 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_maxForceStiffness / workspaceScaleFactor;
rootLabels = new cGenericObject();
camera->m_front_2Dscene.addChild(rootLabels);
// create a small label as title
cLabel* titleLabel = new cLabel();
rootLabels->addChild(titleLabel);
//-----Jake------//
// define its position, color and string message
titleLabel->setPos(300, 60, 0);
titleLabel->m_fontColor.set(1.0, 1.0, 1.0);
titleLabel->m_string = "Haptic Device Pos [mm]:";
string strID;
cStr(strID, 0);
string strDevice = "#" + strID + " - " +info.m_modelName;
cLabel* newLabel = new cLabel();
tool->m_proxyMesh->addChild(newLabel);
newLabel->m_string = strDevice;
newLabel->setPos(0.00, 0.05, 0.00);
newLabel->m_fontColor.set(1.0, 1.0, 1.0);
string strID1;
cStr(strID1, 1);
string strDevice1 = "#" + strID1 + " - index finger";
cLabel* newLabel1 = new cLabel();
index_finger->m_proxyMesh->addChild(newLabel1);
newLabel1->m_string = strDevice1;
newLabel1->setPos(0.00, 0.05, 0.00);
newLabel1->m_fontColor.set(1.0, 1.0, 1.0);
string strID2;
cStr(strID2, 2);
string strDevice2 = "#" + strID2 + " - thumb";
cLabel* newLabel2 = new cLabel();
thumb->m_proxyMesh->addChild(newLabel2);
newLabel2->m_string = strDevice2;
newLabel2->setPos(0.00, 0.05, 0.00);
newLabel2->m_fontColor.set(1.0, 1.0, 1.0);
// crate a small label to indicate the position of the device
cLabel* newPosLabel = new cLabel();
rootLabels->addChild(newPosLabel);
newPosLabel->setPos(300, 50, 0);
newPosLabel->m_fontColor.set(0.6, 0.6, 0.6);
labels[0] = newPosLabel;
cLabel* newPosLabel1 = new cLabel();
rootLabels->addChild(newPosLabel1);
newPosLabel1->setPos(300, 30, 0);
newPosLabel1->m_fontColor.set(0.6, 0.6, 0.6);
labels[1] = newPosLabel1;
cLabel* newPosLabel2 = new cLabel();
rootLabels->addChild(newPosLabel2);
newPosLabel2->setPos(300, 10, 0);
newPosLabel2->m_fontColor.set(0.6, 0.6, 0.6);
labels[2]= newPosLabel2;
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create a virtual mesh
tooth = new cMesh(world);
// add object to world
world->addChild(tooth);
// set the position and orientation of the object at the center of the world
tooth->setPos(0.0, 0.0, 0.0);
tooth->rotate(cVector3d(0.0, 0.0, 1.0), cDegToRad(-10));
tooth->rotate(cVector3d(0.0, 1.0, 0.0), cDegToRad(10));
// load an object file
fileload = tooth->loadFromFile(RESOURCE_PATH("resources/models/tooth/tooth.3ds"));
if (!fileload)
{
#if defined(_MSVC)
fileload = tooth->loadFromFile("../../../bin/resources/models/tooth/tooth.3ds");
#endif
}
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
// make the outside of the tooth rendered in wireframe
((cMesh*)(tooth->getChild(1)))->setWireMode(true);
// make the outside of the tooth rendered in semi-transparent
((cMesh*)(tooth->getChild(1)))->setUseTransparency(false);
((cMesh*)(tooth->getChild(1)))->setTransparencyLevel(transparencyLevel);
// compute a boundary box
tooth->computeBoundaryBox(true);
// resize tooth to screen
tooth->scale(0.003);
// compute collision detection algorithm
tooth->createAABBCollisionDetector(1.01 * proxyRadius, true, false);
// define a default stiffness for the object x
tooth->setStiffness(0.8 * stiffnessMax, true);
// create a new mesh.
drill = new cMesh(world);
// load a drill like mesh and attach it to the tool
fileload = drill->loadFromFile(RESOURCE_PATH("resources/models/drill/drill.3ds"));
if (!fileload)
{
#if defined(_MSVC)
fileload = drill->loadFromFile("../../../bin/resources/models/drill/drill.3ds");
#endif
}
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
// resize tool mesh model
drill->scale(0.004);
// remove the collision detector. we do not want to compute any
// force feedback rendering on the object itself.
drill->deleteCollisionDetector(true);
// define a material property for the mesh
cMaterial mat;
mat.m_ambient.set(0.5, 0.5, 0.5);
mat.m_diffuse.set(0.8, 0.8, 0.8);
mat.m_specular.set(1.0, 1.0, 1.0);
drill->setMaterial(mat, true);
drill->computeAllNormals(true);
// attach drill to tool
tool->m_proxyMesh->addChild(drill);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// 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);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI 3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI3D\n");
printf ("Demo: 13-primitives\n");
printf ("Copyright 2003-2012\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Wireframe (ON/OFF)\n");
printf ("[x] - Exit application\n");
printf ("\n\n>\r");
//-----------------------------------------------------------------------
// 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
camera->set( cVector3d (1.4, 0.0, 0.6), // 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);
// 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();
// if the haptic devices carries a gripper, enable it to behave like a user switch
hapticDevice->setEnableGripperUserSwitch(true);
// 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);
// 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 the radius of the tool (sphere)
double toolRadius = 0.05;
// define a radius for the tool
tool->setRadius(toolRadius);
// hide the device sphere. only show proxy.
tool->setShowContactPoints(true, false);
// enable if objects in the scene are going to rotate of translate
// or possibly collide against the tool. If the environment
// is entirely static, you can set this parameter to "false"
tool->enableDynamicObjects(true);
// 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;
//-----------------------------------------------------------------------
// CREATING OBJECTS
//-----------------------------------------------------------------------
/////////////////////////////////////////////////////////////////////////
// BASE
/////////////////////////////////////////////////////////////////////////
// create a virtual mesh
cMesh* base = new cMesh();
// add object to world
world->addChild(base);
// build mesh using a cylinder primitive
cCreateCylinder(base,
0.01,
0.5,
64,
1,
true,
true,
cVector3d(0.0, 0.0, 0.0),
cMatrix3d(cDegToRad(0), cDegToRad(0), cDegToRad(0), C_EULER_ORDER_XYZ)
);
// set material color
base->m_material->setGrayGainsboro();
base->m_material->setStiffness(0.5 * stiffnessMax);
// build collision detection tree
base->createAABBCollisionDetector(toolRadius);
/////////////////////////////////////////////////////////////////////////
// TEA POT
/////////////////////////////////////////////////////////////////////////
// create a virtual mesh
cMesh* teaPot = new cMesh();
// add object to world
base->addChild(teaPot);
// build mesh using a cylinder primitive
cCreateTeaPot(teaPot,
0.5,
cVector3d(0.0, 0.2, 0.12),
cMatrix3d(cDegToRad(0), cDegToRad(0), cDegToRad(170), C_EULER_ORDER_XYZ)
);
// set material properties
teaPot->m_material->setRedDark();
teaPot->m_material->setStiffness(0.5 * stiffnessMax);
// build collision detection tree
teaPot->createAABBCollisionDetector(toolRadius);
/////////////////////////////////////////////////////////////////////////
// GLASS
/////////////////////////////////////////////////////////////////////////
// create a virtual mesh
cMesh* glass = new cMesh();
// add object to world
base->addChild(glass);
// build mesh using a cylinder primitive
cCreatePipe(glass,
0.15,
0.05,
0.06,
32,
1,
cVector3d(0.0,-0.2, 0.0),
cMatrix3d(cDegToRad(0), cDegToRad(0), cDegToRad(170), C_EULER_ORDER_XYZ)
);
// set material color
glass->m_material->setBlueCornflower();
glass->m_material->setStiffness(0.5 * stiffnessMax);
// build collision detection tree
glass->createAABBCollisionDetector(toolRadius);
/////////////////////////////////////////////////////////////////////////
// CONE
/////////////////////////////////////////////////////////////////////////
// create a virtual mesh
cMesh* cone = new cMesh();
// add object to world
base->addChild(cone);
// build mesh using a cylinder primitive
cCreateCone(cone,
0.15,
0.05,
0.01,
32,
1,
false,
true,
cVector3d(0.25, 0.0, 0.0),
cMatrix3d(cDegToRad(0), cDegToRad(0), cDegToRad(0), C_EULER_ORDER_XYZ)
);
// set material color
cone->m_material->setGreenForest();
cone->m_material->setStiffness(0.5 * stiffnessMax);
// build collision detection tree
cone->createAABBCollisionDetector(toolRadius);
//-----------------------------------------------------------------------
// 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);
// create a background
cBackground* background = new cBackground();
camera->m_backLayer->addChild(background);
// set background properties
background->setCornerColors(cColorf(1.0, 1.0, 1.0),
cColorf(1.0, 1.0, 1.0),
cColorf(0.9, 0.9, 0.9),
cColorf(0.9, 0.9, 0.9));
//-----------------------------------------------------------------------
// 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 windowW = 0.7 * screenH;
int windowH = 0.7 * screenH;
int windowPosX = (screenW - windowH) / 2;
int windowPosY = (screenH - windowW) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(windowW, windowH);
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);
}
//==============================================================================
void cToolGripper::computeInteractionForces()
{
// convert the angle of the gripper into a position in device coordinates.
// this value is device dependent.
double gripperPositionFinger = 0.0;
double gripperPositionThumb = 0.0;
if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
// compute new position of thumb and finger
cVector3d lineFingerThumb = getGlobalRot().getCol1();
cVector3d pFinger = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionFinger * lineFingerThumb;
cVector3d pThumb = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionThumb * lineFingerThumb;
cVector3d posFinger, posThumb;
if (m_hapticDevice->m_specifications.m_rightHand)
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pThumb));
}
else
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pThumb));
}
// compute forces
cVector3d forceThumb = m_hapticPointThumb->computeInteractionForces(posThumb,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
cVector3d forceFinger = m_hapticPointFinger->computeInteractionForces(posFinger,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
// compute torques
double scl = 0.0;
double factor = m_gripperWorkspaceScale * m_workspaceScaleFactor;
if (factor > 0.0)
{
scl = 1.0 / factor;
}
cVector3d torque = scl * cAdd(cCross(cSub(posThumb, m_deviceGlobalPos), forceThumb), cCross(cSub(posFinger, m_deviceGlobalPos), forceFinger));
// compute gripper force
double gripperForce = 0.0;
if ((m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7) ||
(m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7))
{
cVector3d dir = posFinger - posThumb;
if (dir.length() > 0.00001)
{
dir.normalize ();
cVector3d force = cProject (forceFinger, dir);
gripperForce = force.length();
if (force.length() > 0.001)
{
double angle = cAngle(dir, force);
if ((angle > C_PI/2.0) || (angle < -C_PI/2.0)) gripperForce = -gripperForce;
}
}
}
// gripper damping
double gripperAngularVelocity = 0.0;
m_hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);
double gripperDamping = -0.1 * m_hapticDevice->m_specifications.m_maxGripperAngularDamping * gripperAngularVelocity;
// finalize forces, torques and gripper force
m_lastComputedGlobalForce = forceThumb + forceFinger;
m_lastComputedGlobalTorque = torque;
m_lastComputedGripperForce = gripperForce + gripperDamping;
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI 3D\n");
printf ("Demo: ODE mesh\n");
printf ("Copyright 2003-2009\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Instructions:\n\n");
printf ("- Use haptic device and user switch to manipulate gear parts \n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Enable gravity\n");
printf ("[2] - Disable gravity\n");
printf ("[x] - Exit application\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);
//-----------------------------------------------------------------------
// 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.0, 0.0, 0.0);
// 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.2, 0.0, 0.8), // 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
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
// 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
light->m_ambient.set(0.6, 0.6, 0.6);
light->m_diffuse.set(0.8, 0.8, 0.8);
light->m_specular.set(0.8, 0.8, 0.8);
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_front_2Dscene.addChild(logo);
// load a "chai3d" bitmap image file
bool fileload;
fileload = logo->m_image.loadFromFile(RESOURCE_PATH("resources/images/chai3d.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = logo->m_image.loadFromFile("../../../bin/resources/images/chai3d.bmp");
#endif
}
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoomHV(0.25, 0.25);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image.replace(
cColorb(0, 0, 0), // original RGB color
cColorb(0, 0, 0, 0) // new RGBA color
);
// enable transparency
logo->enableTransparency(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
cGenericHapticDevice* hapticDevice;
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info;
if (hapticDevice)
{
info = hapticDevice->getSpecifications();
}
// create a 3D tool and add it to the world
tool = new cGeneric3dofPointer(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// 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.4);
// define a radius for the tool (graphical display)
tool->setRadius(0.015);
// hide the device sphere. only show proxy.
tool->m_deviceSphere->setShowEnabled(false);
// set the physical readius of the proxy.
proxyRadius = 0.0;
tool->m_proxyPointForceModel->setProxyRadius(proxyRadius);
tool->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
// enable if objects in the scene are going to rotate of translate
// or possibly collide against the tool. If the environment
// is entirely static, you can set this parameter to "false"
tool->m_proxyPointForceModel->m_useDynamicProxy = true;
// ajust the color of the tool
tool->m_materialProxy = tool->m_materialProxyButtonPressed;
// 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
stiffnessMax = info.m_maxForceStiffness / workspaceScaleFactor;
// create a small white line that will be enabled every time the operator
// grasps an object. The line indicated the connection between the
// position of the tool and the grasp position on the object
graspLine = new cShapeLine(cVector3d(0,0,0), cVector3d(0,0,0));
world->addChild(graspLine);
graspLine->m_ColorPointA.set(1.0, 1.0, 1.0);
graspLine->m_ColorPointB.set(1.0, 1.0, 1.0);
graspLine->setShowEnabled(false);
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// 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, -0.5));
// create a new ODE object that is automatically added to the ODE world
ODEGear1 = new cODEGenericBody(ODEWorld);
ODEGear2 = new cODEGenericBody(ODEWorld);
ODEAxis1 = new cODEGenericBody(ODEWorld);
ODEAxis2 = new cODEGenericBody(ODEWorld);
// create a virtual mesh that will be used for the geometry
// representation of the dynamic body
cMesh* gear1 = new cMesh(world);
cMesh* gear2 = new cMesh(world);
cMesh* axis1 = new cMesh(world);
cMesh* axis2 = new cMesh(world);
// load object 1 - a gear
fileload = gear1->loadFromFile(RESOURCE_PATH("resources/models/gear/gear.3ds"));
if (!fileload)
{
#if defined(_MSVC)
fileload = gear1->loadFromFile("../../../bin/resources/models/gear/gear.3ds");
#endif
}
gear1->scale(0.0015);
gear1->createAABBCollisionDetector(proxyRadius, true, false);
// load object 2 - a gear
fileload = gear2->loadFromFile(RESOURCE_PATH("resources/models/gear/gear.3ds"));
if (!fileload)
{
#if defined(_MSVC)
fileload = gear2->loadFromFile("../../../bin/resources/models/gear/gear.3ds");
#endif
}
gear2->scale(0.0015);
gear2->createAABBCollisionDetector(proxyRadius, true, false);
// load static object 1 - a shaft
fileload = axis1->loadFromFile(RESOURCE_PATH("resources/models/gear/axis.3ds"));
if (!fileload)
{
#if defined(_MSVC)
fileload = axis1->loadFromFile("../../../bin/resources/models/gear/axis.3ds");
#endif
}
axis1->scale(0.0015);
axis1->createAABBCollisionDetector(proxyRadius, true, false);
// load static object 2 - a shaft
fileload = axis2->loadFromFile(RESOURCE_PATH("resources/models/gear/axis.3ds"));
if (!fileload)
{
#if defined(_MSVC)
fileload = axis2->loadFromFile("../../../bin/resources/models/gear/axis.3ds");
#endif
}
axis2->scale(0.0015);
axis2->createAABBCollisionDetector(proxyRadius, true, false);
// define some material properties for both types of objects
cMaterial mat0, mat1, mat2;
mat0.m_ambient.set(0.7, 0.1, 0.2);
mat0.m_diffuse.set(0.8, 0.15, 0.2);
mat0.m_specular.set(1.0, 0.2, 0.2);
mat0.setStiffness(0.5 * stiffnessMax);
mat0.setDynamicFriction(0.4);
mat0.setStaticFriction(0.9);
gear1->setMaterial(mat0, true);
gear2->setMaterial(mat0, true);
mat1.m_ambient.set(0.2, 0.2, 0.2);
mat1.m_diffuse.set(0.8, 0.8, 0.8);
mat1.m_specular.set(1.0, 1.0, 1.0);
mat1.setStiffness(0.5 * stiffnessMax);
mat1.setDynamicFriction(0.4);
mat1.setStaticFriction(0.9);
axis1->setMaterial(mat1, true);
axis2->setMaterial(mat1, true);
// define image models
ODEGear1->setImageModel(gear1);
ODEGear2->setImageModel(gear2);
ODEAxis1->setImageModel(axis1);
ODEAxis2->setImageModel(axis2);
// create a dynamic model of the ODE object. Here we decide to use a box just like
// the object mesh we just defined
ODEGear1->createDynamicMesh();
ODEGear2->createDynamicMesh();
ODEAxis1->createDynamicCapsule(0.050, 0.47, true); // true = make object static
ODEAxis2->createDynamicCapsule(0.050, 0.47, true); // true = make object static
// define some mass properties for each gear
ODEGear1->setMass(0.6);
ODEGear2->setMass(0.6);
// define temp variables
cVector3d pos;
cMatrix3d rot;
// default distance between both gears when demo starts
double DISTANCE = 0.260;
// position and oriente gear 1
pos = cVector3d(0.0,-DISTANCE, -0.5);
rot.identity();
rot.rotate(cVector3d(1,0,0), cDegToRad(90));
ODEGear1->setPosition(pos);
ODEGear1->setRotation(rot);
// position and oriente gear 1
pos = cVector3d(0.0, DISTANCE, -0.5);
rot.rotate(cVector3d(0,0,1), cDegToRad(14));
ODEGear2->setPosition(pos);
ODEGear2->setRotation(rot);
// position shaft 1
pos = cVector3d(0.0,-DISTANCE, -0.7);
ODEAxis1->setPosition(pos);
// position shaft 2
pos = cVector3d(0.0, DISTANCE, -0.7);
ODEAxis2->setPosition(pos);
// we now create 6 static walls to bound the workspace of our simulation
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 size = 1.0;
ODEGPlane0->createStaticPlane(cVector3d(0.0, 0.0, 2.0 *size), cVector3d(0.0, 0.0 ,-1.0));
ODEGPlane1->createStaticPlane(cVector3d(0.0, 0.0, -size), cVector3d(0.0, 0.0 , 1.0));
ODEGPlane2->createStaticPlane(cVector3d(0.0, size, 0.0), cVector3d(0.0,-1.0, 0.0));
ODEGPlane3->createStaticPlane(cVector3d(0.0, -size, 0.0), cVector3d(0.0, 1.0, 0.0));
ODEGPlane4->createStaticPlane(cVector3d( size, 0.0, 0.0), cVector3d(-1.0,0.0, 0.0));
ODEGPlane5->createStaticPlane(cVector3d(-0.8 * size, 0.0, 0.0), cVector3d( 1.0,0.0, 0.0));
//////////////////////////////////////////////////////////////////////////
// Create some reflexion
//////////////////////////////////////////////////////////////////////////
// we create an intermediate node to which we will attach
// a copy of the object located inside the world
cGenericObject* reflexion = new cGenericObject();
// set this object as a ghost node so that no haptic interactions or
// collision detecting take place within the child nodes added to the
// reflexion node.
reflexion->setAsGhost(true);
// add reflexion node to world
world->addChild(reflexion);
// turn off culling on each object (objects now get rendered on both sides)
gear1->setUseCulling(false, true);
gear2->setUseCulling(false, true);
axis1->setUseCulling(false, true);
axis2->setUseCulling(false, true);
// create a symmetry rotation matrix (z-plane)
cMatrix3d rotRefexion;
rotRefexion.set(1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0);
reflexion->setRot(rotRefexion);
reflexion->setPos(0.0, 0.0, -2.005);
// add objects to the world
reflexion->addChild(ODEWorld);
reflexion->addChild(tool);
//////////////////////////////////////////////////////////////////////////
// Create a Ground
//////////////////////////////////////////////////////////////////////////
// create mesh to model ground surface
cMesh* ground = new cMesh(world);
world->addChild(ground);
// create 4 vertices (one at each corner)
double groundSize = 2.0;
int vertices0 = ground->newVertex(-groundSize, -groundSize, 0.0);
int vertices1 = ground->newVertex( groundSize, -groundSize, 0.0);
int vertices2 = ground->newVertex( groundSize, groundSize, 0.0);
int vertices3 = ground->newVertex(-groundSize, groundSize, 0.0);
// compose surface with 2 triangles
ground->newTriangle(vertices0, vertices1, vertices2);
ground->newTriangle(vertices0, vertices2, vertices3);
// compute surface normals
ground->computeAllNormals();
// position ground at the right level
ground->setPos(0.0, 0.0, -1.0);
// define some material properties and apply to mesh
cMaterial matGround;
matGround.setStiffness(stiffnessMax);
matGround.setDynamicFriction(0.7);
matGround.setStaticFriction(1.0);
matGround.m_ambient.set(0.0, 0.0, 0.0);
matGround.m_diffuse.set(0.0, 0.0, 0.0);
matGround.m_specular.set(0.0, 0.0, 0.0);
ground->setMaterial(matGround);
// enable and set transparency level of ground
ground->setTransparencyLevel(0.75);
ground->setUseTransparency(true);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// 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);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI 3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI 3D\n");
printf ("Demo: 50-GEL-membrane\n");
printf ("Copyright 2003-2009\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Show GEL Skeleton\n");
printf ("[2] - Hide GEL Skeleton\n");
printf ("[x] - Exit application\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);
//-----------------------------------------------------------------------
// 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.0, 0.0, 0.0);
// 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.8, 0.0, 1.1), // 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);
// enable higher rendering quality because we are displaying transparent objects
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
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_front_2Dscene.addChild(logo);
// load a "chai3d" bitmap image file
bool fileload;
fileload = logo->m_image.loadFromFile(RESOURCE_PATH("resources/images/chai3d.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = logo->m_image.loadFromFile("../../../bin/resources/images/chai3d.bmp");
#endif
}
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoomHV(0.4, 0.4);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image.replace(
cColorb(0, 0, 0), // original RGB color
cColorb(0, 0, 0, 0) // new RGBA color
);
// enable transparency
logo->enableTransparency(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;
if (hapticDevice)
{
hapticDevice->open();
info = hapticDevice->getSpecifications();
}
// desired workspace radius of the cursor
cursorWorkspaceRadius = 0.5;
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
workspaceScaleFactor = cursorWorkspaceRadius / info.m_workspaceRadius;
// define a scale factor between the force perceived at the cursor and the
// forces actually sent to the haptic device
deviceForceScale = 0.1 * info.m_maxForce;
// create a large sphere that represents the haptic device
deviceRadius = 0.1;
device = new cShapeSphere(deviceRadius);
world->addChild(device);
device->m_material.m_ambient.set(0.4, 0.4, 0.4, 0.7);
device->m_material.m_diffuse.set(0.7, 0.7, 0.7, 0.7);
device->m_material.m_specular.set(1.0, 1.0, 1.0, 0.7);
device->m_material.setShininess(100);
stiffness = 40;
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create a world which supports deformable object
defWorld = new cGELWorld();
world->addChild(defWorld);
world->setPos(-1.5, 0.0, -0.1);
world->rotate(cVector3d(0,1,0), cDegToRad(30));
// Play with thse numbers carefully!
cGELSkeletonNode::default_kDampingPos = 0.1;
cGELSkeletonNode::default_kDampingRot = 0.1;
defWorld->m_integrationTime = 0.001;
// create a deformable mesh
defObject = new cGELMesh(world);
defWorld->m_gelMeshes.push_front(defObject);
fileload = defObject->loadFromFile(RESOURCE_PATH("resources/models/box/box.3ds"));
if (!fileload)
{
#if defined(_MSVC)
fileload = defObject->loadFromFile("../../../bin/resources/models/box/box.3ds");
#endif
}
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
// set some material color on the object
cMaterial mat;
mat.m_ambient.set(0.2, 0.2, 0.2);
mat.m_diffuse.set(0.6, 0.6, 0.6);
mat.m_ambient.set(1.0, 1.0, 1.0);
mat.setShininess(100);
defObject->setMaterial(mat, true);
// let's create a some environment mapping
cTexture2D* texture = new cTexture2D();
fileload = texture->loadFromFile(RESOURCE_PATH("resources/images/shadow.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = texture->loadFromFile("../../../bin/resources/images/shadow.bmp");
#endif
}
if (!fileload)
{
printf("Error - Texture image failed to load correctly.\n");
close();
return (-1);
}
texture->setEnvironmentMode(GL_DECAL);
texture->setSphericalMappingEnabled(true);
defObject->setTexture(texture, true);
defObject->setTransparencyLevel(0.7, true);
defObject->setUseTexture(true, true);
// build dynamic vertices
defObject->buildVertices();
defObject->m_useSkeletonModel = true;
// setup default values for nodes
cGELSkeletonNode::default_radius = 0.05;
cGELSkeletonNode::default_kDampingPos = 0.4;
cGELSkeletonNode::default_kDampingRot = 0.1;
cGELSkeletonNode::default_mass = 0.01; // [kg]
cGELSkeletonNode::default_showFrame = true;
cGELSkeletonNode::default_color.set(1.0, 0.6, 0.6);
cGELSkeletonNode::default_useGravity = true;
cGELSkeletonNode::default_gravity.set(0.00, 0.00, -1.00);
radius = cGELSkeletonNode::default_radius;
// create an array of nodes
for (int y=0; y<10; y++)
{
for (int x=0; x<10; x++)
{
cGELSkeletonNode* newNode = new cGELSkeletonNode();
nodes[x][y] = newNode;
defObject->m_nodes.push_front(newNode);
newNode->m_pos.set( (-0.45 + 0.1*(double)x), (-0.43 + 0.1*(double)y), 0.0);
}
}
// fix 4 nodes 9corners)
nodes[0][0]->m_fixed = true;
nodes[0][9]->m_fixed = true;
nodes[9][0]->m_fixed = true;
nodes[9][9]->m_fixed = true;
// setup default values for links
cGELSkeletonLink::default_kSpringElongation = 100.0; // [N/m]
cGELSkeletonLink::default_kSpringFlexion = 0.5; // [Nm/RAD]
cGELSkeletonLink::default_kSpringTorsion = 0.1; // [Nm/RAD]
cGELSkeletonLink::default_color.set(0.2, 0.2, 1.0);
// create links between nodes
for (int y=0; y<9; y++)
{
for (int x=0; x<9; x++)
{
cGELSkeletonLink* newLinkX0 = new cGELSkeletonLink(nodes[x+0][y+0], nodes[x+1][y+0]);
cGELSkeletonLink* newLinkX1 = new cGELSkeletonLink(nodes[x+0][y+1], nodes[x+1][y+1]);
cGELSkeletonLink* newLinkY0 = new cGELSkeletonLink(nodes[x+0][y+0], nodes[x+0][y+1]);
cGELSkeletonLink* newLinkY1 = new cGELSkeletonLink(nodes[x+1][y+0], nodes[x+1][y+1]);
defObject->m_links.push_front(newLinkX0);
defObject->m_links.push_front(newLinkX1);
defObject->m_links.push_front(newLinkY0);
defObject->m_links.push_front(newLinkY1);
}
}
// connect skin (mesh) to skeleton (GEM)
defObject->connectVerticesToSkeleton(false);
// show/hide underlying dynamic skeleton model
defObject->m_showSkeletonModel = false;
// define the integration time constant of the dynamics model
defWorld->m_integrationTime = 0.005;
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// 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);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI 3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAddMenuEntry("show skeleton", OPTION_SHOWSKELETON);
glutAddMenuEntry("hide skeleton", OPTION_HIDESKELETON);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI 3D\n");
printf ("Demo: 02-oring\n");
printf ("Copyright 2003-2010\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");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// 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.0, 0.0, 0.0);
// 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 (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
// 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);
// 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
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_front_2Dscene.addChild(logo);
// load a "chai3d" bitmap image file
bool fileload;
fileload = logo->m_image.loadFromFile(RESOURCE_PATH("resources/images/chai3d.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = logo->m_image.loadFromFile("../../../bin/resources/images/chai3d.bmp");
#endif
}
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoomHV(0.4, 0.4);
// here we replace all black pixels (0,0,0) of the logo bitmap
// with transparent black pixels (0, 0, 0, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image.replace(
cColorb(0, 0, 0), // original RGB color
cColorb(0, 0, 0, 0) // new RGBA color
);
// enable transparency
logo->enableTransparency(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
cGenericHapticDevice* hapticDevice;
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info;
if (hapticDevice)
{
info = hapticDevice->getSpecifications();
}
// create a 3D tool and add it to the world
tool = new cGeneric3dofPointer(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// 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_maxForceStiffness / workspaceScaleFactor;
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create a torus object (o-ring)
object = new cShapeTorus(0.23, 0.50);
// add object to world
world->addChild(object);
// set the position of the object at the center of the world
object->setPos(0.0, 0.0, 0.0);
// rotate object of 90 degrees around its y-axis so that it
// stands vertically
object->rotate( cVector3d(0, 1, 0), cDegToRad(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();
fileload = object->m_texture->loadFromFile(RESOURCE_PATH("resources/images/spheremap.bmp"));
if (!fileload)
{
#if defined(_WIN32)
fileload = object->m_texture->loadFromFile("../../../bin/resources/images/spheremap.bmp");
#endif
}
if (!fileload)
{
printf("Error - Texture image failed to load correctly.\n");
close();
return (-1);
}
object->setUseTexture(true);
object->m_texture->setSphericalMappingEnabled(true);
object->m_material.m_ambient.set(1.0, 1.0, 1.0);
object->m_material.m_diffuse.set(1.0, 1.0, 1.0);
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);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// 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);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI 3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
//==============================================================================
cMyCustomDevice::cMyCustomDevice(unsigned int a_deviceNumber)
{
// the connection to your device has not yet been established.
m_deviceReady = false;
////////////////////////////////////////////////////////////////////////////
/*
STEP 1:
Here you should define the specifications of your device.
These values only need to be estimates. Since haptic devices often perform
differently depending of their configuration withing their workspace,
simply use average values.
*/
////////////////////////////////////////////////////////////////////////////
//--------------------------------------------------------------------------
// NAME:
//--------------------------------------------------------------------------
// haptic device model (see file "CGenericHapticDevice.h")
m_specifications.m_model = C_HAPTIC_DEVICE_CUSTOM;
// name of the device manufacturer, research lab, university.
m_specifications.m_manufacturerName = "My Name";
// name of your device
m_specifications.m_modelName = "My Custom Device";
//--------------------------------------------------------------------------
// CHARACTERISTICS: (The following values must be positive or equal to zero)
//--------------------------------------------------------------------------
// the maximum force [N] the device can produce along the x,y,z axis.
m_specifications.m_maxLinearForce = 5.0; // [N]
// the maximum amount of torque your device can provide arround its
// rotation degrees of freedom.
m_specifications.m_maxAngularTorque = 0.2; // [N*m]
// the maximum amount of torque which can be provided by your gripper
m_specifications.m_maxGripperForce = 3.0; // [N]
// the maximum closed loop linear stiffness in [N/m] along the x,y,z axis
m_specifications.m_maxLinearStiffness = 1000.0; // [N/m]
// the maximum amount of angular stiffness
m_specifications.m_maxAngularStiffness = 1.0; // [N*m/Rad]
// the maximum amount of stiffness supported by the gripper
m_specifications.m_maxGripperLinearStiffness = 1000; // [N*m]
// the radius of the physical workspace of the device (x,y,z axis)
m_specifications.m_workspaceRadius = 0.2; // [m]
// the maximum opening angle of the gripper
m_specifications.m_gripperMaxAngleRad = cDegToRad(30.0);
////////////////////////////////////////////////////////////////////////////
/*
DAMPING PROPERTIES:
Start with small values as damping terms can be high;y sensitive to
the quality of your velocity signal and the spatial resolution of your
device. Try gradually increasing the values by using example "01-devices"
and by enabling viscosity with key command "2".
*/
////////////////////////////////////////////////////////////////////////////
// Maximum recommended linear damping factor Kv
m_specifications.m_maxLinearDamping = 20.0; // [N/(m/s)]
//! Maximum recommended angular damping factor Kv (if actuated torques are available)
m_specifications.m_maxAngularDamping = 0.0; // [N*m/(Rad/s)]
//! Maximum recommended angular damping factor Kv for the force gripper. (if actuated gripper is available)
m_specifications.m_maxGripperAngularDamping = 0.0; // [N*m/(Rad/s)]
//--------------------------------------------------------------------------
// CHARACTERISTICS: (The following are of boolean type: (true or false)
//--------------------------------------------------------------------------
// does your device provide sensed position (x,y,z axis)?
m_specifications.m_sensedPosition = true;
// does your device provide sensed rotations (i.e stylus)?
m_specifications.m_sensedRotation = true;
// does your device provide a gripper which can be sensed?
m_specifications.m_sensedGripper = true;
// is you device actuated on the translation degrees of freedom?
m_specifications.m_actuatedPosition = true;
// is your device actuated on the rotation degrees of freedom?
m_specifications.m_actuatedRotation = true;
// is the gripper of your device actuated?
m_specifications.m_actuatedGripper = true;
// can the device be used with the left hand?
m_specifications.m_leftHand = true;
// can the device be used with the right hand?
m_specifications.m_rightHand = true;
////////////////////////////////////////////////////////////////////////////
/*
STEP 2:
Here, you shall implement code which tells the application if your
device is actually connected to your computer and can be accessed.
In practice this may be consist in checking if your I/O board
is active or if your drivers are available.
If your device can be accessed, set:
m_systemAvailable = true;
Otherwise set:
m_systemAvailable = false;
Your actual code may look like:
bool result = checkIfMyDeviceIsAvailable()
m_systemAvailable = result;
If want to support multiple devices, using the method argument
a_deviceNumber to know which device to setup
*/
////////////////////////////////////////////////////////////////////////////
// *** INSERT YOUR CODE HERE ***
m_MyVariable = 0;
m_deviceAvailable = false; // this value should become 'true' when the device is available.
}
