hkl3d/hkl3d.cpp

   1 /* This file is part of the hkl3d library.
   2  *
   3  * The hkl library is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * (at your option) any later version.
   7  *
   8  * The hkl library is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with the hkl library.  If not, see <http://www.gnu.org/licenses/>.
  15  *
  16  * Copyright (C) 2010      Synchrotron SOLEIL
  17  *                         L'Orme des Merisiers Saint-Aubin
  18  *                         BP 48 91192 GIF-sur-YVETTE CEDEX
  19  *
  20  * Authors: Picca Frédéric-Emmanuel <picca@synchrotron-soleil.fr>
  21  *          Oussama Sboui <oussama.sboui@synchrotron-soleil.fr>
  22  */
  23
  24 #include <yaml.h>
  25 #include <stdio.h>
  26 #include <string.h>
  27 #include <sys/time.h>
  28 #include <unistd.h>
  29 #include <libgen.h>
  30 #include <g3d/g3d.h>
  31 #include <g3d/quat.h>
  32 #include <g3d/matrix.h>
  33
  34 #include "hkl3d.h"
  35 #include "btBulletCollisionCommon.h"
  36 #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
  37 #include "BulletCollision/Gimpact/btGImpactShape.h"
  38
  39 #ifdef USE_PARALLEL_DISPATCHER
  40 # include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
  41 # include "BulletMultiThreaded/PlatformDefinitions.h"
  42 # include "BulletMultiThreaded/PosixThreadSupport.h"
  43 # include "BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
  44 #endif
  45
  46 /***************/
  47 /* static part */
  48 /***************/
  49
  50 static float identity[] = {1, 0, 0, 0,
  51                            0, 1, 0, 0,
  52                            0, 0, 1 ,0,
  53                            0, 0, 0, 1};
  54
  55 /***************/
  56 /* Hkl3DObject */
  57 /***************/
  58
  59 static btTriangleMesh *trimesh_from_g3dobject(G3DObject *object)
  60 {
  61         btTriangleMesh *trimesh;
  62         float *vertex;
  63         GSList *faces;
  64
  65         trimesh = new btTriangleMesh();
  66         trimesh->preallocateVertices(object->vertex_count);
  67         faces = object->faces;
  68         vertex = object->vertex_data;
  69         while(faces){
  70                 G3DFace *face;
  71
  72                 face = (G3DFace*)faces->data;
  73                 btVector3 vertex0(vertex[3*(face->vertex_indices[0])],
  74                                   vertex[3*(face->vertex_indices[0])+1],
  75                                   vertex[3*(face->vertex_indices[0])+2]);
  76                 btVector3 vertex1(vertex[3*(face->vertex_indices[1])],
  77                                   vertex[3*(face->vertex_indices[1])+1],
  78                                   vertex[3*(face->vertex_indices[1])+2]);
  79                 btVector3 vertex2(vertex[3*(face->vertex_indices[2])],
  80                                   vertex[3*(face->vertex_indices[2])+1],
  81                                   vertex[3*(face->vertex_indices[2])+2]);
  82                 trimesh->addTriangle(vertex0, vertex1, vertex2, true);
  83
  84                 faces = g_slist_next(faces);
  85         }
  86
  87         return trimesh;
  88 }
  89
  90 static btCollisionShape* shape_from_trimesh(btTriangleMesh *trimesh, int movable)
  91 {
  92         btCollisionShape* shape;
  93
  94         /*
  95          * create the bullet shape depending on the static status or not of the piece
  96          * static : do not move
  97          * movable : connected to a HklGeometry axis.
  98          */
  99         if (movable >= 0){
 100                 shape = dynamic_cast<btGImpactMeshShape*>(new btGImpactMeshShape(trimesh));
 101                 shape->setMargin(btScalar(0));
 102                 shape->setLocalScaling(btVector3(1,1,1));
 103                 /* maybe usefull for softbodies (useless for now) */
 104                 (dynamic_cast<btGImpactMeshShape*>(shape))->postUpdate();
 105                 /* needed for the collision and must be call after the postUpdate (doc) */
 106                 (dynamic_cast<btGImpactMeshShape*>(shape))->updateBound();
 107         }else{
 108                 shape = dynamic_cast<btBvhTriangleMeshShape*>(new btBvhTriangleMeshShape (trimesh, true));
 109                 shape->setMargin(btScalar(0));
 110                 shape->setLocalScaling(btVector3(1,1,1));
 111         }
 112
 113         return shape;
 114 }
 115
 116 static btCollisionObject * btObject_from_shape(btCollisionShape* shape)
 117 {
 118         btCollisionObject *btObject;
 119
 120         /* create the Object and add the shape */
 121         btObject = new btCollisionObject();
 122         btObject->setCollisionShape(shape);
 123         btObject->activate(true);
 124
 125         return btObject;
 126 }
 127
 128
 129 static struct Hkl3DObject *hkl3d_object_new(G3DObject *object, int id, const char* filename)
 130 {
 131         int i;
 132         GSList *faces;
 133         G3DMaterial* material;
 134         struct Hkl3DObject *self = NULL;
 135
 136         self = HKL_MALLOC(Hkl3DObject);
 137
 138         // extract the color from the first face
 139         faces = object->faces;
 140         material = ((G3DFace *)faces->data)->material;
 141
 142         // fill the hkl3d object structure.
 143         self->id = id;
 144         self->axis_name = strdup(object->name);
 145         self->g3dObject = object;
 146         self->meshes = trimesh_from_g3dobject(object);
 147         self->btShape = shape_from_trimesh(self->meshes, false);
 148         self->btObject = btObject_from_shape(self->btShape);
 149         self->color = new btVector3(material->r, material->g, material->b);
 150         self->hide = object->hide;
 151         self->added = false;
 152         self->selected = false;
 153         self->movable = false;
 154         self->filename = filename;
 155
 156         /*
 157          * if the object already contain a transformation set the Hkl3DObject
 158          * transformation with this transformation. Otherwise set it with the
 159          * identity
 160          */
 161         if(object->transformation){
 162                 for(i=0; i<16; i++)
 163                         self->transformation[i] = object->transformation->matrix[i];
 164         }else{
 165                 /* create one as we requiered it to apply our transformations */
 166                 object->transformation = g_new0(G3DTransformation, 1);
 167                 for(i=0; i<16; i++){
 168                         self->transformation[i] = identity[i];
 169                         object->transformation->matrix[i] = identity[i];
 170                 }
 171         }
 172
 173         self->is_colliding = false;
 174
 175         return self;
 176 }
 177
 178 static void hkl3d_object_free(struct Hkl3DObject *self)
 179 {
 180         if(!self)
 181                 return;
 182
 183         if(self->axis_name){
 184                 free(self->axis_name);
 185                 self->axis_name = NULL;
 186         }
 187         if(self->meshes){
 188                 delete self->meshes;
 189                 self->meshes = NULL;
 190         }
 191         if(self->btShape){
 192                 delete self->btShape;
 193                 self->btShape = NULL;
 194         }
 195         if(self->btObject){
 196                 delete self->btObject;
 197                 self->btObject = NULL;
 198         }
 199         if(self->color){
 200                 delete self->color;
 201                 self->color = NULL;
 202         }
 203         /* memory leak in libg3d */
 204         if(self->g3dObject && self->g3dObject->transformation){
 205                 g_free(self->g3dObject->transformation);
 206                 self->g3dObject->transformation = NULL;
 207         }
 208
 209         free(self);
 210 }
 211
 212 /* return 0 if identical 1 if not */
 213 static int hkl3d_object_cmp(struct Hkl3DObject *object1,
 214                             struct Hkl3DObject *object2)
 215 {
 216         if((!strcmp(object1->filename, object2->filename))
 217            && (object1->id == object2->id))
 218                 return 0;
 219         else
 220                 return 1;
 221 }
 222
 223 static void hkl3d_object_set_axis_name(struct Hkl3DObject *self, const char *name)
 224 {
 225         if(!self || !name || self->axis_name == name)
 226                 return;
 227
 228         if(self->axis_name)
 229                 free(self->axis_name);
 230         self->axis_name = strdup(name);
 231 }
 232
 233 void hkl3d_object_fprintf(FILE *f, const struct Hkl3DObject *self)
 234 {
 235         GSList *faces;
 236         G3DMaterial* material;
 237
 238         faces = self->g3dObject->faces;
 239         material = ((G3DFace *)faces->data)->material;
 240         fprintf(f, "id : %d\n", self->id);
 241         fprintf(f, "name : %s (%p)\n", self->axis_name, self->axis_name);
 242         fprintf(f, "btObject : %p\n", self->btObject);
 243         fprintf(f, "g3dObject : %p\n", self->g3dObject);
 244         fprintf(f, "btShape : %p\n", self->btShape);
 245         fprintf(f, "meshes : %p\n", self->meshes);
 246         fprintf(f, "color : %f, %f, %f\n", material->r, material->g, material->b);
 247         fprintf(f, "is_colliding : %d\n", self->is_colliding);
 248         fprintf(f, "hide : %d\n", self->hide);
 249         fprintf(f, "added : %d\n", self->added);
 250         fprintf(f, "selected : %d\n", self->selected);
 251 }
 252
 253 /***************/
 254 /* Hkl3DConfig */
 255 /***************/
 256
 257 static struct Hkl3DConfig *hkl_config_new(void)
 258 {
 259         struct Hkl3DConfig *self = NULL;
 260
 261         self = HKL_MALLOC(Hkl3DConfig);
 262
 263         self->objects = NULL;
 264         self->len = 0;
 265
 266         return self;
 267 }
 268
 269 static void hkl3d_config_add_object(struct Hkl3DConfig *self, struct Hkl3DObject *object)
 270 {
 271         if(!self)
 272                 return;
 273
 274         self->objects = (typeof(self->objects))realloc(self->objects, sizeof(*self->objects) * (self->len + 1));
 275         self->objects[self->len++] = object;
 276 }
 277
 278 /**
 279  * hkl3d_config_release:
 280  * @config:
 281  * @btCollisionWorld:
 282  *
 283  * release the memory of an Hkl3dConfig. It also detach all btObjects from the btCollisionWorld
 284  **/
 285 static void hkl3d_config_release(struct Hkl3DConfig *config, btCollisionWorld *btWorld)
 286 {
 287         int i;
 288
 289         free(config->filename);
 290         for(i=0; i<config->len; ++i){
 291                 btWorld->removeCollisionObject(config->objects[i]->btObject);
 292                 hkl3d_object_free(config->objects[i]);
 293         }
 294         free(config->objects);
 295         config->objects = NULL;
 296         config->len = 0;
 297 }
 298
 299 void hkl3d_config_fprintf(FILE *f, const struct Hkl3DConfig *self)
 300 {
 301         int i;
 302         fprintf(f, "config (%d):\n", self->len);
 303         for(i=0; i<self->len; ++i)
 304                 hkl3d_object_fprintf(f, self->objects[i]);
 305 }
 306
 307 /****************/
 308 /* Hkl3DConfigs */
 309 /****************/
 310
 311 static struct Hkl3DConfigs* hkl3d_configs_new(void)
 312 {
 313         struct Hkl3DConfigs* self = NULL;
 314
 315         self = (struct Hkl3DConfigs*)malloc(sizeof(struct Hkl3DConfigs));
 316         if(!self)
 317                 return NULL;
 318
 319         self->configs = NULL;
 320         self->len = 0;
 321
 322         return self;
 323 }
 324
 325 static void hkl3d_configs_free(struct Hkl3DConfigs *self)
 326 {
 327         if(!self)
 328                 return;
 329         free(self->configs);
 330         free(self);
 331 }
 332
 333 static void hkl3d_configs_release(struct Hkl3DConfigs *self, btCollisionWorld *_btWorld)
 334 {
 335         int i;
 336
 337         for(i=0; i<self->len; ++i)
 338                 hkl3d_config_release(self->configs[i], _btWorld);
 339 }
 340
 341 static struct Hkl3DConfig* hkl3d_configs_get_last(struct Hkl3DConfigs *self)
 342 {
 343         return self->configs[self->len - 1];
 344 }
 345
 346
 347 static void hkl3d_configs_add_config(struct Hkl3DConfigs *self, struct Hkl3DConfig *config)
 348 {
 349         self->configs = (typeof(self->configs))realloc(self->configs, sizeof(*self->configs) * (self->len + 1));
 350         self->configs[self->len++] = config;
 351 }
 352
 353 void hkl3d_configs_fprintf(FILE *f, const struct Hkl3DConfigs *self)
 354 {
 355         int i;
 356         fprintf(f, "configs (%d):\n", self->len);
 357         for(i=0; i<self->len; ++i)
 358                 hkl3d_config_fprintf(f, self->configs[i]);
 359 }
 360
 361 /**************/
 362 /* Hkl3DStats */
 363 /**************/
 364
 365 double hkl3d_stats_get_collision_ms(const struct Hkl3DStats *self)
 366 {
 367         return self->collision.tv_sec*1000. + self->collision.tv_usec/1000.;
 368 }
 369
 370 void hkl3d_stats_fprintf(FILE *f, const struct Hkl3DStats *self)
 371 {
 372         fprintf(f, "transformation : %f ms collision : %f ms \n",
 373                 self->transformation.tv_sec*1000. + self->transformation.tv_usec/1000.,
 374                 hkl3d_stats_get_collision_ms(self));
 375 }
 376
 377 /*************/
 378 /* Hkl3DAxis */
 379 /*************/
 380
 381 static struct Hkl3DAxis *hkl3d_axis_new(void)
 382 {
 383         struct Hkl3DAxis *self = NULL;
 384
 385         self = HKL_MALLOC(Hkl3DAxis);
 386
 387         self->objects = NULL;
 388         self->len = 0;
 389
 390         return self;
 391 }
 392
 393 static void hkl3d_axis_free(struct Hkl3DAxis *self)
 394 {
 395         if(!self)
 396                 return;
 397
 398         free(self->objects);
 399         free(self);
 400 }
 401
 402 /* should be optimized (useless if the Hkl3DObject had a connection with the Hkl3DAxis */
 403 static void hkl3d_axis_attach_object(struct Hkl3DAxis *self, struct Hkl3DObject *object)
 404 {
 405         self->objects = (Hkl3DObject **)realloc(self->objects, sizeof(*self->objects) * (self->len + 1));
 406         self->objects[self->len++] = object;
 407 }
 408
 409 /* should be optimized (useless if the Hkl3DObject had a connection with the Hkl3DAxis */
 410 static void hkl3d_axis_detach_object(struct Hkl3DAxis *self, struct Hkl3DObject *object)
 411 {
 412         int i;
 413
 414         for(i=0; i<self->len; ++i)
 415                 if(!hkl3d_object_cmp(self->objects[i], object)){
 416                         self->len--;
 417                         /* move all above objects of 1 position */
 418                         if(i < self->len)
 419                                 memmove(&self->objects[i], &self->objects[i+1], sizeof(*self->objects) * (self->len - i));
 420                 }
 421 }
 422
 423 /*****************/
 424 /* Hkl3DGeometry */
 425 /*****************/
 426
 427 static struct Hkl3DGeometry *hkl3d_geometry_new(int n)
 428 {
 429         int i;
 430         struct Hkl3DGeometry *self = NULL;
 431
 432         self = HKL_MALLOC(Hkl3DGeometry);
 433
 434         self->axes = (Hkl3DAxis **)malloc(n * sizeof(*self->axes));
 435         self->len = n;
 436
 437         for(i=0; i<n; ++i)
 438                 self->axes[i] = hkl3d_axis_new();
 439
 440         return self;
 441 }
 442
 443 static void hkl3d_geometry_free(struct Hkl3DGeometry *self)
 444 {
 445         if(!self)
 446                 return;
 447
 448         if(self->len){
 449                 int i;
 450
 451                 for(i=0; i<self->len; ++i)
 452                         hkl3d_axis_free(self->axes[i]);
 453                 free(self->axes);
 454         }
 455
 456         free(self);
 457 }
 458
 459 /*********/
 460 /* HKL3D */
 461 /*********/
 462
 463 /*
 464  * Initialize the bullet collision environment.
 465  * create the Hkl3DObjects
 466  * create the Hkl3DConfigs
 467  */
 468 static void hkl3d_init_internals(struct Hkl3D *self, G3DModel *model, const char *filename)
 469 {
 470         struct Hkl3DConfig *config;
 471         GSList *objects; // lets iterate from the first object.
 472
 473         if(!self || !model || !filename)
 474                 return;
 475
 476         config = hkl_config_new();
 477         objects = model->objects;
 478         while(objects){
 479                 G3DObject *object;
 480
 481                 object = (G3DObject*)objects->data;
 482                 if(object->vertex_count){
 483                         int id;
 484                         struct Hkl3DObject *hkl3dObject;
 485
 486                         id = g_slist_index(model->objects, object);
 487                         hkl3dObject = hkl3d_object_new(object, id, filename);
 488
 489                         // insert collision Object in collision world
 490                         self->_btWorld->addCollisionObject(hkl3dObject->btObject);
 491                         hkl3dObject->added = true;
 492
 493                         // remembers objects to avoid memory leak
 494                         hkl3d_config_add_object(config, hkl3dObject);
 495                 }
 496                 objects = g_slist_next(objects);
 497         }
 498
 499         config->filename = strdup(filename);
 500         hkl3d_configs_add_config(self->configs, config);
 501 }
 502
 503 static void hkl3d_apply_transformations(struct Hkl3D *self)
 504 {
 505         int i;
 506         int k;
 507         struct timeval debut, fin;
 508
 509         // set the right transformation of each objects and get numbers
 510         gettimeofday(&debut, NULL);
 511         for(i=0; i<self->geometry->holders_len; i++){
 512                 size_t j;
 513                 btQuaternion btQ(0, 0, 0, 1);
 514
 515                 size_t len = self->geometry->holders[i].config->len;
 516                 for(j=0; j<len; j++){
 517                         size_t k;
 518                         size_t idx = self->geometry->holders[i].config->idx[j];
 519                         HklAxis *axis = &self->geometry->axes[idx];
 520                         G3DMatrix G3DM[16];
 521
 522                         // conversion beetween hkl -> bullet coordinates
 523                         btQ *= btQuaternion(-axis->q.data[1],
 524                                             axis->q.data[3],
 525                                             axis->q.data[2],
 526                                             axis->q.data[0]);
 527
 528                         // move each object connected to that hkl Axis.
 529                         for(k=0; k<self->movingObjects->axes[idx]->len; ++k){
 530                                 self->movingObjects->axes[idx]->objects[k]->btObject->getWorldTransform().setRotation(btQ);
 531                                 self->movingObjects->axes[idx]->objects[k]->btObject->getWorldTransform().getOpenGLMatrix( G3DM );
 532                                 memcpy(self->movingObjects->axes[idx]->objects[k]->g3dObject->transformation->matrix, &G3DM[0], sizeof(G3DM));
 533                         }
 534
 535                 }
 536         }
 537         gettimeofday(&fin, NULL);
 538         timersub(&fin, &debut, &self->stats.transformation);
 539 }
 540
 541 void hkl3d_connect_all_axes(struct Hkl3D *self)
 542 {
 543         int i;
 544         int j;
 545
 546         /* connect use the axes names */
 547         for(i=0;i<self->configs->len;i++)
 548                 for(j=0;j<self->configs->configs[i]->len;j++)
 549                         hkl3d_connect_object_to_axis(self,
 550                                                      self->configs->configs[i]->objects[j],
 551                                                      self->configs->configs[i]->objects[j]->axis_name);
 552 }
 553
 554 /**
 555  * Hkl3D::Hkl3D:
 556  * @filename:
 557  * @geometry:
 558  *
 559  *
 560  *
 561  * Returns:
 562  **/
 563 struct Hkl3D *hkl3d_new(const char *filename, HklGeometry *geometry)
 564 {
 565         struct Hkl3D *self = NULL;
 566
 567         self = HKL_MALLOC(Hkl3D);
 568
 569         self->geometry = geometry;
 570         self->configs = hkl3d_configs_new();
 571         self->movingObjects = hkl3d_geometry_new(geometry->len);
 572
 573         // first initialize the _movingObjects with the right len.
 574         self->_context = g3d_context_new();
 575         self->model= g3d_model_new();
 576
 577         // initialize the bullet part
 578         self->_btCollisionConfiguration = new btDefaultCollisionConfiguration();
 579
 580 #ifdef USE_PARALLEL_DISPATCHER
 581         int maxNumOutstandingTasks = 2;
 582         PosixThreadSupport::ThreadConstructionInfo constructionInfo("collision",
 583                                                                     processCollisionTask,
 584                                                                     createCollisionLocalStoreMemory,
 585                                                                     maxNumOutstandingTasks);
 586         self->_btThreadSupportInterface = new PosixThreadSupport(constructionInfo);
 587         self->_btDispatcher = new SpuGatheringCollisionDispatcher(self->_btThreadSupportInterface,
 588                                                                   maxNumOutstandingTasks,
 589                                                                   self->_btCollisionConfiguration);
 590 #else
 591         self->_btDispatcher = new btCollisionDispatcher(self->_btCollisionConfiguration);
 592 #endif
 593         btGImpactCollisionAlgorithm::registerAlgorithm(self->_btDispatcher);
 594
 595         btVector3 worldAabbMin(-1000,-1000,-1000);
 596         btVector3 worldAabbMax( 1000, 1000, 1000);
 597
 598         self->_btBroadphase = new btAxisSweep3(worldAabbMin, worldAabbMax);
 599
 600         self->_btWorld = new btCollisionWorld(self->_btDispatcher,
 601                                               self->_btBroadphase,
 602                                               self->_btCollisionConfiguration);
 603
 604         self->filename = filename;
 605         if (filename)
 606                 hkl3d_load_config(self, filename);
 607
 608         return self;
 609 }
 610
 611 void hkl3d_free(struct Hkl3D *self)
 612 {
 613         if(!self)
 614                 return;
 615
 616         int i;
 617         int len;
 618
 619         // _remove objects from the collision world and delete all objects and shapes
 620         hkl3d_configs_release(self->configs, self->_btWorld);
 621         hkl3d_configs_free(self->configs);
 622         hkl3d_geometry_free(self->movingObjects);
 623
 624         if (self->_btWorld)
 625                 delete self->_btWorld;
 626         if (self->_btBroadphase)
 627                 delete self->_btBroadphase;
 628         if (self->_btDispatcher)
 629                 delete self->_btDispatcher;
 630 #ifdef USE_PARALLEL_DISPATCHER
 631         if (self->_btThreadSupportInterface){
 632                 //delete _btThreadSupportInterface;
 633                 //_btThreadSupportInterface = 0;
 634         }
 635 #endif
 636         if (self->_btCollisionConfiguration)
 637                 delete self->_btCollisionConfiguration;
 638         g3d_model_free(self->model);
 639         g3d_context_free(self->_context);
 640
 641         free(self);
 642 }
 643
 644 struct Hkl3DConfig *hkl3d_add_model_from_file(struct Hkl3D *self,
 645                                               const char *filename, const char *directory)
 646 {
 647         G3DModel * model;
 648         G3DObject *object;
 649         G3DMaterial *material;
 650         char current[PATH_MAX];
 651         struct Hkl3DConfig *config = NULL;
 652         int res;
 653
 654         /* first set the current directory using the directory parameter*/
 655         getcwd(current, PATH_MAX);
 656         res = chdir(directory);
 657         model = g3d_model_load_full(self->_context, filename, 0);
 658         res = chdir(current);
 659
 660         if(model){
 661                 /* concatenate the added Model with the one from Hkl3D */
 662                 self->model->objects = g_slist_concat(self->model->objects, model->objects);
 663                 self->model->materials = g_slist_concat(self->model->materials, model->materials);
 664
 665                 /* update the Hkl3D internals from the model */
 666                 hkl3d_init_internals(self, model, filename);
 667
 668                 /* if resp == true there is a problem in the diffractometer model. */
 669                 config = hkl3d_configs_get_last(self->configs);
 670         }
 671         return config;
 672 }
 673
 674 /* check that the axis name is really available in the Geometry */
 675 /* if axis name not valid make the object static object->name = NULL */
 676 /* ok so check if the axis was already connected  or not */
 677 /* if already connected check if it was a different axis do the job */
 678 /* if not yet connected do the job */
 679 /* fill movingCollisionObject and movingG3DObjects vectors for transformations */
 680 void hkl3d_connect_object_to_axis(struct Hkl3D *self,
 681                                   struct Hkl3DObject *object, const char *name)
 682 {
 683         bool update = false;
 684         bool connect = false;
 685         int idx = hkl_geometry_get_axis_idx_by_name(self->geometry, name);
 686         if (!object->movable){
 687                 if(idx >= 0){ /* static -> movable */
 688                         update = true;
 689                         connect = true;
 690                         object->movable = true;
 691                 }
 692         }else{
 693                 if(idx < 0){ /* movable -> static */
 694                         object->movable = false;
 695                         update = true;
 696                         connect = false;
 697                 }else{ /* movable -> movable */
 698                         if(strcmp(object->axis_name, name)){ /* not the same axis */
 699                                 update = false;
 700                                 connect = true;
 701                                 int i = hkl_geometry_get_axis_idx_by_name(self->geometry, object->axis_name);
 702                                 struct Hkl3DObject **objects;
 703
 704                                 for(int k=0;k<self->movingObjects->axes[i]->len;k++){
 705                                         objects = self->movingObjects->axes[i]->objects;
 706                                         if(!hkl3d_object_cmp(objects[k], object)){
 707                                                 hkl3d_axis_detach_object(self->movingObjects->axes[i], object);
 708                                                 break;
 709                                         }
 710                                 }
 711                         }
 712                 }
 713         }
 714         hkl3d_object_set_axis_name(object, name);
 715         if(update){
 716                 /* first deconnected if already connected with a different axis */
 717                 self->_btWorld->removeCollisionObject(object->btObject);
 718                 delete object->btObject;
 719                 delete object->btShape;
 720                 object->btShape = shape_from_trimesh(object->meshes, object->movable);
 721                 object->btObject = btObject_from_shape(object->btShape);
 722                 // insert collision Object in collision world
 723                 self->_btWorld->addCollisionObject(object->btObject);
 724                 object->added = true;
 725         }
 726         if(connect)
 727                 hkl3d_axis_attach_object(self->movingObjects->axes[idx], object);
 728 }
 729
 730 void hkl3d_load_config(struct Hkl3D *self, const char *filename)
 731 {
 732         int j,l;
 733         int newFile=0;
 734         int endEvent = 0;
 735         yaml_parser_t parser;
 736         yaml_event_t input_event;
 737         FILE *file;
 738         char *dirc;
 739         char *dir;
 740         struct Hkl3DConfig *config;
 741
 742         /* Clear the objects. */
 743         memset(&parser, 0, sizeof(parser));
 744         memset(&input_event, 0, sizeof(input_event));
 745
 746         file = fopen(filename, "rb");
 747         if (!file){
 748                 fprintf(stderr, "Could not open the %s config file\n", filename);
 749                 return;
 750         }
 751
 752         if (!yaml_parser_initialize(&parser))
 753                 fprintf(stderr, "Could not initialize the parser object\n");
 754         yaml_parser_set_input_file(&parser, file);
 755
 756         /*
 757          * compute the dirname of the config file as all model files
 758          * will be relative to this directory
 759          */
 760         dirc = strdup(filename);
 761         dir = dirname(dirc);
 762
 763         while(!endEvent){
 764                 /* Get the next event. */
 765                 yaml_parser_parse(&parser, &input_event);
 766
 767                 /* Check if this is the stream end. */
 768                 if(input_event.type == YAML_STREAM_END_EVENT)
 769                         endEvent = 1;
 770                 if(input_event.type == YAML_DOCUMENT_START_EVENT){
 771                         j=0;
 772                         /* skip 4 events */
 773                         // DOCUMENT-START
 774                         // SEQUENCE-START
 775                         // MAPPING-START
 776                         // SCALAR fileName key
 777                         for(l=0;l<4;l++){
 778                                 yaml_event_delete(&input_event);
 779                                 yaml_parser_parse(&parser, &input_event);
 780                         }
 781
 782                         /* the add form file method create a default Hkl3DConfig and add it to the HKL3D */
 783                         /* we just need to update this config with the values from the configuration file */
 784                         config = hkl3d_add_model_from_file(self, (const char *)input_event.data.scalar.value, dir);
 785                         /* skip 3 events */
 786                         // SCALAR objects key
 787                         // SEQUENCE-START
 788                         // MAPPING-START
 789                         for(l=0;l<3;l++){
 790                                 yaml_event_delete(&input_event);
 791                                 yaml_parser_parse(&parser, &input_event);
 792                         }
 793                 }
 794
 795                 if((input_event.type==YAML_MAPPING_START_EVENT)&& config){
 796                         j++;
 797                         /* skip 2 events */
 798                         // MAPPING-START
 799                         // SCALAR iD key
 800                         for(l=0;l<2;l++){
 801                                 yaml_event_delete(&input_event);
 802                                 yaml_parser_parse(&parser, &input_event);
 803                         }
 804
 805                         /* get the object id */
 806                         config->objects[j-1]->id = atoi((const char *)input_event.data.scalar.value);
 807
 808                         /* skip 2 more events */
 809                         // SCALAR valueId
 810                         // SCALAR Name key
 811                         for(l=0;l<2;l++){
 812                                 yaml_event_delete(&input_event);
 813                                 yaml_parser_parse(&parser, &input_event);
 814                         }
 815
 816                         /* get the name of the object from the config file */
 817                         hkl3d_object_set_axis_name(config->objects[j-1], (const char *)input_event.data.scalar.value);
 818                         /*  skip 3 events */
 819                         // SCALAR NameValue
 820                         // SCALAR transformation key
 821                         // SEQUENCE-START
 822                         for(l=0;l<3;l++){
 823                                 yaml_event_delete(&input_event);
 824                                 yaml_parser_parse(&parser, &input_event);
 825                         }
 826
 827                         /* get the 16 values of the transformation */
 828                         for(l=0;l<16;l++){
 829                                 config->objects[j-1]->transformation[l] = atof((const char *)input_event.data.scalar.value);
 830                                 yaml_event_delete(&input_event);
 831                                 yaml_parser_parse(&parser, &input_event);
 832                         }
 833
 834                         /* skip 2 events */
 835                         // SEQUENCE-END
 836                         // SCALAR hide key
 837                         for(l=0;l<2;l++){
 838                                 yaml_event_delete(&input_event);
 839                                 yaml_parser_parse(&parser, &input_event);
 840                         }
 841
 842                         /* get the hide value */
 843                         config->objects[j-1]->hide = strcmp((const char *)input_event.data.scalar.value, "no");
 844                         config->objects[j-1]->g3dObject->hide = config->objects[j-1]->hide;
 845                 }
 846                 yaml_event_delete(&input_event);
 847         }
 848
 849         free(dirc);
 850         yaml_parser_delete(&parser);
 851         fclose(file);
 852
 853         /* now that everythings goes fine we can save the filename */
 854         self->filename = filename;
 855
 856         hkl3d_connect_all_axes(self);
 857 }
 858
 859 void hkl3d_save_config(struct Hkl3D *self, const char *filename)
 860 {
 861         int i;
 862
 863         for(i=0; i<self->configs->len; i++){
 864                 int j;
 865                 char number[64];
 866                 int properties1, key1, value1,seq0;
 867                 int root;
 868                 time_t now;
 869                 yaml_emitter_t emitter;
 870                 yaml_document_t output_document;
 871                 yaml_event_t output_event;
 872                 FILE * file;
 873
 874                 memset(&emitter, 0, sizeof(emitter));
 875                 memset(&output_document, 0, sizeof(output_document));
 876                 memset(&output_event, 0, sizeof(output_event));
 877
 878                 if (!yaml_emitter_initialize(&emitter))
 879                         fprintf(stderr, "Could not inialize the emitter object\n");
 880
 881                 /* Set the emitter parameters */
 882                 file = fopen(filename, "a+");
 883                 if(!file){
 884                         fprintf(stderr, "Could not open the config file %s to save\n", filename);
 885                         return;
 886                 }
 887                 yaml_emitter_set_output_file(&emitter, file);
 888                 yaml_emitter_open(&emitter);
 889
 890                 /* Create an output_document object */
 891                 if (!yaml_document_initialize(&output_document, NULL, NULL, NULL, 0, 0))
 892                         fprintf(stderr, "Could not create a output_document object\n");
 893
 894                 /* Create the root of the config file */
 895                 time(&now);
 896                 root = yaml_document_add_sequence(&output_document,
 897                                                   (yaml_char_t *)ctime(&now),
 898                                                   YAML_BLOCK_SEQUENCE_STYLE);
 899
 900                 /* create the property of the root sequence */
 901                 properties1 = yaml_document_add_mapping(&output_document,
 902                                                         (yaml_char_t *)YAML_MAP_TAG,
 903                                                         YAML_BLOCK_MAPPING_STYLE);
 904
 905                 yaml_document_append_sequence_item(&output_document, root, properties1);
 906
 907                 /* add the map key1 : value1 to the property */
 908                 key1 = yaml_document_add_scalar(&output_document,
 909                                                 NULL,
 910                                                 (yaml_char_t *)"FileName",
 911                                                 -1,
 912                                                 YAML_PLAIN_SCALAR_STYLE);
 913                 value1 = yaml_document_add_scalar(&output_document,
 914                                                   NULL,
 915                                                   (yaml_char_t *)self->configs->configs[i]->filename,
 916                                                   -1,
 917                                                   YAML_PLAIN_SCALAR_STYLE);
 918                 yaml_document_append_mapping_pair(&output_document, properties1, key1, value1);
 919
 920                 /* add the map key1 : seq0 to the first property */
 921                 key1 = yaml_document_add_scalar(&output_document,
 922                                                 NULL,
 923                                                 (yaml_char_t *)"Objects",
 924                                                 -1,
 925                                                 YAML_PLAIN_SCALAR_STYLE);
 926                 /* create the sequence of objects */
 927                 seq0 = yaml_document_add_sequence(&output_document,
 928                                                   (yaml_char_t *)YAML_SEQ_TAG,
 929                                                   YAML_BLOCK_SEQUENCE_STYLE);
 930                 for(j=0; j<self->configs->configs[i]->len; j++){
 931                         int k;
 932                         int properties;
 933                         int key;
 934                         int value;
 935                         int seq1;
 936
 937                         properties = yaml_document_add_mapping(&output_document,
 938                                                                (yaml_char_t *)YAML_MAP_TAG,
 939                                                                YAML_BLOCK_MAPPING_STYLE);
 940                         yaml_document_append_sequence_item(&output_document,seq0, properties);
 941
 942                         key = yaml_document_add_scalar(&output_document,
 943                                                        NULL,
 944                                                        (yaml_char_t *)"Id", -1,
 945                                                        YAML_PLAIN_SCALAR_STYLE);
 946
 947                         sprintf(number, "%d", self->configs->configs[i]->objects[j]->id);
 948                         value = yaml_document_add_scalar(&output_document,
 949                                                          NULL,
 950                                                          (yaml_char_t *)number,
 951                                                          -1,
 952                                                          YAML_PLAIN_SCALAR_STYLE);
 953                         yaml_document_append_mapping_pair(&output_document,properties,key,value);
 954
 955                         key = yaml_document_add_scalar(&output_document,
 956                                                        NULL,
 957                                                        (yaml_char_t *)"Name",
 958                                                        -1,
 959                                                        YAML_PLAIN_SCALAR_STYLE);
 960                         value = yaml_document_add_scalar(&output_document,
 961                                                          NULL,
 962                                                          (yaml_char_t *)self->configs->configs[i]->objects[j]->axis_name,
 963                                                          -1,
 964                                                          YAML_PLAIN_SCALAR_STYLE);
 965                         yaml_document_append_mapping_pair(&output_document,properties,key,value);
 966
 967                         key = yaml_document_add_scalar(&output_document,
 968                                                        NULL,
 969                                                        (yaml_char_t *)"Transformation",
 970                                                        -1,
 971                                                        YAML_PLAIN_SCALAR_STYLE);
 972                         seq1 = yaml_document_add_sequence(&output_document,
 973                                                           (yaml_char_t *)YAML_SEQ_TAG,
 974                                                           YAML_FLOW_SEQUENCE_STYLE);
 975                         yaml_document_append_mapping_pair(&output_document,properties, key, seq1);
 976                         for(k=0; k<16; k++){
 977                                 sprintf(number, "%f", self->configs->configs[i]->objects[j]->transformation[k]);
 978                                 value = yaml_document_add_scalar(&output_document,
 979                                                                  NULL,
 980                                                                  (yaml_char_t *)number,
 981                                                                  -1,
 982                                                                  YAML_PLAIN_SCALAR_STYLE);
 983                                 yaml_document_append_sequence_item(&output_document,seq1,value);
 984                         }
 985
 986                         key = yaml_document_add_scalar(&output_document,
 987                                                        NULL,
 988                                                        (yaml_char_t *)"Hide",
 989                                                        -1,
 990                                                        YAML_PLAIN_SCALAR_STYLE);
 991                         if(self->configs->configs[i]->objects[j]->hide)
 992                                 value = yaml_document_add_scalar(&output_document,
 993                                                                  NULL,
 994                                                                  (yaml_char_t *)"yes",
 995                                                                  -1,
 996                                                                  YAML_PLAIN_SCALAR_STYLE);
 997                         else
 998                                 value = yaml_document_add_scalar(&output_document,
 999                                                                  NULL,
1000                                                                  (yaml_char_t *)"no",
1001                                                                  -1,
1002                                                                  YAML_PLAIN_SCALAR_STYLE);
1003                         yaml_document_append_mapping_pair(&output_document,properties,key,value);
1004                 }
1005                 yaml_document_append_mapping_pair(&output_document, properties1, key1, seq0);
1006
1007                 /* flush the document */
1008                 yaml_emitter_dump(&emitter, &output_document);
1009                 fclose(file);
1010
1011                 yaml_document_delete(&output_document);
1012                 yaml_emitter_delete(&emitter);
1013         }
1014 }
1015
1016 /**
1017  * Hkl3D::hide_object:
1018  *
1019  * update the visibility of an Hkl3DObject in the bullet world
1020  * add or remove the object from the _btWorld depending on the hide
1021  * member of the object.
1022  **/
1023 void hkl3d_hide_object(struct Hkl3D *self, struct Hkl3DObject *object, int hide)
1024 {
1025         // first update the G3DObject
1026         object->hide = hide;
1027         object->g3dObject->hide = hide;
1028         if(object->hide){
1029                 if (object->added){
1030                         self->_btWorld->removeCollisionObject(object->btObject);
1031                         object->added = false;
1032                 }
1033         }else{
1034                 if(!object->added){
1035                         self->_btWorld->addCollisionObject(object->btObject);
1036                         object->added = true;
1037                 }
1038         }
1039 }
1040
1041 /* use for the transparency of colliding objects */
1042 struct ContactSensorCallback : public btCollisionWorld::ContactResultCallback
1043 {
1044         ContactSensorCallback(struct Hkl3DObject *object)
1045                 : btCollisionWorld::ContactResultCallback(),
1046                   collisionObject(object->btObject),
1047                   object(object)
1048                 { }
1049
1050         btCollisionObject *collisionObject;
1051         struct Hkl3DObject *object;
1052
1053         virtual btScalar addSingleResult(btManifoldPoint & cp,
1054                                          const btCollisionObject *colObj0, int partId0, int index0,
1055                                          const btCollisionObject *colObj1, int partId1, int index1)
1056                 {
1057                         if(colObj0 == collisionObject
1058                            || colObj1 == collisionObject)
1059                                 object->is_colliding = TRUE;
1060                         return 0;
1061                 }
1062 };
1063
1064 int hkl3d_is_colliding(struct Hkl3D *self)
1065 {
1066         int i;
1067         int j;
1068         bool res = true;
1069         int numManifolds;
1070         struct timeval debut, fin;
1071
1072         //apply geometry transformation
1073         hkl3d_apply_transformations(self);
1074         // perform the collision detection and get numbers
1075         gettimeofday(&debut, NULL);
1076         if(self->_btWorld){
1077                 self->_btWorld->performDiscreteCollisionDetection();
1078                 self->_btWorld->updateAabbs();
1079         }
1080         gettimeofday(&fin, NULL);
1081         timersub(&fin, &debut, &self->stats.collision);
1082
1083         numManifolds = self->_btWorld->getDispatcher()->getNumManifolds();
1084
1085         /* reset all the collisions */
1086         for(i=0; i<self->configs->len; i++)
1087                 for(j=0; j<self->configs->configs[i]->len; j++)
1088                         self->configs->configs[i]->objects[j]->is_colliding = FALSE;
1089
1090         /* check all the collisions */
1091         for(i=0; i<self->configs->len; i++)
1092                 for(j=0; j<self->configs->configs[i]->len; j++){
1093                         struct Hkl3DObject *object = self->configs->configs[i]->objects[j];
1094                         ContactSensorCallback callback(object);
1095                         self->_btWorld->contactTest(object->btObject, callback);
1096                 }
1097
1098         return numManifolds != 0;
1099 }
1100
1101 /**
1102  * Hkl3D::get_bounding_boxes:
1103  * @min:
1104  * @max:
1105  *
1106  * get the bounding boxes of the current world from the bullet internals.
1107  **/
1108 void hkl3d_get_bounding_boxes(struct Hkl3D *self,
1109                               struct btVector3 *min, struct btVector3 *max)
1110 {
1111         self->_btWorld->getBroadphase()->getBroadphaseAabb(*min, *max);
1112 }
1113
1114 int hkl3d_get_nb_manifolds(struct Hkl3D *self)
1115 {
1116         return self->_btDispatcher->getNumManifolds();
1117 }
1118
1119 int hkl3d_get_nb_contacts(struct Hkl3D *self, int manifold)
1120 {
1121         return self->_btDispatcher->getManifoldByIndexInternal(manifold)->getNumContacts();
1122 }
1123
1124 void hkl3d_get_collision_coordinates(struct Hkl3D *self, int manifold, int contact,
1125                                      double *xa, double *ya, double *za,
1126                                      double *xb, double *yb, double *zb)
1127 {
1128         btPersistentManifold *contactManifold;
1129
1130         contactManifold = self->_btDispatcher->getManifoldByIndexInternal(manifold);
1131         btManifoldPoint & pt = contactManifold->getContactPoint(contact);
1132         btVector3 ptA = pt.getPositionWorldOnA();
1133         btVector3 ptB = pt.getPositionWorldOnB();
1134
1135         *xa = ptA.x();
1136         *ya = ptA.y();
1137         *za = ptA.z();
1138         *xb = ptB.x();
1139         *yb = ptB.y();
1140         *zb = ptB.z();
1141 }