From e012be58f4dbe93b6d5a1349a2c9686f41b0dde6 Mon Sep 17 00:00:00 2001
From: Daniel Fiser <danfis@danfis.cz>
Date: Tue, 15 Nov 2011 12:51:08 +0100
Subject: [PATCH] Several fixes concerning Oleh's review.

---
 include/ode/collision.h          |    2 +-
 include/ode/collision_space.h    |   10 +-
 ode/demo/demo_boxstack.cpp       |    2 +-
 ode/demo/demo_space_stress.cpp   |    2 +-
 ode/src/Makefile.am              |    2 +-
 ode/src/collision_kernel.h       |    4 +-
 ode/src/collision_sap_space.cpp  | 1066 --------------------------------------
 ode/src/collision_tsap_space.cpp | 1059 +++++++++++++++++++++++++++++++++++++
 ode/src/fermat/Makefile.am       |    3 +-
 ode/src/fermat/alloc.c           |   98 ----
 ode/src/fermat/alloc.h           |   84 ---
 ode/src/fermat/compiler.h        |   14 +-
 ode/src/fermat/hmap.c            |   10 +-
 ode/src/fermat/tasks.c           |   14 +-
 14 files changed, 1091 insertions(+), 1279 deletions(-)
 delete mode 100644 ode/src/collision_sap_space.cpp
 create mode 100644 ode/src/collision_tsap_space.cpp
 delete mode 100644 ode/src/fermat/alloc.c
 delete mode 100644 ode/src/fermat/alloc.h

diff --git a/include/ode/collision.h b/include/ode/collision.h
index 5d91aea..a1c7d1a 100644
--- a/include/ode/collision.h
+++ b/include/ode/collision.h
@@ -893,7 +893,7 @@ enum {
   dSimpleSpaceClass = dFirstSpaceClass,
   dHashSpaceClass,
   dSweepAndPruneSpaceClass, // SAP
-  dSAPSpaceClass,
+  dThreadedSAPSpaceClass,
   dQuadTreeSpaceClass,
   dLastSpaceClass = dQuadTreeSpaceClass,
 
diff --git a/include/ode/collision_space.h b/include/ode/collision_space.h
index 6af7211..8c35e6d 100644
--- a/include/ode/collision_space.h
+++ b/include/ode/collision_space.h
@@ -71,12 +71,12 @@ ODE_API dSpaceID dSweepAndPruneSpaceCreate( dSpaceID space, int axisorder );
  * If num_threads or hashsize are set to 0, default values 1 and 1023
  * respectively are used.
  */
-ODE_API dSpaceID dSAPSpaceCreate(dSpaceID space, unsigned int num_threads, unsigned int hashsize);
+ODE_API dSpaceID dThreadedSAPSpaceCreate(dSpaceID space, unsigned int num_threads, unsigned int hashsize);
 
-ODE_API unsigned int dSAPSpaceGetNumThreads(dSpaceID space);
-ODE_API void dSAPSpaceSetNumThreads(dSpaceID space, unsigned int num_threads);
-ODE_API unsigned int dSAPSpaceGetHashsize(dSpaceID space);
-ODE_API void dSAPSpaceSetHashsize(dSpaceID space, unsigned int hashsize);
+ODE_API unsigned int dThreadedSAPSpaceGetNumThreads(dSpaceID space);
+ODE_API void dThreadedSAPSpaceSetNumThreads(dSpaceID space, unsigned int num_threads);
+ODE_API unsigned int dThreadedSAPSpaceGetHashsize(dSpaceID space);
+ODE_API void dThreadedSAPSpaceSetHashsize(dSpaceID space, unsigned int hashsize);
 
 
 
diff --git a/ode/demo/demo_boxstack.cpp b/ode/demo/demo_boxstack.cpp
index 27ccfc8..fe000c4 100644
--- a/ode/demo/demo_boxstack.cpp
+++ b/ode/demo/demo_boxstack.cpp
@@ -628,7 +628,7 @@ int main (int argc, char **argv)
   // create world
   dInitODE2(0);
   world = dWorldCreate();
-  space = dSAPSpaceCreate(0, THREADS, 1023 * 1023 * 10);
+  space = dThreadedSAPSpaceCreate(0, THREADS, 1023 * 1023 * 10);
   contactgroup = dJointGroupCreate (0);
   dWorldSetGravity (world,0,0,-GRAVITY);
   dWorldSetCFM (world,1e-5);
diff --git a/ode/demo/demo_space_stress.cpp b/ode/demo/demo_space_stress.cpp
index b487d48..14e91ee 100644
--- a/ode/demo/demo_space_stress.cpp
+++ b/ode/demo/demo_space_stress.cpp
@@ -421,7 +421,7 @@ int main (int argc, char **argv)
 #elif 0
   space = dSweepAndPruneSpaceCreate (0, dSAP_AXES_XYZ);
 #elif 1
-  space = dSAPSpaceCreate (0, THREADS, 1023 * 1023);
+  space = dThreadedSAPSpaceCreate (0, THREADS, 1023 * 1023);
 #else
   space = dSimpleSpaceCreate(0);
 #endif
diff --git a/ode/src/Makefile.am b/ode/src/Makefile.am
index c3b0b08..ed2c5d7 100644
--- a/ode/src/Makefile.am
+++ b/ode/src/Makefile.am
@@ -26,7 +26,7 @@ libode_la_SOURCES =     nextafterf.c \
                         collision_kernel.cpp collision_kernel.h \
                         collision_quadtreespace.cpp \
                         collision_sapspace.cpp \
-                        collision_sap_space.cpp \
+                        collision_tsap_space.cpp \
                         collision_space.cpp \
                         collision_space_internal.h \
                         collision_std.h \
diff --git a/ode/src/collision_kernel.h b/ode/src/collision_kernel.h
index 44a356f..389a92a 100644
--- a/ode/src/collision_kernel.h
+++ b/ode/src/collision_kernel.h
@@ -29,8 +29,6 @@ internal data structures and functions for collision detection.
 #ifndef _ODE_COLLISION_KERNEL_H_
 #define _ODE_COLLISION_KERNEL_H_
 
-#include <stdint.h>
-
 #include <ode/common.h>
 #include <ode/contact.h>
 #include <ode/collision.h>
@@ -111,7 +109,7 @@ struct dxGeom : public dBase {
   dReal aabb[6];	// cached AABB for this space
   unsigned long category_bits,collide_bits;
 
-  uint32_t sap;
+  unsigned int sap;
 
   dxGeom (dSpaceID _space, int is_placeable);
   virtual ~dxGeom();
diff --git a/ode/src/collision_sap_space.cpp b/ode/src/collision_sap_space.cpp
deleted file mode 100644
index f15f939..0000000
--- a/ode/src/collision_sap_space.cpp
+++ /dev/null
@@ -1,1066 +0,0 @@
-/*************************************************************************
- *                                                                       *
- * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
- * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
- *                                                                       *
- * This library is free software; you can redistribute it and/or         *
- * modify it under the terms of EITHER:                                  *
- *   (1) The GNU Lesser General Public License as published by the Free  *
- *       Software Foundation; either version 2.1 of the License, or (at  *
- *       your option) any later version. The text of the GNU Lesser      *
- *       General Public License is included with this library in the     *
- *       file LICENSE.TXT.                                               *
- *   (2) The BSD-style license that is included with this library in     *
- *       the file LICENSE-BSD.TXT.                                       *
- *                                                                       *
- * This 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 files    *
- * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
- *                                                                       *
- *************************************************************************/
-
-/**
- * Parallel Sweep and Prune by Daniel Fiser <danfis@danfis.cz>
- * This code is adapted from fermat library
- * (http://gitview.danfis.cz/fermat).
- *
- *
- * This work was supported by SYMBRION and REPLICATOR projects.
- * The SYMBRION project is funded by European Commission within the work
- * "Future and Emergent Technologies Proactive" under grant agreement no.
- * 216342.
- * The REPLICATOR project is funded within the work programme "Cognitive
- * Systems, Interaction, Robotics" under grant agreement no. 216240.
- * http://www.symbrion.eu/
- * http://www.replicators.eu/
- */
-
-#include <pthread.h>
-#include <ode/common.h>
-#include <ode/matrix.h>
-#include <ode/collision_space.h>
-#include <ode/collision.h>
-
-#include "config.h"
-#include "collision_kernel.h"
-#include "collision_space_internal.h"
-#include "collision_util.h"
-
-#include "fermat/list.h"
-#include "fermat/hmap.h"
-#include "fermat/hfunc.h"
-#include "fermat/tasks.h"
-
-
-#ifdef dDOUBLE
-typedef uint64_t dUInt;
-#else
-typedef uint32_t dUInt;
-#endif
-
-union _real_uint_t {
-    dReal f;
-    dUInt i;
-};
-typedef union _real_uint_t dRealUInt;
-
-#define RADIX_SORT_MASK 0xffu
-
-#define PTR_SIZE sizeof(void *)
-#define PTR32 (PTR_SIZE / 4)
-
-#define SWAP(src, dst, tmp) do { \
-        (tmp) = (src); \
-        (src) = (dst); \
-        (dst) = (tmp); \
-    } while (0)
-
-# define DBG(format, ...) do { \
-    fprintf(stderr, "%s :: " format "\n", __func__, __VA_ARGS__); \
-    fflush(stderr); \
-    } while (0)
-
-# define DBG2(str) do { \
-    fprintf(stderr, "%s :: " str "\n", __func__); \
-    fflush(stderr); \
-    } while (0)
-
-
-
-
-struct sap_geom_t {
-    dxGeom *g;
-    uint32_t min[3];
-    dReal max[3];
-    uint8_t dirty;
-};
-
-struct sap_min_t {
-    uint32_t geom;
-    dReal val;
-};
-
-struct sap_pair_t {
-    dxGeom *g[2];    /*!< Reference to geoms */
-    fer_list_t hmap; /*!< Connection into hash map */
-    fer_list_t list; /*!< Connection into sap.pairs list */
-};
-
-struct radix_sort_t {
-    sap_min_t *min[3];
-    size_t min_len[3], min_alloc[3];
-    uint32_t *counter[3];  /*!< Array for count-sort */
-    uint32_t negative[3];  /*!< Number of negative values */
-};
-
-
-/** Creates new pair from other sap_pair_t struct */
-static sap_pair_t *pairNew(dxGeom *g1, dxGeom *g2);
-/** Deletes collide pair */
-static void pairDel(sap_pair_t *p);
-
-/** Callbacks for hash table */
-static uint32_t pairHash(fer_list_t *key, void *sap);
-static int pairEq(const fer_list_t *key1, const fer_list_t *key2, void *sap);
-
-static void __findPairsTask(int id, void *data,
-                            const fer_tasks_thinfo_t *thinfo);
-static void __radixSortTask(int id, void *data,
-                            const fer_tasks_thinfo_t *thinfo);
-static void __cleanGeomsTask(int id, void *data,
-                             const fer_tasks_thinfo_t *thinfo);
-
-struct dxSAPSpace : public dxSpace
-{
-    uint32_t num_threads;
-    uint32_t hashsize;
-
-    sap_geom_t *geoms;  /*!< Array of geoms */
-    size_t geoms_len;   /*!< Length of .geoms[] */
-    size_t geoms_alloc; /*!< Size of allocated memory for .geoms[] */
-    sap_min_t *min[3];  /*!< Array of min values */
-    uint8_t dirt;       /*!< True if any geom was changed */
-    uint32_t *dirty_geoms; /*!< Array of indices of dirty geoms */ 
-    size_t dirty_geoms_alloc;
-    size_t dirty_geoms_len;
-
-    radix_sort_t rs; /*!< Cached radix sort struct */
-
-    fer_list_t *pairs;    /*!< Array of lists of possible collide pairs (sap_pair_t) */
-    fer_hmap_t *pairs_reg;           /*!< Register (hash map) of collide pairs */
-    pthread_mutex_t *pairs_reg_lock; /*!< Array of locks for .pairs_reg */
-
-    fer_tasks_t *tasks;
-    struct {
-        void *data;
-        dNearCallback *callback;
-        int axis;
-
-        pthread_mutex_t lock;
-        int next_geom;
-    } context;
-
-
-    // Constructor / Destructor
-    dxSAPSpace(dSpaceID _space, unsigned int num_threads, unsigned int hashsize);
-    ~dxSAPSpace();
-
-    // dxSpace
-    virtual dxGeom* getGeom(int i);
-    virtual void add(dxGeom* g);
-    virtual void remove(dxGeom* g);
-    virtual void dirty(dxGeom* g);
-    virtual void computeAABB();
-    virtual void cleanGeoms();
-    virtual void collide(void *data, dNearCallback *callback);
-    virtual void collide2(void *data, dxGeom *geom, dNearCallback *callback);
-
-    void setNumThreads(unsigned int num_threads);
-    void setHashsize(unsigned int hashsize);
-
-  private:
-    void _setDirty(sap_geom_t *geom);
-    /** Removes all pairs from all buckets */
-    void _pairRemoveAll();
-    /** Set min/max values of projected AABB on a specified axis */
-    void _setMinMax(dxGeom *geom, unsigned int axis,
-                    dReal *min, dReal *max);
-    void _sort();
-    /** Performs radix sort */
-    void _radixSort(unsigned int axis);
-    /** Finds collide pairs and calls for each pair a callback */
-    void _findPairs(void *data, dNearCallback *callback);
-    void _findPairsGeom(sap_geom_t *geom, int bucket);
-    void _findPairsGeom1(sap_geom_t *geom, int axis,
-                         void *data, dNearCallback *callback);
-
-    void __reset();
-    void __update();
-
-    friend void __findPairsTask(int id, void *data,
-                                const fer_tasks_thinfo_t *thinfo);
-    friend void __radixSortTask(int id, void *data,
-                                const fer_tasks_thinfo_t *thinfo);
-    friend void __cleanGeomsTask(int id, void *data,
-                                 const fer_tasks_thinfo_t *thinfo);
-};
-
-ODE_API dSpaceID dSAPSpaceCreate(dxSpace* space, unsigned int num_threads, unsigned int hashsize)
-{
-    return new dxSAPSpace(space, num_threads, hashsize);
-}
-
-ODE_API unsigned int dSAPSpaceGetNumThreads(dxSpace *space)
-{
-    return ((dxSAPSpace *)space)->num_threads;
-}
-
-ODE_API void dSAPSpaceSetNumThreads(dSpaceID space, unsigned int num_threads)
-{
-    ((dxSAPSpace *)space)->setNumThreads(num_threads);
-}
-
-ODE_API unsigned int dSAPSpaceGetHashsize(dSpaceID space)
-{
-    return ((dxSAPSpace *)space)->hashsize;
-}
-
-ODE_API void dSAPSpaceSetHashsize(dSpaceID space, unsigned int hashsize)
-{
-    ((dxSAPSpace *)space)->setHashsize(hashsize);
-}
-
-dxSAPSpace::dxSAPSpace(dSpaceID _space, unsigned int _num_threads, unsigned int _hashsize)
-    : dxSpace(_space)
-{
-    int i;
-
-	type = dSweepAndPruneSpaceClass;
-
-	// Init AABB to infinity
-	aabb[0] = -dInfinity;
-	aabb[1] = dInfinity;
-	aabb[2] = -dInfinity;
-	aabb[3] = dInfinity;
-	aabb[4] = -dInfinity;
-	aabb[5] = dInfinity;
-
-    num_threads = _num_threads >= 1 ? _num_threads : 1;
-    hashsize = _hashsize > 0 ? _hashsize : 1023;
-
-    // init geoms and min arrays
-    geoms_alloc = 100;
-    geoms_len = 0;
-    geoms = (sap_geom_t *)dAlloc(sizeof(sap_geom_t) * geoms_alloc);
-    for (i = 0; i < 3; i++){
-        min[i] = (sap_min_t *)dAlloc(sizeof(sap_min_t) * geoms_alloc);
-    }
-
-    // dirty geoms
-    dirty_geoms_alloc = 10;
-    dirty_geoms_len = 0;
-    dirty_geoms = (uint32_t *)dAlloc(sizeof(uint32_t) * dirty_geoms_alloc);
-
-    dirt = 0;
-
-    // init radix sort
-    rs.min[0] = rs.min[1] = rs.min[2] = NULL;
-    rs.min_len[0] = rs.min_len[1] = rs.min_len[2] = 0;
-    rs.min_alloc[0] = rs.min_alloc[1] = rs.min_alloc[2] = 0;
-    rs.counter[0] = (uint32_t *)dAlloc(sizeof(uint32_t) * 256);
-    rs.counter[1] = (uint32_t *)dAlloc(sizeof(uint32_t) * 256);
-    rs.counter[2] = (uint32_t *)dAlloc(sizeof(uint32_t) * 256);
-
-
-    pairs = NULL;
-    pairs_reg = NULL;
-    pairs_reg_lock = NULL;
-    tasks = NULL;
-
-    pthread_mutex_init(&context.lock, NULL);
-}
-
-dxSAPSpace::~dxSAPSpace()
-{
-    int i;
-
-    if (geoms)
-        free(geoms);
-
-    for (i = 0; i < 3; i++){
-        if (min[i])
-            free(min[i]);
-    }
-
-    _pairRemoveAll();
-    if (pairs)
-        free(pairs);
-    if (pairs_reg)
-        ferHMapDel(pairs_reg);
-    if (pairs_reg_lock){
-        for (i = 0; i < num_threads; i++){
-            pthread_mutex_destroy(&pairs_reg_lock[i]);
-        }
-        free(pairs_reg_lock);
-    }
-
-    for (i = 0; i < 3; i++){
-        if (rs.min[i])
-            free(rs.min[i]);
-        if (rs.counter[i])
-            free(rs.counter[i]);
-    }
-
-    if (tasks)
-        ferTasksDel(tasks);
-
-    pthread_mutex_destroy(&context.lock);
-}
-
-void dxSAPSpace::__reset()
-{
-    int i;
-
-    _pairRemoveAll();
-    if (pairs)
-        free(pairs);
-    if (pairs_reg)
-        ferHMapDel(pairs_reg);
-    if (pairs_reg_lock){
-        for (i = 0; i < num_threads; i++){
-            pthread_mutex_destroy(&pairs_reg_lock[i]);
-        }
-        free(pairs_reg_lock);
-    }
-
-    if (tasks)
-        ferTasksDel(tasks);
-
-    pairs          = NULL;
-    pairs_reg      = NULL;
-    pairs_reg_lock = NULL;
-    tasks          = NULL;
-}
-
-void dxSAPSpace::__update()
-{
-    int i;
-
-    // allocate buckets for pairs
-    if (!pairs){
-        pairs = (fer_list_t *)dAlloc(sizeof(fer_list_t) * num_threads);
-        for (i = 0; i < num_threads; i++){
-            ferListInit(&pairs[i]);
-        }
-    }else{
-        // remove all old pairs
-        _pairRemoveAll();
-    }
-
-    // create register for collide pairs
-    if (!pairs_reg){
-        pairs_reg = ferHMapNew(hashsize, pairHash, pairEq, (void *)this);
-        if (num_threads > 1){
-            pairs_reg_lock = (pthread_mutex_t *)dAlloc(sizeof(pthread_mutex_t) * num_threads);
-            for (i = 0; i < num_threads; i++){
-                pthread_mutex_init(&pairs_reg_lock[i], NULL);
-            }
-        }
-    }
-
-    // create thread pool
-    if (!tasks && num_threads > 1){
-        tasks = ferTasksNew(num_threads);
-        ferTasksRun(tasks);
-    }
-
-}
-
-void dxSAPSpace::setNumThreads(unsigned int _num_threads)
-{
-    __reset();
-    num_threads = _num_threads >= 1 ? _num_threads : 1;
-}
-
-void dxSAPSpace::setHashsize(unsigned int _hashsize)
-{
-    __reset();
-    hashsize = _hashsize >= 1 ? _hashsize : 1023;
-}
-
-dxGeom* dxSAPSpace::getGeom(int i)
-{
-    if (i < 0 || i >geoms_len)
-        return NULL;
-    return geoms[i].g;
-}
-
-void dxSAPSpace::add(dxGeom* geom)
-{
-    int i;
-
-    dxSpace::add(geom);
-
-    if (geoms_len >= geoms_alloc){
-        geoms_alloc *= 2;
-        geoms = (sap_geom_t *)dRealloc(geoms, 0, sizeof(sap_geom_t) * geoms_alloc);
-
-        for (i = 0; i < 3; i++){
-            min[i] = (sap_min_t *)dRealloc(min[i], 0, sizeof(sap_min_t) * geoms_alloc);
-        }
-    }
-
-    geoms[geoms_len].g = geom;
-    for (i = 0; i < 3; i++){
-        min[i][geoms_len].geom = geoms_len;
-        geoms[geoms_len].min[i] = geoms_len;
-    }
-    geoms[geoms_len].dirty = 0;
-
-    geom->sap = geoms_len;
-    _setDirty(&geoms[geoms_len]);
-
-    geoms_len++;
-    dirt = 1;
-}
-
-void dxSAPSpace::remove(dxGeom* geom)
-{
-    int i;
-
-    geoms[geom->sap] = geoms[geoms_len - 1];
-    geom->sap = -1;
-    geoms_len--;
-    for (i = 0; i < geoms_len; i++){
-        geoms[i].g->sap = i;
-        geoms[i].min[0] = i;
-        geoms[i].min[1] = i;
-        geoms[i].min[2] = i;
-        min[0][i].geom = i;
-        min[1][i].geom = i;
-        min[2][i].geom = i;
-        _setDirty(&geoms[i]);
-    }
-
-    dxSpace::remove(geom);
-}
-
-void dxSAPSpace::dirty(dxGeom* geom)
-{
-    int i;
-    sap_geom_t *g;
-    dReal _min, _max;
-
-    dxSpace::dirty(geom);
-
-    g = &geoms[geom->sap];
-    _setDirty(g);
-
-    dirt = 1;
-}
-
-void dxSAPSpace::computeAABB()
-{
-}
-
-static void __cleanGeomsTask(int id, void *data,
-                             const fer_tasks_thinfo_t *thinfo)
-{
-    dxSAPSpace *sap = (dxSAPSpace *)data;
-    dReal _min, _max;
-    sap_geom_t *g;
-    int i, ax, from, to, len;
-
-
-    len = sap->dirty_geoms_len / sap->num_threads;
-    from = id * len;
-    to   = from + len;
-    if (id == sap->num_threads - 1)
-        to = sap->dirty_geoms_len;
-
-    for (i = from; i < to; i++){
-        for (ax = 0; ax < 3; ax++){
-            g = &sap->geoms[sap->dirty_geoms[i]];
-            g->dirty = 0;
-
-            if (IS_SPACE(g->g))
-                ((dxSpace *)g->g)->cleanGeoms();
-            g->g->recomputeAABB();
-            g->g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
-
-            // update min/max values
-            sap->_setMinMax(g->g, ax, &_min, &_max);
-            sap->min[ax][g->min[ax]].val = _min;
-            g->max[ax] = _max;
-        }
-    }
-}
-
-void dxSAPSpace::cleanGeoms()
-{
-    uint32_t i, ax;
-    dReal _min, _max;
-    sap_geom_t *g;
-
-    __update();
-
-    if (num_threads > 1 && dirty_geoms_len > num_threads * 10){
-        for (i = 0; i < num_threads; i++){
-            ferTasksAdd(tasks, __cleanGeomsTask, i, (void *)this);
-        }
-        ferTasksBarrier(tasks);
-    }else{
-        for (i = 0; i < dirty_geoms_len; i++){
-            for (ax = 0; ax < 3; ax++){
-                g = &geoms[dirty_geoms[i]];
-                g->dirty = 0;
-
-                if (IS_SPACE(g->g))
-                    ((dxSpace *)g->g)->cleanGeoms();
-                g->g->recomputeAABB();
-                g->g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
-
-                // update min/max values
-                _setMinMax(g->g, ax, &_min, &_max);
-                min[ax][g->min[ax]].val = _min;
-                g->max[ax] = _max;
-            }
-        }
-    }
-    dirty_geoms_len = 0;
-    dirt = 0;
-}
-
-void dxSAPSpace::collide(void *data, dNearCallback *callback)
-{
-    int i;
-
-    if (dirt){
-        cleanGeoms();
-
-        _sort();
-    }
-
-    _findPairs(data, callback);
-}
-
-void dxSAPSpace::collide2(void *data, dxGeom *geom, dNearCallback *callback)
-{
-    size_t i;
-
-    for (i = 0; i < geoms_len; i++){
-        collideAABBs(geoms[i].g, geom, data, callback);
-    }
-}
-
-
-void dxSAPSpace::_setDirty(sap_geom_t *geom)
-{
-    if (geom->dirty)
-        return;
-
-    if (dirty_geoms_len >= dirty_geoms_alloc){
-        dirty_geoms_alloc *= 2;
-        dirty_geoms = (uint32_t *)dRealloc(dirty_geoms, 0, sizeof(uint32_t) * dirty_geoms_alloc);
-    }
-
-    dirty_geoms[dirty_geoms_len] = geom->g->sap;
-    dirty_geoms_len++;
-}
-
-void dxSAPSpace::_pairRemoveAll()
-{
-    fer_list_t *item;
-    sap_pair_t *pair;
-    size_t i;
-
-    if (!pairs)
-        return;
-
-    for (i = 0; i < num_threads; i++){
-        while (!ferListEmpty(&pairs[i])){
-            item = ferListNext(&pairs[i]);
-            pair = FER_LIST_ENTRY(item, sap_pair_t, list);
-            pairDel(pair);
-        }
-    }
-}
-
-static inline void __updateMinMax(dReal m, dReal *min, dReal *max)
-{
-    if (m < *min)
-        *min = m;
-    if (m > *max)
-        *max = m;
-}
-
-void dxSAPSpace::_setMinMax(dxGeom *geom, unsigned int ax,
-                             dReal *min, dReal *max)
-{
-    /** As axis are considered:
-     *  0: 1, 1, 0
-     *  1: 0, 1, 1
-     *  2: 1, 0, 1
-     */
-
-    dReal a, b, c, d;
-    dVector3 center;
-
-    if (ax == 0){
-        a = geom->aabb[0] + geom->aabb[2];
-        b = geom->aabb[0] + geom->aabb[3];
-        c = geom->aabb[1] + geom->aabb[2];
-        d = geom->aabb[1] + geom->aabb[3];
-    }else if (ax == 1){
-        a = geom->aabb[2] + geom->aabb[4];
-        b = geom->aabb[2] + geom->aabb[5];
-        c = geom->aabb[3] + geom->aabb[4];
-        d = geom->aabb[3] + geom->aabb[5];
-    }else{
-        a = geom->aabb[0] + geom->aabb[4];
-        b = geom->aabb[0] + geom->aabb[5];
-        c = geom->aabb[1] + geom->aabb[4];
-        d = geom->aabb[1] + geom->aabb[5];
-    }
-
-    *min = *max = a;
-    __updateMinMax(b, min, max);
-    __updateMinMax(c, min, max);
-    __updateMinMax(d, min, max);
-}
-
-
-
-/*** RADIX SORT ***/
-static inline dUInt realAsUInt(dReal x)
-{
-    dRealUInt v;
-    v.f = x;
-    return v.i;
-}
-
-static inline uint32_t radixSortVal(const sap_min_t *m,
-                                    uint32_t shift)
-{
-    dUInt val;
-
-    val = realAsUInt(m->val);
-    val = (val >> shift) & RADIX_SORT_MASK;
-
-    return val;
-}
-
-static inline void radixSortZeroizeCounter(uint32_t *counter)
-{
-    int i;
-    for (i = 0; i < 256; i++)
-        counter[i] = 0;
-}
-
-static void radixSortCount(const sap_min_t *m,
-                           uint32_t len,
-                           uint32_t *counter, uint32_t shift)
-{
-    uint32_t i;
-    uint32_t val;
-
-    radixSortZeroizeCounter(counter);
-    for (i = 0; i < len; i++){
-        val = radixSortVal(&m[i], shift);
-        ++counter[val];
-    }
-}
-
-static void radixSortCountFinal(const sap_min_t *m,
-                                uint32_t len,
-                                uint32_t *counter,
-                                uint32_t *negative,
-                                uint32_t shift)
-{
-    uint32_t i;
-    uint32_t val;
-
-    radixSortZeroizeCounter(counter);
-    *negative = 0;
-    for (i = 0; i < len; i++){
-        val = radixSortVal(&m[i], shift);
-        ++counter[val];
-
-        if (m[i].val < REAL(0.))
-            ++(*negative);
-    }
-}
-
-static void radixSortFixCounter(uint32_t *counter)
-{
-    uint32_t i, val, t;
-
-    val = 0;
-    for (i = 0; i < 256; i++){
-        t = counter[i];
-        counter[i] = val;
-        val += t;
-    }
-}
-
-static void radixSortSort(sap_min_t *src,
-                          sap_min_t *dst,
-                          uint32_t len,
-                          uint32_t *counter,
-                          uint32_t shift)
-{
-    uint32_t i, val;
-
-    for (i = 0; i < len; i++){
-        val = radixSortVal(&src[i], shift);
-        dst[counter[val]] = src[i];
-        ++counter[val];
-    }
-}
-
-static void radixSortSortFinal(sap_min_t *src,
-                               uint32_t srclen,
-                               sap_min_t *dst,
-                               uint32_t dstlen,
-                               uint32_t *counter,
-                               uint32_t negative,
-                               uint32_t shift,
-                               sap_geom_t *geoms, int ax)
-{
-    uint32_t i, val, pos;
-
-    for (i = 0; i < srclen; i++){
-        val = radixSortVal(&src[i], shift);
-        pos = counter[val];
-        if (pos >= dstlen - negative){
-            pos = dstlen - pos - 1;
-        }else{
-            pos = negative + pos;
-        }
-
-        dst[pos] = src[i];
-
-        geoms[dst[pos].geom].min[ax] = pos;
-
-        ++counter[val];
-    }
-}
-
-void dxSAPSpace::_radixSort(unsigned int ax)
-{
-    /** Radix sort of one axis is performed in single thread because at
-     *  least in my benchmarks and on my computer it was always faster than
-     *  in multiple threads.
-     *  However, the multi-thread implementation is available in fermat
-     *  library. */
-    sap_min_t *src, *dst, *tmp;
-    uint32_t shift, i, len;
-
-    // allocate temporary array
-    if (rs.min_alloc[ax] < geoms_len){
-        if (rs.min[ax])
-            free(rs.min[ax]);
-        rs.min_alloc[ax] = geoms_alloc;
-        rs.min[ax] = (sap_min_t *)dAlloc(sizeof(sap_min_t) * rs.min_alloc[ax]);
-    }
-    rs.min_len[ax] = geoms_len;
-
-
-    len = (uint32_t)sizeof(dReal) - 1;
-    shift = 0;
-    src = min[ax];
-    dst = rs.min[ax];
-    for (i = 0; i < len; i++){
-        radixSortCount(src, rs.min_len[ax], rs.counter[ax], shift);
-        radixSortFixCounter(rs.counter[ax]);
-        radixSortSort(src, dst, rs.min_len[ax], rs.counter[ax], shift);
-
-        shift += 8;
-        SWAP(src, dst, tmp);
-    }
-
-    radixSortCountFinal(src, rs.min_len[ax], rs.counter[ax], &rs.negative[ax], shift);
-    radixSortFixCounter(rs.counter[ax]);
-    radixSortSortFinal(src, rs.min_len[ax],
-                       dst, rs.min_len[ax],
-                       rs.counter[ax], rs.negative[ax], shift,
-                       geoms, ax);
-}
-
-static void __radixSortTask(int id, void *data,
-                            const fer_tasks_thinfo_t *thinfo)
-{
-    dxSAPSpace *sap = (dxSAPSpace *)data;
-    sap->_radixSort(id);
-}
-
-void dxSAPSpace::_sort()
-{
-    int i;
-
-    __update();
-
-    if (num_threads > 1){
-        for (i = 0; i < 3; i++){
-            ferTasksAdd(tasks, __radixSortTask, i, (void *)this);
-        }
-        ferTasksBarrier(tasks);
-    }else{
-        for (i = 0; i < 3; i++){
-            _radixSort(i);
-        }
-    }
-}
-/*** RADIX SORT END ***/
-
-
-
-/*** FIND PAIRS ***/
-
-static dReal minMaxVariance(const sap_min_t *m, size_t mlen)
-{
-    size_t i;
-    dReal avg, len, var, tr1, tr2;
-
-    tr1 = -FLT_MAX / REAL(2.);
-    tr2 =  FLT_MAX / REAL(2.);
-    avg = len = var = 0;
-    for (i = 0; i < mlen; ++i, ++m){
-        if (m->val > tr1 && m->val < tr2){
-            avg += m->val;
-            len += REAL(1.);
-            var += m->val * m->val;
-        }
-    }
-
-    avg /= len;
-    var /= len;
-    var -= avg * avg;
-
-    return var;
-}
-
-static int overlap(dxGeom *g1, dxGeom *g2)
-{
-    // no contacts if both geoms on the same body, and the body is not 0
-    if (g1->body == g2->body && g1->body)
-        return 0;
-
-    // test if the category and collide bitfields match
-    if (((g1->category_bits & g2->collide_bits) ||
-                (g2->category_bits & g1->collide_bits)) == 0) {
-        return 0;
-    }
-
-    // if the bounding boxes are disjoint then don't do anything
-    dReal *bounds1 = g1->aabb;
-    dReal *bounds2 = g2->aabb;
-    if (bounds1[0] > bounds2[1] ||
-            bounds1[1] < bounds2[0] ||
-            bounds1[2] > bounds2[3] ||
-            bounds1[3] < bounds2[2] ||
-            bounds1[4] > bounds2[5] ||
-            bounds1[5] < bounds2[4]){
-        return 0;
-    }
-
-    // check if either object is able to prove that it doesn't intersect the
-    // AABB of the other
-    if (g1->AABBTest (g2,bounds2) == 0)
-        return 0;
-    if (g2->AABBTest (g1,bounds1) == 0)
-        return 0;
-
-    return 1;
-}
-
-void dxSAPSpace::_findPairsGeom(sap_geom_t *g, int bucket)
-{
-    int j;
-    sap_min_t *_min;
-    dxGeom *g2;
-    sap_pair_t *pair;
-    uint32_t id;
-    int lockid;
-
-    _min = min[context.axis];
-    for (j = g->min[context.axis] + 1; j < geoms_len && _min[j].val < g->max[context.axis]; j++){
-        g2 = geoms[_min[j].geom].g;
-        if (overlap(g->g, g2)){
-            // create new pair
-            pair = pairNew(g->g, g2);
-
-            // add this pair to list of pairs (into specified bucket)
-            ferListAppend(&pairs[bucket], &pair->list);
-
-            // obtain hash ID
-            id = ferHMapID(pairs_reg, &pair->hmap);
-
-            // find out mutext corresponding to id
-            lockid = id % num_threads;
-
-            // check if this pair isn't already registered
-            pthread_mutex_lock(&pairs_reg_lock[lockid]);
-            if (ferHMapIDGet(pairs_reg, id, &pair->hmap) == NULL){
-                ferHMapIDPut(pairs_reg, id, &pair->hmap);
-                pthread_mutex_unlock(&pairs_reg_lock[lockid]);
-            }else{
-                pthread_mutex_unlock(&pairs_reg_lock[lockid]);
-                pairDel(pair);
-            }
-        }
-    }
-}
-
-void dxSAPSpace::_findPairsGeom1(sap_geom_t *geom, int axis,
-                                 void *data, dNearCallback *callback)
-{
-    sap_min_t *_min;
-    dxGeom *g2;
-    sap_pair_t *pair, pair_test;
-    int i;
-
-    _min = min[axis];
-    for (i = geom->min[axis] + 1; i < geoms_len && _min[i].val < geom->max[axis]; i++){
-        g2 = geoms[_min[i].geom].g;
-        if (overlap(geom->g, g2)){
-            pair_test.g[0] = geom->g;
-            pair_test.g[1] = g2;
-
-            if (ferHMapGet(pairs_reg, &pair_test.hmap) == NULL){
-                // create new pair
-                pair = pairNew(geom->g, g2);
-
-                // add this pair to list of pairs (into specified bucket)
-                ferListAppend(&pairs[0], &pair->list);
-
-                // add pair into register
-                ferHMapPut(pairs_reg, &pair->hmap);
-
-                // call callback
-                callback(data, geom->g, g2);
-            }
-        }
-    }
-}
-
-static void __findPairsTask(int id, void *data,
-                            const fer_tasks_thinfo_t *thinfo)
-{
-    dxSAPSpace *sap = (dxSAPSpace *)data;
-    int i, max;
-
-    i = id * 5;
-    while (i < sap->geoms_len){
-        max = (i + 5 < sap->geoms_len ? i + 5 : sap->geoms_len);
-        for (; i < max; i++){
-            sap->_findPairsGeom(&sap->geoms[i], id);
-        }
-
-        pthread_mutex_lock(&sap->context.lock);
-        i = sap->context.next_geom;
-        sap->context.next_geom += 5;
-        pthread_mutex_unlock(&sap->context.lock);
-    }
-}
-
-void dxSAPSpace::_findPairs(void *data, dNearCallback *callback)
-{
-    size_t i, axis;
-    dReal var[3];
-    fer_list_t *item;
-    sap_pair_t *pair;
-
-    __update();
-
-    // find axis with largest variance
-    var[0] = minMaxVariance(min[0], geoms_len);
-    var[1] = minMaxVariance(min[1], geoms_len);
-    var[2] = minMaxVariance(min[2], geoms_len);
-    if (var[0] > var[1]){
-        if (var[0] > var[2]){
-            axis = 0;
-        }else{
-            axis = 2;
-        }
-    }else if (var[1] > var[2]){
-        axis = 1;
-    }else{
-        axis = 2;
-    }
-
-
-    if (num_threads > 1){
-        // set up context and run prunning in separate threads
-        context.data     = data;
-        context.callback = callback;
-        context.axis     = axis;
-        context.next_geom = num_threads;
-        for (i = 0; i < num_threads; i++){
-            ferTasksAdd(tasks, __findPairsTask, i, (void *)this);
-        }
-        ferTasksBarrier(tasks);
-
-        for (i = 0; i < num_threads; i++){
-            FER_LIST_FOR_EACH(&pairs[i], item){
-                pair = FER_LIST_ENTRY(item, sap_pair_t, list);
-                context.callback(context.data, pair->g[0], pair->g[1]);
-            }
-        }
-    }else{
-        for (i = 0; i < geoms_len; i++){
-            _findPairsGeom1(&geoms[i], axis, data, callback);
-        }
-    }
-}
-
-/*** FIND PAIRS END ***/
-
-static sap_pair_t *pairNew(dxGeom *g1, dxGeom *g2)
-{
-    sap_pair_t *pair;
-
-    pair = (sap_pair_t *)dAlloc(sizeof(sap_pair_t));
-    pair->g[0] = g1;
-    pair->g[1] = g2;
-    ferListInit(&pair->hmap);
-    ferListInit(&pair->list);
-
-    return pair;
-}
-
-static void pairDel(sap_pair_t *p)
-{
-    // delete it from hash table
-    ferListDel(&p->hmap);
-    ferListDel(&p->list);
-    free(p);
-}
-
-static uint32_t pairHash(fer_list_t *key, void *sap)
-{
-    sap_pair_t *pair = FER_LIST_ENTRY(key, sap_pair_t, hmap);
-    uint32_t k[2 * PTR32];
-    void **v = (void **)k;
-
-    if (pair->g[0] > pair->g[1]){
-        v[0] = (void *)pair->g[0];
-        v[1] = (void *)pair->g[1];
-    }else{
-        v[1] = (void *)pair->g[0];
-        v[0] = (void *)pair->g[1];
-    }
-
-    return ferHashJenkins(k, 2 * PTR32, 0);
-}
-
-static int pairEq(const fer_list_t *key1, const fer_list_t *key2, void *sap)
-{
-    sap_pair_t *p1 = FER_LIST_ENTRY(key1, sap_pair_t, hmap);
-    sap_pair_t *p2 = FER_LIST_ENTRY(key2, sap_pair_t, hmap);
-
-    // geoms must be same
-    return (p1->g[0] == p2->g[0] && p1->g[1] == p2->g[1])
-            || (p1->g[0] == p2->g[1] && p1->g[1] == p2->g[0]);
-}
diff --git a/ode/src/collision_tsap_space.cpp b/ode/src/collision_tsap_space.cpp
new file mode 100644
index 0000000..131c5d0
--- /dev/null
+++ b/ode/src/collision_tsap_space.cpp
@@ -0,0 +1,1059 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This 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 files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/**
+ * Parallel Sweep and Prune by Daniel Fiser <danfis@danfis.cz>
+ * This code is adapted from fermat library
+ * (http://gitview.danfis.cz/fermat).
+ *
+ *
+ * This work was supported by SYMBRION and REPLICATOR projects.
+ * The SYMBRION project is funded by European Commission within the work
+ * "Future and Emergent Technologies Proactive" under grant agreement no.
+ * 216342.
+ * The REPLICATOR project is funded within the work programme "Cognitive
+ * Systems, Interaction, Robotics" under grant agreement no. 216240.
+ * http://www.symbrion.eu/
+ * http://www.replicators.eu/
+ */
+
+#include <pthread.h>
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+
+#include "config.h"
+#include "collision_kernel.h"
+#include "collision_space_internal.h"
+#include "collision_util.h"
+
+#include "fermat/list.h"
+#include "fermat/hmap.h"
+#include "fermat/hfunc.h"
+#include "fermat/tasks.h"
+
+
+#ifdef dDOUBLE
+typedef uint64_t dUInt;
+#else
+typedef uint32_t dUInt;
+#endif
+
+union _real_uint_t {
+    dReal f;
+    dUInt i;
+};
+typedef union _real_uint_t dRealUInt;
+
+#define RADIX_SORT_MASK 0xffu
+#define RADIX_SORT_SIZE (RADIX_SORT_MASK + 1u)
+#define GEOMS_INIT_SIZE 100
+#define HASH_DEFAULT_SIZE 1023
+#define GEOMS_PER_TASK 5
+
+#define PTR_SIZE sizeof(void *)
+#define PTR32 (PTR_SIZE / 4)
+
+#define SWAP(src, dst, tmp) do { \
+        (tmp) = (src); \
+        (src) = (dst); \
+        (dst) = (tmp); \
+    } while (0)
+
+# define DBG(format, ...) do { \
+    fprintf(stderr, "%s :: " format "\n", __func__, __VA_ARGS__); \
+    fflush(stderr); \
+    } while (0)
+
+# define DBG2(str) do { \
+    fprintf(stderr, "%s :: " str "\n", __func__); \
+    fflush(stderr); \
+    } while (0)
+
+
+
+
+struct sap_geom_t {
+    dxGeom *g;
+    unsigned int min[3];
+    dReal max[3];
+    unsigned char dirty;
+};
+
+struct sap_min_t {
+    uint32_t geom;
+    dReal val;
+};
+
+struct sap_pair_t {
+    dxGeom *g[2];    /*!< Reference to geoms */
+    fer_list_t hmap; /*!< Connection into hash map */
+    fer_list_t list; /*!< Connection into sap.pairs list */
+};
+
+struct radix_sort_t {
+    sap_min_t *min[3];
+    size_t min_len[3], min_alloc[3];
+    unsigned int *counter[3];  /*!< Array for count-sort */
+    unsigned int negative[3];  /*!< Number of negative values */
+};
+
+
+/** Creates new pair from other sap_pair_t struct */
+static sap_pair_t *pairNew(dxGeom *g1, dxGeom *g2);
+/** Deletes collide pair */
+static void pairDel(sap_pair_t *p);
+
+/** Callbacks for hash table */
+static uint32_t pairHash(fer_list_t *key, void *sap);
+static int pairEq(const fer_list_t *key1, const fer_list_t *key2, void *sap);
+
+static void __findPairsTask(int id, void *data,
+                            const fer_tasks_thinfo_t *thinfo);
+static void __radixSortTask(int id, void *data,
+                            const fer_tasks_thinfo_t *thinfo);
+static void __cleanGeomsTask(int id, void *data,
+                             const fer_tasks_thinfo_t *thinfo);
+
+struct dxTSAPSpace : public dxSpace
+{
+    unsigned int num_threads;
+    unsigned int hashsize;
+
+    sap_geom_t *geoms;  /*!< Array of geoms */
+    size_t geoms_len;   /*!< Length of .geoms[] */
+    size_t geoms_alloc; /*!< Size of allocated memory for .geoms[] */
+    sap_min_t *min[3];  /*!< Array of min values */
+    unsigned char dirt;       /*!< True if any geom was changed */
+    unsigned int *dirty_geoms; /*!< Array of indices of dirty geoms */ 
+    size_t dirty_geoms_alloc;
+    size_t dirty_geoms_len;
+
+    radix_sort_t rs; /*!< Cached radix sort struct */
+
+    fer_list_t *pairs;    /*!< Array of lists of possible collide pairs (sap_pair_t) */
+    fer_hmap_t *pairs_reg;           /*!< Register (hash map) of collide pairs */
+    pthread_mutex_t *pairs_reg_lock; /*!< Array of locks for .pairs_reg */
+
+    fer_tasks_t *tasks;
+    struct {
+        void *data;
+        dNearCallback *callback;
+        int axis;
+
+        pthread_mutex_t lock;
+        int next_geom;
+    } context;
+
+
+    // Constructor / Destructor
+    dxTSAPSpace(dSpaceID _space, unsigned int num_threads, unsigned int hashsize);
+    ~dxTSAPSpace();
+
+    // dxSpace
+    virtual dxGeom* getGeom(int i);
+    virtual void add(dxGeom* g);
+    virtual void remove(dxGeom* g);
+    virtual void dirty(dxGeom* g);
+    virtual void computeAABB();
+    virtual void cleanGeoms();
+    virtual void collide(void *data, dNearCallback *callback);
+    virtual void collide2(void *data, dxGeom *geom, dNearCallback *callback);
+
+    void setNumThreads(unsigned int num_threads);
+    void setHashsize(unsigned int hashsize);
+
+  private:
+    void _setDirty(sap_geom_t *geom);
+    /** Removes all pairs from all buckets */
+    void _pairRemoveAll();
+    /** Set min/max values of projected AABB on a specified axis */
+    void _setMinMax(dxGeom *geom, unsigned int axis,
+                    dReal *min, dReal *max);
+    void _sort();
+    /** Performs radix sort */
+    void _radixSort(unsigned int axis);
+    /** Finds collide pairs and calls for each pair a callback */
+    void _findPairs(void *data, dNearCallback *callback);
+    void _findPairsGeom(sap_geom_t *geom, int bucket);
+    void _findPairsGeom1(sap_geom_t *geom, int axis,
+                         void *data, dNearCallback *callback);
+
+    void _cleanDirtyGeoms(unsigned int from, unsigned int to);
+
+    void __reset();
+    void __update();
+
+    friend void __findPairsTask(int id, void *data,
+                                const fer_tasks_thinfo_t *thinfo);
+    friend void __radixSortTask(int id, void *data,
+                                const fer_tasks_thinfo_t *thinfo);
+    friend void __cleanGeomsTask(int id, void *data,
+                                 const fer_tasks_thinfo_t *thinfo);
+};
+
+ODE_API dSpaceID dThreadedSAPSpaceCreate(dxSpace* space, unsigned int num_threads, unsigned int hashsize)
+{
+    return new dxTSAPSpace(space, num_threads, hashsize);
+}
+
+ODE_API unsigned int dThreadedSAPSpaceGetNumThreads(dxSpace *space)
+{
+    return ((dxTSAPSpace *)space)->num_threads;
+}
+
+ODE_API void dThreadedSAPSpaceSetNumThreads(dSpaceID space, unsigned int num_threads)
+{
+    ((dxTSAPSpace *)space)->setNumThreads(num_threads);
+}
+
+ODE_API unsigned int dThreadedSAPSpaceGetHashsize(dSpaceID space)
+{
+    return ((dxTSAPSpace *)space)->hashsize;
+}
+
+ODE_API void dThreadedSAPSpaceSetHashsize(dSpaceID space, unsigned int hashsize)
+{
+    ((dxTSAPSpace *)space)->setHashsize(hashsize);
+}
+
+dxTSAPSpace::dxTSAPSpace(dSpaceID _space, unsigned int _num_threads, unsigned int _hashsize)
+    : dxSpace(_space)
+{
+    int i;
+
+	type = dThreadedSAPSpaceClass;
+
+	// Init AABB to infinity
+	aabb[0] = -dInfinity;
+	aabb[1] = dInfinity;
+	aabb[2] = -dInfinity;
+	aabb[3] = dInfinity;
+	aabb[4] = -dInfinity;
+	aabb[5] = dInfinity;
+
+    num_threads = _num_threads >= 1 ? _num_threads : 1;
+    hashsize = _hashsize > 0 ? _hashsize : HASH_DEFAULT_SIZE;
+
+    // init geoms and min arrays
+    geoms_alloc = GEOMS_INIT_SIZE;
+    geoms_len = 0;
+    geoms = (sap_geom_t *)dAlloc(sizeof(sap_geom_t) * geoms_alloc);
+    for (i = 0; i < 3; i++){
+        min[i] = (sap_min_t *)dAlloc(sizeof(sap_min_t) * geoms_alloc);
+    }
+
+    // dirty geoms
+    dirty_geoms_alloc = GEOMS_INIT_SIZE;
+    dirty_geoms_len = 0;
+    dirty_geoms = (unsigned int *)dAlloc(sizeof(unsigned int) * dirty_geoms_alloc);
+
+    dirt = 0;
+
+    // init radix sort
+    rs.min[0] = rs.min[1] = rs.min[2] = NULL;
+    rs.min_len[0] = rs.min_len[1] = rs.min_len[2] = 0;
+    rs.min_alloc[0] = rs.min_alloc[1] = rs.min_alloc[2] = 0;
+    rs.counter[0] = (unsigned int *)dAlloc(sizeof(unsigned int) * RADIX_SORT_SIZE);
+    rs.counter[1] = (unsigned int *)dAlloc(sizeof(unsigned int) * RADIX_SORT_SIZE);
+    rs.counter[2] = (unsigned int *)dAlloc(sizeof(unsigned int) * RADIX_SORT_SIZE);
+
+
+    pairs = NULL;
+    pairs_reg = NULL;
+    pairs_reg_lock = NULL;
+    tasks = NULL;
+
+    dIASSERT(pthread_mutex_init(&context.lock, NULL) == 0);
+}
+
+dxTSAPSpace::~dxTSAPSpace()
+{
+    int i;
+
+    if (geoms)
+        dFree(geoms, sizeof(size_t) * geoms_alloc);
+
+    for (i = 0; i < 3; i++){
+        if (min[i])
+            dFree(min[i], sizeof(sap_min_t) * geoms_alloc);
+    }
+
+    for (i = 0; i < 3; i++){
+        if (rs.min[i])
+            dFree(rs.min[i], sizeof(sap_min_t) * rs.min_alloc[i]);
+        if (rs.counter[i])
+            dFree(rs.counter[i], sizeof(unsigned int) * RADIX_SORT_SIZE);
+    }
+
+    __reset();
+
+    dIASSERT(pthread_mutex_destroy(&context.lock) == 0);
+}
+
+void dxTSAPSpace::__reset()
+{
+    int i;
+
+    _pairRemoveAll();
+    if (pairs)
+        dFree(pairs, sizeof(fer_list_t) * num_threads);
+    if (pairs_reg)
+        ferHMapDel(pairs_reg);
+    if (pairs_reg_lock){
+        for (i = 0; i < num_threads; i++){
+            dIASSERT(pthread_mutex_destroy(&pairs_reg_lock[i]) == 0);
+        }
+        dFree(pairs_reg_lock, sizeof(pthread_mutex_t) * num_threads);
+    }
+
+    if (tasks)
+        ferTasksDel(tasks);
+
+    pairs          = NULL;
+    pairs_reg      = NULL;
+    pairs_reg_lock = NULL;
+    tasks          = NULL;
+}
+
+void dxTSAPSpace::__update()
+{
+    int i;
+
+    // allocate buckets for pairs
+    if (!pairs){
+        pairs = (fer_list_t *)dAlloc(sizeof(fer_list_t) * num_threads);
+        for (i = 0; i < num_threads; i++){
+            ferListInit(&pairs[i]);
+        }
+    }else{
+        // remove all old pairs
+        _pairRemoveAll();
+    }
+
+    // create register for collide pairs
+    if (!pairs_reg){
+        pairs_reg = ferHMapNew(hashsize, pairHash, pairEq, (void *)this);
+        if (num_threads > 1){
+            pairs_reg_lock = (pthread_mutex_t *)dAlloc(sizeof(pthread_mutex_t) * num_threads);
+            for (i = 0; i < num_threads; i++){
+                dIASSERT(pthread_mutex_init(&pairs_reg_lock[i], NULL) == 0);
+            }
+        }
+    }
+
+    // create thread pool
+    if (!tasks && num_threads > 1){
+        tasks = ferTasksNew(num_threads);
+        ferTasksRun(tasks);
+    }
+
+}
+
+void dxTSAPSpace::setNumThreads(unsigned int _num_threads)
+{
+    __reset();
+    num_threads = _num_threads >= 1 ? _num_threads : 1;
+}
+
+void dxTSAPSpace::setHashsize(unsigned int _hashsize)
+{
+    __reset();
+    hashsize = _hashsize >= 1 ? _hashsize : HASH_DEFAULT_SIZE;
+}
+
+dxGeom* dxTSAPSpace::getGeom(int i)
+{
+    if (i < 0 || i >geoms_len)
+        return NULL;
+    return geoms[i].g;
+}
+
+void dxTSAPSpace::add(dxGeom* geom)
+{
+    int i;
+
+    dxSpace::add(geom);
+
+    if (geoms_len == geoms_alloc){
+        geoms_alloc *= 2;
+        geoms = (sap_geom_t *)dRealloc(geoms, 0, sizeof(sap_geom_t) * geoms_alloc);
+
+        for (i = 0; i < 3; i++){
+            min[i] = (sap_min_t *)dRealloc(min[i], 0, sizeof(sap_min_t) * geoms_alloc);
+        }
+    }
+
+    geoms[geoms_len].g = geom;
+    for (i = 0; i < 3; i++){
+        min[i][geoms_len].geom = geoms_len;
+        geoms[geoms_len].min[i] = geoms_len;
+    }
+    geoms[geoms_len].dirty = 0;
+
+    geom->sap = geoms_len;
+    _setDirty(&geoms[geoms_len]);
+
+    geoms_len++;
+    dirt = 1;
+
+    dIASSERT(geoms_len <= geoms_alloc);
+}
+
+void dxTSAPSpace::remove(dxGeom* geom)
+{
+    int i;
+
+    geoms[geom->sap] = geoms[geoms_len - 1];
+    geom->sap = -1;
+    geoms_len--;
+    for (i = 0; i < geoms_len; i++){
+        geoms[i].g->sap = i;
+        geoms[i].min[0] = i;
+        geoms[i].min[1] = i;
+        geoms[i].min[2] = i;
+        min[0][i].geom = i;
+        min[1][i].geom = i;
+        min[2][i].geom = i;
+        _setDirty(&geoms[i]);
+    }
+
+    dxSpace::remove(geom);
+}
+
+void dxTSAPSpace::dirty(dxGeom* geom)
+{
+    int i;
+    sap_geom_t *g;
+    dReal _min, _max;
+
+    dxSpace::dirty(geom);
+
+    g = &geoms[geom->sap];
+    _setDirty(g);
+
+    dirt = 1;
+}
+
+void dxTSAPSpace::computeAABB()
+{
+}
+
+void dxTSAPSpace::_cleanDirtyGeoms(unsigned int from, unsigned int to)
+{
+    dReal _min, _max;
+    sap_geom_t *g;
+    unsigned int i, ax;
+
+    for (i = from; i < to; i++){
+        for (ax = 0; ax < 3; ax++){
+            g = &geoms[dirty_geoms[i]];
+            g->dirty = 0;
+
+            if (IS_SPACE(g->g))
+                ((dxSpace *)g->g)->cleanGeoms();
+            g->g->recomputeAABB();
+            g->g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
+
+            // update min/max values
+            _setMinMax(g->g, ax, &_min, &_max);
+            min[ax][g->min[ax]].val = _min;
+            g->max[ax] = _max;
+        }
+    }
+}
+
+static void __cleanGeomsTask(int id, void *data,
+                             const fer_tasks_thinfo_t *thinfo)
+{
+    dxTSAPSpace *sap = (dxTSAPSpace *)data;
+    unsigned int i, from, to, len;
+
+
+    len = sap->dirty_geoms_len / sap->num_threads;
+    from = id * len;
+    to   = from + len;
+    if (id == sap->num_threads - 1)
+        to = sap->dirty_geoms_len;
+
+    sap->_cleanDirtyGeoms(from, to);
+}
+
+void dxTSAPSpace::cleanGeoms()
+{
+    unsigned int i, ax;
+    dReal _min, _max;
+    sap_geom_t *g;
+
+    __update();
+
+    if (num_threads > 1 && dirty_geoms_len > num_threads * 10){
+        for (i = 0; i < num_threads; i++){
+            ferTasksAdd(tasks, __cleanGeomsTask, i, (void *)this);
+        }
+        ferTasksBarrier(tasks);
+    }else{
+        _cleanDirtyGeoms(0, dirty_geoms_len);
+    }
+    dirty_geoms_len = 0;
+    dirt = 0;
+}
+
+void dxTSAPSpace::collide(void *data, dNearCallback *callback)
+{
+    int i;
+
+    if (dirt){
+        cleanGeoms();
+
+        _sort();
+    }
+
+    _findPairs(data, callback);
+}
+
+void dxTSAPSpace::collide2(void *data, dxGeom *geom, dNearCallback *callback)
+{
+    size_t i;
+
+    for (i = 0; i < geoms_len; i++){
+        collideAABBs(geoms[i].g, geom, data, callback);
+    }
+}
+
+
+void dxTSAPSpace::_setDirty(sap_geom_t *geom)
+{
+    if (geom->dirty)
+        return;
+
+    if (dirty_geoms_len == dirty_geoms_alloc){
+        dirty_geoms_alloc *= 2;
+        dirty_geoms = (unsigned int *)dRealloc(dirty_geoms, 0, sizeof(unsigned int) * dirty_geoms_alloc);
+    }
+
+    dirty_geoms[dirty_geoms_len] = geom->g->sap;
+    dirty_geoms_len++;
+
+    dIASSERT(dirty_geoms_len <= dirty_geoms_alloc);
+}
+
+void dxTSAPSpace::_pairRemoveAll()
+{
+    fer_list_t *item;
+    sap_pair_t *pair;
+    size_t i;
+
+    if (!pairs)
+        return;
+
+    for (i = 0; i < num_threads; i++){
+        while (!ferListEmpty(&pairs[i])){
+            item = ferListNext(&pairs[i]);
+            pair = FER_LIST_ENTRY(item, sap_pair_t, list);
+            pairDel(pair);
+        }
+    }
+}
+
+static inline void __updateMinMax(dReal m, dReal *min, dReal *max)
+{
+    if (m < *min)
+        *min = m;
+    if (m > *max)
+        *max = m;
+}
+
+void dxTSAPSpace::_setMinMax(dxGeom *geom, unsigned int ax,
+                             dReal *min, dReal *max)
+{
+    /** As axis are considered:
+     *  0: 1, 1, 0
+     *  1: 0, 1, 1
+     *  2: 1, 0, 1
+     */
+
+    dReal a, b, c, d;
+    dVector3 center;
+
+    if (ax == 0){
+        a = geom->aabb[0] + geom->aabb[2];
+        b = geom->aabb[0] + geom->aabb[3];
+        c = geom->aabb[1] + geom->aabb[2];
+        d = geom->aabb[1] + geom->aabb[3];
+    }else if (ax == 1){
+        a = geom->aabb[2] + geom->aabb[4];
+        b = geom->aabb[2] + geom->aabb[5];
+        c = geom->aabb[3] + geom->aabb[4];
+        d = geom->aabb[3] + geom->aabb[5];
+    }else{
+        a = geom->aabb[0] + geom->aabb[4];
+        b = geom->aabb[0] + geom->aabb[5];
+        c = geom->aabb[1] + geom->aabb[4];
+        d = geom->aabb[1] + geom->aabb[5];
+    }
+
+    *min = *max = a;
+    __updateMinMax(b, min, max);
+    __updateMinMax(c, min, max);
+    __updateMinMax(d, min, max);
+}
+
+
+
+/*** RADIX SORT ***/
+static inline dUInt realAsUInt(dReal x)
+{
+    dRealUInt v;
+    v.f = x;
+    return v.i;
+}
+
+static inline uint32_t radixSortVal(const sap_min_t *m,
+                                    uint32_t shift)
+{
+    dUInt val;
+
+    val = realAsUInt(m->val);
+    val = (val >> shift) & RADIX_SORT_MASK;
+
+    return val;
+}
+
+static inline void radixSortZeroizeCounter(unsigned int *counter)
+{
+    int i;
+    for (i = 0; i < RADIX_SORT_SIZE; i++)
+        counter[i] = 0;
+}
+
+static void radixSortCount(const sap_min_t *m,
+                           unsigned int len,
+                           unsigned int *counter,
+                           unsigned int shift)
+{
+    unsigned int i;
+    uint32_t val;
+
+    radixSortZeroizeCounter(counter);
+    for (i = 0; i < len; i++){
+        val = radixSortVal(&m[i], shift);
+        ++counter[val];
+    }
+}
+
+static void radixSortCountFinal(const sap_min_t *m,
+                                unsigned int len,
+                                unsigned int *counter,
+                                unsigned int *negative,
+                                unsigned int shift)
+{
+    unsigned int i;
+    uint32_t val;
+
+    radixSortZeroizeCounter(counter);
+    *negative = 0;
+    for (i = 0; i < len; i++){
+        val = radixSortVal(&m[i], shift);
+        ++counter[val];
+
+        if (m[i].val < REAL(0.))
+            ++(*negative);
+    }
+}
+
+static void radixSortFixCounter(unsigned int *counter)
+{
+    uint32_t i, val, t;
+
+    val = 0;
+    for (i = 0; i < RADIX_SORT_SIZE; i++){
+        t = counter[i];
+        counter[i] = val;
+        val += t;
+    }
+}
+
+static void radixSortSort(sap_min_t *src,
+                          sap_min_t *dst,
+                          unsigned int len,
+                          unsigned int *counter,
+                          unsigned int shift)
+{
+    unsigned int i;
+    uint32_t val;
+
+    for (i = 0; i < len; i++){
+        val = radixSortVal(&src[i], shift);
+        dst[counter[val]] = src[i];
+        ++counter[val];
+    }
+}
+
+static void radixSortSortFinal(sap_min_t *src,
+                               unsigned int srclen,
+                               sap_min_t *dst,
+                               unsigned int dstlen,
+                               unsigned int *counter,
+                               unsigned int negative,
+                               unsigned int shift,
+                               sap_geom_t *geoms, int ax)
+{
+    unsigned int i, pos;
+    uint32_t val;
+
+    for (i = 0; i < srclen; i++){
+        val = radixSortVal(&src[i], shift);
+        pos = counter[val];
+        if (pos >= dstlen - negative){
+            pos = dstlen - pos - 1;
+        }else{
+            pos = negative + pos;
+        }
+
+        dst[pos] = src[i];
+
+        geoms[dst[pos].geom].min[ax] = pos;
+
+        ++counter[val];
+    }
+}
+
+void dxTSAPSpace::_radixSort(unsigned int ax)
+{
+    /** Radix sort of one axis is performed in single thread because at
+     *  least in my benchmarks and on my computer it was always faster than
+     *  in multiple threads.
+     *  However, the multi-thread implementation is available in fermat
+     *  library. */
+    sap_min_t *src, *dst, *tmp;
+    unsigned int shift, i, len;
+
+    // allocate temporary array
+    if (rs.min_alloc[ax] < geoms_len){
+        if (rs.min[ax])
+            dFree(rs.min[ax], sizeof(sap_min_t) * rs.min_alloc[ax]);
+        rs.min_alloc[ax] = geoms_alloc;
+        rs.min[ax] = (sap_min_t *)dAlloc(sizeof(sap_min_t) * rs.min_alloc[ax]);
+    }
+    rs.min_len[ax] = geoms_len;
+
+
+    len = (unsigned int)sizeof(dReal) - 1;
+    shift = 0;
+    src = min[ax];
+    dst = rs.min[ax];
+    for (i = 0; i < len; i++){
+        radixSortCount(src, rs.min_len[ax], rs.counter[ax], shift);
+        radixSortFixCounter(rs.counter[ax]);
+        radixSortSort(src, dst, rs.min_len[ax], rs.counter[ax], shift);
+
+        shift += 8;
+        SWAP(src, dst, tmp);
+    }
+
+    radixSortCountFinal(src, rs.min_len[ax], rs.counter[ax], &rs.negative[ax], shift);
+    radixSortFixCounter(rs.counter[ax]);
+    radixSortSortFinal(src, rs.min_len[ax],
+                       dst, rs.min_len[ax],
+                       rs.counter[ax], rs.negative[ax], shift,
+                       geoms, ax);
+}
+
+static void __radixSortTask(int id, void *data,
+                            const fer_tasks_thinfo_t *thinfo)
+{
+    dxTSAPSpace *sap = (dxTSAPSpace *)data;
+    sap->_radixSort(id);
+}
+
+void dxTSAPSpace::_sort()
+{
+    int i;
+
+    __update();
+
+    if (num_threads > 1){
+        for (i = 0; i < 3; i++){
+            ferTasksAdd(tasks, __radixSortTask, i, (void *)this);
+        }
+        ferTasksBarrier(tasks);
+    }else{
+        for (i = 0; i < 3; i++){
+            _radixSort(i);
+        }
+    }
+}
+/*** RADIX SORT END ***/
+
+
+
+/*** FIND PAIRS ***/
+
+static dReal minMaxVariance(const sap_min_t *m, size_t mlen)
+{
+    size_t i;
+    unsigned int len;
+    dReal avg, var, tr1, tr2;
+
+    tr1 = -FLT_MAX / REAL(2.);
+    tr2 =  FLT_MAX / REAL(2.);
+    avg = len = var = 0;
+    for (i = 0; i < mlen; ++i, ++m){
+        if (m->val > tr1 && m->val < tr2){
+            avg += m->val;
+            len++;
+            var += m->val * m->val;
+        }
+    }
+
+    avg /= len;
+    var /= len;
+    var -= avg * avg;
+
+    return var;
+}
+
+static int overlap(dxGeom *g1, dxGeom *g2)
+{
+    // no contacts if both geoms on the same body, and the body is not 0
+    if (g1->body == g2->body && g1->body)
+        return 0;
+
+    // test if the category and collide bitfields match
+    if (((g1->category_bits & g2->collide_bits) ||
+                (g2->category_bits & g1->collide_bits)) == 0) {
+        return 0;
+    }
+
+    // if the bounding boxes are disjoint then don't do anything
+    dReal *bounds1 = g1->aabb;
+    dReal *bounds2 = g2->aabb;
+    if (bounds1[0] > bounds2[1] ||
+            bounds1[1] < bounds2[0] ||
+            bounds1[2] > bounds2[3] ||
+            bounds1[3] < bounds2[2] ||
+            bounds1[4] > bounds2[5] ||
+            bounds1[5] < bounds2[4]){
+        return 0;
+    }
+
+    // check if either object is able to prove that it doesn't intersect the
+    // AABB of the other
+    if (g1->AABBTest (g2,bounds2) == 0)
+        return 0;
+    if (g2->AABBTest (g1,bounds1) == 0)
+        return 0;
+
+    return 1;
+}
+
+void dxTSAPSpace::_findPairsGeom(sap_geom_t *g, int bucket)
+{
+    int j;
+    sap_min_t *_min;
+    dxGeom *g2;
+    sap_pair_t *pair;
+    uint32_t id;
+    int lockid;
+
+    _min = min[context.axis];
+    for (j = g->min[context.axis] + 1; j < geoms_len && _min[j].val < g->max[context.axis]; j++){
+        g2 = geoms[_min[j].geom].g;
+        if (overlap(g->g, g2)){
+            // create new pair
+            pair = pairNew(g->g, g2);
+
+            // add this pair to list of pairs (into specified bucket)
+            ferListAppend(&pairs[bucket], &pair->list);
+
+            // obtain hash ID
+            id = ferHMapID(pairs_reg, &pair->hmap);
+
+            // find out mutext corresponding to id
+            lockid = id % num_threads;
+
+            // check if this pair isn't already registered
+            dIASSERT(pthread_mutex_lock(&pairs_reg_lock[lockid]) == 0);
+            if (ferHMapIDGet(pairs_reg, id, &pair->hmap) == NULL){
+                ferHMapIDPut(pairs_reg, id, &pair->hmap);
+                dIASSERT(pthread_mutex_unlock(&pairs_reg_lock[lockid]) == 0);
+            }else{
+                dIASSERT(pthread_mutex_unlock(&pairs_reg_lock[lockid]) == 0);
+                pairDel(pair);
+            }
+        }
+    }
+}
+
+void dxTSAPSpace::_findPairsGeom1(sap_geom_t *geom, int axis,
+                                 void *data, dNearCallback *callback)
+{
+    sap_min_t *_min;
+    dxGeom *g2;
+    sap_pair_t *pair, pair_test;
+    int i;
+
+    _min = min[axis];
+    for (i = geom->min[axis] + 1; i < geoms_len && _min[i].val < geom->max[axis]; i++){
+        g2 = geoms[_min[i].geom].g;
+        if (overlap(geom->g, g2)){
+            pair_test.g[0] = geom->g;
+            pair_test.g[1] = g2;
+
+            if (ferHMapGet(pairs_reg, &pair_test.hmap) == NULL){
+                // create new pair
+                pair = pairNew(geom->g, g2);
+
+                // add this pair to list of pairs (into specified bucket)
+                ferListAppend(&pairs[0], &pair->list);
+
+                // add pair into register
+                ferHMapPut(pairs_reg, &pair->hmap);
+
+                // call callback
+                callback(data, geom->g, g2);
+            }
+        }
+    }
+}
+
+static void __findPairsTask(int id, void *data,
+                            const fer_tasks_thinfo_t *thinfo)
+{
+    dxTSAPSpace *sap = (dxTSAPSpace *)data;
+    int i, max;
+
+    i = id * GEOMS_PER_TASK;
+    while (i < sap->geoms_len){
+        max = (i + GEOMS_PER_TASK < sap->geoms_len ? i + GEOMS_PER_TASK : sap->geoms_len);
+        for (; i < max; i++){
+            sap->_findPairsGeom(&sap->geoms[i], id);
+        }
+
+        dIASSERT(pthread_mutex_lock(&sap->context.lock) == 0);
+        i = sap->context.next_geom;
+        sap->context.next_geom += GEOMS_PER_TASK;
+        dIASSERT(pthread_mutex_unlock(&sap->context.lock) == 0);
+    }
+}
+
+void dxTSAPSpace::_findPairs(void *data, dNearCallback *callback)
+{
+    size_t i, axis;
+    dReal var[3];
+    fer_list_t *item;
+    sap_pair_t *pair;
+
+    __update();
+
+    // find axis with largest variance
+    var[0] = minMaxVariance(min[0], geoms_len);
+    var[1] = minMaxVariance(min[1], geoms_len);
+    var[2] = minMaxVariance(min[2], geoms_len);
+    if (var[0] > var[1]){
+        if (var[0] > var[2]){
+            axis = 0;
+        }else{
+            axis = 2;
+        }
+    }else if (var[1] > var[2]){
+        axis = 1;
+    }else{
+        axis = 2;
+    }
+
+
+    if (num_threads > 1){
+        // set up context and run prunning in separate threads
+        context.data     = data;
+        context.callback = callback;
+        context.axis     = axis;
+        context.next_geom = num_threads;
+        for (i = 0; i < num_threads; i++){
+            ferTasksAdd(tasks, __findPairsTask, i, (void *)this);
+        }
+        ferTasksBarrier(tasks);
+
+        for (i = 0; i < num_threads; i++){
+            FER_LIST_FOR_EACH(&pairs[i], item){
+                pair = FER_LIST_ENTRY(item, sap_pair_t, list);
+                context.callback(context.data, pair->g[0], pair->g[1]);
+            }
+        }
+    }else{
+        for (i = 0; i < geoms_len; i++){
+            _findPairsGeom1(&geoms[i], axis, data, callback);
+        }
+    }
+}
+
+/*** FIND PAIRS END ***/
+
+static sap_pair_t *pairNew(dxGeom *g1, dxGeom *g2)
+{
+    sap_pair_t *pair;
+
+    pair = (sap_pair_t *)dAlloc(sizeof(sap_pair_t));
+    pair->g[0] = g1;
+    pair->g[1] = g2;
+    ferListInit(&pair->hmap);
+    ferListInit(&pair->list);
+
+    return pair;
+}
+
+static void pairDel(sap_pair_t *p)
+{
+    // delete it from hash table
+    ferListDel(&p->hmap);
+    ferListDel(&p->list);
+    dFree(p, sizeof(sap_pair_t));
+}
+
+static uint32_t pairHash(fer_list_t *key, void *sap)
+{
+    sap_pair_t *pair = FER_LIST_ENTRY(key, sap_pair_t, hmap);
+    uint32_t k[2 * PTR32];
+    void **v = (void **)k;
+
+    if (pair->g[0] > pair->g[1]){
+        v[0] = (void *)pair->g[0];
+        v[1] = (void *)pair->g[1];
+    }else{
+        v[1] = (void *)pair->g[0];
+        v[0] = (void *)pair->g[1];
+    }
+
+    return ferHashJenkins(k, 2 * PTR32, 0);
+}
+
+static int pairEq(const fer_list_t *key1, const fer_list_t *key2, void *sap)
+{
+    sap_pair_t *p1 = FER_LIST_ENTRY(key1, sap_pair_t, hmap);
+    sap_pair_t *p2 = FER_LIST_ENTRY(key2, sap_pair_t, hmap);
+
+    // geoms must be same
+    return (p1->g[0] == p2->g[0] && p1->g[1] == p2->g[1])
+            || (p1->g[0] == p2->g[1] && p1->g[1] == p2->g[0]);
+}
diff --git a/ode/src/fermat/Makefile.am b/ode/src/fermat/Makefile.am
index 649400b..3d318f0 100644
--- a/ode/src/fermat/Makefile.am
+++ b/ode/src/fermat/Makefile.am
@@ -5,6 +5,5 @@ noinst_LTLIBRARIES = libfermat.la
 libfermat_la_SOURCES =  list.h \
                         hmap.h hmap.c \
                         hfunc.h hfunc.c \
-                        tasks.h tasks.c \
-                        alloc.h alloc.c
+                        tasks.h tasks.c
 
diff --git a/ode/src/fermat/alloc.c b/ode/src/fermat/alloc.c
deleted file mode 100644
index 414a99d..0000000
--- a/ode/src/fermat/alloc.c
+++ /dev/null
@@ -1,98 +0,0 @@
-/***
- * fermat
- * -------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *  This file is part of fermat.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#include <stdio.h>
-#include <fermat/alloc.h>
-
-#ifdef FER_MEMCHECK
-#include <fermat/list.h>
-
-/** currently allocated memory */
-static unsigned long cur_alloc = 0L;
-/* maximal amount of allocated mem during algorithm */
-static unsigned long max_alloc = 0L;
-static int reg_at_exit = 0;
-
-struct _info_t {
-    size_t size;
-};
-typedef struct _info_t info_t;
-
-static void stats(void)
-{
-    fprintf(stderr, "MemCheck stats:\n");
-    fprintf(stderr, "    max_alloc: % 10lu bytes\n", max_alloc);
-    fprintf(stderr, "    unfreed:   % 10lu bytes\n", cur_alloc);
-}
-
-void ferFreeCheck(void *_ptr)
-{
-    void *ptr;
-
-    ptr = (void *)((char *)_ptr - sizeof(info_t));
-    cur_alloc -= ((info_t *)ptr)->size;
-    free(ptr);
-}
-#endif /* FER_MEMCHECK */
-
-
-void *ferRealloc(void *ptr, size_t size)
-{
-    void *ret;
-   
-#ifndef FER_MEMCHECK
-    ret = realloc(ptr, size);
-#else /* FER_MEMCHECK */
-    ret = realloc(ptr, size + sizeof(info_t));
-    if (ret){
-        ((info_t *)ret)->size = size;
-        ret = (void *)((char *)ret + sizeof(info_t));
-
-        cur_alloc += size;
-        if (cur_alloc > max_alloc)
-            max_alloc = cur_alloc;
-    }
-#endif /* FER_MEMCHECK */
-
-    if (ret == NULL && size != 0){
-        fprintf(stderr, "Fatal error: Allocation of memory failed!\n");
-        fflush(stderr);
-        exit(-1);
-    }
-
-#ifdef FER_MEMCHECK
-    if (!reg_at_exit){
-        atexit(stats);
-        reg_at_exit = 1;
-    }
-#endif /* FER_MEMCHECK */
-
-    return ret;
-}
-
-void *ferAllocAlign(size_t size, size_t alignment)
-{
-    void *mem;
-
-    if (posix_memalign(&mem, alignment, size) != 0){
-        fprintf(stderr, "Fatal error: Allocation of memory failed!\n");
-        fflush(stderr);
-        exit(-1);
-    }
-
-    return mem;
-}
-
diff --git a/ode/src/fermat/alloc.h b/ode/src/fermat/alloc.h
deleted file mode 100644
index 63e3536..0000000
--- a/ode/src/fermat/alloc.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/***
- * fermat
- * -------
- * Copyright (c)2010 Daniel Fiser <danfis@danfis.cz>
- *
- *  This file is part of fermat.
- *
- *  Distributed under the OSI-approved BSD License (the "License");
- *  see accompanying file BDS-LICENSE for details or see
- *  <http://www.opensource.org/licenses/bsd-license.php>.
- *
- *  This software is distributed WITHOUT ANY WARRANTY; without even the
- *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *  See the License for more information.
- */
-
-#ifndef __FER_ALLOC_H__
-#define __FER_ALLOC_H__
-
-#include <stdlib.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif /* __cplusplus */
-
-/**
- * Alloc - Memory Allocation
- * ==========================
- *
- * Functions and macros for memory allocation.
- */
-
-/* Memory allocation: - internal macro */
-#define _FER_ALLOC_MEMORY(type, ptr_old, size) \
-    (type *)ferRealloc((void *)ptr_old, (size))
-
-/**
- * Allocate memory for one element of given type.
- */
-#define FER_ALLOC(type) \
-    _FER_ALLOC_MEMORY(type, NULL, sizeof(type))
-
-/**
- * Allocates aligned memory
- */
-#define FER_ALLOC_ALIGN(type, align) \
-    (type *)ferAllocAlign(sizeof(type), align)
-
-/**
- * Allocates aligned array
- */
-#define FER_ALLOC_ALIGN_ARR(type, num_els, align) \
-    (type *)ferAllocAlign(sizeof(type) * (num_els), align)
-
-/**
- * Allocate array of elements of given type.
- */
-#define FER_ALLOC_ARR(type, num_elements) \
-    _FER_ALLOC_MEMORY(type, NULL, sizeof(type) * (num_elements))
-
-/**
- * Reallocates array.
- */
-#define FER_REALLOC_ARR(ptr, type, num_elements) \
-    _FER_ALLOC_MEMORY(type, ptr, sizeof(type) * (num_elements))
-
-#ifndef FER_MEMCHECK
-# define FER_FREE(ptr) free(ptr) /*!< Deallocates memory */
-#else /* FER_MEMCHECK */
-# define FER_FREE(ptr) ferFreeCheck(ptr)
-#endif /* FER_MEMCHECK */
-
-void *ferRealloc(void *ptr, size_t size);
-void *ferAllocAlign(size_t size, size_t alignment);
-
-#ifdef FER_MEMCHECK
-void ferFreeCheck(void *ptr);
-#endif /* FER_MEMCHECK */
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-
-#endif /* __FER_ALLOC_H__ */
diff --git a/ode/src/fermat/compiler.h b/ode/src/fermat/compiler.h
index d692f85..64ef14e 100644
--- a/ode/src/fermat/compiler.h
+++ b/ode/src/fermat/compiler.h
@@ -44,11 +44,15 @@
  * Marks inline function.
  */
 #ifdef __GNUC__
-# ifdef FER_DEBUG
-#  define _fer_inline static
-# else /* FER_DEBUG */
-#  define _fer_inline static inline __attribute__((always_inline))
-# endif /* FER_DEBUG */
+#  ifdef FER_DEBUG
+#    define _fer_inline static
+#  else /* FER_DEBUG */
+#    ifdef __NO_INLINE__
+#      define _fer_inline static
+#    else /* __NO_INLINE */
+#      define _fer_inline static inline __attribute__((always_inline))
+#    endif /* __NO_INLINE */
+#  endif /* FER_DEBUG */
 #else /* __GNUC__ */
 # define _fer_inline static inline
 #endif /* __GNUC__ */
diff --git a/ode/src/fermat/hmap.c b/ode/src/fermat/hmap.c
index 3b703b6..bdf1d1b 100644
--- a/ode/src/fermat/hmap.c
+++ b/ode/src/fermat/hmap.c
@@ -14,8 +14,8 @@
  *  See the License for more information.
  */
 
+#include "../../include/ode/memory.h"
 #include "hmap.h"
-#include "alloc.h"
 
 
 struct _fer_hmap_el_t {
@@ -33,8 +33,8 @@ fer_hmap_t *ferHMapNew(size_t size,
     fer_hmap_t *hmap;
     size_t i;
 
-    hmap = FER_ALLOC(fer_hmap_t);
-    hmap->table = FER_ALLOC_ARR(fer_list_t, size);
+    hmap = dAlloc(sizeof(fer_hmap_t));
+    hmap->table = dAlloc(sizeof(fer_list_t) * size);
     hmap->size = size;
 
     hmap->hash = hash_func;
@@ -64,8 +64,8 @@ void ferHMapDel(fer_hmap_t *h)
         }
     }
 
-    FER_FREE(h->table);
-    FER_FREE(h);
+    dFree(h->table, sizeof(fer_list_t) * h->size);
+    dFree(h, sizeof(fer_hmap_t));
 }
 
 
diff --git a/ode/src/fermat/tasks.c b/ode/src/fermat/tasks.c
index 6f1c947..7743182 100644
--- a/ode/src/fermat/tasks.c
+++ b/ode/src/fermat/tasks.c
@@ -14,9 +14,9 @@
  *  See the License for more information.
  */
 
+#include "../../include/ode/memory.h"
 #include <limits.h>
 #include <fermat/tasks.h>
-#include <fermat/alloc.h>
 
 /** Maximal queue size */
 #define QUEUE_SIZE SEM_VALUE_MAX
@@ -63,7 +63,7 @@ fer_tasks_t *ferTasksNew(size_t num_threads)
     fer_tasks_thread_t *th;
     size_t i;
 
-    t = FER_ALLOC(fer_tasks_t);
+    t = dAlloc(sizeof(fer_tasks_t));
     ferListInit(&t->tasks);
     ferListInit(&t->threads);
     t->threads_len = 0;
@@ -125,7 +125,7 @@ void ferTasksDel(fer_tasks_t *t)
 
     pthread_cond_destroy(&t->pending_cond);
 
-    FER_FREE(t);
+    dFree(t, sizeof(fer_tasks_t));
 }
 
 void ferTasksCancelDel(fer_tasks_t *t)
@@ -199,7 +199,7 @@ static fer_tasks_thread_t *threadNew(fer_tasks_t *t)
 {
     fer_tasks_thread_t *th;
 
-    th = FER_ALLOC(fer_tasks_thread_t);
+    th = dAlloc(sizeof(fer_tasks_thread_t));
     th->info.id    = t->next_id++;
     th->tasks      = t;
     ferListInit(&th->list);
@@ -209,7 +209,7 @@ static fer_tasks_thread_t *threadNew(fer_tasks_t *t)
 
 static void threadDel(fer_tasks_thread_t *th)
 {
-    FER_FREE(th);
+    dFree(th, sizeof(fer_tasks_thread_t));
 }
 
 static void threadJoin(fer_tasks_thread_t *th)
@@ -283,7 +283,7 @@ static fer_tasks_task_t *taskNew(fer_tasks_t *t, fer_tasks_fn fn, void *data, in
 {
     fer_tasks_task_t *task;
 
-    task = FER_ALLOC(fer_tasks_task_t);
+    task = dAlloc(sizeof(fer_tasks_task_t));
     task->fn     = fn;
     task->data   = data;
     task->id     = id;
@@ -295,7 +295,7 @@ static fer_tasks_task_t *taskNew(fer_tasks_t *t, fer_tasks_fn fn, void *data, in
 
 static void taskDel(fer_tasks_task_t *task)
 {
-    FER_FREE(task);
+    dFree(task, sizeof(fer_tasks_task_t));
 }
 
 
-- 
1.7.7.3

