Skip to content
GitLab
Menu
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
3840c19c
Commit
3840c19c
authored
May 04, 2020
by
Joseph Mirabel
Browse files
Improve GJK guess
parent
c0ebf022
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/narrowphase/gjk.cpp
View file @
3840c19c
...
...
@@ -533,11 +533,38 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
ray
=
guess
;
support_hint
=
supportHint
;
if
(
ray
.
squaredNorm
()
>
0
)
appendVertex
(
simplices
[
0
],
-
ray
,
false
,
support_hint
);
else
appendVertex
(
simplices
[
0
],
Vec3f
(
1
,
0
,
0
),
true
,
support_hint
);
FCL_REAL
rl
=
ray
.
squaredNorm
();
if
(
rl
>
0
)
{
rl
=
std
::
sqrt
(
rl
);
ray
/=
rl
;
}
else
{
ray
=
Vec3f
(
-
1
,
0
,
0
);
rl
=
1
;
}
appendVertex
(
simplices
[
0
],
-
ray
,
true
,
support_hint
);
FCL_REAL
omega
=
ray
.
dot
(
simplices
[
0
].
vertex
[
0
]
->
w
);
if
(
omega
>
upper_bound
)
{
distance
=
omega
-
inflation
;
simplex
=
&
simplices
[
current
];
return
Valid
;
}
if
(
omega
>
0
)
{
alpha
=
omega
;
if
((
rl
-
omega
)
-
tolerance
*
rl
<=
0
)
{
removeVertex
(
simplices
[
current
]);
distance
=
rl
-
inflation
;
simplex
=
&
simplices
[
current
];
std
::
cout
<<
"here"
<<
std
::
endl
;
return
Valid
;
}
}
ray
=
simplices
[
0
].
vertex
[
0
]
->
w
;
FCL_REAL
rl
=
ray
.
norm
();
rl
=
ray
.
norm
();
if
(
rl
==
0
)
{
status
=
Inside
;
distance
=
-
inflation
-
1.
;
...
...
@@ -569,7 +596,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
const
Vec3f
&
w
=
curr_simplex
.
vertex
[
curr_simplex
.
rank
-
1
]
->
w
;
// check B: no collision if omega > 0
FCL_REAL
omega
=
ray
.
dot
(
w
)
/
rl
;
omega
=
ray
.
dot
(
w
)
/
rl
;
if
(
omega
>
upper_bound
)
{
distance
=
omega
-
inflation
;
...
...
test/gjk.cpp
View file @
3840c19c
...
...
@@ -371,7 +371,6 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision,
// Check that guess works as expected
Vec3f
guess
=
gjk
.
getGuessFromSimplex
();
// TODO the max number of iteration should be 0
details
::
GJK
gjk2
(
0
,
1e-6
);
details
::
GJK
::
Status
status2
=
gjk2
.
evaluate
(
shape
,
guess
);
BOOST_CHECK_EQUAL
(
status2
,
details
::
GJK
::
Valid
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a 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