//==============================================================================
void cDrawArrow(const cVector3d& a_arrowStart,
const cVector3d& a_arrowTip,
const double a_width)
{
#ifdef C_USE_OPENGL
glPushMatrix();
// We don't really care about the up vector, but it can't
// be parallel to the arrow...
cVector3d up = cVector3d(0,1,0);
cVector3d arrow = a_arrowTip-a_arrowStart;
arrow.normalize();
double d = fabs(cDot(up,arrow));
if (d > .9)
{
up = cVector3d(1,0,0);
}
cLookAt(a_arrowStart, a_arrowTip, up);
double distance = cDistance(a_arrowTip,a_arrowStart);
// This flips the z axis around
glRotatef(180,1,0,0);
// create a new OpenGL quadratic object
GLUquadricObj *quadObj;
quadObj = gluNewQuadric();
#define ARROW_CYLINDER_PORTION 0.75
#define ARRROW_CONE_PORTION (1.0 - 0.75)
// set rendering style
gluQuadricDrawStyle(quadObj, GLU_FILL);
// set normal-rendering mode
gluQuadricNormals(quadObj, GLU_SMOOTH);
// render a cylinder and a cone
glRotatef(180,1,0,0);
gluDisk(quadObj,0,a_width,10,10);
glRotatef(180,1,0,0);
gluCylinder(quadObj, a_width, a_width, distance*ARROW_CYLINDER_PORTION, 10, 10);
glTranslated(0, 0, ARROW_CYLINDER_PORTION*distance);
glRotatef(180, 1, 0, 0);
gluDisk(quadObj, 0, a_width*2.0, 10, 10);
glRotatef(180,1,0,0);
gluCylinder(quadObj, a_width*2.0, 0.0, distance*ARRROW_CONE_PORTION, 10, 10);
// delete our quadric object
gluDeleteQuadric(quadObj);
glPopMatrix();
#endif
}
void updateHaptics(void)
{
// a clock to estimate the haptic simulation loop update rate
cPrecisionClock pclock;
pclock.setTimeoutPeriodSeconds(1.0);
pclock.start(true);
int counter = 0, loop_counter = 0;
// main haptic simulation loop
while(simulationRunning)
{
usleep(config_.haptics_sleep);
if(object->m_refresh_cloud)
{
// // Copy over state info from old object
// new_object->m_interactionInside = object->m_interactionInside;
// new_object->m_interactionProjectedPoint = object->m_interactionProjectedPoint;
// new_object->tPlane = object->tPlane;
// //new_object->tPlaneNormal = object->tPlaneNormal;
// new_object->m_shape = object->m_shape;
// new_object->m_showTangent = object->m_showTangent;
//
// PointCloudObject *temp = object;
// object = new_object;
// new_object = temp;
//
// object->setAsGhost(false);
// new_object->setAsGhost(true);
ros::WallTime begin = ros::WallTime::now();
object->applyLastCloud();
ROS_DEBUG_NAMED("time", "Applying cloud took %.3lf ms", (ros::WallTime::now() - begin).toSec()*1000.0);
//refresh_cloud = false;
}
object->m_material.setDynamicFriction(config_.dynamic_friction);
object->scaleZ = config_.scaleZ;
object->m_cloudType = config_.cloud_mode;
object->m_basisType = config_.basis_function;
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
bool buttonState = false;
tool->getHapticDevice()->getUserSwitch(0, buttonState);
// This is how we move the workspace!
if(buttonState)
{
float translate_factor = 0.001;
cVector3d localPos = tool->getDeviceLocalPos();
if(sqrt(localPos.x*localPos.x + localPos.y*localPos.y) > 1.5){
camera->translate(translate_factor*cDot(localPos, cVector3d(1,0,0)),
translate_factor*cDot(localPos, cVector3d(0,1,0)),
0);
//m_cam.strafeRight(0.0005*cDot(localPos, cVector3d(0,1,0)));
//m_cam.moveForward(0.0005*cDot(localPos, cVector3d(-1,0,0)));
tool->m_lastComputedGlobalForce += 2*cVector3d(-localPos.x, -localPos.y, 0);
tool->applyForces();
}
}
// estimate the refresh rate
++counter;
if (pclock.timeoutOccurred()) {
pclock.stop();
rateEstimate = counter;
counter = 0;
pclock.start(true);
}
}
// exit haptics thread
simulationFinished = true;
}
