comparison native/Falcon/HapticManager.cpp @ 5:d66dd5880081

Added support for Falcon Haptic device and Tablet/Mouse as haptic device

+/*  
+ CCmI Editor - A Collaborative Cross-Modal Diagram Editing Tool
+  
+ Copyright (C) 2011  Queen Mary University of London (http://ccmi.eecs.qmul.ac.uk/)
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/  
+
+#include "HapticManager.h"
+
+using namespace HAPI;
+
+// Render a sphere using OpenGL calls.
+void drawSphere() {
+  GLUquadricObj* pObj = gluNewQuadric();
+  gluSphere(pObj, 0.0010, 10, 10);
+  gluDeleteQuadric(pObj );
+}
+
+HapticManager::HapticManager(CollectionsManager * cManager, 
+							 void (*func)(
+								const jchar cmd,
+								const jint ID,
+								const jdouble startx, 
+								const jdouble starty, 
+								const jdouble endx, 
+								const jdouble endy)
+							) 
+		: executeCommand(func), cm(cManager), force_effect(0), last_touched_point(NULL),
+		last_touched_line(NULL),magnetic_mode(LOOSE), pickup_mode(RELEASED), object_unselected(false),
+		magnetic_mode_changed(false), pickedup_point(NULL), pickedup_line(NULL) {}
+
+
+bool HapticManager::init (void){
+	
+	hd = new AnyHapticsDevice();
+	
+	// The haptics renderer to use.
+	hd->setHapticsRenderer( new HAPI::GodObjectRenderer() );
+	
+	/* init the device */
+	if(hd->initDevice() != HAPIHapticsDevice::SUCCESS){
+		return false;
+	}
+	
+	/* enable the device ( forces and positions will be updated) */
+	hd->enableDevice();
+	
+	return true;
+}
+
+/* draws the visual diagram every time it's called. The haptic diagram is redrawn only if *
+ * something has changed in the diagram ( redraw_haptic_scene = true ) as, unlike openGL, *
+ * it doesn't need to be drawn at each frame but only once                                */
+void HapticManager::drawDiagram(bool redraw_haptics_scene, bool pickup, int _attract_to ){
+
+	/* STOP_ATTRACTION means the object has been reached (or eventually deleted). The attraction *
+	 * force must therefore be stopped and a redrawing of the haptic forces is necessary         */
+	if(attraction_mode == STOP_ATTRACTION){
+		attraction_mode = NO_ATTRACTION;
+		attract_to = NO_ID;
+		redraw_haptics_scene = true;
+	}
+
+	/* _attract_to is different from NO_ID there is no attraction going on (attraction_mode = NO_ATTRACTION)*
+	 * It differs from STOP_ATTRACTION in that the haptic scene doesn't have to be repainted                */
+	if(_attract_to != NO_ID){
+		attract_to = _attract_to;
+		attraction_mode = ACTIVE;
+		redraw_haptics_scene = true;
+	}
+
+	if(magnetic_mode_changed){
+		redraw_haptics_scene = true;
+		magnetic_mode_changed = false;
+	}
+
+	if(pickup){
+		pickup_mode = START_DRAGGING;
+		redraw_haptics_scene = true;
+	}
+
+	/* wash before use */
+	if(redraw_haptics_scene){
+		point_set.clear();
+		line_set.clear();
+		point_id_map.clear();
+		line_id_map.clear();
+	}
+
+	/* --- draw the edges --- */
+	glPushAttrib(GL_ENABLE_BIT);
+	glColor3f(1.0f,0.0f,0.0f); // Red
+	glLineWidth(2.0);
+	glDisable(GL_LIGHTING);  
+	glEnable(GL_COLOR_MATERIAL);
+	int numEdges = cm->getEdgesNum();
+	for ( int i = 0; i < numEdges; i++){
+		/* draw the edge in openGL */
+		CollectionsManager::EdgeData & ed = cm->getEdgeData(i);
+		glPushAttrib(GL_ENABLE_BIT);
+		glLineStipple(1, ed.stipplePattern);
+		glEnable(GL_LINE_STIPPLE);
+		glBegin(GL_LINES);
+		for(unsigned int j = 0; j < ed.getSize(); j++){
+			for(unsigned int k = j; k < ed.getSize(); k++){
+				if(ed.adjMatrix[j][k]){
+					glVertex3d(ed.x[j]*2,ed.y[j]*GRAPHIC_SCALE,0);
+					glVertex3d(ed.x[k]*2,ed.y[k]*GRAPHIC_SCALE,0);				
+				}
+			}
+		}
+		glEnd();		
+		glPopAttrib();
+		/* update haptics edge line set if a change in the collection occurred */
+		if(redraw_haptics_scene){
+			/* build the vector with the edges*/
+			for(unsigned int j = 0; j < ed.getSize(); j++){
+				for(unsigned int k = j; k < ed.getSize(); k++){
+					if(ed.adjMatrix[j][k]){
+						line_set.push_back(Collision::LineSegment (
+							Vec3(ed.x[j],ed.y[j],0),
+							Vec3(ed.x[k],ed.y[k],0)
+						));
+					}
+				}
+			}
+
+			for(vector< HAPI::Collision::LineSegment>::iterator itr = line_set.begin(); itr != line_set.end(); itr++){
+				line_id_map.insert(pair<Collision::LineSegment*,int>(&(*itr),ed.hapticId));
+			}
+		}
+	}
+	glPopAttrib();
+
+	/* --- draw the nodes --- */
+	int numNodes = cm->getNodesNum();
+	glColor3f(1.0f, 1.0f, 1.0f); // white	
+	for( int i = 0; i < numNodes; i++){
+		/* draw the nodes in openGL */
+		glPushMatrix();
+		CollectionsManager::NodeData &nd = cm->getNodeData(i);
+		glTranslated(nd.x*GRAPHIC_SCALE,nd.y*GRAPHIC_SCALE, 0);
+		drawSphere();
+		glPopMatrix();
+
+		/* update haptics node line set if a change in the collection occurred */
+		if(redraw_haptics_scene){
+			point_set.push_back(Vec3(nd.x,nd.y,0));
+			point_id_map.insert(pair<Collision::Point*,int>(&(point_set.back()),nd.hapticId)) ;
+		}
+	}
+	
+	/* --- update the haptic force if a change in the collection occurred --- */
+	if(redraw_haptics_scene){
+		/* reset the effects and set up new one, otherwise they would accumulate */
+		hd->clearEffects();
+		/* first lines */
+		if(!line_set.empty()){
+			/* force according to the current attraction_mode */
+			int force_factor = (magnetic_mode == STICKY) ? LINE_FORCE_FACTOR_STICKY : LINE_FORCE_FACTOR_LOOSE;
+			/* if the user is dragging or looking for an object, the force *
+			 * factor is always loose until she drops or find the object   */
+			if(pickup_mode == DRAGGING || pickup_mode == START_DRAGGING || attraction_mode == ACTIVE){
+				force_factor = LOOSE;
+			}
+			
+			/* update the force */
+			force_effect.reset( new HapticShapeConstraint(new HapticLineSet( line_set, 0 ),force_factor));			
+			hd->addEffect(force_effect.get());
+
+			/* recalculate last_touched_line if it was != NULL as the pointed      *
+			   object is now destroyed after clear() and replaced with a new one  */
+			if(last_touched_line != NULL){
+				last_touched_line = NULL;
+				vector<Collision::LineSegment>::iterator itr; 
+				Vec3 closest_point, normal, tex_coord;
+				for( itr=line_set.begin(); itr != line_set.end(); itr++ ){
+					itr->closestPoint(proxy_pos,closest_point,normal,tex_coord);
+					if(pointsDistance(closest_point,proxy_pos) < LINE_COLLISION_THRESHOLD){
+						last_touched_line = &(*itr);
+						break;
+					}
+				}
+				if(last_touched_line == NULL){
+					/* touched line was != NULL and now is NULL. It means it has been deleted *
+					 * while being touched, therefore an unselect command must be issued      */
+					object_unselected = true;
+				}
+			}
+		}else if(last_touched_line != NULL){
+			/* if last_touuched_line was != null it means an edge has been deleted   * 
+			 * which was being touched, therefore un unselect command must be issued */
+			object_unselected = true;
+			/* if there are no lines there cannot be a last touched one */
+			last_touched_line = NULL;
+		}
+		
+		/* now points */
+		if(!point_set.empty()){
+			/* update the force */
+			force_effect.reset( new HapticShapeConstraint(new HapticPointSet( point_set, 0 ),POINT_FORCE_FACTOR ) );
+			hd->addEffect(force_effect.get());
+
+			/* recalculate lastTouchedNode if it was != NULL as the pointed object is now destroyed after clear() */
+			if(last_touched_point != NULL){
+				last_touched_point = NULL;
+				vector<Collision::Point>::iterator itr;
+				for(itr=point_set.begin(); itr != point_set.end(); itr++ ){
+					if(pointsDistance(itr->position,proxy_pos) < POINT_COLLISION_THRESHOLD ){
+						last_touched_point = &(*itr);
+						break;
+					}
+				}
+				if(last_touched_point == NULL){
+					/* touched point was != NULL and now is NULL. It means it has been deleted *
+					 * while being touched, therefore an unselect command must be issued       */
+					object_unselected = true;
+				}
+			}
+		}else if(last_touched_point != NULL){
+			/* if last_touuched_point was != null it means a node has been deleted   * 
+			 * which was being touched, therefore un unselect command must be issued */
+			object_unselected = true;
+			/* if there are no points there cannot be a last touched one */
+			last_touched_point = NULL;
+		}
+
+		if(pickup_mode == START_DRAGGING && attraction_mode == NO_ATTRACTION){
+			force_effect.reset(new HapticSpring(proxy_pos,SPRING_FORCE_FACTOR));
+			hd->addEffect(force_effect.get());
+			/* spring force is initialized not pickup_mode goes to DRAGGING*/
+			pickup_mode = DRAGGING;
+		}else if(attraction_mode == ACTIVE){
+			bool found = false;
+			/* let's look for the object into the nodes */
+			for(map<HAPI::Collision::Point*,int>::iterator itr = point_id_map.begin();itr != point_id_map.end(); itr++){
+				if( (*itr).second == attract_to ){
+					force_effect.reset(new HapticSpring(((*itr).first)->position,SPRING_FORCE_FACTOR));
+					hd->addEffect(force_effect.get());
+					found = true;
+					break;
+				}
+			}
+			/* if not found look into the edges */
+			if(!found){
+				for(map<HAPI::Collision::LineSegment*,int>::iterator itr = line_id_map.begin();itr != line_id_map.end(); itr++){
+					if( (*itr).second == attract_to ){
+						HAPI::Collision::LineSegment* line_ptr = (*itr).first;
+						force_effect.reset(new HapticSpring(
+							midPoint(line_ptr->start, line_ptr->end),
+							SPRING_FORCE_FACTOR));
+						hd->addEffect(force_effect.get());
+						found = true;
+						break;
+					}
+				}
+			}
+
+			if(!found){
+				/* element has been deleted before user could reach it: go back to normal */
+				attraction_mode = STOP_ATTRACTION;
+			}
+
+		}
+		/* just deallocate the memory for the last force effect */
+		force_effect.reset();
+		/* tranfer all the forces to the device */
+		hd->transferObjects();
+	}
+}
+
+
+void HapticManager::drawCursor(){
+	HAPIHapticsRenderer *hr = hd->getHapticsRenderer();
+	if( hr ) {
+		/* save the proxy pos in a global variable */
+		proxy_pos = static_cast< HAPIProxyBasedRenderer * >(hr)->getProxyPosition();
+
+		glPushMatrix();
+		glPushAttrib(GL_CURRENT_BIT | GL_ENABLE_BIT | GL_LIGHTING_BIT);
+		glTranslatef( (GLfloat)proxy_pos.x*GRAPHIC_SCALE,
+			(GLfloat)proxy_pos.y*GRAPHIC_SCALE,
+			(GLfloat)proxy_pos.z );
+		glEnable(GL_COLOR_MATERIAL);
+		glColor3f(0.0f, 0.5f, 1.0f);// Blue
+		drawSphere();
+		glPopAttrib();
+		glPopMatrix();
+	}
+}
+
+bool HapticManager::checkNodeCollision(){
+	/* if the proxy goes far enough from the last touched node, then * 
+	 * lastTouched node can be set back to NULL						 */
+	bool do_unselect = false;
+	if(last_touched_point != NULL){
+		if(pointsDistance(last_touched_point->position, proxy_pos) > POINT_COLLISION_THRESHOLD){
+			last_touched_point = NULL;
+			do_unselect = true;
+		}
+	}
+
+	/* find the closest point among those closer than POINT_COLLISION_THRESHOLD */
+	vector<Collision::Point>::iterator itr;
+	HAPI::Collision::Point* found_point = NULL;
+	double min_dist = POINT_COLLISION_THRESHOLD;
+	double dist; 
+	for(itr=point_set.begin(); itr != point_set.end(); itr++ ){
+		if((dist = pointsDistance(itr->position,proxy_pos)) < min_dist ){
+			found_point = &(*itr);
+			min_dist = dist;
+		}
+	}
+
+	if(found_point != NULL){ 
+		/* if the last touched point is the same, it means the cursor was           *  
+		 * already on it therefore do nothing as the node name is uttered only when *   
+		 * the proxy comes across it and not at each frame (it would make a mess)   */
+		if(last_touched_point != found_point){ 
+			last_touched_point = found_point;
+			/* Setting last_touched_line to NULL, means untouching the eventual touching line.    *
+			* When a node is touched, the connected lines are indeed untouched as only one object *
+			* at time can be touching the position proxy                                          */
+			last_touched_line = NULL;
+			int point_id = point_id_map[last_touched_point];
+			/* select the node for eventual highlighting int the java tree */
+			executeCommand(SELECT_CMD,point_id,0,0,0,0);
+			/* utter the name of the node */
+			executeCommand(SPEAK_NAME_CMD,point_id,0,0,0,0);
+			if(attraction_mode == ACTIVE && point_id == attract_to){
+				/* we reached the attracting node, stop the attraction force */
+				attraction_mode = STOP_ATTRACTION;
+			}
+		}
+		return true;
+	}
+
+	if(do_unselect)
+		executeCommand(UNSELECT_CMD,0,0,0,0,0);
+
+	return false;
+}
+
+bool HapticManager::checkEdgeCollision(){
+	Vec3 closest_point, normal, tex_coord;
+
+	/* id is taken from the last touched line is overwritten. To understand why suppose we have an edge     *
+	 * made of two lines A and B. When going from one line to another we don't have to utter the name (see  *
+	 * comment below. last_touched_line goes from a to NULL before being set to B. If lastTouchedLineId is  *
+	 * calculated before checking for NULL-ness it will become -1 and it won't be useful anymore to avoid   *
+	 * uttering the edge name when going from A to B. In this way instead proxy goes from A to B, the id is *
+	 * to A's, last_touched_line becomes NULL and then it becomes B, but the name is not uttered because id *
+	 * is the same. This is a very long comment. I want to finish the line to make it look better. bye bye  */
+	int lastTouchedLineId = (last_touched_line == NULL) ? - 1 : line_id_map[last_touched_line]; 
+
+	bool do_unselect = false;
+	/* if the proxy goes far enough from the last touched line, then last_touched_line can be set back to NULL */
+	if(last_touched_line != NULL){
+		last_touched_line->closestPoint(proxy_pos,closest_point,normal,tex_coord);
+		if(pointsDistance(closest_point, proxy_pos) > LINE_COLLISION_THRESHOLD){
+			last_touched_line = NULL;
+			do_unselect = true;
+		}
+	}
+
+	/* find the closest line to the proxy among those closer than LINE_COLLISION_THRESHOLD */
+	vector<Collision::LineSegment>::iterator itr; 
+	HAPI::Collision::LineSegment* found_line = NULL;
+	double min_dist = LINE_COLLISION_THRESHOLD;
+	double dist; 
+	for( itr=line_set.begin(); itr != line_set.end(); itr++ ){
+		itr->closestPoint(proxy_pos,closest_point,normal,tex_coord);
+		if((dist = pointsDistance(closest_point,proxy_pos)) < min_dist){
+			found_line = &(*itr);
+			min_dist = dist;
+		}
+	}
+
+	if(found_line != NULL){
+		last_touched_line = found_line;
+		/* proxy can touch only one object at time either line or point */
+		last_touched_point = NULL;
+		/* An edge can be broken into several lines. Such lines will map to the same edge id *
+		 * If the proxy detouches from a line but immediately touches another line mapped to *
+		 * the same id, it just means the proxy is going aling the line and therefore no     *
+		 * name must be uttered again (the name must be uttered when touching an edge and    *
+		 * not when touching each line. We keep track of the last touched id and if it's the *
+		 * as te new touched line id, then nothing is uttered, for we're on the same edge    */
+		if(lastTouchedLineId != line_id_map[last_touched_line]){
+			int line_id = line_id_map[last_touched_line];
+			/* select the node for eventual highlighting int the java tree */
+			executeCommand(SELECT_CMD,line_id,0,0,0,0);
+			/* utter the name of the edge */
+			executeCommand(SPEAK_NAME_CMD,line_id,0,0,0,0);
+			if(attraction_mode == ACTIVE && line_id == attract_to){
+				/* we reached the attracting node, stop the attraction force */
+				attraction_mode = STOP_ATTRACTION;
+			}
+		}
+		return true;
+	}
+
+	if(do_unselect){
+		executeCommand(UNSELECT_CMD,0,0,0,0,0);
+	}
+
+	return false;
+}
+
+bool HapticManager::checkUnselection(void){
+	if(object_unselected){
+		object_unselected = false;
+		executeCommand(UNSELECT_CMD,0,0,0,0,0);
+		return true;
+	}
+	return false;
+}
+
+bool HapticManager::checkButtons(void){
+	HAPIInt32 status = hd->getButtonStatus();
+	/* handle the buttons. Buttons can be pressed one at time. If a button is pressed all the    *
+     * other buttons (either when pressed or released) are ignored untill the button is released */
+	if(pressed_button == NO_BUTTON){
+		if(status & FRONT_BUTTON){ 
+			/* front button pressed, change how magnetic the lines are */ 
+			pressed_button = FRONT_BUTTON;
+			/* switch from sticky to loose and vice versa */
+			magnetic_mode = (magnetic_mode == STICKY) ? LOOSE : STICKY;
+			magnetic_mode_changed = true;
+			/* make a sound according to the new attraction mode */
+			executeCommand(PLAY_SOUND_CMD, (magnetic_mode == STICKY) ? STICKY_MODE_SOUND : LOOSE_MODE_SOUND ,0,0,0,0);
+			return true;
+		}else if(status & REAR_BUTTON){ 
+			pressed_button = REAR_BUTTON;
+			/* rear button pressed: First time it picks up an object, second time it drops it */ 
+			if(pickup_mode == RELEASED){ // pickup mode was released, then now user picked up an object 
+				if(last_touched_point != NULL){
+					pickedup_point = last_touched_point;
+					last_dragging_pos = pickedup_point->position; // last_dragging_pos used in checkMotion 
+					/* send a pickup command to the Java thread which will in turn issue another pickup   *
+					 * command to this thread (pickup = true in drawDiagram) if the lock could be granted */
+					executeCommand(PICKUP_CMD,point_id_map[last_touched_point],0,0,0,0);
+				}else if(last_touched_line != NULL){
+					pickedup_line = last_touched_line;
+					pickup_line_pos = proxy_pos; // the point where the line was picked up 
+					last_dragging_pos = proxy_pos;// last_dragging_pos used in checkMotion 
+					executeCommand(PICKUP_CMD,line_id_map[last_touched_line],0,0,0,0);
+				}/* else user picked up thin air. Do nothing.  */
+			}else if(pickup_mode == DRAGGING){ // pickup mode was dragging, then now user dropped an object 
+				pickup_mode = RELEASED;
+				Vec3 & new_position = hapticToScreenSpace(Vec3(proxy_pos),
+						cm->getScreenWidth(),
+						cm->getScreenHeight());
+				if(pickedup_point){
+					executeCommand(MOVE_CMD,point_id_map[pickedup_point],new_position.x,new_position.y,0,0);
+					pickedup_point = NULL;
+				}else{
+					hapticToScreenSpace(pickup_line_pos,
+						cm->getScreenWidth(),
+						cm->getScreenHeight());
+					executeCommand(MOVE_CMD,
+						line_id_map[pickedup_line],
+						new_position.x,
+						new_position.y,
+						pickup_line_pos.x,
+						pickup_line_pos.y);
+					pickedup_line = NULL;
+				}
+				return true;
+			}
+		} else if(status & ( LEFT_BUTTON | RIGHT_BUTTON )){ // either left or right button pressed
+			pressed_button = (status == LEFT_BUTTON) ? LEFT_BUTTON : RIGHT_BUTTON;
+			if(last_touched_point != NULL){ // priority to nodes 
+				executeCommand(SPEAK_INFO_CMD,point_id_map[last_touched_point], 0,0,0,0);
+				return true;
+			}
+			if(last_touched_line != NULL){
+				executeCommand(SPEAK_INFO_CMD,line_id_map[last_touched_line], 0,0,0,0);
+				return true;
+			}
+		}
+	} else if ( /* deactivate the buttons */
+		(pressed_button == FRONT_BUTTON && !(status & FRONT_BUTTON)) ||
+		(pressed_button == REAR_BUTTON && !(status & REAR_BUTTON))   ||
+		(pressed_button == LEFT_BUTTON && !(status & LEFT_BUTTON))   ||
+		(pressed_button == RIGHT_BUTTON && !(status & RIGHT_BUTTON))) 
+	{ 
+		pressed_button = NO_BUTTON;
+	} 
+	return false;
+}
+
+bool HapticManager::checkMotion(void){
+	if(pickup_mode != DRAGGING)
+		return false;
+	if(pointsDistance(proxy_pos,last_dragging_pos) > DRAGGING_SOUND_THRESHOLD){
+		last_dragging_pos = proxy_pos;
+		executeCommand(PLAY_SOUND_CMD,DRAGGING_SOUND,0,0,0,0);
+		return true;
+	}
+	return false;
+}
+
+void HapticManager::dispose(void){
+	hd->releaseDevice();
+	delete hd;
+}
+
+
+/* static constants initialization */
+const int HapticManager::NO_ID = 0;
+
+const int HapticManager::GRAPHIC_SCALE = 2; 
+const int HapticManager::SPRING_FORCE_FACTOR = 200;
+const int HapticManager::LINE_FORCE_FACTOR_STICKY = 2000;
+const int HapticManager::LINE_FORCE_FACTOR_LOOSE = 100;
+const int HapticManager::POINT_FORCE_FACTOR = 20;
+const double HapticManager::POINT_COLLISION_THRESHOLD = 0.0025;
+const double HapticManager::LINE_COLLISION_THRESHOLD = 0.0025;
+const double HapticManager::DRAGGING_SOUND_THRESHOLD = 0.01;
+
+const int HapticManager::LOOSE_MODE_SOUND = 0;
+const int HapticManager::STICKY_MODE_SOUND = 1;
+const int HapticManager::DRAGGING_SOUND = 3;
+
+const char HapticManager::MOVE_CMD = 'm';
+const char HapticManager::PLAY_SOUND_CMD = 'g';
+const char HapticManager::SPEAK_NAME_CMD = 't';
+const char HapticManager::PICKUP_CMD = 'c';
+const char HapticManager::SPEAK_INFO_CMD = 'i';
+const char HapticManager::SELECT_CMD = 's';
+const char HapticManager::UNSELECT_CMD = 'u';
+
+const HAPIInt32 HapticManager::FRONT_BUTTON = (1 << 2);
+const HAPIInt32 HapticManager::LEFT_BUTTON = (1 << 1);
+const HAPIInt32 HapticManager::RIGHT_BUTTON = (1 << 3);
+const HAPIInt32 HapticManager::REAR_BUTTON = (1 << 0);
+const HAPIInt32 HapticManager::NO_BUTTON = 0;