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()); ... ...