WebSVN - shark - Blame - Rev 1419 - /demos/trunk/rtw/percorso.c

Rev	Author	Line No.	Line
1419	giacomo	1	#include <stdio.h>
		2	#include <math.h>
		3	#include "percorso.h"
		4
		5	int nelem;
		6	PATH_ELEM * head;
		7
		8	/* void print_elem(PATH_ELEM elem) /
		9	/* { */
		10	/* if (elem->type == PATH_ELEM_ARC) { */
		11	/* PATH_DATA_ARC arc= (PATH_DATA_ARC ) elem->data; */
		12	/* printf("Center: x =%3.3lf y =%3.3lf\n", arc->c.x, arc->c.y); */
		13	/* printf("angles: a1=%3.3lf a2=%3.3lf\n", arc->alpha1, arc->alpha2); */
		14	/* printf("Radius: r =%3.3lf\n", arc->r); */
		15	/* } */
		16	/* else if (elem->type == PATH_ELEM_LINE) { */
		17	/* PATH_DATA_LINE line = (PATH_DATA_LINE ) elem->data; */
		18	/* printf("x1 = %3.3lf y1 = %3.3lf x2 = %3.3lf y2 = %3.3lf\n", */
		19	/* line->p1.x, line->p1.y, line->p2.x, line->p2.y); */
		20	/* } */
		21	/* } */
		22
		23	/* void print_point(POINT p) /
		24	/* { */
		25	/* printf("p.x = %3.3lf p.y = %3.3lf\n", p->x, p->y); */
		26	/* } */
		27
		28
		29	/-----------------------------------------------------------------/
		30
		31	inline double point_dist(POINT p1, POINT p2)
		32	{
		33	return sqrt((p1->x - p2->x)*(p1->x - p2->x) +
		34	(p1->y - p2->y)*(p1->y - p2->y));
		35	}
		36
		37	double get_angle_from_arc(POINT p1, PATH_DATA_ARC arc)
		38	{
		39	return acos((p1->x - arc->c.x) / point_dist(p1, &(arc->c)));
		40	}
		41
		42	int inside_arc(PATH_DATA_ARC arc, POINT p)
		43	{
		44	double alpha = get_angle_from_arc(p, arc);
		45	if (p->y < arc->c.y) alpha = 2*M_PI - alpha;
		46
		47	if (alpha <= arc->alpha2 && alpha >= arc->alpha1) return 1;
		48	else return 0;
		49	}
		50
		51	double get_distance_from_arc(POINT p1, PATH_DATA_ARC arc)
		52	{
		53	return point_dist (p1,&(arc->c)) - arc->r;
		54	}
		55
		56
		57	double get_distance_from_line(POINT p, PATH_DATA_LINE l)
		58	{
		59	return ((l->p1.y - l->p2.y) * p->x +
		60	(l->p2.x - l->p1.x) * p->y +
		61	(l->p1.x * l->p2.y - l->p2.x * l->p1.y)) /
		62	point_dist(&(l->p1), &(l->p2));
		63	}
		64
		65
		66	inline double get_t(POINT p, PATH_DATA_LINE line)
		67	{
		68	double t = ( (line->p2.x - line->p1.x) * (p->x - line->p1.x) +
		69	(line->p2.y - line->p1.y) * (p->y - line->p1.y)) /
		70	(point_dist(&(line->p1), &(line->p2)) *
		71	point_dist(&(line->p1), &(line->p2)));
		72	return t;
		73	}
		74
		75
		76	double get_angle_from_line(POINT p, PATH_DATA_LINE l)
		77	{
		78	return atan2(l->p1.y - l->p2.y, l->p1.x - l->p2.x);
		79	}
		80
		81	int includes_elem(POINT p, PATH_ELEM e, double radius)
		82	{
		83	if (e->type == PATH_ELEM_ARC) {
		84	PATH_DATA_ARC arc = (PATH_DATA_ARC ) e->data;
		85	if (get_distance_from_arc(p, arc) <= radius && inside_arc(arc,p))
		86	return 1;
		87	else return 0;
		88	}
		89	else if (e->type==PATH_ELEM_LINE) {
		90	PATH_DATA_LINE line = (PATH_DATA_LINE ) e->data;
		91	double t = get_t(p, line);
		92	double d = get_distance_from_line(p, line);
		93	if (t >= 0 && t <= 1 && d < radius) return 1;
		94	else return 0;
		95	}
		96	return 0;
		97	}
		98
		99	void init_arc(PATH_DATA_ARC *arc, double cx, double cy, double r,
		100	double a1, double a2, int dir)
		101	{
		102	arc->c.x = cx;
		103	arc->c.y = cy;
		104	arc->r = r;
		105	arc->alpha1 = a1;
		106	arc->alpha2 = a2;
		107	arc->dir = dir;
		108	}
		109
		110	void init_line(PATH_DATA_LINE *l, double x1, double y1, double x2,
		111	double y2, int dir)
		112	{
		113	//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", x1,y1,x2,y2);
		114	l->p1.x = x1;
		115	l->p2.x = x2;
		116	l->p1.y = y1;
		117	l->p2.y = y2;
		118	l->dir = dir;
		119	//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", l->p1.x,l->p1.y,l->p2.x,l->p2.y);
		120	}
		121
		122	/---------------------------------------------------------------/
		123
		124	PATH_ELEM find_closest_elem(POINT p, double radius)
		125	{
		126	PATH_ELEM *cur = head;
		127	int i = 0;
		128
		129	for (i=0; i<nelem; i++) {
		130	//printf("Searching element %d \n", i);
		131	//print_elem(cur);
		132	if (includes_elem(p, cur, radius)) return cur;
		133	else cur = cur->next;
		134	}
		135	return 0;
		136	}
		137
		138	double get_distance_from_elem(POINT p, PATH_ELEM e)
		139	{
		140	double d = 0;
		141	if (e->type == PATH_ELEM_ARC) {
		142	PATH_DATA_ARC *arc = e->data;
		143	d = get_distance_from_arc(p, arc);
		144	}
		145	else if (e->type == PATH_ELEM_LINE) {
		146	PATH_DATA_LINE *line = e->data;
		147	d = get_distance_from_line(p, line);
		148	}
		149	return d;
		150	}
		151
		152	double get_angle_from_elem(POINT p, PATH_ELEM e)
		153	{
		154	double alpha = 0;
		155	if (e->type == PATH_ELEM_ARC) {
		156	PATH_DATA_ARC *arc = e->data;
		157	alpha = get_angle_from_arc(p, arc) - M_PI/2;
		158	}
		159	else if (e->type == PATH_ELEM_LINE) {
		160	PATH_DATA_LINE *line = e->data;
		161	alpha = get_angle_from_line(p, line);
		162	}
		163	return alpha;
		164	}
		165
		166
		167	/* ritorna:
		168	1 : curva a sinistra
		169	-1 : curva a destra
		170
		171	int is_curve(PATH_ELEM *e)
		172	{
		173	if (e->type == PATH_ELEM_ARC) {
		174	PATH_DATA_ARC *arc = e->data;
		175	return arc->dir;
		176	}
		177	else return 0;
		178	}
		179
		180	/---------------------------------------------------------------/
		181
		182	PATH_ELEM elems[11];
		183	PATH_DATA_ARC arcs[6];
		184	PATH_DATA_LINE lines[5];
		185
		186	void Init_All_Path()
		187	{
		188	init_line(&lines[0], 0,0,3,0, -1);
		189	elems[0].type = PATH_ELEM_LINE;
		190	elems[0].data = &lines[0];
		191	elems[0].next = &elems[1];
		192
		193	init_arc(&arcs[0], 3,1, 1, -M_PI/2,0, 1);
		194	elems[1].type = PATH_ELEM_ARC;
		195	elems[1].data = &arcs[0];
		196	elems[1].next = &elems[2];
		197
		198	init_line(&lines[1], 4,1,4,7, -1);
		199	elems[2].type = PATH_ELEM_LINE;
		200	elems[2].data = &lines[1];
		201	elems[2].next = &elems[3];
		202
		203	init_arc(&arcs[1], 2,7, 2, 0, M_PI/2, 1);
		204	elems[3].type = PATH_ELEM_ARC;
		205	elems[3].data = &arcs[1];
		206	elems[3].next = &elems[4];
		207
		208	init_line(&lines[2], 2,9,-1,9, 1);
		209	elems[4].type = PATH_ELEM_LINE;
		210	elems[4].data = &lines[2];
		211	elems[4].next = &elems[5];
		212
		213	init_arc(&arcs[2], -2,8, 1, M_PI/2, M_PI, 1);
		214	elems[5].type = PATH_ELEM_ARC;
		215	elems[5].data = &arcs[2];
		216	elems[5].next = &elems[6];
		217
		218	init_line(&lines[3], -3,8,-3,5, 1);
		219	elems[6].type = PATH_ELEM_LINE;
		220	elems[6].data = &lines[3];
		221	elems[6].next = &elems[7];
		222
		223	init_arc(&arcs[3], -2,5, 1, M_PI, 3*M_PI/2, 1);
		224	elems[7].type = PATH_ELEM_ARC;
		225	elems[7].data = &arcs[3];
		226	elems[7].next = &elems[8];
		227
		228	init_arc(&arcs[4], -2,3, 1, 0, M_PI/2, -1);
		229	elems[8].type = PATH_ELEM_ARC;
		230	elems[8].data = &arcs[4];
		231	elems[8].next = &elems[9];
		232
		233	init_line(&lines[4], -1,3,-1,1, 1);
		234	elems[9].type = PATH_ELEM_LINE;
		235	elems[9].data = &lines[4];
		236	elems[9].next = &elems[10];
		237
		238	init_arc(&arcs[5], 0,1, 1, M_PI, 3*M_PI/2, -1);
		239	elems[10].type = PATH_ELEM_ARC;
		240	elems[10].data = &arcs[5];
		241	elems[10].next = &elems[0];
		242
		243	head = &elems[0];
		244	nelem = 11;
		245	}
		246
		247
		248	/* int main(void) */
		249	/* { */
		250	/* double d; */
		251	/* double alpha; */
		252	/* POINT p; */
		253
		254	/* init_all(); */
		255
		256	/* p.x = -1.75; */
		257	/* p.y = 4.25; */
		258
		259	/* PATH_ELEM e = find_closest_elem(&p, head, 1); /
		260
		261	/* if (e != 0) { */
		262	/* d = get_distance_from_elem(&p, e); */
		263	/* alpha = get_angle_from_elem(&p, e); */
		264	/* print_elem(e); */
		265	/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */
		266	/* printf("direzione %d\n", is_curve(e)); */
		267	/* } */
		268	/* else */
		269	/* printf("Not found!\n"); */
		270
		271
		272	/* p.x = 4.75; */
		273	/* p.y = 7.1; */
		274
		275	/* e = find_closest_elem(&p, head, 1); */
		276
		277	/* if (e != 0) { */
		278	/* d = get_distance_from_elem(&p, e); */
		279	/* alpha = get_angle_from_elem(&p, e); */
		280	/* print_elem(e); */
		281	/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */
		282	/* printf("direzione %d\n", is_curve(e)); */
		283	/* } */
		284	/* else */
		285	/* printf("Not found!\n"); */
		286
		287
		288	/* } */

Subversion Repositories shark

(root)/demos/trunk/rtw/percorso.c - Rev 1419