modules/base/math/geometry/test/main.c

   1 #include <inttypes.h>
   2 #include <stdint.h>
   3 #include <math.h>
   4
   5 #include <vect_base.h>
   6 #include <lines.h>
   7 #include <polygon.h>
   8
   9 #include <stdio.h>
  10
  11 #define EPSILON 0.000001
  12
  13 int main(void)
  14 {
  15         vect_t v, w;
  16         int32_t ps;
  17         float n1, n2;
  18         float a;
  19
  20         point_t p1, p2, p3, p4, p5, p6, p7, p;
  21         point_t p8, p9, p10, p11;
  22         line_t l1, l2, l3, l4;
  23         uint8_t ret;
  24
  25         poly_t poly1, poly2;
  26
  27         point_t poly_pts1[4];
  28         point_t poly_pts2[5];
  29
  30         polygon_set_boundingbox(25, 25, 275, 185);
  31
  32         /* basic vect test */
  33         v.x = 1;
  34         v.y = 0;
  35
  36         vect_rot_trigo(&v);
  37         if (v.x != 0 || v.y  != 1)
  38                 printf("error rot rigo ok1\r\n");
  39
  40         vect_rot_trigo(&v);
  41         vect_rot_trigo(&v);
  42         vect_rot_trigo(&v);
  43
  44         if (v.x != 1 || v.y != 0)
  45                 printf("error rot trigo ok2\r\n");
  46
  47         vect_rot_retro(&v);
  48         if (v.x != 0 || v.y  != -1)
  49                 printf("error rot rigo ok1\r\n");
  50
  51         vect_rot_retro(&v);
  52         vect_rot_retro(&v);
  53         vect_rot_retro(&v);
  54
  55         if (v.x != 1 || v.y  != 0)
  56                 printf("error rot retro ok2\r\n");
  57
  58
  59
  60         w.x = 2;
  61         w.y = 2;
  62         ps = vect_pscal(&v, &w);
  63
  64         n1 = vect_norm(&v);
  65         n2 = vect_norm(&w);
  66         if (fabs(n1-1.)>EPSILON || fabs(n2-2.828427)>EPSILON)
  67                 printf("error in norm\r\n");
  68
  69         a = acos((float)ps/(n1*n2))*180./M_PI;
  70
  71         if (fabs(a-45.)>EPSILON)
  72                 printf("error in norm/pscal\r\n");
  73
  74         a = vect_get_angle(&v, &w)*180./M_PI;
  75         if (fabs(a-45.)>EPSILON)
  76                 printf("error in get angle\r\n");
  77
  78         /* basic lines tests */
  79         p1.x = 0;
  80         p1.y = 0;
  81
  82         p2.x = 0;
  83         p2.y = 10;
  84
  85         pts2line(&p1, &p2, &l1);
  86         printf("%2.2f %2.2f %2.2f\r\n", l1.a, l1.b, l1.c);
  87
  88
  89         p3.x = 10;
  90         p3.y = 0;
  91
  92         pts2line(&p1, &p3, &l2);
  93         printf("%2.2f %2.2f %2.2f\r\n", l2.a, l2.b, l2.c);
  94
  95
  96         p4.x = 10;
  97         p4.y = 10;
  98
  99         p5.x = 20;
 100         p5.y = 20;
 101
 102         pts2line(&p4, &p5, &l3);
 103         printf("%2.2f %2.2f %2.2f\r\n", l3.a, l3.b, l3.c);
 104
 105         p6.x = 30;
 106         p6.y = 0;
 107
 108         p7.x = 0;
 109         p7.y = 30;
 110
 111         pts2line(&p6, &p7, &l4);
 112         printf("%2.2f %2.2f %2.2f\r\n", l4.a, l4.b, l4.c);
 113
 114
 115         intersect_line(&l1, &l2, &p);
 116         printf("* %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
 117
 118         intersect_line(&l1, &l4, &p);
 119         printf("* %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
 120
 121         intersect_line(&l2, &l4, &p);
 122         printf("* %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
 123
 124         intersect_line(&l3, &l4, &p);
 125         printf("* %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
 126
 127         ret = intersect_segment(&p1, &p2, &p4, &p5, &p);
 128         printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
 129         if (ret != 0)
 130                 printf("error in segment cros\r\n");
 131
 132         ret = intersect_segment(&p4, &p5, &p6, &p7, &p);
 133         printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
 134         if (ret != 1)
 135                 printf("error in segment cros\r\n");
 136
 137         ret = intersect_segment(&p1, &p2, &p1, &p3, &p);
 138         printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
 139         if (ret != 2)
 140                 printf("error in segment cros\r\n");
 141
 142         p8.x = 105;
 143         p8.y = 60;
 144         p9.x = 200;
 145         p9.y = 150;
 146         p10.x = 195;
 147         p10.y = 150;
 148         p11.x = 195;
 149         p11.y = 60;
 150         ret = intersect_segment(&p8, &p9, &p10, &p11, &p);
 151         printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
 152         if (ret != 1)
 153                 printf("error in segment cros\r\n");
 154
 155
 156
 157
 158         p6.x = 30;
 159         p6.y = 0;
 160
 161         p7.x = 0;
 162         p7.y = 30;
 163
 164         p8.x = 10;
 165         p8.y = 10;
 166
 167         pts2line(&p6, &p7, &l3);
 168         proj_pt_line(&p8, &l3, &p);
 169         printf("proj: %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
 170         if (p.x != 15 || p.y != 15)
 171                 printf("error in proj 1\r\n");
 172
 173
 174         p6.x = 0;
 175         p6.y = 0;
 176
 177         p7.x = 0;
 178         p7.y = 30;
 179
 180         p8.x = 10;
 181         p8.y = 10;
 182
 183         pts2line(&p6, &p7, &l3);
 184         proj_pt_line(&p8, &l3, &p);
 185         printf("proj: %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
 186         if (p.x != 0 || p.y != 10)
 187                 printf("error in proj 2\r\n");
 188
 189
 190         p6.x = 30;
 191         p6.y = 0;
 192
 193         p7.x = 0;
 194         p7.y = 0;
 195
 196         p8.x = 10;
 197         p8.y = 10;
 198
 199         pts2line(&p6, &p7, &l3);
 200         proj_pt_line(&p8, &l3, &p);
 201         printf("proj: %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
 202         if (p.x != 10 || p.y != 0)
 203                 printf("error in proj 3\r\n");
 204
 205
 206         p1.x = 0;
 207         p1.y = 10;
 208
 209         p2.x = 20;
 210         p2.y = 20;
 211
 212         pts2line(&p1, &p2, &l1);
 213         printf("%2.2f %2.2f %2.2f\r\n", l1.a, l1.b, l1.c);
 214
 215         p2.x = 0;
 216         p2.y = 10;
 217
 218         p1.x = 10;
 219         p1.y = -10;
 220
 221         pts2line(&p1, &p2, &l1);
 222         printf("%2.2f %2.2f %2.2f\r\n", l1.a, l1.b, l1.c);
 223
 224
 225         /* basic poly tests */
 226
 227         poly1.pts = poly_pts1;
 228         poly1.l = 4;
 229
 230         poly2.pts = poly_pts2;
 231         poly2.l = 5;
 232
 233
 234         poly1.pts[0].x = 0;
 235         poly1.pts[0].y = 0;
 236
 237         poly1.pts[0].x = 1;
 238         poly1.pts[0].y = 0;
 239
 240         poly1.pts[0].x = 1;
 241         poly1.pts[0].y = 1;
 242
 243         poly1.pts[0].x = 0;
 244         poly1.pts[0].y = 1;
 245
 246
 247
 248         poly1.pts[0].x = 0;
 249         poly1.pts[0].y = 0;
 250
 251         poly1.pts[1].x = 10;
 252         poly1.pts[1].y = 0;
 253
 254         poly1.pts[2].x = 20;
 255         poly1.pts[2].y = 20;
 256
 257         poly1.pts[3].x = 0;
 258         poly1.pts[3].y = 10;
 259
 260
 261         ret =  is_in_poly(&p6, &poly1);
 262         printf("%d\r\n", ret);
 263         if (ret!=0)
 264                 printf("error in is in poly\r\n");
 265
 266
 267
 268         ret =  is_in_poly(&p4, &poly1);
 269         printf("%d\r\n", ret);
 270         if (ret!=1)
 271                 printf("error in is in poly\r\n");
 272
 273
 274         ret =  is_in_poly(&p1, &poly1);
 275         printf("%d\r\n", ret);
 276         if (ret!=2)
 277                 printf("error in is in poly\r\n");
 278
 279
 280
 281
 282         return 0;
 283
 284
 285 }