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
9812d9bc
Commit
9812d9bc
authored
Aug 28, 2019
by
Joseph Mirabel
Browse files
Reflect previous changes in GJK.
parent
bcbd9e72
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @
9812d9bc
...
...
@@ -156,13 +156,13 @@ struct GJK
Status
evaluate
(
const
MinkowskiDiff
&
shape_
,
const
Vec3f
&
guess
);
/// @brief apply the support function along a direction, the result is return in sv
void
getSupport
(
const
Vec3f
&
d
,
SimplexV
&
sv
)
const
;
void
getSupport
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
SimplexV
&
sv
)
const
;
/// @brief discard one vertex from the simplex
void
removeVertex
(
Simplex
&
simplex
);
/// @brief append one vertex to the simplex
void
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
);
void
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
,
bool
isNormalized
=
false
);
/// @brief whether the simplex enclose the origin
bool
encloseOrigin
();
...
...
src/narrowphase/gjk.cpp
View file @
9812d9bc
...
...
@@ -323,7 +323,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
ray
=
guess
;
if
(
ray
.
squaredNorm
()
>
0
)
appendVertex
(
simplices
[
0
],
-
ray
);
else
appendVertex
(
simplices
[
0
],
Vec3f
(
1
,
0
,
0
));
else
appendVertex
(
simplices
[
0
],
Vec3f
(
1
,
0
,
0
)
,
true
);
simplices
[
0
].
coefficient
[
0
]
=
1
;
ray
=
simplices
[
0
].
vertex
[
0
]
->
w
;
lastw
[
0
]
=
lastw
[
1
]
=
lastw
[
2
]
=
lastw
[
3
]
=
ray
;
// cache previous support points, the new support point will compare with it to avoid too close support points
...
...
@@ -417,10 +417,11 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
return
status
;
}
void
GJK
::
getSupport
(
const
Vec3f
&
d
,
SimplexV
&
sv
)
const
void
GJK
::
getSupport
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
SimplexV
&
sv
)
const
{
sv
.
d
.
noalias
()
=
d
.
normalized
();
sv
.
w
.
noalias
()
=
shape
.
support
(
sv
.
d
);
if
(
dIsNormalized
)
sv
.
d
=
d
;
else
sv
.
d
.
noalias
()
=
d
.
normalized
();
shape
.
support
(
sv
.
d
,
true
,
sv
.
w
);
}
void
GJK
::
removeVertex
(
Simplex
&
simplex
)
...
...
@@ -428,11 +429,11 @@ void GJK::removeVertex(Simplex& simplex)
free_v
[
nfree
++
]
=
simplex
.
vertex
[
--
simplex
.
rank
];
}
void
GJK
::
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
)
void
GJK
::
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
,
bool
isNormalized
)
{
simplex
.
coefficient
[
simplex
.
rank
]
=
0
;
// initial weight 0
simplex
.
vertex
[
simplex
.
rank
]
=
free_v
[
--
nfree
];
// set the memory
getSupport
(
v
,
*
simplex
.
vertex
[
simplex
.
rank
++
]);
getSupport
(
v
,
isNormalized
,
*
simplex
.
vertex
[
simplex
.
rank
++
]);
}
bool
GJK
::
encloseOrigin
()
...
...
@@ -445,10 +446,10 @@ bool GJK::encloseOrigin()
{
Vec3f
axis
(
Vec3f
::
Zero
());
axis
[
i
]
=
1
;
appendVertex
(
*
simplex
,
axis
);
appendVertex
(
*
simplex
,
axis
,
true
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
appendVertex
(
*
simplex
,
-
axis
);
appendVertex
(
*
simplex
,
-
axis
,
true
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
}
...
...
@@ -662,7 +663,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
SimplexV
*
w
=
&
sv_store
[
nextsv
++
];
bool
valid
=
true
;
best
->
pass
=
++
pass
;
gjk
.
getSupport
(
best
->
n
,
*
w
);
// At the moment, SimplexF.n is always normalized. This could be revised in the future...
gjk
.
getSupport
(
best
->
n
,
true
,
*
w
);
FCL_REAL
wdist
=
best
->
n
.
dot
(
w
->
w
)
-
best
->
d
;
if
(
wdist
>
tolerance
)
{
...
...
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