HDCallbackCode BaseGeometryPatch::patchCalc(){
HDErrorInfo error;
hduVector3Dd forceVec;
hduVector3Dd targForceVec;
hduVector3Dd loopForceVec;
hduVector3Dd patchMinusDeviceVec;
hduVector3Dd sensorVec;
double sensorRadius = 1.0;
if(simToPhantom == NULL){
return HD_CALLBACK_CONTINUE;
}
HHD hHD = hdGetCurrentDevice();
// reset all the variables that need to accumulate
for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){
phantomToSim->forceMagnitude[sensi] = 0;
phantomToSim->forceVec[sensi][0] = 0;
phantomToSim->forceVec[sensi][1] = 0;
phantomToSim->forceVec[sensi][2] = 0;
}
// Begin haptics frame. ( In general, all state-related haptics calls
// should be made within a frame. )
hdBeginFrame(hHD);
/* Get the current devicePos of the device. */
hdGetDoublev(HD_CURRENT_POSITION, devicePos);
hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, deviceAngle);
hdGetDoublev(HD_CURRENT_TRANSFORM, transformMat);
forceVec[0] = 0;
forceVec[1] = 0;
forceVec[2] = 0;
if(simToPhantom->numTargets > SimToPhantom::MAX_TARGETS)
simToPhantom->numTargets = SimToPhantom::MAX_TARGETS;
// clear all the data structures
if(phantomToSim != NULL){
for(int i = 0; i < PhantomToSim::NUM_SENSORS; ++i){
phantomToSim->forceMagnitude[i] = 0;
phantomToSim->forceVec[i][0] = 0;
phantomToSim->forceVec[i][1] = 0;
phantomToSim->forceVec[i][2] = 0;
for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){
phantomToSim->targForceMagnitude[j][i] = 0;
phantomToSim->targForceVec[j][i][0] = 0;
phantomToSim->targForceVec[j][i][1] = 0;
phantomToSim->targForceVec[j][i][2] = 0;
}
}
for(int j = 0; j < PhantomToSim::MAX_TARGETS; ++j){
phantomToSim->targForcesMagnitude[j] = 0;
}
}
/***/
//printf("BaseGeometryPatch::patchCalc() numtargets = %d\n", simToPhantom->numTargets);
for(int targi = 0; targi < simToPhantom->numTargets; ++targi){
patchPos[0] = simToPhantom->targetX[targi];
patchPos[1] = simToPhantom->targetY[targi];
patchPos[2] = simToPhantom->targetZ[targi];
patchRadius = simToPhantom->targetRadius[targi];
targForceVec[0] = 0;
targForceVec[1] = 0;
targForceVec[2] = 0;
for(int sensi = 0; sensi < SimToPhantom::NUM_SENSORS; ++sensi){
loopForceVec[0] = 0;
loopForceVec[1] = 0;
loopForceVec[2] = 0;
sensorVec[0] = simToPhantom->sensorX[sensi] + devicePos[0];
sensorVec[1] = simToPhantom->sensorY[sensi] + devicePos[1];
sensorVec[2] = simToPhantom->sensorZ[sensi] + devicePos[2];
sensorRadius = simToPhantom->sensorRadius[sensi];
// patchMinusDeviceVec = patchPos-devicePos <
// Create a vector from the device devicePos towards the sphere's center.
//hduVecSubtract(patchMinusDeviceVec, patchPos, devicePos);
hduVecSubtract(patchMinusDeviceVec, patchPos, sensorVec);
hduVector3Dd dirVec;
hduVecNormalize(dirVec, patchMinusDeviceVec);
// If the device position is within the sphere's surface
// center, apply a spring forceVec towards the surface. The forceVec
// calculation differs from a traditional gravitational body in that the
// closer the device is to the center, the less forceVec the well exerts;
// the device behaves as if a spring were connected between itself and
// the well's center. *
double penetrationDist = patchMinusDeviceVec.magnitude() - (patchRadius+sensorRadius);
if(penetrationDist < 0)
{
// > F = k * x <
// F: forceVec in Newtons (N)
// k: Stiffness of the well (N/mm)
// x: Vector from the device endpoint devicePos to the center
// of the well.
hduVecScale(dirVec, dirVec, penetrationDist);
hduVecScale(loopForceVec, dirVec, stiffnessK);
/***
for(int i = 0; i < 3; ++i)
if(loopForceVec[i] < 0)
loopForceVec[i] *= 0.5;
printf("targForceVec[%d][%d] = %.2f, %.2f, %.2f\r", targi, sensi, loopForceVec[0], loopForceVec[1], loopForceVec[2]);
/****/
}
if(phantomToSim != NULL){
phantomToSim->forceMagnitude[sensi] += hduVecMagnitude(loopForceVec);
phantomToSim->forceVec[sensi][0] += loopForceVec[0];
phantomToSim->forceVec[sensi][1] += loopForceVec[1];
phantomToSim->forceVec[sensi][2] += loopForceVec[2];
phantomToSim->targForceMagnitude[targi][sensi] = hduVecMagnitude(loopForceVec);
phantomToSim->targForceVec[targi][sensi][0] = loopForceVec[0];
phantomToSim->targForceVec[targi][sensi][1] = loopForceVec[1];
phantomToSim->targForceVec[targi][sensi][2] = loopForceVec[2];
}
hduVecAdd(targForceVec, targForceVec, loopForceVec);
hduVecAdd(forceVec, forceVec, loopForceVec);
}
if(phantomToSim != NULL){
phantomToSim->targForcesMagnitude[targi] = hduVecMagnitude(targForceVec);
}
}
if(phantomToSim != NULL){
for(int i = 0; i < 16; ++i){
phantomToSim->matrix[i] = transformMat[i];
}
}
// divide the forceVec the number of times that a force was added?
/* Send the forceVec to the device. */
if(simToPhantom->enabled){
hdSetDoublev(HD_CURRENT_FORCE, forceVec);
}
/* End haptics frame. */
hdEndFrame(hHD);
/* Check for errors and abort the callback if a scheduler error
is detected. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error, "BaseGeometryPatch.calcPatch():\n");
if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
/* Signify that the callback should continue running, i.e. that
it will be called again the next scheduler tick. */
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;
}
/*******************************************************************************
Servo callback.
Called every servo loop tick. Simulates a gravity well, which sucks the device
towards its center whenever the device is within a certain range.
*******************************************************************************/
HDCallbackCode HDCALLBACK gravityWellCallback(void *data)
{
//const HDdouble kStiffness = 0.075; /* N/mm */
const HDdouble kStiffness = 0.045; /* N/mm */
const HDdouble kGravityWellInfluence = 400; /* mm */
/* This is the position of the gravity well in cartesian
(i.e. x,y,z) space. */
static const hduVector3Dd wellPos = {0,0,0};
hduVector3Dd wellPos2 = {0.0, 0.0, 0.0};
HDErrorInfo error;
hduVector3Dd position;
hduVector3Dd force;
hduVector3Dd positionTwell;
HHD hHD = hdGetCurrentDevice();
/* Begin haptics frame. ( In general, all state-related haptics calls
should be made within a frame. ) */
WaitForSingleObject( hIOMutex, INFINITE );
hdBeginFrame(hHD);
/* Get the current position of the device. */
hdGetDoublev(HD_CURRENT_POSITION, position);
memset(force, 0, sizeof(hduVector3Dd));
/* > positionTwell = wellPos-position <
Create a vector from the device position towards the gravity
well's center. */
count++;
//wellPos2[0] = (count%5000)/100;
wellPos2[1] = 20*sin((count)*6.28/360);
hduVecSubtract(positionTwell, wellPos2, position);
/* If the device position is within some distance of the gravity well's
center, apply a spring force towards gravity well's center. The force
calculation differs from a traditional gravitational body in that the
closer the device is to the center, the less force the well exerts;
the device behaves as if a spring were connected between itself and
the well's center. */
if (hduVecMagnitude(positionTwell) < kGravityWellInfluence)
{
/* > F = k * x <
F: Force in Newtons (N)
k: Stiffness of the well (N/mm)
x: Vector from the device endpoint position to the center
of the well. */
hduVecScale(force, positionTwell, kStiffness);
}
/* Send the force to the device. */
hdSetDoublev(HD_CURRENT_FORCE, force);
/* Get data for logging */
hdGetDoublev(HD_CURRENT_POSITION, global_position);
hdGetDoublev(HD_CURRENT_FORCE, global_force);
hdGetDoublev(HD_CURRENT_JOINT_ANGLES, global_joint_angles);
hdGetDoublev(HD_CURRENT_JOINT_TORQUE, global_joint_torque);
/* End haptics frame. */
hdEndFrame(hHD);
ReleaseMutex( hIOMutex );
/* Check for errors and abort the callback if a scheduler error
is detected. */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
hduPrintError(stderr, &error,
"Error detected while rendering gravity well\n");
if (hduIsSchedulerError(&error))
{
return HD_CALLBACK_DONE;
}
}
/* Signify that the callback should continue running, i.e. that
it will be called again the next scheduler tick. */
return HD_CALLBACK_CONTINUE;
}
