test_bezierQuad.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *  Copyright (C) 2018-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file test_bezierQuad.cpp
 * Test for Bezier curve computation.
 */

#include <unit_test.h>
#include <float.h>
#include <stdlib.h>
#include <time.h>

#include "../../lib/bezier/BezierQuad.hpp"

class BezierQuadTest : public UnitTest
{
public:
    virtual bool run_tests();

private:

    bool _get_states_from_time();
    bool _get_arc_length();
    bool _set_bez_from_vel();

    float random(float min, float max);

};


bool BezierQuadTest::run_tests()
{
    ut_run_test(_get_states_from_time);
    ut_run_test(_get_arc_length);
    ut_run_test(_set_bez_from_vel);

    return (_tests_failed == 0);
}

bool BezierQuadTest::_get_states_from_time()
{
    // symmetric around 0
    matrix::Vector3f pt0(-0.5f, 0.0f, 0.0f);
    matrix::Vector3f ctrl(0.0f, 0.5f, 0.0f);
    matrix::Vector3f pt1(0.5f, 0.0f, 0.0f);

    // create bezier with default t = [0,1]
    bezier::BezierQuad_f bz(pt0, ctrl, pt1);

    matrix::Vector3f pos, vel, acc;
    float precision = 0.00001;

    // states at time = 0
    bz.getStates(pos, vel, acc, 0.0f);

    ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision);
    ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision);
    ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision);

    ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
    ut_compare_float("slope not equal 1", vel(1), 1.0f, precision);
    ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);

    ut_compare_float("acc not equal 0", acc(0), 0.0f, precision);
    ut_compare_float("acc not equal 1", acc(1), -2.0f, precision);
    ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);

    // states at time = 1
    bz.getStates(pos, vel, acc, 1.0f);

    ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision);
    ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision);
    ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision);

    ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
    ut_compare_float("slope not equal -1", vel(1), -1.0f, precision);
    ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);

    ut_compare_float("acc not equal 0", acc(0), 0.0f, precision);
    ut_compare_float("acc not equal 1", acc(1), -2.0f, precision);
    ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);

    // states at time = 0.5
    bz.getStates(pos, vel, acc, 0.50f);

    // pos must be equal to ctrl(0) and lower than ctrl(1)
    ut_compare_float("pos[0] not equal ctrl[0]", pos(0), ctrl(0), precision);
    ut_assert_true(pos(1) < ctrl(1));

    ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
    ut_compare_float("slope not equal -1", vel(1), 0.0f, precision);
    ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);

    ut_compare_float("acc not equal 0", acc(0), 0.0f, precision);
    ut_compare_float("acc not equal -2", acc(1), -2.0f, precision);
    ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);

    // acceleration
    pt0 = matrix::Vector3f(0.0f, 0.0f, 0.0f);
    ctrl = matrix::Vector3f(0.0f, 0.0f, 0.0f);
    pt1 = matrix::Vector3f(1.0f, 0.0f, 0.0f);

    // create bezier with default t = [0,1]
    bz.setBezier(pt0, ctrl, pt1, 1.0f);

    // states at time = 0.0
    bz.getStates(pos, vel, acc, 0.0f);

    ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision);
    ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision);
    ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision);

    ut_compare_float("slope not equal 0", vel(0), 0.0f, precision);
    ut_compare_float("slope not equal 0", vel(1), 0.0f, precision);
    ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);

    ut_compare_float("acc not equal 2", acc(0), 2.0f, precision);
    ut_compare_float("acc not equal 0", acc(1), 0.0f, precision);
    ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);

    // states at time = 1.0
    bz.getStates(pos, vel, acc, 1.0f);

    ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision);
    ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision);
    ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision);

    ut_compare_float("slope not equal 2", vel(0), 2.0f, precision);
    ut_compare_float("slope not equal 0", vel(1), 0.0f, precision);
    ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);

    ut_compare_float("acc not equal 2", acc(0), 2.0f, precision);
    ut_compare_float("acc not equal 0", acc(1), 0.0f, precision);
    ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);

    // states at time = 0.5
    bz.getStates(pos, vel, acc, 0.5f);

    ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
    ut_compare_float("slope not equal 0", vel(1), 0.0f, precision);
    ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);

    ut_compare_float("acc not equal 2", acc(0), 2.0f, precision);
    ut_compare_float("acc not equal 0", acc(1), 0.0f, precision);
    ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);

    return true;

}

bool BezierQuadTest::_get_arc_length()
{
    // create random numbers
    srand(0); // choose a constant to make it deterministic

    float min = -50.f;
    float max = 50.f;
    float resolution = 0.1f;

    matrix::Vector3f pt0, pt1, ctrl;
    float duration, arc_length, triangle_length, straigth_length;
    float T = 100.0f;

    // loop trough different control points 100x and check if arc_length is in the expected range
    for (int i = 0; i < 100 ; i++) {
        // random bezier point
        pt0 = matrix::Vector3f(random(min, max), random(min, max), random(min, max));
        pt1 = matrix::Vector3f(random(min, max), random(min, max), random(min, max));
        ctrl = matrix::Vector3f(random(min, max), random(min, max), random(min, max));

        // use for each test a new duration
        duration = random(0.0f, T);

        // create bezier
        bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration);

        // compute arc length, triangle length and straigh length
        arc_length = bz.getArcLength(resolution);
        triangle_length = (ctrl - pt0).length() + (pt1 - ctrl).length();
        straigth_length = (pt1 - pt0).length();

        // we also compute length from going point to point and add segment
        float time_increment = duration / T;
        float t = 0.0f + time_increment;
        matrix::Vector3f p0 = pt0;
        float sum_segments = 0.0f;

        for (int s = 0; s < (int)T; s++) {
            matrix::Vector3f nextpt = bz.getPoint(t);
            sum_segments = (nextpt - p0).length() + sum_segments;
            p0 = bz.getPoint(t);
            t = t + time_increment;
        }

        // test comparisons
        ut_assert_true((triangle_length >= arc_length) && (arc_length >= straigth_length)
                   && (fabsf(arc_length - sum_segments) < 1.f));
    }


    return true;
}

bool BezierQuadTest::_set_bez_from_vel()
{
    // create random numbers
    srand(100); // choose a constant to make it deterministic

    float low = -50.0f;
    float max = 50.0f;
    float precision = 0.001f;

    for (int i = 0; i < 20; i++) {

        // set velocity
        matrix::Vector3f ctrl(random(low, max), random(low, max), random(low, max));
        matrix::Vector3f vel0(random(low, max), random(low, max), random(low, max));
        matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max));
        float duration = random(0.0f, 100.0f);

        bezier::BezierQuad_f bz;;
        bz.setBezFromVel(ctrl, vel0, vel1, duration);

        // get velocity back
        matrix::Vector3f v0 = bz.getVelocity(0.0f);
        matrix::Vector3f v1 = bz.getVelocity(duration);
        ut_compare_float("", vel0(0), v0(0), precision);
        ut_compare_float("", vel1(0), v1(0), precision);

        ut_compare_float("", vel0(1), v0(1), precision);
        ut_compare_float("", vel1(1), v1(1), precision);

        ut_compare_float("", vel0(2), v0(2), precision);
        ut_compare_float("", vel1(2), v1(2), precision);
    }

    return true;
}

float BezierQuadTest::random(float min, float max)
{
    float s = rand() / (float)RAND_MAX;
    return (min + s * (max - min));

}

ut_declare_test_c(test_bezierQuad, BezierQuadTest)