GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/BV/kIOS.cpp Lines: 45 89 50.6 %
Date: 2024-02-09 12:57:42 Branches: 41 142 28.9 %

Line Branch Exec Source
1
/*
2
 * Software License Agreement (BSD License)
3
 *
4
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
5
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
6
 *  All rights reserved.
7
 *
8
 *  Redistribution and use in source and binary forms, with or without
9
 *  modification, are permitted provided that the following conditions
10
 *  are met:
11
 *
12
 *   * Redistributions of source code must retain the above copyright
13
 *     notice, this list of conditions and the following disclaimer.
14
 *   * Redistributions in binary form must reproduce the above
15
 *     copyright notice, this list of conditions and the following
16
 *     disclaimer in the documentation and/or other materials provided
17
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
19
 *     contributors may be used to endorse or promote products derived
20
 *     from this software without specific prior written permission.
21
 *
22
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33
 *  POSSIBILITY OF SUCH DAMAGE.
34
 */
35
36
/** \author Jia Pan */
37
38
#include <hpp/fcl/BV/kIOS.h>
39
#include <hpp/fcl/BVH/BVH_utility.h>
40
#include <hpp/fcl/math/transform.h>
41
42
#include <iostream>
43
#include <limits>
44
45
namespace hpp {
46
namespace fcl {
47
48
bool kIOS::overlap(const kIOS& other) const {
49
  for (unsigned int i = 0; i < num_spheres; ++i) {
50
    for (unsigned int j = 0; j < other.num_spheres; ++j) {
51
      FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
52
      FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
53
      if (o_dist > sum_r * sum_r) return false;
54
    }
55
  }
56
57
  return obb.overlap(other.obb);
58
}
59
60
2974
bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
61
                   FCL_REAL& sqrDistLowerBound) const {
62
12856
  for (unsigned int i = 0; i < num_spheres; ++i) {
63
52474
    for (unsigned int j = 0; j < other.num_spheres; ++j) {
64
42592
      FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
65
42592
      FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
66
42592
      if (o_dist > sum_r * sum_r) {
67
211
        o_dist = sqrt(o_dist) - sum_r;
68
211
        sqrDistLowerBound = o_dist * o_dist;
69
211
        return false;
70
      }
71
    }
72
  }
73
74
2763
  return obb.overlap(other.obb, request, sqrDistLowerBound);
75
}
76
77
bool kIOS::contain(const Vec3f& p) const {
78
  for (unsigned int i = 0; i < num_spheres; ++i) {
79
    FCL_REAL r = spheres[i].r;
80
    if ((spheres[i].o - p).squaredNorm() > r * r) return false;
81
  }
82
83
  return true;
84
}
85
86
kIOS& kIOS::operator+=(const Vec3f& p) {
87
  for (unsigned int i = 0; i < num_spheres; ++i) {
88
    FCL_REAL r = spheres[i].r;
89
    FCL_REAL new_r_sqr = (p - spheres[i].o).squaredNorm();
90
    if (new_r_sqr > r * r) {
91
      spheres[i].r = sqrt(new_r_sqr);
92
    }
93
  }
94
95
  obb += p;
96
  return *this;
97
}
98
99
6537
kIOS kIOS::operator+(const kIOS& other) const {
100
6537
  kIOS result;
101
6537
  unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres);
102
26148
  for (unsigned int i = 0; i < new_num_spheres; ++i) {
103
19611
    result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]);
104
  }
105
106
6537
  result.num_spheres = new_num_spheres;
107
108
6537
  result.obb = obb + other.obb;
109
110
6537
  return result;
111
}
112
113
FCL_REAL kIOS::width() const { return obb.width(); }
114
115
FCL_REAL kIOS::height() const { return obb.height(); }
116
117
FCL_REAL kIOS::depth() const { return obb.depth(); }
118
119
7592
FCL_REAL kIOS::volume() const { return obb.volume(); }
120
121
7592
FCL_REAL kIOS::size() const { return volume(); }
122
123
3106
FCL_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const {
124
3106
  FCL_REAL d_max = 0;
125
3106
  long id_a = -1, id_b = -1;
126
18038
  for (unsigned int i = 0; i < num_spheres; ++i) {
127
79164
    for (unsigned int j = 0; j < other.num_spheres; ++j) {
128
64232
      FCL_REAL d = (spheres[i].o - other.spheres[j].o).norm() -
129
64232
                   (spheres[i].r + other.spheres[j].r);
130
64232
      if (d_max < d) {
131
2271
        d_max = d;
132

2271
        if (P && Q) {
133
          id_a = (long)i;
134
          id_b = (long)j;
135
        }
136
      }
137
    }
138
  }
139
140

3106
  if (P && Q) {
141
    if (id_a != -1 && id_b != -1) {
142
      const Vec3f v = spheres[id_a].o - spheres[id_b].o;
143
      FCL_REAL len_v = v.norm();
144
      *P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
145
      *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
146
    }
147
  }
148
149
3106
  return d_max;
150
}
151
152
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
153
             const kIOS& b2) {
154
  kIOS b2_temp = b2;
155
  for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
156
    b2_temp.spheres[i].o.noalias() =
157
        R0.transpose() * (b2_temp.spheres[i].o - T0);
158
  }
159
160
  b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0);
161
  b2_temp.obb.axes.applyOnTheLeft(R0.transpose());
162
163
  return b1.overlap(b2_temp);
164
}
165
166
1648
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
167
             const kIOS& b2, const CollisionRequest& request,
168
             FCL_REAL& sqrDistLowerBound) {
169
1648
  kIOS b2_temp = b2;
170
9368
  for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
171
7720
    b2_temp.spheres[i].o.noalias() =
172


15440
        R0.transpose() * (b2_temp.spheres[i].o - T0);
173
  }
174
175


1648
  b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0);
176

1648
  b2_temp.obb.axes.applyOnTheLeft(R0.transpose());
177
178
3296
  return b1.overlap(b2_temp, request, sqrDistLowerBound);
179
}
180
181
1292
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1,
182
                  const kIOS& b2, Vec3f* P, Vec3f* Q) {
183
1292
  kIOS b2_temp = b2;
184
6398
  for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) {
185

5106
    b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
186
  }
187
188
2584
  return b1.distance(b2_temp, P, Q);
189
}
190
191
kIOS translate(const kIOS& bv, const Vec3f& t) {
192
  kIOS res(bv);
193
  for (size_t i = 0; i < res.num_spheres; ++i) {
194
    res.spheres[i].o += t;
195
  }
196
197
  translate(res.obb, t);
198
  return res;
199
}
200
201
}  // namespace fcl
202
203
}  // namespace hpp