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
e50d403a
Commit
e50d403a
authored
Aug 20, 2019
by
Joseph Mirabel
Browse files
Prepare simplification of obbDisjointAndLowerBoundDistance
parent
64dc11fa
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/BV/OBBRSS.h
View file @
e50d403a
...
...
@@ -157,18 +157,27 @@ public:
OBBRSS
translate
(
const
OBBRSS
&
bv
,
const
Vec3f
&
t
);
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
);
inline
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
)
{
return
overlap
(
R0
,
T0
,
b1
.
obb
,
b2
.
obb
);
}
/// Check collision between two OBBRSS
/// \param b1 first OBBRSS in configuration (R0, T0)
/// \param b2 second OBBRSS in identity position
/// \retval squared lower bound on the distance if OBBRSS do not overlap.
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
);
inline
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
)
{
return
overlap
(
R0
,
T0
,
b1
.
obb
,
b2
.
obb
,
request
,
sqrDistLowerBound
);
}
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
FCL_REAL
distance
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
,
Vec3f
*
P
=
NULL
,
Vec3f
*
Q
=
NULL
);
inline
FCL_REAL
distance
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
,
Vec3f
*
P
=
NULL
,
Vec3f
*
Q
=
NULL
)
{
return
distance
(
R0
,
T0
,
b1
.
rss
,
b2
.
rss
,
P
,
Q
);
}
}
...
...
src/BV/OBB.cpp
View file @
e50d403a
...
...
@@ -294,6 +294,39 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
}
bool
obbDisjointAndLowerBoundDistanceCase
(
const
Matrix3f
&
B
,
const
Vec3f
&
T
,
const
Vec3f
&
a
,
const
Vec3f
&
b
,
const
FCL_REAL
&
breakDistance2
,
const
Matrix3f
&
Bf
,
const
int
&
ia
,
const
int
&
ib
,
FCL_REAL
&
squaredLowerBoundDistance
,
FCL_REAL
&
diff
)
{
const
int
ja
=
(
ia
+
1
)
%
3
,
jb
=
(
ib
+
1
)
%
3
,
ka
=
(
ja
+
1
)
%
3
,
kb
=
(
jb
+
1
)
%
3
;
const
FCL_REAL
s
=
T
[
ka
]
*
B
(
ja
,
ib
)
-
T
[
ja
]
*
B
(
ka
,
ib
);
FCL_REAL
sinus2
;
diff
=
fabs
(
s
)
-
(
a
[
ja
]
*
Bf
(
ka
,
ib
)
+
a
[
ka
]
*
Bf
(
ja
,
ib
)
+
b
[
jb
]
*
Bf
(
ia
,
kb
)
+
b
[
kb
]
*
Bf
(
ia
,
jb
));
// We need to divide by the norm || Aia x Bib ||
// As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
ia
,
ib
)
*
Bf
(
ia
,
ib
);
if
(
sinus2
>
1e-6
)
{
squaredLowerBoundDistance
=
diff
*
diff
/
sinus2
;
if
(
squaredLowerBoundDistance
>
breakDistance2
)
{
return
true
;
}
}
}
return
false
;
}
// B, T orientation and position of 2nd OBB in frame of 1st OBB,
// a extent of 1st OBB,
// b extent of 2nd OBB.
...
...
@@ -306,30 +339,24 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
FCL_REAL
&
squaredLowerBoundDistance
)
{
FCL_REAL
t
,
s
;
FCL_REAL
diff
;
FCL_REAL
breakDistance
(
request
.
break_distance
+
request
.
security_margin
);
FCL_REAL
breakDistance2
=
breakDistance
*
breakDistance
;
FCL_REAL
diff
,
diff2
;
const
FCL_REAL
breakDistance
(
request
.
break_distance
+
request
.
security_margin
);
const
FCL_REAL
breakDistance2
=
breakDistance
*
breakDistance
;
Vec3f
AABB_corner
;
// Matrix3f Bf = abs(B);
// Bf += reps;
Matrix3f
Bf
(
B
.
cwiseAbs
());
// Corner of b axis aligned bounding box the closest to the origin
Vec3f
AABB_corner
(
T
.
cwiseAbs
()
-
Bf
*
b
);
Vec3f
diff3
(
AABB_corner
-
a
);
diff3
=
diff3
.
cwiseMax
(
Vec3f
::
Zero
());
//for (Vec3f::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
squaredLowerBoundDistance
=
diff3
.
squaredNorm
();
AABB_corner
.
noalias
()
=
T
.
cwiseAbs
()
-
Bf
*
b
-
a
;
squaredLowerBoundDistance
=
AABB_corner
.
array
().
max
(
0
).
matrix
().
squaredNorm
();
if
(
squaredLowerBoundDistance
>
breakDistance2
)
return
true
;
AABB_corner
=
(
B
.
transpose
()
*
T
).
cwiseAbs
()
-
Bf
.
transpose
()
*
a
;
// diff3 = | B^T T| - b - Bf^T a
diff3
=
AABB_corner
-
b
;
diff3
=
diff3
.
cwiseMax
(
Vec3f
::
Zero
());
squaredLowerBoundDistance
=
diff3
.
squaredNorm
();
// | B^T T| - b - Bf^T a
AABB_corner
.
noalias
()
=
(
B
.
transpose
()
*
T
).
cwiseAbs
()
-
Bf
.
transpose
()
*
a
-
b
;
squaredLowerBoundDistance
=
AABB_corner
.
array
().
max
(
0
).
matrix
().
squaredNorm
();
if
(
squaredLowerBoundDistance
>
breakDistance2
)
return
true
;
...
...
@@ -344,6 +371,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
// As ||A0|| = ||B0|| = 1,
// 2 2
// || A0 x B0 || + (A0 | B0) = 1
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
0
,
0
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
0
,
0
)
*
Bf
(
0
,
0
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -359,7 +389,10 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
t
=
((
s
<
0.0
)
?
-
s
:
s
);
assert
(
t
==
fabs
(
s
));
diff
=
t
-
(
a
[
1
]
*
Bf
(
2
,
1
)
+
a
[
2
]
*
Bf
(
1
,
1
)
+
b
[
0
]
*
Bf
(
0
,
2
)
+
b
[
2
]
*
Bf
(
0
,
0
));
b
[
2
]
*
Bf
(
0
,
0
)
+
b
[
0
]
*
Bf
(
0
,
2
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
0
,
1
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
0
,
1
)
*
Bf
(
0
,
1
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -376,6 +409,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
diff
=
t
-
(
a
[
1
]
*
Bf
(
2
,
2
)
+
a
[
2
]
*
Bf
(
1
,
2
)
+
b
[
0
]
*
Bf
(
0
,
1
)
+
b
[
1
]
*
Bf
(
0
,
0
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
0
,
2
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
0
,
2
)
*
Bf
(
0
,
2
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -392,6 +428,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
diff
=
t
-
(
a
[
0
]
*
Bf
(
2
,
0
)
+
a
[
2
]
*
Bf
(
0
,
0
)
+
b
[
1
]
*
Bf
(
1
,
2
)
+
b
[
2
]
*
Bf
(
1
,
1
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
1
,
0
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
1
,
0
)
*
Bf
(
1
,
0
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -407,7 +446,10 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
t
=
((
s
<
0.0
)
?
-
s
:
s
);
assert
(
t
==
fabs
(
s
));
diff
=
t
-
(
a
[
0
]
*
Bf
(
2
,
1
)
+
a
[
2
]
*
Bf
(
0
,
1
)
+
b
[
0
]
*
Bf
(
1
,
2
)
+
b
[
2
]
*
Bf
(
1
,
0
));
b
[
2
]
*
Bf
(
1
,
0
)
+
b
[
0
]
*
Bf
(
1
,
2
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
1
,
1
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
1
,
1
)
*
Bf
(
1
,
1
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -424,6 +466,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
diff
=
t
-
(
a
[
0
]
*
Bf
(
2
,
2
)
+
a
[
2
]
*
Bf
(
0
,
2
)
+
b
[
0
]
*
Bf
(
1
,
1
)
+
b
[
1
]
*
Bf
(
1
,
0
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
1
,
2
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
1
,
2
)
*
Bf
(
1
,
2
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -440,6 +485,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
diff
=
t
-
(
a
[
0
]
*
Bf
(
1
,
0
)
+
a
[
1
]
*
Bf
(
0
,
0
)
+
b
[
1
]
*
Bf
(
2
,
2
)
+
b
[
2
]
*
Bf
(
2
,
1
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
2
,
0
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
2
,
0
)
*
Bf
(
2
,
0
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -455,7 +503,10 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
t
=
((
s
<
0.0
)
?
-
s
:
s
);
assert
(
t
==
fabs
(
s
));
diff
=
t
-
(
a
[
0
]
*
Bf
(
1
,
1
)
+
a
[
1
]
*
Bf
(
0
,
1
)
+
b
[
0
]
*
Bf
(
2
,
2
)
+
b
[
2
]
*
Bf
(
2
,
0
));
b
[
2
]
*
Bf
(
2
,
0
)
+
b
[
0
]
*
Bf
(
2
,
2
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
2
,
1
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
2
,
1
)
*
Bf
(
2
,
1
);
if
(
sinus2
>
1e-6
)
{
...
...
@@ -472,6 +523,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
diff
=
t
-
(
a
[
0
]
*
Bf
(
1
,
2
)
+
a
[
1
]
*
Bf
(
0
,
2
)
+
b
[
0
]
*
Bf
(
2
,
1
)
+
b
[
1
]
*
Bf
(
2
,
0
));
obbDisjointAndLowerBoundDistanceCase
(
B
,
T
,
a
,
b
,
breakDistance2
,
Bf
,
2
,
2
,
squaredLowerBoundDistance
,
diff2
);
assert
(
diff
==
diff2
);
if
(
diff
>
0
)
{
sinus2
=
1
-
Bf
(
2
,
2
)
*
Bf
(
2
,
2
);
if
(
sinus2
>
1e-6
)
{
...
...
src/BV/OBBRSS.cpp
View file @
e50d403a
...
...
@@ -42,23 +42,6 @@ namespace hpp
namespace
fcl
{
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
)
{
return
overlap
(
R0
,
T0
,
b1
.
obb
,
b2
.
obb
);
}
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
,
const
CollisionRequest
&
request
,
FCL_REAL
&
sqrDistLowerBound
)
{
return
overlap
(
R0
,
T0
,
b1
.
obb
,
b2
.
obb
,
request
,
sqrDistLowerBound
);
}
FCL_REAL
distance
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
,
Vec3f
*
P
,
Vec3f
*
Q
)
{
return
distance
(
R0
,
T0
,
b1
.
rss
,
b2
.
rss
,
P
,
Q
);
}
OBBRSS
translate
(
const
OBBRSS
&
bv
,
const
Vec3f
&
t
)
{
OBBRSS
res
(
bv
);
...
...
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