GCC Code Coverage Report

Directory:	./
File:	src/distance/triangle_triangle.cpp
Date:	2025-06-03 17:15:40

	Exec	Total	Coverage
Lines:	31	32	96.9%
Functions:	1	1	100.0%
Branches:	43	86	50.0%

  
      Line
      Branch
      Exec
      Source
    
      /*
    
       * Software License Agreement (BSD License)
    
       *
    
       *  Copyright (c) 2024, INRIA
    
       *  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 Open Source Robotics Foundation 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.
    
       */
    
      /** \author Louis Montaut */
    
      #include "coal/shape/geometric_shapes.h"
    
      #include "coal/internal/shape_shape_func.h"
    
      #include "../narrowphase/details.h"
    
      #include "coal/tracy.hh"
    
      namespace coal {
    
      namespace internal {
    
      template <>
    
      29205604
      Scalar ShapeShapeDistance<TriangleP, TriangleP>(
    
          const CollisionGeometry* o1, const Transform3s& tf1,
    
          const CollisionGeometry* o2, const Transform3s& tf2,
    
          const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
    
        COAL_TRACY_ZONE_SCOPED_N(
    
            "coal::internal::ShapeShapeDistance<TriangleP, TriangleP>");
    
        // Transform the triangles in world frame
    
      29205604
        const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      29205604
        const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b),
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      58411208
                           tf1.transform(s1.c));
    
      29205604
        const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      29205604
        const TriangleP t2(tf2.transform(s2.a), tf2.transform(s2.b),
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      58411208
                           tf2.transform(s2.c));
    
        // Reset GJK algorithm
    
        //   We don't need to take into account swept-sphere radius in GJK iterations;
    
        //   the result will be corrected after GJK terminates.
    
        solver->minkowski_difference
    
        1/2✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.

      29205604
            .set<::coal::details::SupportOptions::NoSweptSphere>(&t1, &t2);
    
        1/2✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.

      29205604
        solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance);
    
        // Get GJK initial guess
    
        1/2✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.

      29205604
        Vec3s guess;
    
        1/2✓ Branch 0 taken 29205604 times.
✗ Branch 1 not taken.

      29205604
        if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
    
        1/2✗ Branch 0 not taken.
✓ Branch 1 taken 29205604 times.

      29205604
            solver->enable_cached_guess) {
    
      ✗
          guess = solver->cached_guess;
    
        } else {
    
        7/14✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29205604 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 29205604 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 29205604 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 29205604 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 29205604 times.
✗ Branch 20 not taken.

      29205604
          guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
    
        }
    
        1/2✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.

      29205604
        support_func_guess_t support_hint;
    
      29205604
        solver->epa.status =
    
            details::EPA::DidNotRun;  // EPA is never called in this function
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      29205604
        Vec3ps guess_ = guess.cast<SolverScalar>();
    
        details::GJK::Status gjk_status =
    
        1/2✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.

      29205604
            solver->gjk.evaluate(solver->minkowski_difference, guess_, support_hint);
    
        3/6✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29205604 times.
✗ Branch 8 not taken.

      29205604
        solver->cached_guess = solver->gjk.getGuessFromSimplex().cast<Scalar>();
    
        1/2✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.

      29205604
        solver->support_func_cached_guess = solver->gjk.support_hint;
    
        // Retrieve witness points and normal
    
        3/6✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29205604 times.
✗ Branch 8 not taken.

      29205604
        Vec3ps p1_, p2_, normal_;
    
        1/2✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.

      29205604
        solver->gjk.getWitnessPointsAndNormal(solver->minkowski_difference, p1_, p2_,
    
                                              normal_);
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      29205604
        p1 = p1_.cast<Scalar>();
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      29205604
        p2 = p2_.cast<Scalar>();
    
        2/4✓ Branch 1 taken 29205604 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29205604 times.
✗ Branch 5 not taken.

      29205604
        normal = normal_.cast<Scalar>();
    
      29205604
        Scalar distance = Scalar(solver->gjk.distance);
    
        2/2✓ Branch 0 taken 21227 times.
✓ Branch 1 taken 29184377 times.

      29205604
        if (gjk_status == details::GJK::Collision) {
    
          Scalar penetrationDepth =
    
        1/2✓ Branch 1 taken 21227 times.
✗ Branch 2 not taken.

      21227
              details::computePenetration(t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
    
      21227
          distance = -penetrationDepth;
    
        } else {
    
          // No collision
    
          // TODO On degenerated case, the closest point may be wrong
    
          // (i.e. an object face normal is colinear to gjk.ray
    
          // assert (dist == (w0 - w1).norm());
    
        2/4✓ Branch 1 taken 29184377 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 29184377 times.

      29184377
          assert(solver->gjk.ray.norm() > solver->gjk.getTolerance());
    
        }
    
        // assert(false && "should not reach this point");
    
        // return false;
    
      29205604
        return distance;
    
      29205604
      }
    
      }  // namespace internal
    
      }  // namespace coal

Line	Branch	Exec	Source
1			/*
2			* Software License Agreement (BSD License)
3			*
4			* Copyright (c) 2024, INRIA
5			* All rights reserved.
6			*
7			* Redistribution and use in source and binary forms, with or without
8			* modification, are permitted provided that the following conditions
9			* are met:
10			*
11			* * Redistributions of source code must retain the above copyright
12			* notice, this list of conditions and the following disclaimer.
13			* * Redistributions in binary form must reproduce the above
14			* copyright notice, this list of conditions and the following
15			* disclaimer in the documentation and/or other materials provided
16			* with the distribution.
17			* * Neither the name of Open Source Robotics Foundation nor the names of its
18			* contributors may be used to endorse or promote products derived
19			* from this software without specific prior written permission.
20			*
21			* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22			* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23			* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24			* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25			* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26			* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27			* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28			* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29			* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30			* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31			* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32			* POSSIBILITY OF SUCH DAMAGE.
33			*/
34
35			/** \author Louis Montaut */
36
37			#include "coal/shape/geometric_shapes.h"
38
39			#include "coal/internal/shape_shape_func.h"
40			#include "../narrowphase/details.h"
41
42			#include "coal/tracy.hh"
43
44			namespace coal {
45
46			namespace internal {
47			template <>
48		29205604	Scalar ShapeShapeDistance<TriangleP, TriangleP>(
49			const CollisionGeometry* o1, const Transform3s& tf1,
50			const CollisionGeometry* o2, const Transform3s& tf2,
51			const GJKSolver* solver, const bool, Vec3s& p1, Vec3s& p2, Vec3s& normal) {
52			COAL_TRACY_ZONE_SCOPED_N(
53			"coal::internal::ShapeShapeDistance<TriangleP, TriangleP>");
54			// Transform the triangles in world frame
55		29205604	const TriangleP& s1 = static_cast<const TriangleP&>(*o1);
56	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	29205604	const TriangleP t1(tf1.transform(s1.a), tf1.transform(s1.b),
57	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	58411208	tf1.transform(s1.c));
58
59		29205604	const TriangleP& s2 = static_cast<const TriangleP&>(*o2);
60	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	29205604	const TriangleP t2(tf2.transform(s2.a), tf2.transform(s2.b),
61	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	58411208	tf2.transform(s2.c));
62
63			// Reset GJK algorithm
64			// We don't need to take into account swept-sphere radius in GJK iterations;
65			// the result will be corrected after GJK terminates.
66			solver->minkowski_difference
67	1/2 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken.	29205604	.set<::coal::details::SupportOptions::NoSweptSphere>(&t1, &t2);
68	1/2 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken.	29205604	solver->gjk.reset(solver->gjk_max_iterations, solver->gjk_tolerance);
69
70			// Get GJK initial guess
71	1/2 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken.	29205604	Vec3s guess;
72	1/2 ✓ Branch 0 taken 29205604 times. ✗ Branch 1 not taken.	29205604	if (solver->gjk_initial_guess == GJKInitialGuess::CachedGuess \|\|
73	1/2 ✗ Branch 0 not taken. ✓ Branch 1 taken 29205604 times.	29205604	solver->enable_cached_guess) {
74		✗	guess = solver->cached_guess;
75			} else {
76	7/14 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 29205604 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 29205604 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 29205604 times. ✗ Branch 14 not taken. ✓ Branch 16 taken 29205604 times. ✗ Branch 17 not taken. ✓ Branch 19 taken 29205604 times. ✗ Branch 20 not taken.	29205604	guess = (t1.a + t1.b + t1.c - t2.a - t2.b - t2.c) / 3;
77			}
78	1/2 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken.	29205604	support_func_guess_t support_hint;
79		29205604	solver->epa.status =
80			details::EPA::DidNotRun; // EPA is never called in this function
81
82	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	29205604	Vec3ps guess_ = guess.cast<SolverScalar>();
83			details::GJK::Status gjk_status =
84	1/2 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken.	29205604	solver->gjk.evaluate(solver->minkowski_difference, guess_, support_hint);
85
86	3/6 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 29205604 times. ✗ Branch 8 not taken.	29205604	solver->cached_guess = solver->gjk.getGuessFromSimplex().cast<Scalar>();
87	1/2 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken.	29205604	solver->support_func_cached_guess = solver->gjk.support_hint;
88
89			// Retrieve witness points and normal
90	3/6 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 29205604 times. ✗ Branch 8 not taken.	29205604	Vec3ps p1_, p2_, normal_;
91	1/2 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken.	29205604	solver->gjk.getWitnessPointsAndNormal(solver->minkowski_difference, p1_, p2_,
92			normal_);
93	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	29205604	p1 = p1_.cast<Scalar>();
94	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	29205604	p2 = p2_.cast<Scalar>();
95	2/4 ✓ Branch 1 taken 29205604 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 29205604 times. ✗ Branch 5 not taken.	29205604	normal = normal_.cast<Scalar>();
96		29205604	Scalar distance = Scalar(solver->gjk.distance);
97
98	2/2 ✓ Branch 0 taken 21227 times. ✓ Branch 1 taken 29184377 times.	29205604	if (gjk_status == details::GJK::Collision) {
99			Scalar penetrationDepth =
100	1/2 ✓ Branch 1 taken 21227 times. ✗ Branch 2 not taken.	21227	details::computePenetration(t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal);
101		21227	distance = -penetrationDepth;
102			} else {
103			// No collision
104			// TODO On degenerated case, the closest point may be wrong
105			// (i.e. an object face normal is colinear to gjk.ray
106			// assert (dist == (w0 - w1).norm());
107	2/4 ✓ Branch 1 taken 29184377 times. ✗ Branch 2 not taken. ✗ Branch 4 not taken. ✓ Branch 5 taken 29184377 times.	29184377	assert(solver->gjk.ray.norm() > solver->gjk.getTolerance());
108			}
109			// assert(false && "should not reach this point");
110			// return false;
111
112		29205604	return distance;
113		29205604	}
114			} // namespace internal
115
116			} // namespace coal
117