Commit 9b6a391d authored by Steve T's avatar Steve T
Browse files

added cross product for bezier

parent 518f2535
......@@ -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"
......@@ -447,6 +448,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
bezier_curve_t& operator+=(const bezier_curve_t& other) {
assert_operator_compatible(other);
if(fabs(mult_T_ - other.mult_T_) > bezier_curve_t::MARGIN)
throw std::runtime_error("addition not implemented yet for curves of different mult");
bezier_curve_t other_elevated = other;
if(other.degree() > degree()){
elevate_self(other.degree() - degree());
......@@ -463,6 +466,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
bezier_curve_t& operator-=(const bezier_curve_t& other) {
assert_operator_compatible(other);
if(fabs(mult_T_ - other.mult_T_) > bezier_curve_t::MARGIN)
throw std::runtime_error("addition not implemented yet for curves of different mult");
bezier_curve_t other_elevated = other;
if(other.degree() > degree()){
elevate_self(other.degree() - degree());
......@@ -491,6 +496,38 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return *this;
}
/// \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 polynomials with dimensions != 3 ");
int m =(int)(degree());
int n =(int)(g.degree());
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(3);
for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){
unsigned int mj = bin(m,j);
unsigned int n_ij = bin(n,i-j);
unsigned int mn_i = bin(m+n,i);
num_t mul = num_t(mj*n_ij) / num_t(mn_i);
current_point += mul*curves::cross<bezier_curve_t::point_t>(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
......@@ -531,9 +568,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
void assert_operator_compatible(const bezier_curve_t& other){
if ((fabs(min() - other.min()) > bezier_curve_t::MARGIN) || (fabs(max() - other.max()) > bezier_curve_t::MARGIN) || (fabs(mult_T_ - other.mult_T_) > bezier_curve_t::MARGIN)){
throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges or mult");
void assert_operator_compatible(const bezier_curve_t& other) const{
if ((fabs(min() - other.min()) > bezier_curve_t::MARGIN) || (fabs(max() - other.max()) > bezier_curve_t::MARGIN)){
throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges");
}
}
......
/**
* \file cubic_hermite_spline.h
* \brief class allowing to create a cubic hermite spline of any dimension.
* \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin <jchemin@laas.fr>
* \date 05/2019
*/
#ifndef _CLASS_CROSSIMP
#define _CLASS_CROSSIMP
#include "curves/fwd.h"
namespace curves {
template<typename Point>
Eigen::Vector3d cross(const Point& a, const Point& 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;
}
} // namespace curves
#endif //_CLASS_CROSSIMP
......@@ -449,8 +449,8 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
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);
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){
......@@ -461,11 +461,11 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
}
}
// remove last degrees is they are equal to 0
long finalDegree = nDegree;
while(nCoeffs.col(finalDegree).norm() <= polynomial_t::MARGIN){
--finalDegree;
long final_degree = new_degree;
while(nCoeffs.col(final_degree).norm() <= polynomial_t::MARGIN){
--final_degree;
}
return polynomial_t(nCoeffs.leftCols(finalDegree+1), min(), max());
return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max());
}
/*Attributes*/
......
......@@ -651,6 +651,7 @@ BOOST_PYTHON_MODULE(curves) {
//.def(SerializableVisitor<bezier_t>())
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("cross", &bezier3_t::cross, bp::args("other"), "Compute the cross product of the current bezier by another bezier. 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 curves.")
.def(self += bezier3_t())
.def(self -= bezier3_t())
.def(self *= double())
......@@ -688,6 +689,7 @@ BOOST_PYTHON_MODULE(curves) {
"Loads *this from a binary file.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("cross", &bezier_t::cross, bp::args("other"), "Compute the cross product of the current bezier by another bezier. 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 curves.")
.def(self += bezier_t())
.def(self -= bezier_t())
.def(self *= double())
......
......@@ -77,7 +77,9 @@ class TestCurves(unittest.TestCase):
a1*=0.1
a1/=0.1
b = -a1
c = a.cross(b)
c(0)
# self.assertTrue((a.waypoints == waypoints).all())
# Test : Degree, min, max, derivate
# self.print_str(("test 1")
......@@ -191,6 +193,8 @@ class TestCurves(unittest.TestCase):
a1*=0.1
a1/=0.1
b = -a1
c = a.cross(b)
c(0)
# Test waypoints
self.assertTrue(a.nbWaypoints == 2)
......
......@@ -20,6 +20,34 @@ using namespace curves;
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
BOOST_AUTO_TEST_CASE(crossPoductBezier, * boost::unit_test::tolerance(0.001)) {
t_pointX_t vec1;
t_pointX_t vec2;
for (int i =0; i<4; ++i)
{
vec1.push_back(Eigen::Vector3d::Random());
vec2.push_back(Eigen::Vector3d::Random());
}
for (int i =0; i<3; ++i)
{
vec1.push_back(Eigen::Vector3d::Random());
}
bezier_t p1(vec1.begin(),vec1.end(),0.,1.);
bezier_t p2(vec2.begin(),vec2.end(),0.,1.);
bezier_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(bezierOperations, * boost::unit_test::tolerance(0.001)) {
t_pointX_t vec1;
t_pointX_t vec2;
......@@ -79,6 +107,9 @@ BOOST_AUTO_TEST_CASE(bezierOperations, * boost::unit_test::tolerance(0.001)) {
}
}
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);
......@@ -129,8 +160,6 @@ 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);
......@@ -157,7 +186,6 @@ BOOST_AUTO_TEST_CASE(crossPoductPolynomials, * boost::unit_test::tolerance(0.001
}
}
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);
......
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