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 { ... @@ -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_); 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 { ... @@ -471,7 +471,31 @@ struct polynomial : public curve_abc { } } // 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()); ... ...
 ... @@ -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!