traversal_node_bvhs.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * 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.
 *   * Neither the name of Willow Garage, Inc. 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.
 */

#ifndef FCL_TRAVERSAL_NODE_MESHES_H
#define FCL_TRAVERSAL_NODE_MESHES_H

#include "fcl/collision_data.h"
#include "fcl/traversal/traversal_node_base.h"
#include "fcl/BV/BV_node.h"
#include "fcl/BV/BV.h"
#include "fcl/BVH/BVH_model.h"
#include "fcl/intersect.h"
#include "fcl/ccd/motion.h"

#include <boost/shared_array.hpp>
#include <boost/shared_ptr.hpp>
#include <limits>
#include <vector>
#include <cassert>


namespace fcl
{

template<typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
  BVHCollisionTraversalNode() : CollisionTraversalNodeBase()
  {
    model1 = NULL;
    model2 = NULL;

    num_bv_tests = 0;
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
  }

  bool isFirstNodeLeaf(int b) const
  {
    return model1->getBV(b).isLeaf();
  }

  bool isSecondNodeLeaf(int b) const
  {
    return model2->getBV(b).isLeaf();
  }

  bool firstOverSecond(int b1, int b2) const
  {
    FCL_REAL sz1 = model1->getBV(b1).bv.size();
    FCL_REAL sz2 = model2->getBV(b2).bv.size();

    bool l1 = model1->getBV(b1).isLeaf();
    bool l2 = model2->getBV(b2).isLeaf();

    if(l2 || (!l1 && (sz1 > sz2)))
      return true;
    return false;
  }

  int getFirstLeftChild(int b) const
  {
    return model1->getBV(b).leftChild();
  }

  int getFirstRightChild(int b) const
  {
    return model1->getBV(b).rightChild();
  }

  int getSecondLeftChild(int b) const
  {
    return model2->getBV(b).leftChild();
  }

  int getSecondRightChild(int b) const
  {
    return model2->getBV(b).rightChild();
  }

  bool BVTesting(int b1, int b2) const
  {
    if(enable_statistics) num_bv_tests++;
    return !model1->getBV(b1).overlap(model2->getBV(b2));
  }
  
  const BVHModel<BV>* model1;
  const BVHModel<BV>* model2;

  mutable int num_bv_tests;
  mutable int num_leaf_tests;
  mutable FCL_REAL query_time_seconds;

};


template<typename BV>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
public:
  MeshCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
  {
    vertices1 = NULL;
    vertices2 = NULL;
    tri_indices1 = NULL;
    tri_indices2 = NULL;
  }

  void leafTesting(int b1, int b2) const
  {
    if(this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->model2->getBV(b2);

    int primitive_id1 = node1.primitiveId();
    int primitive_id2 = node2.primitiveId();

    const Triangle& tri_id1 = tri_indices1[primitive_id1];
    const Triangle& tri_id2 = tri_indices2[primitive_id2];

    const Vec3f& p1 = vertices1[tri_id1[0]];
    const Vec3f& p2 = vertices1[tri_id1[1]];
    const Vec3f& p3 = vertices1[tri_id1[2]];
    const Vec3f& q1 = vertices2[tri_id2[0]];
    const Vec3f& q2 = vertices2[tri_id2[1]];
    const Vec3f& q3 = vertices2[tri_id2[2]];
    
    if(this->model1->isOccupied() && this->model2->isOccupied())
    {
      bool is_intersect = false;

      if(!this->request.enable_contact) // only interested in collision or not
      {
        if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
        {
          is_intersect = true;
          if(this->result->numContacts() < this->request.num_max_contacts)
            this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2));
        }
      }
      else // need compute the contact information
      {
        FCL_REAL penetration;
        Vec3f normal;
        unsigned int n_contacts;
        Vec3f contacts[2];

        if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
                                         contacts,
                                         &n_contacts,
                                         &penetration,
                                         &normal))
        {
          is_intersect = true;
          
          if(this->request.num_max_contacts < n_contacts + this->result->numContacts())
            n_contacts = (this->request.num_max_contacts >= this->result->numContacts()) ? (this->request.num_max_contacts - this->result->numContacts()) : 0;
    
          for(unsigned int i = 0; i < n_contacts; ++i)
          {
            this->result->addContact(Contact(this->model1, this->model2, primitive_id1, primitive_id2, contacts[i], normal, penetration));
          }
        }
      }

      if(is_intersect && this->request.enable_cost)
      {
        AABB overlap_part;
        AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
        this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);      
      }
    }   
    else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
    {
      if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3))
      {
        AABB overlap_part;
        AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
        this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);      
      }
    }
  }

  bool canStop() const
  {
    return this->request.isSatisfied(*(this->result));
  }

  Vec3f* vertices1;
  Vec3f* vertices2;

  Triangle* tri_indices1;
  Triangle* tri_indices2;

  FCL_REAL cost_density;
};


class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
{
public:
  MeshCollisionTraversalNodeOBB();

  bool BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;

  void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;

  Matrix3f R;
  Vec3f T;
};

class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
{
public:
  MeshCollisionTraversalNodeRSS();

  bool BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;

  void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;

  Matrix3f R;
  Vec3f T;
};

class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
{
public:
  MeshCollisionTraversalNodekIOS();
 
  bool BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  Matrix3f R;
  Vec3f T;
};

class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
{
public:
  MeshCollisionTraversalNodeOBBRSS();
 
  bool BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  Matrix3f R;
  Vec3f T;
};

struct BVHContinuousCollisionPair
{
  BVHContinuousCollisionPair() {}

  BVHContinuousCollisionPair(int id1_, int id2_, FCL_REAL time) : id1(id1_), id2(id2_), collision_time(time) {}

  int id1;

  int id2;

  FCL_REAL collision_time;
};

template<typename BV>
class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
public:
  MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
  {
    vertices1 = NULL;
    vertices2 = NULL;
    tri_indices1 = NULL;
    tri_indices2 = NULL;
    prev_vertices1 = NULL;
    prev_vertices2 = NULL;

    num_vf_tests = 0;
    num_ee_tests = 0;
  }

  void leafTesting(int b1, int b2) const
  {
    if(this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->model2->getBV(b2);

    FCL_REAL collision_time = 2;
    Vec3f collision_pos;

    int primitive_id1 = node1.primitiveId();
    int primitive_id2 = node2.primitiveId();

    const Triangle& tri_id1 = tri_indices1[primitive_id1];
    const Triangle& tri_id2 = tri_indices2[primitive_id2];

    Vec3f* S0[3];
    Vec3f* S1[3];
    Vec3f* T0[3];
    Vec3f* T1[3];


    for(int i = 0; i < 3; ++i)
    {
      S0[i] = prev_vertices1 + tri_id1[i];
      S1[i] = vertices1 + tri_id1[i];
      T0[i] = prev_vertices2 + tri_id2[i];
      T1[i] = vertices2 + tri_id2[i];
    }

    FCL_REAL tmp;
    Vec3f tmpv;

    // 6 VF checks
    for(int i = 0; i < 3; ++i)
    {
      if(this->enable_statistics) num_vf_tests++;
      if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *(T0[i]), *(S1[0]), *(S1[1]), *(S1[2]), *(T1[i]), &tmp, &tmpv))
      {
        if(collision_time > tmp)
        {
          collision_time = tmp; collision_pos = tmpv;
        }
      }

      if(this->enable_statistics) num_vf_tests++;
      if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *(S0[i]), *(T1[0]), *(T1[1]), *(T1[2]), *(S1[i]), &tmp, &tmpv))
      {
        if(collision_time > tmp)
        {
          collision_time = tmp; collision_pos = tmpv;
        }
      }
    }

    // 9 EE checks
    for(int i = 0; i < 3; ++i)
    {
      int S_id1 = i;
      int S_id2 = i + 1;
      if(S_id2 == 3) S_id2 = 0;
      for(int j = 0; j < 3; ++j)
      {
        int T_id1 = j;
        int T_id2 = j + 1;
        if(T_id2 == 3) T_id2 = 0;

        num_ee_tests++;
        if(Intersect::intersect_EE(*(S0[S_id1]), *(S0[S_id2]), *(T0[T_id1]), *(T0[T_id2]), *(S1[S_id1]), *(S1[S_id2]), *(T1[T_id1]), *(T1[T_id2]), &tmp, &tmpv))
        {
          if(collision_time > tmp)
          {
            collision_time = tmp; collision_pos = tmpv;
          }
        }
      }
    }

    if(!(collision_time > 1)) // collision happens
      pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
  }

  bool canStop() const
  {
    return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
  }

  Vec3f* vertices1;
  Vec3f* vertices2;

  Triangle* tri_indices1;
  Triangle* tri_indices2;

  Vec3f* prev_vertices1;
  Vec3f* prev_vertices2;

  mutable int num_vf_tests;
  mutable int num_ee_tests;

  mutable std::vector<BVHContinuousCollisionPair> pairs;
};



template<typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
{
public:
  BVHDistanceTraversalNode() : DistanceTraversalNodeBase()
  {
    model1 = NULL;
    model2 = NULL;

    num_bv_tests = 0;
    num_leaf_tests = 0;
    query_time_seconds = 0.0;
  }

  bool isFirstNodeLeaf(int b) const
  {
    return model1->getBV(b).isLeaf();
  }

  bool isSecondNodeLeaf(int b) const
  {
    return model2->getBV(b).isLeaf();
  }

  bool firstOverSecond(int b1, int b2) const
  {
    FCL_REAL sz1 = model1->getBV(b1).bv.size();
    FCL_REAL sz2 = model2->getBV(b2).bv.size();

    bool l1 = model1->getBV(b1).isLeaf();
    bool l2 = model2->getBV(b2).isLeaf();

    if(l2 || (!l1 && (sz1 > sz2)))
      return true;
    return false;
  }

  int getFirstLeftChild(int b) const
  {
    return model1->getBV(b).leftChild();
  }

  int getFirstRightChild(int b) const
  {
    return model1->getBV(b).rightChild();
  }

  int getSecondLeftChild(int b) const
  {
    return model2->getBV(b).leftChild();
  }

  int getSecondRightChild(int b) const
  {
    return model2->getBV(b).rightChild();
  }

  FCL_REAL BVTesting(int b1, int b2) const
  {
    if(enable_statistics) num_bv_tests++;
    return model1->getBV(b1).distance(model2->getBV(b2));
  }

  const BVHModel<BV>* model1;
  const BVHModel<BV>* model2;

  mutable int num_bv_tests;
  mutable int num_leaf_tests;
  mutable FCL_REAL query_time_seconds;
};


template<typename BV>
class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
{
public:
  MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>()
  {
    vertices1 = NULL;
    vertices2 = NULL;
    tri_indices1 = NULL;
    tri_indices2 = NULL;

    rel_err = 0;
    abs_err = 0;
  }

  void leafTesting(int b1, int b2) const
  {
    if(this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->model2->getBV(b2);

    int primitive_id1 = node1.primitiveId();
    int primitive_id2 = node2.primitiveId();

    const Triangle& tri_id1 = tri_indices1[primitive_id1];
    const Triangle& tri_id2 = tri_indices2[primitive_id2];

    const Vec3f& t11 = vertices1[tri_id1[0]];
    const Vec3f& t12 = vertices1[tri_id1[1]];
    const Vec3f& t13 = vertices1[tri_id1[2]];

    const Vec3f& t21 = vertices2[tri_id2[0]];
    const Vec3f& t22 = vertices2[tri_id2[1]];
    const Vec3f& t23 = vertices2[tri_id2[2]];

    // nearest point pair
    Vec3f P1, P2;

    FCL_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
                                               P1, P2);

    if(this->request.enable_nearest_points)
    {
      this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2, P1, P2);
    }
    else
    {
      this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2);
    }
  }

  bool canStop(FCL_REAL c) const
  {
    if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
      return true;
    return false;
  }

  Vec3f* vertices1;
  Vec3f* vertices2;

  Triangle* tri_indices1;
  Triangle* tri_indices2;

  FCL_REAL rel_err;
  FCL_REAL abs_err;
};

class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode<RSS>
{
public:
  MeshDistanceTraversalNodeRSS();

  void preprocess();

  void postprocess();

  FCL_REAL BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  Matrix3f R;
  Vec3f T;
};


class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
{
public:
  MeshDistanceTraversalNodekIOS();

  void preprocess();
  
  void postprocess();

  FCL_REAL BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  Matrix3f R;
  Vec3f T;
};

class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode<OBBRSS>
{
public:
  MeshDistanceTraversalNodeOBBRSS();

  void preprocess();

  void postprocess();

  FCL_REAL BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  Matrix3f R;
  Vec3f T;
};


struct ConservativeAdvancementStackData
{
  ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}

  Vec3f P1;
  Vec3f P2;
  int c1;
  int c2;
  FCL_REAL d;
};

template<typename BV>
class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
{
public:
  MeshConservativeAdvancementTraversalNode(FCL_REAL w_ = 1) : MeshDistanceTraversalNode<BV>()
  {
    delta_t = 1;
    toc = 0;
    t_err = (FCL_REAL)0.00001;

    w = w_;

    motion1 = NULL;
    motion2 = NULL;
  }

  FCL_REAL BVTesting(int b1, int b2) const
  {
    if(this->enable_statistics) this->num_bv_tests++;
    Vec3f P1, P2;
    FCL_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);

    stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));

    return d;
  }

  void leafTesting(int b1, int b2) const
  {
    if(this->enable_statistics) this->num_leaf_tests++;

    const BVNode<BV>& node1 = this->model1->getBV(b1);
    const BVNode<BV>& node2 = this->model2->getBV(b2);

    int primitive_id1 = node1.primitiveId();
    int primitive_id2 = node2.primitiveId();

    const Triangle& tri_id1 = this->tri_indices1[primitive_id1];
    const Triangle& tri_id2 = this->tri_indices2[primitive_id2];

    const Vec3f& p1 = this->vertices1[tri_id1[0]];
    const Vec3f& p2 = this->vertices1[tri_id1[1]];
    const Vec3f& p3 = this->vertices1[tri_id1[2]];

    const Vec3f& q1 = this->vertices2[tri_id2[0]];
    const Vec3f& q2 = this->vertices2[tri_id2[1]];
    const Vec3f& q3 = this->vertices2[tri_id2[2]];

    // nearest point pair
    Vec3f P1, P2;

    FCL_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3,
                                               P1, P2);

    if(d < this->min_distance)
    {
      this->min_distance = d;

      this->p1 = P1;
      this->p2 = P2;

      this->last_tri_id1 = primitive_id1;
      this->last_tri_id2 = primitive_id2;
    }


    // n is the local frame of object 1
    Vec3f n = P2 - P1;
    // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
    FCL_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
    FCL_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);

    FCL_REAL bound = bound1 + bound2;

    FCL_REAL cur_delta_t;
    if(bound <= d) cur_delta_t = 1;
    else cur_delta_t = d / bound;

    if(cur_delta_t < delta_t)
      delta_t = cur_delta_t;
  }

  bool canStop(FCL_REAL c) const
  {
    if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
    {
      const ConservativeAdvancementStackData& data = stack.back();
      FCL_REAL d = data.d;
      Vec3f n;
      int c1, c2;

      if(d > c)
      {
        const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
        d = data2.d;
        n = data2.P2 - data2.P1;
        c1 = data2.c1;
        c2 = data2.c2;
        stack[stack.size() - 2] = stack[stack.size() - 1];
      }
      else
      {
        n = data.P2 - data.P1;
        c1 = data.c1;
        c2 = data.c2;
      }

      assert(c == d);

      FCL_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
      FCL_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);

      FCL_REAL bound = bound1 + bound2;

      FCL_REAL cur_delta_t;
      if(bound <= c) cur_delta_t = 1;
      else cur_delta_t = c / bound;

      if(cur_delta_t < delta_t)
        delta_t = cur_delta_t;

      stack.pop_back();

      return true;
    }
    else
    {
      const ConservativeAdvancementStackData& data = stack.back();
      FCL_REAL d = data.d;

      if(d > c)
        stack[stack.size() - 2] = stack[stack.size() - 1];

      stack.pop_back();

      return false;
    }
  }

  mutable FCL_REAL min_distance;

  mutable Vec3f p1, p2;

  mutable int last_tri_id1, last_tri_id2;


  FCL_REAL w;

  FCL_REAL toc;
  FCL_REAL t_err;

  mutable FCL_REAL delta_t;

  MotionBase<BV>* motion1;
  MotionBase<BV>* motion2;

  mutable std::vector<ConservativeAdvancementStackData> stack;
};


template<>
bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const;

template<>
bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const;


class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode<RSS>
{
public:
  MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_ = 1);

  FCL_REAL BVTesting(int b1, int b2) const;

  void leafTesting(int b1, int b2) const;

  bool canStop(FCL_REAL c) const;

  Matrix3f R;
  Vec3f T;
};

}


#endif