[hkl] fix the test suite due to binoculars new projection signature.

[hkl.git] / hkl3d / hkl3d.cpp

/* This file is part of the hkl3d library.
 *
 * The hkl library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * The hkl library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with the hkl library.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Copyright (C) 2010      Synchrotron SOLEIL
 *                         L'Orme des Merisiers Saint-Aubin
 *                         BP 48 91192 GIF-sur-YVETTE CEDEX
 *
 * Authors: Picca Frédéric-Emmanuel <picca@synchrotron-soleil.fr>
 *          Oussama Sboui <oussama.sboui@synchrotron-soleil.fr>
 */

#include <yaml.h>
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <unistd.h>
#include <libgen.h>
#include <g3d/g3d.h>
#include <g3d/quat.h>
#include <g3d/matrix.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include "hkl3d.h"
#include "hkl-geometry-private.h"

#include "btBulletCollisionCommon.h"
#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"

#ifdef USE_PARALLEL_DISPATCHER
# include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
# include "BulletMultiThreaded/PlatformDefinitions.h"
# include "BulletMultiThreaded/PosixThreadSupport.h"
# include "BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
#endif

/***************/
/* static part */
/***************/
static float identity[] = {1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1 ,0,
        0, 0, 0, 1};

static void *_hkl3d_malloc(int size, const char *error)
{
        void *tmp;

        tmp = calloc(1, size);
        if(!tmp){
                fprintf(stderr, "%s", error);
                exit(128);
        }

        return tmp;
}
/* malloc method */
#define HKL3D_MALLOC(type) (type *)_hkl3d_malloc(sizeof(type), "Can not allocate memory for a " #type)

/***************/
/* Hkl3DObject */
/***************/

static btTriangleMesh *trimesh_from_g3dobject(G3DObject *object)
{
        btTriangleMesh *trimesh;
        float *vertex;
        GSList *faces;

        trimesh = new btTriangleMesh();
        trimesh->preallocateVertices(object->vertex_count);
        faces = object->faces;
        vertex = object->vertex_data;
        while(faces){
                G3DFace *face;

                face = (G3DFace*)faces->data;
                btVector3 vertex0(vertex[3*(face->vertex_indices[0])],
                                  vertex[3*(face->vertex_indices[0])+1],
                                  vertex[3*(face->vertex_indices[0])+2]);
                btVector3 vertex1(vertex[3*(face->vertex_indices[1])],
                                  vertex[3*(face->vertex_indices[1])+1],
                                  vertex[3*(face->vertex_indices[1])+2]);
                btVector3 vertex2(vertex[3*(face->vertex_indices[2])],
                                  vertex[3*(face->vertex_indices[2])+1],
                                  vertex[3*(face->vertex_indices[2])+2]);
                trimesh->addTriangle(vertex0, vertex1, vertex2, true);

                faces = g_slist_next(faces);
        }

        return trimesh;
}

static btCollisionShape* shape_from_trimesh(btTriangleMesh *trimesh, int movable)
{
        btCollisionShape* shape;

        /*
         * create the bullet shape depending on the static status or not of the piece
         * static : do not move
         * movable : connected to a HklGeometry axis.
         */
        if (movable >= 0){
                shape = dynamic_cast<btGImpactMeshShape*>(new btGImpactMeshShape(trimesh));
                shape->setMargin(btScalar(0));
                shape->setLocalScaling(btVector3(1,1,1));
                /* maybe usefull for softbodies (useless for now) */
                (dynamic_cast<btGImpactMeshShape*>(shape))->postUpdate();
                /* needed for the collision and must be call after the postUpdate (doc) */
                (dynamic_cast<btGImpactMeshShape*>(shape))->updateBound();
        }else{
                shape = dynamic_cast<btBvhTriangleMeshShape*>(new btBvhTriangleMeshShape (trimesh, true));
                shape->setMargin(btScalar(0));
                shape->setLocalScaling(btVector3(1,1,1));
        }

        return shape;
}

static btCollisionObject * btObject_from_shape(btCollisionShape* shape)
{
        btCollisionObject *btObject;

        /* create the Object and add the shape */
        btObject = new btCollisionObject();
        btObject->setCollisionShape(shape);
        btObject->activate(true);

        return btObject;
}


static Hkl3DObject *hkl3d_object_new(Hkl3DModel *model, G3DObject *object, int id)
{
        int i;
        GSList *faces;
        G3DMaterial* material;
        Hkl3DObject *self = NULL;

        self = HKL3D_MALLOC(Hkl3DObject);

        /* extract the color from the first face */
        /* this is usefull for the bullet GL draw method */
        faces = object->faces;
        material = ((G3DFace *)faces->data)->material;

        /* fill the hkl3d object structure. */
        self->model = model;
        self->id = id;
        self->axis_name = strdup(object->name);
        self->axis = NULL;
        self->g3d = object;
        self->meshes = trimesh_from_g3dobject(object);
        self->btShape = shape_from_trimesh(self->meshes, false);
        self->btObject = btObject_from_shape(self->btShape);
        self->color = new btVector3(material->r, material->g, material->b);
        self->hide = object->hide;
        self->added = false;
        self->selected = false;
        self->movable = false;

        /*
         * if the object already contain a transformation set the Hkl3DObject
         * transformation with this transformation. Otherwise set it with the
         * identity
         */
        if(object->transformation){
                for(i=0; i<16; i++)
                        self->transformation[i] = object->transformation->matrix[i];
        }else{
                /* create one as we requiered it to apply our transformations */
                object->transformation = g_new0(G3DTransformation, 1);
                for(i=0; i<16; i++){
                        self->transformation[i] = identity[i];
                        object->transformation->matrix[i] = identity[i];
                }
        }

        self->is_colliding = false;

        return self;
}

static void hkl3d_object_free(Hkl3DObject *self)
{
        if(!self)
                return;

        /* memory leak in libg3d */
        if(self->g3d && self->g3d->transformation){
                g_free(self->g3d->transformation);
                self->g3d->transformation = NULL;
        }
        if(self->color){
                delete self->color;
                self->color = NULL;
        }
        if(self->btObject){
                delete self->btObject;
                self->btObject = NULL;
        }
        if(self->btShape){
                delete self->btShape;
                self->btShape = NULL;
        }
        if(self->meshes){
                delete self->meshes;
                self->meshes = NULL;
        }
        if(self->axis_name){
                free(self->axis_name);
                self->axis_name = NULL;
        }

        free(self);
}

static void hkl3d_object_set_movable(Hkl3DObject *self, int movable)
{
        if(!self)
                return;

        if(self->movable != movable){
                self->movable = movable;
                delete self->btObject;
                delete self->btShape;
                self->btShape = shape_from_trimesh(self->meshes, movable);
                self->btObject = btObject_from_shape(self->btShape);
        }
}

static void hkl3d_object_set_axis_name(Hkl3DObject *self, const char *name)
{
        if(!self || !name || self->axis_name == name)
                return;

        if(self->axis_name)
                free(self->axis_name);
        self->axis_name = strdup(name);
}

static void matrix_fprintf(FILE *f, const float matrix[])
{
        fprintf(f, "transformation : ");
        for(uint i=0; i<4; ++i){
                if(i)
                        fprintf(f, "                 ");
                for(uint j=0; j<4; ++j){
                        fprintf(f, " %6.3f", matrix[4 * i + j]);
                }
                fprintf(f, "\n");
        }
}

void hkl3d_object_aabb_get(const Hkl3DObject *self, float from[3], float to[3])
{
        btVector3 min, max;

        self->btShape->getAabb(self->btObject->getWorldTransform(), min, max);

        from[0] = min.getX();
        from[1] = min.getY();
        from[2] = min.getZ();
        to[0] = max.getX();
        to[1] = max.getY();
        to[2] = max.getZ();
}

void hkl3d_object_fprintf(FILE *f, const Hkl3DObject *self)
{
        GSList *faces;
        G3DMaterial* material;

        faces = self->g3d->faces;
        material = ((G3DFace *)faces->data)->material;
        fprintf(f, "id : %d\n", self->id);
        fprintf(f, "name : %s (%p)\n", self->axis_name, self->axis_name);
        fprintf(f, "axis : %p\n", self->axis);
        matrix_fprintf(f, self->transformation);
        fprintf(f, "btObject : %p\n", self->btObject);
        fprintf(f, "g3d : %p\n", self->g3d);
        matrix_fprintf(f, self->g3d->transformation->matrix);
        fprintf(f, "btShape : %p\n", self->btShape);
        fprintf(f, "meshes : %p\n", self->meshes);
        fprintf(f, "color : %f, %f, %f\n", material->r, material->g, material->b);
        fprintf(f, "is_colliding : %d\n", self->is_colliding);
        fprintf(f, "hide : %d\n", self->hide);
        fprintf(f, "added : %d\n", self->added);
        fprintf(f, "selected : %d\n", self->selected);
}

/**************/
/* Hkl3DModel */
/**************/

static Hkl3DModel *hkl3d_model_new(void)
{
        Hkl3DModel *self = NULL;

        self = HKL3D_MALLOC(Hkl3DModel);

        self->filename = NULL;
        self->objects = NULL;
        self->len = 0;
        self->g3d = NULL;

        return self;
}

static void hkl3d_model_free(Hkl3DModel *self)
{
        size_t i;

        if(!self)
                return;

        free(self->filename);
        for(i=0; i<self->len; ++i)
                hkl3d_object_free(self->objects[i]);
        free(self->objects);
        g3d_model_free(self->g3d);
        free(self);
}

static void hkl3d_model_add_object(Hkl3DModel *self, Hkl3DObject *object)
{
        if(!self || !object)
                return;

        self->objects = (typeof(self->objects))realloc(self->objects, sizeof(*self->objects) * (self->len + 1));
        self->objects[self->len++] = object;
}

static void hkl3d_model_delete_object(Hkl3DModel *self, Hkl3DObject *object)
{
        size_t i;

        if(!self || !object)
                return;

        if(self != object->model)
                return;

        /* find the index of the object */
        for(i=0; self->objects[i] != object; ++i);

        hkl3d_object_free(object);
        self->len--;
        /* move all above objects of 1 position */
        if(i < self->len)
                memmove(object, object + 1, sizeof(object) * (self->len - i));
}

void hkl3d_model_fprintf(FILE *f, const Hkl3DModel *self)
{
        fprintf(f, "config (%zd):\n", self->len);
        for(size_t i=0; i<self->len; ++i)
                hkl3d_object_fprintf(f, self->objects[i]);
}

/*
 * Initialize the bullet collision environment.
 * create the Hkl3DObjects
 * create the Hkl3DConfig
 */
static Hkl3DModel *hkl3d_model_new_from_file(const char *filename)
{
        G3DModel *model;
        Hkl3DModel *self = NULL;
        GSList *objects; /* lets iterate from the first object. */
        G3DContext *context;

        if(!filename)
                return NULL;

        /* load the model from the file */
        context = g3d_context_new();
        model = g3d_model_load_full(context, filename, 0);
        g3d_context_free(context);
        if(!model)
                return NULL;
        self = hkl3d_model_new();

        self->filename = strdup(filename);
        self->g3d = model;

        /* create all the attached Hkl3DObjects */
        objects = model->objects;
        while(objects){
                G3DObject *object;

                object = (G3DObject*)objects->data;
                if(object->vertex_count){
                        int id;
                        Hkl3DObject *hkl3dObject;

                        id = g_slist_index(model->objects, object);
                        hkl3dObject = hkl3d_object_new(self, object, id);

                        /* remembers objects to avoid memory leak */
                        hkl3d_model_add_object(self, hkl3dObject);
                }
                objects = g_slist_next(objects);
        }
        return self;
}

/***************/
/* Hkl3DConfig */
/***************/

static Hkl3DConfig* hkl3d_config_new(void)
{
        Hkl3DConfig* self = NULL;

        self = (Hkl3DConfig*)malloc(sizeof(Hkl3DConfig));
        if(!self)
                return NULL;

        self->models = NULL;
        self->len = 0;

        return self;
}

static void hkl3d_config_free(Hkl3DConfig *self)
{
        if(!self)
                return;

        for(size_t i=0; i<self->len; ++i)
                hkl3d_model_free(self->models[i]);
        free(self->models);
        free(self);
}

static void hkl3d_config_add_model(Hkl3DConfig *self, Hkl3DModel *model)
{
        self->models = (typeof(self->models))realloc(self->models, sizeof(*self->models) * (self->len + 1));
        self->models[self->len++] = model;
}

void hkl3d_config_fprintf(FILE *f, const Hkl3DConfig *self)
{
        fprintf(f, "models (%zd):\n", self->len);
        for(size_t i=0; i<self->len; ++i)
                hkl3d_model_fprintf(f, self->models[i]);
}

/**************/
/* Hkl3DStats */
/**************/

double hkl3d_stats_get_collision_ms(const Hkl3DStats *self)
{
        return self->collision.tv_sec*1000. + self->collision.tv_usec/1000.;
}

void hkl3d_stats_fprintf(FILE *f, const Hkl3DStats *self)
{
        fprintf(f, "transformation : %f ms collision : %f ms \n",
                self->transformation.tv_sec*1000. + self->transformation.tv_usec/1000.,
                hkl3d_stats_get_collision_ms(self));
}

/*************/
/* Hkl3DAxis */
/*************/

static Hkl3DAxis *hkl3d_axis_new(void)
{
        Hkl3DAxis *self = NULL;

        self = HKL3D_MALLOC(Hkl3DAxis);

        self->objects = NULL; /* do not own the objects */
        self->len = 0;

        return self;
}

static void hkl3d_axis_free(Hkl3DAxis *self)
{
        if(!self)
                return;

        free(self->objects);
        free(self);
}

/* should be optimized (useless if the Hkl3DObject had a connection with the Hkl3DAxis */
static void hkl3d_axis_attach_object(Hkl3DAxis *self, Hkl3DObject *object)
{
        object->axis = self;
        self->objects = (Hkl3DObject **)realloc(self->objects, sizeof(*self->objects) * (self->len + 1));
        self->objects[self->len++] = object;
}

/* should be optimized (useless if the Hkl3DObject had a connection with the Hkl3DAxis */
static void hkl3d_axis_detach_object(Hkl3DAxis *self, Hkl3DObject *object)
{
        size_t i;

        if(!self || !object)
                return;

        if(self != object->axis)
                return;

        /* find the index of the object in the object list */
        for(i=0; self->objects[i] != object; ++i);

        object->axis = NULL;
        /* move all above objects of 1 position */
        self->len--;
        if(i < self->len)
                memmove(object, object+1, sizeof(object) * (self->len - i));
}

static void hkl3d_axis_fprintf(FILE *f, const Hkl3DAxis *self)
{
        if(!f || !self)
                return;

        fprintf(f, "Axis len : %zd\n", self->len);
        for(size_t i=0; i<self->len; ++i)
                hkl3d_object_fprintf(f, self->objects[i]);
}

/*****************/
/* Hkl3DGeometry */
/*****************/

static Hkl3DGeometry *hkl3d_geometry_new(HklGeometry *geometry)
{
        uint i;
        Hkl3DGeometry *self = NULL;

        self = HKL3D_MALLOC(Hkl3DGeometry);

        self->geometry = geometry;
        self->axes = (Hkl3DAxis **)malloc(darray_size(geometry->axes) * sizeof(*self->axes));

        for(i=0; i<darray_size(geometry->axes); ++i)
                self->axes[i] = hkl3d_axis_new();

        return self;
}

static void hkl3d_geometry_free(Hkl3DGeometry *self)
{
        uint i;

        if(!self)
                return;

        for(i=0; i<darray_size(self->geometry->axes); ++i)
                hkl3d_axis_free(self->axes[i]);
        free(self->axes);
        free(self);
}

static void hkl3d_geometry_apply_transformations(Hkl3DGeometry *self)
{
        HklHolder **holder;

        darray_foreach(holder, self->geometry->holders){
                size_t j;
                btQuaternion btQ(0, 0, 0, 1);

                size_t len = (*holder)->config->len;
                for(j=0; j<len; j++){
                        size_t k;
                        size_t idx = (*holder)->config->idx[j];
                        const HklQuaternion *q = hkl_parameter_quaternion_get(darray_item(self->geometry->axes, idx));
                        G3DMatrix G3DM[16];

                        /* conversion beetween hkl -> bullet coordinates */
                        btQ *= btQuaternion(-q->data[1],
                                            q->data[3],
                                            q->data[2],
                                            q->data[0]);

                        /* move each object connected to that hkl Axis. */
                        /* apply the quaternion transformation to the bullet object */
                        /* use the bullet library to compute the OpenGL matrix */
                        /* apply this matrix to the G3DObject for the visualisation */
                        for(k=0; k<self->axes[idx]->len; ++k){
                                self->axes[idx]->objects[k]->btObject->getWorldTransform().setRotation(btQ);
                                self->axes[idx]->objects[k]->btObject->getWorldTransform().getOpenGLMatrix( G3DM );
                                memcpy(self->axes[idx]->objects[k]->g3d->transformation->matrix,
                                       &G3DM[0], sizeof(G3DM));
                        }
                }
        }
}

static void hkl3d_geometry_fprintf(FILE *f, const Hkl3DGeometry *self)
{
        if(!f || !self)
                return;

        fprintf(f, "Hkl3DGeometry : \n");
        hkl_geometry_fprintf(f, self->geometry);
        for(size_t i=0; i<darray_size(self->geometry->axes); ++i)
                hkl3d_axis_fprintf(f, self->axes[i]);
}

static Hkl3DAxis *hkl3d_geometry_axis_get(Hkl3DGeometry *self, const char *name)
{
        for(size_t i=0; i<darray_size(self->geometry->axes); ++i){
                if (!strcmp(hkl_parameter_name_get(darray_item(self->geometry->axes, i)),
                            name))
                        return self->axes[i];
        }
        return NULL;
}

/*********/
/* HKL3D */
/*********/

static void hkl3d_apply_transformations(Hkl3D *self)
{
        struct timeval debut, fin;

        /* set the right transformation of each objects and get numbers */
        gettimeofday(&debut, NULL);
        hkl3d_geometry_apply_transformations(self->geometry);
        gettimeofday(&fin, NULL);
        timersub(&fin, &debut, &self->stats.transformation);
}

void hkl3d_connect_all_axes(Hkl3D *self)
{
        /* connect use the axes names */
        for(size_t i=0;i<self->config->len;i++)
                for(size_t j=0;j<self->config->models[i]->len;j++)
                        hkl3d_connect_object_to_axis(self,
                                                     self->config->models[i]->objects[j],
                                                     self->config->models[i]->objects[j]->axis_name);
}

/**
 * Hkl3D::Hkl3D:
 * @filename:
 * @geometry:
 *
 *
 *
 * Returns:
 **/
Hkl3D *hkl3d_new(const char *filename, HklGeometry *geometry)
{
        Hkl3D *self = NULL;

        self = HKL3D_MALLOC(Hkl3D);

        self->geometry = hkl3d_geometry_new(geometry);
        self->config = hkl3d_config_new();
        self->model= g3d_model_new();

        /* initialize the bullet part */
        self->_btCollisionConfiguration = new btDefaultCollisionConfiguration();

#ifdef USE_PARALLEL_DISPATCHER
        int maxNumOutstandingTasks = 2;
        PosixThreadSupport::ThreadConstructionInfo constructionInfo("collision",
                                                                    processCollisionTask,
                                                                    createCollisionLocalStoreMemory,
                                                                    maxNumOutstandingTasks);
        self->_btThreadSupportInterface = new PosixThreadSupport(constructionInfo);
        self->_btDispatcher = new SpuGatheringCollisionDispatcher(self->_btThreadSupportInterface,
                                                                  maxNumOutstandingTasks,
                                                                  self->_btCollisionConfiguration);
#else
        self->_btDispatcher = new btCollisionDispatcher(self->_btCollisionConfiguration);
#endif
        btGImpactCollisionAlgorithm::registerAlgorithm(self->_btDispatcher);

        btVector3 worldAabbMin(-1000,-1000,-1000);
        btVector3 worldAabbMax( 1000, 1000, 1000);

        self->_btBroadphase = new btAxisSweep3(worldAabbMin, worldAabbMax);

        self->_btWorld = new btCollisionWorld(self->_btDispatcher,
                                              self->_btBroadphase,
                                              self->_btCollisionConfiguration);

        self->filename = filename;
        if (filename)
                hkl3d_load_config(self, filename);

        return self;
}

void hkl3d_free(Hkl3D *self)
{
        /* remove all objects from the collision world */
        for(size_t i=0; i<self->config->len; ++i)
                for(size_t j=0; j<self->config->models[i]->len; ++j)
                        if(self->config->models[i]->objects[j]->added)
                                self->_btWorld->removeCollisionObject(self->config->models[i]->objects[j]->btObject);

        hkl3d_geometry_free(self->geometry);
        hkl3d_config_free(self->config);

        if (self->_btWorld)
                delete self->_btWorld;
        if (self->_btBroadphase)
                delete self->_btBroadphase;
        if (self->_btDispatcher)
                delete self->_btDispatcher;
#ifdef USE_PARALLEL_DISPATCHER
        if (self->_btThreadSupportInterface){
                /* delete _btThreadSupportInterface; */
                /* _btThreadSupportInterface = 0; */
        }
#endif
        if (self->_btCollisionConfiguration)
                delete self->_btCollisionConfiguration;
        g_free(self->model); /* do not use g3d_model_free as it is juste a container for all config->model */

        free(self);
}

Hkl3DModel *hkl3d_add_model_from_file(Hkl3D *self,
                                      const char *filename, const char *directory)
{
        int current;
        Hkl3DModel *model = NULL;
        int res;

        /* first set the current directory using the directory
         * parameter. Maybe using openat should be a better solution
         * in the hkl3d_model_new_from_file */
        current = open(".", O_RDONLY);
        if (current < 0)
                return NULL;
        res = chdir(directory);
        if(res < 0)
                goto close_current;

        model = hkl3d_model_new_from_file(filename);
        if(model){
                /* we can not display two different models with the current g3dviewer code */
                /* so concatenate this loaded model with the one of hkl3d */
                self->model->objects = g_slist_concat(self->model->objects, model->g3d->objects);
                self->model->materials = g_slist_concat(self->model->materials, model->g3d->materials);

                /* update the Hkl3D internals from the model */
                hkl3d_config_add_model(self->config, model);
        }
        /* restore the current directory */
        res = fchdir(current);

        return model;

close_current:
        close(current);
        return NULL;
}

/* check that the axis name is really available in the Geometry */
/* if axis name not valid make the object static object->name = NULL */
/* ok so check if the axis was already connected  or not */
/* if already connected check if it was a different axis do the job */
/* if not yet connected do the job */
/* fill movingCollisionObject and movingG3DObjects vectors for transformations */
void hkl3d_connect_object_to_axis(Hkl3D *self, Hkl3DObject *object, const char *name)
{
        Hkl3DAxis *axis3d = hkl3d_geometry_axis_get(self->geometry, name);
        if (!object->movable){
                if(axis3d){ /* static -> movable */
                        self->_btWorld->removeCollisionObject(object->btObject);
                        hkl3d_object_set_movable(object, true);
                        self->_btWorld->addCollisionObject(object->btObject);
                        object->added = true;
                        hkl3d_axis_attach_object(axis3d, object);
                }
        }else{
                if(!axis3d){ /* movable -> static */
                        self->_btWorld->removeCollisionObject(object->btObject);
                        hkl3d_object_set_movable(object, false);
                        self->_btWorld->addCollisionObject(object->btObject);
                        object->added = true;
                }else{ /* movable -> movable */
                        if(strcmp(object->axis_name, name)){ /* not the same axis */
                                hkl3d_axis_detach_object(object->axis, object);
                                hkl3d_axis_attach_object(axis3d, object);
                        }
                }
        }
        hkl3d_object_set_axis_name(object, name);
}

void hkl3d_load_config(Hkl3D *self, const char *filename)
{
        int j = 0;
        int l;
        int endEvent = 0;
        yaml_parser_t parser;
        yaml_event_t input_event;
        FILE *file;
        char *dirc;
        char *dir;
        Hkl3DModel *config = NULL;

        /* Clear the objects. */
        memset(&parser, 0, sizeof(parser));
        memset(&input_event, 0, sizeof(input_event));

        file = fopen(filename, "rb");
        if (!file){
                fprintf(stderr, "Could not open the %s config file\n", filename);
                return;
        }

        if (!yaml_parser_initialize(&parser))
                fprintf(stderr, "Could not initialize the parser object\n");
        yaml_parser_set_input_file(&parser, file);

        /* compute the dirname of the config file as all model files */
        /* will be relative to this directory */
        dirc = strdup(filename);
        dir = dirname(dirc);

        while(!endEvent){
                /* Get the next event. */
                yaml_parser_parse(&parser, &input_event);

                /* Check if this is the stream end. */
                if(input_event.type == YAML_STREAM_END_EVENT)
                        endEvent = 1;
                if(input_event.type == YAML_DOCUMENT_START_EVENT){
                        j=0;
                        /* skip 4 events */
                        // DOCUMENT-START
                        // SEQUENCE-START
                        // MAPPING-START
                        // SCALAR fileName key
                        for(l=0;l<4;l++){
                                yaml_event_delete(&input_event);
                                yaml_parser_parse(&parser, &input_event);
                        }

                        /* the add form file method create a default Hkl3DModel and add it to the HKL3D */
                        /* we just need to update this config with the values from the configuration file */
                        config = hkl3d_add_model_from_file(self, (const char *)input_event.data.scalar.value, dir);
                        /* skip 3 events */
                        // SCALAR objects key
                        // SEQUENCE-START
                        // MAPPING-START
                        for(l=0;l<3;l++){
                                yaml_event_delete(&input_event);
                                yaml_parser_parse(&parser, &input_event);
                        }
                }

                if((input_event.type==YAML_MAPPING_START_EVENT)&& config){
                        j++;
                        /* skip 2 events */
                        // MAPPING-START
                        // SCALAR iD key
                        for(l=0;l<2;l++){
                                yaml_event_delete(&input_event);
                                yaml_parser_parse(&parser, &input_event);
                        }

                        /* get the object id */
                        config->objects[j-1]->id = atoi((const char *)input_event.data.scalar.value);

                        /* skip 2 more events */
                        // SCALAR valueId
                        // SCALAR Name key
                        for(l=0;l<2;l++){
                                yaml_event_delete(&input_event);
                                yaml_parser_parse(&parser, &input_event);
                        }

                        /* get the name of the object from the config file */
                        hkl3d_object_set_axis_name(config->objects[j-1], (const char *)input_event.data.scalar.value);
                        /*  skip 3 events */
                        // SCALAR NameValue
                        // SCALAR transformation key
                        // SEQUENCE-START
                        for(l=0;l<3;l++){
                                yaml_event_delete(&input_event);
                                yaml_parser_parse(&parser, &input_event);
                        }

                        /* get the 16 values of the transformation */
                        for(l=0;l<16;l++){
                                config->objects[j-1]->transformation[l] = atof((const char *)input_event.data.scalar.value);
                                yaml_event_delete(&input_event);
                                yaml_parser_parse(&parser, &input_event);
                        }

                        /* skip 2 events */
                        // SEQUENCE-END
                        // SCALAR hide key
                        for(l=0;l<2;l++){
                                yaml_event_delete(&input_event);
                                yaml_parser_parse(&parser, &input_event);
                        }

                        /* get the hide value */
                        config->objects[j-1]->hide = strcmp((const char *)input_event.data.scalar.value, "no");
                        config->objects[j-1]->g3d->hide = config->objects[j-1]->hide;
                }
                yaml_event_delete(&input_event);
        }

        free(dirc);
        yaml_parser_delete(&parser);
        fclose(file);

        /* now that everythings goes fine we can save the filename */
        self->filename = filename;

        hkl3d_connect_all_axes(self);
}

void hkl3d_save_config(Hkl3D *self, const char *filename)
{
        for(size_t i=0; i<self->config->len; i++){
                char number[64];
                int properties1, key1, value1,seq0;
                int root;
                time_t now;
                yaml_emitter_t emitter;
                yaml_document_t output_document;
                yaml_event_t output_event;
                FILE * file;

                memset(&emitter, 0, sizeof(emitter));
                memset(&output_document, 0, sizeof(output_document));
                memset(&output_event, 0, sizeof(output_event));

                if (!yaml_emitter_initialize(&emitter))
                        fprintf(stderr, "Could not inialize the emitter object\n");

                /* Set the emitter parameters */
                file = fopen(filename, "a+");
                if(!file){
                        fprintf(stderr, "Could not open the config file %s to save\n", filename);
                        return;
                }
                yaml_emitter_set_output_file(&emitter, file);
                yaml_emitter_open(&emitter);

                /* Create an output_document object */
                if (!yaml_document_initialize(&output_document, NULL, NULL, NULL, 0, 0))
                        fprintf(stderr, "Could not create a output_document object\n");

                /* Create the root of the config file */
                time(&now);
                root = yaml_document_add_sequence(&output_document,
                                                  (yaml_char_t *)ctime(&now),
                                                  YAML_BLOCK_SEQUENCE_STYLE);

                /* create the property of the root sequence */
                properties1 = yaml_document_add_mapping(&output_document,
                                                        (yaml_char_t *)YAML_MAP_TAG,
                                                        YAML_BLOCK_MAPPING_STYLE);

                yaml_document_append_sequence_item(&output_document, root, properties1);

                /* add the map key1 : value1 to the property */
                key1 = yaml_document_add_scalar(&output_document,
                                                NULL,
                                                (yaml_char_t *)"FileName",
                                                -1,
                                                YAML_PLAIN_SCALAR_STYLE);
                value1 = yaml_document_add_scalar(&output_document,
                                                  NULL,
                                                  (yaml_char_t *)self->config->models[i]->filename,
                                                  -1,
                                                  YAML_PLAIN_SCALAR_STYLE);
                yaml_document_append_mapping_pair(&output_document, properties1, key1, value1);

                /* add the map key1 : seq0 to the first property */
                key1 = yaml_document_add_scalar(&output_document,
                                                NULL,
                                                (yaml_char_t *)"Objects",
                                                -1,
                                                YAML_PLAIN_SCALAR_STYLE);
                /* create the sequence of objects */
                seq0 = yaml_document_add_sequence(&output_document,
                                                  (yaml_char_t *)YAML_SEQ_TAG,
                                                  YAML_BLOCK_SEQUENCE_STYLE);
                for(size_t j=0; j<self->config->models[i]->len; j++){
                        int k;
                        int properties;
                        int key;
                        int value;
                        int seq1;

                        properties = yaml_document_add_mapping(&output_document,
                                                               (yaml_char_t *)YAML_MAP_TAG,
                                                               YAML_BLOCK_MAPPING_STYLE);
                        yaml_document_append_sequence_item(&output_document,seq0, properties);

                        key = yaml_document_add_scalar(&output_document,
                                                       NULL,
                                                       (yaml_char_t *)"Id", -1,
                                                       YAML_PLAIN_SCALAR_STYLE);

                        sprintf(number, "%d", self->config->models[i]->objects[j]->id);
                        value = yaml_document_add_scalar(&output_document,
                                                         NULL,
                                                         (yaml_char_t *)number,
                                                         -1,
                                                         YAML_PLAIN_SCALAR_STYLE);
                        yaml_document_append_mapping_pair(&output_document,properties,key,value);

                        key = yaml_document_add_scalar(&output_document,
                                                       NULL,
                                                       (yaml_char_t *)"Name",
                                                       -1,
                                                       YAML_PLAIN_SCALAR_STYLE);
                        value = yaml_document_add_scalar(&output_document,
                                                         NULL,
                                                         (yaml_char_t *)self->config->models[i]->objects[j]->axis_name,
                                                         -1,
                                                         YAML_PLAIN_SCALAR_STYLE);
                        yaml_document_append_mapping_pair(&output_document,properties,key,value);

                        key = yaml_document_add_scalar(&output_document,
                                                       NULL,
                                                       (yaml_char_t *)"Transformation",
                                                       -1,
                                                       YAML_PLAIN_SCALAR_STYLE);
                        seq1 = yaml_document_add_sequence(&output_document,
                                                          (yaml_char_t *)YAML_SEQ_TAG,
                                                          YAML_FLOW_SEQUENCE_STYLE);
                        yaml_document_append_mapping_pair(&output_document,properties, key, seq1);
                        for(k=0; k<16; k++){
                                sprintf(number, "%f", self->config->models[i]->objects[j]->transformation[k]);
                                value = yaml_document_add_scalar(&output_document,
                                                                 NULL,
                                                                 (yaml_char_t *)number,
                                                                 -1,
                                                                 YAML_PLAIN_SCALAR_STYLE);
                                yaml_document_append_sequence_item(&output_document,seq1,value);
                        }

                        key = yaml_document_add_scalar(&output_document,
                                                       NULL,
                                                       (yaml_char_t *)"Hide",
                                                       -1,
                                                       YAML_PLAIN_SCALAR_STYLE);
                        if(self->config->models[i]->objects[j]->hide)
                                value = yaml_document_add_scalar(&output_document,
                                                                 NULL,
                                                                 (yaml_char_t *)"yes",
                                                                 -1,
                                                                 YAML_PLAIN_SCALAR_STYLE);
                        else
                                value = yaml_document_add_scalar(&output_document,
                                                                 NULL,
                                                                 (yaml_char_t *)"no",
                                                                 -1,
                                                                 YAML_PLAIN_SCALAR_STYLE);
                        yaml_document_append_mapping_pair(&output_document,properties,key,value);
                }
                yaml_document_append_mapping_pair(&output_document, properties1, key1, seq0);

                /* flush the document */
                yaml_emitter_dump(&emitter, &output_document);
                fclose(file);

                yaml_document_delete(&output_document);
                yaml_emitter_delete(&emitter);
        }
}

/**
 * Hkl3D::hide_object:
 *
 * update the visibility of an Hkl3DObject in the bullet world
 * add or remove the object from the _btWorld depending on the hide
 * member of the object.
 **/
void hkl3d_hide_object(Hkl3D *self, Hkl3DObject *object, int hide)
{
        /* first update the G3DObject */
        object->hide = hide;
        object->g3d->hide = hide;
        if(object->hide){
                if (object->added){
                        self->_btWorld->removeCollisionObject(object->btObject);
                        object->added = false;
                }
        }else{
                if(!object->added){
                        self->_btWorld->addCollisionObject(object->btObject);
                        object->added = true;
                }
        }
}

/* remove an object from the model */
void hkl3d_remove_object(Hkl3D *self, Hkl3DObject *object)
{
        hkl3d_hide_object(self, object, TRUE);
        hkl3d_axis_detach_object(object->axis, object);

        /* now remove the G3DObject from the model */
        self->model->objects = g_slist_remove(self->model->objects, object->g3d);
        hkl3d_model_delete_object(object->model, object);
}


int hkl3d_is_colliding(Hkl3D *self)
{
        int numManifolds;
        struct timeval debut, fin;

        /* apply geometry transformation */
        hkl3d_apply_transformations(self);

        /* perform the collision detection and get numbers */
        gettimeofday(&debut, NULL);
        if(self->_btWorld){
                self->_btWorld->performDiscreteCollisionDetection();
                self->_btWorld->updateAabbs();
        }
        gettimeofday(&fin, NULL);
        timersub(&fin, &debut, &self->stats.collision);

        numManifolds = self->_btDispatcher->getNumManifolds();

        /* update Hkl3DObject collision from manifolds */
        for(size_t i=0; i<self->config->len; i++){
                for(size_t j=0; j<self->config->models[i]->len; j++){
                        Hkl3DObject *object = self->config->models[i]->objects[j];
                        object->is_colliding = FALSE;
                        for(int k=0; k<numManifolds; ++k){
                                btPersistentManifold *manifold = self->_btDispatcher->getManifoldByIndexInternal(k);
                                object->is_colliding |= object->btObject == manifold->getBody0();
                                object->is_colliding |= object->btObject == manifold->getBody1();
                        }
                }
        }

        return numManifolds != 0;
}

/**
 * Hkl3D::get_bounding_boxes:
 * @min:
 * @max:
 *
 * get the bounding boxes of the current world from the bullet internals.
 **/
void hkl3d_get_bounding_boxes(Hkl3D *self,
                              struct btVector3 *min, struct btVector3 *max)
{
        self->_btWorld->getBroadphase()->getBroadphaseAabb(*min, *max);
}

int hkl3d_get_nb_manifolds(Hkl3D *self)
{
        return self->_btDispatcher->getNumManifolds();
}

int hkl3d_get_nb_contacts(Hkl3D *self, int manifold)
{
        return self->_btDispatcher->getManifoldByIndexInternal(manifold)->getNumContacts();
}

void hkl3d_get_collision_coordinates(Hkl3D *self, int manifold, int contact,
                                     double *xa, double *ya, double *za,
                                     double *xb, double *yb, double *zb)
{
        btPersistentManifold *contactManifold;

        contactManifold = self->_btDispatcher->getManifoldByIndexInternal(manifold);
        btManifoldPoint & pt = contactManifold->getContactPoint(contact);
        btVector3 ptA = pt.getPositionWorldOnA();
        btVector3 ptB = pt.getPositionWorldOnB();

        *xa = ptA.x();
        *ya = ptA.y();
        *za = ptA.z();
        *xb = ptB.x();
        *yb = ptB.y();
        *zb = ptB.z();
}

void hkl3d_fprintf(FILE *f, const Hkl3D *self)
{

        fprintf(f, "filename : %s\n", self->filename);
        hkl3d_geometry_fprintf(f, self->geometry);
        fprintf(f, "\n");
        fprintf(f, "model : %p\n", self->model);
        hkl3d_stats_fprintf(f, &self->stats);
        hkl3d_config_fprintf(f, self->config);

        fprintf(f, "_btCollisionConfiguration : %p\n", self->_btCollisionConfiguration);
        fprintf(f, "_btBroadphase : %p\n", self->_btBroadphase);
        fprintf(f, "_btWorld : %p\n", self->_btWorld);
        fprintf(f, "_btDispatcher : %p\n", self->_btDispatcher);
#ifdef USE_PARALLEL_DISPATCHER
        fprintf(f, "_btThreadSupportInterface : %p\n", self->_btThreadSupportInterface);
#endif
}