Commit 5b73cb39 authored by Steve T's avatar Steve T
Browse files

curve and point addition cpp

parent a5f47719
......@@ -446,6 +446,38 @@ 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_);
}
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<Time, Numeric, Safe, 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 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<Time, Numeric, Safe, Point> {
}
/// \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<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const bezier_cu
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);
......
......@@ -428,6 +428,16 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
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<T,N,S,P,TP> operator+(const polynomial<T,N,S,P,TP>& p1, const polynom
return res+=p2;
}
template <typename T, typename N, bool S, typename P, typename TP >
polynomial<T,N,S,P,TP> operator+(const polynomial<T,N,S,P,TP>& p1, const typename polynomial<T,N,S,P,TP>::point_t& point) {
polynomial<T,N,S,P,TP> res(p1);
return res+=point;
}
template <typename T, typename N, bool S, typename P, typename TP >
polynomial<T,N,S,P,TP> operator+(const typename polynomial<T,N,S,P,TP>::point_t& point, const polynomial<T,N,S,P,TP>& p1) {
polynomial<T,N,S,P,TP> res(p1);
return res+=point;
}
template <typename T, typename N, bool S, typename P, typename TP >
polynomial<T,N,S,P,TP> operator-(const polynomial<T,N,S,P,TP>& p1, const typename polynomial<T,N,S,P,TP>::point_t& point) {
polynomial<T,N,S,P,TP> res(p1);
return res-=point;
}
template <typename T, typename N, bool S, typename P, typename TP >
polynomial<T,N,S,P,TP> operator-(const typename polynomial<T,N,S,P,TP>::point_t& point, const polynomial<T,N,S,P,TP>& p1) {
polynomial<T,N,S,P,TP> res(-p1);
return res+=point;
}
template <typename T, typename N, bool S, typename P, typename TP >
polynomial<T,N,S,P,TP> operator-(const polynomial<T,N,S,P,TP>& p1) {
typename polynomial<T,N,S,P,TP>::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);
......
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