00001 #include <eh_utils.h>
00002
00003 Eh_polygon_2
00004 eh_get_rectangle_polygon( Eh_pt_2 center , double dx , double dy )
00005 {
00006 int n;
00007 Eh_polygon_2 poly=NULL;
00008 Eh_pt_2 *this_corner;
00009 gssize x_offset[4] = { -1 , +1 , +1 , -1 };
00010 gssize y_offset[4] = { -1 , -1 , +1 , +1 };
00011
00012 for ( n=0 ; n<4 ; n++ )
00013 {
00014 this_corner = eh_new( Eh_pt_2 , 1 );
00015
00016 this_corner->x = center.x + x_offset[n]*dx/2.;
00017 this_corner->y = center.y + y_offset[n]*dy/2.;
00018
00019 poly = g_list_append( poly , this_corner );
00020 }
00021 return poly;
00022 }
00023
00024 Eh_polygon_2
00025 eh_get_polygon_from_grid( Eh_grid g , gssize i , gssize j )
00026 {
00027 int n;
00028 Eh_polygon_2 poly=NULL;
00029 Eh_pt_2 *this_corner;
00030 gssize x_offset[4] = { -1 , -1 , +1 , +1 };
00031 gssize y_offset[4] = { -1 , +1 , +1 , -1 };
00032
00033 for ( n=0 ; n<4 ; n++ )
00034 {
00035 this_corner = eh_new( Eh_pt_2 , 1 );
00036
00037 this_corner->x = ( eh_grid_x(g)[x_offset[n]] + eh_grid_x(g)[i] )/ 2.;
00038 this_corner->y = ( eh_grid_y(g)[y_offset[n]] + eh_grid_y(g)[j] )/ 2.;
00039
00040 poly = g_list_append( poly , this_corner );
00041 }
00042 return poly;
00043 }
00044
00045 void
00046 eh_destroy_polygon( Eh_polygon_2 p )
00047 {
00048 GList *this_link;
00049 for ( this_link=p ; this_link ; this_link=this_link->next )
00050 eh_free( this_link->data );
00051 g_list_free( p );
00052 }
00053
00054 GList*
00055 eh_find_polygon_crossings( Eh_pt_2 start ,
00056 double angle ,
00057 Eh_polygon_2 area ,
00058 int in_or_out )
00059 {
00060 GList *crossing=NULL, *in=NULL, *out=NULL;
00061 GList *this_link;
00062 Eh_pt_2 this_corner;
00063 Eh_pt_2 next_corner;
00064 Eh_pt_2 u, v, n;
00065 Eh_pt_2 *intercept;
00066 double angle_to_this_corner, angle_to_next_corner;
00067 double theta;
00068 double m_0, b_0, m_1, b_1;
00069 gboolean is_entering;
00070
00071 m_0 = tan( angle );
00072 b_0 = start.y - start.x*m_0;
00073
00074 for ( this_link=area ; this_link ; this_link = this_link->next )
00075 {
00076 this_corner = *((Eh_pt_2*)(this_link->data));
00077 if ( this_link->next )
00078 next_corner = *((Eh_pt_2*)(this_link->next->data));
00079 else
00080 next_corner = *((Eh_pt_2*)(area->data));
00081
00082 angle_to_this_corner = atan2( this_corner.y - start.y ,
00083 this_corner.x - start.x );
00084 angle_to_next_corner = atan2( next_corner.y - start.y ,
00085 next_corner.x - start.x );
00086
00087
00088
00089
00090
00091 u = eh_get_dir_vector( this_corner , next_corner );
00092 n = eh_get_norm_vector( u );
00093 v = eh_get_dir_vector( start , this_corner );
00094
00095
00096
00097
00098
00099 theta = eh_get_angle_between_vectors( v , n );
00100 if ( theta > -M_PI_2 && theta < M_PI_2 )
00101 is_entering = FALSE;
00102 else
00103 is_entering = TRUE;
00104
00105 if ( is_entering )
00106 swap_dbl( angle_to_this_corner , angle_to_next_corner );
00107
00108
00109
00110
00111 if ( ( ( is_entering && (in_or_out & POLYGON_IN_CROSSINGS ) )
00112 || ( !is_entering && (in_or_out & POLYGON_OUT_CROSSINGS) ) )
00113 && is_between_angles( angle ,
00114 angle_to_this_corner ,
00115 angle_to_next_corner ) )
00116 {
00117
00118 intercept = eh_new( Eh_pt_2 , 1 );
00119
00120 if ( u.x == 0 )
00121 {
00122 intercept->x = this_corner.x;
00123 intercept->y = m_0*intercept->x + b_0;
00124 }
00125 else
00126 {
00127
00128
00129
00130 m_1 = ( next_corner.y - this_corner.y )
00131 / ( next_corner.x - this_corner.x );
00132 b_1 = this_corner.y - ( this_corner.x * m_1 );
00133
00134 intercept->x = ( b_1 - b_0 ) / ( m_0 - m_1 );
00135 intercept->y = m_0*intercept->x + b_0;
00136 }
00137
00138 if ( is_entering )
00139 in = g_list_append( in , intercept );
00140 else
00141 out = g_list_append( out , intercept );
00142
00143 }
00144
00145 }
00146
00147 if ( in_or_out & POLYGON_IN_CROSSINGS )
00148 crossing = g_list_concat( crossing , in );
00149 if ( in_or_out & POLYGON_OUT_CROSSINGS )
00150 crossing = g_list_concat( crossing , out );
00151
00152 return crossing;
00153 }
00154
00155 gboolean
00156 is_between_angles( double angle , double angle_1 , double angle_2 )
00157 {
00158 angle_1 = eh_reduce_angle( angle_1 );
00159 angle_2 = eh_reduce_angle( angle_2 );
00160
00161
00162
00163
00164
00165 if ( angle_1 > angle_2 )
00166 {
00167 if ( angle < angle_2 )
00168 angle += 2.*M_PI;
00169 angle_2 += 2.*M_PI;
00170 }
00171 if ( angle > angle_1 && angle < angle_2 )
00172 return TRUE;
00173 else
00174 return FALSE;
00175 }
00176
00177 gboolean
00178 is_inside_area( Eh_pt_2 x , Eh_polygon_2 area )
00179 {
00180 GList *crossings;
00181 GList *this_link;
00182 Eh_pt_2 *this_corner, *next_corner;
00183 gboolean is_inside = TRUE;
00184 guint number_of_crossings;
00185 Eh_pt_2 u;
00186 double angle;
00187
00188 for ( this_link=area ; this_link && is_inside ; this_link = this_link->next )
00189 {
00190 this_corner = (Eh_pt_2*)(this_link->data);
00191 if ( this_link->next )
00192 next_corner = (Eh_pt_2*)(this_link->next->data);
00193 else
00194 next_corner = (Eh_pt_2*)(area->data);
00195
00196 u = eh_get_dir_vector( *this_corner , *next_corner );
00197
00198 angle = eh_get_vector_angle( u );
00199 crossings = eh_find_polygon_crossings( x , angle , area ,
00200 POLYGON_IN_CROSSINGS
00201 | POLYGON_OUT_CROSSINGS );
00202
00203 number_of_crossings = g_list_length( crossings );
00204
00205
00206
00207
00208
00209 if ( number_of_crossings%2 == 0 )
00210 is_inside = FALSE;
00211 }
00212
00213 return is_inside;
00214 }
00215
00216 Eh_pt_2
00217 eh_get_unit_vector( double angle )
00218 {
00219 Eh_pt_2 u;
00220 u.x = cos( angle );
00221 u.y = sin( angle );
00222 return u;
00223 }
00224
00225 Eh_pt_2
00226 eh_get_dir_vector( Eh_pt_2 point_1 , Eh_pt_2 point_2 )
00227 {
00228 Eh_pt_2 u;
00229
00230 u.x = point_2.x - point_1.x;
00231 u.y = point_2.y - point_1.y;
00232
00233 return eh_normalize_vector( u );
00234 }
00235
00236 Eh_pt_2
00237 eh_get_norm_vector( Eh_pt_2 u )
00238 {
00239 return eh_get_unit_vector( eh_get_vector_angle( u )-M_PI_2 );
00240 }
00241
00242 double
00243 eh_get_vector_length( Eh_pt_2 u )
00244 {
00245 return sqrt( pow( u.x , 2. ) + pow( u.y , 2. ) );
00246 }
00247
00248 double
00249 eh_get_vector_angle( Eh_pt_2 u )
00250 {
00251 return atan2( u.y , u.x );
00252 }
00253
00254 Eh_pt_2
00255 eh_normalize_vector( Eh_pt_2 u )
00256 {
00257 double r = eh_get_vector_length( u );
00258 u.x /= r;
00259 u.y /= r;
00260 return u;
00261 }
00262
00263 double
00264 eh_dot_vectors( Eh_pt_2 u , Eh_pt_2 v )
00265 {
00266 return u.x*v.x + u.y*v.y;
00267 }
00268
00269 double
00270 eh_get_angle_between_vectors( Eh_pt_2 u , Eh_pt_2 v )
00271 {
00272 return acos( eh_dot_vectors( u , v )
00273 / eh_get_vector_length( u )
00274 / eh_get_vector_length( v ) );
00275 }
00276
00277 pos_t*
00278 createPosVec(int size)
00279 {
00280 pos_t *v;
00281
00282
00283
00284 v = eh_new( pos_t , 1 );
00285 v->x = eh_new( double , size );
00286 v->y = eh_new( double , size );
00287 v->size = size;
00288 return v;
00289 }
00290
00291 #define DERIVATIVE_FORWARD_DIFFERENCE
00292
00293 double *derivative(pos_t v)
00294 {
00295 int i;
00296 double *slope;
00297
00298 slope = eh_new( double , v.size );
00299 #if defined(DERIVATIVE_FORWARD_DIFFERENCE)
00300 for (i=0;i<v.size-1;i++)
00301 slope[i] = (v.y[i+1]-v.y[i])/(v.x[i+1]-v.x[i]);
00302 slope[v.size-1] = slope[v.size-2];
00303 #elif defined(DERIVATIVE_BACKWARD_DIFFERENCE)
00304 for (i=1;i<v.size;i++)
00305 slope[i] = (v.y[i]-v.y[i-1])/(v.x[i]-v.x[i-1]);
00306 slope[0] = slope[1];
00307 #elif defined(DERIVATIVE_CENTERED_DIFFERENCE)
00308 for (i=1;i<v.size-1;i++)
00309 slope[i] = (v.y[i+1]-v.y[i-1])/(v.x[i+1]-v.x[i-1]);
00310 slope[v.size-1] = slope[v.size-2];
00311 slope[0] = slope[1];
00312 #endif
00313 return slope;
00314 }
00315
00316 void
00317 destroyPosVec(pos_t *v)
00318 {
00319 eh_free(v->x);
00320 eh_free(v->y);
00321 eh_free(v);
00322 return;
00323 }
00324
00325 Eh_pt_2
00326 eh_create_pt_2( double x , double y )
00327 {
00328 Eh_pt_2 pt;
00329 pt.x = x;
00330 pt.y = y;
00331 return pt;
00332 }
00333
00334 gboolean
00335 eh_cmp_pt_2( Eh_pt_2 a , Eh_pt_2 b , double eps )
00336 {
00337 return fabs( a.x-b.x ) < eps && fabs( a.y-b.y ) < eps;
00338 }
00339