00001
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <stdio.h>
00037 #include "configuration.h"
00038 #include "source_type.h"
00039
00040
00041 Configuration::Configuration()
00042 {
00043 modified = 0;
00044
00045 name[0] = '\0';
00046 origin.set_ellipsoid("WGS 84");
00047
00048 num_types = 0;
00049 types = 0;
00050
00051 sensors = 0;
00052 sources = 0;
00053 materials = 0;
00054 }
00055
00056
00057 Configuration::Configuration(char *ellipsoid_type)
00058 {
00059 modified = 0;
00060
00061 name[0] = '\0';
00062 origin.set_ellipsoid(ellipsoid_type);
00063
00064 num_types = 0;
00065 types = 0;
00066
00067 sensors = 0;
00068 sources = 0;
00069 materials = 0;
00070 }
00071
00072
00073 Configuration::~Configuration()
00074 {
00075 clear();
00076 }
00077
00078
00079 void Configuration::clear()
00080 {
00081 Source_type_tree *t;
00082 Source_type *cur = types;
00083
00084 name[0] = '\0';
00085
00086 num_types = 0;
00087 while (cur != 0) {
00088 Source_type *tmp = cur;
00089 cur = cur->next;
00090 delete tmp;
00091 }
00092 types = 0;
00093
00094 nodes.burn();
00095
00096 t = sensors;
00097 while (t != 0) {
00098 Source_type_tree *tmp = t;
00099 t = t->next;
00100 delete tmp;
00101 }
00102 sensors = 0;
00103
00104 t = sources;
00105 while (t != 0) {
00106 Source_type_tree *tmp = t;
00107 t = t->next;
00108 delete tmp;
00109 }
00110 sources = 0;
00111
00112 obstructions.burn();
00113
00114 t = materials;
00115 while (t != 0) {
00116 Source_type_tree *tmp = t;
00117 t = t->next;
00118 delete tmp;
00119 }
00120 materials = 0;
00121 }
00122
00123
00124
00125 int Configuration::open(char *filename)
00126 {
00127 FILE *file;
00128
00129 if ((file = fopen(filename, "r")) == NULL) {
00130 perror(filename);
00131 return -1;
00132 }
00133 return read_file(file);
00134 }
00135
00136
00137
00138 int Configuration::save(char *filename)
00139 {
00140 FILE *file;
00141
00142 if ((file = fopen(filename, "w")) == NULL) {
00143 perror(filename);
00144 return -1;
00145 }
00146 return write_file(file);
00147 }
00148
00149
00150
00151 void Configuration::add_Source_type(char *type)
00152 {
00153 Source_type_tree *t;
00154 Source_type *iter = types;
00155
00156 while (iter != 0)
00157 iter = iter->next;
00158 num_types++;
00159 iter = new Source_type(type);
00160
00161 t = sensors;
00162 while (t != 0)
00163 t = t->next;
00164 t = new Source_type_tree(type);
00165
00166 t = sources;
00167 while (t != 0)
00168 t = t->next;
00169 t = new Source_type_tree(type);
00170
00171 t = materials;
00172 while (t != 0)
00173 t = t->next;
00174 t = new Source_type_tree(type);
00175 }
00176
00177
00178 int Configuration::Source_type_index(char *type)
00179 {
00180 int i = 0;
00181 Source_type *src_type = types;
00182 while (src_type != 0) {
00183 if (strcmp(src_type->get_name(), type) == 0)
00184 break;
00185 src_type = src_type->next;
00186 i++;
00187 }
00188 return i;
00189 }
00190
00191
00192 char *Configuration::index_Source_type(int idx)
00193 {
00194 int i = 0;
00195 Source_type *src_type = types;
00196 while (i <= idx) {
00197 if (src_type == 0)
00198 return 0;
00199 if (i == idx)
00200 break;
00201 src_type = src_type->next;
00202 i++;
00203 }
00204 if (src_type == 0)
00205 return 0;
00206 return src_type->get_name();
00207 }
00208
00209
00210 AVL_tree<unsigned long> *Configuration::Source_type_tree_index(Source_type_tree *t,
00211 char *type)
00212 {
00213 while (t != 0) {
00214 if (strcmp(type, t->get_name()))
00215 break;
00216 t = t->next;
00217 }
00218
00219 if (t != 0)
00220 return &t->tree;
00221 return 0;
00222 }
00223
00224
00225
00226
00228 #if 0
00229 extern dWorldID myworld;
00230 extern dSpaceID myspace;
00231 extern dJointGroupID contactgroup;
00232 extern dGeomID *geoms;
00233 extern int total_geoms;
00234
00235 void config_sim(Configuration *config)
00236 {
00237 unsigned int i, j = 0;
00238 int total_sources = 0;
00239 AVL_node *list;
00240 dReal elevation = 1, side = MIN_SIZE;
00241 dMass m;
00242 dMassSetBox(&m, 1, side, side, side);
00243 dMassAdjust(&m, 1.0);
00244
00245 myworld = dWorldCreate();
00246 contactgroup = dJointGroupCreate(1000000);
00247 dWorldSetGravity(myworld, 0, 0, -9.81);
00248 dWorldSetCFM(myworld, 1e-5);
00249 dWorldSetERP(myworld, 0.8);
00250 dWorldSetQuickStepNumIterations(myworld, 40);
00251
00252 myspace = dHashSpaceCreate(0);
00253 dCreatePlane(myspace, 0, 0, elevation, 0);
00255
00256 if ((parse_config(config)) < 0)
00257 perror("parse_config");
00258
00259 for (i = 0; i < config->num_types; i++)
00260 total_sources += config->size.sources[i];
00261
00262 total_geoms = total_sources + config->size.nodes +
00263 config->size.obstructions;
00264 geoms = (dGeomID *) malloc(sizeof(dGeomID) * total_geoms);
00265
00266
00267 for (list = config->nodes.find_min();
00268 list != 0; list = list->next) {
00269 Sensor_node *node = (Sensor_node *) list->element;
00270 dBodyID body;
00271 const dReal *pos = dGeomGetPosition(node->shape.geom);
00272 dReal *len;
00273
00274 dGeomBoxGetLengths(node->shape.geom, len);
00275 dMassSetBox(&m, 1, len[0], len[1], len[2]);
00276
00277 if (pos[3] > elevation) {
00278 body = dBodyCreate(myworld);
00279 dBodySetMass(body, &m);
00280 dBodySetPosition(body, pos[0], pos[1], pos[3]);
00281 dGeomSetBody(node->shape.geom, body);
00282 }
00283 geoms[j++] = node->shape.geom;
00284 }
00285
00286 for (i = 0; i < config->size.types; i++) {
00287 for (list = config->sources[i].find_min();
00288 list != 0; list = list->next) {
00289 Source *source = (Source *) list->element;
00290 dBodyID body;
00291 const dReal *pos =
00292 dGeomGetPosition(source->shape.geom);
00293 dReal radius =
00294 dGeomSphereGetRadius(source->shape.geom);
00295
00296 dMassSetBox(&m, 1, radius, radius, radius);
00297
00298 if (pos[3] > elevation) {
00299 body = dBodyCreate(myworld);
00300 dBodySetMass(body, &m);
00301 dBodySetPosition(body, pos[0], pos[1], pos[3]);
00302 dGeomSetBody(source->shape.geom, body);
00303 }
00304 geoms[j++] = source->shape.geom;
00305 }
00306 }
00307
00308 for (list = config->obstructions.find_min();
00309 list != 0; list = list->next) {
00310 interferor_t *obstruction =
00311 (interferor_t *) list->element;
00312 dBodyID body;
00313 const dReal *pos =
00314 dGeomGetPosition(obstruction->shape.geom);
00315 dReal *len, length, radius;
00316
00317 if (dGeomGetClass(obstruction->shape.geom) == dBoxClass) {
00318 dGeomBoxGetLengths(obstruction->shape.geom, len);
00319 dMassSetBox(&m, 1, len[0], len[1], len[2]);
00320 } else {
00321 dGeomCCylinderGetParams(obstruction->shape.geom,
00322 &radius, &length);
00323 dMassSetBox(&m, 1, length, radius, radius);
00324 }
00325
00326 if (pos[3] > elevation) {
00327 body = dBodyCreate(myworld);
00328 dBodySetMass(body, &m);
00329 dBodySetPosition(body, pos[0], pos[1], pos[3]);
00330 dGeomSetBody(obstruction->shape.geom, body);
00331 }
00332 geoms[j++] = obstruction->shape.geom;
00333 }
00334 }
00335 #endif