Commit 5b73cb39 by Steve T

### curve and point addition cpp

parent a5f47719
 ... ... @@ -446,6 +446,38 @@ struct bezier_curve : public curve_abc { return c_split.second.split(t2).first; } /// \brief Compute the cross product of the current bezier curve by another bezier curve. /// The cross product p1Xp2 of 2 bezier curves 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. /// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined /// \param pOther other polynomial to compute the cross product with. /// \return a new polynomial defining the cross product between this and pother bezier_curve_t cross(const bezier_curve_t& g) const { //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html assert_operator_compatible(g); if (dim()!= 3) throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 "); int m =(int)(degree()); int n =(int)(g.degree()); unsigned int mj, n_ij, mn_i; t_point_t new_waypoints; for(int i = 0; i<= m+n; ++i) { bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim()); for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){ mj = bin(m,j); n_ij = bin(n,i-j); mn_i = bin(m+n,i); num_t mul = num_t(mj*n_ij) / num_t(mn_i); current_point += mul*curves::cross(waypointAtIndex(j), g.waypointAtIndex(i-j)); } new_waypoints.push_back(current_point); } return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_ * g.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 ... ... @@ -478,6 +510,20 @@ struct bezier_curve : public curve_abc { return *this; } bezier_curve_t& operator+=(const bezier_curve_t::point_t& point) { for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){ (*it)+=point; } return *this; } bezier_curve_t& operator-=(const bezier_curve_t::point_t& point) { for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){ (*it)-=point; } return *this; } bezier_curve_t& operator/=(const double d) { for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){ (*it)/=d; ... ... @@ -493,38 +539,6 @@ struct bezier_curve : public curve_abc { } /// \brief Compute the cross product of the current bezier curve by another bezier curve. /// The cross product p1Xp2 of 2 bezier curves 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. /// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined /// \param pOther other polynomial to compute the cross product with. /// \return a new polynomial defining the cross product between this and pother bezier_curve_t cross(const bezier_curve_t& g) const { //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html assert_operator_compatible(g); if (dim()!= 3) throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 "); int m =(int)(degree()); int n =(int)(g.degree()); unsigned int mj, n_ij, mn_i; t_point_t new_waypoints; for(int i = 0; i<= m+n; ++i) { bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim()); for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){ mj = bin(m,j); n_ij = bin(n,i-j); mn_i = bin(m+n,i); num_t mul = num_t(mj*n_ij) / num_t(mn_i); current_point += mul*curves::cross(waypointAtIndex(j), g.waypointAtIndex(i-j)); } new_waypoints.push_back(current_point); } return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_ * g.mult_T_); } private: /// \brief Ensure constraints of bezier curve. /// Add 4 points (2 after the first one, 2 before the last one) to biezer curve ... ... @@ -652,6 +666,31 @@ bezier_curve operator-(const bezier_curve& p1, const bezier_cu return res-=p2; } template bezier_curve operator-(const bezier_curve& p1, const typename bezier_curve::point_t& point) { bezier_curve res(p1); return res-=point; } template bezier_curve operator-(const typename bezier_curve::point_t& point, const bezier_curve& p1) { bezier_curve res(-p1); return res+=point; } template bezier_curve operator+(const bezier_curve& p1, const typename bezier_curve::point_t& point) { bezier_curve res(p1); return res+=point; } template bezier_curve operator+(const typename bezier_curve::point_t& point, const bezier_curve& p1) { bezier_curve res(p1); return res+=point; } template bezier_curve operator/(const bezier_curve& p1, const double k) { bezier_curve res(p1); ... ...
 ... ... @@ -428,6 +428,16 @@ struct polynomial : public curve_abc { return *this; } polynomial_t& operator+=(const polynomial_t::point_t& point) { coefficients_.col(0) += point; return *this; } polynomial_t& operator-=(const polynomial_t::point_t& point) { coefficients_.col(0) -= point; return *this; } polynomial_t& operator/=(const double d) { coefficients_ /= d; return *this; ... ... @@ -519,6 +529,31 @@ polynomial operator+(const polynomial& p1, const polynom return res+=p2; } template polynomial operator+(const polynomial& p1, const typename polynomial::point_t& point) { polynomial res(p1); return res+=point; } template polynomial operator+(const typename polynomial::point_t& point, const polynomial& p1) { polynomial res(p1); return res+=point; } template polynomial operator-(const polynomial& p1, const typename polynomial::point_t& point) { polynomial res(p1); return res-=point; } template polynomial operator-(const typename polynomial::point_t& point, const polynomial& p1) { polynomial res(-p1); return res+=point; } template polynomial operator-(const polynomial& p1) { typename polynomial::coeff_t res = -p1.coeff(); ... ...
 ... ... @@ -116,6 +116,31 @@ BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) { } } BOOST_AUTO_TEST_CASE(bezierPointOperations, * boost::unit_test::tolerance(0.001)) { t_pointX_t vec1; for (int i =0; i<6; ++i) { vec1.push_back(Eigen::Vector3d::Random()); } 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); bezier_t pSum = p1 + point; bezier_t pSumR = point + p1; bezier_t pSub = p1 - point; bezier_t pSubR = point - p1; 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.); } } BOOST_AUTO_TEST_CASE(crossPoductLinearVariable, * boost::unit_test::tolerance(0.001)) { linear_variable_t l1(Eigen::Matrix3d::Identity() * 5., Eigen::Vector3d::Random()); ... ... @@ -168,6 +193,24 @@ 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); 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; 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.); } } BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001)) { polynomial_t::coeff_t coeffs1 = Eigen::MatrixXd::Random(3,5); polynomial_t::coeff_t coeffs2 = Eigen::MatrixXd::Random(3,2); ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!