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
b4d102f5
Commit
b4d102f5
authored
Aug 30, 2019
by
Joseph Mirabel
Browse files
[GJK] Store support for each shape in SimplexV.
parent
1bac302a
Changes
3
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @
b4d102f5
...
...
@@ -72,7 +72,7 @@ struct MinkowskiDiff
Vec3f
ot1
;
typedef
void
(
*
GetSupportFunction
)
(
const
MinkowskiDiff
&
minkowskiDiff
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
Vec3f
&
support
);
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
Vec3f
&
support
0
,
Vec3f
&
support1
);
GetSupportFunction
getSupportFunc
;
MinkowskiDiff
()
:
getSupportFunc
(
NULL
)
{}
...
...
@@ -98,10 +98,10 @@ struct MinkowskiDiff
}
/// @brief support function for the pair of shapes
inline
void
support
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
Vec3f
&
supp
)
const
inline
void
support
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
Vec3f
&
supp
0
,
Vec3f
&
supp1
)
const
{
assert
(
getSupportFunc
!=
NULL
);
getSupportFunc
(
*
this
,
d
,
dIsNormalized
,
supp
);
getSupportFunc
(
*
this
,
d
,
dIsNormalized
,
supp
0
,
supp1
);
}
};
...
...
@@ -112,12 +112,10 @@ struct GJK
{
struct
SimplexV
{
/// @brief support
direction
Vec3f
d
;
/// @brief support
vector for shape 0 and 1.
Vec3f
w0
,
w1
;
/// @brieg support vector (i.e., the furthest point on the shape along the support direction)
Vec3f
w
;
SimplexV
()
:
d
(
Vec3f
::
Zero
()),
w
(
Vec3f
::
Zero
())
{}
};
struct
Simplex
...
...
include/hpp/fcl/narrowphase/narrowphase.h
View file @
b4d102f5
...
...
@@ -80,8 +80,7 @@ namespace fcl
Vec3f
w0
(
Vec3f
::
Zero
());
for
(
size_t
i
=
0
;
i
<
epa
.
result
.
rank
;
++
i
)
{
w0
+=
shape
.
support0
(
epa
.
result
.
vertex
[
i
]
->
d
,
false
)
*
epa
.
result
.
coefficient
[
i
];
w0
+=
epa
.
result
.
vertex
[
i
]
->
w0
*
epa
.
result
.
coefficient
[
i
];
}
if
(
penetration_depth
)
*
penetration_depth
=
-
epa
.
depth
;
if
(
normal
)
*
normal
=
tf2
.
getRotation
()
*
epa
.
normal
;
...
...
@@ -131,8 +130,7 @@ namespace fcl
Vec3f
w0
(
Vec3f
::
Zero
());
for
(
size_t
i
=
0
;
i
<
epa
.
result
.
rank
;
++
i
)
{
w0
+=
shape
.
support0
(
epa
.
result
.
vertex
[
i
]
->
d
,
false
)
*
epa
.
result
.
coefficient
[
i
];
w0
+=
epa
.
result
.
vertex
[
i
]
->
w0
*
epa
.
result
.
coefficient
[
i
];
}
distance
=
-
epa
.
depth
;
normal
=
-
epa
.
normal
;
...
...
@@ -221,8 +219,7 @@ namespace fcl
Vec3f
w0
(
Vec3f
::
Zero
());
for
(
size_t
i
=
0
;
i
<
epa
.
result
.
rank
;
++
i
)
{
w0
+=
shape
.
support0
(
epa
.
result
.
vertex
[
i
]
->
d
,
false
)
*
epa
.
result
.
coefficient
[
i
];
w0
+=
epa
.
result
.
vertex
[
i
]
->
w0
*
epa
.
result
.
coefficient
[
i
];
}
assert
(
epa
.
depth
>=
-
eps
);
distance
=
std
::
min
(
0.
,
-
epa
.
depth
);
...
...
src/narrowphase/gjk.cpp
View file @
b4d102f5
...
...
@@ -213,17 +213,16 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
template
<
typename
Shape0
,
typename
Shape1
>
void
getSupportTpl
(
const
Shape0
*
s0
,
const
Shape1
*
s1
,
const
Matrix3f
&
oR1
,
const
Vec3f
&
ot1
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
const
Vec3f
&
dir
,
Vec3f
&
support
0
,
Vec3f
&
support1
)
{
getShapeSupport
(
s0
,
dir
,
support
);
Vec3f
support1
;
getShapeSupport
(
s0
,
dir
,
support0
);
getShapeSupport
(
s1
,
-
oR1
.
transpose
()
*
dir
,
support1
);
support
.
noalias
()
-
=
oR1
*
support1
+
ot1
;
support
1
=
oR1
*
support1
+
ot1
;
}
template
<
typename
Shape0
,
typename
Shape1
>
void
getSupportFuncTpl
(
const
MinkowskiDiff
&
md
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
Vec3f
&
support
)
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
Vec3f
&
support
0
,
Vec3f
&
support1
)
{
enum
{
NeedNormalizedDir
=
bool
(
(
bool
)
shape_traits
<
Shape0
>::
NeedNormalizedDir
...
...
@@ -234,7 +233,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
static_cast
<
const
Shape1
*>
(
md
.
shapes
[
1
]),
md
.
oR1
,
md
.
ot1
,
(
NeedNormalizedDir
&&
!
dirIsNormalized
)
?
dir
.
normalized
()
:
dir
,
support
);
support
0
,
support1
);
}
template
<
typename
Shape0
>
...
...
@@ -316,8 +315,8 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) co
for
(
size_t
i
=
0
;
i
<
getSimplex
()
->
rank
;
++
i
)
{
FCL_REAL
p
=
getSimplex
()
->
coefficient
[
i
];
w0
+=
shape
.
support0
(
getSimplex
()
->
vertex
[
i
]
->
d
,
false
)
*
p
;
w1
+=
shape
.
support1
(
-
getSimplex
()
->
vertex
[
i
]
->
d
,
false
)
*
p
;
w0
+=
getSimplex
()
->
vertex
[
i
]
->
w0
*
p
;
w1
+=
getSimplex
()
->
vertex
[
i
]
->
w1
*
p
;
}
return
true
;
}
...
...
@@ -489,9 +488,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
void
GJK
::
getSupport
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
SimplexV
&
sv
)
const
{
// Was sv.d.noalias() = d.normalized();
sv
.
d
.
noalias
()
=
d
;
shape
.
support
(
sv
.
d
,
dIsNormalized
,
sv
.
w
);
shape
.
support
(
d
,
dIsNormalized
,
sv
.
w0
,
sv
.
w1
);
sv
.
w
.
noalias
()
=
sv
.
w0
-
sv
.
w1
;
}
void
GJK
::
removeVertex
(
Simplex
&
simplex
)
...
...
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