diff --git a/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml b/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml
new file mode 100644
index 00000000000..3614379eb9a
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml
@@ -0,0 +1,44 @@
+
+
+
+
+ Video ARDone 2
+
+
+
+
+
+
+
+
+
+
+
+
+include $(PAPARAZZI_HOME)/sw/ext/ardrone2_vision/Makefile.paths
+
+VISION_MODULE_FOLDER = $(DIR_MODULES)/OpticFlow
+
+$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU
+$(TARGET).CXXFLAGS += -I$(PAPARAZZI_HOME)/sw/include/ -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE) -I$(PAPARAZZI_SRC)/sw/airborne/modules/
+$(TARGET).CXXFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU
+
+$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_module.c
+$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_code.c
+$(TARGET).srcs += $(VISION_MODULE_FOLDER)/hover_stabilization.c
+$(TARGET).srcs += $(DIR_CV)/opticflow/optic_flow_ardrone.c
+$(TARGET).srcs += $(DIR_CV)/opticflow/fast9/fastRosten.c
+$(TARGET).srcs += $(DIR_CV)/encoding/jpeg.c
+$(TARGET).srcs += $(DIR_CV)/encoding/rtp.c
+$(TARGET).srcs += $(DIR_CV)/trig.c
+$(TARGET).srcs += $(DIR_LIB)/udp/socket.c
+$(TARGET).srcs += $(DIR_LIB)/v4l/video.c
+$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread
+$(TARGET).LDFLAGS += -pthread -lrt -static
+
+
+
+
+
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/dummy.c b/sw/airborne/modules/computer_vision/OpticFlow/dummy.c
new file mode 100644
index 00000000000..63978a0e13c
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/dummy.c
@@ -0,0 +1,3 @@
+void dummyFunction(void) {
+ return;
+}
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c
new file mode 100644
index 00000000000..0acb8b76745
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.c
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+// Own Header
+#include "hover_stabilization.h"
+
+// Vision Data
+#include "opticflow_code.h"
+
+// Stabilization
+//#include "stabilization.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "autopilot.h"
+
+// Downlink
+#include "subsystems/datalink/downlink.h"
+
+// Controller Gains
+/* error if some gains are negative */
+#if (VISION_PHI_PGAIN < 0) || \
+ (VISION_PHI_IGAIN < 0) || \
+ (VISION_THETA_PGAIN < 0) || \
+ (VISION_THETA_IGAIN < 0)
+#error "ALL control gains have to be positive!!!"
+#endif
+bool activate_opticflow_hover;
+float vision_desired_vx;
+float vision_desired_vy;
+int32_t vision_phi_pgain;
+int32_t vision_phi_igain;
+int32_t vision_theta_pgain;
+int32_t vision_theta_igain;
+
+// Controller Commands
+struct Int32Eulers cmd_euler;
+
+// Hover Stabilization
+float Velx_Int;
+float Vely_Int;
+float Error_Velx;
+float Error_Vely;
+
+#define CMD_OF_SAT 1500 // 40 deg = 2859.1851
+unsigned char saturateX = 0, saturateY = 0;
+unsigned int set_heading;
+
+void init_hover_stabilization_onvision()
+{
+ INT_EULERS_ZERO(cmd_euler);
+
+ activate_opticflow_hover = VISION_HOVER;
+ vision_phi_pgain = VISION_PHI_PGAIN;
+ vision_phi_igain = VISION_PHI_IGAIN;
+ vision_theta_pgain = VISION_THETA_PGAIN;
+ vision_theta_igain = VISION_THETA_IGAIN;
+ vision_desired_vx = VISION_DESIRED_VX;
+ vision_desired_vy = VISION_DESIRED_VY;
+
+ set_heading = 1;
+
+ Error_Velx = 0;
+ Error_Vely = 0;
+ Velx_Int = 0;
+ Vely_Int = 0;
+}
+
+void run_hover_stabilization_onvision(void)
+{
+ if(autopilot_mode == AP_MODE_VISION_HOVER)
+ {
+ run_opticflow_hover();
+ }
+ else
+ {
+ Velx_Int = 0;
+ Vely_Int = 0;
+ }
+}
+
+void run_opticflow_hover(void)
+{
+ if(flow_count)
+ {
+ Error_Velx = Velx - vision_desired_vx;
+ Error_Vely = Vely - vision_desired_vy;
+ }
+ else
+ {
+ Error_Velx = 0;
+ Error_Vely = 0;
+ }
+
+ if(saturateX==0)
+ {
+ if(activate_opticflow_hover==TRUE)
+ {
+ Velx_Int += vision_theta_igain*Error_Velx;
+ }
+ else
+ {
+ Velx_Int += vision_theta_igain*V_body.x;
+ }
+ }
+ if(saturateY==0)
+ {
+ if(activate_opticflow_hover==TRUE)
+ {
+ Vely_Int += vision_phi_igain*Error_Vely;
+ }
+ else
+ {
+ Vely_Int += vision_phi_igain*V_body.y;
+ }
+ }
+
+ if(set_heading)
+ {
+ cmd_euler.psi = stateGetNedToBodyEulers_i()->psi;
+ set_heading = 0;
+ }
+
+ if(activate_opticflow_hover==TRUE)
+ {
+ cmd_euler.phi = - (vision_phi_pgain*Error_Vely + Vely_Int);
+ cmd_euler.theta = (vision_theta_pgain*Error_Velx + Velx_Int);
+ }
+ else
+ {
+ cmd_euler.phi = - (vision_phi_pgain*V_body.y + Vely_Int);
+ cmd_euler.theta = (vision_theta_pgain*V_body.x + Velx_Int);
+ }
+
+ saturateX = 0; saturateY = 0;
+ if(cmd_euler.phi<-CMD_OF_SAT){cmd_euler.phi = -CMD_OF_SAT; saturateX = 1;}
+ else if(cmd_euler.phi>CMD_OF_SAT){cmd_euler.phi = CMD_OF_SAT; saturateX = 1;}
+ if(cmd_euler.theta<-CMD_OF_SAT){cmd_euler.theta = -CMD_OF_SAT; saturateY = 1;}
+ else if(cmd_euler.theta>CMD_OF_SAT){cmd_euler.theta = CMD_OF_SAT;saturateY = 1;}
+
+ stabilization_attitude_set_rpy_setpoint_i(&cmd_euler);
+ DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &Vely, &Velx_Int, &Vely_Int, &cmd_euler.phi, &cmd_euler.theta);
+}
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h
new file mode 100644
index 00000000000..a0a69dda9cc
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef HOVER_STABILIZATION_H_
+#define HOVER_STABILIZATION_H_
+
+#include
+
+void init_hover_stabilization_onvision(void);
+void run_hover_stabilization_onvision(void);
+void run_opticflow_hover(void);
+
+extern bool activate_opticflow_hover;
+extern float vision_desired_vx;
+extern float vision_desired_vy;
+extern int32_t vision_phi_pgain;
+extern int32_t vision_phi_igain;
+extern int32_t vision_theta_pgain;
+extern int32_t vision_theta_igain;
+
+#endif /* HOVER_STABILIZATION_H_ */
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c
new file mode 100644
index 00000000000..bac9bfc3382
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c
@@ -0,0 +1,332 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.c
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#include
+#include
+
+// Own Header
+#include "opticflow_code.h"
+
+// Computer Vision
+#include "opticflow/optic_flow_gdc.h"
+#include "opticflow/fast9/fastRosten.h"
+#include "opticflow_module.h"
+
+// Paparazzi Data
+#include "subsystems/ins/ins_int.h"
+#include "subsystems/imu.h"
+
+// Downlink
+#include "subsystems/datalink/downlink.h"
+
+// Timer
+#include
+
+// Settable by plugin
+unsigned int imgWidth, imgHeight;
+unsigned int verbose = 0;
+
+// Local variables
+unsigned char *prev_frame, *gray_frame, *prev_gray_frame;
+int old_img_init;
+float OFx, OFy, dx_sum, dy_sum;
+
+// ARDrone Vertical Camera Parameters
+#define FOV_H 0.67020643276
+#define FOV_W 0.89360857702
+#define Fx_ARdrone 343.1211
+#define Fy_ARdrone 348.5053
+
+// Corner Detection
+int *x, *y;
+int count = 0;
+int max_count = 25;
+#define MAX_COUNT 100
+
+// Corner Tracking
+int *new_x, *new_y, *status, *dx, *dy;
+int error_opticflow;
+int flow_count = 0;
+int remove_point;
+int c;
+int borderx = 24, bordery = 24;
+
+// Remove bad corners
+float distance2, min_distance, min_distance2;
+
+// Flow Derotation
+#define FLOW_DEROTATION
+float curr_pitch, curr_roll, curr_yaw, prev_pitch, prev_roll;
+float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans;
+
+// Lateral Velocity Computation
+float Velx, Vely;
+
+// Compute body velocities
+struct FloatVect3 V_Ned;
+struct FloatRMat Rmat_Ned2Body;
+struct FloatVect3 V_body;
+
+// Called by plugin
+void my_plugin_init(void)
+{
+ // Initialize variables
+ gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char));
+ prev_frame = (unsigned char *) calloc(imgWidth*imgHeight*2,sizeof(unsigned char));
+ prev_gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char));
+ x = (int *) calloc(MAX_COUNT,sizeof(int));
+ new_x = (int *) calloc(MAX_COUNT,sizeof(int));
+ y = (int *) calloc(MAX_COUNT,sizeof(int));
+ new_y = (int *) calloc(MAX_COUNT,sizeof(int));
+ status = (int *) calloc(MAX_COUNT,sizeof(int));
+ dx = (int *) calloc(MAX_COUNT,sizeof(int));
+ dy = (int *) calloc(MAX_COUNT,sizeof(int));
+ old_img_init = 1;
+ OFx = 0.0;
+ OFy = 0.0;
+ dx_sum = 0.0;
+ dy_sum = 0.0;
+ diff_roll = 0.0;
+ diff_pitch = 0.0;
+ cam_h = 0.0;
+ prev_pitch = 0.0;
+ prev_roll = 0.0;
+ curr_pitch = 0.0;
+ curr_roll = 0.0;
+ curr_yaw = 0.0;
+ OFx_trans = 0.0;
+ OFy_trans = 0.0;
+ Velx = 0.0;
+ Vely = 0.0;
+}
+
+void my_plugin_run(unsigned char *frame)
+{
+ if(old_img_init == 1)
+ {
+ memcpy(prev_frame,frame,imgHeight*imgWidth*2);
+ CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight);
+ old_img_init = 0;
+ }
+
+ // ***********************************************************************************************************************
+ // Additional information from other sensors
+ // ***********************************************************************************************************************
+
+ // Compute body velocities from ENU
+ V_Ned.x = stateGetSpeedNed_f()->x;
+ V_Ned.y = stateGetSpeedNed_f()->y;
+ V_Ned.z = stateGetSpeedNed_f()->z;
+
+ struct FloatQuat* BodyQuaternions = stateGetNedToBodyQuat_f();
+ FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body,*BodyQuaternions);
+ RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned);
+
+ // ***********************************************************************************************************************
+ // Corner detection
+ // ***********************************************************************************************************************
+
+ // FAST corner detection
+ int fast_threshold = 20;
+ xyFAST* pnts_fast;
+ pnts_fast = fast9_detect((const byte*)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count);
+
+ if(count > MAX_COUNT) count = MAX_COUNT;
+ for(int i = 0; i < count; i++)
+ {
+ x[i] = pnts_fast[i].x;
+ y[i] = pnts_fast[i].y;
+ }
+ free(pnts_fast);
+
+ // Remove neighbouring corners
+ min_distance = 3;
+ min_distance2 = min_distance*min_distance;
+ int *labelmin;
+ labelmin = (int *) calloc(MAX_COUNT,sizeof(int));
+ for(int i = 0; i < count; i++)
+ {
+ for(int j = i+1; j < count; j++)
+ {
+ // distance squared:
+ distance2 = (x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]);
+ if(distance2 < min_distance2)
+ {
+ labelmin[i] = 1;
+ }
+ }
+ }
+
+ int count_fil = count;
+ for(int i = count-1; i >= 0; i-- )
+ {
+ remove_point = 0;
+
+ if(labelmin[i])
+ {
+ remove_point = 1;
+ }
+
+ if(remove_point)
+ {
+ for(c = i; c max_count) count_fil = max_count;
+ count = count_fil;
+ free(labelmin);
+
+ // **********************************************************************************************************************
+ // Corner Tracking
+ // **********************************************************************************************************************
+ CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight);
+
+ error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, 5, 100);
+
+ flow_count = count_fil;
+ for(int i=count_fil-1; i>=0; i--)
+ {
+ remove_point = 1;
+
+ if(status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth-1-borderx) ||
+ new_y[i] < bordery || new_y[i] > (imgHeight-1-bordery)))
+ {
+ remove_point = 0;
+ }
+
+ if(remove_point)
+ {
+ for(c = i; c theta;
+ curr_roll = stateGetNedToBodyEulers_f()->phi;
+ curr_yaw = stateGetNedToBodyEulers_f()->psi;
+
+ diff_pitch = (curr_pitch - prev_pitch)*imgHeight/FOV_H;
+ diff_roll = (curr_roll - prev_roll)*imgWidth/FOV_W;
+
+ prev_pitch = curr_pitch;
+ prev_roll = curr_roll;
+
+#ifdef FLOW_DEROTATION
+ if(flow_count)
+ {
+ OFx_trans = dx_sum - diff_roll;
+ OFy_trans = dy_sum - diff_pitch;
+
+ if((OFx_trans<=0) != (dx_sum<=0))
+ {
+ OFx_trans = 0;
+ OFy_trans = 0;
+ }
+ }
+ else
+ {
+ OFx_trans = dx_sum;
+ OFy_trans = dy_sum;
+ }
+#else
+ OFx_trans = dx_sum;
+ OFy_trans = dy_sum;
+#endif
+
+ // Average Filter
+ OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1);
+
+ // Velocity Computation
+ #ifdef USE_SONAR
+ cam_h = ins_impl.sonar_z;
+ #else
+ cam_h = 1;
+ #endif
+
+ if(flow_count)
+ {
+ Velx = OFy*cam_h*FPS/Fy_ARdrone + 0.05;
+ Vely = -OFx*cam_h*FPS/Fx_ARdrone - 0.1;
+ }
+ else
+ {
+ Velx = 0.0;
+ Vely = 0.0;
+ }
+
+ // **********************************************************************************************************************
+ // Next Loop Preparation
+ // **********************************************************************************************************************
+
+ memcpy(prev_frame,frame,imgHeight*imgWidth*2);
+ memcpy(prev_gray_frame,gray_frame,imgHeight*imgWidth);
+
+ // **********************************************************************************************************************
+ // Downlink Message
+ // **********************************************************************************************************************
+ DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count);
+}
+
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h
new file mode 100644
index 00000000000..f46fcc0d77b
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef _OPT_FL_LAND_H
+#define _OPT_FL_LAND_H
+
+// Settable by pluging
+extern unsigned int imgWidth, imgHeight;
+extern unsigned int verbose;
+
+// Variables used by the controller
+extern float Velx, Vely;
+extern int count;
+extern int flow_count;
+extern struct FloatVect3 V_body;
+
+// Called by plugin
+void my_plugin_init(void);
+void my_plugin_run(unsigned char *frame);
+
+// Timer
+void start_timer_rates(void);
+long end_timer_rates(void);
+#endif
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c
new file mode 100644
index 00000000000..a254d05b1f0
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c
@@ -0,0 +1,218 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+
+// Own header
+#include "opticflow_module.h"
+
+// Navigate Based On Vision
+#include "hover_stabilization.h"
+
+// Paparazzi
+#include "state.h" // for attitude
+#include "boards/ardrone/navdata.h" // for ultrasound Height
+
+// Threaded computer vision
+#include
+
+// Frame Rate (FPS)
+#include
+float FPS;
+volatile long timestamp;
+#define USEC_PER_SEC 1000000L
+long time_elapsed (struct timeval *t1, struct timeval *t2)
+{
+ long sec, usec;
+ sec = t2->tv_sec - t1->tv_sec;
+ usec = t2->tv_usec - t1->tv_usec;
+ if (usec < 0)
+ {
+ --sec;
+ usec = usec + USEC_PER_SEC;
+ }
+ return sec*USEC_PER_SEC + usec;
+}
+
+struct timeval start_time;
+struct timeval end_time;
+
+void start_timer() {
+ gettimeofday (&start_time, NULL);
+}
+long end_timer() {
+ gettimeofday (&end_time, NULL);
+ return time_elapsed(&start_time, &end_time);
+}
+
+void opticflow_module_init(void)
+{
+ // Immediately start the vision thread when the module initialized
+ opticflow_module_start();
+
+ // Stabilization Code Initialization
+ init_hover_stabilization_onvision();
+
+ // Frame Rate Initialization
+ FPS = 0.0;
+ timestamp=0;
+ start_timer();
+}
+
+
+volatile uint8_t computervision_thread_has_results = 0;
+
+void opticflow_module_run(void)
+{
+
+ // Read Latest Vision Module Results
+ if (computervision_thread_has_results)
+ {
+ computervision_thread_has_results = 0;
+ run_hover_stabilization_onvision();
+ }
+}
+
+/////////////////////////////////////////////////////////////////////////
+// COMPUTER VISION THREAD
+
+// Video
+#include "v4l/video.h"
+#include "resize.h"
+
+// Payload Code
+#include "opticflow_code.h"
+
+// Downlink Video
+//#define DOWNLINK_VIDEO 1
+
+#ifdef DOWNLINK_VIDEO
+#include "encoding/jpeg.h"
+#include "encoding/rtp.h"
+#endif
+
+#include
+
+pthread_t computervision_thread;
+volatile uint8_t computervision_thread_status = 0;
+volatile uint8_t computer_vision_thread_command = 0;
+void *computervision_thread_main(void* data);
+void *computervision_thread_main(void* data)
+{
+ // Video Input
+ struct vid_struct vid;
+ vid.device = (char*)"/dev/video2"; // video1 = front camera; video2 = bottom camera
+ vid.w=320;
+ vid.h=240;
+ vid.n_buffers = 4;
+
+ if (video_init(&vid)<0) {
+ printf("Error initialising video\n");
+ computervision_thread_status = -1;
+ return 0;
+ }
+
+ // Video Grabbing
+ struct img_struct* img_new = video_create_image(&vid);
+
+ // Video Resizing
+ #define DOWNSIZE_FACTOR 1
+ struct img_struct small;
+ small.w = vid.w / DOWNSIZE_FACTOR;
+ small.h = vid.h / DOWNSIZE_FACTOR;
+ small.buf = (uint8_t*)malloc(small.w*small.h*2);
+
+#ifdef DOWNLINK_VIDEO
+ // Video Compression
+ uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2);
+
+ // Network Transmit
+ struct UdpSocket* vsock;
+ //#define FMS_UNICAST 0
+ //#define FMS_BROADCAST 1
+ vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST);
+#endif
+
+ // First Apply Settings before init
+ imgWidth = small.w;
+ imgHeight = small.h;
+ verbose = 2;
+ my_plugin_init();
+
+ while (computer_vision_thread_command > 0)
+ {
+ video_grab_image(&vid, img_new);
+
+ // FPS
+ timestamp = end_timer();
+ FPS = (float) 1000000/(float)timestamp;
+ // printf("dt = %d, FPS = %f\n",timestamp, FPS);
+ start_timer();
+
+ // Resize
+ //resize_uyuv(img_new, &small, DOWNSIZE_FACTOR);
+
+ // Run Image Processing
+ my_plugin_run(img_new->buf);
+ // my_plugin_run(small.buf);
+
+ #ifdef DOWNLINK_VIDEO
+ // JPEG encode the image:
+ uint32_t quality_factor = 10; //20 if no resize,
+ uint8_t dri_header = 0;
+ uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h)
+ uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header);
+ uint32_t size = end-(jpegbuf);
+
+ printf("Sending an image ...%u\n",size);
+ uint32_t delta_t_per_frame = 0; // 0 = use drone clock
+ send_rtp_frame(vsock, jpegbuf,size, small.w, small.h,0, quality_factor, dri_header, delta_t_per_frame);
+ #endif
+ computervision_thread_has_results++;
+ }
+ printf("Thread Closed\n");
+ video_close(&vid);
+ computervision_thread_status = -100;
+ return 0;
+}
+
+void opticflow_module_start(void)
+{
+ computer_vision_thread_command = 1;
+ int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL);
+ if(rc) {
+ printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
+ }
+}
+
+void opticflow_module_stop(void)
+{
+ computer_vision_thread_command = 0;
+}
+
+
+
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h
new file mode 100644
index 00000000000..b5e635a1ff1
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef OPTICFLOW_LAND_H
+#define OPTICFLOW_LAND_H
+
+// Module functions
+extern void opticflow_module_init(void);
+extern void opticflow_module_run(void);
+extern void opticflow_module_start(void);
+extern void opticflow_module_stop(void);
+
+// Frame Rate
+extern float FPS;
+struct timeval;
+long time_elapsed(struct timeval *t1, struct timeval *t2);
+void start_timer(void);
+long end_timer(void);
+
+#endif /* OPTICFLOW_LAND_H */
diff --git a/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h b/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h
new file mode 100644
index 00000000000..2039dd4edd6
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h
@@ -0,0 +1,32 @@
+#ifndef VMSTRTS_H
+#define VMSTRTS_H
+
+/***
+ an exact copy of this file to exist in ppz
+ this files keeps the structs that are serialized and streamed over tcp/ip through localhost
+ */
+
+#define N_BINS 10
+
+struct gst2ppz_message_struct
+{
+ unsigned int ID; // Keep different modules for using each others data
+ unsigned int counter; // counter to keep track of data
+ unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction
+ unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction
+};
+extern struct gst2ppz_message_struct gst2ppz;
+
+struct ppz2gst_message_struct
+{
+ unsigned int ID; // Keep different modules for using each others data
+ unsigned int counter; //counter to keep track of data
+ int pitch;
+ int roll;
+ int alt;
+ int adjust_factor; // 0-10 :adjust brightness
+};
+extern struct ppz2gst_message_struct ppz2gst;
+
+#endif /* VMSTRTS_H */
+
diff --git a/sw/airborne/modules/computer_vision/cv/color.h b/sw/airborne/modules/computer_vision/cv/color.h
index 46ffdb413a8..86737831f14 100644
--- a/sw/airborne/modules/computer_vision/cv/color.h
+++ b/sw/airborne/modules/computer_vision/cv/color.h
@@ -40,10 +40,8 @@ inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output)
}
}
-inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
- uint8_t u_M, uint8_t v_m, uint8_t v_M);
-inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m,
- uint8_t u_M, uint8_t v_m, uint8_t v_M)
+inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
+inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M)
{
int cnt = 0;
uint8_t *source = input->buf;
diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c
index 119f4bc2cf2..7d336dd1d6f 100644
--- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c
+++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c
@@ -433,8 +433,7 @@ void MakeTables(int q)
-uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format,
- uint32_t image_width, uint32_t image_height, uint8_t add_dri_header)
+uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, uint32_t image_width, uint32_t image_height, uint8_t add_dri_header)
{
uint16_t i, j;
diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c
index 22211268186..541da0068e0 100644
--- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c
+++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c
@@ -7,9 +7,7 @@
#include "rtp.h"
-void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber,
- uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code,
- uint8_t has_dri_header);
+void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header);
// http://www.ietf.org/rfc/rfc3550.txt
@@ -51,19 +49,16 @@ void test_rtp_frame(struct UdpSocket *sock)
uint8_t quality_code = 0x54;
if (toggle) {
- send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
- quality_code, 0);
+ send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
} else {
- send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code,
- quality_code, 0);
+ send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0);
}
framecounter++;
timecounter += 3600;
}
-void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code,
- uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
+void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t)
{
static uint32_t packetcounter = 0;
static uint32_t timecounter = 0;
@@ -88,8 +83,7 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int
len = JpegLen;
}
- send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code,
- has_dri_header);
+ send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header);
JpegLen -= len;
Jpeg += len;
diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE
new file mode 100644
index 00000000000..ee48fe68090
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE
@@ -0,0 +1,30 @@
+Copyright (c) 2006, 2008 Edward Rosten
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+
+ *Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+ *Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+
+ *Neither the name of the University of Cambridge nor the names of
+ its contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c
new file mode 100644
index 00000000000..9601a02e8e0
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c
@@ -0,0 +1,6069 @@
+/*
+Copyright (c) 2006, 2008 Edward Rosten
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+
+ *Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+ *Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+
+ *Neither the name of the University of Cambridge nor the names of
+ its contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include
+#include "fastRosten.h"
+
+#define Compare(X, Y) ((X)>=(Y))
+
+xyFAST* fast9_detect_nonmax(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners)
+{
+ xyFAST* corners;
+ int num_corners;
+ int* scores;
+ xyFAST* nonmax;
+
+ corners = fast9_detect(im, xsize, ysize, stride, b, &num_corners);
+ scores = fast9_score(im, stride, corners, num_corners, b);
+ nonmax = nonmax_suppression(corners, scores, num_corners, ret_num_corners);
+
+ free(corners);
+ free(scores);
+
+ return nonmax;
+}
+
+xyFAST* nonmax_suppression(const xyFAST* corners, const int* scores, int num_corners, int* ret_num_nonmax)
+{
+ int num_nonmax=0;
+ int last_row;
+ int* row_start;
+ int i, j;
+ xyFAST* ret_nonmax;
+ const int sz = (int)num_corners;
+
+ /*Point above points (roughly) to the pixel above the one of interest, if there
+ is a feature there.*/
+ int point_above = 0;
+ int point_below = 0;
+
+
+ if(num_corners < 1)
+ {
+ *ret_num_nonmax = 0;
+ return 0;
+ }
+
+ ret_nonmax = (xyFAST*)malloc(num_corners * sizeof(xyFAST));
+
+ /* Find where each row begins
+ (the corners are output in raster scan order). A beginning of -1 signifies
+ that there are no corners on that row. */
+ last_row = corners[num_corners-1].y;
+ row_start = (int*)malloc((last_row+1)*sizeof(int));
+
+ for(i=0; i < last_row+1; i++)
+ row_start[i] = -1;
+
+ {
+ int prev_row = -1;
+ for(i=0; i< num_corners; i++)
+ if(corners[i].y != prev_row)
+ {
+ row_start[corners[i].y] = i;
+ prev_row = corners[i].y;
+ }
+ }
+
+
+
+ for(i=0; i < sz; i++)
+ {
+ int score = scores[i];
+ xyFAST pos = corners[i];
+
+ /*Check left */
+ if(i > 0)
+ if(corners[i-1].x == pos.x-1 && corners[i-1].y == pos.y && Compare(scores[i-1], score))
+ continue;
+
+ /*Check right*/
+ if(i < (sz - 1))
+ if(corners[i+1].x == pos.x+1 && corners[i+1].y == pos.y && Compare(scores[i+1], score))
+ continue;
+
+ /*Check above (if there is a valid row above)*/
+ if(pos.y != 0 && row_start[pos.y - 1] != -1)
+ {
+ /*Make sure that current point_above is one
+ row above.*/
+ if(corners[point_above].y < pos.y - 1)
+ point_above = row_start[pos.y-1];
+
+ /*Make point_above point to the first of the pixels above the current point,
+ if it exists.*/
+ for(; corners[point_above].y < pos.y && corners[point_above].x < pos.x - 1; point_above++)
+ {}
+
+
+ for(j=point_above; corners[j].y < pos.y && corners[j].x <= pos.x + 1; j++)
+ {
+ int x = corners[j].x;
+ if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && Compare(scores[j], score))
+ goto cont;
+ }
+
+ }
+
+ /*Check below (if there is anything below)*/
+ if(pos.y != last_row && row_start[pos.y + 1] != -1 && point_below < sz) /*Nothing below*/
+ {
+ if(corners[point_below].y < pos.y + 1)
+ point_below = row_start[pos.y+1];
+
+ /* Make point below point to one of the pixels belowthe current point, if it
+ exists.*/
+ for(; point_below < sz && corners[point_below].y == pos.y+1 && corners[point_below].x < pos.x - 1; point_below++)
+ {}
+
+ for(j=point_below; j < sz && corners[j].y == pos.y+1 && corners[j].x <= pos.x + 1; j++)
+ {
+ int x = corners[j].x;
+ if( (x == pos.x - 1 || x ==pos.x || x == pos.x+1) && Compare(scores[j],score))
+ goto cont;
+ }
+ }
+
+ ret_nonmax[num_nonmax++] = corners[i];
+ cont:
+ ;
+ }
+
+ free(row_start);
+ *ret_num_nonmax = num_nonmax;
+ return ret_nonmax;
+}
+
+int fast9_corner_score(const byte* p, const int pixel[], int bstart)
+{
+ int bmin = bstart;
+ int bmax = 255;
+ int b = (bmax + bmin)/2;
+
+ /*Compute the score using binary search*/
+ for(;;)
+ {
+ int cb = *p + b;
+ int c_b= *p - b;
+
+
+ if( p[pixel[0]] > cb)
+ if( p[pixel[1]] > cb)
+ if( p[pixel[2]] > cb)
+ if( p[pixel[3]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[7]] < c_b)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[14]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[6]] < c_b)
+ if( p[pixel[15]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[13]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[13]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[5]] < c_b)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[12]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[14]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[6]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[12]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[6]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[4]] < c_b)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[11]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[12]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[13]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[11]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[3]] < c_b)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[10]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[4]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[10]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[4]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[2]] < c_b)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[9]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[3]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[9]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[3]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[12]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[1]] < c_b)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[2]] > cb)
+ if( p[pixel[3]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[8]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[2]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[2]] > cb)
+ if( p[pixel[3]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[8]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[2]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[0]] < c_b)
+ if( p[pixel[1]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[3]] > cb)
+ if( p[pixel[2]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[2]] < c_b)
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[1]] < c_b)
+ if( p[pixel[2]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[3]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[2]] < c_b)
+ if( p[pixel[3]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[4]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[3]] < c_b)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[13]] < c_b)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[12]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[6]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[14]] < c_b)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[6]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[15]] < c_b)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[13]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[6]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[4]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[9]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[3]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[8]] > cb)
+ if( p[pixel[7]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[10]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[3]] > cb)
+ if( p[pixel[2]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[2]] < c_b)
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[7]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[7]] > cb)
+ if( p[pixel[8]] > cb)
+ if( p[pixel[9]] > cb)
+ if( p[pixel[6]] > cb)
+ if( p[pixel[5]] > cb)
+ if( p[pixel[4]] > cb)
+ if( p[pixel[3]] > cb)
+ if( p[pixel[2]] > cb)
+ if( p[pixel[1]] > cb)
+ goto is_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] > cb)
+ if( p[pixel[11]] > cb)
+ if( p[pixel[12]] > cb)
+ if( p[pixel[13]] > cb)
+ if( p[pixel[14]] > cb)
+ if( p[pixel[15]] > cb)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else if( p[pixel[7]] < c_b)
+ if( p[pixel[8]] < c_b)
+ if( p[pixel[9]] < c_b)
+ if( p[pixel[6]] < c_b)
+ if( p[pixel[5]] < c_b)
+ if( p[pixel[4]] < c_b)
+ if( p[pixel[3]] < c_b)
+ if( p[pixel[2]] < c_b)
+ if( p[pixel[1]] < c_b)
+ goto is_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ if( p[pixel[10]] < c_b)
+ if( p[pixel[11]] < c_b)
+ if( p[pixel[12]] < c_b)
+ if( p[pixel[13]] < c_b)
+ if( p[pixel[14]] < c_b)
+ if( p[pixel[15]] < c_b)
+ goto is_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+ else
+ goto is_not_a_corner;
+
+ is_a_corner:
+ bmin=b;
+ goto end_if;
+
+ is_not_a_corner:
+ bmax=b;
+ goto end_if;
+
+ end_if:
+
+ if(bmin == bmax - 1 || bmin == bmax)
+ return bmin;
+ b = (bmin + bmax) / 2;
+ }
+}
+
+static void make_offsets(int pixel[], int row_stride)
+{
+ pixel[0] = 0 + row_stride * 3;
+ pixel[1] = 1 + row_stride * 3;
+ pixel[2] = 2 + row_stride * 2;
+ pixel[3] = 3 + row_stride * 1;
+ pixel[4] = 3 + row_stride * 0;
+ pixel[5] = 3 + row_stride * -1;
+ pixel[6] = 2 + row_stride * -2;
+ pixel[7] = 1 + row_stride * -3;
+ pixel[8] = 0 + row_stride * -3;
+ pixel[9] = -1 + row_stride * -3;
+ pixel[10] = -2 + row_stride * -2;
+ pixel[11] = -3 + row_stride * -1;
+ pixel[12] = -3 + row_stride * 0;
+ pixel[13] = -3 + row_stride * 1;
+ pixel[14] = -2 + row_stride * 2;
+ pixel[15] = -1 + row_stride * 3;
+}
+
+
+
+int* fast9_score(const byte* i, int stride, xyFAST* corners, int num_corners, int b)
+{
+ int* scores = (int*)malloc(sizeof(int)* num_corners);
+ int n;
+
+ int pixel[16];
+ make_offsets(pixel, stride);
+
+ for(n=0; n < num_corners; n++)
+ scores[n] = fast9_corner_score(i + corners[n].y*stride + corners[n].x, pixel, b);
+
+ return scores;
+}
+
+
+xyFAST* fast9_detect(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners)
+{
+ int num_corners=0;
+ xyFAST* ret_corners;
+ int rsize=512;
+ int pixel[16];
+ int x, y;
+
+ ret_corners = (xyFAST*)malloc(sizeof(xyFAST)*rsize);
+ make_offsets(pixel, stride);
+
+ for(y=3; y < ysize - 3; y++)
+ for(x=3; x < xsize - 3; x++)
+ {
+ const byte* p = im + y*stride + x;
+
+ int cb = *p + b;
+ int c_b= *p - b;
+ if(p[pixel[0]] > cb)
+ if(p[pixel[1]] > cb)
+ if(p[pixel[2]] > cb)
+ if(p[pixel[3]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else if(p[pixel[7]] < c_b)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else if(p[pixel[14]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[6]] < c_b)
+ if(p[pixel[15]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else if(p[pixel[13]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[13]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[5]] < c_b)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[12]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[14]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[6]] < c_b)
+ {}
+ else
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[12]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[6]] < c_b)
+ {}
+ else
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[4]] < c_b)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[11]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[12]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[13]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ {}
+ else
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[11]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ {}
+ else
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[3]] < c_b)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[10]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[4]] < c_b)
+ {}
+ else
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[10]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[4]] < c_b)
+ {}
+ else
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[2]] < c_b)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[9]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[3]] < c_b)
+ {}
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[9]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[3]] < c_b)
+ {}
+ else
+ if(p[pixel[12]] < c_b)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[1]] < c_b)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[2]] > cb)
+ if(p[pixel[3]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[8]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[2]] < c_b)
+ {}
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[2]] > cb)
+ if(p[pixel[3]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[8]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[2]] < c_b)
+ {}
+ else
+ if(p[pixel[11]] < c_b)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[0]] < c_b)
+ if(p[pixel[1]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[3]] > cb)
+ if(p[pixel[2]] > cb)
+ {}
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[2]] < c_b)
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[1]] < c_b)
+ if(p[pixel[2]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[3]] > cb)
+ {}
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[2]] < c_b)
+ if(p[pixel[3]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[4]] > cb)
+ {}
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[3]] < c_b)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ {}
+ else
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[13]] < c_b)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[12]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[6]] > cb)
+ {}
+ else
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[14]] < c_b)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[6]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[15]] < c_b)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[13]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[6]] > cb)
+ {}
+ else
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ {}
+ else
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[4]] > cb)
+ {}
+ else
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[9]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[3]] > cb)
+ {}
+ else
+ if(p[pixel[12]] > cb)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[8]] > cb)
+ if(p[pixel[7]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[10]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[3]] > cb)
+ if(p[pixel[2]] > cb)
+ {}
+ else
+ if(p[pixel[11]] > cb)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[2]] < c_b)
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[7]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[7]] > cb)
+ if(p[pixel[8]] > cb)
+ if(p[pixel[9]] > cb)
+ if(p[pixel[6]] > cb)
+ if(p[pixel[5]] > cb)
+ if(p[pixel[4]] > cb)
+ if(p[pixel[3]] > cb)
+ if(p[pixel[2]] > cb)
+ if(p[pixel[1]] > cb)
+ {}
+ else
+ if(p[pixel[10]] > cb)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] > cb)
+ if(p[pixel[11]] > cb)
+ if(p[pixel[12]] > cb)
+ if(p[pixel[13]] > cb)
+ if(p[pixel[14]] > cb)
+ if(p[pixel[15]] > cb)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else if(p[pixel[7]] < c_b)
+ if(p[pixel[8]] < c_b)
+ if(p[pixel[9]] < c_b)
+ if(p[pixel[6]] < c_b)
+ if(p[pixel[5]] < c_b)
+ if(p[pixel[4]] < c_b)
+ if(p[pixel[3]] < c_b)
+ if(p[pixel[2]] < c_b)
+ if(p[pixel[1]] < c_b)
+ {}
+ else
+ if(p[pixel[10]] < c_b)
+ {}
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ if(p[pixel[10]] < c_b)
+ if(p[pixel[11]] < c_b)
+ if(p[pixel[12]] < c_b)
+ if(p[pixel[13]] < c_b)
+ if(p[pixel[14]] < c_b)
+ if(p[pixel[15]] < c_b)
+ {}
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ else
+ continue;
+ if(num_corners == rsize)
+ {
+ rsize*=2;
+ ret_corners = (xyFAST*)realloc(ret_corners, sizeof(xyFAST)*rsize);
+ }
+ ret_corners[num_corners].x = x;
+ ret_corners[num_corners].y = y;
+ num_corners++;
+
+ }
+
+ *ret_num_corners = num_corners;
+ return ret_corners;
+
+}
+
diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h
new file mode 100644
index 00000000000..88d25714909
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h
@@ -0,0 +1,51 @@
+/*
+Copyright (c) 2006, 2008 Edward Rosten
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+
+ *Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+ *Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+
+ *Neither the name of the University of Cambridge nor the names of
+ its contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef FAST_H
+#define FAST_H
+
+typedef struct { int x, y; } xyFAST;
+typedef unsigned char byte;
+
+int fast9_corner_score(const byte* p, const int pixel[], int bstart);
+
+xyFAST* fast9_detect(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners);
+
+int* fast9_score(const byte* i, int stride, xyFAST* corners, int num_corners, int b);
+
+xyFAST* fast9_detect_nonmax(const byte* im, int xsize, int ysize, int stride, int b, int* ret_num_corners);
+
+xyFAST* nonmax_suppression(const xyFAST* corners, const int* scores, int num_corners, int* ret_num_nonmax);
+
+
+#endif
diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c
new file mode 100644
index 00000000000..f46243b9648
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c
@@ -0,0 +1,624 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * - Initial fixed-point C implementation by G. de Croon
+ * - Algorithm: Lucas-Kanade by Yves Bouguet
+ * - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_ardrone.c
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#include
+#include
+#include
+#include
+#include "optic_flow_gdc.h"
+#include "../../modules/OpticFlow/opticflow_module.h"
+
+#define int_index(x,y) (y * IMG_WIDTH + x)
+#define uint_index(xx, yy) (((yy * IMG_WIDTH + xx) * 2) & 0xFFFFFFFC)
+#define NO_MEMORY -1
+#define OK 0
+#define N_VISUAL_INPUTS 51
+#define N_ACTIONS 3
+#define MAX_COUNT_PT 50
+
+unsigned int IMG_WIDTH, IMG_HEIGHT;
+
+void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height)
+{
+ int x, y;
+ unsigned int ix;
+
+ // printf("W = %d, H = %d\n\r", IMG_WIDTH, IMG_HEIGHT);
+
+ for (x = 0; x < width; x++) {
+ for (y = 0; y < height; y++) {
+ ix = (y * width + x);
+ ImC[ix] = ImA[ix] * ImB[ix];
+ // If we want to keep the values in [0, 255]:
+ // ImC[ix] /= 255;
+ }
+ }
+}
+
+void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height)
+{
+ int x, y;
+ unsigned int ix;
+
+ // printf("W = %d, H = %d\n\r", IMG_WIDTH, IMG_HEIGHT);
+
+ for (x = 0; x < width; x++) {
+ for (y = 0; y < height; y++) {
+ ix = (y * width + x);
+ ImC[ix] = ImA[ix] - ImB[ix];
+ }
+ }
+
+}
+
+void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, int subpixel_factor)
+{
+ int x, y, x_0, y_0, x_0_or, y_0_or, i, j, window_size, alpha_x, alpha_y, max_x, max_y;
+ //int printed, limit;
+ unsigned int ix1, ix2, Y;
+ window_size = half_window_size * 2 + 1;
+ max_x = (IMG_WIDTH - 1) * subpixel_factor;
+ max_y = (IMG_HEIGHT - 1) * subpixel_factor;
+ //printed = 0; limit = 4;
+
+ for (i = 0; i < window_size; i++) {
+ for (j = 0; j < window_size; j++) {
+ // index for this position in the patch:
+ ix1 = (j * window_size + i);
+
+ // determine subpixel coordinates of the current pixel:
+ x = center_x + (i - half_window_size) * subpixel_factor;
+ if (x < 0) { x = 0; }
+ if (x > max_x) { x = max_x; }
+ y = center_y + (j - half_window_size) * subpixel_factor;
+ if (y < 0) { y = 0; }
+ if (y > max_y) { y = max_y; }
+ // pixel to the top left:
+ x_0_or = (x / subpixel_factor);
+ x_0 = x_0_or * subpixel_factor;
+ y_0_or = (y / subpixel_factor);
+ y_0 = y_0_or * subpixel_factor;
+ /*if(printed < limit)
+ {
+ printf("x_0_or = %d, y_0_or = %d;\n\r", x_0_or, y_0_or);
+ printf("x_0 = %d, y_0 = %d\n\r");
+ printed++;
+ }*/
+
+
+ if (x == x_0 && y == y_0) {
+ // simply copy the pixel:
+// ix2 = uint_index(x_0_or, y_0_or);
+// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
+ ix2 = y_0_or * IMG_WIDTH + x_0_or;
+ Y = (unsigned int)frame_buf[ix2 + 1];
+ Patch[ix1] = (int) Y;
+ } else {
+ // blending according to how far the subpixel coordinates are from the pixel coordinates
+ alpha_x = (x - x_0);
+ alpha_y = (y - y_0);
+
+ // the patch pixel is a blend from the four surrounding pixels:
+ ix2 = y_0_or * IMG_WIDTH + x_0_or;
+ Y = (unsigned int)frame_buf[ix2 + 1];
+// ix2 = uint_index(x_0_or, y_0_or);
+// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
+ Patch[ix1] = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * ((int) Y);
+
+ ix2 = y_0_or * IMG_WIDTH + (x_0_or + 1);
+ Y = (unsigned int)frame_buf[ix2 + 1];
+// ix2 = uint_index((x_0_or+1), y_0_or);
+// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
+ //if(printed < limit) printf("subpixel: TR = %d\n\r", Y);
+ Patch[ix1] += alpha_x * (subpixel_factor - alpha_y) * ((int) Y);
+
+ ix2 = (y_0_or + 1) * IMG_WIDTH + x_0_or;
+ Y = (unsigned int)frame_buf[ix2 + 1];
+// ix2 = uint_index(x_0_or, (y_0_or+1));
+// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
+ //if(printed < limit) printf("subpixel: BL = %d\n\r", Y);
+ Patch[ix1] += (subpixel_factor - alpha_x) * alpha_y * ((int) Y);
+
+ ix2 = (y_0_or + 1) * IMG_WIDTH + (x_0_or + 1);
+ Y = (unsigned int)frame_buf[ix2 + 1];
+// ix2 = uint_index((x_0_or+1), (y_0_or+1));
+// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1;
+ //if(printed < limit) printf("subpixel: BR = %d\n\r", Y);
+ Patch[ix1] += alpha_x * alpha_y * ((int) Y);
+
+ // normalize patch value
+ Patch[ix1] /= (subpixel_factor * subpixel_factor);
+
+ /*if(printed < limit)
+ {
+
+ printf("alpha_x = %d, alpha_y = %d, x_0 = %d, y_0 = %d, x = %d, y = %d, Patch[ix1] = %d\n\r", alpha_x, alpha_y, x_0, y_0, x, y, Patch[ix1]);
+ // printed++;
+ }
+ */
+
+ }
+ }
+ }
+
+ return;
+}
+
+void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size)
+{
+ unsigned int ix1, ix2;
+ int x, y, padded_patch_size, patch_size, Y1, Y2;
+ // int printed; printed = 0;
+
+ padded_patch_size = 2 * (half_window_size + 1) + 1;
+ patch_size = 2 * half_window_size + 1;
+ // currently we use [0 0 0; -1 0 1; 0 0 0] as mask for dx
+ for (x = 1; x < padded_patch_size - 1; x++) {
+ for (y = 1; y < padded_patch_size - 1; y++) {
+ // index in DX, DY:
+ ix2 = (unsigned int)((y - 1) * patch_size + (x - 1));
+
+ ix1 = (unsigned int)(y * padded_patch_size + x - 1);
+ Y1 = Patch[ix1];
+ ix1 = (unsigned int)(y * padded_patch_size + x + 1);
+ Y2 = Patch[ix1];
+// DX[ix2] = Y2 - Y1;
+ DX[ix2] = (Y2 - Y1) / 2;
+
+ ix1 = (unsigned int)((y - 1) * padded_patch_size + x);
+ Y1 = Patch[ix1];
+ ix1 = (unsigned int)((y + 1) * padded_patch_size + x);
+ Y2 = Patch[ix1];
+// DY[ix2] = Y2 - Y1;
+ DY[ix2] = (Y2 - Y1) / 2;
+
+ /*if(printed < 1 && DX[ix2] > 0)
+ {
+ printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]);
+ printed++;
+ }
+ else if(printed == 1 && DX[ix2] < 0)
+ {
+ printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]);
+ printed++;
+ }*/
+
+
+ }
+ }
+
+ return;
+}
+
+int getSumPatch(int *Patch, int size)
+{
+ int x, y, sum; // , threshold
+ unsigned int ix;
+
+ // in order to keep the sum within range:
+ //threshold = 50000; // typical values are far below this threshold
+
+ sum = 0;
+ for (x = 0; x < size; x++) {
+ for (y = 0; y < size; y++) {
+ ix = (y * size) + x;
+ //if(sum < threshold && sum > -threshold)
+ //{
+ sum += Patch[ix]; // do not check thresholds
+ //}
+ /*else
+ {
+ if(sum > threshold)
+ {
+ sum = threshold;
+ }
+ else
+ {
+ sum = -threshold;
+ }
+ }*/
+ }
+ }
+
+ return sum;
+}
+
+int calculateG(int *G, int *DX, int *DY, int half_window_size)
+{
+ int patch_size;
+ int *DXX; int *DXY; int *DYY;
+
+ patch_size = 2 * half_window_size + 1;
+
+ // allocate memory:
+ DXX = (int *) malloc(patch_size * patch_size * sizeof(int));
+ DXY = (int *) malloc(patch_size * patch_size * sizeof(int));
+ DYY = (int *) malloc(patch_size * patch_size * sizeof(int));
+
+ if (DXX == 0 || DXY == 0 || DYY == 0) {
+ return NO_MEMORY;
+ }
+
+ // then determine the second order gradients
+ multiplyImages(DX, DX, DXX, patch_size, patch_size);
+ multiplyImages(DX, DY, DXY, patch_size, patch_size);
+ multiplyImages(DY, DY, DYY, patch_size, patch_size);
+
+ // calculate G:
+ G[0] = getSumPatch(DXX, patch_size);
+ G[1] = getSumPatch(DXY, patch_size);
+ G[2] = G[1];
+ G[3] = getSumPatch(DYY, patch_size);
+
+ // free memory:
+ free((char *) DXX); free((char *) DXY); free((char *) DYY);
+
+ // no errors:
+ return OK;
+}
+
+
+
+int calculateError(int *ImC, int width, int height)
+{
+ int x, y, error;
+ unsigned int ix;
+
+ error = 0;
+
+ for (x = 0; x < width; x++) {
+ for (y = 0; y < height; y++) {
+ ix = (y * width + x);
+ error += ImC[ix] * ImC[ix];
+ }
+ }
+
+ return error;
+}
+
+int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations)
+{
+ // A straightforward one-level implementation of Lucas-Kanade.
+ // For all points:
+ // (1) determine the subpixel neighborhood in the old image
+ // (2) get the x- and y- gradients
+ // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
+ // (4) iterate over taking steps in the image to minimize the error:
+ // [a] get the subpixel neighborhood in the new image
+ // [b] determine the image difference between the two neighborhoods
+ // [c] calculate the 'b'-vector
+ // [d] calculate the additional flow step and possibly terminate the iteration
+ int p, subpixel_factor, x, y, it, step_threshold, step_x, step_y, v_x, v_y, Det;
+ int b_x, b_y, patch_size, padded_patch_size, error;
+ unsigned int ix1, ix2;
+ int *I_padded_neighborhood; int *I_neighborhood; int *J_neighborhood;
+ int *DX; int *DY; int *ImDiff; int *IDDX; int *IDDY;
+ int G[4];
+ int error_threshold;
+
+ // set the image width and height
+ IMG_WIDTH = imW;
+ IMG_HEIGHT = imH;
+ // spatial resolution of flow is 1 / subpixel_factor
+ subpixel_factor = 10;
+ // determine patch sizes and initialize neighborhoods
+ patch_size = (2 * half_window_size + 1);
+ error_threshold = (25 * 25) * (patch_size * patch_size);
+
+ padded_patch_size = (2 * half_window_size + 3);
+ I_padded_neighborhood = (int *) malloc(padded_patch_size * padded_patch_size * sizeof(int));
+ I_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int));
+ J_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int));
+ if (I_padded_neighborhood == 0 || I_neighborhood == 0 || J_neighborhood == 0) {
+ return NO_MEMORY;
+ }
+
+ DX = (int *) malloc(patch_size * patch_size * sizeof(int));
+ DY = (int *) malloc(patch_size * patch_size * sizeof(int));
+ IDDX = (int *) malloc(patch_size * patch_size * sizeof(int));
+ IDDY = (int *) malloc(patch_size * patch_size * sizeof(int));
+ ImDiff = (int *) malloc(patch_size * patch_size * sizeof(int));
+ if (DX == 0 || DY == 0 || ImDiff == 0 || IDDX == 0 || IDDY == 0) {
+ return NO_MEMORY;
+ }
+
+ for (p = 0; p < n_found_points; p++) {
+ //printf("*** NEW POINT ***\n\r");
+ // status: point is not yet lost:
+ status[p] = 1;
+
+ //printf("Normal coordinate: (%d,%d)\n\r", p_x[p], p_y[p]);
+ // We want to be able to take steps in the image of 1 / subpixel_factor:
+ p_x[p] *= subpixel_factor;
+ p_y[p] *= subpixel_factor;
+ //printf("Subpixel coordinate: (%d,%d)\n\r", p_x[p], p_y[p]);
+
+ // if the pixel is outside the ROI in the image, do not track it:
+ if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor && p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) {
+// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]);
+ status[p] = 0;
+ }
+
+ // (1) determine the subpixel neighborhood in the old image
+ // we determine a padded neighborhood with the aim of subsequent gradient processing:
+ getSubPixel_gray(I_padded_neighborhood, old_image_buf, p_x[p], p_y[p], half_window_size + 1, subpixel_factor);
+
+ // Also get the original-sized neighborhood
+ for (x = 1; x < padded_patch_size - 1; x++) {
+ for (y = 1; y < padded_patch_size - 1; y++) {
+ ix1 = (y * padded_patch_size + x);
+ ix2 = ((y - 1) * patch_size + (x - 1));
+ I_neighborhood[ix2] = I_padded_neighborhood[ix1];
+ }
+ }
+
+ // (2) get the x- and y- gradients
+ getGradientPatch(I_padded_neighborhood, DX, DY, half_window_size);
+
+ // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window
+ error = calculateG(G, DX, DY, half_window_size);
+ if (error == NO_MEMORY) { return NO_MEMORY; }
+
+ for (it = 0; it < 4; it++) {
+ // printf("G[%d] = %d\n\r", it, G[it]);
+ G[it] /= 255; // to keep values in range
+ // printf("G[%d] = %d\n\r", it, G[it]);
+ }
+ // calculate G's determinant:
+ Det = G[0] * G[3] - G[1] * G[2];
+ //printf("Det = %d\n\r", Det);
+ Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units
+ //printf("Det = %d\n\r", Det);
+ if (Det < 1) {
+ status[p] = 0;
+// printf("irrevertible G\n");
+ }
+
+ // (4) iterate over taking steps in the image to minimize the error:
+ it = 0;
+ step_threshold = 2; // 0.2 as smallest step (L1)
+ v_x = 0;
+ v_y = 0;
+ step_x = step_threshold + 1;
+ step_y = step_threshold + 1;
+
+ while (status[p] == 1 && it < max_iterations && (abs(step_x) >= step_threshold || abs(step_y) >= step_threshold)) {
+ //printf("it = %d, (p_x+v_x,p_y+v_y) = (%d,%d)\n\r", it, p_x[p]+v_x, p_y[p]+v_y);
+ //printf("it = %d;", it);
+ // if the pixel goes outside the ROI in the image, stop tracking:
+ if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor) && p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor && p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor) && p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) {
+// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]);
+ status[p] = 0;
+ break;
+ }
+
+ // [a] get the subpixel neighborhood in the new image
+ // clear J:
+ for (x = 0; x < patch_size; x++) {
+ for (y = 0; y < patch_size; y++) {
+ ix2 = (y * patch_size + x);
+ J_neighborhood[ix2] = 0;
+ }
+ }
+
+
+ getSubPixel_gray(J_neighborhood, new_image_buf, p_x[p] + v_x, p_y[p] + v_y, half_window_size, subpixel_factor);
+ // [b] determine the image difference between the two neighborhoods
+ //printf("I = ");
+ //printIntMatrix(I_neighborhood, patch_size, patch_size);
+ //printf("J = ");
+ //printIntMatrix(J_neighborhood, patch_size, patch_size);
+ //getSubPixel(J_neighborhood, new_image_buf, subpixel_factor * ((p_x[p]+v_x)/subpixel_factor), subpixel_factor * ((p_y[p]+v_y) / subpixel_factor), half_window_size, subpixel_factor);
+ //printf("J2 = ");
+ //printIntMatrix(J_neighborhood, patch_size, patch_size);
+ //printf("figure(); subplot(1,2,1); imshow(I/255); subplot(1,2,2); imshow(J/255);\n\r");
+ getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size);
+ //printf("ImDiff = ");
+ //printIntMatrix(ImDiff, patch_size, patch_size);
+ error = calculateError(ImDiff, patch_size, patch_size) / 255;
+
+// if(error > error_threshold) printf("error threshold\n");
+ if (error > error_threshold && it > max_iterations / 2) {
+ status[p] = 0;
+// printf("occlusion\n");
+ break;
+ }
+ //printf("error(%d) = %d;\n\r", it+1, error);
+ // [c] calculate the 'b'-vector
+ //printf("DX = ");
+ //printIntMatrix(DX, patch_size, patch_size);
+ multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size);
+ //printf("IDDX = ");
+ //printIntMatrix(IDDX, patch_size, patch_size);
+ multiplyImages(ImDiff, DY, IDDY, patch_size, patch_size);
+ //printf("DY = ");
+ //printIntMatrix(DY, patch_size, patch_size);
+ //printf("IDDY = ");
+ //printIntMatrix(IDDY, patch_size, patch_size);
+ //printf("figure(); subplot(2,3,1); imagesc(ImDiff); subplot(2,3,2); imagesc(DX); subplot(2,3,3); imagesc(DY);");
+ //printf("subplot(2,3,4); imagesc(IDDY); subplot(2,3,5); imagesc(IDDX);\n\r");
+ // division by 255 to keep values in range:
+ b_x = getSumPatch(IDDX, patch_size) / 255;
+ b_y = getSumPatch(IDDY, patch_size) / 255;
+ //printf("b_x = %d; b_y = %d;\n\r", b_x, b_y);
+ // [d] calculate the additional flow step and possibly terminate the iteration
+ step_x = (G[3] * b_x - G[1] * b_y) / Det;
+ step_y = (G[0] * b_y - G[2] * b_x) / Det;
+ v_x += step_x;
+ v_y += step_y; // - (?) since the origin in the image is in the top left of the image, with y positive pointing down
+ //printf("step = [%d,%d]; v = [%d,%d];\n\r", step_x, step_y, v_x, v_y);
+ //printf("pause(0.5);\n\r");
+ // next iteration
+ it++;
+ // step_size = abs(step_x);
+ // step_size += abs(step_y);
+ //printf("status = %d, it = %d, step_size = %d\n\r", status[p], it, step_size);
+ } // iteration to find the right window in the new image
+
+ //printf("figure(); plot(error(1:(it+1)));\n\r");
+// printf("it = %d\n",it);
+ new_x[p] = (p_x[p] + v_x) / subpixel_factor;
+ new_y[p] = (p_y[p] + v_y) / subpixel_factor;
+ p_x[p] /= subpixel_factor;
+ p_y[p] /= subpixel_factor;
+ }
+
+
+
+ // free all allocated variables:
+ free((int *) I_padded_neighborhood);
+ free((int *) I_neighborhood);
+ free((int *) J_neighborhood);
+ free((int *) DX);
+ free((int *) DY);
+ free((int *) ImDiff);
+ free((int *) IDDX);
+ free((int *) IDDY);
+ // no errors:
+ return OK;
+}
+
+void quick_sort(float *a, int n)
+{
+ if (n < 2) {
+ return;
+ }
+ float p = a[n / 2];
+ float *l = a;
+ float *r = a + n - 1;
+ while (l <= r) {
+ if (*l < p) {
+ l++;
+ continue;
+ }
+ if (*r > p) {
+ r--;
+ continue; // we need to check the condition (l <= r) every time we change the value of l or r
+ }
+ float t = *l;
+ *l++ = *r;
+ *r-- = t;
+ }
+ quick_sort(a, r - a + 1);
+ quick_sort(l, a + n - l);
+}
+
+void quick_sort_int(int *a, int n)
+{
+ if (n < 2) {
+ return;
+ }
+ int p = a[n / 2];
+ int *l = a;
+ int *r = a + n - 1;
+ while (l <= r) {
+ if (*l < p) {
+ l++;
+ continue;
+ }
+ if (*r > p) {
+ r--;
+ continue;
+ }
+ int t = *l;
+ *l++ = *r;
+ *r-- = t;
+ }
+ quick_sort_int(a, r - a + 1);
+ quick_sort_int(l, a + n - l);
+}
+
+void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH)
+{
+ int x, y;
+ unsigned char *Y, *gray;
+ for (y = 0; y < imH; y++) {
+ Y = frame + (imW * 2 * y) + 1;
+ gray = grayframe + (imW * y);
+ for (x = 0; x < imW; x += 2) {
+ gray[x] = *Y;
+ Y += 2;
+ gray[x + 1] = *Y;
+ Y += 2;
+ }
+ }
+}
+
+unsigned int OF_buf_point = 0;
+unsigned int OF_buf_point2 = 0;
+float x_avg, y_avg, x_buf[24], y_buf[24], x_buf2[24], y_buf2[24];
+
+void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType)
+{
+
+ if (OF_FilterType == 1) { //1. moving average 2. moving median
+
+ x_avg = 0.0;
+ y_avg = 0.0;
+
+ if (count) {
+ x_buf[OF_buf_point] = dx;
+ y_buf[OF_buf_point] = dy;
+ } else {
+ x_buf[OF_buf_point] = 0.0;
+ y_buf[OF_buf_point] = 0.0;
+ }
+ OF_buf_point = (OF_buf_point + 1) % 20;
+
+ for (int i = 0; i < 20; i++) {
+ x_avg += x_buf[i] * 0.05;
+ y_avg += y_buf[i] * 0.05;
+ }
+
+ *OFx = x_avg;
+ *OFy = y_avg;
+
+ } else if (OF_FilterType == 2) {
+ if (count) {
+ x_buf2[OF_buf_point2] = dx;
+ y_buf2[OF_buf_point2] = dy;
+ } else {
+ x_buf2[OF_buf_point2] = 0.0;
+ y_buf2[OF_buf_point2] = 0.0;
+ }
+ OF_buf_point2 = (OF_buf_point2 + 1) % 11; // 11
+
+ quick_sort(x_buf2, 11); // 11
+ quick_sort(y_buf2, 11); // 11
+
+ *OFx = x_buf2[6]; // 6
+ *OFy = y_buf2[6]; // 6
+ } else {
+ printf("no filter type selected!\n");
+ }
+}
+
diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h
new file mode 100644
index 00000000000..14cc568efe5
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2014 Hann Woei Ho
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/*
+ * @file paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_gdc.h
+ * @brief optical-flow based hovering for Parrot AR.Drone 2.0
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+#ifndef OPTIC
+#define OPTIC
+void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height);
+void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height);
+void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, int subpixel_factor);
+void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size);
+int getSumPatch(int *Patch, int size);
+int calculateG(int *G, int *DX, int *DY, int half_window_size);
+int calculateError(int *ImC, int width, int height);
+int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations);
+void quick_sort(float *a, int n);
+void quick_sort_int(int *a, int n);
+void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH);
+void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType);
+#endif
diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h
index e2bbf7096ff..22f06888cf6 100644
--- a/sw/airborne/modules/computer_vision/cv/resize.h
+++ b/sw/airborne/modules/computer_vision/cv/resize.h
@@ -36,13 +36,13 @@ inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int
*dest++ = *source++; // U
*dest++ = *source++; // Y
// now skip 3 pixels
- if (pixelskip) { source += (pixelskip + 1) * 2; }
+ source += (pixelskip + 1) * 2;
*dest++ = *source++; // U
*dest++ = *source++; // V
- if (pixelskip) { source += (pixelskip - 1) * 2; }
+ source += (pixelskip - 1) * 2;
}
// skip 3 rows
- if (pixelskip) { source += pixelskip * input->w * 2; }
+ source += pixelskip * input->w * 2;
}
}
diff --git a/sw/airborne/modules/computer_vision/cv/trig.c b/sw/airborne/modules/computer_vision/cv/trig.c
new file mode 100644
index 00000000000..47110a216ff
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/trig.c
@@ -0,0 +1,158 @@
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
+ * Trigonometry
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
+#include "trig.h"
+
+static int cosine[] = {
+ 10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925, 9903, 9877,
+ 9848, 9816, 9781, 9744, 9703, 9659, 9613, 9563, 9511, 9455,
+ 9397, 9336, 9272, 9205, 9135, 9063, 8988, 8910, 8829, 8746,
+ 8660, 8572, 8480, 8387, 8290, 8192, 8090, 7986, 7880, 7771,
+ 7660, 7547, 7431, 7314, 7193, 7071, 6947, 6820, 6691, 6561,
+ 6428, 6293, 6157, 6018, 5878, 5736, 5592, 5446, 5299, 5150,
+ 5000, 4848, 4695, 4540, 4384, 4226, 4067, 3907, 3746, 3584,
+ 3420, 3256, 3090, 2924, 2756, 2588, 2419, 2250, 2079, 1908,
+ 1736, 1564, 1392, 1219, 1045, 872, 698, 523, 349, 175,
+ 0
+};
+
+int sin_zelf(int ix)
+{
+ while (ix < 0) {
+ ix = ix + 360;
+ }
+ while (ix >= 360) {
+ ix = ix - 360;
+ }
+ if (ix < 90) { return cosine[90 - ix] / 10; }
+ if (ix < 180) { return cosine[ix - 90] / 10; }
+ if (ix < 270) { return -cosine[270 - ix] / 10; }
+ if (ix < 360) { return -cosine[ix - 270] / 10; }
+ return 0;
+}
+
+int cos_zelf(int ix)
+{
+ while (ix < 0) {
+ ix = ix + 360;
+ }
+ while (ix >= 360) {
+ ix = ix - 360;
+ }
+ if (ix < 90) { return cosine[ix] / 10; }
+ if (ix < 180) { return -cosine[180 - ix] / 10; }
+ if (ix < 270) { return -cosine[ix - 180] / 10; }
+ if (ix < 360) { return cosine[360 - ix] / 10; }
+ return 0;
+}
+
+int tan_zelf(int ix)
+{
+
+ while (ix < 0) {
+ ix = ix + 360;
+ }
+ while (ix >= 360) {
+ ix = ix - 360;
+ }
+ if (ix == 90) { return 9999; }
+ if (ix == 270) { return -9999; }
+ if (ix < 90) { return (1000 * cosine[90 - ix]) / cosine[ix]; }
+ if (ix < 180) { return -(1000 * cosine[ix - 90]) / cosine[180 - ix]; }
+ if (ix < 270) { return (1000 * cosine[270 - ix]) / cosine[ix - 180]; }
+ if (ix < 360) { return -(1000 * cosine[ix - 270]) / cosine[360 - ix]; }
+ return 0;
+}
+
+int asin_zelf(int y, int hyp)
+{
+ int quot, sgn, ix;
+ if ((y > hyp) || (y == 0)) {
+ return 0;
+ }
+ sgn = hyp * y;
+ if (hyp < 0) {
+ hyp = -hyp;
+ }
+ if (y < 0) {
+ y = -y;
+ }
+ quot = (y * 10000) / hyp;
+ if (quot > 9999) {
+ quot = 9999;
+ }
+ for (ix = 0; ix < 90; ix++)
+ if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) {
+ break;
+ }
+ if (sgn < 0) {
+ return -(90 - ix);
+ } else {
+ return 90 - ix;
+ }
+}
+
+int acos_zelf(int x, int hyp)
+{
+ int quot, sgn, ix;
+ if (x > hyp) {
+ return 0;
+ }
+ if (x == 0) {
+ if (hyp < 0) {
+ return -90;
+ } else {
+ return 90;
+ }
+ return 0;
+ }
+ sgn = hyp * x;
+ if (hyp < 0) {
+ hyp = -hyp;
+ }
+ if (x < 0) {
+ x = -x;
+ }
+ quot = (x * 10000) / hyp;
+ if (quot > 9999) {
+ quot = 9999;
+ }
+ for (ix = 0; ix < 90; ix++)
+ if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) {
+ break;
+ }
+ if (sgn < 0) {
+ return -ix;
+ } else {
+ return ix;
+ }
+}
+
+//atan_zelf(y/x) in degrees
+int atan_zelf(int y, int x)
+{
+ int angle, flip, t, xy;
+
+ if (x < 0) { x = -x; }
+ if (y < 0) { y = -y; }
+ flip = 0; if (x < y) { flip = 1; t = x; x = y; y = t; }
+ if (x == 0) { return 90; }
+
+ xy = (y * 1000) / x;
+ angle = (360 * xy) / (6283 + ((((1764 * xy) / 1000) * xy) / 1000));
+ if (flip) { angle = 90 - angle; }
+ return angle;
+}
+
+unsigned int isqrt(unsigned int val)
+{
+ unsigned int temp, g = 0, b = 0x8000, bshft = 15;
+ do {
+ if (val >= (temp = (((g << 1) + b) << bshft--))) {
+ g += b;
+ val -= temp;
+ }
+ } while (b >>= 1);
+ return g;
+}
diff --git a/sw/airborne/modules/computer_vision/cv/trig.h b/sw/airborne/modules/computer_vision/cv/trig.h
new file mode 100644
index 00000000000..9603d41789c
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/trig.h
@@ -0,0 +1,7 @@
+int cos_zelf(int ix);
+int sin_zelf(int);
+int tan_zelf(int);
+int acos_zelf(int, int);
+int asin_zelf(int, int);
+int atan_zelf(int, int);
+unsigned int isqrt(unsigned int);
diff --git a/sw/airborne/modules/computer_vision/lib/paparazzi.h b/sw/airborne/modules/computer_vision/lib/paparazzi.h
new file mode 100644
index 00000000000..f3b2aef8f13
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/paparazzi.h
@@ -0,0 +1,32 @@
+
+////////////////////////////////////////////////
+// Paparazzi communication interface
+
+#include // pthread_create
+#include // gprint
+
+#include "socket.h"
+#include "video_message_structs.h"
+
+struct gst2ppz_message_struct gst2ppz;
+struct ppz2gst_message_struct ppz2gst;
+
+inline void paparazzi_message_server_start(void);
+inline void paparazzi_message_send(void);
+
+struct UdpSocket *sock;
+
+inline void paparazzi_message_server_start(void)
+{
+ sock = udp_socket("192.168.1.1", 2000, 2001, FMS_UNICAST);
+}
+
+
+inline void paparazzi_message_send(void)
+{
+ udp_write(sock, (char *) &gst2ppz, sizeof(gst2ppz));
+ int ret = udp_read(sock, (unsigned char *) &ppz2gst, sizeof(ppz2gst));
+ printf("read: %d \n",ret);
+}
+
+
diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.c b/sw/airborne/modules/computer_vision/lib/tcp/socket.c
new file mode 100644
index 00000000000..9479c698fad
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/tcp/socket.c
@@ -0,0 +1,139 @@
+#include "socket.h"
+#include /* socket definitions */
+#include /* socket types */
+#include /* inet (3) funtions */
+#include /* misc. UNIX functions */
+#include
+#include /* memset */
+
+#include
+#include
+
+
+
+
+/* Global constants */
+
+#define ECHO_PORT (2002)
+#define MAX_LINE (1000)
+
+/* Global variables */
+int list_s; /* listening socket */
+int conn_s; /* connection socket */
+struct sockaddr_in servaddr; /* socket address structure */
+char buffer[MAX_LINE]; /* character buffer */
+char *endptr; /* for strtol() */
+
+int closeSocket(void)
+{
+ return close(conn_s);
+}
+
+int initSocket(void)
+{
+
+ /* Create the listening socket */
+
+ if ((list_s = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+ fprintf(stderr, "ECHOSERV: Error creating listening socket.\n");
+ return -1;
+ }
+
+
+ /* Set all bytes in socket address structure to
+ zero, and fill in the relevant data members */
+
+ memset(&servaddr, 0, sizeof(servaddr));
+ servaddr.sin_family = AF_INET;
+ servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+ servaddr.sin_port = htons(ECHO_PORT);
+
+ /* Bind our socket addresss to the
+ listening socket, and call listen() */
+
+ if (bind(list_s, (struct sockaddr *) &servaddr, sizeof(servaddr)) < 0) {
+ fprintf(stderr, "ECHOSERV: Error calling bind()\n");
+ exit(EXIT_FAILURE);
+ }
+
+ if (listen(list_s, LISTENQ) < 0) {
+ fprintf(stderr, "ECHOSERV: Error calling listen()\n");
+ exit(EXIT_FAILURE);
+ }
+
+
+
+ /* Wait for a connection, then accept() it */
+ if ((conn_s = accept(list_s, NULL, NULL)) < 0) {
+ fprintf(stderr, "ECHOSERV: Error calling function accept()\n");
+ return -1;
+ }
+ printf("Connected!\n");
+ return 1;
+}
+
+/* Read a line from a socket */
+
+ssize_t Readline_socket(void *vptr, size_t maxlen)
+{
+ ssize_t n, rc;
+ char c, *buffer;
+
+ buffer = vptr;
+
+ for (n = 1; n < maxlen; n++) {
+
+ if ((rc = read(conn_s, &c, 1)) == 1) {
+ *buffer++ = c;
+ if (c == '\n') {
+ break;
+ }
+ } else if (rc == 0) {
+ if (n == 1) {
+ return 0;
+ } else {
+ break;
+ }
+ } else {
+ if (errno == EINTR) {
+ continue;
+ }
+ return -1;
+ }
+ }
+
+ *buffer = 0;
+ return n;
+}
+
+
+/* Write a line to a socket */
+
+ssize_t Writeline_socket(struct img_struct *img, size_t n)
+{
+ size_t nleft;
+ ssize_t nwritten;
+ unsigned char *buf1 = img->buf;
+ nleft = n;
+
+ //printf("socket buf1 %d, %d, %d, %d, %d, %d\n",buf1[0],buf1[1],buf1[2],buf1[3],buf1[4],buf1[5]);
+
+ while (nleft > 0) {
+ if ((nwritten = write(conn_s, buf1, nleft)) <= 0) {
+ if (errno == EINTR) {
+ nwritten = 0;
+ } else {
+ return -1;
+ }
+ }
+ nleft -= nwritten;
+ buf1 += nwritten;
+ }
+
+ return n;
+
+}
+
+
+
+
diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.h b/sw/airborne/modules/computer_vision/lib/tcp/socket.h
new file mode 100644
index 00000000000..04091cd4848
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/lib/tcp/socket.h
@@ -0,0 +1,17 @@
+#ifndef SOCKET_H
+#define SOCKET_H
+
+#include /* for ssize_t data type */
+#include "./video/video.h"
+
+#define LISTENQ (1024) /* Backlog for listen() */
+
+
+/* Function declarations */
+int initSocket(void) ;
+ssize_t Readline(void *vptr, size_t maxlen);
+ssize_t Writeline(struct img_struct *img, size_t maxlen);
+int closeSocket(void);
+
+#endif /* SOCKET_H */
+
diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.c b/sw/airborne/modules/computer_vision/lib/udp/socket.c
index fba489e2828..34d73e2023b 100644
--- a/sw/airborne/modules/computer_vision/lib/udp/socket.c
+++ b/sw/airborne/modules/computer_vision/lib/udp/socket.c
@@ -22,8 +22,7 @@
# define SOCKET_ERROR -1
# define IO_SOCKET ioctl
-struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast)
-{
+struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) {
struct UdpSocket *me = malloc(sizeof(struct UdpSocket));
diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c
new file mode 100644
index 00000000000..e69de29bb2d
diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h
new file mode 100644
index 00000000000..e69de29bb2d
diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.c b/sw/airborne/modules/computer_vision/lib/v4l/video.c
index e0a9fb5c318..b3a090ced43 100644
--- a/sw/airborne/modules/computer_vision/lib/v4l/video.c
+++ b/sw/airborne/modules/computer_vision/lib/v4l/video.c
@@ -43,10 +43,10 @@
pthread_t video_thread;
-void *video_thread_main(void *data);
-void *video_thread_main(void *data)
+void *video_thread_main(void* data);
+void *video_thread_main(void* data)
{
- struct vid_struct *vid = (struct vid_struct *)data;
+ struct vid_struct* vid = (struct vid_struct*)data;
printf("video_thread_main started\n");
while (1) {
fd_set fds;
@@ -61,7 +61,7 @@ void *video_thread_main(void *data)
r = select(vid->fd + 1, &fds, NULL, NULL, &tv);
if (-1 == r) {
- if (EINTR == errno) { continue; }
+ if (EINTR == errno) continue;
printf("select err\n");
}
@@ -84,13 +84,13 @@ void *video_thread_main(void *data)
vid->seq++;
- if (vid->trigger) {
+ if(vid->trigger) {
// todo add timestamp again
//vid->img->timestamp = util_timestamp();
vid->img->seq = vid->seq;
- memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w * vid->h * 2);
- vid->trigger = 0;
+ memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2);
+ vid->trigger=0;
}
if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) {
@@ -108,9 +108,9 @@ int video_init(struct vid_struct *vid)
unsigned int i;
enum v4l2_buf_type type;
- vid->seq = 0;
- vid->trigger = 0;
- if (vid->n_buffers == 0) { vid->n_buffers = 4; }
+ vid->seq=0;
+ vid->trigger=0;
+ if(vid->n_buffers==0) vid->n_buffers=4;
vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0);
@@ -148,7 +148,7 @@ int video_init(struct vid_struct *vid)
printf("Buffer count = %d\n", vid->n_buffers);
- vid->buffers = (struct buffer_struct *)calloc(vid->n_buffers, sizeof(struct buffer_struct));
+ vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct));
for (i = 0; i < vid->n_buffers; ++i) {
struct v4l2_buffer buf;
@@ -164,11 +164,11 @@ int video_init(struct vid_struct *vid)
}
vid->buffers[i].length = buf.length;
- printf("buffer%d.length=%d\n", i, buf.length);
- vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset);
+ printf("buffer%d.length=%d\n",i,buf.length);
+ vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset);
if (MAP_FAILED == vid->buffers[i].buf) {
- printf("mmap() failed.\n");
+ printf ("mmap() failed.\n");
return -1;
}
}
@@ -188,14 +188,14 @@ int video_init(struct vid_struct *vid)
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (ioctl(vid->fd, VIDIOC_STREAMON, &type) < 0) {
+ if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) {
printf("ioctl() VIDIOC_STREAMON failed.\n");
return -1;
}
//start video thread
int rc = pthread_create(&video_thread, NULL, video_thread_main, vid);
- if (rc) {
+ if(rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
return 202;
}
@@ -207,28 +207,27 @@ void video_close(struct vid_struct *vid)
{
int i;
for (i = 0; i < (int)vid->n_buffers; ++i) {
- if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) { printf("munmap() failed.\n"); }
+ if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n");
}
close(vid->fd);
}
struct img_struct *video_create_image(struct vid_struct *vid)
{
- struct img_struct *img = (struct img_struct *)malloc(sizeof(struct img_struct));
- img->w = vid->w;
- img->h = vid->h;
- img->buf = (unsigned char *)malloc(vid->h * vid->w * 2);
+ struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct));
+ img->w=vid->w;
+ img->h=vid->h;
+ img->buf = (unsigned char*)malloc(vid->h*vid->w*2);
return img;
}
pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER;
-void video_grab_image(struct vid_struct *vid, struct img_struct *img)
-{
+void video_grab_image(struct vid_struct *vid, struct img_struct *img) {
pthread_mutex_lock(&video_grab_mutex);
vid->img = img;
- vid->trigger = 1;
+ vid->trigger=1;
// while(vid->trigger) pthread_yield();
- while (vid->trigger) { usleep(1); }
+ while(vid->trigger) usleep(1);
pthread_mutex_unlock(&video_grab_mutex);
}
diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h
index 351ba68522e..125892ad1b9 100644
--- a/sw/airborne/modules/computer_vision/lib/v4l/video.h
+++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h
@@ -25,7 +25,7 @@
#include "../../cv/image.h"
struct buffer_struct {
- void *buf;
+ void * buf;
size_t length;
};
@@ -39,7 +39,7 @@ struct vid_struct {
//private members
int trigger;
struct img_struct *img;
- struct buffer_struct *buffers;
+ struct buffer_struct * buffers;
int fd;
};