line.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <utils/math/angle.h>
00027 #include <models/shape/line.h>
00028
00029
00030
00031
00032 using namespace std;
00033
00034 namespace firevision {
00035 #if 0
00036 }
00037 #endif
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 LineShape::LineShape(unsigned int roi_width, unsigned int roi_height)
00048 {
00049 r = 0;
00050 phi = 0;
00051 count = 0;
00052
00053 max_length = (int)sqrt( roi_width * roi_width + roi_height * roi_height );
00054 last_calc_r = last_calc_phi = 0.f;
00055
00056 this->roi_width = roi_width;
00057 this->roi_height = roi_height;
00058
00059 }
00060
00061
00062
00063 LineShape::~LineShape()
00064 {
00065 }
00066
00067
00068
00069
00070
00071 void
00072 LineShape::printToStream(std::ostream &stream)
00073 {
00074 stream << "r=" << r << " phi=" << phi
00075 << " count= " << count;
00076 }
00077
00078 void
00079 LineShape::setMargin(unsigned int margin)
00080 {
00081 this->margin = margin;
00082 }
00083
00084
00085 bool
00086 LineShape::isClose(unsigned int in_roi_x, unsigned int in_roi_y)
00087 {
00088 return false;
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103 }
00104
00105
00106 void
00107 LineShape::calcPoints()
00108 {
00109
00110 if ((last_calc_r == r) && (last_calc_phi == phi)) return;
00111 last_calc_r = r;
00112 last_calc_phi = phi;
00113
00114 float rad_angle = fawkes::deg2rad(phi);
00115
00116
00117 bool reverse_direction = false;
00118
00119
00120
00121
00122 if ( rad_angle < M_PI/4 ) {
00123 x1 = (int)round( r * cos( rad_angle ) );
00124 y1 = (int)round( r * sin( rad_angle ) );
00125 y2 = 0;
00126 x2 = (int)round( r / cos( rad_angle ) );
00127 } else if ( rad_angle < M_PI/2 ) {
00128 x1 = (int)round( r * cos( rad_angle ) );
00129 y1 = (int)round( r * sin( rad_angle ) );
00130 x2 = 0;
00131 y2 = (int)round( r / cos( M_PI/2 - rad_angle ) );
00132 } else if ( rad_angle < 3.0/4.0 * M_PI ) {
00133 x1 = (int)round(-r * cos( M_PI - rad_angle ) );
00134 y1 = (int)round( r * sin( M_PI - rad_angle ) );
00135 x2 = 0;
00136 y2 = (int)round( r / cos( rad_angle - M_PI/2 ) );
00137
00138
00139
00140 if (r >= 0.0) {
00141 reverse_direction = true;
00142 }
00143 } else {
00144
00145 x1 = (int)round(-r * cos( M_PI - rad_angle ) );
00146 y1 = (int)round( r * sin( M_PI - rad_angle ) );
00147 y2 = 0;
00148 x2 = (int)round(-r / cos( M_PI - rad_angle ) );
00149
00150
00151
00152 if (r < 0.0) {
00153 reverse_direction = true;
00154 }
00155 }
00156
00157 if ( ! (x1 == x2 &&
00158 y1 == y2 ) ) {
00159
00160
00161 float vx, vy, length;
00162 vx = x1 - x2 ;
00163 vy = y1 - y2 ;
00164 length = sqrt( vx * vx + vy * vy );
00165
00166 vx /= length;
00167 vy /= length;
00168 vx *= max_length;
00169 vy *= max_length;
00170
00171 if ( ! reverse_direction) {
00172 x1 += (int)vx;
00173 y1 += (int)vy;
00174 } else {
00175 x1 -= (int)vx;
00176 y1 -= (int)vy;
00177 }
00178
00179 } else {
00180
00181
00182 if (x2 == 0) {
00183 x1 = roi_width;
00184 y1 = y2;
00185 } else if (y2 == 0) {
00186 x1 = x2;
00187 y1 = roi_height;
00188 } else {
00189 cout << "ERROR!" << endl
00190 << " This case should not have occurred. Please have a look at method" << endl
00191 << " \"LineShape::calc()\". Treatment of special case is not correct." << endl;
00192
00193 }
00194 }
00195 }
00196
00197
00198
00199
00200
00201
00202
00203
00204 void
00205 LineShape::getPoints(int *x1, int *y1, int *x2, int *y2)
00206 {
00207 calcPoints();
00208
00209 *x1 = this->x1;
00210 *y1 = this->y1;
00211 *x2 = this->x2;
00212 *y2 = this->y2;
00213 }
00214
00215 }