/demos/trunk/rtw/control.c |
---|
0,0 → 1,67 |
#include "kernel/kern.h" |
#include "modules/cabs.h" |
#include "tmwtypes.h" |
#include "rtw.h" |
extern CAB input_cid[NINPUTCAB]; |
TASK CONTROL_body(void *arg) { |
real_T *p; |
input_cid[0] = cab_create("INPUT0", sizeof(real_T), 2); |
input_cid[1] = cab_create("INPUT1", sizeof(real_T), 2); |
while(1) { |
/* Reserve a message */ |
p = (real_T *)cab_reserve(input_cid[0]); |
/* Save PAR1 data */ |
*p = 1000.0; |
/* Put CAB message */ |
cab_putmes(input_cid[0], p); |
/* Reserve a message */ |
p = (real_T *)cab_reserve(input_cid[1]); |
/* Save PAR2 data */ |
*p = 1000.0; |
/* Put CAB message */ |
cab_putmes(input_cid[1], p); |
task_endcycle(); |
} |
return NULL; |
} |
void activate_control() { |
HARD_TASK_MODEL CONTROL_task; |
PID CONTROL_pid; |
hard_task_default_model(CONTROL_task); |
hard_task_def_mit(CONTROL_task,10000); |
hard_task_def_wcet(CONTROL_task,500); |
hard_task_def_usemath(CONTROL_task); |
hard_task_def_ctrl_jet(CONTROL_task); |
CONTROL_pid = task_create("CONTROL",CONTROL_body,&CONTROL_task,NULL); |
if (CONTROL_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [CONTROL] Task\n"); |
sys_end(); |
} |
task_activate(CONTROL_pid); |
} |
/demos/trunk/rtw/rtw.h |
---|
5,6 → 5,6 |
#define NOUTPUTCAB 10 |
void activate_sensors(); |
void activate_actuators(); |
void activate_control(); |
#endif |
/demos/trunk/rtw/percorso.h |
---|
0,0 → 1,48 |
#ifndef __PERCORSO_H__ |
#define __PERCORSO_H__ |
#define PATH_ELEM_ARC 1 |
#define PATH_ELEM_LINE 2 |
#define CLOCKWISE -1 |
#define COUNTERCLOCK 1 |
typedef struct point { |
double x,y; |
} POINT; |
typedef struct path_elem { |
int type; |
void *data; |
struct path_elem *next; |
} PATH_ELEM; |
typedef struct pd_arc { |
POINT c; |
double r; |
double alpha1; |
double alpha2; |
int dir; |
} PATH_DATA_ARC; |
typedef struct pd_line { |
POINT p1; |
POINT p2; |
double alpha; |
int dir; |
} PATH_DATA_LINE; |
PATH_ELEM *find_closest_elem(POINT *p, double radius); |
double get_distance_from_elem(POINT *p, PATH_ELEM *e); |
double get_angle_from_elem(POINT *p, PATH_ELEM *e); |
/* ritorna: |
1 : curva a sinistra |
-1 : curva a destra |
0 : dritto |
*/ |
int is_curve(PATH_ELEM *e); |
void Init_All_Path(); |
#endif |
/demos/trunk/rtw/makefile |
---|
7,8 → 7,7 |
endif |
include $(BASE)/config/config.mk |
MATLAB_ROOT = /matlab |
MATLAB_RTW_DEMO_DIR = /usr/local/home/giacomo/shark_mat/carrello0_grt_rtw/ |
MATLAB_RTW_DEMO_DIR = ./carrello0_grt_rtw/ |
DEFINE_MODEL = carrello0 |
DEFINE_NUMST = 2 |
22,9 → 21,9 |
PROGS = rtw |
RTW_CFG = "-I$(MATLAB_ROOT)/simulink/include -I$(MATLAB_ROOT)/extern/include -I$(MATLAB_ROOT)/rtw/c/src -I$(MATLAB_ROOT)/rtw/c/libsrc -I$(MATLAB_RTW_DEMO_DIR) -DUSE_RTMODEL -DRT -DMODEL=$(DEFINE_MODEL) -DNUMST=$(DEFINE_NUMST) -DNCSTATES=$(DEFINE_NCSTATES) -DTID01EQ=$(DEFINE_TID01EQ)" |
RTW_CFG = "-I. -I$(MATLAB_RTW_DEMO_DIR) -DUSE_RTMODEL -DRT -DMODEL=$(DEFINE_MODEL) -DNUMST=$(DEFINE_NUMST) -DNCSTATES=$(DEFINE_NCSTATES) -DTID01EQ=$(DEFINE_TID01EQ)" |
RTW_OBJS = $(MATLAB_RTW_DEMO_DIR)/$(FILE_CODE) $(MATLAB_ROOT)/rtw/c/src/$(DEFINE_SOLVER) $(MODULES_rtwlib) |
RTW_OBJS = $(MATLAB_RTW_DEMO_DIR)/$(FILE_CODE) $(DEFINE_SOLVER) $(MODULES_rtwlib) |
ifeq ($(FILE_DATA),$(DEFINE_MODEL)_data.o) |
RTW_OBJS += $(MATLAB_RTW_DEMO_DIR)/$(FILE_DATA) |
33,10 → 32,5 |
include $(BASE)/config/example.mk |
rtw: |
make -f $(SUBMAKE) OTHERLIBS=$(DEFINE_RTWLIB) OTHERINCL=$(RTW_CFG) APP=rtw INIT= OTHEROBJS="initfile.o sensors.o actuators.o rt_sim_shark.o $(RTW_OBJS)" SHARKOPT="__OLDCHAR__" |
make -f $(SUBMAKE) OTHERLIBS=$(DEFINE_RTWLIB) OTHERINCL=$(RTW_CFG) APP=rtw INIT= OTHEROBJS="initfile.o control.o sensors.o percorso.o rt_sim_shark.o $(RTW_OBJS)" SHARKOPT="__OLDCHAR__" |
clean_total: |
rm -rf *.o |
rm -rf $(PROGS) |
rm -rf $(RTW_OBJS) |
/demos/trunk/rtw/rtw.c |
---|
9,6 → 9,7 |
#include "kernel/kern.h" |
#include "modules/cabs.h" |
#include "percorso.h" |
#include "rtw.h" |
368,6 → 369,9 |
TASK RTW_body(void *arg) { |
real_T *input, *p; |
input = (real_T *)rtmGetU(S); |
simulation_run = 1; |
while (!GBLbuf.stopExecutionFlag && |
378,6 → 382,21 |
(boolean_T *)&rtmGetStopRequested(S)); |
if (rtmGetStopRequested(S)) break; |
/* Get PAR0 */ |
if (input_cid[0] != -1) { |
p = (real_T *)cab_getmes(input_cid[0]); |
input[0] = *p; |
cab_unget(input_cid[0], p); |
} |
/* Get PAR1 */ |
if (input_cid[1] != -1) { |
p = (real_T *)cab_getmes(input_cid[1]); |
input[1] = *p; |
cab_unget(input_cid[1], p); |
} |
rt_OneStep(S); |
task_endcycle(); |
395,124 → 414,94 |
} |
TASK INPUT_body(void *input_port) { |
TASK OUTPUT_body(void *arg) { |
real_T *input, *p; |
int in = (int)(input_port); |
real_T *output, *p; |
real_T th; |
if (in >= NINPUTCAB) return NULL; |
POINT pos; |
PATH_ELEM *e; |
real_T distance = 0; |
real_T angle = 0; |
real_T curve = 0; |
input = (real_T *)rtmGetU(S); |
output_cid[0] = cab_create("DISTANCE", sizeof(real_T), 2); |
output_cid[1] = cab_create("ANGLE", sizeof(real_T), 2); |
output_cid[2] = cab_create("CURVE", sizeof(real_T), 2); |
while(1) { |
output = (real_T *)rtmGetY(S); |
/* Get CAB message */ |
p = (real_T *)cab_getmes(input_cid[in]); |
while(1) { |
/* Set INPUT port */ |
input[in] = *p; |
pos.x = output[0]; |
pos.y = output[1]; |
th = output[2]; |
e = find_closest_elem(&pos, 1.0); |
if (e != 0) { |
distance = get_distance_from_elem(&pos, e); |
angle = th - get_angle_from_elem(&pos,e); |
curve = is_curve(e); |
} |
/* Send Distance */ |
p = (real_T *)cab_reserve(output_cid[0]); |
*p = distance; |
cab_putmes(output_cid[0], p); |
/* Release CAB message */ |
cab_unget(input_cid[in], p); |
/* Send Angle */ |
p = (real_T *)cab_reserve(output_cid[1]); |
*p = angle; |
cab_putmes(output_cid[1], p); |
task_endcycle(); |
/* Send Curve */ |
p = (real_T *)cab_reserve(output_cid[2]); |
*p = curve; |
cab_putmes(output_cid[2], p); |
} |
return NULL; |
task_endcycle(); |
} |
return NULL; |
} |
TASK OUTPUT_body(void *output_port) { |
void Init_Rtw() { |
real_T *output, *p; |
int out = (int)(output_port); |
char cname[20]; |
if (out >= NOUTPUTCAB) return NULL; |
sprintf(cname,"OUTPUT%d",out); |
output_cid[out] = cab_create(cname, sizeof(real_T), 2); |
HARD_TASK_MODEL RTW_task,OUTPUT_task; |
PID RTW_pid,OUTPUT_pid; |
output = (real_T *)rtmGetY(S); |
int i; |
while(1) { |
/* Reserve a message */ |
p = (real_T *)cab_reserve(output_cid[out]); |
/* Save data */ |
*p = output[out]; |
/* Put CAB message */ |
cab_putmes(output_cid[out], p); |
task_endcycle(); |
} |
return NULL; |
} |
int main() { |
HARD_TASK_MODEL RTW_task,INPUT_task,OUTPUT_task; |
PID RTW_pid,INPUT_pid,OUTPUT_pid; |
int i; |
int RTW_period; |
int RTW_wcet; |
int INPUT_period; |
int INPUT_wcet; |
int OUTPUT_period; |
int OUTPUT_wcet; |
Init_RealTime_Workshop(); |
Init_All_Path(); |
for (i=0;i<NINPUTCAB;i++) input_cid[i] = -1; |
for (i=0;i<NOUTPUTCAB;i++) output_cid[i] = -1; |
RTW_period =(int)(rtmGetStepSize(S) * 1000000.0); |
RTW_wcet = (int)(rtmGetStepSize(S) * 1000000.0 * 0.45); |
INPUT_period = (int)(10000.0); |
INPUT_wcet = (int)(10000.0 * 0.10); |
OUTPUT_period = (int)(10000.0); |
OUTPUT_wcet = (int)(10000.0 * 0.10); |
cprintf("Task Setup\n"); |
cprintf("RTW (P %8d W %8d)\n",RTW_period,RTW_wcet); |
cprintf("INPUT (P %8d W %8d)\n",INPUT_period,INPUT_wcet); |
cprintf("OUTPUT (P %8d W %8d)\n",OUTPUT_period,OUTPUT_wcet); |
hard_task_default_model(RTW_task); |
hard_task_def_mit(RTW_task,RTW_period); |
hard_task_def_wcet(RTW_task,RTW_wcet); |
hard_task_def_usemath(RTW_task); |
hard_task_def_ctrl_jet(RTW_task); |
RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL); |
if (RTW_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n"); |
sys_end(); |
} |
hard_task_default_model(INPUT_task); |
hard_task_def_mit(INPUT_task,INPUT_period); |
hard_task_def_wcet(INPUT_task,INPUT_wcet); |
/* Set input port number */ |
hard_task_def_arg(INPUT_task,(void *)(0)); |
hard_task_def_usemath(INPUT_task); |
hard_task_def_ctrl_jet(INPUT_task); |
INPUT_pid = task_create("INPUT0",INPUT_body,&INPUT_task,NULL); |
if (INPUT_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [INPUT0] Task\n"); |
cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n"); |
sys_end(); |
} |
519,23 → 508,26 |
hard_task_default_model(OUTPUT_task); |
hard_task_def_mit(OUTPUT_task,OUTPUT_period); |
hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet); |
/* Set output port number */ |
hard_task_def_arg(OUTPUT_task,(void *)(0)); |
hard_task_def_usemath(OUTPUT_task); |
hard_task_def_ctrl_jet(OUTPUT_task); |
OUTPUT_pid = task_create("OUTPUT0",OUTPUT_body,&OUTPUT_task,NULL); |
OUTPUT_pid = task_create("OUTPUT",OUTPUT_body,&OUTPUT_task,NULL); |
if (OUTPUT_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [OUTPUT0] Task\n"); |
cprintf("Error: Cannot create RealTime Workshop [OUTPUT] Task\n"); |
sys_end(); |
} |
task_activate(INPUT_pid); |
task_activate(RTW_pid); |
task_activate(OUTPUT_pid); |
task_activate(OUTPUT_pid); |
} |
int main() { |
Init_Rtw(); |
activate_sensors(); |
activate_actuators(); |
activate_control(); |
while(simulation_run); |
/demos/trunk/rtw/sensors.c |
---|
5,30 → 5,28 |
#include "rtw.h" |
extern CAB input_cid[NINPUTCAB]; |
extern CAB output_cid[NOUTPUTCAB]; |
TASK SENSOR_body(void *input_port) { |
TASK SENSOR_body(void *output_port) { |
real_T *p; |
int in = (int)(input_port); |
char cname[20]; |
real_T *p, value; |
int out = (int)(output_port); |
if (in >= NINPUTCAB) return NULL; |
if (out >= NOUTPUTCAB) return NULL; |
sprintf(cname,"INPUT%d",in); |
input_cid[in] = cab_create(cname, sizeof(real_T), 2); |
while(1) { |
/* Get CAB message */ |
p = (real_T *)cab_getmes(output_cid[out]); |
/* Reserve a message */ |
p = (real_T *)cab_reserve(input_cid[in]); |
/* Set value */ |
value = *p; |
/* Save data */ |
*p = 1000.0; |
/* Put CAB message */ |
cab_putmes(input_cid[in], p); |
/* Release CAB message */ |
cab_unget(output_cid[out], p); |
cprintf("Value %d = %lf\n",out,value); |
task_endcycle(); |
} |
40,24 → 38,39 |
void activate_sensors() { |
HARD_TASK_MODEL SENSOR_task; |
PID SENSOR_pid; |
PID DISTANCE_pid,ANGLE_pid,CURVE_pid; |
hard_task_default_model(SENSOR_task); |
hard_task_def_mit(SENSOR_task,10000); |
hard_task_def_wcet(SENSOR_task,1000); |
/* Set sensor port number */ |
hard_task_def_arg(SENSOR_task,(void *)(0)); |
hard_task_def_mit(SENSOR_task,60000); |
hard_task_def_wcet(SENSOR_task,4000); |
hard_task_def_usemath(SENSOR_task); |
hard_task_def_ctrl_jet(SENSOR_task); |
SENSOR_pid = task_create("SENSOR0",SENSOR_body,&SENSOR_task,NULL); |
if (SENSOR_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [SENSOR0] Task\n"); |
hard_task_def_arg(SENSOR_task,(void *)(0)); |
DISTANCE_pid = task_create("DISTANCE",SENSOR_body,&SENSOR_task,NULL); |
if (DISTANCE_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n"); |
sys_end(); |
} |
task_activate(SENSOR_pid); |
hard_task_def_arg(SENSOR_task,(void *)(1)); |
ANGLE_pid = task_create("ANGLE",SENSOR_body,&SENSOR_task,NULL); |
if (DISTANCE_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n"); |
sys_end(); |
} |
hard_task_def_arg(SENSOR_task,(void *)(2)); |
CURVE_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL); |
if (CURVE_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [CURVE] Task\n"); |
sys_end(); |
} |
task_activate(DISTANCE_pid); |
task_activate(ANGLE_pid); |
task_activate(CURVE_pid); |
} |
/demos/trunk/rtw/percorso.c |
---|
0,0 → 1,288 |
#include <stdio.h> |
#include <math.h> |
#include "percorso.h" |
int nelem; |
PATH_ELEM * head; |
/* void print_elem(PATH_ELEM *elem) */ |
/* { */ |
/* if (elem->type == PATH_ELEM_ARC) { */ |
/* PATH_DATA_ARC *arc= (PATH_DATA_ARC *) elem->data; */ |
/* printf("Center: x =%3.3lf y =%3.3lf\n", arc->c.x, arc->c.y); */ |
/* printf("angles: a1=%3.3lf a2=%3.3lf\n", arc->alpha1, arc->alpha2); */ |
/* printf("Radius: r =%3.3lf\n", arc->r); */ |
/* } */ |
/* else if (elem->type == PATH_ELEM_LINE) { */ |
/* PATH_DATA_LINE *line = (PATH_DATA_LINE *) elem->data; */ |
/* printf("x1 = %3.3lf y1 = %3.3lf x2 = %3.3lf y2 = %3.3lf\n", */ |
/* line->p1.x, line->p1.y, line->p2.x, line->p2.y); */ |
/* } */ |
/* } */ |
/* void print_point(POINT *p) */ |
/* { */ |
/* printf("p.x = %3.3lf p.y = %3.3lf\n", p->x, p->y); */ |
/* } */ |
/*-----------------------------------------------------------------*/ |
inline double point_dist(POINT *p1, POINT *p2) |
{ |
return sqrt((p1->x - p2->x)*(p1->x - p2->x) + |
(p1->y - p2->y)*(p1->y - p2->y)); |
} |
double get_angle_from_arc(POINT *p1, PATH_DATA_ARC *arc) |
{ |
return acos((p1->x - arc->c.x) / point_dist(p1, &(arc->c))); |
} |
int inside_arc(PATH_DATA_ARC *arc, POINT *p) |
{ |
double alpha = get_angle_from_arc(p, arc); |
if (p->y < arc->c.y) alpha = 2*M_PI - alpha; |
if (alpha <= arc->alpha2 && alpha >= arc->alpha1) return 1; |
else return 0; |
} |
double get_distance_from_arc(POINT *p1, PATH_DATA_ARC *arc) |
{ |
return point_dist (p1,&(arc->c)) - arc->r; |
} |
double get_distance_from_line(POINT *p, PATH_DATA_LINE *l) |
{ |
return ((l->p1.y - l->p2.y) * p->x + |
(l->p2.x - l->p1.x) * p->y + |
(l->p1.x * l->p2.y - l->p2.x * l->p1.y)) / |
point_dist(&(l->p1), &(l->p2)); |
} |
inline double get_t(POINT *p, PATH_DATA_LINE *line) |
{ |
double t = ( (line->p2.x - line->p1.x) * (p->x - line->p1.x) + |
(line->p2.y - line->p1.y) * (p->y - line->p1.y)) / |
(point_dist(&(line->p1), &(line->p2)) * |
point_dist(&(line->p1), &(line->p2))); |
return t; |
} |
double get_angle_from_line(POINT *p, PATH_DATA_LINE *l) |
{ |
return atan2(l->p1.y - l->p2.y, l->p1.x - l->p2.x); |
} |
int includes_elem(POINT *p, PATH_ELEM *e, double radius) |
{ |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = (PATH_DATA_ARC *) e->data; |
if (get_distance_from_arc(p, arc) <= radius && inside_arc(arc,p)) |
return 1; |
else return 0; |
} |
else if (e->type==PATH_ELEM_LINE) { |
PATH_DATA_LINE *line = (PATH_DATA_LINE *) e->data; |
double t = get_t(p, line); |
double d = get_distance_from_line(p, line); |
if (t >= 0 && t <= 1 && d < radius) return 1; |
else return 0; |
} |
return 0; |
} |
void init_arc(PATH_DATA_ARC *arc, double cx, double cy, double r, |
double a1, double a2, int dir) |
{ |
arc->c.x = cx; |
arc->c.y = cy; |
arc->r = r; |
arc->alpha1 = a1; |
arc->alpha2 = a2; |
arc->dir = dir; |
} |
void init_line(PATH_DATA_LINE *l, double x1, double y1, double x2, |
double y2, int dir) |
{ |
//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", x1,y1,x2,y2); |
l->p1.x = x1; |
l->p2.x = x2; |
l->p1.y = y1; |
l->p2.y = y2; |
l->dir = dir; |
//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", l->p1.x,l->p1.y,l->p2.x,l->p2.y); |
} |
/*---------------------------------------------------------------*/ |
PATH_ELEM *find_closest_elem(POINT *p, double radius) |
{ |
PATH_ELEM *cur = head; |
int i = 0; |
for (i=0; i<nelem; i++) { |
//printf("Searching element %d \n", i); |
//print_elem(cur); |
if (includes_elem(p, cur, radius)) return cur; |
else cur = cur->next; |
} |
return 0; |
} |
double get_distance_from_elem(POINT *p, PATH_ELEM *e) |
{ |
double d = 0; |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = e->data; |
d = get_distance_from_arc(p, arc); |
} |
else if (e->type == PATH_ELEM_LINE) { |
PATH_DATA_LINE *line = e->data; |
d = get_distance_from_line(p, line); |
} |
return d; |
} |
double get_angle_from_elem(POINT *p, PATH_ELEM *e) |
{ |
double alpha = 0; |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = e->data; |
alpha = get_angle_from_arc(p, arc) - M_PI/2; |
} |
else if (e->type == PATH_ELEM_LINE) { |
PATH_DATA_LINE *line = e->data; |
alpha = get_angle_from_line(p, line); |
} |
return alpha; |
} |
/* ritorna: |
1 : curva a sinistra |
-1 : curva a destra |
0 : dritto */ |
int is_curve(PATH_ELEM *e) |
{ |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = e->data; |
return arc->dir; |
} |
else return 0; |
} |
/*---------------------------------------------------------------*/ |
PATH_ELEM elems[11]; |
PATH_DATA_ARC arcs[6]; |
PATH_DATA_LINE lines[5]; |
void Init_All_Path() |
{ |
init_line(&lines[0], 0,0,3,0, -1); |
elems[0].type = PATH_ELEM_LINE; |
elems[0].data = &lines[0]; |
elems[0].next = &elems[1]; |
init_arc(&arcs[0], 3,1, 1, -M_PI/2,0, 1); |
elems[1].type = PATH_ELEM_ARC; |
elems[1].data = &arcs[0]; |
elems[1].next = &elems[2]; |
init_line(&lines[1], 4,1,4,7, -1); |
elems[2].type = PATH_ELEM_LINE; |
elems[2].data = &lines[1]; |
elems[2].next = &elems[3]; |
init_arc(&arcs[1], 2,7, 2, 0, M_PI/2, 1); |
elems[3].type = PATH_ELEM_ARC; |
elems[3].data = &arcs[1]; |
elems[3].next = &elems[4]; |
init_line(&lines[2], 2,9,-1,9, 1); |
elems[4].type = PATH_ELEM_LINE; |
elems[4].data = &lines[2]; |
elems[4].next = &elems[5]; |
init_arc(&arcs[2], -2,8, 1, M_PI/2, M_PI, 1); |
elems[5].type = PATH_ELEM_ARC; |
elems[5].data = &arcs[2]; |
elems[5].next = &elems[6]; |
init_line(&lines[3], -3,8,-3,5, 1); |
elems[6].type = PATH_ELEM_LINE; |
elems[6].data = &lines[3]; |
elems[6].next = &elems[7]; |
init_arc(&arcs[3], -2,5, 1, M_PI, 3*M_PI/2, 1); |
elems[7].type = PATH_ELEM_ARC; |
elems[7].data = &arcs[3]; |
elems[7].next = &elems[8]; |
init_arc(&arcs[4], -2,3, 1, 0, M_PI/2, -1); |
elems[8].type = PATH_ELEM_ARC; |
elems[8].data = &arcs[4]; |
elems[8].next = &elems[9]; |
init_line(&lines[4], -1,3,-1,1, 1); |
elems[9].type = PATH_ELEM_LINE; |
elems[9].data = &lines[4]; |
elems[9].next = &elems[10]; |
init_arc(&arcs[5], 0,1, 1, M_PI, 3*M_PI/2, -1); |
elems[10].type = PATH_ELEM_ARC; |
elems[10].data = &arcs[5]; |
elems[10].next = &elems[0]; |
head = &elems[0]; |
nelem = 11; |
} |
/* int main(void) */ |
/* { */ |
/* double d; */ |
/* double alpha; */ |
/* POINT p; */ |
/* init_all(); */ |
/* p.x = -1.75; */ |
/* p.y = 4.25; */ |
/* PATH_ELEM *e = find_closest_elem(&p, head, 1); */ |
/* if (e != 0) { */ |
/* d = get_distance_from_elem(&p, e); */ |
/* alpha = get_angle_from_elem(&p, e); */ |
/* print_elem(e); */ |
/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */ |
/* printf("direzione %d\n", is_curve(e)); */ |
/* } */ |
/* else */ |
/* printf("Not found!\n"); */ |
/* p.x = 4.75; */ |
/* p.y = 7.1; */ |
/* e = find_closest_elem(&p, head, 1); */ |
/* if (e != 0) { */ |
/* d = get_distance_from_elem(&p, e); */ |
/* alpha = get_angle_from_elem(&p, e); */ |
/* print_elem(e); */ |
/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */ |
/* printf("direzione %d\n", is_curve(e)); */ |
/* } */ |
/* else */ |
/* printf("Not found!\n"); */ |
/* } */ |