Commit 8b6561a2 authored by Steve T's avatar Steve T
Browse files

added cross product operator for polynomial

parent e4506a4d
......@@ -618,7 +618,7 @@ bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const bezier_cu
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1) {
std::vector<typename bezier_curve<T,N,S,P>::point_t> ts;
for (int i = 0; i <= p1.degree(); ++i){
for (std::size_t i = 0; i <= p1.degree(); ++i){
ts.push_back(bezier_curve<T,N,S,P>::point_t::Zero(p1.dim()));
}
bezier_curve<T,N,S,P> res (ts.begin(),ts.end(),p1.min(),p1.max());
......
......@@ -439,6 +439,35 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
return *this;
}
/// \brief 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.
/// \param pOther other polynomial to compute the cross product with.
/// \return a new polynomial defining the cross product between this and pother
polynomial_t cross(const polynomial_t& pOther) const {
assert_operator_compatible(pOther);
if (dim()!= 3)
throw std::invalid_argument("Can't perform cross product polynomials with dimensions != 3 ");
std::size_t nDegree =degree() + pOther.degree();
coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,nDegree+1);
Eigen::Vector3d currentVec;
Eigen::Vector3d currentVecCrossed;
for(long i = 0; i< coefficients_.cols(); ++i){
currentVec = coefficients_.col(i);
for(long j = 0; j< pOther.coeff().cols(); ++j){
currentVecCrossed = pOther.coeff().col(j);
nCoeffs.col(i+j) += currentVec.cross(currentVecCrossed);
}
}
// remove last degrees is they are equal to 0
long finalDegree = nDegree;
while(nCoeffs.col(finalDegree).norm() <= polynomial_t::MARGIN){
--finalDegree;
}
return polynomial_t(nCoeffs.leftCols(finalDegree+1), min(), max());
}
/*Attributes*/
std::size_t dim_; // const
coeff_t coefficients_; // const
......@@ -448,9 +477,9 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
private:
void assert_operator_compatible(const polynomial_t& other){
if ((fabs(min() - other.min()) > polynomial_t::MARGIN) || (fabs(max() - other.max()) > polynomial_t::MARGIN) ){
throw std::invalid_argument("Can't perform base operation (+ - ) on two polynomials with different time ranges");
void assert_operator_compatible(const polynomial_t& other) const{
if ((fabs(min() - other.min()) > polynomial_t::MARGIN) || (fabs(max() - other.max()) > polynomial_t::MARGIN) || dim() != other.dim()){
throw std::invalid_argument("Can't perform base operation (+ - ) on two polynomials with different time ranges or different dimensions");
}
}
......
......@@ -129,4 +129,50 @@ BOOST_AUTO_TEST_CASE(polynomialOperations, * boost::unit_test::tolerance(0.001))
}
}
BOOST_AUTO_TEST_CASE(crossPoductPolynomials, * 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);
polynomial_t::coeff_t coeffsDim4 = Eigen::MatrixXd::Random(4,2);
polynomial_t p1(coeffs1,0.,1.);
polynomial_t p2(coeffs2,0.,1.);
polynomial_t p3(coeffs2,0.,0.5);
polynomial_t p4(coeffs2,0.1,1.);
polynomial_t p5(coeffs2,0.1,.5);
polynomial_t pDim4(coeffsDim4,0.,1.);
BOOST_CHECK_THROW( p1.cross(p3), std::exception );
BOOST_CHECK_THROW( p1.cross(p4), std::exception );
BOOST_CHECK_THROW( p1.cross(p5), std::exception );
BOOST_CHECK_THROW( p1.cross(pDim4), std::exception );
BOOST_CHECK_THROW( pDim4.cross(p1), std::exception );
polynomial_t pCross = p1.cross(p2);
for (double i = 0.; i <=100.; ++i ){
double dt = i / 100.;
Eigen::Vector3d v1 = p1(dt);
Eigen::Vector3d v2 = p2(dt);
BOOST_TEST(( pCross(dt) - v1.cross(v2)).norm()==0.);
}
}
BOOST_AUTO_TEST_CASE(crossPoductPolynomialSimplification, * 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,3);
coeffs2.col(2) =coeffs1.col(4);
polynomial_t p1(coeffs1,0.,1.);
polynomial_t p2(coeffs2,0.,1.);
polynomial_t pCross = p1.cross(p2);
BOOST_CHECK_EQUAL (pCross.degree(), 5);
for (double i = 0.; i <=100.; ++i ){
double dt = i / 100.;
Eigen::Vector3d v1 = p1(dt);
Eigen::Vector3d v2 = p2(dt);
BOOST_TEST(( pCross(dt) - v1.cross(v2)).norm()==0.);
}
}
BOOST_AUTO_TEST_SUITE_END()
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