00001
00002
00003
00004
00005
00006
00007 #include "Configuration.h"
00008
00009 #include <cmath>
00010
00011 #ifdef UNIT_TEST_ALM_UT
00012 #warning Compiling unit test code for almUtility.cc
00013 #include <iostream>
00014 #endif
00015
00016
00017
00018 void dm_index2angles(int index, double &azimuth, double &altitude)
00019 {
00020 static const int width = ALM_DM_H_SIZE;
00021 static const int height = ALM_DM_V_SIZE;
00022
00023 static const double xsize = ALM_DM_RIGHT - ALM_DM_LEFT;
00024 static const double ysize = ALM_DM_BOTTOM -ALM_DM_TOP;
00025
00026 azimuth = ALM_DM_LEFT + xsize * (double)(index % width) / (double)width;
00027 altitude = ALM_DM_TOP + ysize * (double)(index / width) / (double)height;
00028 }
00029
00030
00031 bool angles2dm_index(double azimuth, double altitude, int &index)
00032 {
00033 static const int width = ALM_DM_H_SIZE;
00034 static const int height = ALM_DM_V_SIZE;
00035
00036 static const double xsize = ALM_DM_RIGHT - ALM_DM_LEFT;
00037 static const double ysize = ALM_DM_BOTTOM -ALM_DM_TOP;
00038
00039 double dx = azimuth - ALM_DM_LEFT;
00040 double dy = altitude - ALM_DM_TOP;
00041
00042 int xindex = (int) ((dx / xsize) * (double) width);
00043 int yindex = (int) ((dy / ysize) * (double) height);
00044
00045 if((xindex < 0) || (yindex < 0)) return false;
00046 if((xindex >= width) || (yindex >= height)) return false;
00047
00048 index = yindex*width + xindex;
00049 return true;
00050 }
00051
00052
00053
00054
00055 void hm_index2xy(int index, double &x, double &y)
00056 {
00057 static const int width = 2*ALM_HM_SIZE;
00058 static const int height = 2*ALM_HM_SIZE;
00059
00060 static const double xsize = 2*ALM_HM_RADIUS;
00061 static const double ysize = -2*ALM_HM_RADIUS;
00062
00063 x = (double) -ALM_HM_RADIUS +
00064 xsize * (double)(index % width) / (double)width;
00065 y = (double) ALM_HM_RADIUS +
00066 ysize * (double)(index / width) / (double)height;
00067 }
00068
00069
00070
00071 bool xy2hm_index(double x, double y, int &index)
00072 {
00073 static const int width = 2*ALM_HM_SIZE;
00074 static const int height = 2*ALM_HM_SIZE;
00075
00076 static const double xsize = 2*ALM_HM_RADIUS;
00077 static const double ysize = -2*ALM_HM_RADIUS;
00078
00079 double dx = x - -ALM_HM_RADIUS;
00080 double dy = y - ALM_HM_RADIUS;
00081
00082 int xindex = (int) ((dx / xsize) * (double) width);
00083 int yindex = (int) ((dy / ysize) * (double) height);
00084
00085 if((xindex < 0) || (yindex < 0)) return false;
00086 if((xindex >= width) || (yindex >= height)) return false;
00087
00088 index = yindex*width + xindex;
00089 return true;
00090 }
00091
00092
00093
00094 void gm_index2xy(int index, double &x, double &y)
00095 {
00096 static const int width = AGM_H_SIZE;
00097 static const int height = AGM_V_SIZE;
00098
00099 static const double xsize = (double) AGM_RIGHT - (double) AGM_LEFT;
00100 static const double ysize = (double) AGM_BOTTOM - (double) AGM_TOP;
00101
00102 x = (double) AGM_LEFT + xsize * (double)(index % width) / (double)width;
00103 y = (double) AGM_TOP + ysize * (double)(index / width) / (double)height;
00104 }
00105
00106
00107
00108 bool xy2gm_index(double x, double y, int &index)
00109 {
00110 static const int width = AGM_H_SIZE;
00111 static const int height = AGM_V_SIZE;
00112
00113 static const double xsize = (double) AGM_RIGHT - (double) AGM_LEFT;
00114 static const double ysize = (double) AGM_BOTTOM - (double) AGM_TOP;
00115
00116 double dx = x - AGM_LEFT;
00117 double dy = y - AGM_TOP;
00118
00119 int xindex = (int) ((dx / xsize) * (double) width);
00120 int yindex = (int) ((dy / ysize) * (double) height);
00121
00122 if((xindex < 0) || (yindex < 0)) return false;
00123 if((xindex >= width) || (yindex >= height)) return false;
00124
00125 index = yindex*width + xindex;
00126 return true;
00127 }
00128
00129
00130
00131
00132
00133
00134 void head_range2xyz(double depth, double pan, double tilt,
00135 double &x, double &y, double &z)
00136 {
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149 double sinntilt = sin(-tilt);
00150 double cosntilt = cos(-tilt);
00151 double sinnpan = sin(-pan);
00152 double cosnpan = cos(-pan);
00153
00154 double t11 = cosntilt*cosnpan;
00155 double t31 = -sinntilt*cosnpan;
00156 double t14 = AIBO_HEAD_LENGTH*cosntilt*cosnpan + AIBO_NECK_HEIGHT*sinntilt;
00157 double t24 = -AIBO_HEAD_LENGTH*sinnpan;
00158 double t34 = -AIBO_HEAD_LENGTH*sinntilt*cosnpan + AIBO_NECK_HEIGHT*cosntilt +
00159 AIBO_TILT_PIVOT_HEIGHT;
00160
00161 x = t11*depth + t14;
00162 y = -sinnpan*depth + t24;
00163 z = t31*depth + t34;
00164 }
00165
00166
00167
00168
00169 void xyz2neck_range(double x, double y, double z,
00170 double &depth, double &azimuth, double &altitude)
00171 {
00172 double dz = z - AIBO_TILT_PIVOT_HEIGHT;
00173
00174 depth = sqrt(x*x + y*y + dz*dz);
00175 azimuth = atan2(y, x);
00176 altitude = asin(dz/depth);
00177 }
00178
00179
00180
00181 void neck_range2xyz(double depth, double azimuth, double altitude,
00182 double &x, double &y, double &z)
00183 {
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196 double sinnaz = sin(-azimuth);
00197 double cosnaz = cos(-azimuth);
00198 double sinnalt = sin(-altitude);
00199 double cosnalt = cos(-altitude);
00200
00201 double t11 = cosnalt*cosnaz;
00202 double t31 = -sinnalt*cosnaz;
00203
00204 x = t11*depth;
00205 y = -sinnaz*depth;
00206 z = t31*depth + AIBO_TILT_PIVOT_HEIGHT;
00207 }
00208
00209 #ifdef UNIT_TEST_ALM_UT
00210
00211 int main(int argc, char **argv)
00212 {
00213 int index;
00214 double az, alt, depth, x, y, z;
00215
00216 using namespace std;
00217
00218 cout << " Mismatches of 1 or of " << ALM_DM_H_SIZE << "+-1 are probably OK"
00219 << endl;
00220 for(int i=0;
00221 i < ALM_DM_V_SIZE*ALM_DM_H_SIZE;
00222 i += ALM_DM_V_SIZE*ALM_DM_H_SIZE/13) {
00223 cout << "dm_index2angles(" << i <<", az, alt)";
00224 dm_index2angles(i, az, alt);
00225 cout << "\taz = " << az << "\talt = " << alt << endl;
00226
00227 cout << "angles2dm_index(az, alt, index)";
00228 angles2dm_index(az, alt, index);
00229 cout << "\tindex = " << index;
00230
00231 if(index != i) cout << "\t***MISMATCH***";
00232
00233 cout << endl;
00234 }
00235 cout << endl << endl;
00236
00237 for(int i=0;
00238 i < 4*ALM_HM_SIZE*ALM_HM_SIZE;
00239 i += 4*ALM_HM_SIZE*ALM_HM_SIZE/13) {
00240 cout << "hm_index2xy(" << i <<", x, y)";
00241 hm_index2xy(i, x, y);
00242 cout << "\tx = " << x << "\ty = " << y << endl;
00243
00244 cout << "xy2hm_index(x, y, index)";
00245 xy2hm_index(x, y, index);
00246 cout << "\tindex = " << index;
00247
00248 if(index != i) cout << "\t***MISMATCH***";
00249
00250 cout << endl;
00251 }
00252 cout << endl << endl;
00253
00254 for(int i=0;
00255 i < AGM_V_SIZE*AGM_H_SIZE;
00256 i += AGM_V_SIZE*AGM_H_SIZE/13) {
00257 cout << "gm_index2xy(" << i <<", x, y)";
00258 gm_index2xy(i, x, y);
00259 cout << "\tx = " << x << "\ty = " << y << endl;
00260
00261 cout << "xy2gm_index(x, y, index)";
00262 xy2gm_index(x, y, index);
00263 cout << "\tindex = " << index;
00264
00265 if(index != i) cout << "\t***MISMATCH***";
00266
00267 cout << endl;
00268 }
00269 cout << endl << endl;
00270
00271 cout << "head_range2xyz(200, 0, 0, x, y, z)";
00272 head_range2xyz(200, 0, 0, x, y, z);
00273 cout << "\tx = " << x << " y = " << y << " z = " << z << endl;
00274 cout << "head_range2xyz(200, R(45), 0, x, y, z)";
00275 head_range2xyz(200, R(45), 0, x, y, z);
00276 cout << "\tx = " << x << " y = " << y << " z = " << z << endl;
00277 cout << "head_range2xyz(200, 0, R(45), x, y, z)";
00278 head_range2xyz(200, 0, R(45), x, y, z);
00279 cout << "\tx = " << x << " y = " << y << " z = " << z << endl;
00280 cout << "head_range2xyz(200, R(45), R(45), x, y, z)";
00281 head_range2xyz(200, R(45), R(45), x, y, z);
00282 cout << "\tx = " << x << " y = " << y << " z = " << z << endl;
00283 cout << endl << endl;
00284
00285 cout << "neck_range2xyz(200, 0, 0, x, y, z)";
00286 neck_range2xyz(200, 0, 0, x, y, z);
00287 cout << "\tx = " << x << " y = " << y << " z = " << z << endl;
00288 cout << "xyz2neck_range(x, y, z, depth, az, alt)";
00289 xyz2neck_range(x, y, z, depth, az, alt);
00290 cout << "\tdepth = " << depth << " az = " << az << " alt = " << alt << endl;
00291 cout << "neck_range2xyz(200, R(45), 0, x, y, z)";
00292 neck_range2xyz(200, R(45), 0, x, y, z);
00293 cout << "\tx = " << x << " y = " << y << " z = " << z << endl;
00294 cout << "xyz2neck_range(x, y, z, depth, az, alt)";
00295 xyz2neck_range(x, y, z, depth, az, alt);
00296 cout << "\tdepth = " << depth << " az = " << az << " alt = " << alt << endl;
00297 cout << "neck_range2xyz(200, 0, R(45), x, y, z)";
00298 neck_range2xyz(200, 0, R(45), x, y, z);
00299 cout << "\tx = " << x << " y = " << y << " z = " << z << endl;
00300 cout << "xyz2neck_range(x, y, z, depth, az, alt)";
00301 xyz2neck_range(x, y, z, depth, az, alt);
00302 cout << "\tdepth = " << depth << " az = " << az << " alt = " << alt << endl;
00303
00304 return 0;
00305 }
00306
00307 #endif