Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/geometry.hxx	Lines:	108	125	86.4 %
Date:	2024-04-26 13:14:21	Branches:	118	290	40.7 %


//
// Copyright (c) 2015-2021 CNRS INRIA
//

#ifndef __pinocchio_algo_geometry_hxx__
#define __pinocchio_algo_geometry_hxx__

#include <boost/foreach.hpp>
#include <sstream>

namespace pinocchio
{
  /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
  /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
  /* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                       const GeometryModel & geom_model,
                                       GeometryData & geom_data,
                                       const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    assert(model.check(data) && "data is not consistent with model.");

    forwardKinematics(model, data, q);
    updateGeometryPlacements(model, data, geom_model, geom_data);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                       const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                       const GeometryModel & geom_model,
                                       GeometryData & geom_data)
  {
    PINOCCHIO_UNUSED_VARIABLE(model);
    assert(model.check(data) && "data is not consistent with model.");

    for(GeomIndex i=0; i < (GeomIndex) geom_model.ngeoms; ++i)
    {
      const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint;
      if (joint_id>0) geom_data.oMg[i] =  (data.oMi[joint_id] * geom_model.geometryObjects[i].placement);
      else            geom_data.oMg[i] =  geom_model.geometryObjects[i].placement;
    }
  }
#ifdef PINOCCHIO_WITH_HPP_FCL

  /* --- COLLISIONS ----------------------------------------------------------------- */
  /* --- COLLISIONS ----------------------------------------------------------------- */
  /* --- COLLISIONS ----------------------------------------------------------------- */

  inline bool computeCollision(const GeometryModel & geom_model,
                               GeometryData & geom_data,
                               const PairIndex pair_id)
  {
    PINOCCHIO_CHECK_INPUT_ARGUMENT( geom_model.collisionPairs.size() == geom_data.collisionResults.size() );
    PINOCCHIO_CHECK_INPUT_ARGUMENT( pair_id < geom_model.collisionPairs.size() );
    const CollisionPair & pair = geom_model.collisionPairs[pair_id];

    PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first  < geom_model.ngeoms );
    PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_model.ngeoms );

    fcl::CollisionRequest & collision_request = geom_data.collisionRequests[pair_id];
    collision_request.distance_upper_bound = collision_request.security_margin + 1e-6; // TODO: change the margin

    fcl::CollisionResult & collision_result = geom_data.collisionResults[pair_id];
    collision_result.clear();

    fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[pair.first ])),
                     oM2 (toFclTransform3f(geom_data.oMg[pair.second]));

    try
    {
      GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[pair_id];
      do_computations(oM1, oM2, collision_request, collision_result);
    }
    catch(std::invalid_argument & e)
    {
      std::stringstream ss;
      ss << "Problem when trying to check the collision of collision pair #" << pair_id << " (" << pair.first << "," << pair.second << ")" << std::endl;
      ss << "hpp-fcl original error:\n" << e.what() << std::endl;
      throw std::invalid_argument(ss.str());
    }


    return collision_result.isCollision();
  }

  inline bool computeCollisions(const GeometryModel & geom_model,
                                GeometryData & geom_data,
                                const bool stopAtFirstCollision)
  {
    bool isColliding = false;

    for (std::size_t cp_index = 0;
         cp_index < geom_model.collisionPairs.size(); ++cp_index)
    {
      const CollisionPair & cp = geom_model.collisionPairs[cp_index];

      if(geom_data.activeCollisionPairs[cp_index]
         && !(geom_model.geometryObjects[cp.first].disableCollision || geom_model.geometryObjects[cp.second].disableCollision))
      {
        bool res = computeCollision(geom_model,geom_data,cp_index);
        if(!isColliding && res)
        {
          isColliding = true;
          geom_data.collisionPairIndex = cp_index; // first pair to be in collision
          if(stopAtFirstCollision)
            return true;
        }
      }
    }

    return isColliding;
  }

  // WARNING, if stopAtFirstcollision = true, then the collisions vector will not be fulfilled.
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline bool computeCollisions(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                const GeometryModel & geom_model,
                                GeometryData & geom_data,
                                const Eigen::MatrixBase<ConfigVectorType> & q,
                                const bool stopAtFirstCollision)
  {
    updateGeometryPlacements(model, data, geom_model, geom_data, q);
    return computeCollisions(geom_model,geom_data, stopAtFirstCollision);
  }

  /* --- DISTANCES ----------------------------------------------------------------- */
  /* --- DISTANCES ----------------------------------------------------------------- */
  /* --- DISTANCES ----------------------------------------------------------------- */

  inline fcl::DistanceResult & computeDistance(const GeometryModel & geom_model,
                                               GeometryData & geom_data,
                                               const PairIndex pair_id)
  {
    PINOCCHIO_CHECK_INPUT_ARGUMENT( pair_id < geom_model.collisionPairs.size() );
    PINOCCHIO_CHECK_INPUT_ARGUMENT( geom_model.collisionPairs.size() == geom_data.collisionResults.size() );
    const CollisionPair & pair = geom_model.collisionPairs[pair_id];

    PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first  < geom_model.ngeoms );
    PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_model.ngeoms );

    fcl::DistanceRequest & distance_request = geom_data.distanceRequests[pair_id];
    fcl::DistanceResult & distance_result = geom_data.distanceResults[pair_id];
    distance_result.clear();

    fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[pair.first ])),
                     oM2 (toFclTransform3f(geom_data.oMg[pair.second]));

    try
    {
      GeometryData::ComputeDistance & do_computations = geom_data.distance_functors[pair_id];
      do_computations(oM1, oM2, distance_request, distance_result);
    }
    catch(std::invalid_argument & e)
    {
      std::stringstream ss;
      ss << "Problem when trying to compute the distance of collision pair #" << pair_id << " (" << pair.first << "," << pair.second << ")" << std::endl;
      ss << "hpp-fcl original error:\n" << e.what() << std::endl;
      throw std::invalid_argument(ss.str());
    }

    return geom_data.distanceResults[pair_id];
  }

  inline std::size_t computeDistances(const GeometryModel & geom_model,
                                      GeometryData & geom_data)
  {
    std::size_t min_index = geom_model.collisionPairs.size();
    double min_dist = std::numeric_limits<double>::infinity();

    for (std::size_t cp_index = 0;
         cp_index < geom_model.collisionPairs.size(); ++cp_index)
    {
      const CollisionPair & cp = geom_model.collisionPairs[cp_index];

      if(   geom_data.activeCollisionPairs[cp_index]
         && !(geom_model.geometryObjects[cp.first].disableCollision || geom_model.geometryObjects[cp.second].disableCollision))
      {
        computeDistance(geom_model,geom_data,cp_index);
        if(geom_data.distanceResults[cp_index].min_distance < min_dist)
        {
          min_index = cp_index;
          min_dist = geom_data.distanceResults[cp_index].min_distance;
        }
      }
    }

    return min_index;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const GeometryModel & geom_model,
                                      GeometryData & geom_data)
  {
    assert(model.check(data) && "data is not consistent with model.");
    updateGeometryPlacements(model,data,geom_model,geom_data);
    return computeDistances(geom_model,geom_data);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                      DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                      const GeometryModel & geom_model,
                                      GeometryData & geom_data,
                                      const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    assert(model.check(data) && "data is not consistent with model.");
    updateGeometryPlacements(model, data, geom_model, geom_data, q);
    return computeDistances(geom_model,geom_data);
  }

  /* --- RADIUS -------------------------------------------------------------------- */
  /* --- RADIUS -------------------------------------------------------------------- */
  /* --- RADIUS -------------------------------------------------------------------- */

  /// Given p1..3 being either "min" or "max", return one of the corners of the
  /// AABB cub of the FCL object.
#define PINOCCHIO_GEOM_AABB(FCL,p1,p2,p3)                                     \
  SE3::Vector3(                                                         \
    FCL->aabb_local.p1##_ [0],                                          \
    FCL->aabb_local.p2##_ [1],                                          \
    FCL->aabb_local.p3##_ [2])

  /// For all bodies of the model, compute the point of the geometry model
  /// that is the further from the center of the joint. This quantity is used
  /// in some continuous collision test.
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void computeBodyRadius(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                const GeometryModel & geom_model,
                                GeometryData & geom_data)
  {
    geom_data.radius.resize(model.joints.size(),0);
    BOOST_FOREACH(const GeometryObject & geom_object,geom_model.geometryObjects)
    {
      const GeometryObject::CollisionGeometryPtr & geometry
        = geom_object.geometry;

      // Force computation of the Local AABB
      // TODO: change for a more elegant solution
      const_cast<hpp::fcl::CollisionGeometry&>(*geometry).computeLocalAABB();

      const GeometryModel::SE3 & jMb = geom_object.placement; // placement in joint.
      const Model::JointIndex i = geom_object.parentJoint;
      assert (i<geom_data.radius.size());

      double radius = geom_data.radius[i] * geom_data.radius[i];

      // The radius is simply the one of the 8 corners of the AABB cube, expressed
      // in the joint frame, whose norm is the highest.
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,min,min)).squaredNorm(),radius);
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,min,max)).squaredNorm(),radius);
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,max,min)).squaredNorm(),radius);
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,max,max)).squaredNorm(),radius);
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,min,min)).squaredNorm(),radius);
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,min,max)).squaredNorm(),radius);
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,max,min)).squaredNorm(),radius);
      radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,max,max)).squaredNorm(),radius);

      // Don't forget to sqroot the squared norm before storing it.
      geom_data.radius[i] = sqrt(radius);
    }
  }

#undef PINOCCHIO_GEOM_AABB
#endif // PINOCCHIO_WITH_HPP_FCL

  /* --- APPEND GEOMETRY MODEL ----------------------------------------------------------- */

  inline void appendGeometryModel(GeometryModel & geom_model1,
                                  const GeometryModel & geom_model2)
  {
    assert (geom_model1.ngeoms == geom_model1.geometryObjects.size());
    Index nGeom1 = geom_model1.geometryObjects.size();
    Index nColPairs1 = geom_model1.collisionPairs.size();
    assert (geom_model2.ngeoms == geom_model2.geometryObjects.size());
    Index nGeom2 = geom_model2.geometryObjects.size();
    Index nColPairs2 = geom_model2.collisionPairs.size();

    /// Append the geometry objects and geometry positions
    geom_model1.geometryObjects.insert(geom_model1.geometryObjects.end(),
        geom_model2.geometryObjects.begin(), geom_model2.geometryObjects.end());
    geom_model1.ngeoms += nGeom2;

    /// 1. copy the collision pairs and update geom_data1 accordingly.
    geom_model1.collisionPairs.reserve(nColPairs1 + nColPairs2 + nGeom1 * nGeom2);
    for (Index i = 0; i < nColPairs2; ++i)
    {
      const CollisionPair& cp = geom_model2.collisionPairs[i];
      geom_model1.collisionPairs.push_back(
          CollisionPair (cp.first + nGeom1, cp.second + nGeom1)
          );
    }

    /// 2. add the collision pairs between geom_model1 and geom_model2.
    for (Index i = 0; i < nGeom1; ++i) {
      Index parent_i = geom_model1.geometryObjects[i].parentJoint;
      for (Index j = nGeom1; j < nGeom1 + nGeom2; ++j) {
        if (parent_i != geom_model1.geometryObjects[j].parentJoint)
          geom_model1.collisionPairs.push_back(CollisionPair(i, j));
      }
    }
  }

} // namespace pinocchio

#endif // ifnded __pinocchio_algo_geometry_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2021 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_algo_geometry_hxx__
6			#define __pinocchio_algo_geometry_hxx__
7
8			#include <boost/foreach.hpp>
9			#include <sstream>
10
11			namespace pinocchio
12			{
13			/* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
14			/* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
15			/* --- GEOMETRY PLACEMENTS -------------------------------------------------------- */
16			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
17		16	inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
18			DataTpl<Scalar,Options,JointCollectionTpl> & data,
19			const GeometryModel & geom_model,
20			GeometryData & geom_data,
21			const Eigen::MatrixBase<ConfigVectorType> & q)
22			{
23	✓✗	16	assert(model.check(data) && "data is not consistent with model.");
24
25		16	forwardKinematics(model, data, q);
26		16	updateGeometryPlacements(model, data, geom_model, geom_data);
27		16	}
28
29			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
30		22	inline void updateGeometryPlacements(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
31			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
32			const GeometryModel & geom_model,
33			GeometryData & geom_data)
34			{
35			PINOCCHIO_UNUSED_VARIABLE(model);
36	✓✗	22	assert(model.check(data) && "data is not consistent with model.");
37
38	✓✓	488	for(GeomIndex i=0; i < (GeomIndex) geom_model.ngeoms; ++i)
39			{
40		466	const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint;
41	✓✓	466	if (joint_id>0) geom_data.oMg[i] = (data.oMi[joint_id] * geom_model.geometryObjects[i].placement);
42		11	else geom_data.oMg[i] = geom_model.geometryObjects[i].placement;
43			}
44		22	}
45			#ifdef PINOCCHIO_WITH_HPP_FCL
46
47			/* --- COLLISIONS ----------------------------------------------------------------- */
48			/* --- COLLISIONS ----------------------------------------------------------------- */
49			/* --- COLLISIONS ----------------------------------------------------------------- */
50
51		2131	inline bool computeCollision(const GeometryModel & geom_model,
52			GeometryData & geom_data,
53			const PairIndex pair_id)
54			{
55	✗✓✗✗	2131	PINOCCHIO_CHECK_INPUT_ARGUMENT( geom_model.collisionPairs.size() == geom_data.collisionResults.size() );
56	✗✓✗✗	2131	PINOCCHIO_CHECK_INPUT_ARGUMENT( pair_id < geom_model.collisionPairs.size() );
57		2131	const CollisionPair & pair = geom_model.collisionPairs[pair_id];
58
59	✗✓✗✗	2131	PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geom_model.ngeoms );
60	✗✓✗✗	2131	PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_model.ngeoms );
61
62		2131	fcl::CollisionRequest & collision_request = geom_data.collisionRequests[pair_id];
63		2131	collision_request.distance_upper_bound = collision_request.security_margin + 1e-6; // TODO: change the margin
64
65		2131	fcl::CollisionResult & collision_result = geom_data.collisionResults[pair_id];
66		2131	collision_result.clear();
67
68	✓✗	2131	fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[pair.first ])),
69	✓✗	2131	oM2 (toFclTransform3f(geom_data.oMg[pair.second]));
70
71			try
72			{
73		2131	GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[pair_id];
74	✓✗	2131	do_computations(oM1, oM2, collision_request, collision_result);
75			}
76			catch(std::invalid_argument & e)
77			{
78			std::stringstream ss;
79			ss << "Problem when trying to check the collision of collision pair #" << pair_id << " (" << pair.first << "," << pair.second << ")" << std::endl;
80			ss << "hpp-fcl original error:\n" << e.what() << std::endl;
81			throw std::invalid_argument(ss.str());
82			}
83
84
85		4262	return collision_result.isCollision();
86			}
87
88		6	inline bool computeCollisions(const GeometryModel & geom_model,
89			GeometryData & geom_data,
90			const bool stopAtFirstCollision)
91			{
92		6	bool isColliding = false;
93
94		2060	for (std::size_t cp_index = 0;
95	✓✓	2060	cp_index < geom_model.collisionPairs.size(); ++cp_index)
96			{
97		2054	const CollisionPair & cp = geom_model.collisionPairs[cp_index];
98
99		2054	if(geom_data.activeCollisionPairs[cp_index]
100	✓✗✓✗ ✓✗✓✗	2054	&& !(geom_model.geometryObjects[cp.first].disableCollision \|\| geom_model.geometryObjects[cp.second].disableCollision))
101			{
102		2054	bool res = computeCollision(geom_model,geom_data,cp_index);
103	✓✗✗✓	2054	if(!isColliding && res)
104			{
105			isColliding = true;
106			geom_data.collisionPairIndex = cp_index; // first pair to be in collision
107			if(stopAtFirstCollision)
108			return true;
109			}
110			}
111			}
112
113		6	return isColliding;
114			}
115
116			// WARNING, if stopAtFirstcollision = true, then the collisions vector will not be fulfilled.
117			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
118		4	inline bool computeCollisions(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
119			DataTpl<Scalar,Options,JointCollectionTpl> & data,
120			const GeometryModel & geom_model,
121			GeometryData & geom_data,
122			const Eigen::MatrixBase<ConfigVectorType> & q,
123			const bool stopAtFirstCollision)
124			{
125		4	updateGeometryPlacements(model, data, geom_model, geom_data, q);
126		4	return computeCollisions(geom_model,geom_data, stopAtFirstCollision);
127			}
128
129			/* --- DISTANCES ----------------------------------------------------------------- */
130			/* --- DISTANCES ----------------------------------------------------------------- */
131			/* --- DISTANCES ----------------------------------------------------------------- */
132
133		141	inline fcl::DistanceResult & computeDistance(const GeometryModel & geom_model,
134			GeometryData & geom_data,
135			const PairIndex pair_id)
136			{
137	✗✓✗✗	141	PINOCCHIO_CHECK_INPUT_ARGUMENT( pair_id < geom_model.collisionPairs.size() );
138	✗✓✗✗	141	PINOCCHIO_CHECK_INPUT_ARGUMENT( geom_model.collisionPairs.size() == geom_data.collisionResults.size() );
139		141	const CollisionPair & pair = geom_model.collisionPairs[pair_id];
140
141	✗✓✗✗	141	PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geom_model.ngeoms );
142	✗✓✗✗	141	PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_model.ngeoms );
143
144		141	fcl::DistanceRequest & distance_request = geom_data.distanceRequests[pair_id];
145		141	fcl::DistanceResult & distance_result = geom_data.distanceResults[pair_id];
146	✓✗	141	distance_result.clear();
147
148	✓✗	141	fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[pair.first ])),
149	✓✗	141	oM2 (toFclTransform3f(geom_data.oMg[pair.second]));
150
151			try
152			{
153		141	GeometryData::ComputeDistance & do_computations = geom_data.distance_functors[pair_id];
154	✓✗	141	do_computations(oM1, oM2, distance_request, distance_result);
155			}
156			catch(std::invalid_argument & e)
157			{
158			std::stringstream ss;
159			ss << "Problem when trying to compute the distance of collision pair #" << pair_id << " (" << pair.first << "," << pair.second << ")" << std::endl;
160			ss << "hpp-fcl original error:\n" << e.what() << std::endl;
161			throw std::invalid_argument(ss.str());
162			}
163
164		282	return geom_data.distanceResults[pair_id];
165			}
166
167		2	inline std::size_t computeDistances(const GeometryModel & geom_model,
168			GeometryData & geom_data)
169			{
170		2	std::size_t min_index = geom_model.collisionPairs.size();
171		2	double min_dist = std::numeric_limits<double>::infinity();
172
173		142	for (std::size_t cp_index = 0;
174	✓✓	142	cp_index < geom_model.collisionPairs.size(); ++cp_index)
175			{
176		140	const CollisionPair & cp = geom_model.collisionPairs[cp_index];
177
178		140	if( geom_data.activeCollisionPairs[cp_index]
179	✓✗✓✗ ✓✗✓✗	140	&& !(geom_model.geometryObjects[cp.first].disableCollision \|\| geom_model.geometryObjects[cp.second].disableCollision))
180			{
181		140	computeDistance(geom_model,geom_data,cp_index);
182	✓✓	140	if(geom_data.distanceResults[cp_index].min_distance < min_dist)
183			{
184		10	min_index = cp_index;
185		10	min_dist = geom_data.distanceResults[cp_index].min_distance;
186			}
187			}
188			}
189
190		2	return min_index;
191			}
192
193			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
194			inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
195			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
196			const GeometryModel & geom_model,
197			GeometryData & geom_data)
198			{
199			assert(model.check(data) && "data is not consistent with model.");
200			updateGeometryPlacements(model,data,geom_model,geom_data);
201			return computeDistances(geom_model,geom_data);
202			}
203
204			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
205		1	inline std::size_t computeDistances(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
206			DataTpl<Scalar,Options,JointCollectionTpl> & data,
207			const GeometryModel & geom_model,
208			GeometryData & geom_data,
209			const Eigen::MatrixBase<ConfigVectorType> & q)
210			{
211	✓✗	1	assert(model.check(data) && "data is not consistent with model.");
212		1	updateGeometryPlacements(model, data, geom_model, geom_data, q);
213		1	return computeDistances(geom_model,geom_data);
214			}
215
216			/* --- RADIUS -------------------------------------------------------------------- */
217			/* --- RADIUS -------------------------------------------------------------------- */
218			/* --- RADIUS -------------------------------------------------------------------- */
219
220			/// Given p1..3 being either "min" or "max", return one of the corners of the
221			/// AABB cub of the FCL object.
222			#define PINOCCHIO_GEOM_AABB(FCL,p1,p2,p3) \
223			SE3::Vector3( \
224			FCL->aabb_local.p1##_ [0], \
225			FCL->aabb_local.p2##_ [1], \
226			FCL->aabb_local.p3##_ [2])
227
228			/// For all bodies of the model, compute the point of the geometry model
229			/// that is the further from the center of the joint. This quantity is used
230			/// in some continuous collision test.
231			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
232		1	inline void computeBodyRadius(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
233			const GeometryModel & geom_model,
234			GeometryData & geom_data)
235			{
236	✓✗	1	geom_data.radius.resize(model.joints.size(),0);
237	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗	41	BOOST_FOREACH(const GeometryObject & geom_object,geom_model.geometryObjects)
238			{
239		20	const GeometryObject::CollisionGeometryPtr & geometry
240			= geom_object.geometry;
241
242			// Force computation of the Local AABB
243			// TODO: change for a more elegant solution
244	✓✗	20	const_cast<hpp::fcl::CollisionGeometry&>(*geometry).computeLocalAABB();
245
246		20	const GeometryModel::SE3 & jMb = geom_object.placement; // placement in joint.
247		20	const Model::JointIndex i = geom_object.parentJoint;
248	✗✓	20	assert (i<geom_data.radius.size());
249
250		20	double radius = geom_data.radius[i] * geom_data.radius[i];
251
252			// The radius is simply the one of the 8 corners of the AABB cube, expressed
253			// in the joint frame, whose norm is the highest.
254	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,min,min)).squaredNorm(),radius);
255	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,min,max)).squaredNorm(),radius);
256	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,max,min)).squaredNorm(),radius);
257	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,min,max,max)).squaredNorm(),radius);
258	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,min,min)).squaredNorm(),radius);
259	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,min,max)).squaredNorm(),radius);
260	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,max,min)).squaredNorm(),radius);
261	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	20	radius = std::max (jMb.act(PINOCCHIO_GEOM_AABB(geometry,max,max,max)).squaredNorm(),radius);
262
263			// Don't forget to sqroot the squared norm before storing it.
264		20	geom_data.radius[i] = sqrt(radius);
265			}
266		1	}
267
268			#undef PINOCCHIO_GEOM_AABB
269			#endif // PINOCCHIO_WITH_HPP_FCL
270
271			/* --- APPEND GEOMETRY MODEL ----------------------------------------------------------- */
272
273		2	inline void appendGeometryModel(GeometryModel & geom_model1,
274			const GeometryModel & geom_model2)
275			{
276	✗✓	2	assert (geom_model1.ngeoms == geom_model1.geometryObjects.size());
277		2	Index nGeom1 = geom_model1.geometryObjects.size();
278		2	Index nColPairs1 = geom_model1.collisionPairs.size();
279	✗✓	2	assert (geom_model2.ngeoms == geom_model2.geometryObjects.size());
280		2	Index nGeom2 = geom_model2.geometryObjects.size();
281		2	Index nColPairs2 = geom_model2.collisionPairs.size();
282
283			/// Append the geometry objects and geometry positions
284		2	geom_model1.geometryObjects.insert(geom_model1.geometryObjects.end(),
285	✓✗	4	geom_model2.geometryObjects.begin(), geom_model2.geometryObjects.end());
286		2	geom_model1.ngeoms += nGeom2;
287
288			/// 1. copy the collision pairs and update geom_data1 accordingly.
289		2	geom_model1.collisionPairs.reserve(nColPairs1 + nColPairs2 + nGeom1 * nGeom2);
290	✗✓	2	for (Index i = 0; i < nColPairs2; ++i)
291			{
292			const CollisionPair& cp = geom_model2.collisionPairs[i];
293			geom_model1.collisionPairs.push_back(
294			CollisionPair (cp.first + nGeom1, cp.second + nGeom1)
295			);
296			}
297
298			/// 2. add the collision pairs between geom_model1 and geom_model2.
299	✓✓	22	for (Index i = 0; i < nGeom1; ++i) {
300		20	Index parent_i = geom_model1.geometryObjects[i].parentJoint;
301	✓✓	420	for (Index j = nGeom1; j < nGeom1 + nGeom2; ++j) {
302	✓✓	400	if (parent_i != geom_model1.geometryObjects[j].parentJoint)
303	✓✗	380	geom_model1.collisionPairs.push_back(CollisionPair(i, j));
304			}
305			}
306		2	}
307
308			} // namespace pinocchio
309
310			#endif // ifnded __pinocchio_algo_geometry_hxx__