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
...@@ -478,6 +478,25 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -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_); 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) { bezier_curve_t& operator+=(const bezier_curve_t& other) {
assert_operator_compatible(other); assert_operator_compatible(other);
bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier
......
...@@ -471,7 +471,31 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -471,7 +471,31 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
} }
// remove last degrees is they are equal to 0 // remove last degrees is they are equal to 0
long final_degree = new_degree; 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; --final_degree;
} }
return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max()); return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max());
......
...@@ -626,6 +626,8 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -626,6 +626,8 @@ BOOST_PYTHON_MODULE(curves) {
/** END base abstracts class for each dimension/type : **/ /** END base abstracts class for each dimension/type : **/
/** BEGIN bezier3 curve**/ /** 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<>()) class_<bezier3_t, bases<curve_3_t>, boost::shared_ptr<bezier3_t> >("bezier3", init<>())
.def("__init__", make_constructor(&wrapBezier3Constructor)) .def("__init__", make_constructor(&wrapBezier3Constructor))
.def("__init__", make_constructor(&wrapBezier3ConstructorBounds)) .def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
...@@ -651,7 +653,8 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -651,7 +653,8 @@ BOOST_PYTHON_MODULE(curves) {
//.def(SerializableVisitor<bezier_t>()) //.def(SerializableVisitor<bezier_t>())
.def(bp::self == bp::self) .def(bp::self == bp::self)
.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("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 /= double()) .def(self /= double())
.def(self + bezier3_t()) .def(self + bezier3_t())
...@@ -668,6 +671,8 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -668,6 +671,8 @@ BOOST_PYTHON_MODULE(curves) {
.def_pickle(curve_pickle_suite<bezier3_t>()); .def_pickle(curve_pickle_suite<bezier3_t>());
/** END bezier3 curve**/ /** END bezier3 curve**/
/** BEGIN bezier 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<>()) class_<bezier_t, bases<curve_abc_t>, boost::shared_ptr<bezier_t> >("bezier", init<>())
.def("__init__", make_constructor(&wrapBezierConstructor)) .def("__init__", make_constructor(&wrapBezierConstructor))
.def("__init__", make_constructor(&wrapBezierConstructorBounds)) .def("__init__", make_constructor(&wrapBezierConstructorBounds))
...@@ -693,7 +698,8 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -693,7 +698,8 @@ BOOST_PYTHON_MODULE(curves) {
"Loads *this from a binary file.") "Loads *this from a binary file.")
.def(bp::self == bp::self) .def(bp::self == bp::self)
.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()) .def(self -= bezier_t())
.def(self + bezier_t()) .def(self + bezier_t())
...@@ -741,6 +747,8 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -741,6 +747,8 @@ BOOST_PYTHON_MODULE(curves) {
.def("norm", &linear_variable_t::norm) .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"); .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> >( class_<bezier_linear_variable_t, bases<curve_abc_t>, boost::shared_ptr<bezier_linear_variable_t> >(
"bezier_linear_variable", no_init) "bezier_linear_variable", no_init)
.def("__init__", make_constructor(&wrapBezierLinearConstructor)) .def("__init__", make_constructor(&wrapBezierLinearConstructor))
...@@ -758,7 +766,8 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -758,7 +766,8 @@ BOOST_PYTHON_MODULE(curves) {
.def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex) .def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex)
.def_readonly("degree", &bezier_linear_variable_t::degree_) .def_readonly("degree", &bezier_linear_variable_t::degree_)
.def_readonly("nbWaypoints", &bezier_linear_variable_t::size_) .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(bp::self != bp::self) .def(bp::self != bp::self)
.def(self += bezier_linear_variable_t()) .def(self += bezier_linear_variable_t())
...@@ -783,6 +792,9 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -783,6 +792,9 @@ BOOST_PYTHON_MODULE(curves) {
/** END variable points bezier curve**/ /** END variable points bezier curve**/
/** BEGIN polynomial curve function**/ /** 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<>()) class_<polynomial_t, bases<curve_abc_t>, boost::shared_ptr<polynomial_t> >("polynomial", init<>())
.def("__init__", .def("__init__",
make_constructor(&wrapPolynomialConstructor1, default_call_policies(), args("coeffs", "min", "max")), make_constructor(&wrapPolynomialConstructor1, default_call_policies(), args("coeffs", "min", "max")),
...@@ -836,15 +848,20 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -836,15 +848,20 @@ BOOST_PYTHON_MODULE(curves) {
"Saves *this inside a binary file.") "Saves *this inside a binary file.")
.def("loadFromBinary", &polynomial_t::loadFromBinary<polynomial_t>, bp::args("filename"), .def("loadFromBinary", &polynomial_t::loadFromBinary<polynomial_t>, bp::args("filename"),
"Loads *this from a binary file.") "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(bp::self != bp::self) .def(bp::self != bp::self)
.def(self += polynomial_t()) .def(self += polynomial_t())
.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()) .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)
.def(self * double()) .def(self * double())
.def(self / double()) .def(self / double())
......
...@@ -301,6 +301,7 @@ class TestCurves(unittest.TestCase): ...@@ -301,6 +301,7 @@ class TestCurves(unittest.TestCase):
a.max() a.max()
a(0.4) a(0.4)
#arithmetic
waypoints2 = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.]]).transpose() waypoints2 = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.]]).transpose()
a1 = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.] a1 = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.]
b = a + a1 b = a + a1
...@@ -310,6 +311,13 @@ class TestCurves(unittest.TestCase): ...@@ -310,6 +311,13 @@ class TestCurves(unittest.TestCase):
a1*=0.1 a1*=0.1
a1/=0.1 a1/=0.1
b = -a1 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 # Test get coefficient at degree
self.assertTrue((a.coeff() == waypoints).all()) self.assertTrue((a.coeff() == waypoints).all())
......
...@@ -125,19 +125,21 @@ BOOST_AUTO_TEST_CASE(bezierPointOperations, * boost::unit_test::tolerance(0.001) ...@@ -125,19 +125,21 @@ BOOST_AUTO_TEST_CASE(bezierPointOperations, * boost::unit_test::tolerance(0.001)
} }
bezier_t p1(vec1.begin(),vec1.end(),0.,1.); bezier_t p1(vec1.begin(),vec1.end(),0.,1.);
Eigen::Vector3d point; point << 1., 1. , 1.; Eigen::Vector3d point = bezier_t::point_t::Random(3);
//bezier_t::point_t point = bezier_t::point_t::Random(3);
bezier_t pSum = p1 + point; bezier_t pSum = p1 + point;
bezier_t pSumR = point + p1; bezier_t pSumR = point + p1;
bezier_t pSub = p1 - point; bezier_t pSub = p1 - point;
bezier_t pSubR = point - p1; bezier_t pSubR = point - p1;
bezier_t pcross = p1.cross(point);
for (double i = 0.; i <=100.; ++i ){ for (double i = 0.; i <=100.; ++i ){
double dt = i / 100.; double dt = i / 100.;
BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.); BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.);
BOOST_TEST((pSumR(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(( pSub(dt) - (p1(dt)-point)).norm()==0.);
BOOST_TEST((pSubR(dt) - (point-p1(dt))).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 ...@@ -195,19 +197,22 @@ BOOST_AUTO_TEST_CASE(crossProductBezierLinearVariable, * boost::unit_test::toler
BOOST_AUTO_TEST_CASE(polynomialPointOperations, * boost::unit_test::tolerance(0.001)) { BOOST_AUTO_TEST_CASE(polynomialPointOperations, * boost::unit_test::tolerance(0.001)) {
polynomial_t::coeff_t coeffs1 = Eigen::MatrixXd::Random(3,5); 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 p1(coeffs1,0.,1.);
polynomial_t pSum = p1 + point; polynomial_t pSum = p1 + point;
polynomial_t pSumR = point + p1; polynomial_t pSumR = point + p1;
polynomial_t pSub = p1 - point; polynomial_t pSub = p1 - point;
polynomial_t pSubR = point - p1; polynomial_t pSubR = point - p1;
polynomial_t pcross = p1.cross(point);
for (double i = 0.; i <=100.; ++i ){ for (double i = 0.; i <=100.; ++i ){
double dt = i / 100.; double dt = i / 100.;
BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.); BOOST_TEST(( pSum(dt) - (p1(dt)+point)).norm()==0.);
BOOST_TEST((pSumR(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(( pSub(dt) - (p1(dt)-point)).norm()==0.);
BOOST_TEST((pSubR(dt) - (point-p1(dt))).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 ...@@ -272,6 +277,9 @@ BOOST_AUTO_TEST_CASE(crossPoductPolynomials, * boost::unit_test::tolerance(0.001
polynomial_t p5(coeffs2,0.1,.5); polynomial_t p5(coeffs2,0.1,.5);
polynomial_t pDim4(coeffsDim4,0.,1.); 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(p3), std::exception );
BOOST_CHECK_THROW( p1.cross(p4), std::exception ); BOOST_CHECK_THROW( p1.cross(p4), std::exception );
BOOST_CHECK_THROW( p1.cross(p5), std::exception ); BOOST_CHECK_THROW( p1.cross(p5), std::exception );
......
Markdown is supported
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