transform.hpp

//
// Copyright (c) 2019 INRIA
//

#pragma once

#include "pinocchio/spatial.hpp"
#include <coal/math/transform.h>

namespace boost
{
 namespace python
 {
 namespace converter
 {

 template<typename Scalar, int Options>
 struct implicit<::coal::Transform3s, ::pinocchio::SE3Tpl<Scalar, Options>>
 {
 typedef ::coal::Transform3s Source;
 typedef ::pinocchio::SE3Tpl<Scalar, Options> Target;

 static void * convertible(PyObject * obj)
 {
 // Find a converter which can produce a Source instance from
 // obj. The user has told us that Source can be converted to
 // Target, and instantiating construct() below, ensures that
 // at compile-time.
 return implicit_rvalue_convertible_from_python(obj, registered<Source>::converters) ? obj
 : 0;
 }

 static void construct(PyObject * obj, rvalue_from_python_stage1_data * data)
 {
 void * storage =
 reinterpret_cast<rvalue_from_python_storage<Target> *>(reinterpret_cast<void *>(data))
 ->storage.bytes;

 arg_from_python<Source> get_source(obj);
 bool convertible = get_source.convertible();
 BOOST_VERIFY(convertible);

 const Source & t = get_source();
 new (storage) Target(t.getRotation(), t.getTranslation());

 // record successful construction
 data->convertible = storage;
 }
 };

 template<typename Scalar, int Options>
 struct implicit<::pinocchio::SE3Tpl<Scalar, Options>, ::coal::Transform3s>
 {
 typedef ::pinocchio::SE3Tpl<Scalar, Options> Source;
 typedef ::coal::Transform3s Target;

 static void * convertible(PyObject * obj)
 {
 // Find a converter which can produce a Source instance from
 // obj. The user has told us that Source can be converted to
 // Target, and instantiating construct() below, ensures that
 // at compile-time.
 return implicit_rvalue_convertible_from_python(obj, registered<Source>::converters) ? obj
 : 0;
 }

 static void construct(PyObject * obj, rvalue_from_python_stage1_data * data)
 {
 void * storage =
 reinterpret_cast<rvalue_from_python_storage<Target> *>(reinterpret_cast<void *>(data))
 ->storage.bytes;

 arg_from_python<Source> get_source(obj);
 bool convertible = get_source.convertible();
 BOOST_VERIFY(convertible);

 const Source & M = get_source();
 new (storage) Target(M.rotation(), M.translation());

 // record successful construction
 data->convertible = storage;
 }
 };

 } // namespace converter
 } // namespace python
} // namespace boost