+#include <SDL/SDL.h>
+#include <SDL/SDL_thread.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+#include <time.h>
+#include <sys/time.h>
+
+#include "icons.h"
+
+#include "divers.h"
+#include "vtplayer-ctrl.h"
+#include "params.h"
+#include "evenements.h"
+#include "guidance.h"
+
+#ifdef __WIN32
+#include "gettimeofday.h"
+#endif
+
+#define EPSILON 0.0000001
+
+#define RESET 0x00000000
+
+#define RIGHT_H 0
+#define LEFT_H 1
+
+#define LEFT_H 1
+
+SDL_mutex * tacton_mutex;
+#define LOCK SDL_mutexP(tacton_mutex)
+#define UNLOCK SDL_mutexV(tacton_mutex)
+
+#ifdef DEBUG
+#define dprintf(x...) printf(x)
+#else
+#define dprintf(x...)
+#endif
+
+/// A set is composed of the icons of the 8 directions
+typedef struct set_t
+{
+ u32 *icons[8];
+ int nbframes[8];
+ int speeds[3];
+}iconset_t;
+
+iconset_t *set = NULL;
+
+int currentspeed = 100;
+int currentsegment = 0;
+unsigned char *shapepixels;
+u32 currenttacton;
+u32 tacton;
+u32 tactonfixed;
+
+extern param_list **list_params;
+extern param_list **list_params;
+extern struct mouse_status_t mouse_status;
+extern int resol_x;
+extern int resol_y;
+extern shape_type shape;
+extern int condition;
+extern int left_handed;
+extern FILE *logfile;
+extern int running;
+extern int maxtime;
+extern int training;
+
+u32 get_tacton()
+{
+ u32 r;
+ LOCK;
+ switch(condition)
+ {
+ case CONDITION_MT:
+ if (left_handed)
+ r = (tacton >> 16) + (tactonfixed << 16);
+ else
+ r = tacton + tactonfixed;
+ break;
+ case CONDITION_MV:
+ if (left_handed)
+ r = tacton >> 16;
+ else
+ r = tacton;
+ break;
+ case CONDITION_PT:
+ if (left_handed)
+ r = tacton + tactonfixed;
+ else
+ r = (tacton >> 16) + (tactonfixed << 16);
+ break;
+ case CONDITION_PV:
+ if (left_handed)
+ r = tacton;
+ else
+ r = tacton >> 16;
+ break;
+ }
+// r = tacton;
+ UNLOCK;
+ return r;
+}
+
+/// Loads a Tacton set from a text file
+// format :
+// 8 lines for the eight directions
+// separator : ';'
+// - nb frames : integer
+// - the frames : u32
+void load_set()
+{
+ FILE *f;
+ int i, j, k, currentnumber = 0;
+ char buffer[BUFFERSIZE + 1];
+ //Left handed
+ int hand = RIGHT_H;
+ char *temp;
+ if ((temp = get_params("hand")) && (strcmp(temp, "left") == 0))
+ hand = LEFT_H;
+
+ char buffer2[BUFFERSIZE + 1], *namebuffer;
+ snprintf(buffer2, BUFFERSIZE, "%s", get_params("tactonsfile"));
+ if (fileExists(buffer2))
+ namebuffer = strdup(buffer2);
+ else
+ {
+ snprintf(buffer2, BUFFERSIZE, "data/%s", get_params("tactonsfile"));
+ if (fileExists(buffer2))
+ namebuffer = strdup(buffer2);
+ else
+ {
+ snprintf(buffer2, BUFFERSIZE, "../data/%s", get_params("tactonsfile"));
+ if (fileExists(buffer2))
+ namebuffer = strdup(buffer2);
+ else
+ return;
+ }
+ }
+ if ((f = fopen(namebuffer,"r")) == NULL)
+ return;
+ free(namebuffer);
+
+ set = (iconset_t *) malloc(sizeof(iconset_t));
+
+ for (i = 0 ; i < 8 ; i++)
+ {
+ dprintf("reading Tacton %d : ", i);
+ //we read a line from the buffer
+ if (fgets(buffer, BUFFERSIZE, f) == NULL)
+ {
+ fprintf(stderr, "some Tactons are missing\n");
+ free(set);
+ set = NULL;
+ return;
+ }
+ //we get the first number : the number of frames
+ j = 0, currentnumber = 0;
+ while(j < BUFFERSIZE && j < strlen(buffer) && buffer[j] != ';')
+ {
+ if ( buffer[j] >= '0' && buffer[j] <= '9' )
+ currentnumber = currentnumber * 10 + buffer[j] - '0';
+ j++;
+ }
+ j++;
+ set->nbframes[i] = currentnumber;
+ set->icons[i] = (u32 *) malloc(set->nbframes[i] * sizeof(u32));
+ dprintf("%d frames :\n", set->nbframes[i]);
+
+ //we read the frames
+ for (k = 0 ; k < set->nbframes[i] ; k++)
+ {
+ currentnumber = 0;
+ while(j < BUFFERSIZE && j < strlen(buffer) && buffer[j] != ';')
+ {
+ if ( buffer[j] >= '0' && buffer[j] <= '9' )
+ currentnumber = currentnumber * 16 + buffer[j] - '0';
+ if ( buffer[j] >= 'a' && buffer[j] <= 'f' )
+ currentnumber = currentnumber * 16 + buffer[j] - 'a' + 10;
+ j++;
+ }
+ j++;
+ if (hand == 1)
+ set->icons[i][k] = currentnumber << 16;
+ else
+ set->icons[i][k] = currentnumber;
+ dprintf("frame %2d : %08lx\n", k, set->icons[i][k]);
+ }
+ }
+ //read speed values from parameters
+ temp = get_params("speed1");
+ if (temp)
+ set->speeds[0] = atoi(temp);
+ else
+ set->speeds[0] = 100;
+ temp = get_params("speed2");
+ if (temp)
+ set->speeds[1] = atoi(temp);
+ else
+ set->speeds[1] = 100;
+ temp = get_params("speed3");
+ if (temp)
+ set->speeds[2] = atoi(temp);
+ else
+ set->speeds[2] = 100;
+
+ dprintf("speed1 = %d, speed2 = %d, speed3 = %d\n", set->speeds[0], set->speeds[1], set->speeds[2]);
+
+ return;
+}
+
+int tacton_thread(void *p)
+{
+ int temp = 0;//, temp2=0;
+ struct timeval *now = (struct timeval *) malloc(sizeof(struct timeval));
+ struct timeval *start = (struct timeval *) malloc(sizeof(struct timeval));
+ gettimeofday(start, NULL);
+ int status = 1;
+ while(running)
+ {
+ #if 0
+// LOCK;
+ if (tacton != currenttacton)
+ {
+ status = 1;
+ gettimeofday(start, NULL);
+ currenttacton = tacton;
+ set_pads(tacton);
+// UNLOCK;
+ continue;
+ }
+// UNLOCK;
+#endif
+// printf("current=%ld, target=%ld, diff=%d\n", 1000 * now->tv_sec + now->tv_usec / 1000, currentspeed + 1000 *start->tv_sec + start->tv_usec / 1000, currentspeed);
+ gettimeofday(now, NULL);
+ unsigned int sec = now->tv_sec - start->tv_sec;
+ unsigned int msec = (unsigned int)((now->tv_usec - start->tv_usec) / 1000);
+
+ temp++;
+// if (temp % 100 == 0)
+// dprintf("tacton_thread still alive %d@%d:%d, tacton=%08lx %d:%d\n", temp, (int)now->tv_sec, (int)now->tv_usec, tacton, sec, msec);
+ if (1000 * sec + msec > currentspeed)
+ {
+// temp2 = 0;
+ status = !status;
+ start->tv_usec += 1000 * currentspeed;
+ if (start->tv_usec >= 1000000)
+ {
+ start->tv_usec -= 1000000;
+ start->tv_sec++;
+ }
+ sec = now->tv_sec - start->tv_sec;
+ msec = (unsigned int)((now->tv_usec - start->tv_usec) / 1000);
+
+// gettimeofday(start, NULL);
+
+// LOCK;
+ if (status == 1)
+ set_pads(tacton | tactonfixed);
+ else
+ set_pads(tactonfixed);
+// UNLOCK;
+ }
+// else
+// {
+// temp2++;
+// if (temp2 > 10)
+// dprintf("Too many times ! %d, tacton=%08lx %d:%d=>%d\n", temp2, tacton, sec, msec, 1000 * sec + msec);
+// }
+
+ SDL_Delay(10);
+ }
+
+ if (now)
+ free(now);
+ if (start)
+ free(start);
+
+ dprintf("Tacton thread quitting\n");
+ return (0);
+}
+
+SDL_Thread *create_tacton_thread()
+{
+ SDL_Thread *r;
+ dprintf("Create the tactons thread\n");
+ if ((r = SDL_CreateThread(tacton_thread, NULL)) == NULL)
+ {
+ printf("Impossible to run the tactons thread: %s\n", SDL_GetError());
+ return NULL;
+ }
+ return r;
+}
+
+void display_config_from_angle(double angle, int outside)
+{
+ angle += M_PI/8;
+ while (angle < EPSILON)
+ angle += 2 * M_PI;
+// LOCK;
+ if (angle < M_PI_4 + EPSILON)
+ tacton = 0x88880000;
+ else if (angle < M_PI_2 + EPSILON)
+ tacton = 0x888f0000;
+ else if (angle < 3*M_PI_4 + EPSILON)
+ tacton = 0x000f0000;
+ else if (angle < M_PI + EPSILON)
+ tacton = 0x111f0000;
+ else if (angle < 5*M_PI_4 + EPSILON)
+ tacton = 0x11110000;
+ else if (angle < 3*M_PI_2 + EPSILON)
+ tacton = 0xf1110000;
+ else if (angle < 7*M_PI_4 + EPSILON)
+ tacton = 0xf0000000;
+ else //if (angle < 2*M_PI + EPSILON)
+ tacton = 0xf8880000;
+ if (((condition == CONDITION_MT) && !left_handed) || (!(condition == CONDITION_MT) && left_handed))
+ {
+ tacton >>= 16;
+ if (outside)
+ tactonfixed = 0xffff0000;
+ else
+ tactonfixed = 0x00000000;
+ }
+ else if (outside)
+ tactonfixed = 0x0000ffff;
+ else
+ tactonfixed = 0x00000000;
+// UNLOCK;
+// set_pads(tacton);
+// dprintf("%08lx %08lx\n", config, config | 0x0000ffff);
+}
+
+int pins_thread(void *p)
+{
+// int temp = 0;
+ dprintf("Guidance start\n");
+ if (shape.nbv < 2)
+ {
+ printf("Shape too small\n");
+ return (1);
+ }
+
+ //récupération des paramètres. Il serait temps de faire une fonction sympa pour ça...
+ dprintf("Get parameters\n");
+ currentsegment = 0;
+ char *buffer;
+ buffer = get_params("distance1");
+ int distance1;
+ if (buffer)
+ distance1 = atoi(buffer);
+ else
+ distance1 = 100;
+ buffer = get_params("distance2");
+ int distance2;
+ if (buffer)
+ distance2 = atoi(buffer);
+ else
+ distance2 = 100;
+ buffer = get_params("distance3");
+ int distance3;
+ if (buffer)
+ distance3 = atoi(buffer);
+ else
+ distance3 = 100;
+ buffer = get_params("stripwidth");
+ int stripwidth;
+ if (buffer)
+ stripwidth = atoi(buffer);
+ else
+ stripwidth = 50;
+ buffer = get_params("speed1");
+ int speed1;
+ if (buffer)
+ speed1 = atoi(buffer);
+ else
+ speed1 = 50;
+ buffer = get_params("speed2");
+ int speed2;
+ if (buffer)
+ speed2 = atoi(buffer);
+ else
+ speed2 = 50;
+ buffer = get_params("speed3");
+ int speed3;
+ if (buffer)
+ speed3 = atoi(buffer);
+ else
+ speed3 = 50;
+
+ dprintf("Set coordinates function\n");
+ int (*x)(), (*y)();
+ if (condition == CONDITION_PT)
+ {
+ x = get_mouse_x;
+ y = get_mouse_y;
+ }
+ else// if (strcmp(get_params("condition"),"Mv") == 0)
+ {
+ x = get_vtp_x;
+ y = get_vtp_y;
+ }
+
+ dprintf("Compute first segment\n");
+ int abx = shape.x[1] - shape.x[0];
+ int aby = shape.y[1] - shape.y[0];
+ double abl = sqrt(abx * abx + aby * aby);
+ double segmentangle = acos(abx / abl);
+ if (aby > -EPSILON)
+ segmentangle *= -1;
+ dprintf("abx=%d, aby=%d, abl=%f, segmentangle=%f\n", abx, aby, abl, segmentangle);
+ dprintf("Run the loop\n");
+
+ struct timeval *now = (struct timeval *) malloc(sizeof(struct timeval));
+ struct timeval *start = (struct timeval *) malloc(sizeof(struct timeval));
+ gettimeofday(start, NULL);
+
+ //compute the cartesian equation of the line
+ double line_a = shape.y[(currentsegment + 1) % shape.nbv] - shape.y[currentsegment];
+ double line_b = shape.x[currentsegment] - shape.x[(currentsegment + 1) % shape.nbv];
+ double line_c = shape.x[(currentsegment + 1) % shape.nbv] * shape.y[currentsegment] - shape.x[currentsegment] * shape.y[(currentsegment + 1) % shape.nbv];
+
+ //position relative to the line for the guidance
+ double outsidepos = line_a * x() + line_b * y() + line_c;
+
+ while(running)
+ {
+ gettimeofday(now, NULL);
+ int sec = now->tv_sec - start->tv_sec;
+ int msec = (int)((now->tv_usec - start->tv_usec) / 1000);
+ if (logfile)
+ fprintf(logfile, "%ld;%d;%d;%08lx;%d\n", 1000 * (long int)sec + msec, x(), resol_y - y() - 1, tacton, currentspeed);
+// else
+// dprintf("Error with logfile\n");
+ //if maxtime is elapsed we exit the test
+ if (sec > maxtime && !training)
+ {
+ dprintf("time : %d/%d\n", sec, maxtime);
+ running = 0;
+ break;
+ }
+
+ int amx = x() - shape.x[currentsegment];
+ int amy = y() - shape.y[currentsegment];
+ double aml = sqrt(amx * amx + amy * amy);
+ int bmx = x() - shape.x[(currentsegment + 1) % shape.nbv];
+ int bmy = y() - shape.y[(currentsegment + 1) % shape.nbv];
+ double bml = sqrt(bmx * bmx + bmy * bmy);
+
+ //distance from the pointer's projection to the target
+ double between;
+// dprintf("abx=%d, aby=%d, abl=%f, amx=%d, amy=%d, aml=%f, bmx=%d, bmy=%d, bml=%f\n", abx, aby, abl, amx, amy, aml, bmx, bmy, bml);
+ if (aml > bml)
+ between = abl - (amx * abx + amy * aby) / abl;
+ else
+ between = (bmx * (-abx) + bmy * (-aby)) / abl;
+
+ //distance from the cursor to the segment
+ //we have to add fabs, otherwise sqrt(-0.0) = nan
+ double dist = sqrt(fabs(bml * bml - between * between));
+// dprintf("dist=%f, between=%f, abl=%f, aml=%f, stripwidth=%d, test=%f\n", dist, between, abl, aml, stripwidth, bml * bml - between * between);
+
+ if (between > 0)
+ {
+ //if we are before the first point
+ if ((between > abl) && (aml > stripwidth))
+ {
+ if (aml < distance1)
+ currentspeed = speed1;
+ else if (aml < distance2)
+ currentspeed = speed2;
+ else //if (between < distance3)
+ currentspeed = speed3;
+ double guideangle = acos((-amx) / aml);
+ if (amy < 0)
+ guideangle *= -1;
+ display_config_from_angle(guideangle, 1);
+ outsidepos = 0;//line_a * x() + line_b * y() + line_c;
+ }
+ else //we are between the two points
+ {
+ double newoutsidepos = line_a * x() + line_b * y() + line_c;
+ //we are in the strip
+ //we continue to guide if we come back from the outside and we didn't cross the line
+ if (dist < stripwidth && outsidepos * newoutsidepos <= EPSILON)
+ {
+ if (between < distance1)
+ currentspeed = speed1;
+ else if (between < distance2)
+ currentspeed = speed2;
+ else //if (between < distance3)
+ currentspeed = speed3;
+ display_config_from_angle(segmentangle, 0);
+ outsidepos = 0;
+ }
+ else
+ {
+ if (dist < distance1)
+ currentspeed = speed1;
+ else if (dist < distance2)
+ currentspeed = speed2;
+ else //if (dist < distance3)
+ currentspeed = speed3;
+ if (abx > 0)
+ {
+ if (amy / aml > aby / abl)
+ display_config_from_angle(segmentangle + M_PI_2, 1);
+ else
+ display_config_from_angle(segmentangle - M_PI_2, 1);
+ }
+ else if (abx < 0)
+ {
+ if (amy / aml > aby / abl)
+ display_config_from_angle(segmentangle - M_PI_2, 1);
+ else
+ display_config_from_angle(segmentangle + M_PI_2, 1);
+ }
+ else // abx == 0
+ {
+ if (aby > 0)
+ {
+ if (amx > 0)
+ display_config_from_angle(segmentangle - M_PI_2, 1);
+ else
+ display_config_from_angle(segmentangle + M_PI_2, 1);
+ }
+ else if (aby < 0)
+ {
+ if (amx > 0)
+ display_config_from_angle(segmentangle + M_PI_2, 1);
+ else
+ display_config_from_angle(segmentangle - M_PI_2, 1);
+ }
+ else // aby == 0 // should not happen
+ {
+ printf("Vector too short!\n");
+ display_config_from_angle(segmentangle, 1);
+ }
+ }
+ outsidepos = line_a * x() + line_b * y() + line_c;
+ }
+ }
+ }
+ else //we are too far => guide to the target
+ {
+ // if we are at the target
+ if (bml < stripwidth)
+ {
+ currentsegment++;
+ dprintf("Point %d/%d\n", currentsegment, shape.nbv);
+ //if we dit a complete lap
+ if (currentsegment >= shape.nbv)
+ {
+ dprintf("New lap\n");
+ currentsegment = 0;
+ }
+ abx = shape.x[(currentsegment + 1) % shape.nbv] - shape.x[currentsegment];
+ aby = shape.y[(currentsegment + 1) % shape.nbv] - shape.y[currentsegment];
+ abl = sqrt(abx * abx + aby * aby);
+ line_a = shape.y[(currentsegment + 1) % shape.nbv] - shape.y[currentsegment];
+ line_b = shape.x[currentsegment] - shape.x[(currentsegment + 1) % shape.nbv];
+ line_c = shape.x[(currentsegment + 1) % shape.nbv] * shape.y[currentsegment] - shape.x[currentsegment] * shape.y[(currentsegment + 1) % shape.nbv];
+
+ segmentangle = acos(abx / abl);
+ if (aby > -EPSILON)
+ segmentangle *= -1;
+ // dprintf("abx=%d, aby=%d, abl=%f, segmentangle=%f\n", abx, aby, abl, segmentangle);
+ display_config_from_angle(segmentangle, 0);
+// SDL_Delay(10);
+// continue;
+ }
+ else
+ {
+ if (bml < distance1)
+ currentspeed = speed1;
+ else if (bml < distance2)
+ currentspeed = speed2;
+ else //if (between < distance3)
+ currentspeed = speed3;
+ double guideangle = acos((-bmx) / bml);
+ if (bmy < 0)
+ guideangle *= -1;
+ display_config_from_angle(guideangle, 1);
+ }
+/* double guideangle = acos(bmx / bml);
+ if (bmy > 0)
+ guideangle *= -1;
+ guideangle += segmentangle - M_PI;
+ display_config_from_angle(guideangle, 1);*/
+ }
+// temp++;
+// if (temp % 100 == 0)
+// dprintf("pins_thread still alive %d, tacton=%08lx %d:%d\n", temp, tacton, sec, msec);
+
+ SDL_Delay(10);
+ }
+ dprintf("Destroying Tacton Mutex\n");
+ SDL_DestroyMutex(tacton_mutex);
+ dprintf("Pins thread quitting\n");
+ return (0);
+}
+
+SDL_Thread *create_pins_thread()
+{
+ SDL_Thread *r;
+ //load theTactons set
+// printf("Load the tactons\n");
+// load_set(get_params("tactonsfile"));
+ dprintf("Create the tacton mutex\n");
+ if((tacton_mutex = SDL_CreateMutex()) == NULL)
+ {
+ printf("Error while creating the mutex : %s\n", SDL_GetError());
+ return NULL;
+ }
+ dprintf("Create the pins thread\n");
+ if ((r = SDL_CreateThread(pins_thread, NULL)) == NULL)
+ {
+ printf("Impossible to run the pins thread: %s\n", SDL_GetError());
+ return NULL;
+ }
+ return r;
+}
+
+u32 getpixel(int x, int y)
+{
+ if (x < 0)
+ x = 0;
+ if (x >= resol_x)
+ x = resol_x - 1;
+ if (y < 0)
+ y = 0;
+ if (y >= resol_y)
+ y = resol_y - 1;
+ int pixel = 4 * (x + (resol_y - y - 1) * resol_x);
+
+ if (shapepixels[pixel])
+ return 0x00000000;
+ else
+ return 0xffffffff;
+}
+
+int translation_thread(void *p)
+{
+// int temp = 0;
+ int (*x)(), (*y)();
+ if (condition == CONDITION_PV)
+ {
+ x = get_mouse_x;
+ y = get_mouse_y;
+ }
+ else// if (strcmp(get_params("condition"),"Mv") == 0)
+ {
+ x = get_vtp_x;
+ y = get_vtp_y;
+ }
+
+ struct timeval *now = (struct timeval *) malloc(sizeof(struct timeval));
+ struct timeval *start = (struct timeval *) malloc(sizeof(struct timeval));
+ gettimeofday(start, NULL);
+ while (running)
+ {
+ gettimeofday(now, NULL);
+ int sec = now->tv_sec - start->tv_sec;
+ int msec = (now->tv_usec - start->tv_usec) / 1000;
+ if (logfile)
+ fprintf(logfile, "%ld;%d;%d;%08lx\n", 1000 * (long int)sec + msec, x(), resol_y - y() - 1, tacton);
+// else
+// dprintf("Error with logfile\n");
+ //if maxtime is elapsed we exit the test
+ if (sec > maxtime && !training)
+ {
+ dprintf("time : %d/%d\n", sec, maxtime);
+ running = 0;
+ break;
+ }
+
+// printf("%d %d\n", x(), y());
+// LOCK;
+ tacton =
+ (0x80000000 & getpixel(x() + 2, y() + 2)) + \
+ (0x40000000 & getpixel(x() + 1, y() + 2)) + \
+ (0x20000000 & getpixel(x(), y() + 2)) + \
+ (0x10000000 & getpixel(x() - 1, y() + 2)) + \
+ (0x08000000 & getpixel(x() + 2, y() + 1)) + \
+ (0x04000000 & getpixel(x() + 1, y() + 1)) + \
+ (0x02000000 & getpixel(x(), y() + 1)) + \
+ (0x01000000 & getpixel(x() - 1, y() + 1)) + \
+ (0x00800000 & getpixel(x() + 2, y())) + \
+ (0x00400000 & getpixel(x() + 1, y())) + \
+ (0x00200000 & getpixel(x(), y())) + \
+ (0x00100000 & getpixel(x() - 1, y())) + \
+ (0x00080000 & getpixel(x() + 2, y() - 1)) + \
+ (0x00040000 & getpixel(x() + 1, y() - 1)) + \
+ (0x00020000 & getpixel(x(), y() - 1)) + \
+ (0x00010000 & getpixel(x() - 1, y() - 1));
+ if (((condition == CONDITION_MV) && !left_handed) || (!(condition == CONDITION_MV) && left_handed))
+ tacton >>= 16;
+ set_pads(tacton);
+// UNLOCK;
+ SDL_Delay(30);
+ }
+ if (now)
+ free(now);
+ if (start)
+ free(start);
+ dprintf("Translation thread quitting\n");
+ return(0);
+}
+
+SDL_Thread *create_translation_thread()
+{
+ SDL_Thread *r;
+ //load theTactons set
+ dprintf("Create the conversion thread\n");
+ if ((r = SDL_CreateThread(translation_thread, NULL)) == NULL)
+ {
+ printf("Impossible to run the translation thread: %s\n", SDL_GetError());
+ return NULL;
+ }
+ return r;
+}
+
+#undef LOCK
+#undef UNLOCK
projects / shapes.git / commitdiff