add guidewire simulation

demo/guideWireSimulation/.editorconfig

+# Rules in this file were initially inferred by Visual Studio IntelliCode from the D:\PROJECTS\SimplerRenderer\guideWireSimulation codebase based on best match to current usage at 2021/5/15
+# You can modify the rules from these initially generated values to suit your own policies
+# You can learn more about editorconfig here: https://docs.microsoft.com/en-us/visualstudio/ide/editorconfig-code-style-settings-reference
+[*.cs]
+

demo/guideWireSimulation/CMakeLists.txt

+cmake_minimum_required(VERSION 3.0)
+project(guideWireSimulation)
+
+set(Qt5_DIR "C:\\Qt\\5.15.0\\msvc2019_64\\lib\\cmake\\Qt5")
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTORCC ON)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+FIND_PACKAGE(Qt5 COMPONENTS Core Gui Qml Quick Widgets REQUIRED)
+#FIND_PACKAGE(Qt5 COMPONENTS Widgets REQUIRED)
+find_package(CGAL)
+find_package(Boost)
+
+#Boost
+set(Boost_INCLUDE_DIR D:\\boost\\boost_1_71_0)
+set(Boost_LIBRARIES_DIR D:\\boost\\boost_1_71_0\\lib64-msvc-14.2)
+
+#dtk
+set(DTK_INCLUDE_DIR D:\\PROJECTS\\SimplerRenderer\\dtk\\install\\include)
+set(DTK_LIBRARIES_DIR D:\\PROJECTS\\SimplerRenderer\\dtk\\install\\lib)
+
+#freeglut
+set(FREEGLUT_INCLUDE_DIR D:\\PROJECTS\\SimplerRenderer\\freeglut-3.2.1\\include)
+set(FREEGLUT_LIBRARIES_DIR D:\\PROJECTS\\SimplerRenderer\\freeglut-3.2.1\\build\\lib)
+
+#gmp
+set(GMP_INCLUDE_DIR D:\\CGAL\\CGAL-4.14.3\\auxiliary\\gmp\\include)
+set(GMP_LIBRARIES_DIR D:\\CGAL\\CGAL-4.14.3\\auxiliary\\gmp\\lib)
+
+aux_source_directory(. SOURCE_FILES)
+file(GLOB HEADER_FILES *.h)
+
+set(GUIDEWIRE_HEADERS 
+	glDrawer.h
+	glThread.h
+	guideWireSimulation.h
+)
+
+set(GUIDEWIRE_FORMS 
+	simulationParameters.ui
+	guideWireSimulation.ui
+)
+
+set(GUIDEWIRE_RESOURCES 
+	guideWireSimulation.qrc
+)
+
+#调用预编译器moc，需要使用 QT5_WRAP_CPP宏
+QT5_WRAP_CPP(GUIDEWIRE_MOC_HEADERS ${GUIDEWIRE_HEADERS})
+
+#使用uic处理.ui文件
+QT5_WRAP_UI(GUIDEWIRE_FORMS_HEADERS ${GUIDEWIRE_FORMS})
+
+#使用rcc处理.qrc文件
+QT5_ADD_RESOURCES(GUIDEWIRE_RCC_SOURCES ${GUIDEWIRE_RESOURCES})
+
+#这些生成的中间文件都会在build目录下，这样的话，编译器则不能定位由uic程序产生的诸如_ui_mainwindow.h等文件。所以，我们需要把build目录添加到包含目录中
+INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR})
+INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${DTK_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${FREEGLUT_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${GMP_INCLUDE_DIR})
+
+LINK_DIRECTORIES(${Boost_LIBRARIES_DIR})
+LINK_DIRECTORIES(${DTK_LIBRARIES_DIR})
+LINK_DIRECTORIES(${FREEGLUT_LIBRARIES_DIR})
+LINK_DIRECTORIES(${GMP_LIBRARIES_DIR})
+
+
+#生成可执行文件
+ADD_EXECUTABLE(GUIDEWIRE 
+	${GUIDEWIRE_SOURCES} 
+	${GUIDEWIRE_MOC_HEADERS} 
+    	${GUIDEWIRE_FORMS_HEADERS}
+	${GUIDEWIRE_RCC_SOURCES} 
+	${SOURCE_FILES}
+	${HEADER_FILES}
+)
+
+#为target添加需要链接的共享库
+TARGET_LINK_LIBRARIES(GUIDEWIRE Qt5::Core Qt5::Gui Qt5::Qml Qt5::Quick ${Qt5Widgets_LIBRARIES})
+#TARGET_LINK_LIBRARIES(GUIDEWIRE ${Qt5Widgets_LIBRARIES})
+TARGET_LINK_LIBRARIES(GUIDEWIRE CGAL::CGAL)
+TARGET_LINK_LIBRARIES(GUIDEWIRE dtk)
+TARGET_LINK_LIBRARIES(GUIDEWIRE ${Boost_LIBRARIES})
+TARGET_LINK_LIBRARIES(GUIDEWIRE ${FREEGLUT_LIBRARIES})
+TARGET_LINK_LIBRARIES(GUIDEWIRE libgmp-10.lib libgmp-10.dll)
+TARGET_LINK_LIBRARIES(GUIDEWIRE libmpfr-4.lib libmpfr-4.dll)
+
+INSTALL(TARGETS GUIDEWIRE
+    RUNTIME DESTINATION lib
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+)

demo/guideWireSimulation/README.md

+# guideWireSimulation
+
+A blood flow induced physical simulation of guidewire shape for virtual vascular intervention training system in real time. Virtual vascular intervention training system, which is a low cost, safe and effective solution, is able to provide an immersive virtual training environment for trainees.
+
+## Dependencies
+
+### [Boost](https://github.com/boostorg/boost)
+
+  Boost is a mandatory dependency of CGAL. Binary versions of Boost are available on SourceForge. The Boost installers install both Boost headers and precompiled libraries. Please note that the CGAL project is not responsible for the files provided on this website. When CGAL 5.2.1 was released, the latest version of Boost was 1.71. A typical installation of Boost would consist of the following steps:
+
+  Download and run the file boost_1_71_0-msvc-XX.Y-64.exe (where XX.Y = 14.0 for VC 2015, XX.Y = 14.1 for 2017, XX.Y = 14.2 for VC 2019).
+Extract the files to a new directory, e.g. c:\dev\libboost_1_71_0.
+Set the following two environment variables to point respectively to the path of the libraries and the headers
+```bash
+  BOOST_LIBRARYDIR = C:\dev\libboost_1_71_0\lib64-msvc-XX.Y
+  BOOST_INCLUDEDIR = C:\dev\libboost_1_71_0
+```
+as this will help cmake to find Boost.
+Add the path to the Boost dlls (C:\dev\libboost_1_71_0\lib64-msvc-XX.Y) files to the PATH environment variable.
+
+
+### [CGAL](https://github.com/CGAL/cgal)
+
+  CGAL offers data structures and algorithms like triangulations, Voronoi diagrams, Polygons, Cell Complexes and Polyhedra, arrangements of curves, mesh generation, geometry processing, convex hull algorithms, to name just a few.
+
+  All these data structures and algorithms operate on geometric objects like points and segments, and perform geometric tests on them. These objects and predicates are regrouped in CGAL Kernels.
+
+  Finally, the Support Library offers geometric object generators and spatial sorting functions, as well as a matrix search framework and a solver for linear and quadratic programs. It further offers interfaces to third party software such as the GUI libraries Qt, Geomview, and the Boost Graph Library.
+
+  [How to install CGAL](https://doc.cgal.org/latest/Manual/index.html)
+
+### [Qt5](https://doc.qt.io/qt-5/qt5-intro.html)
+
+ Qt is a full development framework with tools designed to streamline the creation of applications and user interfaces for desktop, embedded, and mobile platforms.
+
+ GuideWire simulation build GUI by QT5.
+
+  [How to build Qt with CMAKE](https://doc.qt.io/qt-5/cmake-get-started.html#build-a-gui-executable)
+
+### [freeglut](http://freeglut.sourceforge.net)
+
+ Freeglut, the Free OpenGL Utility Toolkit, is meant to be a free alternative to Mark Kilgard's GLUT library. 
+
+ GuideWire Render by Opengl and freeglut.
+
+  [How to build freeglut with CMAKE](https://doc.qt.io/qt-5/cmake-get-started.html#build-a-gui-executable)
+
+### [dtk](https://github.com/Deformable-Toolkit/dtk)
+
+ A deformable toolkit used for deforamable physical simulation
+
+ GuideWire simulation build GUI by QT5.
+
+  [How to build dtk with CMAKE](https://github.com/Deformable-Toolkit/dtk)
+
+
+## How to build
+
+  guidewire simuation is build by CMake. so you can build easily by CMAKE GUI or command line. 
+
+### build with CLI
+
+Before congiguration. Modify the CMakeLists.txt in current folder first.
+
+#### build executable program
+
+  First，configuarte with command:
+
+```bash
+$ cmake -S "SRC_DIR" -B "DESTINATION_DIR" -G "Generator"
+```
+
+  second, compile with:
+
+```bash
+$ cmake --build "DESTINATION_DIR" --config Release
+```
+
+### example
+
+<div align = center>
+![guideWire](./guideWire.jpg)
+</div>
\ No newline at end of file

demo/guideWireSimulation/bak/DevicePhantomOmni.cpp

+#include "DevicePhantomOmni.h"
+
+HHD DevicePhantomOmni::ghHD;
+hduVector3Dd DevicePhantomOmni::mForce;
+hduVector3Dd DevicePhantomOmni::mLastForce;
+hduVector3Dd DevicePhantomOmni::mCurrentHaticForce;
+HDulong DevicePhantomOmni::mForceRate;
+HDulong DevicePhantomOmni::mHapticRate;
+bool DevicePhantomOmni::mPeriodStartFlag;
+int	 DevicePhantomOmni::mPeriodHapticTimes;
+
+DevicePhantomOmni::DevicePhantomOmni()
+{
+	ghHD = HD_INVALID_HANDLE;
+	hUpdateDeviceCallback = HD_INVALID_HANDLE;
+	mHapticRate = 1000;
+	mForceRate = 1000;
+	mCurrentHaticForce = hduVector3Dd(0, 0, 0);
+	mPeriodStartFlag = false;
+	mPeriodHapticTimes = 0;
+}
+
+void DevicePhantomOmni::InitHD()
+{
+	ghHD = hdInitDevice(HD_DEFAULT_DEVICE);
+
+	hdEnable(HD_FORCE_OUTPUT);
+	hUpdateDeviceCallback = hdScheduleAsynchronous(
+		TouchScene, 0, HD_MAX_SCHEDULER_PRIORITY);
+
+	hdStartScheduler();
+}
+
+void DevicePhantomOmni::SetHapticForce(hduVector3Dd force, HDulong forceRate)
+{
+	// 判断force是不是超过了phantom Omni的输出最大值，如果超过则对超过部分进行截短
+	HDdouble maxForce;
+	hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE , &maxForce);
+	if (hduVecMagnitude(force) > maxForce)
+	{
+		hduVecNormalizeInPlace(force);
+		hduVecScaleInPlace(force, maxForce);
+	}
+	mLastForce = mForce;
+	mForce = force;
+	mForceRate = forceRate;
+	mPeriodStartFlag = true;
+}
+
+void DevicePhantomOmni::SetHapticRate(HDulong hapticRate)
+{
+	mHapticRate = hapticRate;
+	hdSetSchedulerRate(hapticRate);
+}
+
+HDCallbackCode HDCALLBACK DevicePhantomOmni::TouchScene(void *pUserData)
+{
+	//根据SetHapticForce来产生连续的力触觉
+	//hduVector3Dd torqueV;
+
+	if (mPeriodStartFlag)
+		mPeriodHapticTimes = 0;
+	mPeriodStartFlag = false;
+	mPeriodHapticTimes ++;
+	hdBeginFrame(ghHD);
+	HDulong times = mHapticRate / mForceRate;
+	hduVector3Dd clampForce = (mForce - mLastForce) / times;
+	if (mPeriodHapticTimes <= times)
+		mCurrentHaticForce = mCurrentHaticForce + clampForce;
+	hdSetDoublev(HD_CURRENT_FORCE, mCurrentHaticForce);
+	hdSetDoublev(HD_CURRENT_TORQUE, hduVector3Dd(5, 5, 5)); 
+	//hdGetDoublev(HD_CURRENT_TORQUE, torqueV);
+	hdEndFrame(ghHD);
+	return HD_CALLBACK_CONTINUE;
+}
+
+HDCallbackCode HDCALLBACK DevicePhantomOmni::CopyHapticDisplayState(void *pUserData)
+{
+	int currentButtons;
+	HapticDisplayState *pState = (HapticDisplayState *) pUserData;
+
+	hdGetDoublev(HD_CURRENT_POSITION, pState->position);
+	hdGetDoublev(HD_CURRENT_TRANSFORM, pState->transform);
+	hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, pState->gimbalAngle);
+	//hdGetDoublev(HD_CURRENT_TORQUE, torqueV);
+	 
+	hdGetIntegerv(HD_CURRENT_BUTTONS, &currentButtons);
+	pState->button1 = currentButtons & HD_DEVICE_BUTTON_1;
+	pState->button2 = currentButtons & HD_DEVICE_BUTTON_2;
+
+	return HD_CALLBACK_DONE;
+}
+
+
+HapticDisplayState * DevicePhantomOmni::GetCurrentDisplayState()
+{
+	mLastDisplayState = mCurrentDisplayState;
+	hdScheduleSynchronous(CopyHapticDisplayState, &mCurrentDisplayState, HD_DEFAULT_SCHEDULER_PRIORITY);
+	return &mCurrentDisplayState;
+}
+
+HapticDisplayState * DevicePhantomOmni::GetLastDisplayState()
+{
+	return &mLastDisplayState;
+}
+
+HDdouble * DevicePhantomOmni::GetWorkspaceModel(const HDdouble * modelMatrix, const HDdouble *projMatrix)
+{
+	hduMapWorkspaceModel(modelMatrix, projMatrix, mWorkspaceModel);
+	return mWorkspaceModel;
+}
+
+
+DevicePhantomOmni::~DevicePhantomOmni()
+{
+	// nothing
+}
+

demo/guideWireSimulation/bak/DevicePhantomOmni.h

+#ifndef DEVICEPHANTOMOMNI_H
+#define DEVICEPHANTOMOMNI_H
+
+#include <cmath>
+#include <memory>
+
+
+#include <HD/hd.h>
+#include <HDU/hdu.h>
+#include <HDU/hduVector.h>
+
+
+typedef struct
+{
+	hduVector3Dd position;
+	hduVector3Dd gimbalAngle;
+	HDdouble transform[16];
+	bool button1;
+	bool button2;	
+} HapticDisplayState;
+
+
+
+class DevicePhantomOmni
+{
+public:
+	typedef std::shared_ptr<DevicePhantomOmni>Ptr;
+	static Ptr New()
+	{
+		return Ptr(new DevicePhantomOmni);
+	}
+	DevicePhantomOmni();
+	void InitHD();
+	void SetHapticForce(hduVector3Dd force, HDulong forceRate);
+	void SetHapticRate(HDulong hapticRate);
+	
+	HapticDisplayState * GetCurrentDisplayState();
+	HapticDisplayState * GetLastDisplayState();
+
+	HDdouble * GetWorkspaceModel(const HDdouble * modelMatrix, const HDdouble *projMatrix);
+	~DevicePhantomOmni();
+	static HDCallbackCode HDCALLBACK TouchScene(void *pUserData);
+	static HDCallbackCode HDCALLBACK CopyHapticDisplayState(void *pUserData);
+	static HHD ghHD;
+	static hduVector3Dd mForce;
+	static hduVector3Dd mLastForce;
+	static hduVector3Dd mCurrentHaticForce;
+	static HDulong mForceRate;
+	static HDulong mHapticRate;
+	static bool mPeriodStartFlag;
+	static int	mPeriodHapticTimes;
+
+
+
+private:
+	HapticDisplayState mCurrentDisplayState;
+	HapticDisplayState mLastDisplayState;
+	hduVector3Dd mAngularVelocity;
+
+	HDdouble mWorkspaceModel[16];
+
+	HDSchedulerHandle hUpdateDeviceCallback;
+	
+	
+};
+
+#endif

demo/guideWireSimulation/bak/guideWireSimulation.cpp

+#include "guidewiresimulation.h"
+#include "glDrawer.h"
+#include <QAction>
+#include <QFileDialog>
+#include <QMessageBox>
+#include <QTimer>
+#include <QLabel>
+
+
+
+// global variable
+
+guideWireSimulation::guideWireSimulation(QWidget *parent, Qt::WFlags flags)
+	: QMainWindow(parent, flags)
+{
+	// setup ui
+	SimulationParameterdialog = new QDialog(this);
+	simuParamsWidget.setupUi(SimulationParameterdialog);
+	ui.setupUi(this);
+	this->setWindowIcon(QIcon(":/Resources/igst.jpg"));
+	this->setWindowTitle(QString("real-time simulate guide wire in minimally invasive vascular interventions"));
+
+	OpenGLWidget = new GLDrawer(ui.centralWidget);
+	ui.gridLayout->addWidget(OpenGLWidget);
+
+	// set toolbar
+	fileOpenAction = new QAction(tr("&File"), this); 
+	fileOpenAction->setStatusTip(tr("Load  vascular netgrid.")); 
+	fileOpenAction->setIcon(QIcon(":/Resources/load_vascular.png")); 
+	connect(fileOpenAction, SIGNAL(triggered()), this, SLOT(open())); 
+
+	startAction = new QAction(tr("&Start"), this); 
+	startAction->setStatusTip(tr("Start simulation.")); 
+	startAction->setIcon(QIcon(":/Resources/start.png")); 
+	connect(startAction, SIGNAL(triggered()), this, SLOT(start())); 
+
+	pauseAction = new QAction(tr("&Pause"), this); 
+	pauseAction->setStatusTip(tr("Pause  simulation.")); 
+	pauseAction->setIcon(QIcon(":/Resources/pause.png")); 
+	connect(pauseAction, SIGNAL(triggered()), this, SLOT(pause())); 
+
+
+	quitAction = new QAction(tr("&Quit"), this); 
+	quitAction->setStatusTip(tr("Quit  simulation.")); 
+	quitAction->setIcon(QIcon(":/Resources/quit.png")); 
+	connect(quitAction, SIGNAL(triggered()), this, SLOT(quit())); 
+
+	centerAction = new QAction(tr("&Center"), this); 
+	centerAction->setStatusTip(tr("Place vasulcar in center.")); 
+	centerAction->setIcon(QIcon(":/Resources/center.png")); 
+	connect(centerAction, SIGNAL(triggered()), this, SLOT(center())); 
+
+	zoomOutAction = new QAction(tr("&Zoom Out"), this); 
+	zoomOutAction->setStatusTip(tr("Zoom out.")); 
+	zoomOutAction->setIcon(QIcon(":/Resources/zoom-out.png")); 
+	connect(zoomOutAction, SIGNAL(triggered()), this, SLOT(zoomOut())); 
+
+	zoomInAction = new QAction(tr("&Zoom In"), this); 
+	zoomInAction->setStatusTip(tr("Zoom in.")); 
+	zoomInAction->setIcon(QIcon(":/Resources/zoom-in.png")); 
+	connect(zoomInAction, SIGNAL(triggered()), this, SLOT(zoomIn())); 
+
+
+	ui.toolBar->addAction(fileOpenAction);
+	ui.toolBar->addAction(startAction);
+	ui.toolBar->addAction(pauseAction);
+	ui.toolBar->addAction(quitAction);
+	ui.toolBar->addAction(centerAction);
+	ui.toolBar->addAction(zoomOutAction);
+	ui.toolBar->addAction(zoomInAction);
+
+	// set focus
+	OpenGLWidget->setFocusPolicy(Qt::StrongFocus);
+
+
+	// set menu bar connect 
+	connect(ui.actionFileOpen, SIGNAL(triggered()), this, SLOT(open()));
+	connect(ui.actionSimulationParams, SIGNAL(triggered()), this, SLOT(simulationParams()));
+	connect(ui.actionAbout, SIGNAL(triggered()), this, SLOT(about()));
+	connect(ui.actionCenter, SIGNAL(triggered()), this, SLOT(center()));
+	connect(ui.actionHowDoI, SIGNAL(triggered()), this, SLOT(howDoI()));
+	connect(ui.actionQuit, SIGNAL(triggered()), this, SLOT(quit()));
+	connect(ui.actionStart, SIGNAL(triggered()), this, SLOT(start()));
+	connect(ui.actionPause, SIGNAL(triggered()), this, SLOT(pause()));
+	connect(ui.actionZoomIn, SIGNAL(triggered()), this, SLOT(zoomIn()));
+	connect(ui.actionZoomOut, SIGNAL(triggered()), this, SLOT(zoomOut()));
+
+	// set simulation parameters connect
+	connect(simuParamsWidget.horizontalSlidergwStiff, SIGNAL(valueChanged(int)), simuParamsWidget.spinBoxgwStiff, SLOT(setValue(int)));
+	connect(simuParamsWidget.horizontalSliderVascularStiff, SIGNAL(valueChanged(int)), simuParamsWidget.spinBoxVascularStiff, SLOT(setValue(int)));
+
+	connect(simuParamsWidget.spinBoxgwStiff, SIGNAL(valueChanged(int)), simuParamsWidget.horizontalSlidergwStiff, SLOT(setValue(int)));
+	connect(simuParamsWidget.spinBoxVascularStiff, SIGNAL(valueChanged(int)), simuParamsWidget.horizontalSliderVascularStiff, SLOT(setValue(int)));
+
+	connect(simuParamsWidget.pushButtonApply, SIGNAL(pressed()), this, SLOT(apply()));
+	connect(simuParamsWidget.pushButtonCancel, SIGNAL(pressed()), this, SLOT(cancel()));
+
+	// set toolbar and menubar status
+	startAction->setEnabled(false);
+	ui.actionStart->setEnabled(false);
+
+	// create new phantom omni ptr and initiation
+	mDevicePhantomOmni = DevicePhantomOmni::New();
+	mDevicePhantomOmni->InitHD();
+	mCurrentDisplayState = mDevicePhantomOmni->GetCurrentDisplayState();
+	mLastDisplayState = mDevicePhantomOmni->GetLastDisplayState();
+	mPhantomOmniVelocity = hduVector3Dd(0, 0, 0);
+	mPhantomOmniTwistVelocity = hduVector3Dd(0, 0, 0);
+	mTimes = 0;		
+}
+
+void guideWireSimulation::open()
+{
+	QFileDialog* fd = new QFileDialog(this);
+	fd->resize(600,400);  
+	fd->setFilter( "*.s3m *.S3M *.S3m *.s3M");
+	fd->setViewMode(QFileDialog::List);  
+	fd->setDirectory(QDir(QString("..//data")));
+	if ( fd->exec() == QDialog::Accepted )  
+	{
+		OpenGLWidget->vascularFile =  (fd->selectedFiles()[0]).toStdString();
+		OpenGLWidget->initializeVascularModel();
+		OpenGLWidget->centerCount = 0;
+		center();
+		startAction->setEnabled(true);
+		ui.actionStart->setEnabled(false);
+	}
+	fd->close();
+	delete fd;
+}
+
+void guideWireSimulation::start()
+{
+	startAction->setEnabled(false);
+	ui.actionStart->setEnabled(false);
+	fileOpenAction->setEnabled(false);
+	ui.actionFileOpen->setEnabled(false);
+
+	OpenGLWidget->initializeGWModel();
+	OpenGLWidget->updateGL();
+
+	timer = new QTimer(this);
+	timer->start(20);
+	connect(timer, SIGNAL(timeout()), this, SLOT(idle()));
+
+}
+
+void guideWireSimulation::idle()
+{
+	// 获取Phantom Omni的状态信息，计算Phantom Omni手柄的速度
+	mDevicePhantomOmni->GetCurrentDisplayState();
+
+	if (mCurrentDisplayState->button1)
+	{
+		// 计算用户给导丝的外力
+		mPhantomOmniVelocity = (mCurrentDisplayState->position - mLastDisplayState->position) * 200.0; 
+		(OpenGLWidget->core)->ApplyExternalForce(1, dtkT3<double>(0, -mPhantomOmniVelocity[2], 0) );
+	}
+	else
+	{
+		(OpenGLWidget->core)->ApplyExternalForce(1, dtkT3<double>(0, 0, 0) );
+	}
+
+	// 计算用户给导丝的扭曲力
+	if (mCurrentDisplayState->button2)
+	{
+		mTimes ++;
+		mPhantomOmniTwistVelocity += mCurrentDisplayState->gimbalAngle - mLastDisplayState->gimbalAngle;
+		if (mTimes % 10 == 0)
+		{
+			(OpenGLWidget->core)->ApplyExternalTwist(1, mPhantomOmniTwistVelocity[2]);
+			mTimes = 0;
+			mPhantomOmniTwistVelocity = hduVector3Dd(0, 0, 0);
+		}
+	}
+	
+	(OpenGLWidget->core)->Update(OpenGLWidget->timeslice, OpenGLWidget->avoid);
+	OpenGLWidget->updateGL();
+
+	// 将用户受到的外力实时传递给力反馈Phantom Omni
+	dtkT3<double> dtkForce = (OpenGLWidget->core)->GetHapticTranslationForce();
+	if (dtkForce.y > 0)
+		dtkForce =  dtkForce + (OpenGLWidget->core)->GetHapticCollisionForce();
+	
+	hduVector3Dd hduForce = hduVector3Dd(dtkForce.x, dtkForce.z, dtkForce.y);
+	mDevicePhantomOmni->SetHapticForce(hduForce, 50);
+}
+
+void guideWireSimulation::pause()
+{
+
+}
+
+
+void guideWireSimulation::quit()
+{
+	QMessageBox messagebox(QString("Quit Program?"), QString("Do you real want to quit program?"), QMessageBox::Information, QMessageBox::Yes, QMessageBox::NoButton, QMessageBox::No, this);
+	//messagebox.show();
+	
+	if (messagebox.exec() == QMessageBox::No)
+		return;
+	else
+	{
+		this->close();
+	}
+}
+
+void guideWireSimulation::center()
+{
+
+	OpenGLWidget->xRotate = 0;
+	OpenGLWidget->yRotate = 0;
+	OpenGLWidget->xscale = 1;
+	OpenGLWidget->yscale = 1;
+	OpenGLWidget->zscale = 1;
+	OpenGLWidget->xTranslate = 0;
+	OpenGLWidget->yTranslate = 0;
+
+	if (OpenGLWidget->centerCount == 0)
+	{
+		OpenGLWidget->centerCount = 1;
+		// compute center position
+		dtkPoints::Ptr vascularPoints = (OpenGLWidget->core->mTriangleMeshes[0])->GetPoints();
+		double xmin, xmax; 
+		double ymin, ymax;
+		double zmin, zmax;
+		xmin = ymin = zmin = 10000;
+		xmax = ymax = zmax = -10000;
+		dtkID size = vascularPoints->GetNumberOfPoints();
+		GK::Point3 point;
+		for (dtkID i = 0; i < size; i++)
+		{
+			point = vascularPoints->GetPoint(i);
+
+			if (point.x() < xmin)
+				xmin = point.x();
+
+			if (point.x() > xmax)
+				xmax = point.x();
+
+			if (point.y() < ymin)
+				ymin = point.y();
+
+			if (point.y() > ymax)
+				ymax = point.y();
+
+			if (point.z() < zmin)
+				zmin = point.z();
+
+			if (point.z() > zmax)
+				zmax = point.z();
+		}
+
+		double maxAxis = xmax - xmin;
+		int indexAxis = 0; // 0:x, 1:y, 2:z
+		if (ymax - ymin > maxAxis)
+		{
+			maxAxis = ymax - ymin;
+			indexAxis = 1;
+		}
+		if (zmax - zmin > maxAxis)
+		{
+			maxAxis = zmax - zmin;
+			indexAxis = 2;
+		}
+
+		if (indexAxis == 0)
+		{
+			OpenGLWidget->upVector = dtkT3<double>(1, 0, 0);
+			OpenGLWidget->eyePosition = dtkT3<double>((xmin + xmax) / 2, ymin , (zmin + zmax) / 2);
+			OpenGLWidget->centerPosition = dtkT3<double>((OpenGLWidget->eyePosition).x, ymax, (OpenGLWidget-> eyePosition).z);
+			
+			OpenGLWidget->screenXAxis = dtkT3<double>(0, 0, 1);
+
+		}
+		else if (indexAxis == 1)
+		{
+			OpenGLWidget->upVector = dtkT3<double>(0, 1, 0);
+			OpenGLWidget->eyePosition = dtkT3<double>((xmin + xmax) / 2, (ymin + ymax) / 2, zmin);
+
+			OpenGLWidget->centerPosition = dtkT3<double>((OpenGLWidget->eyePosition).x, (OpenGLWidget->eyePosition).y,  zmax);
+
+			OpenGLWidget->screenXAxis = dtkT3<double>(1, 0, 0);
+
+		}
+		else
+		{
+			OpenGLWidget->upVector = dtkT3<double>(0, 0, 1);
+			OpenGLWidget->eyePosition = dtkT3<double>(xmin , (ymin + ymax) / 2, (zmin + zmax) / 2);
+			OpenGLWidget->centerPosition = dtkT3<double>((xmax, OpenGLWidget->eyePosition).y, (OpenGLWidget->eyePosition).z);
+
+			OpenGLWidget->screenXAxis = dtkT3<double>(0, 1, 0);
+		}
+
+	}
+	
+
+	OpenGLWidget->updateGL();
+
+}
+
+void guideWireSimulation::zoomOut()
+{
+	OpenGLWidget->xscale *= 1.2;
+	OpenGLWidget->yscale *= 1.2;
+	OpenGLWidget->zscale *= 1.2;
+	OpenGLWidget->updateGL();
+}
+
+void guideWireSimulation::zoomIn()
+{
+	OpenGLWidget->xscale /= 1.2;
+	OpenGLWidget->yscale /= 1.2;
+	OpenGLWidget->zscale /= 1.2;
+	OpenGLWidget->updateGL();
+}
+
+void guideWireSimulation::simulationParams()
+{
+	SimulationParameterdialog->exec();
+}
+
+void guideWireSimulation::about()
+{
+	QString aboutString("This is real-time simulate guide wire in minimally invasive vascular interventions \n ");
+	QMessageBox messagebox(QString("About"), aboutString, QMessageBox::Information, QMessageBox::NoButton, QMessageBox::NoButton, QMessageBox::NoButton, this);
+	messagebox.exec();
+}
+
+void guideWireSimulation::howDoI()
+{
+	QString operationGuide = QString("push guide wire :                     w\n");
+	operationGuide = operationGuide + QString("pull  guide wire :                      s\n");
+	operationGuide = operationGuide + QString("left turn guide wire tip :           a\n");
+	operationGuide = operationGuide + QString("right turn guide wire tip :         d\n");
+	operationGuide = operationGuide + QString("translate up view frustum :      5\n");
+	operationGuide = operationGuide + QString("translate down view frustum : 8\n");
+	operationGuide = operationGuide + QString("translate left view frustum :     7\n");
+	operationGuide = operationGuide + QString("translate right view frustum :   9\n");
+
+	QMessageBox messagebox(QString("How Do I"), operationGuide, QMessageBox::Information, QMessageBox::NoButton, QMessageBox::NoButton, QMessageBox::NoButton, this);
+	messagebox.exec();
+
+}
+
+void guideWireSimulation::apply()
+{
+	// set simulation parameters
+
+	// close the dialog
+	SimulationParameterdialog->close();
+}
+
+void guideWireSimulation::cancel()
+{
+	// close the dialog
+	SimulationParameterdialog->close();
+}
+
+guideWireSimulation::~guideWireSimulation()
+{
+	
+}
+

demo/guideWireSimulation/bak/guideWireSimulation.h

+#ifndef GUIDEWIRESIMULATION_H
+#define GUIDEWIRESIMULATION_H
+
+#include <QtGui/QMainWindow>
+#include "ui_guidewiresimulation.h"
+#include "ui_simulationParameters.h"
+#include "DevicePhantomOmni.h"
+
+
+class GLDrawer;
+class QTimer;
+class guideWireSimulation : public QMainWindow
+{
+	Q_OBJECT
+
+public:
+	guideWireSimulation(QWidget *parent = 0, Qt::WFlags flags = 0);
+	~guideWireSimulation();
+
+	public slots:
+		void open();
+		void simulationParams();
+		void start();
+		void pause();
+		void quit();
+		void center();
+		void zoomOut();
+		void zoomIn();
+		void about();
+		void howDoI();
+		void apply();
+		void cancel();
+		void idle(); // equal glutIdleFunc
+		
+	
+private:
+	Ui::guideWireSimulationClass ui;
+	Ui::SimulationParametersClass simuParamsWidget;
+	QDialog  * SimulationParameterdialog;
+	GLDrawer * OpenGLWidget ;
+
+	QAction * fileOpenAction ;
+	QAction * startAction;
+	QAction * pauseAction;
+	QAction * quitAction;
+	QAction * centerAction ;
+	QAction * zoomOutAction;
+	QAction * zoomInAction ;
+	QTimer * timer;
+	int mTimes;
+
+	// Phantom Omni
+	DevicePhantomOmni::Ptr mDevicePhantomOmni;
+	HapticDisplayState * mCurrentDisplayState;
+	HapticDisplayState * mLastDisplayState;
+	hduVector3Dd mPhantomOmniVelocity;
+	hduVector3Dd mPhantomOmniTwistVelocity;
+};
+
+#endif // GUIDEWIRESIMULATION_H

demo/guideWireSimulation/collisionResponse.cpp

+#include "collisionResponse.h"
+#include "dtkCollisionDetectPrimitive.h"
+
+namespace dtk
+{
+	void collisionResponse::Update( double timeslice, std::vector<dtkIntersectTest::IntersectResult::Ptr>& intersectResults, 
+		const std::vector< dtkID3>& avoid_1 , double stiffness)
+	{
+		dtkID size = intersectResults.size();
+		for (dtkID i = 0; i < size; i++)
+		{
+			dtkIntersectTest::IntersectResult::Ptr result = intersectResults[i];
+			dtkCollisionDetectPrimitive* pri_1;
+			dtkCollisionDetectPrimitive* pri_2;
+			result->GetProperty( dtkIntersectTest::INTERSECT_PRIMITIVE_1, pri_1 );
+			result->GetProperty( dtkIntersectTest::INTERSECT_PRIMITIVE_2, pri_2 );
+
+			dtkCollisionDetectPrimitive::Type type1 = pri_1->GetType();
+			dtkCollisionDetectPrimitive::Type type2 = pri_2->GetType();
+
+			bool needAvoid = false;
+
+			for (int i = 0; i < avoid_1.size(); i++)
+			{
+				if (avoid_1[i].a == pri_1->mDetailIDs[0] && avoid_1[i].b == pri_1->mDetailIDs[1] && avoid_1[i].c == pri_1->mDetailIDs[2] )
+				{
+					needAvoid = true;
+					break;
+				}
+				if (avoid_1[i].a == pri_1->mDetailIDs[0] && avoid_1[i].b == pri_1->mDetailIDs[2] && avoid_1[i].c == pri_1->mDetailIDs[1] )
+				{
+					needAvoid = true;
+					break;
+				}
+				if (avoid_1[i].a == pri_1->mDetailIDs[1] && avoid_1[i].b == pri_1->mDetailIDs[0] && avoid_1[i].c == pri_1->mDetailIDs[2] )
+				{
+					needAvoid = true;
+					break;
+				}
+				if (avoid_1[i].a == pri_1->mDetailIDs[1] && avoid_1[i].b == pri_1->mDetailIDs[2] && avoid_1[i].c == pri_1->mDetailIDs[0] )
+				{
+					needAvoid = true;
+					break;
+				}
+				if (avoid_1[i].a == pri_1->mDetailIDs[2] && avoid_1[i].b == pri_1->mDetailIDs[0] && avoid_1[i].c == pri_1->mDetailIDs[1] )
+				{
+					needAvoid = true;
+					break;
+				}
+				if (avoid_1[i].a == pri_1->mDetailIDs[2] && avoid_1[i].b == pri_1->mDetailIDs[1] && avoid_1[i].c == pri_1->mDetailIDs[0] )
+				{
+					needAvoid = true;
+					break;
+				}
+			}
+			
+			if (needAvoid)
+				continue;
+
+			if ( type1 == dtkCollisionDetectPrimitive::TRIANGLE )
+				if (type2 == dtkCollisionDetectPrimitive::SPHERE )
+				{
+					GK::Vector3 normal;
+					result->GetProperty( dtkIntersectTest::INTERSECT_NORMAL, normal );
+					double lengthPuncture = GK::Length(normal); 
+
+					dtkPhysMassPoint* massPoint21 = mGuideWireMassPoints[pri_2->mMajorID]->GetMassPoint( pri_2->mDetailIDs[0] );
+
+					dtkT3<double> impulse( normal[0], normal[1], normal[2] );
+
+					//------------------------------------------------
+					double impulseSquare = dot(impulse, impulse);
+					dtkT3<double> proj1 = dot(massPoint21->GetVel(), impulse) / impulseSquare * impulse;
+					//-------------------------------------------------
+		
+					// 剔除碰撞检测的假点
+				/*	GK::Point3 trianglePoint = (pri_1->mPts)->GetPoint(pri_1->mDetailIDs[0]);
+					dtkID sphereCenterID = pri_2->mDetailIDs[0];
+					double distanceLast1, distanceLast2, distanceCenter, distanceNext1, distanceNext2; 
+					distanceLast1 = distanceLast2 = 0;
+					if ( sphereCenterID >= 2 )
+					{
+						GK::Point3 last1SphereCenter = (pri_2->mPts)->GetPoint(sphereCenterID - 2);
+						distanceLast1 = GK::DotProduct(normal, last1SphereCenter - trianglePoint);
+					}
+					else if ( sphereCenterID >= 1 )
+					{
+						GK::Point3 last2SphereCenter = (pri_2->mPts)->GetPoint(sphereCenterID - 1);
+						distanceLast2 = GK::DotProduct(normal, last2SphereCenter - trianglePoint);
+					}	
+						
+					GK::Point3 sphereCenter	 = (pri_2->mPts)->GetPoint(pri_2->mDetailIDs[0]);
+					GK::Point3 nextSphereCenter = (pri_2->mPts)->GetPoint(pri_2->mDetailIDs[0] + 1);
+					GK::Point3 nextNextSphereCenter = (pri_2->mPts)->GetPoint(pri_2->mDetailIDs[0] + 2);
+
+					distanceCenter = GK::DotProduct(normal, sphereCenter - trianglePoint);
+					distanceNext1 = GK::DotProduct(normal, nextSphereCenter - trianglePoint);
+					distanceNext2 = GK::DotProduct(normal, nextNextSphereCenter - trianglePoint);
+
+
+					if (distanceLast2 < 0 && distanceCenter < 0 && distanceNext1 < 0)
+						return;*/
+					
+					if (lengthPuncture <=0) 
+						return ;
+
+
+					dtkT3<double> impulse0 = -proj1 * (massPoint21->GetMass() ) * 1.0;
+
+					// compute contact force
+					dtkT3<double> contactForce = impulse / timeslice / timeslice * 8.0;
+					// 如果是导丝的尖端碰到血管壁，则使每个尖端都受到这样的接触力
+
+					dtkPoints::Ptr points = mGuideWireMassPoints[1]->GetPoints();
+					if (massPoint21->GetPointID() <= mGuideWireMassPoints[1]->GetLastTipID())
+					{
+						//if (length(impulse / timeslice / timeslice * 4.0) > length(mGuideWireMassPoints[1]->mContactForces[0]) )
+						//{
+						//	for (dtkID i = 0; i < mGuideWireMassPoints[1]->GetLastTipID(); i++)
+						//	{
+						//		mGuideWireMassPoints[1]->SetContactForces(massPoint21->GetPointID(), \
+						//			impulse / timeslice / timeslice * 4.0);	// 8.0
+						//	}
+						//}
+						mGuideWireMassPoints[1]->SetContactForces(massPoint21->GetPointID(), \
+										impulse / timeslice / timeslice * 4.0);	// 8.0
+
+					}
+					else
+						mGuideWireMassPoints[1]->SetContactForces(massPoint21->GetPointID(), \
+						impulse / timeslice / timeslice * 6.0);    // 16.0
+
+					// 设置导丝质点的碰撞标志
+					mGuideWireMassPoints[1]->SetCollisionFlag(massPoint21->GetPointID(), true);
+				}
+				else
+					assert(false);
+			else
+				assert(false);
+		}
+	}
+
+	collisionResponse::collisionResponse(dtkStaticTriangleMesh::Ptr ptr)
+	{
+		mVascularSurfaceMesh = ptr;
+		// smooth vascular surface normals
+		const std::vector<dtkID3>& triFacets = mVascularSurfaceMesh->GetECTable(); 
+		mVascularSurfaceNormals.clear();
+		dtkPoints::Ptr points= mVascularSurfaceMesh->GetPoints();
+		mVascularSurfaceNormals.resize( mVascularSurfaceMesh->GetPoints()->GetNumberOfPoints(), GK::Vector3(0, 0, 0) );
+		// initial the contact forces
+
+
+		GK::Vector3 normal;
+
+		for( dtkID i = 0; i < triFacets.size(); i++ )
+		{
+			GK::Point3 p1 = points->GetPoint(triFacets[i][0]);
+			GK::Point3 p2 = points->GetPoint(triFacets[i][1]);
+			GK::Point3 p3 = points->GetPoint(triFacets[i][2]);
+
+			normal = GK::CrossProduct( p1 - p2, p3 - p2 );
+			mVascularSurfaceNormals[triFacets[i][0]] = mVascularSurfaceNormals[triFacets[i][0]] + normal;
+			mVascularSurfaceNormals[triFacets[i][1]] = mVascularSurfaceNormals[triFacets[i][1]] + normal;
+			mVascularSurfaceNormals[triFacets[i][2]] = mVascularSurfaceNormals[triFacets[i][2]] + normal;
+		}
+
+		for( dtkID i = 0; i < mVascularSurfaceNormals.size(); i++ )
+		{
+			if (mVascularSurfaceNormals[i].x() == 0 && mVascularSurfaceNormals[i].y() == 0 && mVascularSurfaceNormals[i].z() == 0)
+			{
+				double nValue = std::sqrt(1/3.0);
+				mVascularSurfaceNormals[i] = GK::Vector3(nValue, nValue, nValue); 
+			}
+			else
+				mVascularSurfaceNormals[i] = GK::Normalize( mVascularSurfaceNormals[i] );
+		}
+	}
+
+	collisionResponse::~collisionResponse()
+	{
+		// nothing
+	}
+}
\ No newline at end of file

demo/guideWireSimulation/collisionResponse.h

+#ifndef COLLISIONRESPONSE_H
+#define  COLLISIONRESPONSE_H
+
+#include <memory>
+#include <boost/utility.hpp>
+
+#include "dtkPhysMassSpring.h"
+#include "dtkIntersectTest.h"
+#include "guideWire.h"
+#include <vector>
+#include <map>
+
+namespace dtk
+{
+	class collisionResponse : public boost::noncopyable
+	{
+
+	public:
+		typedef std::shared_ptr< collisionResponse > Ptr;
+
+		static Ptr New(dtkStaticTriangleMesh::Ptr ptr) 
+		{
+			return Ptr( new collisionResponse(ptr) );
+		}
+
+	public:
+		~collisionResponse();
+
+		void Update( double timeslice, 
+			std::vector<dtkIntersectTest::IntersectResult::Ptr>& intersectResults, const std::vector< dtkID3>& avoid_1, double stiffness);
+
+		void SetMassSpring( dtkID i, dtkPhysMassSpring::Ptr massSpring )
+		{
+			mMassSprings[i] = massSpring;
+		}
+
+		void RemoveMassSpring( dtkID i )
+		{
+			mMassSprings.erase( i );
+		}
+
+		dtkPhysMassSpring::Ptr GetMassSpring( dtkID majorID )
+		{
+			assert( mMassSprings.find( majorID ) != mMassSprings.end() );
+			return mMassSprings[majorID];
+		}
+
+		void SetGuideWire(dtkID i, guideWire::Ptr massPoints)
+		{
+			mGuideWireMassPoints[i] = massPoints;
+		}
+
+		void RemoveGuideWire( dtkID i)
+		{
+			mGuideWireMassPoints.erase(i);
+		}
+
+		guideWire::Ptr GetGuideWire( dtkID majorID)
+		{
+			assert(mGuideWireMassPoints.find(majorID) != mGuideWireMassPoints.end());
+			return mGuideWireMassPoints[majorID];
+		}
+
+		void collisionResponse::ResetContactForces()
+		{
+			mGuideWireMassPoints[1]->ResetContactForces();
+		}
+
+		void collisionResponse::ResetCollisionFlag()
+		{
+			mGuideWireMassPoints[1]->ResetCollisionFlag();
+		}
+
+
+	private:
+		collisionResponse(dtkStaticTriangleMesh::Ptr ptr);
+	private:
+		std::map<dtkID, dtkPhysMassSpring::Ptr > mMassSprings;
+		std::map<dtkID, guideWire::Ptr > mGuideWireMassPoints;
+
+		dtkStaticTriangleMesh::Ptr mVascularSurfaceMesh;
+		std::vector<GK::Vector3> mVascularSurfaceNormals; 
+	};
+}
+
+#endif