Unverified Commit 991a37b3 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #52 from stonneau/devel

arithmetic operations on curves
parents 48f7bd66 98647d94
......@@ -35,5 +35,20 @@ void PseudoInverse(_Matrix_Type_& pinvmat) {
}
pinvmat = (svd.matrixV() * m_sigma_inv * svd.matrixU().transpose());
}
template<typename Matrix3, typename Point >
Matrix3 skew(const Point& x) {
Matrix3 res = Matrix3::Zero(3,3);
res(0, 1) = -x(2);
res(0, 2) = x(1);
res(1, 0) = x(2);
res(1, 2) = -x(0);
res(2, 0) = -x(1);
res(2, 1) = x(0);
return res;
}
static const double MARGIN(0.001);
} // namespace curves
#endif //_SPLINEMATH
......@@ -10,6 +10,7 @@
#define _CLASS_BEZIERCURVE
#include "curve_abc.h"
#include "cross_implementation.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "piecewise_curve.h"
......@@ -215,7 +216,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
if (order == 0) {
return *this;
}
num_t new_degree = (num_t)(degree_ + 1);
num_t new_degree_inv = 1. / ((num_t)(degree_ + 1));
t_point_t n_wp;
point_t current_sum = point_t::Zero(dim_);
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
......@@ -223,12 +224,42 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
n_wp.push_back(current_sum);
for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit) {
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
n_wp.push_back(current_sum * new_degree_inv);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_ * (T_max_ - T_min_));
return integ.compute_primitive(order - 1);
}
/// \brief Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent.
/// Order elevation is required for addition / substraction and other comparison operations.
/// \param order : number of order the curve must be updated
/// \return An equivalent Bezier, with one more degree.
bezier_curve_t elevate(const std::size_t order) const {
t_point_t new_waypoints = control_points_, temp_waypoints;
for (std::size_t i = 1; i<= order; ++i)
{
num_t new_degree_inv = 1. / ((num_t)(degree_ + i));
temp_waypoints.push_back(*new_waypoints.begin());
num_t idx_deg_inv = 0.;
for (typename t_point_t::const_iterator pit = new_waypoints.begin()+1; pit != new_waypoints.end(); ++pit) {
idx_deg_inv += new_degree_inv;
temp_waypoints.push_back(idx_deg_inv * (*(pit-1)) + (1 - idx_deg_inv) * (*pit));
}
temp_waypoints.push_back(*(new_waypoints.end()-1));
new_waypoints = temp_waypoints;
temp_waypoints.clear();
}
return bezier_curve_t (new_waypoints.begin(), new_waypoints.end(), T_min_, T_max_, mult_T_);
}
/// \brief Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent.
/// Order elevation is required for addition / substraction and other comparison operations.
/// \param order : number of order the curve must be updated
void elevate_self(const std::size_t order) {
if (order > 0)
(*this) = elevate(order);
}
/// \brief Evaluate the derivative order N of curve at time t.
/// If derivative is to be evaluated several times, it is
/// rather recommended to compute derived curve using compute_derivate.
......@@ -415,6 +446,118 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
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_);
}
/// \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
if(other.degree() > degree()){
elevate_self(other.degree() - degree());
}
else if(other_elevated.degree() < degree()){
other_elevated.elevate_self(degree() - other_elevated.degree());
}
typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
(*it)+=(*otherit);
}
return *this;
}
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_);
if(other.degree() > degree()){
elevate_self(other.degree() - degree());
}
else if(other_elevated.degree() < degree()){
other_elevated.elevate_self(degree() - other_elevated.degree());
}
typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
(*it)-=(*otherit);
}
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;
}
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;
}
return *this;
}
private:
/// \brief Ensure constraints of bezier curve.
/// Add 4 points (2 after the first one, 2 before the last one) to biezer curve
......@@ -453,6 +596,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
"Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
}
}
void assert_operator_compatible(const bezier_curve_t& other) const{
if ((fabs(min() - other.min()) > MARGIN) || (fabs(max() - other.max()) > MARGIN)){
throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges");
}
}
/*Operations*/
public:
......@@ -483,7 +634,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/*const*/ std::size_t degree_;
/*const*/ std::vector<Bern<Numeric> > bernstein_;
/*const*/ t_point_t control_points_;
static const double MARGIN;
/* Attributes */
static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) {
......@@ -512,8 +662,71 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
}; // End struct bezier_curve
template <typename Time, typename Numeric, bool Safe, typename Point>
const double bezier_curve<Time, Numeric, Safe, Point>::MARGIN(0.001);
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const bezier_curve<T,N,S,P>& p2) {
bezier_curve<T,N,S,P> res(p1);
return res+=p2;
}
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 (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());
res-=p1;
return res;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const bezier_curve<T,N,S,P>& p2) {
bezier_curve<T,N,S,P> res(p1);
return res-=p2;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const typename bezier_curve<T,N,S,P>::point_t& point) {
bezier_curve<T,N,S,P> res(p1);
return res-=point;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const typename bezier_curve<T,N,S,P>::point_t& point, const bezier_curve<T,N,S,P>& p1) {
bezier_curve<T,N,S,P> res(-p1);
return res+=point;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const typename bezier_curve<T,N,S,P>::point_t& point) {
bezier_curve<T,N,S,P> res(p1);
return res+=point;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator+(const typename bezier_curve<T,N,S,P>::point_t& point, const bezier_curve<T,N,S,P>& p1) {
bezier_curve<T,N,S,P> res(p1);
return res+=point;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator/(const bezier_curve<T,N,S,P>& p1, const double k) {
bezier_curve<T,N,S,P> res(p1);
return res/=k;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator*(const bezier_curve<T,N,S,P>& p1,const double k) {
bezier_curve<T,N,S,P> res(p1);
return res*=k;
}
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator*(const double k, const bezier_curve<T,N,S,P>& p1) {
bezier_curve<T,N,S,P> res(p1);
return res*=k;
}
} // namespace curves
......
/**
* \file cross_implementation.h
* \brief class allowing to create a cubic hermite spline of any dimension.
* \author Steve Tonneau
* \date 09/2020
*/
#ifndef _CLASS_CROSSIMP
#define _CLASS_CROSSIMP
#include "curves/fwd.h"
namespace curves {
inline
Eigen::Vector3d cross(const Eigen::VectorXd& a, const Eigen::VectorXd& b){
Eigen::Vector3d c;
c << a[1]*b[2] - a[2]*b[1], a[2]*b[0] - a[0]*b[2], a[0]*b[1] - a[1]*b[0] ;
return c;
}
inline
Eigen::Vector3d cross(const Eigen::Vector3d& a, const Eigen::Vector3d& b){
return a.cross(b);
}
inline
Eigen::Vector3f cross(const Eigen::Vector3f& a, const Eigen::Vector3f& b){
return a.cross(b);
}
template<typename N, bool S>
linear_variable<N,S> cross(const linear_variable<N,S>& a, const linear_variable<N,S>& b){
return a.cross(b);
}
} // namespace curves
#endif //_CLASS_CROSSIMP
......@@ -311,7 +311,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
public:
/// \brief Get dimension of curve.
/// \return dimension of curve.
std::size_t virtual dim() const { return dim_; };
std::size_t virtual dim() const { return dim_; }
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
Time virtual min() const { return time_control_points_.front(); }
......
......@@ -87,6 +87,8 @@ typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr_t;
typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
typedef exact_cubic<double, double, true, pointX_t, t_pointX_t, polynomial_t> exact_cubic_t;
typedef bezier_curve<double, double, true, pointX_t> bezier_t;
typedef linear_variable<double, true> linear_variable_t;
typedef bezier_curve<double, double, true, linear_variable_t> bezier_linear_variable_t;
typedef constant_curve<double, double, true, pointX_t, pointX_t> constant_t;
typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
typedef piecewise_curve<double, double, true, pointX_t, pointX_t, curve_abc_t> piecewise_t;
......
......@@ -26,11 +26,14 @@ template <typename Numeric = double, bool Safe = true>
struct linear_variable : public serialization::Serializable {
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
typedef Eigen::Matrix<Numeric, 3,1> vector_3_t;
typedef Eigen::Matrix<Numeric, 3,3> matrix_3_t;
typedef linear_variable<Numeric> linear_variable_t;
linear_variable() : B_(matrix_x_t::Identity(0, 0)), c_(vector_x_t::Zero(0)), zero(true) {} // variable
linear_variable(const vector_x_t& c) : B_(matrix_x_t::Zero(c.size(), c.size())), c_(c), zero(false) {} // constant
linear_variable(const matrix_x_t& B, const vector_x_t& c) : B_(B), c_(c), zero(false) {} // mixed
linear_variable(const linear_variable_t& other) : B_(other.B()), c_(other.c()), zero(other.isZero()) {} // copy constructor
/// \brief Linear evaluation for vector x.
/// \param val : vector to evaluate the linear variable.
......@@ -95,14 +98,45 @@ struct linear_variable : public serialization::Serializable {
return *this;
}
/// \brief Compute the cross product of the current linear_variable and the other.
/// This method of course only makes sense for dimension 3 curves and dimension 3 unknown,
/// since otherwise the result is non-linear.
/// 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 other
linear_variable_t cross(const linear_variable_t& other) const {
if (B().rows() !=3)
throw std::invalid_argument("Can't perform cross product on linear variables with dimensions != 3 ");
if (B().cols() !=3)
throw std::invalid_argument("Can't perform cross product on linear variables more than one unknown ");
if (isZero() || other.isZero())
return linear_variable_t::Zero(3);
if ((B().squaredNorm() - B().diagonal().squaredNorm() > MARGIN ) || (other.B().squaredNorm() - other.B().diagonal().squaredNorm() > MARGIN ) )
throw std::invalid_argument("Can't perform cross product on linear variables if B is not diagonal ");
// (B1 x + c1) X (B2 x + c2) = (-c2X B1) x + (bX B2) x + b1Xb2
typename linear_variable_t::matrix_3_t newB = skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(-other.c()) * B() +
skew<typename linear_variable_t::matrix_3_t, typename linear_variable_t::vector_3_t>(c()) * other.B();
typename linear_variable_t::vector_3_t newC = curves::cross(c(),other.c());
return linear_variable_t(newB,newC);
}
/// \brief Get a linear variable equal to zero.
/// \param dim : Dimension of linear variable.
/// \return Linear variable equal to zero.
///
static linear_variable_t Zero(size_t dim = 0) {
return linear_variable_t(matrix_x_t::Zero(dim, dim), vector_x_t::Zero(dim));
}
/// \brief Get a linear variable equal to the variable
/// \param dim : Dimension of linear variable.
/// \return Linear variable equal to the variable.
///
static linear_variable_t X(size_t dim = 0) {
return linear_variable_t(matrix_x_t::Identity(dim, dim), vector_x_t::Zero(dim));
}
/// \brief Get dimension of linear variable.
/// \return Dimension of linear variable.
///
......@@ -156,6 +190,11 @@ linear_variable<N, S> operator-(const linear_variable<N, S>& w1, const linear_va
return res -= w2;
}
template <typename N, bool S>
linear_variable<N, S> operator-(const linear_variable<N, S>& w1) {
return linear_variable<N, S> (-w1.B(), -w1.c());
}
template <typename N, bool S>
linear_variable<N, S> operator*(const double k, const linear_variable<N, S>& w) {
linear_variable<N, S> res(w.B(), w.c());
......@@ -182,6 +221,11 @@ BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) {
return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_, bIn.T_max_);
}
template <typename N, bool S>
std::ostream &operator<<(std::ostream &os, const linear_variable<N, S>& l) {
return os << "linear_variable: \n \t B:\n"<< l.B() << "\t c: \n" << l.c().transpose();
}
} // namespace curves
DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Numeric, bool Safe),
......
......@@ -130,7 +130,7 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
}
const std::size_t first_variable_idx = i;
// variables
for (; i + 4 < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
for (; i + 4 < numControlPoints; ++i) variables_.push_back(var_t::X(pDef.dim_));
// end constraints
if (flag & END_POS) {
if (flag & END_VEL) {
......@@ -147,7 +147,7 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
++i;
} else
while (i < numControlPoints - 3) {
variables_.push_back(var_t::Zero(pDef.dim_));
variables_.push_back(var_t::X(pDef.dim_));
++i;
}
variables_.push_back(var_t(acc));
......@@ -155,7 +155,7 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
++i;
} else
while (i < numControlPoints - 2) {
variables_.push_back(var_t::Zero(pDef.dim_));
variables_.push_back(var_t::X(pDef.dim_));
++i;
}
variables_.push_back(var_t(vel));
......@@ -163,7 +163,7 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
++i;
} else {
while (i < numControlPoints - 1) {
variables_.push_back(var_t::Zero(pDef.dim_));
variables_.push_back(var_t::X(pDef.dim_));
++i;
}
}
......@@ -172,7 +172,7 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
++i;
}
// add remaining variables (only if no end_pos constraints)
for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
for (; i < numControlPoints; ++i) variables_.push_back(var_t::X(pDef.dim_));
if (numControlPoints <= numConstants) {
throw std::runtime_error("numControlPoints < numConstants");
......
......@@ -582,7 +582,6 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
std::size_t size_; // Number of segments in piecewise curve = size of curves_
Time T_min_, T_max_;
static const double MARGIN;
/* Attributes */
// Serialization of the class
......@@ -602,10 +601,6 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
ar& boost::serialization::make_nvp("T_max", T_max_);
}
}; // End struct piecewise curve
template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate, typename CurveType>
const double piecewise_curve<Time, Numeric, Safe, Point, Point_derivate, CurveType>::MARGIN(0.001);
} // namespace curves
DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
......
......@@ -400,6 +400,107 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
virtual std::size_t degree() const { return degree_; }
/*Helpers*/
polynomial_t& operator+=(const polynomial_t& p1) {
assert_operator_compatible(p1);
if (p1.degree() > degree()) {
polynomial_t::coeff_t res = p1.coeff();
res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_;
coefficients_ = res;
degree_ = p1.degree();
}
else{
coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) += p1.coeff();
}
return *this;
}
polynomial_t& operator-=(const polynomial_t& p1) {
assert_operator_compatible(p1);
if (p1.degree() > degree()) {
polynomial_t::coeff_t res = -p1.coeff();
res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_;
coefficients_ = res;
degree_ = p1.degree();
}
else{
coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) -= p1.coeff();
}
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;
}
polynomial_t& operator*=(const double d) {
coefficients_ *= d;
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 on polynomials with dimensions != 3 ");
std::size_t new_degree =degree() + pOther.degree();
coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,new_degree+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 final_degree = new_degree;
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.