HDCallbackCode HDCALLBACK CybPhantom::setForce(void *pUserData) {
//cout << "setForce()" << endl;
hduVector3Dd position;//buffer para a recepção de dados do socket
hduVector3Dd force;
HDdouble forceClamp;
static const HDdouble kAnchorStiffness = 0.1;
hdBeginFrame(hdGetCurrentDevice()); /*Começa o frame.*/
hdGetDoublev(HD_CURRENT_POSITION, position);
//cout << anchorPosition[0] << " " << anchorPosition[1] << " " << anchorPosition[2] << endl;
hduVecSubtract(force, anchor, position);
hduVecScaleInPlace(force, kAnchorStiffness);
//hdGetDoublev(HD_NOMINAL_MAX_CONTINUOUS_FORCE, &forceClamp);
/*if (hduVecMagnitude(force) > forceClamp)
{
hduVecNormalizeInPlace(force);
hduVecScaleInPlace(force, forceClamp);
}*/
//cout << force[0] << " " << force[1] << " " << force[2] << endl;
hdSetDoublev(HD_CURRENT_FORCE, force);
hdEndFrame(hdGetCurrentDevice());
return HD_CALLBACK_CONTINUE;
}
HDCallbackCode HDCALLBACK touchScene(void *pUserData){
static const HDdouble stiffness = 0.05;
hduVector3Dd position;
hduVector3Dd initialPosition(0, 0, 0);
hduVector3Dd force((double) g_force.x, (double) g_force.y, (double) g_force.z);
int button;
// Get Haptic Arm State
hdGetIntegerv(HD_CURRENT_BUTTONS,&button);
g_button1 = (button & HD_DEVICE_BUTTON_1);
g_button2 = (button & HD_DEVICE_BUTTON_2);
g_button3 = (button & HD_DEVICE_BUTTON_3);
printf("\t%i\t%i %i %i\r",button,g_button1,g_button2,g_button3);
if(g_button2) g_doExit = true;
hdBeginFrame(ghHD);
hdGetDoublev(HD_CURRENT_POSITION, position);
switch(g_selecMode){
case MOVE:
{
hduVector3Dd tempForce = shakeBaby();
hduVecSubtract(force, initialPosition, position);
hduVecScaleInPlace(force, stiffness);
force += tempForce;
hdSetDoublev(HD_CURRENT_FORCE, force);
g_position_out.v[0] = (float)position[0];
g_position_out.v[1] = 0.f;
g_position_out.v[2] = (float)position[2];
break;
}
case CAM:
{
hduVecSubtract(force, initialPosition, position);
hduVecScaleInPlace(force, stiffness);
hdSetDoublev(HD_CURRENT_FORCE, force);
g_position_out.v[0] = (float)position[0];
g_position_out.v[1] = (float)position[1];
g_position_out.v[2] = 0.f;
break;
}
case ARM :
{
g_position_out.v[0] = (float)position[0];
g_position_out.v[1] = (float)position[1];
g_position_out.v[2] = (float)position[2];
hduVecScaleInPlace(force, stiffness);
hdSetDoublev(HD_CURRENT_FORCE, force);
break;
}
}
hdEndFrame(ghHD);
return HD_CALLBACK_CONTINUE;
}
HDCallbackCode HDCALLBACK HapticDevice::aSchedule(void *pUserData)
{
HapticSynchronizer * hs = (HapticSynchronizer *) pUserData;
for(unsigned int i=0;i<NB_DEVICES_MAX;i++)
{
if(hs[i].m_data!=NULL)
{
HapticData * data = hs[i].m_data;
hduVector3Dd force( 0, 0, 0 );
hdBeginFrame(hs[i].m_data->m_id);
HDdouble forceClamp;
hduVector3Dd distance = data->m_realPosition - data->m_position;
force = data->m_force;
/* if we are nearly at the center don't recalculate anything,
since the center is a singular point. */
if(data->m_nbCollision == 1)
{
if(distance.magnitude() > EPSILON && data->m_ready)
{
/* force is calculated from F=kx; k is the stiffness
and x is the vector magnitude and direction. In this case,
k = STIFFNESS, and x is the penetration vector from
the actual position to the desired position. */
force = STIFFNESS*distance;
force *= BALL_MASS;
force[0] = 0;
}
}
// Check if we need to clamp the force.
hdGetDoublev(HD_NOMINAL_MAX_FORCE, &forceClamp);
if (hduVecMagnitude(force) > forceClamp)
{
hduVecNormalizeInPlace(force);
hduVecScaleInPlace(force, forceClamp);
}
hdSetDoublev(HD_CURRENT_FORCE, force);
hdEndFrame(data->m_id);
}
}
HDErrorInfo error;
if (HD_DEVICE_ERROR(error = hdGetError()))
{
// hduPrintError(stderr, &error, "Error during scheduler callback");
if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
return HD_CALLBACK_CONTINUE;
}
/******************************************************************************
* Main scheduler callback for rendering the anchored spring force.
*****************************************************************************/
HDCallbackCode HDCALLBACK AnchoredSpringForceCallback(void *pUserData)
{
static hduVector3Dd anchor;
static HDboolean bRenderForce = (1==2);
HDErrorInfo error;
HDint nCurrentButtons, nLastButtons;
hduVector3Dd position;
hduVector3Dd force;
force[0] = 0;
force[1] = 0;
force[2] = 0;
hdBeginFrame(hdGetCurrentDevice());
hdGetDoublev(HD_CURRENT_POSITION, position);
hdGetIntegerv(HD_CURRENT_BUTTONS, &nCurrentButtons);
hdGetIntegerv(HD_LAST_BUTTONS, &nLastButtons);
if ((nCurrentButtons & HD_DEVICE_BUTTON_1) != 0 &&
(nLastButtons & HD_DEVICE_BUTTON_1) == 0)
{
/* Detected button down */
memcpy(anchor, position, sizeof(hduVector3Dd));
bRenderForce = (1==1);
}
else if ((nCurrentButtons & HD_DEVICE_BUTTON_1) == 0 &&
(nLastButtons & HD_DEVICE_BUTTON_1) != 0)
{
/* Detected button up */
bRenderForce = (1==2);
/* Send zero force to the device, or else it will just continue
rendering the last force sent */
hdSetDoublev(HD_CURRENT_FORCE, force);
}
if (bRenderForce)
{
/* Compute spring force as F = k * (anchor - pos), which will attract
the device position towards the anchor position */
hduVecSubtract(force, anchor, position);
hduVecScaleInPlace(force, gSpringStiffness);
hdSetDoublev(HD_CURRENT_FORCE, force);
}
//////////////////////////////////////////////
//More stuff oliver has added: call to update position and force display//
if (count>MAX_COUNT)
{
updateUser(position, force);
count -= MAX_COUNT;
}
//////////////////////////////////////////////
//////////////////////////////////////////////
hdEndFrame(hdGetCurrentDevice());
/* Check if an error occurred while attempting to render the force */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
if (hduIsForceError(&error))
{
bRenderForce = (1==2);
}
else if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
count++;
return HD_CALLBACK_CONTINUE;
}
