ClipperLib::ExPolygons PlaneExt::PolygonizeConcaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull, tVertices &polygon_indices)
{
ClipperLib::ExPolygons polygon;
// Create transformed point cloud (polygon is aligned with XY plane
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_hull_proj (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud(*plane_hull, *plane_hull_proj, Eigen::Vector3f(-a*planeShift, -b*planeShift, -c*planeShift), Eigen::Quaternion<float>(0,0,0,0));
pcl::transformPointCloud(*plane_hull_proj, *plane_hull_proj, planeTransXY);
// save each point into Clipper lib polygon structure
if (plane_hull_proj->points.size() > 0)
{
for (unsigned int i = 0; i < polygon_indices.size(); ++i)
{
ClipperLib::ExPolygon clipperPoly;
clipperPoly.outer.resize(polygon_indices[i].vertices.size());
for (unsigned int j = 0; j < polygon_indices[i].vertices.size(); ++j)
{
clipperPoly.outer[j].X = CONVERT_TO_LONG(plane_hull_proj->points[polygon_indices[i].vertices[j]].x);
clipperPoly.outer[j].Y = CONVERT_TO_LONG(plane_hull_proj->points[polygon_indices[i].vertices[j]].y);
}
// Orientation check
if (!ClipperLib::Orientation(clipperPoly.outer))
{
ClipperLib::ReversePolygon(clipperPoly.outer);
}
polygon.push_back(clipperPoly);
}
}
return polygon;
}
static PyObject *
int_and(PyIntObject *v, PyIntObject *w)
{
register long a, b;
CONVERT_TO_LONG(v, a);
CONVERT_TO_LONG(w, b);
return PyInt_FromLong(a & b);
}
static PyObject *
int_add(PyIntObject *v, PyIntObject *w)
{
register long a, b, x;
CONVERT_TO_LONG(v, a);
CONVERT_TO_LONG(w, b);
x = a + b;
if ((x^a) >= 0 || (x^b) >= 0)
return PyInt_FromLong(x);
if (err_ovf("integer addition"))
return NULL;
return PyLong_Type.tp_as_number->nb_add((PyObject *)v, (PyObject *)w);
}
static PyObject *
int_divmod(PyIntObject *x, PyIntObject *y)
{
long xi, yi;
long d, m;
CONVERT_TO_LONG(x, xi);
CONVERT_TO_LONG(y, yi);
switch (i_divmod(xi, yi, &d, &m)) {
case DIVMOD_OK:
return Py_BuildValue("(ll)", d, m);
case DIVMOD_OVERFLOW:
return PyLong_Type.tp_as_number->nb_divmod((PyObject *)x,
(PyObject *)y);
default:
return NULL;
}
}
static PyObject *
int_mod(PyIntObject *x, PyIntObject *y)
{
long xi, yi;
long d, m;
CONVERT_TO_LONG(x, xi);
CONVERT_TO_LONG(y, yi);
switch (i_divmod(xi, yi, &d, &m)) {
case DIVMOD_OK:
return PyInt_FromLong(m);
case DIVMOD_OVERFLOW:
return PyLong_Type.tp_as_number->nb_remainder((PyObject *)x,
(PyObject *)y);
default:
return NULL;
}
}
static PyObject *
int_lshift(PyIntObject *v, PyIntObject *w)
{
register long a, b;
CONVERT_TO_LONG(v, a);
CONVERT_TO_LONG(w, b);
if (b < 0) {
PyErr_SetString(PyExc_ValueError, "negative shift count");
return NULL;
}
if (a == 0 || b == 0)
return int_pos(v);
if (b >= LONG_BIT) {
return PyInt_FromLong(0L);
}
a = (long)((unsigned long)a << b);
return PyInt_FromLong(a);
}
static PyObject *
int_classic_div(PyIntObject *x, PyIntObject *y)
{
long xi, yi;
long d, m;
CONVERT_TO_LONG(x, xi);
CONVERT_TO_LONG(y, yi);
if (Py_DivisionWarningFlag &&
PyErr_Warn(PyExc_DeprecationWarning, "classic int division") < 0)
return NULL;
switch (i_divmod(xi, yi, &d, &m)) {
case DIVMOD_OK:
return PyInt_FromLong(d);
case DIVMOD_OVERFLOW:
return PyLong_Type.tp_as_number->nb_divide((PyObject *)x,
(PyObject *)y);
default:
return NULL;
}
}
static PyObject *
int_rshift(PyIntObject *v, PyIntObject *w)
{
register long a, b;
CONVERT_TO_LONG(v, a);
CONVERT_TO_LONG(w, b);
if (b < 0) {
PyErr_SetString(PyExc_ValueError, "negative shift count");
return NULL;
}
if (a == 0 || b == 0)
return int_pos(v);
if (b >= LONG_BIT) {
if (a < 0)
a = -1;
else
a = 0;
}
else {
a = Py_ARITHMETIC_RIGHT_SHIFT(long, a, b);
}
return PyInt_FromLong(a);
}
static PyObject *
int_pow(PyIntObject *v, PyIntObject *w, PyIntObject *z)
{
register long iv, iw, iz=0, ix, temp, prev;
CONVERT_TO_LONG(v, iv);
CONVERT_TO_LONG(w, iw);
if (iw < 0) {
if ((PyObject *)z != Py_None) {
PyErr_SetString(PyExc_TypeError, "pow() 2nd argument "
"cannot be negative when 3rd argument specified");
return NULL;
}
/* Return a float. This works because we know that
this calls float_pow() which converts its
arguments to double. */
return PyFloat_Type.tp_as_number->nb_power(
(PyObject *)v, (PyObject *)w, (PyObject *)z);
}
if ((PyObject *)z != Py_None) {
CONVERT_TO_LONG(z, iz);
if (iz == 0) {
PyErr_SetString(PyExc_ValueError,
"pow() 3rd argument cannot be 0");
return NULL;
}
}
/*
* XXX: The original exponentiation code stopped looping
* when temp hit zero; this code will continue onwards
* unnecessarily, but at least it won't cause any errors.
* Hopefully the speed improvement from the fast exponentiation
* will compensate for the slight inefficiency.
* XXX: Better handling of overflows is desperately needed.
*/
temp = iv;
ix = 1;
while (iw > 0) {
prev = ix; /* Save value for overflow check */
if (iw & 1) {
ix = ix*temp;
if (temp == 0)
break; /* Avoid ix / 0 */
if (ix / temp != prev) {
if (err_ovf("integer exponentiation"))
return NULL;
return PyLong_Type.tp_as_number->nb_power(
(PyObject *)v,
(PyObject *)w,
(PyObject *)z);
}
}
iw >>= 1; /* Shift exponent down by 1 bit */
if (iw==0) break;
prev = temp;
temp *= temp; /* Square the value of temp */
if (prev!=0 && temp/prev!=prev) {
if (err_ovf("integer exponentiation"))
return NULL;
return PyLong_Type.tp_as_number->nb_power(
(PyObject *)v, (PyObject *)w, (PyObject *)z);
}
if (iz) {
/* If we did a multiplication, perform a modulo */
ix = ix % iz;
temp = temp % iz;
}
}
if (iz) {
long div, mod;
switch (i_divmod(ix, iz, &div, &mod)) {
case DIVMOD_OK:
ix = mod;
break;
case DIVMOD_OVERFLOW:
return PyLong_Type.tp_as_number->nb_power(
(PyObject *)v, (PyObject *)w, (PyObject *)z);
default:
return NULL;
}
}
return PyInt_FromLong(ix);
}
static PyObject *
int_mul(PyObject *v, PyObject *w)
{
long a, b;
long longprod; /* a*b in native long arithmetic */
double doubled_longprod; /* (double)longprod */
double doubleprod; /* (double)a * (double)b */
if (USE_SQ_REPEAT(v)) {
repeat:
/* sequence * int */
a = PyInt_AsLong(w);
#if LONG_MAX != INT_MAX
if (a > INT_MAX) {
PyErr_SetString(PyExc_ValueError,
"sequence repeat count too large");
return NULL;
}
else if (a < INT_MIN)
a = INT_MIN;
/* XXX Why don't I either
- set a to -1 whenever it's negative (after all,
sequence repeat usually treats negative numbers
as zero(); or
- raise an exception when it's less than INT_MIN?
I'm thinking about a hypothetical use case where some
sequence type might use a negative value as a flag of
some kind. In those cases I don't want to break the
code by mapping all negative values to -1. But I also
don't want to break e.g. []*(-sys.maxint), which is
perfectly safe, returning []. As a compromise, I do
map out-of-range negative values.
*/
#endif
return (*v->ob_type->tp_as_sequence->sq_repeat)(v, a);
}
if (USE_SQ_REPEAT(w)) {
PyObject *tmp = v;
v = w;
w = tmp;
goto repeat;
}
CONVERT_TO_LONG(v, a);
CONVERT_TO_LONG(w, b);
longprod = a * b;
doubleprod = (double)a * (double)b;
doubled_longprod = (double)longprod;
/* Fast path for normal case: small multiplicands, and no info
is lost in either method. */
if (doubled_longprod == doubleprod)
return PyInt_FromLong(longprod);
/* Somebody somewhere lost info. Close enough, or way off? Note
that a != 0 and b != 0 (else doubled_longprod == doubleprod == 0).
The difference either is or isn't significant compared to the
true value (of which doubleprod is a good approximation).
*/
{
const double diff = doubled_longprod - doubleprod;
const double absdiff = diff >= 0.0 ? diff : -diff;
const double absprod = doubleprod >= 0.0 ? doubleprod :
-doubleprod;
/* absdiff/absprod <= 1/32 iff
32 * absdiff <= absprod -- 5 good bits is "close enough" */
if (32.0 * absdiff <= absprod)
return PyInt_FromLong(longprod);
else if (err_ovf("integer multiplication"))
return NULL;
else
return PyLong_Type.tp_as_number->nb_multiply(v, w);
}
}
