void updateHaptics(void)
{
// simulation in now running
simulationRunning = true;
simulationFinished = false;
// main haptic simulation loop
while(simulationRunning)
{
// 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 haptic device
tool->applyForces();
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// precion clock
cPrecisionClock clock;
int i = 0;
// main haptic simulation loop
while(simulationRunning)
{
bool blIO = false;
blIO = deltaCtrl.SetFGetPosStatues(dF,dPos,statu,dT, blOutRange); // changing point
dPos[2]+=0.1;
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// update my pose************MINE***************** we get the force in
tool->updateMyPose(dPos);
// compute interaction forces
tool->computeInteractionForces();
// we need to get the force out to our device**************** MINE
tool->getLastComputedForce(dF);
// send forces to device
tool->applyForces();
// stop clock
double time = clock.stop();
// check if contact occured with cylinder
if(tool->isInContact(cylinder))
{
// get force applied on the y axis
double force = tool->m_lastComputedGlobalForce.y();
// shift cylinderaccording to force
const double K = 0.5;
double pos = cylinder->getLocalPos().y();
pos = cClamp(pos - K * time * force,-0.5, 0.6);
cylinder->setLocalPos(0.0, pos, 0.0);
}
// restart clock
clock.start(true);
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// initialize frequency counter
frequencyCounter.reset();
// initialize precision clock
cPrecisionClock clock;
clock.reset();
// simulation in now running
simulationRunning = true;
simulationFinished = false;
// main haptic simulation loop
while(simulationRunning)
{
// 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 haptic device
tool->applyForces();
// stop clock
double time = clock.stop();
// scroll text according to the haptic device position
const double K = 4000;
double command = tool->m_hapticPoint->getGlobalPosGoal().y();
double pos = textPosition + K * time * command;
textPosition = cClamp(pos, -text->getWidth(), (double)(windowW));
// restart clock
clock.start(true);
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
cPrecisionClock clock;
double ang;
// simulation in now running
simulationRunning = true;
simulationFinished = false;
double t0 = clock.getCPUTimeSeconds();
double rate = 2*M_PI/videoEven->getDuration();
// main haptic simulation loop
while(simulationRunning)
{
// rotate movie (5 deg per second)
ang = -rate*(clock.getCPUTimeSeconds()-t0);
anchor->setLocalRot (cMatrix3d(cVector3d(0,1,0),ang));
// 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 haptic device
tool->applyForces();
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// reset clock
cPrecisionClock clock;
clock.reset();
// simulation in now running
simulationRunning = true;
simulationFinished = false;
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// SIMULATION TIME
/////////////////////////////////////////////////////////////////////
// stop the simulation clock
clock.stop();
// read the time increment in seconds
double timeInterval = clock.getCurrentTimeSeconds();
// restart the simulation clock
clock.reset();
clock.start();
/////////////////////////////////////////////////////////////////////
// HAPTIC FORCE COMPUTATION
/////////////////////////////////////////////////////////////////////
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// get interaction forces magnitude
double force = tool->m_lastComputedGlobalForce.length();
// send forces to haptic device
tool->applyForces();
/////////////////////////////////////////////////////////////////////
// INTERACTION WITH PALETTE
/////////////////////////////////////////////////////////////////////
if (tool->isInContact(palette))
{
cCollisionEvent* contact = tool->m_hapticPoint->getCollisionEvent(0);
if (contact != NULL)
{
// retrieve contact information
cVector3d localPos = contact->m_localPos;
unsigned int triangleIndex = contact->m_triangleIndex;
cTriangleArrayPtr triangles = contact->m_triangles;
// retrieve texture coordinate
cVector3d texCoord = triangles->getTexCoordAtPosition(triangleIndex, localPos);
// retrieve pixel information
int px, py;
palette->m_texture->m_image->getPixelLocation(texCoord, px, py);
// retrieve color information at pixel
palette->m_texture->m_image->getPixelColor(px, py, paintColor);
// update color of tool
tool->m_hapticPoint->m_sphereProxy->m_material->setColor(paintColor);
}
}
/////////////////////////////////////////////////////////////////////
// INTERACTION WITH CANVAS
/////////////////////////////////////////////////////////////////////
if (tool->isInContact(canvas))
{
cCollisionEvent* contact = tool->m_hapticPoint->getCollisionEvent(0);
if (contact != NULL)
{
// retrieve contact information
cVector3d localPos = contact->m_localPos;
unsigned int triangleIndex = contact->m_triangleIndex;
cTriangleArrayPtr triangles = contact->m_triangles;
// retrieve texture coordinate
cVector3d texCoord = triangles->getTexCoordAtPosition(triangleIndex, localPos);
// retrieve pixel information
int px, py;
canvas->m_texture->m_image->getPixelLocation(texCoord, px, py);
// paint color at tool position
const double K_INK = 20;
const double K_SIZE = 10;
const int BRUSH_SIZE = 25;
double size = cClamp((K_SIZE * force), 0.0, (double)(BRUSH_SIZE));
for (int x=-BRUSH_SIZE; x<BRUSH_SIZE; x++)
{
for (int y=-BRUSH_SIZE; y<BRUSH_SIZE; y++)
{
// compute new color percentage
double distance = sqrt((double)(x*x+y*y));
if (distance <= size)
{
// get color at location
cColorb color, newColor;
canvas->m_texture->m_image->getPixelColor(px+x, py+y, color);
// compute color factor based of pixel position and force interaction
double factor = cClamp(K_INK * timeInterval * cClamp(force, 0.0, 10.0) * cClamp(1 - distance/size, 0.0, 1.0), 0.0, 1.0);
// compute new color
newColor.setR((1.0 - factor) * color.getR() + factor * paintColor.getR());
newColor.setG((1.0 - factor) * color.getG() + factor * paintColor.getG());
newColor.setB((1.0 - factor) * color.getB() + factor * paintColor.getB());
// assign new color to pixel
canvas->m_texture->m_image->setPixelColor(px+x, py+y, newColor);
}
}
}
// update texture
canvas->m_texture->markForUpdate();
}
}
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// reset clock
cPrecisionClock clock;
clock.reset();
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// SIMULATION TIME
/////////////////////////////////////////////////////////////////////
// stop the simulation clock
clock.stop();
// read the time increment in seconds
double timeInterval = clock.getCurrentTimeSeconds();
// restart the simulation clock
clock.reset();
clock.start();
// update frequency counter
frequencyCounter.signal(1);
/////////////////////////////////////////////////////////////////////
// HAPTIC FORCE COMPUTATION
/////////////////////////////////////////////////////////////////////
// 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();
/////////////////////////////////////////////////////////////////////
// HAPTIC SIMULATION
/////////////////////////////////////////////////////////////////////
// get position of cursor in global coordinates
cVector3d toolPos = tool->getDeviceGlobalPos();
// get position of object in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute a vector from the center of mass of the object (point of rotation) to the tool
cVector3d v = cSub(toolPos, objectPos);
// compute angular acceleration based on the interaction forces
// between the tool and the object
cVector3d angAcc(0,0,0);
if (v.length() > 0.0)
{
// get the last force applied to the cursor in global coordinates
// we negate the result to obtain the opposite force that is applied on the
// object
cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);
// compute the effective force that contributes to rotating the object.
cVector3d force = toolForce - cProject(toolForce, v);
// compute the resulting torque
cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force));
// update rotational acceleration
const double INERTIA = 0.4;
angAcc = (1.0 / INERTIA) * torque;
}
// update rotational velocity
angVel.add(timeInterval * angAcc);
// set a threshold on the rotational velocity term
const double MAX_ANG_VEL = 10.0;
double vel = angVel.length();
if (vel > MAX_ANG_VEL)
{
angAcc.mul(MAX_ANG_VEL / vel);
}
// add some damping too
const double DAMPING = 0.1;
angVel.mul(1.0 - DAMPING * timeInterval);
// if user switch is pressed, set velocity to zero
if (tool->getUserSwitch(0) == 1)
{
angVel.zero();
}
// compute the next rotation configuration of the object
if (angVel.length() > C_SMALL)
{
object->rotateAboutGlobalAxisRad(cNormalize(angVel), timeInterval * angVel.length());
}
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
cMode state = IDLE;
cGenericObject* object;
cTransform tool_T_object;
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////////
// HAPTIC RENDERING
/////////////////////////////////////////////////////////////////////////
// update frequency counter
frequencyCounter.signal(1);
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
/////////////////////////////////////////////////////////////////////////
// MANIPULATION
/////////////////////////////////////////////////////////////////////////
// compute transformation from world to tool (haptic device)
cTransform world_T_tool = tool->getDeviceGlobalTransform();
// get status of user switch
bool button = tool->getUserSwitch(0);
//
// MODE 1:
// Idle mode - user presses the user switch
//
if ((state == IDLE) && (button == true))
{
// check if at least one contact has occured
if (tool->m_hapticPoint->getNumCollisionEvents() > 0)
{
// get contact event
cCollisionEvent* collisionEvent = tool->m_hapticPoint->getCollisionEvent(0);
// get object from contact event
object = collisionEvent->m_object;
// get transformation from object
cTransform world_T_object = object->getGlobalTransform();
// compute inverse transformation from contact point to object
cTransform tool_T_world = world_T_tool;
tool_T_world.invert();
// store current transformation tool
tool_T_object = tool_T_world * world_T_object;
// update state
state = SELECTION;
}
}
//
// MODE 2:
// Selection mode - operator maintains user switch enabled and moves object
//
else if ((state == SELECTION) && (button == true))
{
// compute new tranformation of object in global coordinates
cTransform world_T_object = world_T_tool * tool_T_object;
// compute new tranformation of object in local coordinates
cTransform parent_T_world = object->getParent()->getLocalTransform();
parent_T_world.invert();
cTransform parent_T_object = parent_T_world * world_T_object;
// assign new local transformation to object
object->setLocalTransform(parent_T_object);
// set zero forces when manipulating objects
tool->m_lastComputedGlobalForce.zero();
}
//
// MODE 3:
// Finalize Selection mode - operator releases user switch.
//
else
{
state = IDLE;
}
/////////////////////////////////////////////////////////////////////////
// FINALIZE
/////////////////////////////////////////////////////////////////////////
// send forces to device
tool->applyForces();
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// initialize frequency counter
frequencyCounter.reset();
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// READ HAPTIC DEVICE
/////////////////////////////////////////////////////////////////////
// read position
cVector3d position;
hapticDevice->getPosition(position);
// read orientation
cMatrix3d rotation;
hapticDevice->getRotation(rotation);
// read gripper position
double gripperAngle;
hapticDevice->getGripperAngleRad(gripperAngle);
// read linear velocity
cVector3d linearVelocity;
hapticDevice->getLinearVelocity(linearVelocity);
// read angular velocity
cVector3d angularVelocity;
hapticDevice->getAngularVelocity(angularVelocity);
// read gripper angular velocity
double gripperAngularVelocity;
hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);
// read userswitch status (button 0)
bool button0, button1, button2, button3;
button0 = false;
button1 = false;
button2 = false;
button3 = false;
hapticDevice->getUserSwitch(0, button0);
hapticDevice->getUserSwitch(1, button1);
hapticDevice->getUserSwitch(2, button2);
hapticDevice->getUserSwitch(3, button3);
/////////////////////////////////////////////////////////////////////
// UPDATE 3D MODELS
/////////////////////////////////////////////////////////////////////
// update arrow
velocity->m_pointA = position;
velocity->m_pointB = cAdd(position, linearVelocity);
// update position and orientation of cursor
cursor->setLocalPos(position);
cursor->setLocalRot(rotation);
// adjust the color of the cursor according to the status of
// the user switch (ON = TRUE / OFF = FALSE)
if (button0)
{
cursor->m_material->setGreenMediumAquamarine();
}
else if (button1)
{
cursor->m_material->setYellowGold();
}
else if (button2)
{
cursor->m_material->setOrangeCoral();
}
else if (button3)
{
cursor->m_material->setPurpleLavender();
}
else
{
cursor->m_material->setBlueRoyal();
}
// update global variable for graphic display update
hapticDevicePosition = position;
/////////////////////////////////////////////////////////////////////
// COMPUTE AND SEND FORCE AND TORQUE
/////////////////////////////////////////////////////////////////////
cVector3d force (0,0,0);
cVector3d torque (0,0,0);
double gripperForce = 0.0;
// apply force field
if (useForceField)
{
// compute linear force
double Kp = 20; // [N/m]
cVector3d forceField = -Kp * position;
force.add(forceField);
// compute angular torque
double Kr = 0.05; // [N/m.rad]
cVector3d axis;
double angle;
rotation.toAngleAxis(angle, axis);
torque = (-Kr * angle) * axis;
}
// apply damping term
if (useDamping)
{
cHapticDeviceInfo info = hapticDevice->getSpecifications();
// compute linear damping force
double Kv = 1.0 * info.m_maxLinearDamping;
cVector3d forceDamping = -Kv * linearVelocity;
force.add(forceDamping);
// compute angluar damping force
double Kvr = 1.0 * info.m_maxAngularDamping;
cVector3d torqueDamping = -Kvr * angularVelocity;
torque.add(torqueDamping);
// compute gripper angular damping force
double Kvg = 1.0 * info.m_maxGripperAngularDamping;
gripperForce = gripperForce - Kvg * gripperAngularVelocity;
}
// send computed force, torque and gripper force to haptic device
hapticDevice->setForceAndTorqueAndGripperForce(force, torque, gripperForce);
// update frequency counter
frequencyCounter.signal(1);
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics( void )
{
// simulation in now running
simulationRunning = true;
simulationFinished = false;
// main haptic simulation loop
while( simulationRunning )
{
/////////////////////////////////////////////////////////////////////////
// HAPTIC RENDERING
/////////////////////////////////////////////////////////////////////////
// update frequency counter
frequencyCounter.signal( 1 );
// compute global reference frames for each object
world->computeGlobalPositions( true );
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// update selected particle to cursor
cVector3d proxy_pos, tool_pos;
tool_pos = tool->getDeviceLocalPos();
proxy_pos.x( tool_pos.x() - SELECTED_PARTICLE_RADIUS );
proxy_pos.y( tool_pos.y() - SELECTED_PARTICLE_RADIUS );
proxy_pos.z( tool_pos.z() + SELECTED_PARTICLE_RADIUS );
selected_particle->setLocalPos( proxy_pos );
cVector3d x_vector( 0, 0, 0 );
x_vector.x( proxy_pos.x() );
// temp_vector is on the y-z plane
cVector3d temp_vector = proxy_pos - x_vector;
cVector3d unit_vector = temp_vector;
unit_vector.normalize();
cVector3d force( 0, 0, 0 );
// If user has picked up a electron, set magnetical effect around shells
if ( ELECTRON == is_selected )
{
for ( int i = 0; i < NUM_SHELLS; i++ )
{
// Set force field depending on the number of electrons placed
if ( particles_left[ELECTRON] == 0 )
{
break;
}
// If less than two electrons are placed, skip force for outer shells
// If at least two electrons are placed, skip first shell
if ( current_atom_num - particles_left[ELECTRON] < 2 && i > 0 ||
current_atom_num - particles_left[ELECTRON] >= 2 && i == 0 )
{
continue;
}
// If we are within the force field
if( temp_vector.length() < ( TORUS_OUTER + OUTER_THRESHOLD + TORUS_DISTANCE*i ) &&
temp_vector.length() > ( TORUS_OUTER - OUTER_THRESHOLD + TORUS_DISTANCE*i ) &&
proxy_pos.x() < 0.07 )
{
in_ok_position = ELECTRON;
// If we are close to the center of the torus
if ( temp_vector.length() >= ( TORUS_OUTER + INNER_THRESHOLD + TORUS_DISTANCE*i ) ||
temp_vector.length() <= ( TORUS_OUTER - INNER_THRESHOLD + TORUS_DISTANCE*i ) )
{
// We are either inside the inner or outer force field
if ( temp_vector.length() > ( TORUS_OUTER - OUTER_THRESHOLD + TORUS_DISTANCE*i ) &&
temp_vector.length() <= ( TORUS_OUTER - INNER_THRESHOLD + TORUS_DISTANCE*i ) )
{
// When on the inside force field for the torus
force = SPRING_CONSTANT * ( unit_vector*( TORUS_OUTER - INNER_THRESHOLD + TORUS_DISTANCE*i ) - temp_vector );
}
else {
// When on the outer force field for the torus
force = SPRING_CONSTANT * ( unit_vector*( TORUS_OUTER + INNER_THRESHOLD + TORUS_DISTANCE*i ) - temp_vector );
}
}
break;
}
else {
in_ok_position = -1;
}
}
// Prevent the user place an electron in the nucleus
if ( temp_vector.length() <= NUCLEUS_RADIUS )
{
force = SPRING_CONSTANT * ( unit_vector*NUCLEUS_RADIUS - temp_vector );
}
}
else if ( PROTON == is_selected || NEUTRON == is_selected )
{
// If cursor is close to the nucleus in the y-z plane, set force to centrum
if ( temp_vector.length() < ( NUCLEUS_RADIUS + 0.05 ) &&
proxy_pos.x() < 0.07 &&
!( PROTON == is_selected && particles_left[PROTON] == 0 ) &&
!( NEUTRON == is_selected && particles_left[NEUTRON] == 0 ) )
{
in_ok_position = PROTON;
if ( temp_vector.length() >= NUCLEUS_RADIUS )
{
force = SPRING_CONSTANT * ( unit_vector*( NUCLEUS_RADIUS ) - temp_vector );
}
// Cannot push inwards
if ( proxy_pos.x() < 0.03 )
{
force.x( SPRING_CONSTANT*( -proxy_pos.x() + 0.03 ) );
}
// Smaller force when pulling
else if ( proxy_pos.x() > 0.04 )
{
force.x( -500*( proxy_pos.x() - 0.04 ) );
}
}
else {
in_ok_position = -1;
}
}
// send forces to haptic device
tool->getHapticDevice()->setForce( force );
}
// exit haptics thread
simulationFinished = true;
}
void updateHaptics(void)
{
// precision clock
cPrecisionClock clock;
clock.reset();
// simulation in now running
simulationRunning = true;
simulationFinished = false;
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// SIMULATION TIME
/////////////////////////////////////////////////////////////////////
// stop the simulation clock
clock.stop();
// read the time increment in seconds
double timeInterval = clock.getCurrentTimeSeconds();
// restart the simulation clock
clock.reset();
clock.start();
// update frequency counter
frequencyCounter.signal(1);
/////////////////////////////////////////////////////////////////////
// HAPTIC FORCE COMPUTATION
/////////////////////////////////////////////////////////////////////
// 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 haptic device
tool->applyForces();
/////////////////////////////////////////////////////////////////////
// HAPTIC SIMULATION
/////////////////////////////////////////////////////////////////////
// check if contact occurred with cylinder
if(tool->isInContact(cylinder))
{
// get force applied on the y axis
double force = tool->m_lastComputedGlobalForce.y();
// shift cylinder according to force
const double K = 0.5;
double pos = cylinder->getLocalPos().y();
pos = cClamp(pos - K * timeInterval * force,-0.5, 0.6);
cylinder->setLocalPos(0.0, pos, 0.0);
}
}
// exit haptics thread
simulationFinished = true;
}