diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx
index ab28ca8..f15e777 100644
--- a/fcl/fcl.pyx
+++ b/fcl/fcl.pyx
@@ -16,26 +16,51 @@ cimport octomap_defs as octomap
 cimport std_defs as std 
 from collision_data import Contact, CostSource, CollisionRequest, ContinuousCollisionRequest, CollisionResult, ContinuousCollisionResult, DistanceRequest, DistanceResult
 
+"""
+Eigen::Transform linear and translation parts are returned as Eigen::Block
+It can be an rvalue and an lvalue, so in C++ you could assign something to translation() like:
+    `tf.translation() = (Vector3d (0., 0., 50));`
+In python and cython however, a function call is never an lvalue, so we workaround with the following macro
+"""
+cdef extern from *:
+    """
+    /* Verbatim C as a workaround for assingment to lvalue-returning functions*/
+    #define ASSIGN(a, b) a = b
+    """
+    void ASSIGN(defs.Vector3d&, defs.Vector3d)
+    void ASSIGN(defs.Matrix3d&, defs.Matrix3d)
+    #void ASSIGN[T](T&, T)  # This doesn't work somehow
+
 ###############################################################################
 # Transforms
 ###############################################################################
 cdef class Transform:
-    cdef defs.Transform3f *thisptr
+    cdef defs.Transform3d *thisptr
 
     def __cinit__(self, *args):
         if len(args) == 0:
-            self.thisptr = new defs.Transform3f()
+            self.thisptr = new defs.Transform3d()
+            self.thisptr.setIdentity()
         elif len(args) == 1:
             if isinstance(args[0], Transform):
-                self.thisptr = new defs.Transform3f(deref((<Transform> args[0]).thisptr))
+                self.thisptr = new defs.Transform3d(deref((<Transform> args[0]).thisptr))
             else:
                 data = numpy.array(args[0])
                 if data.shape == (3,3):
-                    self.thisptr = new defs.Transform3f(numpy_to_mat3f(data))
+                    self.thisptr = new defs.Transform3d()
+                    self.thisptr.setIdentity()
+                    ASSIGN(self.thisptr.linear(),
+                           numpy_to_mat3d(data))
                 elif data.shape == (4,):
-                    self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(data))
+                    self.thisptr = new defs.Transform3d()
+                    self.thisptr.setIdentity()
+                    ASSIGN(self.thisptr.linear(),
+                           numpy_to_quaternion3d(data).toRotationMatrix())
                 elif data.shape == (3,):
-                    self.thisptr = new defs.Transform3f(numpy_to_vec3f(data))
+                    self.thisptr = new defs.Transform3d()
+                    self.thisptr.setIdentity()
+                    ASSIGN(self.thisptr.translation(),
+                           numpy_to_vec3d(data))
                 else:
                     raise ValueError('Invalid input to Transform().')
         elif len(args) == 2:
@@ -45,9 +70,19 @@ cdef class Transform:
                 raise ValueError('Translation must be (3,).')
 
             if rot.shape == (3,3):
-                self.thisptr = new defs.Transform3f(numpy_to_mat3f(rot), numpy_to_vec3f(trans))
+                self.thisptr = new defs.Transform3d()
+                self.thisptr.setIdentity()
+                ASSIGN(self.thisptr.linear(),
+                       numpy_to_mat3d(rot))
+                ASSIGN(self.thisptr.translation(),
+                       numpy_to_vec3d(trans))
             elif rot.shape == (4,):
-                self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(rot), numpy_to_vec3f(trans))
+                self.thisptr = new defs.Transform3d()
+                self.thisptr.setIdentity()
+                ASSIGN(self.thisptr.linear(),
+                       numpy_to_quaternion3d(rot).toRotationMatrix())
+                ASSIGN(self.thisptr.translation(),
+                       numpy_to_vec3d(trans))
             else:
                 raise ValueError('Invalid input to Transform().')
         else:
@@ -58,29 +93,33 @@ cdef class Transform:
             free(self.thisptr)
 
     def getRotation(self):
-        return mat3f_to_numpy(self.thisptr.getRotation())
+        return mat3d_to_numpy(self.thisptr.linear())
 
     def getTranslation(self):
-        return vec3f_to_numpy(self.thisptr.getTranslation())
+        return vec3d_to_numpy(self.thisptr.translation())
 
     def getQuatRotation(self):
-        return quaternion3f_to_numpy(self.thisptr.getQuatRotation())
+        cdef defs.Quaterniond quaternion = defs.Quaterniond(self.thisptr.linear())
+        return quaternion3d_to_numpy(quaternion)
 
     def setRotation(self, R):
-        self.thisptr.setRotation(numpy_to_mat3f(R))
+        ASSIGN(self.thisptr.linear(),
+               numpy_to_mat3d(R))
 
     def setTranslation(self, T):
-        self.thisptr.setTranslation(numpy_to_vec3f(T))
+        ASSIGN(self.thisptr.translation(),
+               numpy_to_vec3d(T))
 
     def setQuatRotation(self, q):
-        self.thisptr.setQuatRotation(numpy_to_quaternion3f(q))
+        ASSIGN(self.thisptr.linear(),
+               numpy_to_quaternion3d(q).toRotationMatrix())
 
 ###############################################################################
 # Collision objects and geometries
 ###############################################################################
 
 cdef class CollisionObject:
-    cdef defs.CollisionObject *thisptr
+    cdef defs.CollisionObjectd *thisptr
     cdef defs.PyObject *geom
     cdef bool _no_instance
 
@@ -92,9 +131,9 @@ cdef class CollisionObject:
         self._no_instance = _no_instance
         if geom.getNodeType() is not None and not self._no_instance:
             if tf is not None:
-                self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr), deref(tf.thisptr))
+                self.thisptr = new defs.CollisionObjectd(defs.shared_ptr[defs.CollisionGeometryd](geom.thisptr), deref(tf.thisptr))
             else:
-                self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr))
+                self.thisptr = new defs.CollisionObjectd(defs.shared_ptr[defs.CollisionGeometryd](geom.thisptr))
             self.thisptr.setUserData(<void*> self.geom) # Save the python geometry object for later retrieval
         else:
             if not self._no_instance:
@@ -112,24 +151,24 @@ cdef class CollisionObject:
         return self.thisptr.getNodeType()
 
     def getTranslation(self):
-        return vec3f_to_numpy(self.thisptr.getTranslation())
+        return vec3d_to_numpy(self.thisptr.getTranslation())
 
     def setTranslation(self, vec):
-        self.thisptr.setTranslation(numpy_to_vec3f(vec))
+        self.thisptr.setTranslation(numpy_to_vec3d(vec))
         self.thisptr.computeAABB()
 
     def getRotation(self):
-        return mat3f_to_numpy(self.thisptr.getRotation())
+        return mat3d_to_numpy(self.thisptr.getRotation())
 
     def setRotation(self, mat):
-        self.thisptr.setRotation(numpy_to_mat3f(mat))
+        self.thisptr.setRotation(numpy_to_mat3d(mat))
         self.thisptr.computeAABB()
 
     def getQuatRotation(self):
-        return quaternion3f_to_numpy(self.thisptr.getQuatRotation())
+        return quaternion3d_to_numpy(self.thisptr.getQuatRotation())
 
     def setQuatRotation(self, q):
-        self.thisptr.setQuatRotation(numpy_to_quaternion3f(q))
+        self.thisptr.setQuatRotation(numpy_to_quaternion3d(q))
         self.thisptr.computeAABB()
 
     def getTransform(self):
@@ -151,7 +190,7 @@ cdef class CollisionObject:
         return self.thisptr.isUncertain()
 
 cdef class CollisionGeometry:
-    cdef defs.CollisionGeometry *thisptr
+    cdef defs.CollisionGeometryd *thisptr
 
     def __cinit__(self):
         pass
@@ -175,7 +214,7 @@ cdef class CollisionGeometry:
     property aabb_center:
         def __get__(self):
             if self.thisptr:
-                return vec3f_to_numpy(self.thisptr.aabb_center)
+                return vec3d_to_numpy(self.thisptr.aabb_center)
             else:
                 return None
         def __set__(self, value):
@@ -188,153 +227,149 @@ cdef class CollisionGeometry:
 
 cdef class TriangleP(CollisionGeometry):
     def __cinit__(self, a, b, c):
-        self.thisptr = new defs.TriangleP(numpy_to_vec3f(a), numpy_to_vec3f(b), numpy_to_vec3f(c))
+        self.thisptr = new defs.TrianglePd(numpy_to_vec3d(a), numpy_to_vec3d(b), numpy_to_vec3d(c))
 
     property a:
         def __get__(self):
-            return vec3f_to_numpy((<defs.TriangleP*> self.thisptr).a)
+            return vec3d_to_numpy((<defs.TrianglePd*> self.thisptr).a)
         def __set__(self, value):
-            (<defs.TriangleP*> self.thisptr).a[0] = <double?> value[0]
-            (<defs.TriangleP*> self.thisptr).a[1] = <double?> value[1]
-            (<defs.TriangleP*> self.thisptr).a[2] = <double?> value[2]
+            (<defs.TrianglePd*> self.thisptr).a[0] = <double?> value[0]
+            (<defs.TrianglePd*> self.thisptr).a[1] = <double?> value[1]
+            (<defs.TrianglePd*> self.thisptr).a[2] = <double?> value[2]
 
     property b:
         def __get__(self):
-            return vec3f_to_numpy((<defs.TriangleP*> self.thisptr).b)
+            return vec3d_to_numpy((<defs.TrianglePd*> self.thisptr).b)
         def __set__(self, value):
-            (<defs.TriangleP*> self.thisptr).b[0] = <double?> value[0]
-            (<defs.TriangleP*> self.thisptr).b[1] = <double?> value[1]
-            (<defs.TriangleP*> self.thisptr).b[2] = <double?> value[2]
+            (<defs.TrianglePd*> self.thisptr).b[0] = <double?> value[0]
+            (<defs.TrianglePd*> self.thisptr).b[1] = <double?> value[1]
+            (<defs.TrianglePd*> self.thisptr).b[2] = <double?> value[2]
 
     property c:
         def __get__(self):
-            return vec3f_to_numpy((<defs.TriangleP*> self.thisptr).c)
+            return vec3d_to_numpy((<defs.TrianglePd*> self.thisptr).c)
         def __set__(self, value):
-            (<defs.TriangleP*> self.thisptr).c[0] = <double?> value[0]
-            (<defs.TriangleP*> self.thisptr).c[1] = <double?> value[1]
-            (<defs.TriangleP*> self.thisptr).c[2] = <double?> value[2]
+            (<defs.TrianglePd*> self.thisptr).c[0] = <double?> value[0]
+            (<defs.TrianglePd*> self.thisptr).c[1] = <double?> value[1]
+            (<defs.TrianglePd*> self.thisptr).c[2] = <double?> value[2]
 
 cdef class Box(CollisionGeometry):
     def __cinit__(self, x, y, z):
-        self.thisptr = new defs.Box(x, y, z)
+        self.thisptr = new defs.Boxd(x, y, z)
 
     property side:
         def __get__(self):
-            return vec3f_to_numpy((<defs.Box*> self.thisptr).side)
+            return vec3d_to_numpy((<defs.Boxd*> self.thisptr).side)
         def __set__(self, value):
-            (<defs.Box*> self.thisptr).side[0] = <double?> value[0]
-            (<defs.Box*> self.thisptr).side[1] = <double?> value[1]
-            (<defs.Box*> self.thisptr).side[2] = <double?> value[2]
+            (<defs.Boxd*> self.thisptr).side[0] = <double?> value[0]
+            (<defs.Boxd*> self.thisptr).side[1] = <double?> value[1]
+            (<defs.Boxd*> self.thisptr).side[2] = <double?> value[2]
 
 cdef class Sphere(CollisionGeometry):
     def __cinit__(self, radius):
-        self.thisptr = new defs.Sphere(radius)
+        self.thisptr = new defs.Sphered(radius)
 
     property radius:
         def __get__(self):
-            return (<defs.Sphere*> self.thisptr).radius
+            return (<defs.Sphered*> self.thisptr).radius
         def __set__(self, value):
-            (<defs.Sphere*> self.thisptr).radius = <double?> value
+            (<defs.Sphered*> self.thisptr).radius = <double?> value
 
 cdef class Ellipsoid(CollisionGeometry):
     def __cinit__(self, a, b, c):
-        self.thisptr = new defs.Ellipsoid(<double?> a, <double?> b, <double?> c)
+        self.thisptr = new defs.Ellipsoidd(<double?> a, <double?> b, <double?> c)
 
     property radii:
         def __get__(self):
-            return vec3f_to_numpy((<defs.Ellipsoid*> self.thisptr).radii)
+            return vec3d_to_numpy((<defs.Ellipsoidd*> self.thisptr).radii)
         def __set__(self, values):
-            (<defs.Ellipsoid*> self.thisptr).radii = numpy_to_vec3f(values)
+            (<defs.Ellipsoidd*> self.thisptr).radii = numpy_to_vec3d(values)
 
 cdef class Capsule(CollisionGeometry):
     def __cinit__(self, radius, lz):
-        self.thisptr = new defs.Capsule(radius, lz)
+        self.thisptr = new defs.Capsuled(radius, lz)
 
     property radius:
         def __get__(self):
-            return (<defs.Capsule*> self.thisptr).radius
+            return (<defs.Capsuled*> self.thisptr).radius
         def __set__(self, value):
-            (<defs.Capsule*> self.thisptr).radius = <double?> value
+            (<defs.Capsuled*> self.thisptr).radius = <double?> value
 
     property lz:
         def __get__(self):
-            return (<defs.Capsule*> self.thisptr).lz
+            return (<defs.Capsuled*> self.thisptr).lz
         def __set__(self, value):
-            (<defs.Capsule*> self.thisptr).lz = <double?> value
+            (<defs.Capsuled*> self.thisptr).lz = <double?> value
 
 cdef class Cone(CollisionGeometry):
     def __cinit__(self, radius, lz):
-        self.thisptr = new defs.Cone(radius, lz)
+        self.thisptr = new defs.Coned(radius, lz)
 
     property radius:
         def __get__(self):
-            return (<defs.Cone*> self.thisptr).radius
+            return (<defs.Coned*> self.thisptr).radius
         def __set__(self, value):
-            (<defs.Cone*> self.thisptr).radius = <double?> value
+            (<defs.Coned*> self.thisptr).radius = <double?> value
 
     property lz:
         def __get__(self):
-            return (<defs.Cone*> self.thisptr).lz
+            return (<defs.Coned*> self.thisptr).lz
         def __set__(self, value):
-            (<defs.Cone*> self.thisptr).lz = <double?> value
+            (<defs.Coned*> self.thisptr).lz = <double?> value
 
 cdef class Cylinder(CollisionGeometry):
     def __cinit__(self, radius, lz):
-        self.thisptr = new defs.Cylinder(radius, lz)
+        self.thisptr = new defs.Cylinderd(radius, lz)
 
     property radius:
         def __get__(self):
-            return (<defs.Cylinder*> self.thisptr).radius
+            return (<defs.Cylinderd*> self.thisptr).radius
         def __set__(self, value):
-            (<defs.Cylinder*> self.thisptr).radius = <double?> value
+            (<defs.Cylinderd*> self.thisptr).radius = <double?> value
 
     property lz:
         def __get__(self):
-            return (<defs.Cylinder*> self.thisptr).lz
+            return (<defs.Cylinderd*> self.thisptr).lz
         def __set__(self, value):
-            (<defs.Cylinder*> self.thisptr).lz = <double?> value
+            (<defs.Cylinderd*> self.thisptr).lz = <double?> value
 
 cdef class Halfspace(CollisionGeometry):
-    def __cinit__(self, n, d):
-        self.thisptr = new defs.Halfspace(defs.Vec3f(<double?> n[0],
-                                                     <double?> n[1],
-                                                     <double?> n[2]),
+    def __cinit__(self, np.ndarray[double, ndim=1] n, d):
+        self.thisptr = new defs.Halfspaced(defs.Vector3d(&n[0]),
                                           <double?> d)
 
     property n:
         def __get__(self):
-            return vec3f_to_numpy((<defs.Halfspace*> self.thisptr).n)
+            return vec3d_to_numpy((<defs.Halfspaced*> self.thisptr).n)
         def __set__(self, value):
-            (<defs.Halfspace*> self.thisptr).n[0] = <double?> value[0]
-            (<defs.Halfspace*> self.thisptr).n[1] = <double?> value[1]
-            (<defs.Halfspace*> self.thisptr).n[2] = <double?> value[2]
+            (<defs.Halfspaced*> self.thisptr).n[0] = <double?> value[0]
+            (<defs.Halfspaced*> self.thisptr).n[1] = <double?> value[1]
+            (<defs.Halfspaced*> self.thisptr).n[2] = <double?> value[2]
 
     property d:
         def __get__(self):
-            return (<defs.Halfspace*> self.thisptr).d
+            return (<defs.Halfspaced*> self.thisptr).d
         def __set__(self, value):
-            (<defs.Halfspace*> self.thisptr).d = <double?> value
+            (<defs.Halfspaced*> self.thisptr).d = <double?> value
 
 cdef class Plane(CollisionGeometry):
-    def __cinit__(self, n, d):
-        self.thisptr = new defs.Plane(defs.Vec3f(<double?> n[0],
-                                                 <double?> n[1],
-                                                 <double?> n[2]),
+    def __cinit__(self, np.ndarray[double, ndim=1] n, d):
+        self.thisptr = new defs.Planed(defs.Vector3d(&n[0]),
                                       <double?> d)
 
     property n:
         def __get__(self):
-            return vec3f_to_numpy((<defs.Plane*> self.thisptr).n)
+            return vec3d_to_numpy((<defs.Planed*> self.thisptr).n)
         def __set__(self, value):
-            (<defs.Plane*> self.thisptr).n[0] = <double?> value[0]
-            (<defs.Plane*> self.thisptr).n[1] = <double?> value[1]
-            (<defs.Plane*> self.thisptr).n[2] = <double?> value[2]
+            (<defs.Planed*> self.thisptr).n[0] = <double?> value[0]
+            (<defs.Planed*> self.thisptr).n[1] = <double?> value[1]
+            (<defs.Planed*> self.thisptr).n[2] = <double?> value[2]
 
     property d:
         def __get__(self):
-            return (<defs.Plane*> self.thisptr).d
+            return (<defs.Planed*> self.thisptr).d
         def __set__(self, value):
-            (<defs.Plane*> self.thisptr).d = <double?> value
+            (<defs.Planed*> self.thisptr).d = <double?> value
 
 cdef class BVHModel(CollisionGeometry):
     def __cinit__(self):
@@ -355,20 +390,21 @@ cdef class BVHModel(CollisionGeometry):
         return n
 
     def addVertex(self, x, y, z):
-        n = (<defs.BVHModel*> self.thisptr).addVertex(defs.Vec3f(<double?> x, <double?> y, <double?> z))
+        cdef np.ndarray[double, ndim=1] n = numpy.array([x, y, z])
+        n = (<defs.BVHModel*> self.thisptr).addVertex(defs.Vector3d(&n[0]))
         return self._check_ret_value(n)
 
     def addTriangle(self, v1, v2, v3):
-        n = (<defs.BVHModel*> self.thisptr).addTriangle(numpy_to_vec3f(v1),
-                                                        numpy_to_vec3f(v2),
-                                                        numpy_to_vec3f(v3))
+        n = (<defs.BVHModel*> self.thisptr).addTriangle(numpy_to_vec3d(v1),
+                                                        numpy_to_vec3d(v2),
+                                                        numpy_to_vec3d(v3))
         return self._check_ret_value(n)
 
     def addSubModel(self, verts, triangles):
-        cdef vector[defs.Vec3f] ps
+        cdef vector[defs.Vector3d] ps
         cdef vector[defs.Triangle] tris
         for vert in verts:
-            ps.push_back(numpy_to_vec3f(vert))
+            ps.push_back(numpy_to_vec3d(vert))
         for tri in triangles:
             tris.push_back(defs.Triangle(<size_t?> tri[0], <size_t?> tri[1], <size_t?> tri[2]))
         n = (<defs.BVHModel*> self.thisptr).addSubModel(ps, tris)
@@ -406,7 +442,7 @@ cdef class OcTree(CollisionGeometry):
 
         self.tree = new octomap.OcTree(r) 
         self.tree.readBinaryData(ss)
-        self.thisptr = new defs.OcTree(defs.shared_ptr[octomap.OcTree](self.tree))
+        self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree))
 
 
 ###############################################################################
@@ -414,11 +450,11 @@ cdef class OcTree(CollisionGeometry):
 ###############################################################################
 
 cdef class DynamicAABBTreeCollisionManager:
-    cdef defs.DynamicAABBTreeCollisionManager *thisptr
+    cdef defs.DynamicAABBTreeCollisionManagerd *thisptr
     cdef list objs
 
     def __cinit__(self):
-        self.thisptr = new defs.DynamicAABBTreeCollisionManager()
+        self.thisptr = new defs.DynamicAABBTreeCollisionManagerd()
         self.objs = []
 
     def __dealloc__(self):
@@ -426,7 +462,7 @@ cdef class DynamicAABBTreeCollisionManager:
             del self.thisptr
 
     def registerObjects(self, other_objs):
-        cdef vector[defs.CollisionObject*] pobjs
+        cdef vector[defs.CollisionObjectd*] pobjs
         for obj in other_objs:
             self.objs.append(obj)
             pobjs.push_back((<CollisionObject?> obj).thisptr)
@@ -445,7 +481,7 @@ cdef class DynamicAABBTreeCollisionManager:
         self.thisptr.setup()
 
     def update(self, arg=None):
-        cdef vector[defs.CollisionObject*] objs
+        cdef vector[defs.CollisionObjectd*] objs
         if hasattr(arg, "__len__"):
             for a in arg:
                 objs.push_back((<CollisionObject?> a).thisptr)
@@ -547,10 +583,10 @@ def collide(CollisionObject o1, CollisionObject o2,
     if result is None:
         result = CollisionResult()
 
-    cdef defs.CollisionResult cresult
+    cdef defs.CollisionResultd cresult
 
     cdef size_t ret = defs.collide(o1.thisptr, o2.thisptr,
-                                   defs.CollisionRequest(
+                                   defs.CollisionRequestd(
                                        <size_t?> request.num_max_contacts,
                                        <bool?> request.enable_contact,
                                        <size_t?> request.num_max_cost_sources,
@@ -562,12 +598,12 @@ def collide(CollisionObject o1, CollisionObject o2,
 
     result.is_collision = result.is_collision or cresult.isCollision()
 
-    cdef vector[defs.Contact] contacts
+    cdef vector[defs.Contactd] contacts
     cresult.getContacts(contacts)
     for idx in range(contacts.size()):
         result.contacts.append(c_to_python_contact(contacts[idx], o1, o2))
 
-    cdef vector[defs.CostSource] costs
+    cdef vector[defs.CostSourced] costs
     cresult.getCostSources(costs)
     for idx in range(costs.size()):
         result.cost_sources.append(c_to_python_costsource(costs[idx]))
@@ -583,13 +619,13 @@ def continuousCollide(CollisionObject o1, Transform tf1_end,
     if result is None:
         result = ContinuousCollisionResult()
 
-    cdef defs.ContinuousCollisionResult cresult
+    cdef defs.ContinuousCollisionResultd cresult
 
-    cdef defs.FCL_REAL ret = defs.continuousCollide(o1.thisptr, deref(tf1_end.thisptr),
+    cdef double ret = defs.continuousCollide(o1.thisptr, deref(tf1_end.thisptr),
                                                     o2.thisptr, deref(tf2_end.thisptr),
-                                                    defs.ContinuousCollisionRequest(
+                                                    defs.ContinuousCollisionRequestd(
                                                         <size_t?>             request.num_max_iterations,
-                                                        <defs.FCL_REAL?>      request.toc_err,
+                                                        <double?>      request.toc_err,
                                                         <defs.CCDMotionType?> request.ccd_motion_type,
                                                         <defs.GJKSolverType?> request.gjk_solver_type,
                                                         <defs.CCDSolverType?> request.ccd_solver_type,
@@ -609,18 +645,18 @@ def distance(CollisionObject o1, CollisionObject o2,
     if result is None:
         result = DistanceResult()
 
-    cdef defs.DistanceResult cresult
+    cdef defs.DistanceResultd cresult
 
     cdef double dis = defs.distance(o1.thisptr, o2.thisptr,
-                                    defs.DistanceRequest(
+                                    defs.DistanceRequestd(
                                         <bool?> request.enable_nearest_points,
                                         <defs.GJKSolverType?> request.gjk_solver_type
                                     ),
                                     cresult)
 
     result.min_distance = min(cresult.min_distance, result.min_distance)
-    result.nearest_points = [vec3f_to_numpy(cresult.nearest_points[0]),
-                             vec3f_to_numpy(cresult.nearest_points[1])]
+    result.nearest_points = [vec3d_to_numpy(cresult.nearest_points[0]),
+                             vec3d_to_numpy(cresult.nearest_points[1])]
     result.o1 = c_to_python_collision_geometry(cresult.o1, o1, o2)
     result.o2 = c_to_python_collision_geometry(cresult.o2, o1, o2)
     result.b1 = cresult.b1
@@ -670,7 +706,7 @@ cdef class CollisionFunction:
         self.py_func = py_func
         self.py_args = py_args
 
-    cdef bool eval_func(self, defs.CollisionObject*o1, defs.CollisionObject*o2):
+    cdef bool eval_func(self, defs.CollisionObjectd*o1, defs.CollisionObjectd*o2):
         cdef object py_r = defs.PyObject_CallObject(self.py_func,
                                                     (copy_ptr_collision_object(o1),
                                                      copy_ptr_collision_object(o2),
@@ -686,18 +722,18 @@ cdef class DistanceFunction:
         self.py_func = py_func
         self.py_args = py_args
 
-    cdef bool eval_func(self, defs.CollisionObject*o1, defs.CollisionObject*o2, defs.FCL_REAL& dist):
+    cdef bool eval_func(self, defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, double& dist):
         cdef object py_r = defs.PyObject_CallObject(self.py_func,
                                                     (copy_ptr_collision_object(o1),
                                                      copy_ptr_collision_object(o2),
                                                      self.py_args))
-        (&dist)[0] = <defs.FCL_REAL?> py_r[1]
+        (&dist)[0] = <double?> py_r[1]
         return <bool?> py_r[0]
 
-cdef inline bool CollisionCallBack(defs.CollisionObject*o1, defs.CollisionObject*o2, void*cdata):
+cdef inline bool CollisionCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata):
     return (<CollisionFunction> cdata).eval_func(o1, o2)
 
-cdef inline bool DistanceCallBack(defs.CollisionObject*o1, defs.CollisionObject*o2, void*cdata, defs.FCL_REAL& dist):
+cdef inline bool DistanceCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata, double& dist):
     return (<DistanceFunction> cdata).eval_func(o1, o2, dist)
 
 
@@ -705,56 +741,57 @@ cdef inline bool DistanceCallBack(defs.CollisionObject*o1, defs.CollisionObject*
 # Helper Functions
 ###############################################################################
 
-cdef quaternion3f_to_numpy(defs.Quaternion3f q):
-    return numpy.array([q.getW(), q.getX(), q.getY(), q.getZ()])
+cdef quaternion3d_to_numpy(defs.Quaterniond q):
+    return numpy.array([q.w(), q.x(), q.y(), q.z()])
 
-cdef defs.Quaternion3f numpy_to_quaternion3f(a):
-    return defs.Quaternion3f(<double?> a[0], <double?> a[1], <double?> a[2], <double?> a[3])
+cdef defs.Quaterniond numpy_to_quaternion3d(a):
+    return defs.Quaterniond(<double?> a[0], <double?> a[1], <double?> a[2], <double?> a[3])
 
-cdef vec3f_to_numpy(defs.Vec3f vec):
+cdef vec3d_to_numpy(defs.Vector3d vec):
     return numpy.array([vec[0], vec[1], vec[2]])
 
-cdef defs.Vec3f numpy_to_vec3f(a):
-    return defs.Vec3f(<double?> a[0], <double?> a[1], <double?> a[2])
+cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1] a):
+    return defs.Vector3d(&a[0])
 
-cdef mat3f_to_numpy(defs.Matrix3f m):
+cdef mat3d_to_numpy(defs.Matrix3d m):
     return numpy.array([[m(0,0), m(0,1), m(0,2)],
                         [m(1,0), m(1,1), m(1,2)],
                         [m(2,0), m(2,1), m(2,2)]])
 
-cdef defs.Matrix3f numpy_to_mat3f(a):
-    return defs.Matrix3f(<double?> a[0][0], <double?> a[0][1], <double?> a[0][2],
-                         <double?> a[1][0], <double?> a[1][1], <double?> a[1][2],
-                         <double?> a[2][0], <double?> a[2][1], <double?> a[2][2])
+cdef defs.Matrix3d numpy_to_mat3d(np.ndarray[double, ndim=2] a):
+    # NOTE Eigen defaults to column-major storage,
+    # which corresponds to non-default Fortran mode of ordering in numpy
+    cdef np.ndarray[double, ndim=2, mode='fortran'] f = np.ndarray.copy(a, order='F')
+    return defs.Matrix3d(&f[0, 0])
 
-cdef c_to_python_collision_geometry(defs.const_CollisionGeometry*geom, CollisionObject o1, CollisionObject o2):
-    cdef CollisionGeometry o1_py_geom = <CollisionGeometry> ((<defs.CollisionObject*> o1.thisptr).getUserData())
-    cdef CollisionGeometry o2_py_geom = <CollisionGeometry> ((<defs.CollisionObject*> o2.thisptr).getUserData())
-    if geom == <defs.const_CollisionGeometry*> o1_py_geom.thisptr:
+cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2):
+    cdef CollisionGeometry o1_py_geom = <CollisionGeometry> ((<defs.CollisionObjectd*> o1.thisptr).getUserData())
+    cdef CollisionGeometry o2_py_geom = <CollisionGeometry> ((<defs.CollisionObjectd*> o2.thisptr).getUserData())
+    if geom == <defs.const_CollisionGeometryd*> o1_py_geom.thisptr:
         return o1_py_geom
     else:
         return o2_py_geom
 
-cdef c_to_python_contact(defs.Contact contact, CollisionObject o1, CollisionObject o2):
+cdef c_to_python_contact(defs.Contactd contact, CollisionObject o1, CollisionObject o2):
     c = Contact()
     c.o1 = c_to_python_collision_geometry(contact.o1, o1, o2)
     c.o2 = c_to_python_collision_geometry(contact.o2, o1, o2)
     c.b1 = contact.b1
     c.b2 = contact.b2
-    c.normal = vec3f_to_numpy(contact.normal)
-    c.pos = vec3f_to_numpy(contact.pos)
+    c.normal = vec3d_to_numpy(contact.normal)
+    c.pos = vec3d_to_numpy(contact.pos)
     c.penetration_depth = contact.penetration_depth
     return c
 
-cdef c_to_python_costsource(defs.CostSource cost_source):
+cdef c_to_python_costsource(defs.CostSourced cost_source):
     c = CostSource()
-    c.aabb_min = vec3f_to_numpy(cost_source.aabb_min)
-    c.aabb_max = vec3f_to_numpy(cost_source.aabb_max)
+    c.aabb_min = vec3d_to_numpy(cost_source.aabb_min)
+    c.aabb_max = vec3d_to_numpy(cost_source.aabb_max)
     c.cost_density = cost_source.cost_density
     c.total_cost = cost_source.total_cost
     return c
 
-cdef copy_ptr_collision_object(defs.CollisionObject*cobj):
+cdef copy_ptr_collision_object(defs.CollisionObjectd*cobj):
     geom = <CollisionGeometry> cobj.getUserData()
     co = CollisionObject(geom, _no_instance=True)
     (<CollisionObject> co).thisptr = cobj
diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd
index d5461fd..649f2f0 100644
--- a/fcl/fcl_defs.pxd
+++ b/fcl/fcl_defs.pxd
@@ -18,136 +18,127 @@ cdef extern from "Python.h":
 #        shared_ptr(T*) except +
 #        T* get()
 
-cdef extern from "fcl/data_types.h" namespace "fcl":
-    ctypedef double FCL_REAL
-
-cdef extern from "fcl/math/vec_3f.h" namespace "fcl":
-    cdef cppclass Vec3f:
-        Vec3f() except +
-        Vec3f(FCL_REAL x, FCL_REAL y, FCL_REAL z) except +
-        FCL_REAL& operator[](size_t i)
-
-cdef extern from "fcl/math/matrix_3f.h" namespace "fcl":
-    cdef cppclass Matrix3f:
-        Matrix3f() except +
-        Matrix3f(FCL_REAL xx, FCL_REAL xy, FCL_REAL xz,
-                 FCL_REAL yx, FCL_REAL yy, FCL_REAL yz,
-                 FCL_REAL zx, FCL_REAL zy, FCL_REAL zz) except +
-        FCL_REAL operator()(size_t i, size_t j)
-
-cdef extern from "fcl/math/transform.h" namespace "fcl":
-    cdef cppclass Quaternion3f:
-        Quaternion3f() except +
-        Quaternion3f(FCL_REAL a, FCL_REAL b,
-                     FCL_REAL c, FCL_REAL d) except +
-        void fromRotation(Matrix3f& R)
-        void fromAxisAngle(Vec3f& axis, FCL_REAL angle)
-        FCL_REAL& getW()
-        FCL_REAL& getX()
-        FCL_REAL& getY()
-        FCL_REAL& getZ()
-
-    cdef cppclass Transform3f:
-        Transform3f() except +
-        Transform3f(Matrix3f& R_, Vec3f& T_)
-        Transform3f(Quaternion3f& q_, Vec3f& T_)
-        Transform3f(Matrix3f& R_)
-        Transform3f(Quaternion3f& q_)
-        Transform3f(Vec3f& T_)
-        Transform3f(Transform3f& tf_)
-        Matrix3f& getRotation()
-        Vec3f& getTranslation()
-        Quaternion3f& getQuatRotation()
-        void setRotation(Matrix3f& R_)
-        void setTranslation(Vec3f& T_)
-        void setQuatRotation(Quaternion3f & q_)
-
-cdef extern from "fcl/collision_data.h" namespace "fcl":
-
+cdef extern from "fcl/common/types.h" namespace "fcl":
+    cdef cppclass Vector3d:
+        Vector3d() except +
+        Vector3d(double *data) except +
+        double& operator[](size_t i)
+
+    cdef cppclass Matrix3d:
+        Matrix3d() except +
+        Matrix3d(double *data)
+        double operator()(size_t i, size_t j)
+
+    cdef cppclass Quaterniond:
+        Quaterniond() except +
+        Quaterniond(double a, double b, double c, double d) except +
+        Quaterniond(Matrix3d& R) except +
+        double& w()
+        double& x()
+        double& y()
+        double& z()
+        Matrix3d& toRotationMatrix()
+
+    cdef cppclass Transform3d:
+        Transform3d() except +
+        Transform3d(Transform3d& tf_)
+        void setIdentity()
+        Matrix3d& linear()
+        Vector3d& translation()
+
+cdef extern from "fcl/narrowphase/continuous_collision_request.h" namespace "fcl":
     cdef enum CCDMotionType:
         CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE
 
     cdef enum CCDSolverType:
         CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER
 
+    cdef cppclass ContinuousCollisionRequestd:
+         size_t num_max_iterations_,
+         double toc_err_,
+         CCDMotionType ccd_motion_type_,
+         GJKSolverType gjk_solver_type_,
+         GJKSolverType ccd_solver_type_
+
+         ContinuousCollisionRequestd(
+                             size_t num_max_iterations_,
+                             double toc_err_,
+                             CCDMotionType ccd_motion_type_,
+                             GJKSolverType gjk_solver_type_,
+                             CCDSolverType ccd_solver_type_ )
+
+cdef extern from "fcl/narrowphase/gjk_solver_type.h" namespace "fcl":
     cdef enum GJKSolverType:
         GST_LIBCCD, GST_INDEP
 
-    cdef cppclass Contact:
-        CollisionGeometry *o1
-        CollisionGeometry *o2
+cdef extern from "fcl/narrowphase/contact.h" namespace "fcl":
+    cdef cppclass Contactd:
+        CollisionGeometryd *o1
+        CollisionGeometryd *o2
         int b1
         int b2
-        Vec3f normal
-        Vec3f pos
-        FCL_REAL penetration_depth
-        Contact() except +
-        Contact(CollisionGeometry* o1_,
-                CollisionGeometry* o2_,
+        Vector3d normal
+        Vector3d pos
+        double penetration_depth
+        Contactd() except +
+        Contactd(CollisionGeometryd* o1_,
+                CollisionGeometryd* o2_,
                 int b1_, int b2_) except +
 
-    cdef cppclass CostSource:
-        Vec3f aabb_min
-        Vec3f aabb_max
-        FCL_REAL cost_density
-        FCL_REAL total_cost
+cdef extern from "fcl/narrowphase/cost_source.h" namespace "fcl":
+    cdef cppclass CostSourced:
+        Vector3d aabb_min
+        Vector3d aabb_max
+        double cost_density
+        double total_cost
 
-    cdef cppclass CollisionResult:
-        CollisionResult() except +
+cdef extern from "fcl/narrowphase/collision_result.h" namespace "fcl":
+    cdef cppclass CollisionResultd:
+        CollisionResultd() except +
         bool isCollision()
-        void getContacts(vector[Contact]& contacts_)
-        void getCostSources(vector[CostSource]& cost_sources_)
+        void getContacts(vector[Contactd]& contacts_)
+        void getCostSources(vector[CostSourced]& cost_sources_)
 
-    cdef cppclass ContinuousCollisionResult:
-        ContinuousCollisionResult() except +
+cdef extern from "fcl/narrowphase/continuous_collision_result.h" namespace "fcl":
+    cdef cppclass ContinuousCollisionResultd:
+        ContinuousCollisionResultd() except +
         bool is_collide
-        FCL_REAL time_of_contact
-        Transform3f contact_tf1, contact_tf2
+        double time_of_contact
+        Transform3d contact_tf1, contact_tf2
 
-    cdef cppclass CollisionRequest:
+cdef extern from "fcl/narrowphase/collision_request.h" namespace "fcl":
+    cdef cppclass CollisionRequestd:
         size_t num_max_contacts
         bool enable_contact
         size_t num_max_cost_sources
         bool enable_cost
         bool use_approximate_cost
         GJKSolverType gjk_solver_type
-        CollisionRequest(size_t num_max_contacts_,
+        CollisionRequestd(size_t num_max_contacts_,
                          bool enable_contact_,
                          size_t num_max_cost_sources_,
                          bool enable_cost_,
                          bool use_approximate_cost_,
                          GJKSolverType gjk_solver_type_)
 
-    cdef cppclass ContinuousCollisionRequest:
-         size_t num_max_iterations_,
-         FCL_REAL toc_err_,
-         CCDMotionType ccd_motion_type_,
-         GJKSolverType gjk_solver_type_,
-         GJKSolverType ccd_solver_type_
-
-         ContinuousCollisionRequest(
-                             size_t num_max_iterations_,
-                             FCL_REAL toc_err_,
-                             CCDMotionType ccd_motion_type_,
-                             GJKSolverType gjk_solver_type_,
-                             CCDSolverType ccd_solver_type_ )
-
-    cdef cppclass DistanceResult:
-        FCL_REAL min_distance
-        Vec3f* nearest_points
-        CollisionGeometry* o1
-        CollisionGeometry* o2
+cdef extern from "fcl/narrowphase/distance_result.h" namespace "fcl":
+    cdef cppclass DistanceResultd:
+        double min_distance
+        Vector3d* nearest_points
+        CollisionGeometryd* o1
+        CollisionGeometryd* o2
         int b1
         int b2
-        DistanceResult(FCL_REAL min_distance_) except +
-        DistanceResult() except +
+        DistanceResultd(double min_distance_) except +
+        DistanceResultd() except +
 
-    cdef cppclass DistanceRequest:
+cdef extern from "fcl/narrowphase/distance_request.h" namespace "fcl":
+    cdef cppclass DistanceRequestd:
         bool enable_nearest_points
         GJKSolverType gjk_solver_type
-        DistanceRequest(bool enable_nearest_points_, GJKSolverType gjk_solver_type_) except +
+        DistanceRequestd(bool enable_nearest_points_, GJKSolverType gjk_solver_type_) except +
 
-cdef extern from "fcl/collision_object.h" namespace "fcl":
+cdef extern from "fcl/geometry/collision_geometry.h" namespace "fcl":
     cdef enum OBJECT_TYPE:
         OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT
     cdef enum NODE_TYPE:
@@ -155,34 +146,37 @@ cdef extern from "fcl/collision_object.h" namespace "fcl":
         GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX,
         GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT
 
-    cdef cppclass CollisionGeometry:
-        CollisionGeometry() except +
+    cdef cppclass CollisionGeometryd:
+        CollisionGeometryd() except +
         OBJECT_TYPE getObjectType()
         NODE_TYPE getNodeType()
         void computeLocalAABB()
-        Vec3f aabb_center
-        FCL_REAL aabb_radius
-        FCL_REAL cost_density
-        FCL_REAL threshold_occupied
-        FCL_REAL threshold_free
-
-    cdef cppclass CollisionObject:
-        CollisionObject() except +
-        CollisionObject(shared_ptr[CollisionGeometry]& cgeom_) except +
-        CollisionObject(shared_ptr[CollisionGeometry]& cgeom_, Transform3f& tf) except +
+        Vector3d aabb_center
+        double aabb_radius
+        double cost_density
+        double threshold_occupied
+        double threshold_free
+
+    ctypedef CollisionGeometryd const_CollisionGeometryd "const fcl::CollisionGeometryd"
+
+cdef extern from "fcl/narrowphase/collision_object.h" namespace "fcl":
+    cdef cppclass CollisionObjectd:
+        CollisionObjectd() except +
+        CollisionObjectd(shared_ptr[CollisionGeometryd]& cgeom_) except +
+        CollisionObjectd(shared_ptr[CollisionGeometryd]& cgeom_, Transform3d& tf) except +
         OBJECT_TYPE getObjectType()
         NODE_TYPE getNodeType()
-        Vec3f& getTranslation()
-        Matrix3f& getRotation()
-        Quaternion3f& getQuatRotation()
-        Transform3f& getTransform()
-        CollisionGeometry* getCollisionGeometry()
-        void setTranslation(Vec3f& T)
-        void setRotation(Matrix3f& M)
-        void setQuatRotation(Quaternion3f& q)
-        void setTransform(Quaternion3f& q, Vec3f& T)
-        void setTransform(Matrix3f& q, Vec3f& T)
-        void setTransform(Transform3f& tf)
+        Vector3d& getTranslation()
+        Matrix3d& getRotation()
+        Quaterniond& getQuatRotation()
+        Transform3d& getTransform()
+        CollisionGeometryd* getCollisionGeometry()
+        void setTranslation(Vector3d& T)
+        void setRotation(Matrix3d& M)
+        void setQuatRotation(Quaterniond& q)
+        void setTransform(Quaterniond& q, Vector3d& T)
+        void setTransform(Matrix3d& q, Vector3d& T)
+        void setTransform(Transform3d& tf)
         void setUserData(void *data)
         void computeAABB()
         void *getUserData()
@@ -190,82 +184,91 @@ cdef extern from "fcl/collision_object.h" namespace "fcl":
         bool isFree()
         bool isUncertain()
 
-    ctypedef CollisionGeometry const_CollisionGeometry "const fcl::CollisionGeometry"
-    ctypedef CollisionObject const_CollisionObject "const fcl::CollisionObject"
-
-cdef extern from "fcl/shape/geometric_shapes.h" namespace "fcl":
-    cdef cppclass ShapeBase(CollisionGeometry):
-        ShapeBase() except +
-
-    cdef cppclass TriangleP(ShapeBase):
-        TriangleP(Vec3f& a_, Vec3f& b_, Vec3f& c_) except +
-        Vec3f a, b, c
-
-    cdef cppclass Box(ShapeBase):
-        Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) except +
-        Vec3f side
-
-    cdef cppclass Sphere(ShapeBase):
-        Sphere(FCL_REAL radius_) except +
-        FCL_REAL radius
-
-    cdef cppclass Ellipsoid(ShapeBase):
-        Ellipsoid(FCL_REAL a_, FCL_REAL b_, FCL_REAL c_) except +
-        Vec3f radii
-
-    cdef cppclass Capsule(ShapeBase):
-        Capsule(FCL_REAL radius_, FCL_REAL lz_) except +
-        FCL_REAL radius
-        FCL_REAL lz
-
-    cdef cppclass Cone(ShapeBase):
-        Cone(FCL_REAL radius_, FCL_REAL lz_) except +
-        FCL_REAL radius
-        FCL_REAL lz
-
-    cdef cppclass Cylinder(ShapeBase):
-        Cylinder(FCL_REAL radius_, FCL_REAL lz_) except +
-        FCL_REAL radius
-        FCL_REAL lz
-
-    cdef cppclass Convex(ShapeBase):
-        Convex(Vec3f* plane_nomals_,
-               FCL_REAL* plane_dis_,
+    ctypedef CollisionObjectd const_CollisionObjectd "const fcl::CollisionObjectd"
+
+cdef extern from "fcl/geometry/shape/shape_base.h" namespace "fcl":
+    cdef cppclass ShapeBased(CollisionGeometryd):
+        ShapeBased() except +
+
+cdef extern from "fcl/geometry/shape/triangle_p.h" namespace "fcl":
+    cdef cppclass TrianglePd(ShapeBased):
+        TrianglePd(Vector3d& a_, Vector3d& b_, Vector3d& c_) except +
+        Vector3d a, b, c
+
+cdef extern from "fcl/geometry/shape/box.h" namespace "fcl":
+    cdef cppclass Boxd(ShapeBased):
+        Boxd(double x, double y, double z) except +
+        Vector3d side
+
+cdef extern from "fcl/geometry/shape/sphere.h" namespace "fcl":
+    cdef cppclass Sphered(ShapeBased):
+        Sphered(double radius_) except +
+        double radius
+
+cdef extern from "fcl/geometry/shape/ellipsoid.h" namespace "fcl":
+    cdef cppclass Ellipsoidd(ShapeBased):
+        Ellipsoidd(double a_, double b_, double c_) except +
+        Vector3d radii
+
+cdef extern from "fcl/geometry/shape/capsule.h" namespace "fcl":
+    cdef cppclass Capsuled(ShapeBased):
+        Capsuled(double radius_, double lz_) except +
+        double radius
+        double lz
+
+cdef extern from "fcl/geometry/shape/cone.h" namespace "fcl":
+    cdef cppclass Coned(ShapeBased):
+        Coned(double radius_, double lz_) except +
+        double radius
+        double lz
+
+cdef extern from "fcl/geometry/shape/cylinder.h" namespace "fcl":
+    cdef cppclass Cylinderd(ShapeBased):
+        Cylinderd(double radius_, double lz_) except +
+        double radius
+        double lz
+
+cdef extern from "fcl/geometry/shape/convex.h" namespace "fcl":
+    cdef cppclass Convexd(ShapeBased):
+        Convexd(Vector3d* plane_nomals_,
+               double* plane_dis_,
                int num_planes,
-               Vec3f* points_,
+               Vector3d* points_,
                int num_points_,
                int* polygons_) except +
 
-    cdef cppclass Halfspace(ShapeBase):
-        Halfspace(Vec3f& n_, FCL_REAL d_) except +
-        Vec3f n
-        FCL_REAL d
+cdef extern from "fcl/geometry/shape/halfspace.h" namespace "fcl":
+    cdef cppclass Halfspaced(ShapeBased):
+        Halfspaced(Vector3d& n_, double d_) except +
+        Vector3d n
+        double d
 
-    cdef cppclass Plane(ShapeBase):
-        Plane(Vec3f& n_, FCL_REAL d_) except +
-        Vec3f n
-        FCL_REAL d
+cdef extern from "fcl/geometry/shape/plane.h" namespace "fcl":
+    cdef cppclass Planed(ShapeBased):
+        Planed(Vector3d& n_, double d_) except +
+        Vector3d n
+        double d
 
-cdef extern from "fcl/broadphase/broadphase.h" namespace "fcl":
-    ctypedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata)
-    ctypedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist)
+cdef extern from "fcl/broadphase/broadphase_collision_manager.h" namespace "fcl":
+    ctypedef bool (*CollisionCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata)
+    ctypedef bool (*DistanceCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata, double& dist)
 
 cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl":
-    cdef cppclass DynamicAABBTreeCollisionManager:
-        DynamicAABBTreeCollisionManager() except +
-        void registerObjects(vector[CollisionObject*]& other_objs)
-        void registerObject(CollisionObject* obj)
-        void unregisterObject(CollisionObject* obj)
-        void collide(DynamicAABBTreeCollisionManager* mgr, void* cdata, CollisionCallBack callback)
-        void distance(DynamicAABBTreeCollisionManager* mgr, void* cdata, DistanceCallBack callback)
-        void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback)
-        void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback)
+    cdef cppclass DynamicAABBTreeCollisionManagerd:
+        DynamicAABBTreeCollisionManagerd() except +
+        void registerObjects(vector[CollisionObjectd*]& other_objs)
+        void registerObject(CollisionObjectd* obj)
+        void unregisterObject(CollisionObjectd* obj)
+        void collide(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, CollisionCallBack callback)
+        void distance(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, DistanceCallBack callback)
+        void collide(CollisionObjectd* obj, void* cdata, CollisionCallBack callback)
+        void distance(CollisionObjectd* obj, void* cdata, DistanceCallBack callback)
         void collide(void* cdata, CollisionCallBack callback)
         void distance(void* cdata, DistanceCallBack callback)
         void setup()
         void update()
-        void update(CollisionObject* updated_obj)
-        void update(vector[CollisionObject*] updated_objs)
+        void update(CollisionObjectd* updated_obj)
+        void update(vector[CollisionObjectd*] updated_objs)
         void clear()
         bool empty()
         size_t size()
@@ -277,42 +280,41 @@ cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl"
         bool octree_as_geometry_collide
         bool octree_as_geometry_distance
 
-cdef extern from "fcl/collision.h" namespace "fcl":
-    size_t collide(CollisionObject* o1, CollisionObject* o2,
-                   CollisionRequest& request,
-                   CollisionResult& result)
+cdef extern from "fcl/narrowphase/collision.h" namespace "fcl":
+    size_t collide(CollisionObjectd* o1, CollisionObjectd* o2,
+                   CollisionRequestd& request,
+                   CollisionResultd& result)
 
-    size_t collide(CollisionGeometry* o1, Transform3f& tf1,
-                   CollisionGeometry* o2, Transform3f& tf2,
-                   CollisionRequest& request,
-                   CollisionResult& result)
+    size_t collide(CollisionGeometryd* o1, Transform3d& tf1,
+                   CollisionGeometryd* o2, Transform3d& tf2,
+                   CollisionRequestd& request,
+                   CollisionResultd& result)
 
-cdef extern from "fcl/continuous_collision.h" namespace "fcl":
-    FCL_REAL continuousCollide(CollisionGeometry* o1, Transform3f& tf1_beg, Transform3f& tf1_end,
-                               CollisionGeometry* o2, Transform3f& tf2_beg, Transform3f& tf2_end,
-                               ContinuousCollisionRequest& request,
-                               ContinuousCollisionResult& result)
+cdef extern from "fcl/narrowphase/continuous_collision.h" namespace "fcl":
+    double continuousCollide(CollisionGeometryd* o1, Transform3d& tf1_beg, Transform3d& tf1_end,
+                               CollisionGeometryd* o2, Transform3d& tf2_beg, Transform3d& tf2_end,
+                               ContinuousCollisionRequestd& request,
+                               ContinuousCollisionResultd& result)
 
-    FCL_REAL continuousCollide(CollisionObject* o1, Transform3f& tf1_end,
-                               CollisionObject* o2, Transform3f& tf2_end,
-                               ContinuousCollisionRequest& request,
-                               ContinuousCollisionResult& result)
+    double continuousCollide(CollisionObjectd* o1, Transform3d& tf1_end,
+                               CollisionObjectd* o2, Transform3d& tf2_end,
+                               ContinuousCollisionRequestd& request,
+                               ContinuousCollisionResultd& result)
 
 
-cdef extern from "fcl/distance.h" namespace "fcl":
-    FCL_REAL distance(CollisionObject* o1, CollisionObject* o2,
-                      DistanceRequest& request, DistanceResult& result)
-    FCL_REAL distance(CollisionGeometry* o1, Transform3f& tf1,
-                      CollisionGeometry* o2, Transform3f& tf2,
-                      DistanceRequest& request, DistanceResult& result)
+cdef extern from "fcl/narrowphase/distance.h" namespace "fcl":
+    double distance(CollisionObjectd* o1, CollisionObjectd* o2,
+                      DistanceRequestd& request, DistanceResultd& result)
+    double distance(CollisionGeometryd* o1, Transform3d& tf1,
+                      CollisionGeometryd* o2, Transform3d& tf2,
+                      DistanceRequestd& request, DistanceResultd& result)
 
-cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl":
+cdef extern from "fcl/geometry/BVH/BVH_internal.h" namespace "fcl":
     cdef enum BVHModelType:
         BVH_MODEL_UNKNOWN,   # unknown model type
         BVH_MODEL_TRIANGLES, # triangle model
         BVH_MODEL_POINTCLOUD # point cloud model
 
-cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl":
     cdef enum  BVHReturnCode:
         BVH_OK = 0,                              # BVH is valid
         BVH_ERR_MODEL_OUT_OF_MEMORY = -1,        # Cannot allocate memory for vertices and triangles
@@ -325,7 +327,6 @@ cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl":
         BVH_ERR_UNKNOWN = -8                     # Unknown failure
 
 
-cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl":
     cdef enum BVHBuildState:
         BVH_BUILD_STATE_EMPTY,         # empty state, immediately after constructor
         BVH_BUILD_STATE_BEGUN,         # after beginModel(), state for adding geometry primitives
@@ -334,36 +335,37 @@ cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl":
         BVH_BUILD_STATE_UPDATED,       # after tree has been build for updated geometry, ready for ccd use
         BVH_BUILD_STATE_REPLACE_BEGUN, # after beginReplaceModel(), state for replacing geometry primitives
 
-cdef extern from "fcl/data_types.h" namespace "fcl":
+cdef extern from "fcl/math/triangle.h" namespace "fcl":
     cdef cppclass Triangle:
         Triangle() except +
         Triangle(size_t p1, size_t p2, size_t p3) except +
         size_t vids[3]
 
-cdef extern from "fcl/BVH/BV_splitter.h" namespace "fcl":
+# TODO what about these guys?
+cdef extern from "fcl/geometry/BVH/detail/BV_splitter_base.h" namespace "fcl":
     cdef cppclass BVSplitterBase:
         pass
 
-cdef extern from "fcl/BVH/BV_fitter.h" namespace "fcl":
+cdef extern from "fcl/geometry/BVH/detail/BV_fitter_base.h" namespace "fcl":
     cdef cppclass BVFitterBase:
         pass
 
-cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl":
+cdef extern from "fcl/geometry/BVH/BVH_model.h" namespace "fcl":
     # Cython only accepts type template parameters.
     # see https://groups.google.com/forum/#!topic/cython-users/xAZxdCFw6Xs
-    cdef cppclass BVHModel "fcl::BVHModel<fcl::OBBRSS>" ( CollisionGeometry ):
+    cdef cppclass BVHModel "fcl::BVHModel<fcl::OBBRSSd>" ( CollisionGeometryd ):
         # Constructing an empty BVH
         BVHModel() except +
         BVHModel(BVHModel& other) except +
         #
         #Geometry point data
-        Vec3f* vertices
+        Vector3d* vertices
         #
         #Geometry triangle index data, will be NULL for point clouds
         Triangle* tri_indices
         #
         #Geometry point data in previous frame
-        Vec3f* prev_vertices
+        Vector3d* prev_vertices
         #
         #Number of triangles
         int num_tris
@@ -383,16 +385,16 @@ cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl":
 
         int beginModel(int num_tris_, int num_vertices_)
 
-        int addVertex(const Vec3f& p)
+        int addVertex(const Vector3d& p)
 
-        int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3)
+        int addTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3)
 
-        #int addSubModel(const std::vector<Vec3f>& ps)
-        # void getCostSources(vector[CostSource]& cost_sources_)
+        #int addSubModel(const std::vector<Vector3d>& ps)
+        # void getCostSources(vector[CostSourced]& cost_sources_)
 
-        #int addSubModel(const vector[Vec3f]& ps)
+        #int addSubModel(const vector[Vector3d]& ps)
         #
-        int addSubModel(const vector[Vec3f]& ps, const vector[Triangle]& ts)
+        int addSubModel(const vector[Vector3d]& ps, const vector[Triangle]& ts)
 
         int endModel()
 
@@ -401,9 +403,9 @@ cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl":
         # void computeLocalAABB()
 
 
-cdef extern from "fcl/octree.h" namespace "fcl":
-    cdef cppclass OcTree(CollisionGeometry):
+cdef extern from "fcl/geometry/octree/octree.h" namespace "fcl":
+    cdef cppclass OcTreed(CollisionGeometryd):
         # Constructing
-        OcTree(FCL_REAL resolution) except +
-        OcTree(shared_ptr[octomap.OcTree]& tree_) except +
+        OcTreed(double resolution) except +
+        OcTreed(shared_ptr[octomap.OcTree]& tree_) except +
 
diff --git a/setup.py b/setup.py
index 4dedd51..c00ce0b 100644
--- a/setup.py
+++ b/setup.py
@@ -20,7 +20,7 @@
         platform_supported = True
         include_dirs = ['/usr/include',
                         '/usr/local/include',
-                        '/usr/include/eigen3']
+                        '/usr/local/include/eigen3']
         lib_dirs = ['/usr/lib',
                     '/usr/local/lib']