00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifdef HAVE_CONFIG_H
00029 #include <config.h>
00030 #endif
00031
00032 #include <fors_pattern.h>
00033
00034 #include <fors_double.h>
00035 #include <fors_utils.h>
00036
00037 #include <cpl.h>
00038
00039 #include <math.h>
00040
00041 struct _fors_pattern
00042 {
00043 double ratsq;
00044 double dratsq;
00045
00046 double theta;
00047 double dtheta;
00048 const fors_point *ref, *min, *max;
00049
00050 };
00051
00052 #undef cleanup
00053 #define cleanup
00054
00069 fors_pattern *fors_pattern_new(const fors_point *ref,
00070 const fors_point *p1,
00071 const fors_point *p2,
00072 double sigma)
00073 {
00074 fors_pattern *p = cpl_malloc(sizeof(*p));
00075
00076 assure( ref != NULL, return p, NULL );
00077 assure( p1 != NULL, return p, NULL );
00078 assure( p2 != NULL, return p, NULL );
00079 assure( sigma >= 0, return p, NULL );
00080
00081 p->ref = ref;
00082 {
00083 double r1 = fors_point_distsq(ref, p1);
00084 double r2 = fors_point_distsq(ref, p2);
00085 double dr1 = sqrt(8*sigma*sigma*r1);
00086 double dr2 = sqrt(8*sigma*sigma*r2);
00087
00088 double dt1, dt2;
00089 double t1 = double_atan2(ref->y - p1->y, sqrt(2)*sigma,
00090 ref->x - p1->x, sqrt(2)*sigma,
00091 &dt1);
00092
00093 double t2 = double_atan2(ref->y - p2->y, sqrt(2)*sigma,
00094 ref->x - p2->x, sqrt(2)*sigma,
00095 &dt2);
00096
00097 if (r1 < r2) {
00098 p->ratsq = double_divide(r1, dr1,
00099 r2, dr2,
00100 &p->dratsq);
00101
00102 p->theta = double_subtract(t1, dt1,
00103 t2, dt2,
00104 &p->dtheta);
00105 p->min = p1;
00106 p->max = p2;
00107 }
00108 else {
00109 p->ratsq = double_divide(r2, dr2,
00110 r1, dr1,
00111 &p->dratsq);
00112 p->theta = double_subtract(t2, dt2,
00113 t1, dt1,
00114 &p->dtheta);
00115 p->min = p2;
00116 p->max = p1;
00117 }
00118
00119 while (p->theta < 0 ) p->theta += 2*M_PI;
00120 while (p->theta >= 2*M_PI) p->theta -= 2*M_PI;
00121 }
00122
00123 return p;
00124 }
00125
00126 #undef cleanup
00127 #define cleanup
00128
00137 fors_pattern_list *
00138 fors_pattern_new_from_points(fors_point_list *points,
00139 double tolerance,
00140 double sigma)
00141 {
00142 fors_pattern_list *patterns = fors_pattern_list_new();
00143 double tol_sq = tolerance * tolerance;
00144 fors_point *ref, *p1, *p2;
00145
00146 assure( points != NULL, return NULL, NULL );
00147
00148 for (ref = fors_point_list_first(points);
00149 ref != NULL;
00150 ref = fors_point_list_next(points)) {
00151
00152 for (fors_point_list_first_pair(points, &p1, &p2);
00153 p1 != NULL;
00154 fors_point_list_next_pair(points, &p1, &p2)) {
00155
00156 if (fors_point_distsq(ref, p1) > tol_sq &&
00157 fors_point_distsq(ref, p2) > tol_sq &&
00158 fors_point_distsq(p1 , p2) > tol_sq) {
00159
00160 fors_pattern_list_insert(patterns,
00161 fors_pattern_new(
00162 ref, p1, p2, sigma));
00163 }
00164 }
00165 }
00166
00167 cpl_msg_debug(cpl_func,
00168 "Created %d pattern(s)", fors_pattern_list_size(patterns));
00169
00170 return patterns;
00171 }
00172
00173 #undef cleanup
00174 #define cleanup
00175
00180 #if 0
00181 static fors_pattern *
00182 fors_pattern_duplicate(const fors_pattern *p)
00183 {
00184 fors_pattern *d = NULL;
00185
00186 assure( p != NULL, return NULL, NULL );
00187
00188 d = cpl_malloc(sizeof(*d));
00189
00190 d->ratsq = p->ratsq;
00191 d->theta = p->theta;
00192
00193 return d;
00194 }
00195 #endif
00196
00201 void fors_pattern_delete(fors_pattern **p)
00202 {
00203 if (p && *p) {
00204 cpl_free(*p); *p = NULL;
00205 }
00206 return;
00207 }
00208
00214 const fors_point *
00215 fors_pattern_get_ref(const fors_pattern *p)
00216 {
00217 assure( p != NULL, return NULL, NULL );
00218
00219 return p->ref;
00220 }
00221
00230 void fors_pattern_error(const fors_pattern *p,
00231 double *dr2,
00232 double *dtheta)
00233 {
00234 assure( p != NULL, return, NULL );
00235 assure( dr2 != NULL, return, NULL );
00236 assure( dtheta != NULL, return, NULL );
00237
00238 *dr2 = p->dratsq;
00239 *dtheta = p->dtheta / (2*M_PI);
00240
00241 return;
00242 }
00243
00244
00253 double fors_pattern_distsq(const fors_pattern *p,
00254 const fors_pattern *q)
00255 {
00256 assure( p != NULL, return -1, NULL );
00257 assure( q != NULL, return -1, NULL );
00258
00259 double dtheta = fors_angle_diff(&p->theta, &q->theta);
00260
00261
00262
00263
00264
00265 return
00266 (p->ratsq - q->ratsq) * (p->ratsq - q->ratsq) / (1.0 * 1.0) +
00267 (dtheta * dtheta) / (M_PI*M_PI);
00268 }
00269
00276 double fors_pattern_dist_per_error(const fors_pattern *p,
00277 const fors_pattern *q)
00278 {
00279 double dtheta = fors_angle_diff(&p->theta, &q->theta);
00280
00281 double p_error_r;
00282 double p_error_t;
00283
00284 fors_pattern_error(p,
00285 &p_error_r,
00286 &p_error_t);
00287
00288 double q_error_r;
00289 double q_error_t;
00290
00291 fors_pattern_error(q,
00292 &q_error_r,
00293 &q_error_t);
00294
00295
00296 double rr = p_error_r*p_error_r + q_error_r*q_error_r;
00297 double tt = p_error_t*p_error_t + q_error_t*q_error_t;
00298
00299 return sqrt(
00300 (p->ratsq - q->ratsq) * (p->ratsq - q->ratsq) / ((1.0*1.0) * rr) +
00301 (dtheta * dtheta) / ((M_PI*M_PI) * tt)
00302 );
00303 }
00304
00305
00306
00307
00312 void fors_pattern_print(const fors_pattern *p)
00313 {
00314 if (p == NULL) {
00315 cpl_msg_info(cpl_func, "NULL pattern");
00316 }
00317 else {
00318 cpl_msg_info(cpl_func, "Rmin^2/Rmax^2 = %f ; theta = %f",
00319 p->ratsq, p->theta);
00320 }
00321 return;
00322 }
00323
00331 double fors_pattern_get_scale(const fors_pattern *p,
00332 const fors_pattern *q)
00333 {
00334 assure( p != NULL, return 0, NULL );
00335 assure( q != NULL, return 0, NULL );
00336
00337 double s1 = sqrt(fors_point_distsq(p->ref, p->max));
00338 double s2 = sqrt(fors_point_distsq(q->ref, q->max));
00339
00340 return (s2 == 0) ? 0 : s1/s2;
00341 }
00342
00350 double fors_pattern_get_angle(const fors_pattern *p,
00351 const fors_pattern *q)
00352 {
00353 assure( p != NULL, return -1, NULL );
00354 assure( q != NULL, return -1, NULL );
00355
00356 double t1 = atan2(p->ref->y - p->max->y,
00357 p->ref->x - p->max->x);
00358 double t2 = atan2(q->ref->y - q->max->y,
00359 q->ref->x - q->max->x);
00360
00361 double t = t1 - t2;
00362
00363 while (t >= 2*M_PI) t -= 2*M_PI;
00364 while (t < 0 ) t += 2*M_PI;
00365
00366 return t;
00367 }
00368
00369
00370 #define LIST_DEFINE
00371 #undef LIST_ELEM
00372 #define LIST_ELEM fors_pattern
00373 #include <list.h>
00374