*** empty log message ***
[folded-ctf.git] / pose.cc
1
2 ///////////////////////////////////////////////////////////////////////////
3 // This program is free software: you can redistribute it and/or modify  //
4 // it under the terms of the version 3 of the GNU General Public License //
5 // as published by the Free Software Foundation.                         //
6 //                                                                       //
7 // This program is distributed in the hope that it will be useful, but   //
8 // WITHOUT ANY WARRANTY; without even the implied warranty of            //
9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      //
10 // General Public License for more details.                              //
11 //                                                                       //
12 // You should have received a copy of the GNU General Public License     //
13 // along with this program. If not, see <http://www.gnu.org/licenses/>.  //
14 //                                                                       //
15 // Written by Francois Fleuret, (C) IDIAP                                //
16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
17 ///////////////////////////////////////////////////////////////////////////
18
19 #include <string.h>
20
21 #include "pose.h"
22
23 Pose::Pose() {
24   memset(this, 0, sizeof(*this));
25 }
26
27 void Pose::horizontal_flip(scalar_t scene_width) {
28   _head_xc = scene_width - 1 - _head_xc;
29   _belly_xc = scene_width - 1 - _belly_xc;
30 }
31
32 const scalar_t tolerance_scale_ratio_for_hit = 1.5;
33 const scalar_t tolerance_distance_factor_for_hit = 1.0;
34
35 bool Pose::hit(int level, Pose *pose) {
36
37   // Head size
38
39   if(_head_radius/pose->_head_radius > tolerance_scale_ratio_for_hit ||
40      pose->_head_radius/_head_radius > tolerance_scale_ratio_for_hit)
41     return false;
42
43   scalar_t sq_delta = _head_radius * pose->_head_radius * sq(tolerance_distance_factor_for_hit);
44
45   // Head location
46
47   if(sq(_head_xc - pose->_head_xc) + sq(_head_yc - pose->_head_yc) > sq_delta)
48     return false;
49
50   if(level == 0) return true;
51
52   // Belly location
53
54   if(sq(_belly_xc - pose->_belly_xc) + sq(_belly_yc - pose->_belly_yc) > 4 * sq_delta)
55     return false;
56
57   if(level == 1) return true;
58
59   cerr << "Hit criterion is undefined for level " << level << endl;
60
61   abort();
62 }
63
64 const scalar_t tolerance_scale_ratio_for_collide = 1.2;
65 const scalar_t tolerance_distance_factor_for_collide = 0.25;
66
67 bool Pose::collide(int level, Pose *pose) {
68
69   // Head size
70
71   if(_head_radius/pose->_head_radius > tolerance_scale_ratio_for_collide ||
72      pose->_head_radius/_head_radius > tolerance_scale_ratio_for_collide)
73     return false;
74
75   scalar_t sq_delta = _head_radius * pose->_head_radius * sq(tolerance_distance_factor_for_collide);
76
77   // Head location
78
79   if(sq(_head_xc - pose->_head_xc) + sq(_head_yc - pose->_head_yc) <= sq_delta)
80     return true;
81
82   if(level == 0) return false;
83
84   // Belly location
85
86   if(sq(_belly_xc - pose->_belly_xc) + sq(_belly_yc - pose->_belly_yc) <= sq_delta)
87     return true;
88
89   if(level == 1) return false;
90
91   cerr << "Colliding criterion is undefined for level " << level << endl;
92
93   abort();
94 }
95
96 void Pose::draw(int thickness, int r, int g, int b, int level, RGBImage *image) {
97   // Draw the head circle
98
99   image->draw_ellipse(thickness, r, g, b,
100                       _head_xc, _head_yc, _head_radius, _head_radius, 0);
101
102   if(level == 1) {
103
104     scalar_t vx = _belly_xc - _head_xc, vy = _belly_yc - _head_yc;
105     scalar_t l = sqrt(vx * vx + vy * vy);
106     vx /= l;
107     vy /= l;
108
109     scalar_t belly_radius = 12 + thickness / 2;
110
111     if(l > _head_radius + thickness + belly_radius) {
112       image->draw_line(thickness, r, g, b,
113                        _head_xc + vx * (_head_radius + thickness/2),
114                        _head_yc + vy * (_head_radius + thickness/2),
115                        _belly_xc - vx * (belly_radius - thickness/2),
116                        _belly_yc - vy * (belly_radius - thickness/2));
117     }
118
119     // An ugly way to make a filled disc
120     for(scalar_t u = 0; u < belly_radius; u += thickness / 2) {
121       image->draw_ellipse(thickness, r, g, b, _belly_xc, _belly_yc, u, u, 0);
122     }
123
124   }
125 }
126
127 void Pose::print(ostream *out) {
128   (*out) << "  _head_xc " << _head_xc << endl;
129   (*out) << "  _head_yc " << _head_yc << endl;
130   (*out) << "  _head_radius " << _head_radius << endl;
131   (*out) << "  _belly_xc " << _belly_xc << endl;
132   (*out) << "  _belly_yc " << _belly_yc << endl;
133 }
134
135 void Pose::write(ostream *out) {
136   write_var(out, &_bounding_box_xmin);
137   write_var(out, &_bounding_box_ymin);
138   write_var(out, &_bounding_box_xmax);
139   write_var(out, &_bounding_box_ymax);
140   write_var(out, &_head_xc); write_var(out, &_head_yc);
141   write_var(out, &_head_radius);
142   write_var(out, &_belly_xc);
143   write_var(out, &_belly_yc);
144 }
145
146 void Pose::read(istream *in) {
147   read_var(in, &_bounding_box_xmin);
148   read_var(in, &_bounding_box_ymin);
149   read_var(in, &_bounding_box_xmax);
150   read_var(in, &_bounding_box_ymax);
151   read_var(in, &_head_xc); read_var(in, &_head_yc);
152   read_var(in, &_head_radius);
153   read_var(in, &_belly_xc);
154   read_var(in, &_belly_yc);
155 }