PROBLEM:
As robotics takes on a larger role in surgery, a major challenge arises in how to simulate a sense of touch for surgeons operating on patients through a robotic interface. Without a sense of touch, a surgeon can only rely on their vision when performing surgery, and must guess how much force they are applying or what the tissue they are operating on feels like. This can have catastrophic results. Similarly, training for surgical procedures traditionally was vision based, with pig cadavers being brought in for some force application training. This is costly, can be inaccurate, and a pig must be killed for this to happen. As a result, training for certain surgical procedures is inefficient or ineffective.
PROJECT:
As a research assistant for Dr. Fey in the Human Enabled Robotics Laboratory (HERO) at UT Austin, I have taken on responsibilities for a project that involves designing and programming haptic devices to simulate surgical procedures. Haptic devices use sensors and robotic technology to simulate a sense of touch. Currently I am working with surgeons from UT Southwestern as well as other members of the HERO Lab to create a training device for TROCAR insertion. This is a procedure where a portal is stabbed into the abdomen to allow other instruments to enter.
INDIVIDUAL CONTRIBUTIONS:
I’ve been developing a C++ script, with the Chai3D library, to create a visual simulation of flesh entry (a critical characteristic of the process) in combination with a manufactured haptic device that provides force feedback. This is intended to be used for surgical training.
Using Chai3D, I’ve created a virtual world with different layers of flesh (skin, fat, muscle) that need to be penetrated during the process. The code sets up a connection with the haptic device so that its motion in the physical world, is tracked in real time by the tool location in the simulation. Stiffness properties for each flesh layer are declared, and a system of nodes are defined within each model layer to create a gel membrane like effect. Using a force algorithm, based on the virtual proximity of the tool and a node, force feedback is computed and sent to the haptic device. As a result, anyone using the device will feel realistic force feedback pushing back against them as they push down to penetrate the flesh. I’ve also added a camera at the base of the tool to provide a top down view of the tool entering the body, something that is important in the actual procedure and needed to be simulated as well.
A video of one iteration of the model working with the haptic device is provided below along with the source code I am using*. I am now working with professional surgeons to make small adjustments and improve training realism. Next steps would be to build and program a unique haptic device for this simulation process using linear actuators and Arduino.
*In order to actually execute the code you will have to have a registered haptic device, install Chai3D, and run the source code in the Gel Membrane module to get all needed pathways.
Main Script:
//==============================================================================
/*
Software License Agreement (BSD License)
Copyright (c) 2003-2016, CHAI3D.
(www.chai3d.org)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.
* Neither the name of CHAI3D nor the names of its contributors may
be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
\author <http://www.chai3d.org>
\author Francois Conti
\version 3.2.0 $Rev: 1907 $
*/
//==============================================================================
//------------------------------------------------------------------------------
#include "chai3d.h"
#include "GEL3D.h"
//------------------------------------------------------------------------------
#include <GLFW/glfw3.h>
#include <iostream>
//------------------------------------------------------------------------------
using namespace chai3d;
using namespace std;
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
// GENERAL SETTINGS
//------------------------------------------------------------------------------
// stereo Mode
/*
C_STEREO_DISABLED: Stereo is disabled
C_STEREO_ACTIVE: Active stereo for OpenGL NVDIA QUADRO cards
C_STEREO_PASSIVE_LEFT_RIGHT: Passive stereo where L/R images are rendered next to each other
C_STEREO_PASSIVE_TOP_BOTTOM: Passive stereo where L/R images are rendered above each other
*/
cStereoMode stereoMode = C_STEREO_DISABLED;
// fullscreen mode
bool fullscreen = false;
// mirrored display
bool mirroredDisplay = false;
//---------------------------------------------------------------------------
// DECLARED VARIABLES
//---------------------------------------------------------------------------
// a world that contains all objects of the virtual environment
cWorld* world;
// a camera to render the world in the window display
cCamera* camera;
// a light source to illuminate the objects in the world
cDirectionalLight* light;
// a haptic device handler
cHapticDeviceHandler* handler;
// a haptic device
cGenericHapticDevicePtr hapticDevice;
// force scale factor
double deviceForceScale;
// scale factor between the device workspace and cursor workspace
double workspaceScaleFactor;
// desired workspace radius of the virtual cursor
double cursorWorkspaceRadius;
// a label to display the rate [Hz] at which the simulation is running
cLabel* labelRates;
// flag to indicate if the haptic simulation currently running
// flag to indicate if the haptic simulation currently running
bool simulationRunning = false;
// flag to indicate if the haptic simulation has terminated
bool simulationFinished = true;
// a frequency counter to measure the simulation graphic rate
cFrequencyCounter freqCounterGraphics;
// a frequency counter to measure the simulation haptic rate
cFrequencyCounter freqCounterHaptics;
// haptic thread
cThread* hapticsThread;
// a handle to window display context
GLFWwindow* window = NULL;
// current width of window
int width = 0;
// current height of window
int height = 0;
// swap interval for the display context (vertical synchronization)
int swapInterval = 1;
// root resource path
string resourceRoot;
//---------------------------------------------------------------------------
// GEL
//---------------------------------------------------------------------------
// deformable world
cGELWorld* defWorld;
// first layer object mesh
cGELMesh* defObject;
// second layer object mesh
cGELMesh* defObject2;
// third layer object mesh
cGELMesh* defObject3;
// third layer object mesh
cGELMesh* defObject4;
// third layer object mesh
cGELMesh* defObject5;
// third layer object mesh
cGELMesh* defObject6;
// third layer object mesh
cGELMesh* defObject7;
// third layer object mesh
cGELMesh* defObject8;
// dynamic nodes
cGELSkeletonNode* nodes[10][10];
// dynamic nodes layer 2
cGELSkeletonNode* nodes2[10][10];
// dynamic nodes layer 3
cGELSkeletonNode* nodes3[10][10];
// dynamic nodes layer 4
cGELSkeletonNode* nodes3[10][10];
// dynamic nodes layer 5
cGELSkeletonNode* nodes3[10][10];
// dynamic nodes layer 6
cGELSkeletonNode* nodes3[10][10];
// dynamic nodes layer 7
cGELSkeletonNode* nodes3[10][10];
// dynamic nodes layer 8
cGELSkeletonNode* nodes3[10][10];
// haptic device model
cShapeSphere* device;
double deviceRadius;
// radius of the dynamic model sphere (GEM)
double radius;
// stiffness properties between the haptic device tool and the model (GEM)
double stiffnesslayer1;
double stiffnesslayer2;
double stiffnesslayer3;
double stiffnesslayer4;
double stiffnesslayer5;
double stiffnesslayer6;
double stiffnesslayer7;
double stiffnesslayer8;
// z height for each layer
double z_layer1;
double z_layer2;
double z_layer3;
double z_layer4;
double z_layer5;
double z_layer6;
double z_layer7;
double z_layer8;
// Add frame buffer for the second camera that shows the device view
cFrameBufferPtr frameBuffer;
// Add a side view for the side view panel for the device view
cViewPanel* sideViewPanel;
//---------------------------------------------------------------------------
// DECLARED FUNCTIONS
//---------------------------------------------------------------------------
// callback when the window display is resized
void windowSizeCallback(GLFWwindow* a_window, int a_width, int a_height);
// callback when an error GLFW occurs
void errorCallback(int error, const char* a_description);
// callback when a key is pressed
void keyCallback(GLFWwindow* a_window, int a_key, int a_scancode, int a_action, int a_mods);
// this function renders the scene
void updateGraphics(void);
// this function contains the main haptics simulation loop
void updateHaptics(void);
// this function closes the application
void close(void);
// compute forces between tool and environment
cVector3d computeForce(const cVector3d& a_cursor,
double a_cursorRadius,
const cVector3d& a_spherePos,
double a_radius,
double a_stiffness);
//---------------------------------------------------------------------------
// DECLARED MACROS
//---------------------------------------------------------------------------
// convert to resource path
#define RESOURCE_PATH(p) (char*)((resourceRoot+string(p)).c_str())
//===========================================================================
/*
DEMO: GEM_membrane.cpp
This application illustrates the use of the GEM libraries to simulate
deformable object. In this example we load a simple mesh object and
build a dynamic skeleton composed of volumetric spheres and 3 dimensional
springs which model torsion, flexion and elongation properties.
*/
//===========================================================================
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
cout << endl;
cout << "-----------------------------------" << endl;
cout << "CHAI3D" << endl;
cout << "Demo: 50-GEL-membrane" << endl;
cout << "Copyright 2003-2016" << endl;
cout << "-----------------------------------" << endl << endl << endl;
cout << "Keyboard Options:" << endl << endl;
cout << "[s] - Show/Hide GEL Skeleton" << endl;
cout << "[m] - Enable/Disable vertical mirroring" << endl;
cout << "[q] - Exit application" << endl;
cout << endl << endl;
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0, string(argv[0]).find_last_of("/\\") + 1);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// initialize GLFW library
if (!glfwInit())
{
cout << "failed initialization" << endl;
cSleepMs(1000);
return 1;
}
// set error callback
glfwSetErrorCallback(errorCallback);
// compute desired size of window
const GLFWvidmode* mode = glfwGetVideoMode(glfwGetPrimaryMonitor());
int w = 0.8 * mode->height;
int h = 0.5 * mode->height;
int x = 0.5 * (mode->width - w);
int y = 0.5 * (mode->height - h);
// set OpenGL version
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
// set active stereo mode
if (stereoMode == C_STEREO_ACTIVE)
{
glfwWindowHint(GLFW_STEREO, GL_TRUE);
}
else
{
glfwWindowHint(GLFW_STEREO, GL_FALSE);
}
// create display context
window = glfwCreateWindow(w, h, "CHAI3D", NULL, NULL);
if (!window)
{
cout << "failed to create window" << endl;
cSleepMs(1000);
glfwTerminate();
return 1;
}
// get width and height of window
glfwGetWindowSize(window, &width, &height);
// set position of window
glfwSetWindowPos(window, x, y);
// set key callback
glfwSetKeyCallback(window, keyCallback);
// set resize callback
glfwSetWindowSizeCallback(window, windowSizeCallback);
// set current display context
glfwMakeContextCurrent(window);
// sets the swap interval for the current display context
glfwSwapInterval(swapInterval);
// initialize GLEW library
#ifdef GLEW_VERSION
if (glewInit() != GLEW_OK)
{
cout << "failed to initialize GLEW library" << endl;
glfwTerminate();
return 1;
}
#endif
//-----------------------------------------------------------------------
// 3D - SCENEGRAPH
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and orient the camera
camera->set(cVector3d(1.5, 0.0, 1.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
camera->setClippingPlanes(0.01, 10.0);
// set stereo mode
camera->setStereoMode(stereoMode);
// set stereo eye separation and focal length (applies only if stereo is enabled)
camera->setStereoEyeSeparation(0.02);
camera->setStereoFocalLength(3.0);
// set vertical mirrored display mode
camera->setMirrorVertical(mirroredDisplay);
// enable multi-pass rendering to handle transparent objects
camera->setUseMultipassTransparency(true);
// create a directional light source
light = new cDirectionalLight(world);
// insert light source inside world
world->addChild(light);
// enable light source
light->setEnabled(true);
// define direction of light beam
light->setDir(0.0, 0.0, -1.0);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device found
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo hapticDeviceInfo = hapticDevice->getSpecifications();
// open connection to haptic device
hapticDevice->open();
// desired workspace radius of the cursor
cursorWorkspaceRadius = 0.7;
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
workspaceScaleFactor = cursorWorkspaceRadius / hapticDeviceInfo.m_workspaceRadius;
// define a scale factor between the force perceived at the cursor and the
// forces actually sent to the haptic device
deviceForceScale = 5.0;
// create a large sphere that represents the haptic device
deviceRadius = 0.1;
device = new cShapeSphere(deviceRadius);
world->addChild(device);
device->m_material->setWhite();
device->m_material->setShininess(100);
// interaction stiffness between tool and deformable layer models
stiffnesslayer1 = 40;
stiffnesslayer2 = 50;
stiffnesslayer3 = 75;
//--------------------------------------------------------------------------------------------------
// Add the second camera to the haptic device tool. This will be a side buffer view from the tool
//--------------------------------------------------------------------------------------------------
// Create the camera
cCamera* deviceCamera = new cCamera(world);
// Now attach the camera to the haptice device
device->addChild(deviceCamera);
deviceCamera->rotateAboutGlobalAxisDeg(cVector3d(0.0, 1.0, 0.0), -90);
// Create a frame buffer for the side view
frameBuffer = cFrameBuffer::create();
frameBuffer->setup(deviceCamera);
// create panel to display side view
sideViewPanel = new cViewPanel(frameBuffer);
camera->m_frontLayer->addChild(sideViewPanel);
sideViewPanel->setLocalPos(10, 10);
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create a world which supports deformable object
defWorld = new cGELWorld();
world->addChild(defWorld);
// create 3 deformable meshes
defObject = new cGELMesh();
defWorld->m_gelMeshes.push_front(defObject);
defObject2 = new cGELMesh();
defWorld->m_gelMeshes.push_front(defObject2);
defObject3 = new cGELMesh();
defWorld->m_gelMeshes.push_front(defObject3);
z_layer1 = 0.3;
z_layer2 = 0.0;
z_layer3 = -0.15;
// Set the z position of each layer
defObject->setLocalPos(-0.0, -0.0, z_layer1);
defObject2->setLocalPos(-0.0, -0.0, z_layer2);
defObject3->setLocalPos(-0.0, -0.0, z_layer3);
// load models, use the box model for all of the objects
bool fileload;
fileload = defObject->loadFromFile(RESOURCE_PATH("../resources/models/box/box.obj"));
fileload = defObject2->loadFromFile(RESOURCE_PATH("../resources/models/box/box.obj"));
fileload = defObject3->loadFromFile(RESOURCE_PATH("../resources/models/box/box.obj"));
if (!fileload)
{
#if defined(_MSVC)
fileload = defObject->loadFromFile("../../../bin/resources/models/box/box.obj");
fileload = defObject2->loadFromFile("../../../bin/resources/models/box/box.obj");
fileload = defObject3->loadFromFile("../../../bin/resources/models/box/box.obj");
#endif
}
if (!fileload)
{
cout << "Error - 3D Model failed to load correctly." << endl;
close();
return (-1);
}
// set some material color on the layers
cMaterial mat;
mat.setWhite();
mat.setShininess(100);
defObject->setMaterial(mat, true);
defObject2->setMaterial(mat, true);
defObject3->setMaterial(mat, true);
// let's create some environment mapping
shared_ptr<cTexture2d> textureSkin(new cTexture2d());
fileload = textureSkin->loadFromFile(RESOURCE_PATH("../resources/images/skin.jpg"));
if (!fileload)
{
#if defined(_MSVC)
fileload = textureSkin->loadFromFile("../../../bin/resources/images/skin.jpg");
#endif
}
if (!fileload)
{
cout << "Error - Texture failed to load correctly." << endl;
close();
return (-1);
}
shared_ptr<cTexture2d> textureFat(new cTexture2d());
fileload = textureFat->loadFromFile(RESOURCE_PATH("../resources/images/fat.jpg"));
if (!fileload)
{
#if defined(_MSVC)
fileload = textureFat->loadFromFile("../../../bin/resources/images/fat.jpg");
#endif
}
if (!fileload)
{
cout << "Error - Texture failed to load correctly." << endl;
close();
return (-1);
}
shared_ptr<cTexture2d> textureMuscle(new cTexture2d());
fileload = textureMuscle->loadFromFile(RESOURCE_PATH("../resources/images/muscle.jpg"));
if (!fileload)
{
#if defined(_MSVC)
fileload = textureMuscle->loadFromFile("../../../bin/resources/images/muscle.jpg");
#endif
}
if (!fileload)
{
cout << "Error - Texture failed to load correctly." << endl;
close();
return (-1);
}
// enable environmental texturing
textureSkin->setEnvironmentMode(GL_DECAL);
textureSkin->setSphericalMappingEnabled(true);
textureFat->setEnvironmentMode(GL_DECAL);
textureFat->setSphericalMappingEnabled(true);
textureMuscle->setEnvironmentMode(GL_DECAL);
textureMuscle->setSphericalMappingEnabled(true);
// assign and enable texturing for all objects
defObject->setTexture(textureSkin, true);
defObject->setUseTexture(true, true);
defObject2->setTexture(textureFat, true);
defObject2->setUseTexture(true, true);
defObject3->setTexture(textureMuscle, true);
defObject3->setUseTexture(true, true);
// set object to be transparent
defObject->setTransparencyLevel(0.8, true, true);
defObject2->setTransparencyLevel(0.8, true, true);
defObject3->setTransparencyLevel(0.9, true, true);
// build dynamic vertices
defObject->buildVertices();
defObject2->buildVertices();
defObject3->buildVertices();
// set default properties for skeleton nodes
cGELSkeletonNode::s_default_radius = 0.05; // [m]
cGELSkeletonNode::s_default_kDampingPos = 2.5;
cGELSkeletonNode::s_default_kDampingRot = 0.6;
cGELSkeletonNode::s_default_mass = 0.002; // [kg]
cGELSkeletonNode::s_default_showFrame = true;
cGELSkeletonNode::s_default_color.setBlueCornflower();
cGELSkeletonNode::s_default_useGravity = true;
cGELSkeletonNode::s_default_gravity.set(0.00, 0.00, -9.81);
radius = cGELSkeletonNode::s_default_radius;
// use internal skeleton as deformable model
defObject->m_useSkeletonModel = true;
defObject2->m_useSkeletonModel = true;
defObject3->m_useSkeletonModel = true;
// create an array of nodes
for (int y = 0; y < 10; y++)
{
for (int x = 0; x < 10; x++)
{
cGELSkeletonNode* newNode = new cGELSkeletonNode();
cGELSkeletonNode* newNode2 = new cGELSkeletonNode();
cGELSkeletonNode* newNode3 = new cGELSkeletonNode();
nodes[x][y] = newNode;
nodes2[x][y] = newNode2;
nodes3[x][y] = newNode3;
defObject->m_nodes.push_front(newNode);
defObject2->m_nodes.push_front(newNode2);
defObject3->m_nodes.push_front(newNode3);
newNode->m_pos.set((-0.45 + 0.1 * (double)x), (-0.43 + 0.1 * (double)y), 0.0);
newNode2->m_pos.set((-0.45 + 0.1 * (double)x), (-0.43 + 0.1 * (double)y), 0.0);
newNode3->m_pos.set((-0.45 + 0.1 * (double)x), (-0.43 + 0.1 * (double)y), 0.0);
}
}
// set corner nodes as fixed
nodes[0][0]->m_fixed = true;
nodes[0][9]->m_fixed = true;
nodes[9][0]->m_fixed = true;
nodes[9][9]->m_fixed = true;
nodes2[0][0]->m_fixed = true;
nodes2[0][9]->m_fixed = true;
nodes2[9][0]->m_fixed = true;
nodes2[9][9]->m_fixed = true;
nodes3[0][0]->m_fixed = true;
nodes3[0][9]->m_fixed = true;
nodes3[9][0]->m_fixed = true;
nodes3[9][9]->m_fixed = true;
// set default physical properties for links
cGELSkeletonLink::s_default_kSpringElongation = 25.0; // [N/m]
cGELSkeletonLink::s_default_kSpringFlexion = 0.5; // [Nm/RAD]
cGELSkeletonLink::s_default_kSpringTorsion = 0.1; // [Nm/RAD]
cGELSkeletonLink::s_default_color.setBlueCornflower();
// 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);
cGELSkeletonLink* newLinkX02 = new cGELSkeletonLink(nodes2[x + 0][y + 0], nodes2[x + 1][y + 0]);
cGELSkeletonLink* newLinkX12 = new cGELSkeletonLink(nodes2[x + 0][y + 1], nodes2[x + 1][y + 1]);
cGELSkeletonLink* newLinkY02 = new cGELSkeletonLink(nodes2[x + 0][y + 0], nodes2[x + 0][y + 1]);
cGELSkeletonLink* newLinkY12 = new cGELSkeletonLink(nodes2[x + 1][y + 0], nodes2[x + 1][y + 1]);
defObject2->m_links.push_front(newLinkX02);
defObject2->m_links.push_front(newLinkX12);
defObject2->m_links.push_front(newLinkY02);
defObject2->m_links.push_front(newLinkY12);
cGELSkeletonLink* newLinkX03 = new cGELSkeletonLink(nodes3[x + 0][y + 0], nodes3[x + 1][y + 0]);
cGELSkeletonLink* newLinkX13 = new cGELSkeletonLink(nodes3[x + 0][y + 1], nodes3[x + 1][y + 1]);
cGELSkeletonLink* newLinkY03 = new cGELSkeletonLink(nodes3[x + 0][y + 0], nodes3[x + 0][y + 1]);
cGELSkeletonLink* newLinkY13 = new cGELSkeletonLink(nodes3[x + 1][y + 0], nodes3[x + 1][y + 1]);
defObject3->m_links.push_front(newLinkX03);
defObject3->m_links.push_front(newLinkX13);
defObject3->m_links.push_front(newLinkY03);
defObject3->m_links.push_front(newLinkY13);
}
}
// connect skin (mesh) to skeleton (GEM)
defObject->connectVerticesToSkeleton(false);
defObject2->connectVerticesToSkeleton(false);
defObject3->connectVerticesToSkeleton(false);
// show/hide underlying dynamic skeleton model
defObject->m_showSkeletonModel = false;
defObject2->m_showSkeletonModel = false;
defObject3->m_showSkeletonModel = false;
//--------------------------------------------------------------------------
// WIDGETS
//--------------------------------------------------------------------------
// create a font
cFontPtr font = NEW_CFONTCALIBRI20();
// create a label to display the haptic and graphic rate of the simulation
labelRates = new cLabel(font);
camera->m_frontLayer->addChild(labelRates);
labelRates->m_fontColor.setBlack();
// create a background
cBackground* background = new cBackground();
camera->m_backLayer->addChild(background);
// set background properties
background->setCornerColors(cColorf(1.00f, 1.00f, 1.00f),
cColorf(0.95f, 0.95f, 0.95f),
cColorf(0.85f, 0.85f, 0.85f),
cColorf(0.80f, 0.80f, 0.80f));
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// create a thread which starts the main haptics rendering loop
hapticsThread = new cThread();
hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
// setup callback when application exits
atexit(close);
//--------------------------------------------------------------------------
// MAIN GRAPHIC LOOP
//--------------------------------------------------------------------------
// call window size callback at initialization
windowSizeCallback(window, width, height);
// main graphic loop
while (!glfwWindowShouldClose(window))
{
// get width and height of window
glfwGetWindowSize(window, &width, &height);
// render graphics
updateGraphics();
// swap buffers
glfwSwapBuffers(window);
// process events
glfwPollEvents();
// signal frequency counter
freqCounterGraphics.signal(1);
}
// close window
glfwDestroyWindow(window);
// terminate GLFW library
glfwTerminate();
// exit
return 0;
}
//---------------------------------------------------------------------------
void windowSizeCallback(GLFWwindow* a_window, int a_width, int a_height)
{
// update window size
width = a_width;
height = a_height;
// Update the size of the sideview framebuffer
int side = 0.3 * cMin(width, height);
frameBuffer->setSize(side, side);
int radius = 0.25 * side;
sideViewPanel->setSize(side, side);
sideViewPanel->setCornerRadius(radius, radius, radius, radius);
}
//------------------------------------------------------------------------------
void errorCallback(int a_error, const char* a_description)
{
cout << "Error: " << a_description << endl;
}
//---------------------------------------------------------------------------
void keyCallback(GLFWwindow* a_window, int a_key, int a_scancode, int a_action, int a_mods)
{
// filter calls that only include a key press
if ((a_action != GLFW_PRESS) && (a_action != GLFW_REPEAT))
{
return;
}
// option - exit
else if ((a_key == GLFW_KEY_ESCAPE) || (a_key == GLFW_KEY_Q))
{
glfwSetWindowShouldClose(a_window, GLFW_TRUE);
}
// option - show/hide skeleton
else if (a_key == GLFW_KEY_S)
{
defObject->m_showSkeletonModel = !defObject->m_showSkeletonModel;
defObject2->m_showSkeletonModel = !defObject2->m_showSkeletonModel;
defObject3->m_showSkeletonModel = !defObject3->m_showSkeletonModel;
}
// option - toggle fullscreen
else if (a_key == GLFW_KEY_F)
{
// toggle state variable
fullscreen = !fullscreen;
// get handle to monitor
GLFWmonitor* monitor = glfwGetPrimaryMonitor();
// get information about monitor
const GLFWvidmode* mode = glfwGetVideoMode(monitor);
// set fullscreen or window mode
if (fullscreen)
{
glfwSetWindowMonitor(window, monitor, 0, 0, mode->width, mode->height, mode->refreshRate);
glfwSwapInterval(swapInterval);
}
else
{
int w = 0.8 * mode->height;
int h = 0.5 * mode->height;
int x = 0.5 * (mode->width - w);
int y = 0.5 * (mode->height - h);
glfwSetWindowMonitor(window, NULL, x, y, w, h, mode->refreshRate);
glfwSwapInterval(swapInterval);
}
}
// option - toggle vertical mirroring
else if (a_key == GLFW_KEY_M)
{
mirroredDisplay = !mirroredDisplay;
camera->setMirrorVertical(mirroredDisplay);
}
}
//---------------------------------------------------------------------------
void close(void)
{
// stop the simulation
simulationRunning = false;
// wait for graphics and haptics loops to terminate
while (!simulationFinished) { cSleepMs(100); }
// close haptic device
hapticDevice->close();
// delete resources
delete hapticsThread;
delete world;
delete handler;
}
//---------------------------------------------------------------------------
void updateGraphics(void)
{
/////////////////////////////////////////////////////////////////////
// UPDATE WIDGETS
/////////////////////////////////////////////////////////////////////
// update haptic and graphic rate data
labelRates->setText(cStr(freqCounterGraphics.getFrequency(), 0) + " Hz / " +
cStr(freqCounterHaptics.getFrequency(), 0) + " Hz");
// update position of label
labelRates->setLocalPos((int)(0.5 * (width - labelRates->getWidth())), 15);
/////////////////////////////////////////////////////////////////////
// UPDATE DEFORMABLE MODELS
/////////////////////////////////////////////////////////////////////
// update skins deformable objects
defWorld->updateSkins(true);
/////////////////////////////////////////////////////////////////////
// RENDER SCENE
/////////////////////////////////////////////////////////////////////
// update shadow maps (if any)
world->updateShadowMaps(false, mirroredDisplay);
// render the side view framebuffer
frameBuffer->renderView();
// render world
camera->renderView(width, height);
// wait until all GL commands are completed
glFinish();
// check for any OpenGL errors
GLenum err;
err = glGetError();
if (err != GL_NO_ERROR) cout << "Error: " << gluErrorString(err) << endl;
}
//---------------------------------------------------------------------------
void updateHaptics(void)
{
// initialize precision clock
cPrecisionClock clock;
clock.reset();
// simulation in now running
simulationRunning = true;
simulationFinished = false;
// main haptic simulation loop
while (simulationRunning)
{
// stop clock
double time = cMin(0.001, clock.stop());
// restart clock
clock.start(true);
// read position from haptic device
cVector3d pos;
hapticDevice->getPosition(pos);
pos.mul(workspaceScaleFactor);
device->setLocalPos(pos);
// clear all external forces
defWorld->clearExternalForces();
// compute reaction forces
cVector3d force(0.0, 0.0, 0.0);
for (int y = 0; y < 10; y++)
{
for (int x = 0; x < 10; x++)
{
cVector3d nodePos = nodes[x][y]->m_pos;
nodePos.z(nodePos.z() + z_layer1);
cVector3d f = computeForce(pos, deviceRadius, nodePos, radius, stiffnesslayer1);
cVector3d tmpfrc = -1.0 * f;
nodes[x][y]->setExternalForce(tmpfrc);
force.add(f);
// std::cout << "Force 1 " << f << std::endl;
// std::cout << "force " << x << " " << y << " " << f << std::endl;
// std::cout << "Node 1 " << nodePos << std::endl;
// Layer 2
cVector3d nodePos2 = nodes2[x][y]->m_pos;
nodePos.z(nodePos.z() + z_layer2);
cVector3d f2 = computeForce(pos, deviceRadius, nodePos2, radius, stiffnesslayer2);
cVector3d tmpfrc2 = -1.0 * f2;
nodes2[x][y]->setExternalForce(tmpfrc2);
force.add(f2);
// std::cout << "force2 " << x << " " << y << " " << f2 << std::endl;
// std::cout << "Node 2 " << nodePos2 << std::endl;
// Layer 3
cVector3d nodePos3 = nodes3[x][y]->m_pos;
nodePos3.z(nodePos3.z() + z_layer3);
cVector3d f3 = computeForce(pos, deviceRadius, nodePos3, radius, stiffnesslayer3);
cVector3d tmpfrc3 = -1.0 * f3;
nodes3[x][y]->setExternalForce(tmpfrc3);
force.add(f3);
}
}
// integrate dynamics
defWorld->updateDynamics(time);
// scale force
force.mul(deviceForceScale / workspaceScaleFactor);
// send forces to haptic device
hapticDevice->setForce(force);
// signal frequency counter
freqCounterHaptics.signal(1);
}
//exit haptics thread
simulationFinished = true;
}
//---------------------------------------------------------------------------
cVector3d computeForce(const cVector3d& a_cursor,
double a_cursorRadius,
const cVector3d& a_spherePos,
double a_radius,
double a_stiffness)
{
// compute the reaction forces between the tool and the ith sphere.
cVector3d force;
force.zero();
cVector3d vSphereCursor = a_cursor - a_spherePos;
// check if both objects are intersecting
if (vSphereCursor.length() < 0.0000001)
{
return (force);
}
if (vSphereCursor.length() > (a_cursorRadius + a_radius))
{
return (force);
}
// compute penetration distance between tool and surface of sphere
double penetrationDistance = (a_cursorRadius + a_radius) - vSphereCursor.length();
cVector3d forceDirection = cNormalize(vSphereCursor);
force = cMul(penetrationDistance * a_stiffness, forceDirection);
// return result
return (force);
}
//---------------------------------------------------------------------------