From 8c341d0ba3f2a7a45cab6d29191cb328479145b2 Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 16:16:34 +0900 Subject: [PATCH 1/4] Buildable version with reduced functionality: Updated header file locations Disabled functions not present in Eigen objects Disabled distance function Set Eigen scalar template parameter to `double` everywhere --- fcl/__init__.py | 4 +- fcl/fcl.pyx | 370 ++++++++++++++++++------------------- fcl/fcl_defs.pxd | 463 ++++++++++++++++++++++++----------------------- setup.py | 2 +- 4 files changed, 426 insertions(+), 413 deletions(-) diff --git a/fcl/__init__.py b/fcl/__init__.py index e852be0..fae3eb8 100644 --- a/fcl/__init__.py +++ b/fcl/__init__.py @@ -1,5 +1,5 @@ -from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback - +from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, defaultCollisionCallback, defaultDistanceCallback +#distance from .collision_data import OBJECT_TYPE, NODE_TYPE, CCDMotionType, CCDSolverType, GJKSolverType, Contact, CostSource, CollisionRequest, CollisionResult, ContinuousCollisionRequest, ContinuousCollisionResult, DistanceRequest, DistanceResult, CollisionData, DistanceData from .version import __version__ diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index ab28ca8..c867aa7 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -20,36 +20,36 @@ 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() elif len(args) == 1: if isinstance(args[0], Transform): - self.thisptr = new defs.Transform3f(deref(( args[0]).thisptr)) - else: - data = numpy.array(args[0]) - if data.shape == (3,3): - self.thisptr = new defs.Transform3f(numpy_to_mat3f(data)) - elif data.shape == (4,): - self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(data)) - elif data.shape == (3,): - self.thisptr = new defs.Transform3f(numpy_to_vec3f(data)) - else: - raise ValueError('Invalid input to Transform().') + self.thisptr = new defs.Transform3d(deref(( args[0]).thisptr)) +# else: +# data = numpy.array(args[0]) +# if data.shape == (3,3): +# self.thisptr = new defs.Transform3d(numpy_to_mat3d(data)) +# elif data.shape == (4,): +# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(data)) +# elif data.shape == (3,): +# self.thisptr = new defs.Transform3d(numpy_to_vec3d(data)) +# else: +# raise ValueError('Invalid input to Transform().') elif len(args) == 2: rot = numpy.array(args[0]) trans = numpy.array(args[1]).squeeze() if not trans.shape == (3,): raise ValueError('Translation must be (3,).') - if rot.shape == (3,3): - self.thisptr = new defs.Transform3f(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)) - else: - raise ValueError('Invalid input to Transform().') +# if rot.shape == (3,3): +# self.thisptr = new defs.Transform3d(numpy_to_mat3d(rot), numpy_to_vec3d(trans)) +# elif rot.shape == (4,): +# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(rot), numpy_to_vec3d(trans)) +# else: +# raise ValueError('Invalid input to Transform().') else: raise ValueError('Too many arguments to Transform().') @@ -57,30 +57,30 @@ cdef class Transform: if self.thisptr: free(self.thisptr) - def getRotation(self): - return mat3f_to_numpy(self.thisptr.getRotation()) +# def getRotation(self): +# return mat3d_to_numpy(self.thisptr.getRotation()) - def getTranslation(self): - return vec3f_to_numpy(self.thisptr.getTranslation()) +# def getTranslation(self): +# return vec3d_to_numpy(self.thisptr.getTranslation()) - def getQuatRotation(self): - return quaternion3f_to_numpy(self.thisptr.getQuatRotation()) +# def getQuatRotation(self): +# return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) - def setRotation(self, R): - self.thisptr.setRotation(numpy_to_mat3f(R)) +# def setRotation(self, R): +# self.thisptr.setRotation(numpy_to_mat3d(R)) - def setTranslation(self, T): - self.thisptr.setTranslation(numpy_to_vec3f(T)) +# def setTranslation(self, T): +# self.thisptr.setTranslation(numpy_to_vec3d(T)) - def setQuatRotation(self, q): - self.thisptr.setQuatRotation(numpy_to_quaternion3f(q)) +# def setQuatRotation(self, q): +# self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) ############################################################################### # 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 +92,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( self.geom) # Save the python geometry object for later retrieval else: if not self._no_instance: @@ -112,24 +112,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.computeAABB() +# def setRotation(self, mat): +# self.thisptr.setRotation(numpy_to_mat3d(mat)) +# self.thisptr.computeAABB() - def getQuatRotation(self): - return quaternion3f_to_numpy(self.thisptr.getQuatRotation()) +# def getQuatRotation(self): +# 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 +151,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 +175,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 +188,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_vec3d(a), numpy_to_vec3d(b), numpy_to_vec3d(c)) property a: def __get__(self): - return vec3f_to_numpy(( self.thisptr).a) + return vec3d_to_numpy(( self.thisptr).a) def __set__(self, value): - ( self.thisptr).a[0] = value[0] - ( self.thisptr).a[1] = value[1] - ( self.thisptr).a[2] = value[2] + ( self.thisptr).a[0] = value[0] + ( self.thisptr).a[1] = value[1] + ( self.thisptr).a[2] = value[2] property b: def __get__(self): - return vec3f_to_numpy(( self.thisptr).b) + return vec3d_to_numpy(( self.thisptr).b) def __set__(self, value): - ( self.thisptr).b[0] = value[0] - ( self.thisptr).b[1] = value[1] - ( self.thisptr).b[2] = value[2] + ( self.thisptr).b[0] = value[0] + ( self.thisptr).b[1] = value[1] + ( self.thisptr).b[2] = value[2] property c: def __get__(self): - return vec3f_to_numpy(( self.thisptr).c) + return vec3d_to_numpy(( self.thisptr).c) def __set__(self, value): - ( self.thisptr).c[0] = value[0] - ( self.thisptr).c[1] = value[1] - ( self.thisptr).c[2] = value[2] + ( self.thisptr).c[0] = value[0] + ( self.thisptr).c[1] = value[1] + ( self.thisptr).c[2] = 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(( self.thisptr).side) + return vec3d_to_numpy(( self.thisptr).side) def __set__(self, value): - ( self.thisptr).side[0] = value[0] - ( self.thisptr).side[1] = value[1] - ( self.thisptr).side[2] = value[2] + ( self.thisptr).side[0] = value[0] + ( self.thisptr).side[1] = value[1] + ( self.thisptr).side[2] = 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 ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value cdef class Ellipsoid(CollisionGeometry): def __cinit__(self, a, b, c): - self.thisptr = new defs.Ellipsoid( a, b, c) + self.thisptr = new defs.Ellipsoidd( a, b, c) property radii: def __get__(self): - return vec3f_to_numpy(( self.thisptr).radii) + return vec3d_to_numpy(( self.thisptr).radii) def __set__(self, values): - ( self.thisptr).radii = numpy_to_vec3f(values) + ( 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 ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = 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 ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = 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 ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = value cdef class Halfspace(CollisionGeometry): def __cinit__(self, n, d): - self.thisptr = new defs.Halfspace(defs.Vec3f( n[0], + self.thisptr = new defs.Halfspaced(defs.Vector3d( n[0], n[1], n[2]), d) property n: def __get__(self): - return vec3f_to_numpy(( self.thisptr).n) + return vec3d_to_numpy(( self.thisptr).n) def __set__(self, value): - ( self.thisptr).n[0] = value[0] - ( self.thisptr).n[1] = value[1] - ( self.thisptr).n[2] = value[2] + ( self.thisptr).n[0] = value[0] + ( self.thisptr).n[1] = value[1] + ( self.thisptr).n[2] = value[2] property d: def __get__(self): - return ( self.thisptr).d + return ( self.thisptr).d def __set__(self, value): - ( self.thisptr).d = value + ( self.thisptr).d = value cdef class Plane(CollisionGeometry): def __cinit__(self, n, d): - self.thisptr = new defs.Plane(defs.Vec3f( n[0], + self.thisptr = new defs.Planed(defs.Vector3d( n[0], n[1], n[2]), d) property n: def __get__(self): - return vec3f_to_numpy(( self.thisptr).n) + return vec3d_to_numpy(( self.thisptr).n) def __set__(self, value): - ( self.thisptr).n[0] = value[0] - ( self.thisptr).n[1] = value[1] - ( self.thisptr).n[2] = value[2] + ( self.thisptr).n[0] = value[0] + ( self.thisptr).n[1] = value[1] + ( self.thisptr).n[2] = value[2] property d: def __get__(self): - return ( self.thisptr).d + return ( self.thisptr).d def __set__(self, value): - ( self.thisptr).d = value + ( self.thisptr).d = value cdef class BVHModel(CollisionGeometry): def __cinit__(self): @@ -355,20 +355,20 @@ cdef class BVHModel(CollisionGeometry): return n def addVertex(self, x, y, z): - n = ( self.thisptr).addVertex(defs.Vec3f( x, y, z)) + n = ( self.thisptr).addVertex(defs.Vector3d( x, y, z)) return self._check_ret_value(n) def addTriangle(self, v1, v2, v3): - n = ( self.thisptr).addTriangle(numpy_to_vec3f(v1), - numpy_to_vec3f(v2), - numpy_to_vec3f(v3)) + n = ( 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( tri[0], tri[1], tri[2])) n = ( self.thisptr).addSubModel(ps, tris) @@ -406,7 +406,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 +414,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 +426,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(( obj).thisptr) @@ -445,7 +445,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(( a).thisptr) @@ -471,18 +471,18 @@ cdef class DynamicAABBTreeCollisionManager: else: raise ValueError - def distance(self, *args): - if len(args) == 2 and inspect.isroutine(args[1]): - fn = DistanceFunction(args[1], args[0]) - self.thisptr.distance( fn, DistanceCallBack) - elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): - fn = DistanceFunction(args[2], args[1]) - self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) - elif len(args) == 3 and inspect.isroutine(args[2]): - fn = DistanceFunction(args[2], args[1]) - self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) - else: - raise ValueError +# def distance(self, *args): +# if len(args) == 2 and inspect.isroutine(args[1]): +# fn = DistanceFunction(args[1], args[0]) +# self.thisptr.distance( fn, DistanceCallBack) +# elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): +# fn = DistanceFunction(args[2], args[1]) +# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) +# elif len(args) == 3 and inspect.isroutine(args[2]): +# fn = DistanceFunction(args[2], args[1]) +# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) +# else: +# raise ValueError def clear(self): self.thisptr.clear() @@ -547,10 +547,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( request.num_max_contacts, request.enable_contact, request.num_max_cost_sources, @@ -562,12 +562,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 +583,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( request.num_max_iterations, - request.toc_err, + request.toc_err, request.ccd_motion_type, request.gjk_solver_type, request.ccd_solver_type, @@ -601,31 +601,31 @@ def continuousCollide(CollisionObject o1, Transform tf1_end, result.time_of_contact = min(cresult.time_of_contact, result.time_of_contact) return ret -def distance(CollisionObject o1, CollisionObject o2, - request = None, result=None): - - if request is None: - request = DistanceRequest() - if result is None: - result = DistanceResult() - - cdef defs.DistanceResult cresult - - cdef double dis = defs.distance(o1.thisptr, o2.thisptr, - defs.DistanceRequest( - request.enable_nearest_points, - 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.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 - result.b2 = cresult.b2 - return dis +#def distance(CollisionObject o1, CollisionObject o2, +# request = None, result=None): +# +# if request is None: +# request = DistanceRequest() +# if result is None: +# result = DistanceResult() +# +# cdef defs.DistanceResultd cresult +# +# cdef double dis = defs.distance(o1.thisptr, o2.thisptr, +# defs.DistanceRequestd( +# request.enable_nearest_points, +# request.gjk_solver_type +# ), +# cresult) +# +# result.min_distance = min(cresult.min_distance, result.min_distance) +# 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 +# result.b2 = cresult.b2 +# return dis ############################################################################### # Collision and Distance Callback Functions @@ -651,9 +651,9 @@ def defaultDistanceCallback(CollisionObject o1, CollisionObject o2, cdata): if cdata.done: return True, result.min_distance - - distance(o1, o2, request, result) - +# +# distance(o1, o2, request, result) +# dist = result.min_distance if dist <= 0: @@ -670,7 +670,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 +686,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] = py_r[1] + (&dist)[0] = py_r[1] return 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 ( 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 ( cdata).eval_func(o1, o2, dist) @@ -705,56 +705,56 @@ 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.getW(), q.getX(), q.getY(), q.getZ()]) -cdef defs.Quaternion3f numpy_to_quaternion3f(a): - return defs.Quaternion3f( a[0], a[1], a[2], a[3]) +cdef defs.Quaterniond numpy_to_quaternion3d(a): + return defs.Quaterniond( a[0], a[1], a[2], 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( a[0], a[1], a[2]) +cdef defs.Vector3d numpy_to_vec3d(a): + return defs.Vector3d( a[0], a[1], a[2]) -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( a[0][0], a[0][1], a[0][2], - a[1][0], a[1][1], a[1][2], - a[2][0], a[2][1], a[2][2]) +#cdef defs.Matrix3d numpy_to_mat3d(a): +# return defs.Matrix3d( a[0][0], a[0][1], a[0][2], +# a[1][0], a[1][1], a[1][2], +# a[2][0], a[2][1], a[2][2]) -cdef c_to_python_collision_geometry(defs.const_CollisionGeometry*geom, CollisionObject o1, CollisionObject o2): - cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) - cdef CollisionGeometry o2_py_geom = (( o2.thisptr).getUserData()) - if geom == o1_py_geom.thisptr: +cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): + cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) + cdef CollisionGeometry o2_py_geom = (( o2.thisptr).getUserData()) + if geom == 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 = cobj.getUserData() co = CollisionObject(geom, _no_instance=True) ( co).thisptr = cobj diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd index d5461fd..50dd926 100644 --- a/fcl/fcl_defs.pxd +++ b/fcl/fcl_defs.pxd @@ -18,136 +18,138 @@ 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 x, double y, double z) except + + double& operator[](size_t i) + + cdef cppclass Matrix3d: + Matrix3d() except + +# Matrix3d(double xx, double xy, double xz, +# double yx, double yy, double yz, +# double zx, double zy, double zz) except + + double operator()(size_t i, size_t j) + + cdef cppclass Quaterniond: + Quaterniond() except + + Quaterniond(double a, double b, + double c, double d) except + + void fromRotation(Matrix3d& R) + void fromAxisAngle(Vector3d& axis, double angle) +# double& getW() +# double& getX() +# double& getY() +# double& getZ() + + cdef cppclass Transform3d: + Transform3d() except + +# Transform3d(Matrix3d& R_, Vector3d& T_) +# Transform3d(Quaterniond& q_, Vector3d& T_) +# Transform3d(Matrix3d& R_) +# Transform3d(Quaterniond& q_) +# Transform3d(Vector3d& T_) + Transform3d(Transform3d& tf_) +# Matrix3d& getRotation() +# Vector3d& getTranslation() +# Quaterniond& getQuatRotation() +# void setRotation(Matrix3d& R_) +# void setTranslation(Vector3d& T_) +# void setQuatRotation(Quaterniond & q_) + +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 +157,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 +195,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 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 +291,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 +338,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 +346,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" ( CollisionGeometry ): + cdef cppclass BVHModel "fcl::BVHModel" ( 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 +396,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& ps) - # void getCostSources(vector[CostSource]& cost_sources_) + #int addSubModel(const std::vector& 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 +414,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'] From 4a57234a00473d95fea133208f83074a899e3f36 Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 19:22:40 +0900 Subject: [PATCH 2/4] Fixed Transform extension type: Vector3, Matrix3, Quaternion and Transform are now native Eigen objects in FCL, so helper functions were changed accordingly. --- fcl/fcl.pyx | 134 +++++++++++++++++++++++++++++------------------ fcl/fcl_defs.pxd | 35 +++++-------- 2 files changed, 96 insertions(+), 73 deletions(-) diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index c867aa7..a2d0a3f 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -16,6 +16,21 @@ 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 ############################################################################### @@ -25,31 +40,51 @@ cdef class Transform: def __cinit__(self, *args): if len(args) == 0: self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() elif len(args) == 1: if isinstance(args[0], Transform): self.thisptr = new defs.Transform3d(deref(( args[0]).thisptr)) -# else: -# data = numpy.array(args[0]) -# if data.shape == (3,3): -# self.thisptr = new defs.Transform3d(numpy_to_mat3d(data)) -# elif data.shape == (4,): -# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(data)) -# elif data.shape == (3,): -# self.thisptr = new defs.Transform3d(numpy_to_vec3d(data)) -# else: -# raise ValueError('Invalid input to Transform().') + else: + data = numpy.array(args[0]) + if data.shape == (3,3): + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(data)) + elif data.shape == (4,): + 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.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(data)) + else: + raise ValueError('Invalid input to Transform().') elif len(args) == 2: rot = numpy.array(args[0]) trans = numpy.array(args[1]).squeeze() if not trans.shape == (3,): raise ValueError('Translation must be (3,).') -# if rot.shape == (3,3): -# self.thisptr = new defs.Transform3d(numpy_to_mat3d(rot), numpy_to_vec3d(trans)) -# elif rot.shape == (4,): -# self.thisptr = new defs.Transform3d(numpy_to_quaternion3d(rot), numpy_to_vec3d(trans)) -# else: -# raise ValueError('Invalid input to Transform().') + if rot.shape == (3,3): + 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.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: raise ValueError('Too many arguments to Transform().') @@ -57,23 +92,27 @@ cdef class Transform: if self.thisptr: free(self.thisptr) -# def getRotation(self): -# return mat3d_to_numpy(self.thisptr.getRotation()) + def getRotation(self): + return mat3d_to_numpy(self.thisptr.linear()) -# def getTranslation(self): -# return vec3d_to_numpy(self.thisptr.getTranslation()) + def getTranslation(self): + return vec3d_to_numpy(self.thisptr.translation()) -# def getQuatRotation(self): -# return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) + def getQuatRotation(self): + cdef defs.Quaterniond quaternion = defs.Quaterniond(self.thisptr.linear()) + return quaternion3d_to_numpy(quaternion) -# def setRotation(self, R): -# self.thisptr.setRotation(numpy_to_mat3d(R)) + def setRotation(self, R): + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(R)) -# def setTranslation(self, T): -# self.thisptr.setTranslation(numpy_to_vec3d(T)) + def setTranslation(self, T): + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(T)) -# def setQuatRotation(self, q): -# self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) + def setQuatRotation(self, q): + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(q).toRotationMatrix()) ############################################################################### # Collision objects and geometries @@ -121,12 +160,12 @@ cdef class CollisionObject: def getRotation(self): return mat3d_to_numpy(self.thisptr.getRotation()) -# def setRotation(self, mat): -# self.thisptr.setRotation(numpy_to_mat3d(mat)) -# self.thisptr.computeAABB() + def setRotation(self, mat): + self.thisptr.setRotation(numpy_to_mat3d(mat)) + self.thisptr.computeAABB() -# def getQuatRotation(self): -# return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) + def getQuatRotation(self): + return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) def setQuatRotation(self, q): self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) @@ -295,10 +334,8 @@ cdef class Cylinder(CollisionGeometry): ( self.thisptr).lz = value cdef class Halfspace(CollisionGeometry): - def __cinit__(self, n, d): - self.thisptr = new defs.Halfspaced(defs.Vector3d( n[0], - n[1], - n[2]), + def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + self.thisptr = new defs.Halfspaced(defs.Vector3d(&n[0]), d) property n: @@ -316,10 +353,8 @@ cdef class Halfspace(CollisionGeometry): ( self.thisptr).d = value cdef class Plane(CollisionGeometry): - def __cinit__(self, n, d): - self.thisptr = new defs.Planed(defs.Vector3d( n[0], - n[1], - n[2]), + def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + self.thisptr = new defs.Planed(defs.Vector3d(&n[0]), d) property n: @@ -355,7 +390,8 @@ cdef class BVHModel(CollisionGeometry): return n def addVertex(self, x, y, z): - n = ( self.thisptr).addVertex(defs.Vector3d( x, y, z)) + cdef np.ndarray[double, ndim=1, mode="c"] n = numpy.array([x, y, z]) + n = ( self.thisptr).addVertex(defs.Vector3d(&n[0])) return self._check_ret_value(n) def addTriangle(self, v1, v2, v3): @@ -705,8 +741,8 @@ cdef inline bool DistanceCallBack(defs.CollisionObjectd*o1, defs.CollisionObject # Helper Functions ############################################################################### -#cdef quaternion3d_to_numpy(defs.Quaterniond 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.Quaterniond numpy_to_quaternion3d(a): return defs.Quaterniond( a[0], a[1], a[2], a[3]) @@ -714,18 +750,16 @@ cdef defs.Quaterniond numpy_to_quaternion3d(a): cdef vec3d_to_numpy(defs.Vector3d vec): return numpy.array([vec[0], vec[1], vec[2]]) -cdef defs.Vector3d numpy_to_vec3d(a): - return defs.Vector3d( a[0], a[1], a[2]) +cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1, mode="c"] a): + return defs.Vector3d(&a[0]) 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.Matrix3d numpy_to_mat3d(a): -# return defs.Matrix3d( a[0][0], a[0][1], a[0][2], -# a[1][0], a[1][1], a[1][2], -# a[2][0], a[2][1], a[2][2]) +cdef defs.Matrix3d numpy_to_mat3d(np.ndarray[double, ndim=2, mode="c"] a): + return defs.Matrix3d(&a[0, 0]) cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd index 50dd926..f392565 100644 --- a/fcl/fcl_defs.pxd +++ b/fcl/fcl_defs.pxd @@ -21,41 +21,30 @@ cdef extern from "Python.h": cdef extern from "fcl/common/types.h" namespace "fcl": cdef cppclass Vector3d: Vector3d() except + - Vector3d(double x, double y, double z) except + + Vector3d(double *data) except + double& operator[](size_t i) cdef cppclass Matrix3d: Matrix3d() except + -# Matrix3d(double xx, double xy, double xz, -# double yx, double yy, double yz, -# double zx, double zy, double zz) 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 + - void fromRotation(Matrix3d& R) - void fromAxisAngle(Vector3d& axis, double angle) -# double& getW() -# double& getX() -# double& getY() -# double& getZ() + 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(Matrix3d& R_, Vector3d& T_) -# Transform3d(Quaterniond& q_, Vector3d& T_) -# Transform3d(Matrix3d& R_) -# Transform3d(Quaterniond& q_) -# Transform3d(Vector3d& T_) Transform3d(Transform3d& tf_) -# Matrix3d& getRotation() -# Vector3d& getTranslation() -# Quaterniond& getQuatRotation() -# void setRotation(Matrix3d& R_) -# void setTranslation(Vector3d& T_) -# void setQuatRotation(Quaterniond & q_) + void setIdentity() + Matrix3d& linear() + Vector3d& translation() cdef extern from "fcl/narrowphase/continuous_collision_request.h" namespace "fcl": cdef enum CCDMotionType: From e435d10238bfdaa08276b3bc39d4a8e574640d3f Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 19:38:01 +0900 Subject: [PATCH 3/4] Returned all previous functionallity: distance function and its dependants. Now the old test/* is passing. --- fcl/__init__.py | 4 +-- fcl/fcl.pyx | 80 ++++++++++++++++++++++++------------------------ fcl/fcl_defs.pxd | 6 ++-- 3 files changed, 45 insertions(+), 45 deletions(-) diff --git a/fcl/__init__.py b/fcl/__init__.py index fae3eb8..e852be0 100644 --- a/fcl/__init__.py +++ b/fcl/__init__.py @@ -1,5 +1,5 @@ -from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, defaultCollisionCallback, defaultDistanceCallback -#distance +from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback + from .collision_data import OBJECT_TYPE, NODE_TYPE, CCDMotionType, CCDSolverType, GJKSolverType, Contact, CostSource, CollisionRequest, CollisionResult, ContinuousCollisionRequest, ContinuousCollisionResult, DistanceRequest, DistanceResult, CollisionData, DistanceData from .version import __version__ diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index a2d0a3f..b2fb29c 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -507,18 +507,18 @@ cdef class DynamicAABBTreeCollisionManager: else: raise ValueError -# def distance(self, *args): -# if len(args) == 2 and inspect.isroutine(args[1]): -# fn = DistanceFunction(args[1], args[0]) -# self.thisptr.distance( fn, DistanceCallBack) -# elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): -# fn = DistanceFunction(args[2], args[1]) -# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) -# elif len(args) == 3 and inspect.isroutine(args[2]): -# fn = DistanceFunction(args[2], args[1]) -# self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) -# else: -# raise ValueError + def distance(self, *args): + if len(args) == 2 and inspect.isroutine(args[1]): + fn = DistanceFunction(args[1], args[0]) + self.thisptr.distance( fn, DistanceCallBack) + elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): + fn = DistanceFunction(args[2], args[1]) + self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) + elif len(args) == 3 and inspect.isroutine(args[2]): + fn = DistanceFunction(args[2], args[1]) + self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) + else: + raise ValueError def clear(self): self.thisptr.clear() @@ -637,31 +637,31 @@ def continuousCollide(CollisionObject o1, Transform tf1_end, result.time_of_contact = min(cresult.time_of_contact, result.time_of_contact) return ret -#def distance(CollisionObject o1, CollisionObject o2, -# request = None, result=None): -# -# if request is None: -# request = DistanceRequest() -# if result is None: -# result = DistanceResult() -# -# cdef defs.DistanceResultd cresult -# -# cdef double dis = defs.distance(o1.thisptr, o2.thisptr, -# defs.DistanceRequestd( -# request.enable_nearest_points, -# request.gjk_solver_type -# ), -# cresult) -# -# result.min_distance = min(cresult.min_distance, result.min_distance) -# 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 -# result.b2 = cresult.b2 -# return dis +def distance(CollisionObject o1, CollisionObject o2, + request = None, result=None): + + if request is None: + request = DistanceRequest() + if result is None: + result = DistanceResult() + + cdef defs.DistanceResultd cresult + + cdef double dis = defs.distance(o1.thisptr, o2.thisptr, + defs.DistanceRequestd( + request.enable_nearest_points, + request.gjk_solver_type + ), + cresult) + + result.min_distance = min(cresult.min_distance, result.min_distance) + 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 + result.b2 = cresult.b2 + return dis ############################################################################### # Collision and Distance Callback Functions @@ -687,9 +687,9 @@ def defaultDistanceCallback(CollisionObject o1, CollisionObject o2, cdata): if cdata.done: return True, result.min_distance -# -# distance(o1, o2, request, result) -# + + distance(o1, o2, request, result) + dist = result.min_distance if dist <= 0: diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd index f392565..649f2f0 100644 --- a/fcl/fcl_defs.pxd +++ b/fcl/fcl_defs.pxd @@ -260,11 +260,11 @@ cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl" 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 distance(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, DistanceCallBack callback) void collide(CollisionObjectd* obj, void* cdata, CollisionCallBack callback) -# void distance(CollisionObjectd* obj, void* cdata, DistanceCallBack callback) + void distance(CollisionObjectd* obj, void* cdata, DistanceCallBack callback) void collide(void* cdata, CollisionCallBack callback) -# void distance(void* cdata, DistanceCallBack callback) + void distance(void* cdata, DistanceCallBack callback) void setup() void update() void update(CollisionObjectd* updated_obj) From 71239b7ddb7de10734f19e590d8099b8e40f958f Mon Sep 17 00:00:00 2001 From: Dmitry Neverov Date: Mon, 27 Apr 2020 20:51:19 +0900 Subject: [PATCH 4/4] Fixed ordering mismatch bug --- fcl/fcl.pyx | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx index b2fb29c..f15e777 100644 --- a/fcl/fcl.pyx +++ b/fcl/fcl.pyx @@ -334,7 +334,7 @@ cdef class Cylinder(CollisionGeometry): ( self.thisptr).lz = value cdef class Halfspace(CollisionGeometry): - def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + def __cinit__(self, np.ndarray[double, ndim=1] n, d): self.thisptr = new defs.Halfspaced(defs.Vector3d(&n[0]), d) @@ -353,7 +353,7 @@ cdef class Halfspace(CollisionGeometry): ( self.thisptr).d = value cdef class Plane(CollisionGeometry): - def __cinit__(self, np.ndarray[double, ndim=1, mode="c"] n, d): + def __cinit__(self, np.ndarray[double, ndim=1] n, d): self.thisptr = new defs.Planed(defs.Vector3d(&n[0]), d) @@ -390,7 +390,7 @@ cdef class BVHModel(CollisionGeometry): return n def addVertex(self, x, y, z): - cdef np.ndarray[double, ndim=1, mode="c"] n = numpy.array([x, y, z]) + cdef np.ndarray[double, ndim=1] n = numpy.array([x, y, z]) n = ( self.thisptr).addVertex(defs.Vector3d(&n[0])) return self._check_ret_value(n) @@ -750,7 +750,7 @@ cdef defs.Quaterniond numpy_to_quaternion3d(a): cdef vec3d_to_numpy(defs.Vector3d vec): return numpy.array([vec[0], vec[1], vec[2]]) -cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1, mode="c"] a): +cdef defs.Vector3d numpy_to_vec3d(np.ndarray[double, ndim=1] a): return defs.Vector3d(&a[0]) cdef mat3d_to_numpy(defs.Matrix3d m): @@ -758,8 +758,11 @@ cdef mat3d_to_numpy(defs.Matrix3d m): [m(1,0), m(1,1), m(1,2)], [m(2,0), m(2,1), m(2,2)]]) -cdef defs.Matrix3d numpy_to_mat3d(np.ndarray[double, ndim=2, mode="c"] a): - return defs.Matrix3d(&a[0, 0]) +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_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData())