annotate native/Falcon/HapticManager.cpp @ 8:ea7885bd9bff tip

fixed bug : render solid line as dotted/dashed when moving the stylus from dotted/dashed to solid

/*  
 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;