Verified Commit e3add2cd authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: add first version of user-type registration

parent 66e39351
......@@ -120,6 +120,7 @@ SET(${PROJECT_NAME}_HEADERS
include/eigenpy/registration.hpp
include/eigenpy/angle-axis.hpp
include/eigenpy/quaternion.hpp
include/eigenpy/user-type.hpp
include/eigenpy/stride.hpp
include/eigenpy/version.hpp
)
......@@ -150,6 +151,7 @@ SET(${PROJECT_NAME}_SOURCES
src/matrix-float.cpp
src/matrix-complex-float.cpp
src/matrix-complex-double.cpp
src/user-type.cpp
src/matrix-double.cpp
src/matrix-long-double.cpp
src/matrix-complex-long-double.cpp
......
//
// Copyright (c) 2020 INRIA
//
#ifndef __eigenpy_user_type_hpp__
#define __eigenpy_user_type_hpp__
#include "eigenpy/fwd.hpp"
#include "eigenpy/numpy-type.hpp"
#include "eigenpy/exception.hpp"
#include <algorithm>
#include <map>
#include <typeinfo>
#include <string>
#include <iostream>
namespace eigenpy
{
namespace internal
{
template<typename T, int type_code = NumpyEquivalentType<T>::type_code>
struct SpecialMethods
{
static void copyswap(void * /*dst*/, void * /*src*/, int /*swap*/, void * /*arr*/) {};
static PyObject * getitem(void * /*ip*/, void * /*ap*/) { return NULL; };
static int setitem(PyObject * /*op*/, void * /*ov*/, void * /*ap*/) { return -1; }
static void copyswapn(void * /*dest*/, long /*dstride*/, void * /*src*/,
long /*sstride*/, long /*n*/, int /*swap*/, void * /*arr*/) {};
static npy_bool nonzero(void * /*ip*/, void * /*array*/) { return (npy_bool)false; };
// static void cast(void * /*from*/, void * /*to*/, npy_intp /*n*/, void * /*fromarr*/, void * /*toarr*/) {};
};
template<typename T>
struct SpecialMethods<T,NPY_USERDEF>
{
static void copyswap(void * dst, void * src, int swap, void * /*arr*/)
{
// std::cout << "copyswap" << std::endl;
if (src != NULL)
{
T & t1 = *static_cast<T*>(dst);
T & t2 = *static_cast<T*>(src);
t1 = t2;
}
if(swap)
{
T & t1 = *static_cast<T*>(dst);
T & t2 = *static_cast<T*>(src);
std::swap(t1,t2);
}
}
static PyObject * getitem(void * ip, void * ap)
{
// std::cout << "getitem" << std::endl;
PyArrayObject * py_array = static_cast<PyArrayObject *>(ap);
if((py_array==NULL) || PyArray_ISBEHAVED_RO(py_array))
{
T * elt_ptr = static_cast<T*>(ip);
bp::object m(boost::ref(*elt_ptr));
Py_INCREF(m.ptr());
return m.ptr();
}
else
{
T * elt_ptr = static_cast<T*>(ip);
bp::object m(boost::ref(*elt_ptr));
Py_INCREF(m.ptr());
return m.ptr();
}
}
static int setitem(PyObject * src_obj, void * dest_ptr, void * array)
{
// std::cout << "setitem" << std::endl;
if(array == NULL)
{
eigenpy::Exception("Cannot retrieve the type stored in the array.");
return -1;
}
PyArrayObject * py_array = static_cast<PyArrayObject *>(array);
PyArray_Descr * descr = PyArray_DTYPE(py_array);
PyTypeObject * array_scalar_type = descr->typeobj;
PyTypeObject * src_obj_type = Py_TYPE(src_obj);
if(array_scalar_type != src_obj_type)
{
return -1;
}
bp::extract<T&> extract_src_obj(src_obj);
if(!extract_src_obj.check())
{
std::stringstream ss;
ss << "The input type is of wrong type. ";
ss << "The expected type is " << bp::type_info(typeid(T)).name() << std::endl;
eigenpy::Exception(ss.str());
return -1;
}
const T & src = extract_src_obj();
T & dest = *static_cast<T*>(dest_ptr);
dest = src;
return 0;
}
static void copyswapn(void * dst, long dstride, void * src, long sstride,
long n, int swap, void * array)
{
// std::cout << "copyswapn" << std::endl;
char *dstptr = static_cast<char*>(dst);
char *srcptr = static_cast<char*>(src);
PyArrayObject * py_array = static_cast<PyArrayObject *>(array);
PyArray_CopySwapFunc * copyswap = PyArray_DESCR(py_array)->f->copyswap;
for (npy_intp i = 0; i < n; i++)
{
copyswap(dstptr, srcptr, swap, array);
dstptr += dstride;
srcptr += sstride;
}
}
static npy_bool nonzero(void * ip, void * array)
{
// std::cout << "nonzero" << std::endl;
static const T ZeroValue = T(0);
PyArrayObject * py_array = static_cast<PyArrayObject *>(array);
if(py_array == NULL || PyArray_ISBEHAVED_RO(py_array))
{
const T & value = *static_cast<T*>(ip);
std::cout << "value != ZeroValue: " << (bool)(value != ZeroValue) << std::endl;
std::cout << "value != ZeroValue: " << (npy_bool)(value != ZeroValue) << std::endl;
return (npy_bool)(value != ZeroValue);
}
else
{
T tmp_value;
PyArray_DESCR(py_array)->f->copyswap(&tmp_value, ip, PyArray_ISBYTESWAPPED(py_array),
array);
return (npy_bool)(tmp_value != ZeroValue);
}
}
// static void cast(void * from, void * to, npy_intp n, void * fromarr, void * toarr)
// {
// }
};
#define EIGENPY_REGISTER_BINARY_OPERATOR(name,op) \
template<typename T1, typename T2, typename R> \
void binary_op_##name(char** args, npy_intp * dimensions, npy_intp * steps, void * /*data*/) \
{ \
std::cout << "binary_op_##name" << std::endl; \
npy_intp is0 = steps[0], is1 = steps[1], \
os = steps[2], n = *dimensions; \
char * i0 = args[0], *i1 = args[1], *o = args[2]; \
int k; \
for (k = 0; k < n; k++) \
{ \
T1 & x = *static_cast<T1*>(static_cast<void*>(i0)); \
T2 & y = *static_cast<T2*>(static_cast<void*>(i1)); \
R & res = *static_cast<R*>(static_cast<void*>(o)); \
res = x op y; \
i0 += is0; i1 += is1; o += os; \
} \
} \
\
template<typename T> \
void binary_op_##name(char** args, npy_intp * dimensions, npy_intp * steps, void * data) \
{ \
binary_op_##name<T,T,T>(args,dimensions,steps,data); \
}
EIGENPY_REGISTER_BINARY_OPERATOR(add,+)
EIGENPY_REGISTER_BINARY_OPERATOR(subtract,-)
EIGENPY_REGISTER_BINARY_OPERATOR(multiply,*)
EIGENPY_REGISTER_BINARY_OPERATOR(divide,/)
EIGENPY_REGISTER_BINARY_OPERATOR(equal,==)
EIGENPY_REGISTER_BINARY_OPERATOR(not_equal,!=)
EIGENPY_REGISTER_BINARY_OPERATOR(less,<)
EIGENPY_REGISTER_BINARY_OPERATOR(greater,>)
EIGENPY_REGISTER_BINARY_OPERATOR(less_equal,<=)
EIGENPY_REGISTER_BINARY_OPERATOR(greater_equal,>=)
} // namespace internal
/// \brief Structure collecting all the types registers in Numpy via EigenPy
struct EIGENPY_DLLEXPORT Register
{
static PyArray_Descr * getPyArrayDescr(PyTypeObject * py_type_ptr)
{
MapDescr::iterator it = py_array_descr_bindings.find(py_type_ptr);
if(it != py_array_descr_bindings.end())
return it->second;
else
return NULL;
}
template<typename Scalar>
static bool isRegistered()
{
return isRegistered(Register::getPyType<Scalar>());
}
static bool isRegistered(PyTypeObject * py_type_ptr)
{
if(getPyArrayDescr(py_type_ptr) != NULL)
return true;
else
return false;
}
static int getTypeCode(PyTypeObject * py_type_ptr)
{
MapCode::iterator it = py_array_code_bindings.find(py_type_ptr);
if(it != py_array_code_bindings.end())
return it->second;
else
return PyArray_TypeNum(py_type_ptr);
}
template<typename Scalar>
static PyTypeObject * getPyType()
{
if(!isNumpyNativeType<Scalar>())
{
const PyTypeObject * const_py_type_ptr = bp::converter::registered_pytype<Scalar>::get_pytype();
if(const_py_type_ptr == NULL)
{
std::stringstream ss;
ss << "The type " << typeid(Scalar).name() << " does not have a registered converter inside Boot.Python." << std::endl;
throw std::invalid_argument(ss.str());
}
PyTypeObject * py_type_ptr = const_cast<PyTypeObject *>(const_py_type_ptr);
return py_type_ptr;
}
else
{
PyArray_Descr * new_descr = PyArray_DescrFromType(NumpyEquivalentType<Scalar>::type_code);
return new_descr->typeobj;
}
}
template<typename Scalar>
static int getTypeCode()
{
if(isNumpyNativeType<Scalar>())
return NumpyEquivalentType<Scalar>::type_code;
else
{
const std::type_info & info = typeid(Scalar);
if(type_to_py_type_bindings.find(&info) != type_to_py_type_bindings.end())
{
PyTypeObject * py_type = type_to_py_type_bindings[&info];
int code = py_array_code_bindings[py_type];
std::cout << "type name: " << py_type->tp_name << std::endl;
std::cout << "code: " << code << std::endl;
std::cout << "PyArray_TypeNum(py_type): " << PyArray_TypeNum(py_type) << std::endl;
return code;
}
else
return -1; // type not registered
}
}
static int registerNewType(PyTypeObject * py_type_ptr,
const std::type_info * type_info_ptr,
const int type_size,
PyArray_GetItemFunc * getitem,
PyArray_SetItemFunc * setitem,
PyArray_NonzeroFunc * nonzero,
PyArray_CopySwapFunc * copyswap,
PyArray_CopySwapNFunc * copyswapn)
{
namespace bp = boost::python;
PyArray_Descr * descr_ptr = new PyArray_Descr(*PyArray_DescrFromType(NPY_OBJECT));
PyArray_Descr & descr = *descr_ptr;
descr.typeobj = py_type_ptr;
descr.kind = 'V';
descr.byteorder = '=';
std::cout << "type name: " << py_type_ptr->tp_name << std::endl;
descr.elsize = type_size;
descr.flags = NPY_LIST_PICKLE | NPY_USE_GETITEM | NPY_USE_SETITEM | NPY_NEEDS_INIT | NPY_NEEDS_PYAPI;
// descr->names = PyTuple_New(0);
// descr->fields = PyDict_New();
std::cout << "descr->elsize: " << descr.elsize << std::endl;
// std::cout << "size of Scalar: " << sizeof(Scalar) << std::endl;
std::cout << "type_num: " << descr.type_num << std::endl;
PyArray_ArrFuncs * funcs_ptr = new PyArray_ArrFuncs;
PyArray_ArrFuncs & funcs = *funcs_ptr;
descr.f = funcs_ptr;
PyArray_InitArrFuncs(funcs_ptr);
funcs.getitem = getitem;
funcs.setitem = setitem;
funcs.nonzero = nonzero;
funcs.copyswap = copyswap;
funcs.copyswapn = copyswapn;
// f->cast = cast;
const int code = PyArray_RegisterDataType(descr_ptr);
assert(code >= 0 && "The return code should be positive");
PyArray_Descr * new_descr = PyArray_DescrFromType(code);
std::cout << "new type_num: " << new_descr->type_num << std::endl;
type_to_py_type_bindings.insert(std::make_pair(type_info_ptr,py_type_ptr));
py_array_descr_bindings[py_type_ptr] = new_descr;
py_array_code_bindings[py_type_ptr] = code;
// PyArray_RegisterCanCast(descr,NPY_OBJECT,NPY_NOSCALAR);
return code;
}
static Register & instance()
{
return self;
}
private:
Register() {};
struct Compare_PyTypeObject
{
bool operator()(const PyTypeObject * a, const PyTypeObject * b) const
{
return std::string(a->tp_name) < std::string(b->tp_name);
}
};
struct Compare_TypeInfo
{
bool operator()(const std::type_info * a, const std::type_info * b) const
{
return std::string(a->name()) < std::string(b->name());
}
};
typedef std::map<const std::type_info *,PyTypeObject *,Compare_TypeInfo> MapInfo;
static MapInfo type_to_py_type_bindings;
typedef std::map<PyTypeObject *,PyArray_Descr *,Compare_PyTypeObject> MapDescr;
static MapDescr py_array_descr_bindings;
typedef std::map<PyTypeObject *,int,Compare_PyTypeObject> MapCode;
static MapCode py_array_code_bindings;
static Register self;
};
template<typename Scalar>
int registerNewType(PyTypeObject * py_type_ptr = NULL)
{
// Check whether the type is a Numpy native type.
// In this case, the registration is not required.
if(isNumpyNativeType<Scalar>())
return NumpyEquivalentType<Scalar>::type_code;
// Retrieve the registered type for the current Scalar
if(py_type_ptr == NULL)
{ // retrive the type from Boost.Python
py_type_ptr = Register::getPyType<Scalar>();
}
if(Register::isRegistered(py_type_ptr))
return Register::getTypeCode(py_type_ptr); // the type is already registered
PyArray_GetItemFunc * getitem = &internal::SpecialMethods<Scalar>::getitem;
PyArray_SetItemFunc * setitem = &internal::SpecialMethods<Scalar>::setitem;
PyArray_NonzeroFunc * nonzero = &internal::SpecialMethods<Scalar>::nonzero;
PyArray_CopySwapFunc * copyswap = &internal::SpecialMethods<Scalar>::copyswap;
PyArray_CopySwapNFunc * copyswapn = &internal::SpecialMethods<Scalar>::copyswapn;
// PyArray_CastFunc * cast = &internal::SpecialMethods<Scalar>::cast;
int code = Register::registerNewType(py_type_ptr,
&typeid(Scalar),
sizeof(Scalar),
getitem, setitem, nonzero,
copyswap, copyswapn);
std::cout << "code: " << code << std::endl;
PyObject* numpy_str;
#if PY_MAJOR_VERSION >= 3
numpy_str = PyUnicode_FromString("numpy");
#else
numpy_str = PyString_FromString("numpy");
#endif
PyObject* numpy;
numpy = PyImport_Import(numpy_str);
Py_DECREF(numpy_str);
// load numpy
std::cout << "numpy loaded" << std::endl;
import_ufunc();
#define REGISTER_BINARY_UFUNC(name,T1,T2,R) { \
PyUFuncObject* ufunc = \
(PyUFuncObject*)PyObject_GetAttrString(numpy, #name); \
std::cout << "ufunc" << std::endl; \
int _types[3] = { Register::getTypeCode<T1>(), Register::getTypeCode<T2>(), Register::getTypeCode<R>()}; \
if (!ufunc) { \
std::cout << "ufunc not available" << std::endl; \
/*goto fail; \*/ \
} \
std::cout << "step 1" << std::endl; \
if (sizeof(_types)/sizeof(int)!=ufunc->nargs) { \
PyErr_Format(PyExc_AssertionError, \
"ufunc %s takes %d arguments, our loop takes %lu", \
#name, ufunc->nargs, (unsigned long) \
(sizeof(_types)/sizeof(int))); \
Py_DECREF(ufunc); \
std::cout << "failed 1" << std::endl; \
} \
std::cout << "step 2" << std::endl; \
if (PyUFunc_RegisterLoopForType((PyUFuncObject*)ufunc, code, \
internal::binary_op_##name<T1,T2,R>, _types, 0) < 0) { \
/*Py_DECREF(ufunc);*/ \
std::cout << "failed 2" << std::endl; \
/*goto fail; \*/ \
} \
Py_DECREF(ufunc); \
}
std::cout << "register func" << std::endl;
REGISTER_BINARY_UFUNC(add,Scalar,Scalar,Scalar);
REGISTER_BINARY_UFUNC(subtract,Scalar,Scalar,Scalar);
REGISTER_BINARY_UFUNC(multiply,Scalar,Scalar,Scalar);
REGISTER_BINARY_UFUNC(divide,Scalar,Scalar,Scalar);
// Comparison operators
REGISTER_BINARY_UFUNC(equal,Scalar,Scalar,bool);
REGISTER_BINARY_UFUNC(not_equal,Scalar,Scalar,bool);
REGISTER_BINARY_UFUNC(greater,Scalar,Scalar,bool);
REGISTER_BINARY_UFUNC(less,Scalar,Scalar,bool);
REGISTER_BINARY_UFUNC(greater_equal,Scalar,Scalar,bool);
REGISTER_BINARY_UFUNC(less_equal,Scalar,Scalar,bool);
std::cout << "register func" << std::endl;
Py_DECREF(numpy);
return code;
}
} // namespace eigenpy
#endif // __eigenpy_user_type_hpp__
/*
* Copyright 2020 INRIA
*/
#include "eigenpy/user-type.hpp"
namespace eigenpy
{
Register::MapDescr Register::py_array_descr_bindings;
Register::MapCode Register::py_array_code_bindings;
Register::MapInfo Register::type_to_py_type_bindings;
} // namespace eigenpy
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment