From b2a56230ebf72292d63f61961543e224fdcfbb98 Mon Sep 17 00:00:00 2001
From: Jeff Ichnowski <jeffi@berkeley.edu>
Date: Mon, 23 Mar 2020 23:30:05 -0400
Subject: [PATCH] Updating to support FCL 0.6.x

---
 fcl/fcl.pyx         | 255 ++++++++++++++--------------
 fcl/fcl_defs.pxd    | 403 +++++++++++++++++++++-----------------------
 fcl/fcl_helpers.hpp |  51 ++++++
 setup.py            |   3 +-
 4 files changed, 375 insertions(+), 337 deletions(-)
 create mode 100644 fcl/fcl_helpers.hpp

diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx
index ab28ca8..684913e 100644
--- a/fcl/fcl.pyx
+++ b/fcl/fcl.pyx
@@ -20,22 +20,23 @@ from collision_data import Contact, CostSource, CollisionRequest, ContinuousColl
 # 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(defs.mat3_to_transform(numpy_to_mat3f(data)))
                 elif data.shape == (4,):
-                    self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(data))
+                    self.thisptr = new defs.Transform3d(defs.mat3_to_transform(numpy_to_quaternion3f(data).toRotationMatrix()))
                 elif data.shape == (3,):
-                    self.thisptr = new defs.Transform3f(numpy_to_vec3f(data))
+                    self.thisptr = new defs.Transform3d(defs.vec3_to_transform(numpy_to_vec3f(data)))
                 else:
                     raise ValueError('Invalid input to Transform().')
         elif len(args) == 2:
@@ -45,9 +46,9 @@ 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(defs.mat3_vec3_to_transform(numpy_to_mat3f(rot), numpy_to_vec3f(trans)))
             elif rot.shape == (4,):
-                self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(rot), numpy_to_vec3f(trans))
+                self.thisptr = new defs.Transform3d(defs.mat3_vec3_to_transform(numpy_to_quaternion3f(rot).toRotationMatrix(), numpy_to_vec3f(trans)))
             else:
                 raise ValueError('Invalid input to Transform().')
         else:
@@ -58,29 +59,29 @@ cdef class Transform:
             free(self.thisptr)
 
     def getRotation(self):
-        return mat3f_to_numpy(self.thisptr.getRotation())
+        return mat3f_to_numpy(self.thisptr.linear())
 
     def getTranslation(self):
-        return vec3f_to_numpy(self.thisptr.getTranslation())
+        return vec3f_to_numpy(self.thisptr.translation())
 
     def getQuatRotation(self):
-        return quaternion3f_to_numpy(self.thisptr.getQuatRotation())
+        return quaternion3f_to_numpy(defs.Quaterniond(self.thisptr.linear()))
 
     def setRotation(self, R):
-        self.thisptr.setRotation(numpy_to_mat3f(R))
+        defs.set_transform_rotation(self.thisptr, numpy_to_mat3f(R))
 
     def setTranslation(self, T):
-        self.thisptr.setTranslation(numpy_to_vec3f(T))
+        defs.set_transform_translation(self.thisptr, numpy_to_vec3f(T))
 
     def setQuatRotation(self, q):
-        self.thisptr.setQuatRotation(numpy_to_quaternion3f(q))
+        defs.set_transform_rotation(self.thisptr, numpy_to_quaternion3f(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 +93,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:
@@ -151,7 +152,7 @@ cdef class CollisionObject:
         return self.thisptr.isUncertain()
 
 cdef class CollisionGeometry:
-    cdef defs.CollisionGeometry *thisptr
+    cdef defs.CollisionGeometryd *thisptr
 
     def __cinit__(self):
         pass
@@ -188,153 +189,153 @@ 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_vec3f(a), numpy_to_vec3f(b), numpy_to_vec3f(c))
 
     property a:
         def __get__(self):
-            return vec3f_to_numpy((<defs.TriangleP*> self.thisptr).a)
+            return vec3f_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 vec3f_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 vec3f_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 vec3f_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 vec3f_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_vec3f(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],
+        self.thisptr = new defs.Halfspaced(defs.Vector3d(<double?> n[0],
                                                      <double?> n[1],
                                                      <double?> n[2]),
                                           <double?> d)
 
     property n:
         def __get__(self):
-            return vec3f_to_numpy((<defs.Halfspace*> self.thisptr).n)
+            return vec3f_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],
+        self.thisptr = new defs.Planed(defs.Vector3d(<double?> n[0],
                                                  <double?> n[1],
                                                  <double?> n[2]),
                                       <double?> d)
 
     property n:
         def __get__(self):
-            return vec3f_to_numpy((<defs.Plane*> self.thisptr).n)
+            return vec3f_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,7 +356,7 @@ 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))
+        n = (<defs.BVHModel*> self.thisptr).addVertex(defs.Vector3d(<double?> x, <double?> y, <double?> z))
         return self._check_ret_value(n)
 
     def addTriangle(self, v1, v2, v3):
@@ -365,7 +366,7 @@ cdef class BVHModel(CollisionGeometry):
         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))
@@ -396,29 +397,32 @@ cdef class BVHModel(CollisionGeometry):
         else:
             return False
 
-cdef class OcTree(CollisionGeometry):
-    cdef octomap.OcTree* tree
+# TODO: OcTree is an optional component that will only be present when
+# FCL_HAVE_OCTOMAP is 1.
+#
+# cdef class OcTree(CollisionGeometry):
+#     cdef octomap.OcTree* tree
 
-    def __cinit__(self, r, data):
-        cdef std.stringstream ss
-        cdef vector[char] vd = data
-        ss.write(vd.data(), len(data))
+#     def __cinit__(self, r, data):
+#         cdef std.stringstream ss
+#         cdef vector[char] vd = data
+#         ss.write(vd.data(), len(data))
 
-        self.tree = new octomap.OcTree(r) 
-        self.tree.readBinaryData(ss)
-        self.thisptr = new defs.OcTree(defs.shared_ptr[octomap.OcTree](self.tree))
+#         self.tree = new octomap.OcTree(r) 
+#         self.tree.readBinaryData(ss)
+#         self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree))
 
 
 ###############################################################################
 # Collision managers
 ###############################################################################
 
-cdef class DynamicAABBTreeCollisionManager:
-    cdef defs.DynamicAABBTreeCollisionManager *thisptr
+cdef class DynamicAABBTreeCollisionManagerd:
+    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 +430,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 +449,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)
@@ -462,9 +466,9 @@ cdef class DynamicAABBTreeCollisionManager:
         if len(args) == 2 and inspect.isroutine(args[1]):
             fn = CollisionFunction(args[1], args[0])
             self.thisptr.collide(<void*> fn, CollisionCallBack)
-        elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager):
+        elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManagerd):
             fn = CollisionFunction(args[2], args[1])
-            self.thisptr.collide((<DynamicAABBTreeCollisionManager?> args[0]).thisptr, <void*> fn, CollisionCallBack)
+            self.thisptr.collide((<DynamicAABBTreeCollisionManagerd?> args[0]).thisptr, <void*> fn, CollisionCallBack)
         elif len(args) == 3 and inspect.isroutine(args[2]):
             fn = CollisionFunction(args[2], args[1])
             self.thisptr.collide((<CollisionObject?> args[0]).thisptr, <void*> fn, CollisionCallBack)
@@ -475,9 +479,9 @@ cdef class DynamicAABBTreeCollisionManager:
         if len(args) == 2 and inspect.isroutine(args[1]):
             fn = DistanceFunction(args[1], args[0])
             self.thisptr.distance(<void*> fn, DistanceCallBack)
-        elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager):
+        elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManagerd):
             fn = DistanceFunction(args[2], args[1])
-            self.thisptr.distance((<DynamicAABBTreeCollisionManager?> args[0]).thisptr, <void*> fn, DistanceCallBack)
+            self.thisptr.distance((<DynamicAABBTreeCollisionManagerd?> args[0]).thisptr, <void*> fn, DistanceCallBack)
         elif len(args) == 3 and inspect.isroutine(args[2]):
             fn = DistanceFunction(args[2], args[1])
             self.thisptr.distance((<CollisionObject?> args[0]).thisptr, <void*> fn, DistanceCallBack)
@@ -547,10 +551,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 +566,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 +587,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,10 +613,10 @@ 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
                                     ),
@@ -670,7 +674,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 +690,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,37 +709,38 @@ 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 quaternion3f_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_quaternion3f(a):
+    return defs.Quaterniond(<double?> a[0], <double?> a[1], <double?> a[2], <double?> a[3])
 
-cdef vec3f_to_numpy(defs.Vec3f vec):
+cdef vec3f_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_vec3f(a):
+    return defs.Vector3d(<double?> a[0], <double?> a[1], <double?> a[2])
 
-cdef mat3f_to_numpy(defs.Matrix3f m):
+cdef mat3f_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_mat3f(a):
+    return defs.make_matrix_3d(
+        <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 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)
@@ -746,7 +751,7 @@ cdef c_to_python_contact(defs.Contact contact, CollisionObject o1, CollisionObje
     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)
@@ -754,7 +759,7 @@ cdef c_to_python_costsource(defs.CostSource cost_source):
     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..dc2ca72 100644
--- a/fcl/fcl_defs.pxd
+++ b/fcl/fcl_defs.pxd
@@ -18,51 +18,36 @@ 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/data_types.h" namespace "fcl":
+#    ctypedef double FCL_REAL
+
+cdef extern from "fcl/fcl.h" namespace "fcl":
+    cdef cppclass Vector3d:
+        Vector3d() except +
+        Vector3d(double x, double y, double z) except +
+        double& operator[](size_t i)
+
+    cdef cppclass Matrix3d:
+        Matrix3d() except +
+        double operator()(size_t i, size_t j)
+
+    cdef cppclass Quaterniond:
+        Quaterniond() except +
+        Quaterniond(double a, double b,
+                    double c, double d) except +
+        Quaterniond(const Matrix3d& R) except +
+        Matrix3d toRotationMatrix()
+        double& w()
+        double& x()
+        double& y()
+        double& z()
+
+    cdef cppclass Transform3d:
+        Transform3d() except +
+        Transform3d(Transform3d& tf_)
+        void setIdentity()
+        Matrix3d& linear()
+        Vector3d& translation()
 
     cdef enum CCDMotionType:
         CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE
@@ -73,81 +58,80 @@ cdef extern from "fcl/collision_data.h" namespace "fcl":
     cdef enum GJKSolverType:
         GST_LIBCCD, GST_INDEP
 
-    cdef cppclass Contact:
-        CollisionGeometry *o1
-        CollisionGeometry *o2
+    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 cppclass CostSourced:
+        Vector3d aabb_min
+        Vector3d aabb_max
+        double cost_density
+        double total_cost
 
-    cdef cppclass CollisionResult:
-        CollisionResult() except +
+    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 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 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:
+    cdef cppclass ContinuousCollisionRequestd:
          size_t num_max_iterations_,
-         FCL_REAL toc_err_,
+         double toc_err_,
          CCDMotionType ccd_motion_type_,
          GJKSolverType gjk_solver_type_,
          GJKSolverType ccd_solver_type_
 
-         ContinuousCollisionRequest(
+         ContinuousCollisionRequestd(
                              size_t num_max_iterations_,
-                             FCL_REAL toc_err_,
+                             double 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 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 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 enum OBJECT_TYPE:
         OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT
     cdef enum NODE_TYPE:
@@ -155,34 +139,34 @@ 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
+
+    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 +174,79 @@ 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"
+    ctypedef CollisionGeometryd const_CollisionGeometryd "const fcl::CollisionGeometryd"
+    ctypedef CollisionObjectd const_CollisionObjectd "const fcl::CollisionObjectd"
 
-cdef extern from "fcl/shape/geometric_shapes.h" namespace "fcl":
-    cdef cppclass ShapeBase(CollisionGeometry):
+    cdef cppclass ShapeBase(CollisionGeometryd):
         ShapeBase() except +
 
-    cdef cppclass TriangleP(ShapeBase):
-        TriangleP(Vec3f& a_, Vec3f& b_, Vec3f& c_) except +
-        Vec3f a, b, c
+    cdef cppclass TrianglePd(ShapeBase):
+        TrianglePd(Vector3d& a_, Vector3d& b_, Vector3d& c_) except +
+        Vector3d a, b, c
 
-    cdef cppclass Box(ShapeBase):
-        Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) except +
-        Vec3f side
+    cdef cppclass Boxd(ShapeBase):
+        Boxd(double x, double y, double z) except +
+        Vector3d side
 
-    cdef cppclass Sphere(ShapeBase):
-        Sphere(FCL_REAL radius_) except +
-        FCL_REAL radius
+    cdef cppclass Sphered(ShapeBase):
+        Sphered(double radius_) except +
+        double radius
 
-    cdef cppclass Ellipsoid(ShapeBase):
-        Ellipsoid(FCL_REAL a_, FCL_REAL b_, FCL_REAL c_) except +
-        Vec3f radii
+    cdef cppclass Ellipsoidd(ShapeBase):
+        Ellipsoidd(double a_, double b_, double c_) except +
+        Vector3d radii
 
-    cdef cppclass Capsule(ShapeBase):
-        Capsule(FCL_REAL radius_, FCL_REAL lz_) except +
-        FCL_REAL radius
-        FCL_REAL lz
+    cdef cppclass Capsuled(ShapeBase):
+        Capsuled(double radius_, double lz_) except +
+        double radius
+        double lz
 
-    cdef cppclass Cone(ShapeBase):
-        Cone(FCL_REAL radius_, FCL_REAL lz_) except +
-        FCL_REAL radius
-        FCL_REAL lz
+    cdef cppclass Coned(ShapeBase):
+        Coned(double radius_, double lz_) except +
+        double radius
+        double lz
 
-    cdef cppclass Cylinder(ShapeBase):
-        Cylinder(FCL_REAL radius_, FCL_REAL lz_) except +
-        FCL_REAL radius
-        FCL_REAL lz
+    cdef cppclass Cylinderd(ShapeBase):
+        Cylinderd(double radius_, double lz_) except +
+        double radius
+        double lz
 
-    cdef cppclass Convex(ShapeBase):
-        Convex(Vec3f* plane_nomals_,
-               FCL_REAL* plane_dis_,
+    cdef cppclass Convexd(ShapeBase):
+        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 cppclass Plane(ShapeBase):
-        Plane(Vec3f& n_, FCL_REAL d_) except +
-        Vec3f n
-        FCL_REAL 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_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 Halfspaced(ShapeBase):
+        Halfspaced(Vector3d& n_, double d_) except +
+        Vector3d n
+        double d
+
+    cdef cppclass Planed(ShapeBase):
+        Planed(Vector3d& n_, double d_) except +
+        Vector3d n
+        double d
+
+    ctypedef bool (*CollisionCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata)
+    ctypedef bool (*DistanceCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata, double& dist)
+
+    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 +258,36 @@ 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)
+    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)
+    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)
 
+    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/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/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
@@ -324,8 +299,6 @@ cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl":
         BVH_ERR_INCORRECT_DATA = -7,             # BVH data is not valid
         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 +307,32 @@ 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 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":
     cdef cppclass BVSplitterBase:
         pass
 
-cdef extern from "fcl/BVH/BV_fitter.h" namespace "fcl":
     cdef cppclass BVFitterBase:
         pass
 
-cdef extern from "fcl/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 +352,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()
 
@@ -400,10 +369,22 @@ 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 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 +
+
+cdef extern from "fcl_helpers.hpp" namespace "python_fcl_helpers":
+    Matrix3d make_matrix_3d(
+        double xx, double xy, double xz,
+        double yx, double yy, double yz,
+        double zx, double zy, double zz)
+    
+    Transform3d mat3_to_transform(const Matrix3d& r)
+    Transform3d vec3_to_transform(const Vector3d& t)
+    Transform3d quat_to_transform(const Quaterniond& q)
+    Transform3d mat3_vec3_to_transform(const Matrix3d& r, const Vector3d& t)
+    Transform3d quat_vec3_to_transform(const Quaterniond& q, const Vector3d& t)
+    void set_transform_rotation(Transform3d* m, const Matrix3d& r)
+    void set_transform_translation(Transform3d* m, const Vector3d& t)
+    
diff --git a/fcl/fcl_helpers.hpp b/fcl/fcl_helpers.hpp
new file mode 100644
index 0000000..c14a48e
--- /dev/null
+++ b/fcl/fcl_helpers.hpp
@@ -0,0 +1,51 @@
+#pragma once
+#ifndef PYTHON_FCL_HELPERS_HPP
+#define PYTHON_FCL_HELPERS_HPP
+
+#include <fcl/fcl.h>
+
+namespace python_fcl_helpers {
+    inline fcl::Matrix3d make_matrix_3d(
+        double xx, double xy, double xz,
+        double yx, double yy, double yz,
+        double zx, double zy, double zz)
+    {
+        fcl::Matrix3d m;
+        m << xx, xy, xz,
+             yx, yy, yz,
+             zx, zy, zz;
+        return m;
+    }
+
+    inline fcl::Transform3d mat3_to_transform(const fcl::Matrix3d& r) {
+        fcl::Transform3d m;
+        m.setIdentity();
+        m.linear() = r;
+        return m;
+    }
+    
+    inline fcl::Transform3d vec3_to_transform(const fcl::Vector3d& t) {
+        fcl::Transform3d m;
+        m.setIdentity();
+        m.translation() = t;
+        return m;
+    }
+    
+    inline fcl::Transform3d mat3_vec3_to_transform(const fcl::Matrix3d& r, const fcl::Vector3d& t) {
+        fcl::Transform3d m;
+        m.setIdentity();
+        m.linear() = r;
+        m.translation() = t;
+        return m;
+    }
+
+    inline void set_transform_rotation(fcl::Transform3d* m, const fcl::Matrix3d& r) {
+        m->linear() = r;
+    }
+
+    inline void set_transform_translation(fcl::Transform3d* m, const fcl::Vector3d& t) {
+        m->translation() = t;
+    }
+}
+
+#endif // PYTHON_FCL_HELPERS_HPP
diff --git a/setup.py b/setup.py
index 4dedd51..2aa5296 100644
--- a/setup.py
+++ b/setup.py
@@ -20,7 +20,8 @@
         platform_supported = True
         include_dirs = ['/usr/include',
                         '/usr/local/include',
-                        '/usr/include/eigen3']
+                        '/usr/include/eigen3',
+                        '/opt/local/include/eigen3']
         lib_dirs = ['/usr/lib',
                     '/usr/local/lib']