Commit 180e65b9 by Steve T

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

parent 70ff0294
 ... ... @@ -478,6 +478,25 @@ struct bezier_curve : public curve_abc { 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 ... ...
 ... ... @@ -471,7 +471,31 @@ struct polynomial : public curve_abc { } // 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()); ... ...
 ... ... @@ -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_, boost::shared_ptr >("bezier3", init<>()) .def("__init__", make_constructor(&wrapBezier3Constructor)) .def("__init__", make_constructor(&wrapBezier3ConstructorBounds)) ... ... @@ -651,7 +653,8 @@ BOOST_PYTHON_MODULE(curves) { //.def(SerializableVisitor()) .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 + bezier3_t()) ... ... @@ -668,6 +671,8 @@ BOOST_PYTHON_MODULE(curves) { .def_pickle(curve_pickle_suite()); /** 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_, boost::shared_ptr >("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_, boost::shared_ptr >( "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_, boost::shared_ptr >("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, 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()) ... ...
 ... ... @@ -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()) ... ...
 ... ... @@ -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 ); ... ...
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