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 #include <iostream>
  24 #include <string.h>
  25 #include <sys/time.h>
  26 #include "hkl3d.h"
  27 #include "btBulletCollisionCommon.h"
  28 #include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
  29
  30 #ifdef USE_PARALLEL_DISPATCHER
  31 # include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
  32 # include "BulletMultiThreaded/PlatformDefinitions.h"
  33 # include "BulletMultiThreaded/PosixThreadSupport.h"
  34 # include "BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
  35 #endif
  36
  37 Hkl3D::Hkl3D(const char *filename, HklGeometry *geometry)
  38 {
  39         _geometry = geometry;
  40         _len = HKL_LIST_LEN(geometry->axes);
  41
  42         // read the model from the file
  43         _context = g3d_context_new();
  44         _model = g3d_model_load_full(_context, filename,G3D_MODEL_SCALE);
  45
  46         // load model from libg3d into bullet Hullshape.
  47         if(this->isModelFileCompatibleWithGeometry() == false)
  48                 throw "Model not compatible with the HklGeometry\n";
  49         this->loadG3dFaceInBtConvexHullShape();
  50         // initialize the bullet part
  51         _btCollisionConfiguration = new btDefaultCollisionConfiguration();
  52
  53 #ifdef USE_PARALLEL_DISPATCHER
  54         int maxNumOutstandingTasks = 2;
  55         PosixThreadSupport::ThreadConstructionInfo constructionInfo("collision",
  56                                                                     processCollisionTask,
  57                                                                     createCollisionLocalStoreMemory,
  58                                                                     maxNumOutstandingTasks);
  59         _btThreadSupportInterface = new PosixThreadSupport(constructionInfo);
  60         _btDispatcher = new SpuGatheringCollisionDispatcher(_btThreadSupportInterface,
  61                                                             maxNumOutstandingTasks,
  62                                                             _btCollisionConfiguration);
  63 #else
  64         _btDispatcher = new btCollisionDispatcher(_btCollisionConfiguration);
  65 #endif
  66         btGImpactCollisionAlgorithm::registerAlgorithm(_btDispatcher);
  67
  68         btVector3 worldAabbMin(-1000,-1000,-1000);
  69         btVector3 worldAabbMax( 1000, 1000, 1000);
  70
  71         _btBroadphase = new btAxisSweep3(worldAabbMin, worldAabbMax);
  72
  73         _btCollisionWorld = new btCollisionWorld(_btDispatcher,
  74                                                  _btBroadphase,
  75                                                  _btCollisionConfiguration);
  76
  77         // add all objects to the world
  78         for(int i=0; i<_btCollisionObjects.size(); i++)
  79                 _btCollisionWorld->addCollisionObject(_btCollisionObjects[i]);
  80
  81         // if resp == true there is a problem in the diffractometer model.
  82         bool resp = this->is_colliding();
  83 }
  84
  85 Hkl3D::~Hkl3D(void)
  86 {
  87         size_t i;
  88         size_t len;
  89
  90         len = _btCollisionObjects.size();
  91
  92         // detach the objects from the collision world
  93         len = _btCollisionObjects.size();
  94         for(i=0; i<len; ++i)
  95                 _btCollisionWorld->removeCollisionObject(_btCollisionObjects[i]);
  96
  97         // delete all objects and shapes
  98         for(i=0; i<len; ++i){
  99                 delete _btMeshes[i];
 100                 delete _btCollisionShapes[i];
 101                 delete _btCollisionObjects[i];
 102         }
 103
 104         if (_btCollisionWorld) delete _btCollisionWorld;
 105         if (_btBroadphase) delete _btBroadphase;
 106         if (_btDispatcher) delete _btDispatcher;
 107 #ifdef USE_PARALLEL_DISPATCHER
 108         if (_btThreadSupportInterface){
 109                 //delete _btThreadSupportInterface;
 110                 //_btThreadSupportInterface = 0;
 111         }
 112 #endif
 113         if (_btCollisionConfiguration) delete _btCollisionConfiguration;
 114
 115         g3d_model_free(_model);
 116         g3d_context_free(_context);
 117 }
 118
 119 bool Hkl3D::is_colliding(void)
 120 {
 121         int i;
 122         bool res = true;
 123         struct timeval debut, fin, dt;
 124         int numManifolds;
 125
 126         // set the right transformation of each objects and get numbers
 127         gettimeofday(&debut, NULL);
 128         for(i=0;i<HKL_LIST_LEN(this->_geometry->holders);i++){
 129                 size_t j;
 130                 btQuaternion btQ(0, 0, 0, 1);
 131
 132                 size_t len = HKL_LIST_LEN(this->_geometry->holders[i].idx);
 133                 for(j=0; j<len; j++){
 134                         size_t k;
 135                         size_t idx = this->_geometry->holders[i].idx[j];
 136                         HklAxis *axis = &this->_geometry->axes[idx];
 137                         G3DMatrix G3DM[16];
 138
 139                         // convertion beetween hkl -> bullet coordinates
 140                         btQ *= btQuaternion(-axis->q.data[1],
 141                                           axis->q.data[3],
 142                                           axis->q.data[2],
 143                                           axis->q.data[0]);
 144
 145                         // move each object connected to that hkl Axis.
 146                         for(k=0; k<_movingBtCollisionObjects[idx].size(); ++k){
 147                                 _movingBtCollisionObjects[idx][k]->getWorldTransform().setRotation(btQ);
 148                                 _movingBtCollisionObjects[idx][k]->getWorldTransform().getOpenGLMatrix( G3DM );
 149                                 memcpy(_movingG3DObjects[idx][k]->transformation->matrix, &G3DM[0], sizeof(G3DM));
 150                         }
 151                 }
 152         }
 153         gettimeofday(&fin, NULL);
 154         timersub(&fin, &debut, &dt);
 155         fprintf(stdout, "transformation (%f ms)", dt.tv_sec*1000.+dt.tv_usec/1000.);
 156
 157         // perform the collision detection and get numbers
 158         gettimeofday(&debut, NULL);
 159         if(_btCollisionWorld)
 160                 _btCollisionWorld->performDiscreteCollisionDetection();
 161         gettimeofday(&fin, NULL);
 162         timersub(&fin, &debut, &dt);
 163         fprintf(stdout, " collision (%f ms)", dt.tv_sec*1000.+dt.tv_usec/1000.);
 164
 165         numManifolds = _btCollisionWorld->getDispatcher()->getNumManifolds();
 166         fprintf(stdout, " manifolds (%d)\n", numManifolds);
 167
 168         return numManifolds == 0;
 169 }
 170
 171 /*
 172  * Check that for each moving part in the hkl library there is a
 173  * corresponding model in the 3d model file.
 174  *!!!!!!!!!!!! Missing error handling in isModelCompatiblewithGeometry method
 175  */
 176 bool Hkl3D::isModelFileCompatibleWithGeometry(void)
 177 {
 178         size_t i;
 179         bool found;
 180
 181         for(i=0; i<_len; ++i){
 182                 const char *name;
 183                 GSList *objects;
 184                 std::string resp;
 185
 186                 name = ((HklParameter*)(&_geometry->axes[i]))->name;
 187                 objects = _model->objects;
 188                 found = false;
 189                 while(objects || !found){
 190                         G3DObject *object;
 191
 192                         object = (G3DObject *)objects->data;
 193                         if(!strcmp(object->name, name)){
 194                                 found = true;
 195                                 fprintf(stdout, "%s found in the 3d model file\n", object->name);
 196                         }
 197                         objects = g_slist_next(objects);
 198                 }
 199                 if(found == false){
 200                         resp = name;
 201                         resp += " not found in model file\n";
 202                 }
 203         }
 204
 205         return found;
 206 }
 207
 208 /*
 209  * load the model in the hkl3d structure
 210  */
 211 void Hkl3D::loadG3dFaceInBtConvexHullShape(void)
 212 {
 213         GSList *objects; // lets iterate from the first object.
 214
 215         // first initialize the _movingBtCollisionObjects with the right len.
 216         _movingBtCollisionObjects.resize(_len);
 217         _movingG3DObjects.resize(_len);
 218         // for each object in the model file do the g3d -> bullet conversion
 219         objects = _model->objects;
 220         while(objects){
 221                 G3DObject *object;
 222                 G3DMaterial *material;
 223
 224                 object = (G3DObject*)objects->data;
 225                 if(object->vertex_count){
 226                         GSList *faces;
 227                         btCollisionObject *btObject;
 228                         btTriangleMesh *trimesh;
 229                         float *vertex;
 230                         btGImpactMeshShape *shape;
 231                         int idx;
 232
 233                         trimesh = new btTriangleMesh();
 234                         trimesh->preallocateVertices(object->vertex_count);
 235                         faces = object->faces;
 236                         vertex = object->vertex_data;
 237
 238                         // extract the color from the first face
 239                         // usefull for the demo
 240                         material = ((G3DFace *)faces->data)->material;
 241                         _colors.push_back(btVector3(material->r, material->g, material->b));
 242                         fprintf(stdout, "colors: %f %f %f\n",
 243                                 material->r, material->g, material->b);
 244
 245                         while(faces){
 246                                 G3DFace * face;
 247
 248                                 face = (G3DFace*)faces->data;
 249
 250                                 btVector3 vertex0(vertex[3*(face->vertex_indices[0])],
 251                                                   vertex[3*(face->vertex_indices[0])+1],
 252                                                   vertex[3*(face->vertex_indices[0])+2]);
 253                                 btVector3 vertex1(vertex[3*(face->vertex_indices[1])],
 254                                                   vertex[3*(face->vertex_indices[1])+1],
 255                                                   vertex[3*(face->vertex_indices[1])+2]);
 256                                 btVector3 vertex2(vertex[3*(face->vertex_indices[2])],
 257                                                   vertex[3*(face->vertex_indices[2])+1],
 258                                                   vertex[3*(face->vertex_indices[2])+2]);
 259
 260                                 trimesh->addTriangle(vertex0, vertex1, vertex2, false);
 261                                 faces = g_slist_next(faces);
 262                         }
 263
 264                         // create the shape
 265                         shape = new btGImpactMeshShape(trimesh);
 266                         shape->setLocalScaling(btVector3(1.f,1.f,1.f));
 267                         shape->setMargin(0.0f);
 268                         shape->updateBound();
 269
 270                         // create the Object and add the shape
 271                         btObject = new btCollisionObject();
 272                         btObject->setCollisionShape(shape);
 273                         btObject->activate(true);
 274
 275                         // now populate also the moving part
 276                         idx = hkl_geometry_get_axis_idx_by_name(_geometry, object->name);
 277                         if (idx >= 0){
 278                                 _movingBtCollisionObjects[idx].push_back(btObject);
 279                                 object->transformation = g_new0(G3DTransformation, 1);
 280                                 _movingG3DObjects[idx].push_back(object);
 281                         }
 282
 283                         // remembers objects to avoid memory leak
 284                         _btCollisionShapes.push_back(shape);
 285                         _btMeshes.push_back(trimesh);
 286                         _btCollisionObjects.push_back(btObject);
 287                 }
 288                 objects = g_slist_next(objects);
 289         }
 290 }