Skip to content

Commit c901379

Browse files
jorisvjorisv
committed
spline: Add deBoor algorithm to evaluate the BSpline
1 parent 5f8ef41 commit c901379

2 files changed

Lines changed: 189 additions & 0 deletions

File tree

‎include/pinocchio/src/multibody/joint/spline-utils.hxx‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -269,6 +269,50 @@ namespace pinocchio
269269

270270
return knots;
271271
}
272+
273+
/** De Boor algorithm implementation.
274+
* \p start_i Knot vector index that contains x
275+
* \p degree Curve degree
276+
* \p x Value to evaluate
277+
* \p knots Knot vector of size m
278+
* \p control_points control points of size m - degree - 1
279+
* \p workspace of size degree + 1
280+
* \return BSpline value at x
281+
*/
282+
template<typename Scalar>
283+
Scalar deBoorBasis(
284+
size_t start_i,
285+
size_t degree,
286+
Scalar x,
287+
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> & knots,
288+
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> & control_points,
289+
Eigen::Matrix<Scalar, Eigen::Dynamic, 1> & workspace)
290+
{
291+
assert(workspace.size() == degree + 1);
292+
assert(knots.size() == control_points.size() + degree + 1);
293+
assert(degree <= start_i);
294+
assert(start_i < knots.size() - 1 - degree);
295+
assert(knots[degree] <= x);
296+
assert(x <= knots[knots.size() - 1 - degree]);
297+
298+
for (int i = 0; i < static_cast<int>(degree) + 1; ++i)
299+
{
300+
workspace[i] = control_points[i + start_i - degree];
301+
}
302+
for (int r = 1; r < static_cast<int>(degree) + 1; ++r)
303+
{
304+
const int current_degree = static_cast<int>(degree) - r;
305+
for (int i = static_cast<int>(degree); i > r - 1; --i)
306+
{
307+
const int current_knot = i + static_cast<int>(start_i) - static_cast<int>(degree);
308+
const int last_knot = current_knot + current_degree + 1;
309+
Scalar alpha = (x - knots[current_knot]) / (knots[last_knot] - knots[current_knot]);
310+
workspace[i] = (1. - alpha) * workspace[i - 1] + alpha * workspace[i];
311+
}
312+
}
313+
return workspace[degree];
314+
}
315+
272316
} // namespace internal
273317

274318
} // namespace pinocchio

‎unittest/joint-spline.cpp‎

Lines changed: 145 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -758,4 +758,149 @@ BOOST_AUTO_TEST_CASE(vsFiniteDifference)
758758
}
759759
}
760760

761+
// Test deBoorBasis with degree 0.
762+
BOOST_AUTO_TEST_CASE(deBoorBasis_degree0)
763+
{
764+
using pinocchio::internal::deBoorBasis;
765+
766+
const size_t degree = 0;
767+
Eigen::VectorXd knots(3);
768+
knots << 0., 2., 3.;
769+
Eigen::VectorXd control_points(2);
770+
control_points << 5., 0.;
771+
Eigen::VectorXd workspace(1);
772+
773+
BOOST_CHECK_SMALL(deBoorBasis(0, degree, 0., knots, control_points, workspace) - 5., 1e-8);
774+
BOOST_CHECK_SMALL(deBoorBasis(0, degree, 1., knots, control_points, workspace) - 5., 1e-8);
775+
BOOST_CHECK_SMALL(deBoorBasis(0, degree, 1.5, knots, control_points, workspace) - 5., 1e-8);
776+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 2., knots, control_points, workspace) - 0., 1e-8);
777+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 2.5, knots, control_points, workspace) - 0., 1e-8);
778+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 3., knots, control_points, workspace) - 0., 1e-8);
779+
780+
control_points << 0., 7.;
781+
BOOST_CHECK_SMALL(deBoorBasis(0, degree, 0., knots, control_points, workspace) - 0., 1e-8);
782+
BOOST_CHECK_SMALL(deBoorBasis(0, degree, 1., knots, control_points, workspace) - 0., 1e-8);
783+
BOOST_CHECK_SMALL(deBoorBasis(0, degree, 1.5, knots, control_points, workspace) - 0., 1e-8);
784+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 2., knots, control_points, workspace) - 7., 1e-8);
785+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 2.5, knots, control_points, workspace) - 7., 1e-8);
786+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 3., knots, control_points, workspace) - 7., 1e-8);
787+
}
788+
789+
// Test deBoorBasis with degree 1.
790+
BOOST_AUTO_TEST_CASE(deBoorBasis_degree1)
791+
{
792+
using pinocchio::internal::deBoorBasis;
793+
794+
const size_t degree = 1;
795+
Eigen::VectorXd knots(5);
796+
knots << 0., 2., 3., 5., 5.5;
797+
Eigen::VectorXd control_points(3);
798+
control_points << 2., 0., 0.;
799+
Eigen::VectorXd workspace(2);
800+
801+
BOOST_CHECK_SMALL(
802+
deBoorBasis(1, degree, 2., knots, control_points, workspace) - (1. * 2. + 0. * 0.), 1e-8);
803+
BOOST_CHECK_SMALL(
804+
deBoorBasis(1, degree, 2.5, knots, control_points, workspace) - (0.5 * 2. + 0.5 * 0.), 1e-8);
805+
BOOST_CHECK_SMALL(
806+
deBoorBasis(1, degree, 3., knots, control_points, workspace) - (0. * 2. + 1. * 0.), 1e-8);
807+
BOOST_CHECK_SMALL(deBoorBasis(2, degree, 3.5, knots, control_points, workspace) - 0., 1e-8);
808+
BOOST_CHECK_SMALL(deBoorBasis(2, degree, 4., knots, control_points, workspace) - 0., 1e-8);
809+
BOOST_CHECK_SMALL(deBoorBasis(2, degree, 5., knots, control_points, workspace) - 0., 1e-8);
810+
811+
control_points << 0., 3., 0.;
812+
BOOST_CHECK_SMALL(
813+
deBoorBasis(1, degree, 2., knots, control_points, workspace) - (1. * 0. + 0. * 3.), 1e-8);
814+
BOOST_CHECK_SMALL(
815+
deBoorBasis(1, degree, 2.5, knots, control_points, workspace) - (0.5 * 0. + 0.5 * 3.), 1e-8);
816+
BOOST_CHECK_SMALL(
817+
deBoorBasis(1, degree, 3., knots, control_points, workspace) - (0. * 0. + 1. * 3.), 1e-8);
818+
BOOST_CHECK_SMALL(
819+
deBoorBasis(2, degree, 3.5, knots, control_points, workspace) - (0.75 * 3. + 0.25 * 0.), 1e-8);
820+
BOOST_CHECK_SMALL(
821+
deBoorBasis(2, degree, 4., knots, control_points, workspace) - (0.5 * 3. + .5 * 0.), 1e-8);
822+
BOOST_CHECK_SMALL(
823+
deBoorBasis(2, degree, 5., knots, control_points, workspace) - (0. * 3. + 1. * 0.), 1e-8);
824+
825+
control_points << 0., 0., 4.;
826+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 2., knots, control_points, workspace) - 0., 1e-8);
827+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 2.5, knots, control_points, workspace) - 0., 1e-8);
828+
BOOST_CHECK_SMALL(deBoorBasis(1, degree, 3., knots, control_points, workspace) - 0., 1e-8);
829+
BOOST_CHECK_SMALL(
830+
deBoorBasis(2, degree, 3.5, knots, control_points, workspace) - (0.75 * 0. + 0.25 * 4.), 1e-8);
831+
BOOST_CHECK_SMALL(
832+
deBoorBasis(2, degree, 4., knots, control_points, workspace) - (0.5 * 0. + .5 * 4.), 1e-8);
833+
BOOST_CHECK_SMALL(
834+
deBoorBasis(2, degree, 5., knots, control_points, workspace) - (0. * 0. + 1. * 4.), 1e-8);
835+
}
836+
837+
// test deBoorBasis with degree 3 against bsplineBasis.
838+
BOOST_AUTO_TEST_CASE(deBoorBasis_degree3)
839+
{
840+
using pinocchio::internal::bsplineBasis;
841+
using pinocchio::internal::deBoorBasis;
842+
843+
const size_t degree = 3;
844+
Eigen::VectorXd knots(8);
845+
knots << 0., 2., 3., 5., 5.5, 8., 8.5, 10.;
846+
Eigen::VectorXd control_points(4);
847+
control_points << 1., 0., 0., 0.;
848+
Eigen::VectorXd workspace(4);
849+
850+
BOOST_CHECK_SMALL(
851+
deBoorBasis(3, degree, 5., knots, control_points, workspace)
852+
- bsplineBasis(0, degree, 5., knots),
853+
1e-8);
854+
BOOST_CHECK_SMALL(
855+
deBoorBasis(3, degree, 5.3, knots, control_points, workspace)
856+
- bsplineBasis(0, degree, 5.3, knots),
857+
1e-8);
858+
BOOST_CHECK_SMALL(
859+
deBoorBasis(3, degree, 5.5, knots, control_points, workspace)
860+
- bsplineBasis(0, degree, 5.5, knots),
861+
1e-8);
862+
863+
control_points << 0., 1., 0., 0.;
864+
BOOST_CHECK_SMALL(
865+
deBoorBasis(3, degree, 5., knots, control_points, workspace)
866+
- bsplineBasis(1, degree, 5., knots),
867+
1e-8);
868+
BOOST_CHECK_SMALL(
869+
deBoorBasis(3, degree, 5.3, knots, control_points, workspace)
870+
- bsplineBasis(1, degree, 5.3, knots),
871+
1e-8);
872+
BOOST_CHECK_SMALL(
873+
deBoorBasis(3, degree, 5.5, knots, control_points, workspace)
874+
- bsplineBasis(1, degree, 5.5, knots),
875+
1e-8);
876+
877+
control_points << 0., 0., 1., 0.;
878+
BOOST_CHECK_SMALL(
879+
deBoorBasis(3, degree, 5., knots, control_points, workspace)
880+
- bsplineBasis(2, degree, 5., knots),
881+
1e-8);
882+
BOOST_CHECK_SMALL(
883+
deBoorBasis(3, degree, 5.3, knots, control_points, workspace)
884+
- bsplineBasis(2, degree, 5.3, knots),
885+
1e-8);
886+
BOOST_CHECK_SMALL(
887+
deBoorBasis(3, degree, 5.5, knots, control_points, workspace)
888+
- bsplineBasis(2, degree, 5.5, knots),
889+
1e-8);
890+
891+
control_points << 0., 0., 0., 1.;
892+
BOOST_CHECK_SMALL(
893+
deBoorBasis(3, degree, 5., knots, control_points, workspace)
894+
- bsplineBasis(3, degree, 5., knots),
895+
1e-8);
896+
BOOST_CHECK_SMALL(
897+
deBoorBasis(3, degree, 5.3, knots, control_points, workspace)
898+
- bsplineBasis(3, degree, 5.3, knots),
899+
1e-8);
900+
BOOST_CHECK_SMALL(
901+
deBoorBasis(3, degree, 5.5, knots, control_points, workspace)
902+
- bsplineBasis(3, degree, 5.5, knots),
903+
1e-8);
904+
}
905+
761906
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)