Skip to content
GitLab
Commit 180e65b9 authored by Steve T's avatar Steve T
Browse files

added cross product between a curve and a point with python bindings

parent 70ff0294
Hide whitespace changes
Inline Side-by-side
include/curves/bezier_curve.h
View file @ 180e65b9
......@@ -478,6 +478,25 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_ * g.mult_T_);
}
/// \brief Compute the cross product of the current bezier b by a point point.
/// The cross product pXpoint of is defined such that
/// forall t, bXpoint(t) = b(t) X point, with X designing the cross product.
/// This method of course only makes sense for dimension 3 polynomials.
/// \param point point to compute the cross product with.
/// \return a new polynomial defining the cross product between this and point
bezier_curve_t cross(const bezier_curve_t::point_t& point) const {
//See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
//http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
if (dim()!= 3)
throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
t_point_t new_waypoints;
for(typename t_point_t::const_iterator cit = waypoints().begin(); cit != waypoints().end(); ++cit){
new_waypoints.push_back(curves::cross(*cit, point));
}
return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_);
}
bezier_curve_t& operator+=(const bezier_curve_t& other) {
assert_operator_compatible(other);
bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier
......
include/curves/polynomial.h
View file @ 180e65b9
......@@ -471,7 +471,31 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
}
// remove last degrees is they are equal to 0
long final_degree = new_degree;
while(nCoeffs.col(final_degree).norm() <= curves::MARGIN){
while(nCoeffs.col(final_degree).norm() <= curves::MARGIN && final_degree >0){
--final_degree;
}
return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max());
}
/// \brief Compute the cross product of the current polynomial p by a point point.
/// The cross product pXpoint of is defined such that
/// forall t, pXpoint(t) = p(t) X point, with X designing the cross product.
/// This method of course only makes sense for dimension 3 polynomials.
/// \param point point to compute the cross product with.
/// \return a new polynomial defining the cross product between this and point
polynomial_t cross(const polynomial_t::point_t& point) const {
if (dim()!= 3)
throw std::invalid_argument("Can't perform cross product on polynomials with dimensions != 3 ");
coeff_t nCoeffs = coefficients_;
Eigen::Vector3d currentVec;
Eigen::Vector3d pointVec = point;
for(long i = 0; i< coefficients_.cols(); ++i){
currentVec = coefficients_.col(i);
nCoeffs.col(i) = currentVec.cross(pointVec);
}
// remove last degrees is they are equal to 0
long final_degree = degree();
while(nCoeffs.col(final_degree).norm() <= curves::MARGIN && final_degree >0){
--final_degree;
}
return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max());
......
python/curves/curves_python.cpp
View file @ 180e65b9
......@@ -626,6 +626,8 @@ BOOST_PYTHON_MODULE(curves) {
/** END base abstracts class for each dimension/type : **/
/** BEGIN bezier3 curve**/
bezier3_t (bezier3_t::*cross_bez3) (const bezier3_t&) const = &bezier3_t::cross;
bezier3_t (bezier3_t::*cross_pointBez3) (const bezier3_t::point_t&) const = &bezier3_t::cross;
class_<bezier3_t, bases<curve_3_t>, boost::shared_ptr<bezier3_t> >("bezier3", init<>())
.def("__init__", make_constructor(&wrapBezier3Constructor))
.def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
......@@ -650,8 +652,9 @@ BOOST_PYTHON_MODULE(curves) {
"Loads *this from a binary file.")
//.def(SerializableVisitor<bezier_t>())
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("cross", &bezier3_t::cross, bp::args("other"), "Compute the cross product of the current bezier by another bezier. The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. This method of course only makes sense for dimension 3 curves.")
.def(bp::self != bp::self)
.def("cross", cross_bez3, bp::args("other"), "Compute the cross product of the current bezier by another bezier. The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. This method of course only makes sense for dimension 3 curves.")
.def("cross", cross_pointBez3, bp::args("point"), "Compute the cross product PXpt of the current Bezier P by a point pt, such that for all t, PXpt(t) = P(t) X pt")
.def(self *= double())
.def(self /= double())
.def(self + bezier3_t())
......@@ -668,6 +671,8 @@ BOOST_PYTHON_MODULE(curves) {
.def_pickle(curve_pickle_suite<bezier3_t>());
/** END bezier3 curve**/
/** BEGIN bezier curve**/
bezier_t (bezier_t::*cross_bez) (const bezier_t&) const = &bezier_t::cross;
bezier_t (bezier_t::*cross_pointBez) (const bezier_t::point_t&) const = &bezier_t::cross;
class_<bezier_t, bases<curve_abc_t>, boost::shared_ptr<bezier_t> >("bezier", init<>())
.def("__init__", make_constructor(&wrapBezierConstructor))
.def("__init__", make_constructor(&wrapBezierConstructorBounds))
......@@ -693,7 +698,8 @@ BOOST_PYTHON_MODULE(curves) {
"Loads *this from a binary file.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("cross", &bezier_t::cross, bp::args("other"), "Compute the cross product of the current bezier by another bezier. The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. This method of course only makes sense for dimension 3 curves.")
.def("cross", cross_bez, bp::args("other"), "Compute the cross product of the current bezier by another bezier. The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. This method of course only makes sense for dimension 3 curves.")
.def("cross", cross_pointBez, bp::args("point"), "Compute the cross product PXpt of the current Bezier P by a point pt, such that for all t, PXpt(t) = P(t) X pt")
.def(self += bezier_t())
.def(self -= bezier_t())
.def(self + bezier_t())
......@@ -741,6 +747,8 @@ BOOST_PYTHON_MODULE(curves) {
.def("norm", &linear_variable_t::norm)
.def("cross", &linear_variable_t::cross,bp::args("other"),"Compute the cross product of the current linear_variable and the other. Only works for dimension 3");
bezier_linear_variable_t (bezier_linear_variable_t::*cross_bez_var) (const bezier_linear_variable_t&) const = &bezier_linear_variable_t::cross;
bezier_linear_variable_t (bezier_linear_variable_t::*cross_point_var) (const bezier_linear_variable_t::point_t&) const = &bezier_linear_variable_t::cross;
class_<bezier_linear_variable_t, bases<curve_abc_t>, boost::shared_ptr<bezier_linear_variable_t> >(
"bezier_linear_variable", no_init)
.def("__init__", make_constructor(&wrapBezierLinearConstructor))
......@@ -758,7 +766,8 @@ BOOST_PYTHON_MODULE(curves) {
.def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex)
.def_readonly("degree", &bezier_linear_variable_t::degree_)
.def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
.def("cross", &bezier_linear_variable_t::cross,bp::args("other"),"Compute the cross product of the current bezier_linear_variable and the other. Only works for dimension 3")
.def("cross", cross_bez_var, bp::args("other"), "Compute the cross product of the current Bezier by another Bezier. The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. This method of course only makes sense for dimension 3 polynomials.")
.def("cross", cross_point_var, bp::args("point"), "Compute the cross product PXpt of the current Bezier P by a point pt, such that for all t, PXpt(t) = P(t) X pt")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def(self += bezier_linear_variable_t())
......@@ -783,6 +792,9 @@ BOOST_PYTHON_MODULE(curves) {
/** END variable points bezier curve**/
/** BEGIN polynomial curve function**/
polynomial_t (polynomial_t::*cross_pol) (const polynomial_t&) const = &polynomial_t::cross;
polynomial_t (polynomial_t::*cross_point) (const polynomial_t::point_t&) const = &polynomial_t::cross;
class_<polynomial_t, bases<curve_abc_t>, boost::shared_ptr<polynomial_t> >("polynomial", init<>())
.def("__init__",
make_constructor(&wrapPolynomialConstructor1, default_call_policies(), args("coeffs", "min", "max")),
......@@ -836,15 +848,20 @@ BOOST_PYTHON_MODULE(curves) {
"Saves *this inside a binary file.")
.def("loadFromBinary", &polynomial_t::loadFromBinary<polynomial_t>, bp::args("filename"),
"Loads *this from a binary file.")
.def("cross", &polynomial_t::cross,"Compute the cross product of the current polynomial by another polynomial. The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. This method of course only makes sense for dimension 3 polynomials.")
.def("cross", cross_pol, bp::args("other"), "Compute the cross product of the current polynomial by another polynomial. The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product. This method of course only makes sense for dimension 3 polynomials.")
.def("cross", cross_point, bp::args("point"), "Compute the cross product PXpt of the current polynomial P by a point pt, such that for all t, PXpt(t) = P(t) X pt")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def(self += polynomial_t())
.def(self -= polynomial_t())
.def(self *= double())
.def(self /= double())
.def(self + polynomial_t())
.def(self - polynomial_t())
.def(self += polynomial_t::point_t())
.def(self -= polynomial_t::point_t())
.def(self + polynomial_t::point_t())
.def(self - polynomial_t::point_t())
.def(self *= double())
.def(self /= double())
.def(-self)
.def(self * double())
.def(self / double())
......
python/test/test.py
View file @ 180e65b9
......@@ -301,6 +301,7 @@ class TestCurves(unittest.TestCase):
a.max()
a(0.4)
#arithmetic
waypoints2 = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.]]).transpose()
a1 = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.]
b = a + a1
......@@ -310,6 +311,13 @@ class TestCurves(unittest.TestCase):
a1*=0.1
a1/=0.1
b = -a1
c = a.cross(array([1., 2., 3.]))
c = a.cross(a)
c(0)
b += array([1., 2., 3.])
b -= array([1., 2., 3.])
b = a + array([1., 2., 3.])
b = a - array([1., 2., 3.])
# Test get coefficient at degree
self.assertTrue((a.coeff() == waypoints).all())
......
tests/test-operations.cpp
View file @ 180e65b9
......@@ -125,19 +125,21 @@ BOOST_AUTO_TEST_CASE(bezierPointOperations, * boost::unit_test::tolerance(0.001)
}
bezier_t p1(vec1.begin(),vec1.end(),0.,1.);
Eigen::Vector3d point; point << 1., 1. , 1.;
//bezier_t::point_t point = bezier_t::point_t::Random(3);
Eigen::Vector3d point = bezier_t::point_t::Random(3);
bezier_t pSum = p1 + point;
bezier_t pSumR = point + p1;
bezier_t pSub = p1 - point;
bezier_t pSubR = point - p1;
bezier_t pcross = p1.cross(point);
for (double i = 0.; i <=100.; ++i ){
double dt = i / 100.;
BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.);
BOOST_TEST((pSumR(dt) - (p1(dt)+point)).norm()==0.);
BOOST_TEST(( pSub(dt) - (p1(dt)-point)).norm()==0.);
BOOST_TEST((pSubR(dt) - (point-p1(dt))).norm()==0.);
Eigen::Vector3d p1dt = p1(dt);
BOOST_TEST((pcross(dt) - (p1dt.cross(point))).norm()==0.);
}
}
......@@ -195,19 +197,22 @@ BOOST_AUTO_TEST_CASE(crossProductBezierLinearVariable, * boost::unit_test::toler
BOOST_AUTO_TEST_CASE(polynomialPointOperations, * boost::unit_test::tolerance(0.001)) {
polynomial_t::coeff_t coeffs1 = Eigen::MatrixXd::Random(3,5);
polynomial_t::point_t point = polynomial_t::point_t::Random(3);
Eigen::Vector3d point = Eigen::Vector3d::Random(3);
polynomial_t p1(coeffs1,0.,1.);
polynomial_t pSum = p1 + point;
polynomial_t pSumR = point + p1;
polynomial_t pSub = p1 - point;
polynomial_t pSubR = point - p1;
polynomial_t pcross = p1.cross(point);
for (double i = 0.; i <=100.; ++i ){
double dt = i / 100.;
BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.);
BOOST_TEST((pSumR(dt) - (p1(dt)+point)).norm()==0.);
BOOST_TEST(( pSub(dt) - (p1(dt)-point)).norm()==0.);
BOOST_TEST((pSubR(dt) - (point-p1(dt))).norm()==0.);
Eigen::Vector3d p1dt = p1(dt);
BOOST_TEST((pcross(dt) - (p1dt.cross(point))).norm()==0.);
}
}
......@@ -272,6 +277,9 @@ BOOST_AUTO_TEST_CASE(crossPoductPolynomials, * boost::unit_test::tolerance(0.001
polynomial_t p5(coeffs2,0.1,.5);
polynomial_t pDim4(coeffsDim4,0.,1.);
//testing reduction
BOOST_CHECK_EQUAL (p1.cross(p1).degree(), 0);
BOOST_CHECK_THROW( p1.cross(p3), std::exception );
BOOST_CHECK_THROW( p1.cross(p4), std::exception );
BOOST_CHECK_THROW( p1.cross(p5), std::exception );
......
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