Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-fcl
Commits
afa59832
Commit
afa59832
authored
Jun 13, 2019
by
Joseph Mirabel
Browse files
Add missing overlap method.
parent
c3edc377
Changes
7
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/BV/AABB.h
View file @
afa59832
...
...
@@ -262,6 +262,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
return
res
;
}
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
AABB
&
b1
,
const
AABB
&
b2
);
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
AABB
&
b1
,
const
AABB
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
);
}
}
// namespace hpp
...
...
include/hpp/fcl/BV/RSS.h
View file @
afa59832
...
...
@@ -155,6 +155,10 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
RSS
&
b1
,
const
RSS
&
b2
);
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
RSS
&
b1
,
const
RSS
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
);
}
...
...
include/hpp/fcl/BV/kDOP.h
View file @
afa59832
...
...
@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include
<stdexcept>
#include
<hpp/fcl/math/
vec
_3f.h>
#include
<hpp/fcl/math/
matrix
_3f.h>
namespace
hpp
{
...
...
@@ -176,6 +176,20 @@ public:
};
template
<
size_t
N
>
bool
overlap
(
const
Matrix3f
&
/*R0*/
,
const
Vec3f
&
/*T0*/
,
const
KDOP
<
N
>&
/*b1*/
,
const
KDOP
<
N
>&
/*b2*/
)
{
throw
std
::
logic_error
(
"not implemented"
);
}
template
<
size_t
N
>
bool
overlap
(
const
Matrix3f
&
/*R0*/
,
const
Vec3f
&
/*T0*/
,
const
KDOP
<
N
>&
/*b1*/
,
const
KDOP
<
N
>&
/*b2*/
,
const
CollisionRequest
&
/*request*/
,
FCL_REAL
&
/*sqrDistLowerBound*/
)
{
throw
std
::
logic_error
(
"not implemented"
);
}
/// @brief translate the KDOP BV
template
<
size_t
N
>
...
...
include/hpp/fcl/BV/kIOS.h
View file @
afa59832
...
...
@@ -157,6 +157,12 @@ kIOS translate(const kIOS& bv, const Vec3f& t);
/// @todo Not efficient
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
kIOS
&
b1
,
const
kIOS
&
b2
);
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
/// @todo Not efficient
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
kIOS
&
b1
,
const
kIOS
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
);
/// @brief Approximate distance between two kIOS bounding volumes
/// @todo P and Q is not returned, need implementation
FCL_REAL
distance
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
kIOS
&
b1
,
const
kIOS
&
b2
,
Vec3f
*
P
=
NULL
,
Vec3f
*
Q
=
NULL
);
...
...
src/BV/AABB.cpp
View file @
afa59832
...
...
@@ -128,6 +128,20 @@ FCL_REAL AABB::distance(const AABB& other) const
return
std
::
sqrt
(
result
);
}
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
AABB
&
b1
,
const
AABB
&
b2
)
{
AABB
bb1
(
translate
(
rotate
(
b1
,
R0
),
T0
));
return
bb1
.
overlap
(
b2
);
}
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
AABB
&
b1
,
const
AABB
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
)
{
AABB
bb1
(
translate
(
rotate
(
b1
,
R0
),
T0
));
return
bb1
.
overlap
(
b2
,
request
,
sqrDistLowerBound
);
}
}
}
// namespace hpp
src/BV/RSS.cpp
View file @
afa59832
...
...
@@ -862,6 +862,25 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
return
(
dist
<=
(
b1
.
r
+
b2
.
r
));
}
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
RSS
&
b1
,
const
RSS
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
)
{
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
Vec3f
Ttemp
(
R0
*
b2
.
Tr
+
T0
-
b1
.
Tr
);
Vec3f
T
(
b1
.
axes
.
transpose
()
*
Ttemp
);
Matrix3f
R
(
b1
.
axes
.
transpose
()
*
R0
*
b2
.
axes
);
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
b1
.
l
,
b2
.
l
)
-
b1
.
r
-
b2
.
r
;
if
(
dist
<=
0
)
return
true
;
sqrDistLowerBound
=
dist
*
dist
;
return
false
;
}
bool
RSS
::
contain
(
const
Vec3f
&
p
)
const
{
Vec3f
local_p
=
p
-
Tr
;
...
...
src/BV/kIOS.cpp
View file @
afa59832
...
...
@@ -61,17 +61,27 @@ bool kIOS::overlap(const kIOS& other) const
}
return
obb
.
overlap
(
other
.
obb
);
return
true
;
}
bool
kIOS
::
overlap
(
const
kIOS
&
other
,
const
CollisionRequest
&
,
FCL_REAL
&
sqrDistLowerBound
)
const
bool
kIOS
::
overlap
(
const
kIOS
&
other
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
)
const
{
for
(
unsigned
int
i
=
0
;
i
<
num_spheres
;
++
i
)
{
sqrDistLowerBound
=
sqrt
(
-
1
);
return
overlap
(
other
);
for
(
unsigned
int
j
=
0
;
j
<
other
.
num_spheres
;
++
j
)
{
FCL_REAL
o_dist
=
(
spheres
[
i
].
o
-
other
.
spheres
[
j
].
o
).
squaredNorm
();
FCL_REAL
sum_r
=
spheres
[
i
].
r
+
other
.
spheres
[
j
].
r
;
if
(
o_dist
>
sum_r
*
sum_r
)
{
o_dist
=
sqrt
(
o_dist
)
-
sum_r
;
sqrDistLowerBound
=
o_dist
*
o_dist
;
return
false
;
}
}
}
return
obb
.
overlap
(
other
.
obb
,
request
,
sqrDistLowerBound
);
}
bool
kIOS
::
contain
(
const
Vec3f
&
p
)
const
{
...
...
@@ -192,6 +202,23 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
return
b1
.
overlap
(
b2_temp
);
}
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
kIOS
&
b1
,
const
kIOS
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
)
{
kIOS
b2_temp
=
b2
;
for
(
unsigned
int
i
=
0
;
i
<
b2_temp
.
num_spheres
;
++
i
)
{
b2_temp
.
spheres
[
i
].
o
=
R0
*
b2_temp
.
spheres
[
i
].
o
+
T0
;
}
b2_temp
.
obb
.
To
=
R0
*
b2_temp
.
obb
.
To
+
T0
;
b2_temp
.
obb
.
axes
.
applyOnTheLeft
(
R0
);
return
b1
.
overlap
(
b2_temp
,
request
,
sqrDistLowerBound
);
}
FCL_REAL
distance
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
kIOS
&
b1
,
const
kIOS
&
b2
,
Vec3f
*
P
,
Vec3f
*
Q
)
{
kIOS
b2_temp
=
b2
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment