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
bcbd9e72
Commit
bcbd9e72
authored
Aug 28, 2019
by
Joseph Mirabel
Browse files
Add MinkowskiDiff::support prototype.
parent
d691c1a9
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @
bcbd9e72
...
...
@@ -66,7 +66,11 @@ struct MinkowskiDiff
/// @brief transform from shape1 to shape0
Transform3f
toshape0
;
MinkowskiDiff
()
{
}
typedef
void
(
*
GetSupportFunction
)
(
const
MinkowskiDiff
&
minkowskiDiff
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
Vec3f
&
support
);
GetSupportFunction
getSupportFunc
;
MinkowskiDiff
()
:
getSupportFunc
(
NULL
)
{}
void
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
);
...
...
@@ -88,6 +92,13 @@ struct MinkowskiDiff
return
support0
(
d
)
-
support1
(
-
d
);
}
/// @brief support function for the pair of shapes
inline
void
support
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
Vec3f
&
supp
)
const
{
assert
(
getSupportFunc
!=
NULL
);
getSupportFunc
(
*
this
,
d
,
dIsNormalized
,
supp
);
}
/// @brief support function for the d-th shape (d = 0 or 1)
inline
Vec3f
support
(
const
Vec3f
&
d
,
size_t
index
)
const
{
...
...
src/narrowphase/gjk.cpp
View file @
bcbd9e72
...
...
@@ -46,6 +46,26 @@ namespace fcl
namespace
details
{
struct
shape_traits_base
{
enum
{
NeedNormalizedDir
=
true
};
};
template
<
typename
Shape
>
struct
shape_traits
:
shape_traits_base
{};
template
<
>
struct
shape_traits
<
TriangleP
>
:
shape_traits_base
{
enum
{
NeedNormalizedDir
=
false
};
};
template
<
>
struct
shape_traits
<
Box
>
:
shape_traits_base
{
enum
{
NeedNormalizedDir
=
false
};
};
void
getShapeSupport
(
const
TriangleP
*
triangle
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
FCL_REAL
dota
=
dir
.
dot
(
triangle
->
a
);
...
...
@@ -192,10 +212,78 @@ void getSupportTpl (const Shape0* s0, const Shape1* s1,
support
.
noalias
()
-=
oM1
*
support1
+
ot1
;
}
template
<
typename
Shape0
,
typename
Shape1
>
void
getSupportFuncTpl
(
const
MinkowskiDiff
&
md
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
Vec3f
&
support
)
{
enum
{
NeedNormalizedDir
=
bool
(
(
bool
)
shape_traits
<
Shape0
>::
NeedNormalizedDir
||
(
bool
)
shape_traits
<
Shape1
>::
NeedNormalizedDir
)
};
getSupportTpl
<
Shape0
,
Shape1
>
(
static_cast
<
const
Shape0
*>
(
md
.
shapes
[
0
]),
static_cast
<
const
Shape1
*>
(
md
.
shapes
[
1
]),
md
.
toshape0
.
getRotation
(),
md
.
toshape0
.
getTranslation
(),
(
NeedNormalizedDir
&&
!
dirIsNormalized
)
?
dir
.
normalized
()
:
dir
,
support
);
}
template
<
typename
Shape0
>
MinkowskiDiff
::
GetSupportFunction
makeGetSupportFunction1
(
const
ShapeBase
*
s1
)
{
switch
(
s1
->
getNodeType
())
{
case
GEOM_TRIANGLE
:
return
getSupportFuncTpl
<
Shape0
,
TriangleP
>
;
case
GEOM_BOX
:
return
getSupportFuncTpl
<
Shape0
,
Box
>
;
case
GEOM_SPHERE
:
return
getSupportFuncTpl
<
Shape0
,
Sphere
>
;
case
GEOM_CAPSULE
:
return
getSupportFuncTpl
<
Shape0
,
Capsule
>
;
case
GEOM_CONE
:
return
getSupportFuncTpl
<
Shape0
,
Cone
>
;
case
GEOM_CYLINDER
:
return
getSupportFuncTpl
<
Shape0
,
Cylinder
>
;
case
GEOM_CONVEX
:
return
getSupportFuncTpl
<
Shape0
,
Convex
>
;
default:
throw
std
::
logic_error
(
"Unsupported geometric shape"
);
}
}
void
MinkowskiDiff
::
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
)
{
shapes
[
0
]
=
shape0
;
shapes
[
1
]
=
shape1
;
switch
(
shape0
->
getNodeType
())
{
case
GEOM_TRIANGLE
:
getSupportFunc
=
makeGetSupportFunction1
<
TriangleP
>
(
shape1
);
break
;
case
GEOM_BOX
:
getSupportFunc
=
makeGetSupportFunction1
<
Box
>
(
shape1
);
break
;
case
GEOM_SPHERE
:
getSupportFunc
=
makeGetSupportFunction1
<
Sphere
>
(
shape1
);
break
;
case
GEOM_CAPSULE
:
getSupportFunc
=
makeGetSupportFunction1
<
Capsule
>
(
shape1
);
break
;
case
GEOM_CONE
:
getSupportFunc
=
makeGetSupportFunction1
<
Cone
>
(
shape1
);
break
;
case
GEOM_CYLINDER
:
getSupportFunc
=
makeGetSupportFunction1
<
Cylinder
>
(
shape1
);
break
;
case
GEOM_CONVEX
:
getSupportFunc
=
makeGetSupportFunction1
<
Convex
>
(
shape1
);
break
;
default:
throw
std
::
logic_error
(
"Unsupported geometric shape"
);
}
}
void
GJK
::
initialize
()
...
...
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