#define _USE_MATH_DEFINES
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <iostream>
#if defined(WIN32) || defined(WIN64)
#define NOMINMAX
#include "windows.h"
#endif
#ifdef MACOSX
#include "OpenGL/glu.h"
#else
#include "GL/glu.h"
#endif
#include <GLFW/glfw3.h>
#include "CGeometry.h"
#include "FontGL.h"
constexpr int SwapInterval = 1;
bool simulationRunning = true;
bool simulationFinished = false;
chai3d::cVector3d devicePosition;
chai3d::cMatrix3d deviceRotation;
double deviceGripperAngleDeg = 0.0;
chai3d::cVector3d deviceLinearVelocity;
chai3d::cVector3d deviceAngularVelocity;
uint32_t deviceSwitches = 0x00000000;
chai3d::cVector3d deviceForce;
chai3d::cVector3d deviceTorque;
double deviceGripperForce = 0.0;
double deviceRollAngleRad = 0.0;
bool flagSaturation = false;
chai3d::cVector3d holdPosition;
bool flagHoldPosition = false;
bool flagHoldPositionReady = false;
double workspaceRadius = 0.05;
bool flagWorkspace = true;
bool flagWorkspaceReady = false;
double coneAngleLimit = chai3d::cDegToRad(10.0);
double rollAngleLimit = chai3d::cDegToRad(40.0);
chai3d::cVector3d coneAxis;
chai3d::cVector3d rollAxis;
chai3d::cMatrix3d coneRotation;
bool flagCone = false;
bool flagConeReady = false;
GLFWwindow* window = nullptr;
int windowWidth = 0;
int windowHeight = 0;
int updateGraphics()
{
GLUquadricObj *sphere;
chai3d::cTransform mat;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glClearColor(0.0, 0.0, 0.0, 1.0);
mat.set(devicePosition, deviceRotation);
glPushMatrix();
glMultMatrixd((const double *)mat.getData());
glEnable(GL_COLOR_MATERIAL);
glColor3f(0.5f, 0.5f, 0.5f);
sphere = gluNewQuadric();
gluSphere(sphere, 0.005, 32, 32);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(1.0f, 0.0f, 0.0f);
glVertex3d(0.00, 0.00, 0.00);
glVertex3d(0.02, 0.00, 0.00);
glColor3f(0.0f, 1.0f, 0.0f);
glVertex3d(0.00, 0.00, 0.00);
glVertex3d(0.00, 0.02, 0.00);
glColor3f(0.0f, 0.0f, 1.0f);
glVertex3d(0.00, 0.00, 0.00);
glVertex3d(0.00, 0.00, 0.02);
glEnd();
glEnable(GL_LIGHTING);
glDisable(GL_LIGHTING);
glBegin(GL_LINE_STRIP);
glColor3f(1.0f, 1.0f, 1.0f);
glVertex3d(0.0, 0.0, 0.0);
for (int index = -10; index <= 10; index++)
{
double angle = 0.1 * (double)(index) * deviceGripperAngleDeg;
double px = 0.1 * chai3d::cCosDeg(angle);
double py = 0.1 * chai3d::cSinDeg(angle);
glVertex3d(-px, py, 0.0);
}
glVertex3d(0.0, 0.0, 0.0);
glColor3f(0.5f, 0.5f, 0.5f);
glVertex3d(-0.1, 0.0, 0.0);
glEnd();
glEnable(GL_LIGHTING);
glPopMatrix();
if (flagConeReady)
{
mat.set(devicePosition, coneRotation);
glPushMatrix();
glMultMatrixd((const double *)mat.getData());
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(0.0f, 1.0f, 1.0f);
glVertex3d(0.0, 0.0, 0.0);
glVertex3d(-0.1, 0.0, 0.0);
glEnd();
glBegin(GL_LINE_STRIP);
for (int index = 0; index <= 36; index++)
{
double SIZE = 0.1;
double radius = SIZE * chai3d::cTanRad(coneAngleLimit);
double angle = 10 * (double)(index);
double px = radius * chai3d::cCosDeg(angle);
double py = radius * chai3d::cSinDeg(angle);
glVertex3d(-SIZE, px, py);
}
glEnd();
glEnable(GL_LIGHTING);
glPopMatrix();
mat.set(devicePosition, deviceRotation);
glPushMatrix();
glMultMatrixd((const double *)mat.getData());
for (int pass = 0; pass < 2; pass++)
{
if (pass == 0)
{
glColor3f(1.0f, 1.0f, 1.0f);
glDepthMask(GL_TRUE);
glBegin(GL_LINE_STRIP);
}
else if (pass == 1)
{
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor4f(1.0f, 1.0f, 1.0f, 0.3f);
glDepthMask(GL_FALSE);
glBegin(GL_TRIANGLE_FAN);
}
glVertex3d(0.0, 0.0, 0.0);
double angleMin = -rollAngleLimit + deviceRollAngleRad;
double angleMax = rollAngleLimit + deviceRollAngleRad;
for (int index = 0; index <= 36; index++)
{
double angle = angleMax + ((chai3d::C_TWO_PI - (angleMax - angleMin)) / 36.0) * (double)(index);
double px = 0.02 * chai3d::cCosRad(angle);
double py = 0.02 * chai3d::cSinRad(angle);
glVertex3d(0, -py, px);
}
glVertex3d(0.0, 0.0, 0.0);
glEnd();
glDisable(GL_BLEND);
glDepthMask(GL_TRUE);
}
glEnable(GL_LIGHTING);
glPopMatrix();
}
static double lastFrequencyUpdateTime =
dhdGetTime();
static char frequencyString[16] = "0.000 kHz";
if (time - lastFrequencyUpdateTime > 0.1)
{
lastFrequencyUpdateTime = time;
snprintf(frequencyString, 10, "%0.03f kHz", frequency);
}
glDisable(GL_LIGHTING);
glColor3f(1.0, 1.0, 1.0);
glRasterPos3f(0.0f, -0.01f, -0.1f);
for (char* character = frequencyString; *character != '\0'; character++)
{
render_character(*character, HELVETICA12);
}
glEnable(GL_LIGHTING);
glDisable(GL_LIGHTING);
glColor3f(1.0, 1.0, 1.0);
glRasterPos3f(0.0f, -0.03f, 0.1f);
for (int index = 0; index < 16; index++)
{
if (chai3d::cCheckBit(deviceSwitches, index))
{
render_character('1', HELVETICA12);
}
else
{
render_character('0', HELVETICA12);
}
render_character(' ', HELVETICA12);
}
glEnable(GL_LIGHTING);
GLenum err = glGetError();
if (err != GL_NO_ERROR)
{
return -1;
}
return 0;
}
void* hapticsLoop(void* a_userData)
{
double px = 0.0;
double py = 0.0;
double pz = 0.0;
double rot[3][3] = {};
double vx = 0.0;
double vy = 0.0;
double vz = 0.0;
double wx = 0.0;
double wy = 0.0;
double wz = 0.0;
{
std::cout <<
"error: failed to enable force rendering (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
simulationRunning = false;
return nullptr;
}
while (simulationRunning)
{
{
std::cout << std::endl <<
"error: failed to read position (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
devicePosition.set(px, py, pz);
deviceRotation.set(rot[0][0], rot[0][1], rot[0][2],
rot[1][0], rot[1][1], rot[1][2],
rot[2][0], rot[2][1], rot[2][2]);
{
std::cout << std::endl <<
"error: failed to read gripper angle (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
{
std::cout << std::endl <<
"error: failed to read linear velocity (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
deviceLinearVelocity.set(vx, vy, vz);
{
std::cout << std::endl <<
"error: failed to read gripper angle (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
break;
}
deviceAngularVelocity.set(wx, wy, wz);
deviceForce.zero();
deviceTorque.zero();
deviceGripperForce = 0.0;
constexpr double Kp = 2000.0;
constexpr double Kv = 10.0;
if (flagHoldPosition)
{
if (flagHoldPositionReady)
{
chai3d::cVector3d force = -Kp * (devicePosition - holdPosition) - Kv * deviceLinearVelocity;
deviceForce = deviceForce + force;
}
else
{
holdPosition = devicePosition;
flagHoldPositionReady = true;
}
}
if (flagWorkspace)
{
double distance = devicePosition.length();
if (flagWorkspaceReady)
{
double depth = distance - workspaceRadius;
if (depth > 0.0)
{
chai3d::cVector3d normal = -chai3d::cNormalize(devicePosition);
chai3d::cVector3d force = Kp * depth * normal;
chai3d::cVector3d damping = -Kv * chai3d::cProject(deviceLinearVelocity, normal);
deviceForce = deviceForce + (force + damping);
}
}
else
{
if (distance < workspaceRadius)
{
flagWorkspaceReady = true;
}
}
}
if (flagCone)
{
if (flagConeReady)
{
chai3d::cVector3d toolAxis = -deviceRotation.getCol0();
double toolAngle = chai3d::cAngle(toolAxis, coneAxis);
if (toolAngle > coneAngleLimit)
{
chai3d::cVector3d torqueDirection = chai3d::cNormalize(cCross(coneAxis, toolAxis));
double deltaAngle = toolAngle - coneAngleLimit;
chai3d::cVector3d torque = -10.0 * deltaAngle * torqueDirection;
chai3d::cVector3d damping = -0.02 * chai3d::cProject(deviceAngularVelocity, torqueDirection);
deviceTorque = deviceTorque + (torque + damping);
}
chai3d::cVector3d toolRoll = deviceRotation.getCol2();
chai3d::cVector3d ProjectedRollAxis = chai3d::cProjectPointOnPlane(rollAxis, chai3d::cVector3d(0, 0, 0), toolAxis);
double currentRollAngle = chai3d::cAngle(toolRoll, ProjectedRollAxis);
chai3d::cVector3d t = chai3d::cNormalize(coneAxis);
double sign = 1.0;
chai3d::cVector3d c = cCross(toolRoll, ProjectedRollAxis);
if (chai3d::cAngle(t, c) < chai3d::C_PI_DIV_2)
{
sign = -1.0;
}
deviceRollAngleRad = sign * currentRollAngle;
if (currentRollAngle > rollAngleLimit)
{
double deltaAngle = currentRollAngle - rollAngleLimit;
chai3d::cVector3d torque = -4.0 * sign * deltaAngle * t;
chai3d::cVector3d damping = -0.02 * chai3d::cProject(deviceAngularVelocity, t);
deviceTorque = deviceTorque + (torque + damping);
}
}
else
{
coneAxis = -deviceRotation.getCol0();
rollAxis = deviceRotation.getCol2();
coneRotation = deviceRotation;
flagConeReady = true;
}
}
constexpr double MaxTorque = 0.3;
if (deviceTorque.length() > MaxTorque)
{
deviceTorque = MaxTorque * chai3d::cNormalize(deviceTorque);
}
deviceTorque(0), deviceTorque(1), deviceTorque(2),
deviceGripperForce);
if (error == 2)
{
flagSaturation = true;
}
else
{
flagSaturation = false;
}
}
simulationRunning = false;
simulationFinished = true;
return nullptr;
}
void onExit()
{
simulationRunning = false;
while (!simulationFinished)
{
}
{
std::cout <<
"error: failed to close the connection (" <<
dhdErrorGetLastStr() <<
")" << std::endl;
return;
}
std::cout << "connection closed" << std::endl;
return;
}
void onWindowResized(GLFWwindow* a_window,
int a_width,
int a_height)
{
windowWidth = a_width;
windowHeight = a_height;
double glAspect = (static_cast<double>(a_width) / static_cast<double>(a_height));
glViewport(0, 0, a_width, a_height);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60, glAspect, 0.01, 10);
gluLookAt(0.2, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 1.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void onKeyPressed(GLFWwindow* a_window,
int a_key,
int a_scanCode,
int a_action,
int a_modifiers)
{
if (a_action != GLFW_PRESS)
{
return;
}
if ((a_key == GLFW_KEY_ESCAPE) || (a_key == GLFW_KEY_Q))
{
exit(0);
}
if (a_key == GLFW_KEY_H)
{
flagHoldPosition = !flagHoldPosition;
flagHoldPositionReady = false;
}
if (a_key == GLFW_KEY_W)
{
flagWorkspace = !flagWorkspace;
flagWorkspaceReady = false;
}
if (a_key == GLFW_KEY_C)
{
flagCone = !flagCone;
flagConeReady = false;
}
if (a_key == GLFW_KEY_1)
{
coneAngleLimit = chai3d::cMax(coneAngleLimit - chai3d::cDegToRad(1.0), chai3d::cDegToRad(0.0));
}
if (a_key == GLFW_KEY_2)
{
coneAngleLimit = chai3d::cMin(coneAngleLimit + chai3d::cDegToRad(1.0), chai3d::cDegToRad(35.0));
}
if (a_key == GLFW_KEY_3)
{
rollAngleLimit = chai3d::cMax(rollAngleLimit - chai3d::cDegToRad(1.0), chai3d::cDegToRad(0.0));
}
if (a_key == GLFW_KEY_4)
{
rollAngleLimit = chai3d::cMin(rollAngleLimit + chai3d::cDegToRad(1.0), chai3d::cDegToRad(180.0));
}
}
void onError(int a_error,
const char* a_description)
{
std::cout << "error: " << a_description << std::endl;
}
int initializeGLFW()
{
if (!glfwInit())
{
return -1;
}
glfwSetErrorCallback(onError);
const GLFWvidmode* mode = glfwGetVideoMode(glfwGetPrimaryMonitor());
windowWidth = static_cast<int>(0.8 * mode->height);
windowHeight = static_cast<int>(0.5 * mode->height);
int x = static_cast<int>(0.5 * (mode->width - windowWidth));
int y = static_cast<int>(0.5 * (mode->height - windowHeight));
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 2);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 1);
glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_TRUE);
glfwWindowHint(GLFW_SAMPLES, 4);
glfwWindowHint(GLFW_STEREO, GL_FALSE);
glfwWindowHint(GLFW_VISIBLE, GL_FALSE);
#ifdef MACOSX
glfwWindowHint(GLFW_COCOA_RETINA_FRAMEBUFFER, GL_FALSE);
#endif
window = glfwCreateWindow(windowWidth, windowHeight, "Force Dimension - OpenGL Sphere Example", nullptr, nullptr);
if (!window)
{
return -1;
}
glfwMakeContextCurrent(window);
glfwSetKeyCallback(window, onKeyPressed);
glfwSetWindowSizeCallback(window, onWindowResized);
glfwSetWindowPos(window, x, y);
glfwSwapInterval(SwapInterval);
glfwShowWindow(window);
onWindowResized(window, windowWidth, windowHeight);
static const GLfloat mat_ambient[] = { 0.5f, 0.5f, 0.5f };
GLfloat mat_diffuse[] = { 0.5f, 0.5f, 0.5f };
GLfloat mat_specular[] = { 0.5f, 0.5f, 0.5f };
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, mat_ambient);
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, mat_diffuse);
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, mat_specular);
glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 1.0);
GLfloat ambient[] = { 0.5f, 0.5f, 0.5f, 1.0f };
GLfloat diffuse[] = { 0.8f, 0.8f, 0.8f, 1.0f };
GLfloat specular[] = { 1.0f, 1.0f, 1.0f, 1.0f };
glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuse);
glLightfv(GL_LIGHT0, GL_SPECULAR, specular);
glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 1.0);
glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.0);
glLightf(GL_LIGHT0, GL_QUADRATIC_ATTENUATION, 0.0);
GLfloat lightPos[] = { 2.0, 0.0, 0.0, 1.0f };
GLfloat lightDir[] = { -1.0, 0.0, 0.0, 1.0f };
glLightfv(GL_LIGHT0, GL_POSITION, lightPos);
glLightfv(GL_LIGHT0, GL_SPOT_DIRECTION, lightDir);
glLightf(GL_LIGHT0, GL_SPOT_CUTOFF, 180);
glLightf(GL_LIGHT0, GL_SPOT_EXPONENT, 1.0);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
return 0;
}
int initializeHaptics()
{
{
return -1;
}
{
return -1;
}
return 0;
}
int main(int argc,
char* argv[])
{
std::cout << "Copyright (C) 2001-2023 Force Dimension" << std::endl;
std::cout << "All Rights Reserved." << std::endl << std::endl;
if (initializeHaptics() < 0)
{
std::cout << "error: failed to initialize haptics" << std::endl;
return -1;
}
if (initializeGLFW() < 0)
{
std::cout << "error: failed to initialize GLFW" << std::endl;
return -1;
}
#if defined(WIN32) || defined(WIN64)
DWORD threadHandle;
CreateThread(nullptr, 0, (LPTHREAD_START_ROUTINE)(hapticsLoop), nullptr, 0x0000, &threadHandle);
SetThreadPriority(&threadHandle, THREAD_PRIORITY_ABOVE_NORMAL);
#else
pthread_t threadHandle;
pthread_create(&threadHandle, nullptr, hapticsLoop, nullptr);
struct sched_param schedulerParameters;
memset(&schedulerParameters, 0, sizeof(struct sched_param));
schedulerParameters.sched_priority = 10;
pthread_setschedparam(threadHandle, SCHED_RR, &schedulerParameters);
#endif
atexit(onExit);
std::cout <<
dhdGetSystemName() <<
" device detected" << std::endl << std::endl;
std::cout << "press 'h' to toggle hold device in position" << std::endl;
std::cout << " 'w' to toggle spherical workspace limit" << std::endl;
std::cout << " 'c' to toggle conical rotational limit" << std::endl;
std::cout << " '1' to increase cone angle" << std::endl;
std::cout << " '2' to decrease cone angle" << std::endl;
std::cout << " '3' to increase roll angle" << std::endl;
std::cout << " '4' to decrease roll angle" << std::endl;
std::cout << " 'q' to quit" << std::endl << std::endl;
while (simulationRunning && !glfwWindowShouldClose(window))
{
if (updateGraphics() < 0)
{
std::cout << "error: failed to update graphics" << std::endl;
break;
}
glfwSwapBuffers(window);
glfwPollEvents();
}
glfwDestroyWindow(window);
glfwTerminate();
return 0;
}