FCL motion and continuous collision checking

fcl_continuous_collision_test.cpp

#include <iostream>
#include <fcl/shape/geometric_shapes.h>
#include <fcl/collision.h>
#include <fcl/continuous_collision.h>
#include <fcl/ccd/motion.h>

using namespace std;
using namespace fcl;

int main(int argc, char** argv)
{
    Transform3f tf0(Vec3f(1,1,1)), tf1(Vec3f(2,2,2));
    Transform3f tf;
    TranslationMotion tm = TranslationMotion(tf0, tf1);
    InterpMotion im = InterpMotion(tf0, tf1);
    ScrewMotion sm = ScrewMotion(tf0, tf1);

    std::cout << "-------------- TranslationMotion --------------" << std::endl;
    std::cout << "getCurrentTransform at motion initialization" << std::endl;
    tm.getCurrentTransform(tf);
    std::cout << "T = " << tf.getTranslation() << std::endl;
    std::cout << "R = " << tf.getRotation() << std::endl;
    std::cout << "q = " << tf.getQuatRotation() << std::endl;
    for (double t = 0.0; t <= 1.0; t = t + 0.2) {
        std::cout << "TranslationMotion interpolated at t = " << t << std::endl;
        tm.integrate(t);
        tm.getCurrentTransform(tf);
        std::cout << "T = " << tf.getTranslation() << std::endl;
        std::cout << "R = " << tf.getRotation() << std::endl;
        std::cout << "q = " << tf.getQuatRotation() << std::endl;
    }

    std::cout << "-------------- InterpMotion --------------" << std::endl;
    std::cout << "getCurrentTransform at motion initialization" << std::endl;
    im.getCurrentTransform(tf);
    std::cout << "T = " << tf.getTranslation() << std::endl;
    std::cout << "R = " << tf.getRotation() << std::endl;
    std::cout << "q = " << tf.getQuatRotation() << std::endl;
    for (double t = 0.0; t <= 1.0; t = t + 0.2) {
        std::cout << "InterpMotion interpolated at t = " << t << std::endl;
        im.integrate(t);
        im.getCurrentTransform(tf);
        std::cout << "T = " << tf.getTranslation() << std::endl;
        std::cout << "R = " << tf.getRotation() << std::endl;
        std::cout << "q = " << tf.getQuatRotation() << std::endl;
    }

    std::cout << "-------------- ScrewMotion --------------" << std::endl;
    std::cout << "getCurrentTransform at motion initialization" << std::endl;
    sm.getCurrentTransform(tf);
    std::cout << "T = " << tf.getTranslation() << std::endl;
    std::cout << "R = " << tf.getRotation() << std::endl;
    std::cout << "q = " << tf.getQuatRotation() << std::endl;
    for (double t = 0.0; t <= 1.0; t = t + 0.2) {
        std::cout << "ScrewMotion interpolated at t = " << t << std::endl;
        sm.integrate(t);
        sm.getCurrentTransform(tf);
        std::cout << "T = " << tf.getTranslation() << std::endl;
        std::cout << "R = " << tf.getRotation() << std::endl;
        std::cout << "q = " << tf.getQuatRotation() << std::endl;
    }

    std::cout << "-------------- Continuous Collision Failures --------------" << std::endl;
    ContinuousCollisionRequest request;
    request.ccd_solver_type = CCDC_CONSERVATIVE_ADVANCEMENT;
    request.ccd_motion_type = CCDM_TRANS;
    ContinuousCollisionResult result;

    std::shared_ptr<Box> box0(new Box(0.02,0.02,0.02));
    std::shared_ptr<Box> box1(new Box(0.03,0.38,0.98));
    tf0 = Transform3f(Vec3f(0.1,0.1,0.1));
    tf1 = Transform3f(Vec3f(0.275,0.2,0.5));
    Transform3f tf2 = Transform3f(Vec3f(0.416767, 0.332619, 0.552623));
    CollisionObject co0(box0, tf0);
    CollisionObject co1(box1, tf1);

    continuousCollide(&co0, tf2, &co1, tf1, request, result);
    std::cout << "CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_TRANS" << std::endl;
    std::cout << "box0 with side " << ((Box*)(co0.collisionGeometry().get()))->side << " from " << co0.getTranslation() << " to " << tf2.getTranslation() << std::endl;
    std::cout << "box1 with side " << box1->side << " from " << co1.getTranslation() << " to " << tf1.getTranslation() << std::endl;
    std::cout << "continuous collision result: " << result.is_collide << std::endl;

    result.is_collide = false;
    result.time_of_contact = 1.0;
    request.ccd_motion_type = CCDM_LINEAR;
    continuousCollide(&co0, tf2, &co1, tf1, request, result);
    std::cout << "CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_LINEAR" << std::endl;
    std::cout << "box0 with side " << box0->side << " from " << co0.getTranslation() << " to " << tf2.getTranslation() << std::endl;
    std::cout << "box1 with side " << box1->side << " from " << co1.getTranslation() << " to " << tf1.getTranslation() << std::endl;
    std::cout << "continuous collision result: " << result.is_collide << std::endl;

    result.is_collide = false;
    result.time_of_contact = 1.0;
    request.ccd_motion_type = CCDM_SCREW;
    continuousCollide(&co0, tf2, &co1, tf1, request, result);
    std::cout << "CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_SCREW" << std::endl;
    std::cout << "box0 with side " << box0->side << " from " << co0.getTranslation() << " to " << tf2.getTranslation() << std::endl;
    std::cout << "box1 with side " << box1->side << " from " << co1.getTranslation() << " to " << tf1.getTranslation() << std::endl;
    std::cout << "continuous collision result: " << result.is_collide << std::endl;

    std::cout << "Collision Verification" << std::endl;
    InterpMotion im0 = InterpMotion(co0.getTransform(), tf2);
    InterpMotion im1 = InterpMotion(co1.getTransform(), tf1);
    double toc = (.25 - .1) / (0.416767 - .1);
    Transform3f tf3, tf4;
    CollisionRequest req;
    CollisionResult res;

    im0.integrate(toc+.001);
    im1.integrate(toc+.001);
    im0.getCurrentTransform(tf3);
    im1.getCurrentTransform(tf4);
    collide(co0.collisionGeometry().get(), tf3, co1.collisionGeometry().get(), tf4, req, res);
    std::cout << "------" << std::endl << "collision result at time t = " << toc+.001 << ": " << res.isCollision() << std::endl;
    std::cout << "box0 with side " << box0->side << " at " << tf3.getTranslation() << std::endl;
    std::cout << "box1 with side " << box1->side << " at " << tf4.getTranslation() << std::endl;

    res.clear();
    im0.integrate(toc-.001);
    im1.integrate(toc-.001);
    im0.getCurrentTransform(tf3);
    im1.getCurrentTransform(tf4);
    collide(co0.collisionGeometry().get(), tf3, co1.collisionGeometry().get(), tf4, req, res);
    std::cout << "------" << std::endl << "collision result at time t = " << toc-.001 << ": " << res.isCollision() << std::endl;
    std::cout << "box0 with side " << box0->side << " at " << tf3.getTranslation() << std::endl;
    std::cout << "box1 with side " << box1->side << " at " << tf4.getTranslation() << std::endl;
    return 0;
}

// Output
/*
-------------- TranslationMotion --------------
getCurrentTransform at motion initialization
T = (0 0 0)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.2
T = (1.2 1.2 1.2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.4
T = (1.4 1.4 1.4)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.6
T = (1.6 1.6 1.6)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 0.8
T = (1.8 1.8 1.8)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
TranslationMotion interpolated at t = 1
T = (2 2 2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
-------------- InterpMotion --------------
getCurrentTransform at motion initialization
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.2
T = (1.2 1.2 1.2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.4
T = (1.4 1.4 1.4)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.6
T = (1.6 1.6 1.6)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 0.8
T = (1.8 1.8 1.8)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
InterpMotion interpolated at t = 1
T = (2 2 2)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
-------------- ScrewMotion --------------
getCurrentTransform at motion initialization
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0
T = (1 1 1)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.2
T = (1.34641 1.34641 1.34641)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.4
T = (1.69282 1.69282 1.69282)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.6
T = (2.03923 2.03923 2.03923)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 0.8
T = (2.38564 2.38564 2.38564)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
ScrewMotion interpolated at t = 1
T = (2.73205 2.73205 2.73205)
R = [(1 0 0)(0 1 0)(0 0 1)]
q = (1 0 0 0)
-------------- Continuous Collision Failures --------------
CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_TRANS
        box0 with side (0.02 0.02 0.02) from (0.1 0.1 0.1) to (0.416767 0.332619 0.552623)
box1 with side (0.03 0.38 0.98) from (0.275 0.2 0.5) to (0.275 0.2 0.5)
continuous collision result: 1
CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_LINEAR
        box0 with side (0.02 0.02 0.02) from (0.1 0.1 0.1) to (0.416767 0.332619 0.552623)
box1 with side (0.03 0.38 0.98) from (0.275 0.2 0.5) to (0.275 0.2 0.5)
continuous collision result: 0
CCDC_CONSERVATIVE_ADVANCEMENT, CCDM_SCREW
        box0 with side (0.02 0.02 0.02) from (0.1 0.1 0.1) to (0.416767 0.332619 0.552623)
box1 with side (0.03 0.38 0.98) from (0.275 0.2 0.5) to (0.275 0.2 0.5)
continuous collision result: 0
Collision Verification
------
collision result at time t = 0.474534: 1
box0 with side (0.02 0.02 0.02) at (0.250317 0.210386 0.314785)
box1 with side (0.03 0.38 0.98) at (0.275 0.2 0.5)
------
collision result at time t = 0.472534: 0
box0 with side (0.02 0.02 0.02) at (0.249683 0.20992 0.31388)
box1 with side (0.03 0.38 0.98) at (0.275 0.2 0.5)

Process finished with exit code 0
*/