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
3351029d
Commit
3351029d
authored
Nov 14, 2019
by
LucileRemigy
Committed by
Joseph Mirabel
Nov 14, 2019
Browse files
Delete lz for Cone, Cylinder and Capsule, now use halfLength (#98)
parent
36604bee
Changes
8
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/shape/geometric_shapes.h
View file @
3351029d
...
@@ -151,20 +151,16 @@ public:
...
@@ -151,20 +151,16 @@ public:
class
Capsule
:
public
ShapeBase
class
Capsule
:
public
ShapeBase
{
{
public:
public:
Capsule
(
FCL_REAL
radius_
,
FCL_REAL
lz_
)
:
ShapeBase
(),
radius
(
radius_
)
,
lz
(
lz_
)
Capsule
(
FCL_REAL
radius_
,
FCL_REAL
lz_
)
:
ShapeBase
(),
radius
(
radius_
)
{
{
lz
=
lz_
;
halfLength
=
lz_
/
2
;
HalfLength
=
lz_
/
2
;
}
}
/// @brief Radius of capsule
/// @brief Radius of capsule
FCL_REAL
radius
;
FCL_REAL
radius
;
/// @brief Length along z axis
FCL_REAL
lz
;
/// @brief Half Length along z axis
/// @brief Half Length along z axis
FCL_REAL
H
alfLength
;
FCL_REAL
h
alfLength
;
/// @brief Compute AABB
/// @brief Compute AABB
void
computeLocalAABB
();
void
computeLocalAABB
();
...
@@ -174,15 +170,15 @@ public:
...
@@ -174,15 +170,15 @@ public:
FCL_REAL
computeVolume
()
const
FCL_REAL
computeVolume
()
const
{
{
return
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
radius
*
radius
*
(
lz
+
radius
*
4
/
3.0
);
return
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
radius
*
radius
*
(
(
halfLength
*
2
)
+
radius
*
4
/
3.0
);
}
}
Matrix3f
computeMomentofInertia
()
const
Matrix3f
computeMomentofInertia
()
const
{
{
FCL_REAL
v_cyl
=
radius
*
radius
*
lz
*
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
();
FCL_REAL
v_cyl
=
radius
*
radius
*
(
halfLength
*
2
)
*
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
();
FCL_REAL
v_sph
=
radius
*
radius
*
radius
*
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
4
/
3.0
;
FCL_REAL
v_sph
=
radius
*
radius
*
radius
*
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
4
/
3.0
;
FCL_REAL
ix
=
v_cyl
*
lz
*
lz
/
12.0
+
0.25
*
v_cyl
*
radius
+
0.4
*
v_sph
*
radius
*
radius
+
0.25
*
v_sph
*
lz
*
lz
;
FCL_REAL
ix
=
v_cyl
*
halfLength
*
halfLength
/
3.
+
0.25
*
v_cyl
*
radius
+
0.4
*
v_sph
*
radius
*
radius
+
v_sph
*
halfLength
*
halfLength
;
FCL_REAL
iz
=
(
0.5
*
v_cyl
+
0.4
*
v_sph
)
*
radius
*
radius
;
FCL_REAL
iz
=
(
0.5
*
v_cyl
+
0.4
*
v_sph
)
*
radius
*
radius
;
return
(
Matrix3f
()
<<
ix
,
0
,
0
,
return
(
Matrix3f
()
<<
ix
,
0
,
0
,
...
@@ -196,20 +192,16 @@ public:
...
@@ -196,20 +192,16 @@ public:
class
Cone
:
public
ShapeBase
class
Cone
:
public
ShapeBase
{
{
public:
public:
Cone
(
FCL_REAL
radius_
,
FCL_REAL
lz_
)
:
ShapeBase
(),
radius
(
radius_
)
,
lz
(
lz_
)
Cone
(
FCL_REAL
radius_
,
FCL_REAL
lz_
)
:
ShapeBase
(),
radius
(
radius_
)
{
{
lz
=
lz_
;
halfLength
=
lz_
/
2
;
HalfLength
=
lz_
/
2
;
}
}
/// @brief Radius of the cone
/// @brief Radius of the cone
FCL_REAL
radius
;
FCL_REAL
radius
;
/// @brief Length along z axis
FCL_REAL
lz
;
/// @brief Half Length along z axis
/// @brief Half Length along z axis
FCL_REAL
H
alfLength
;
FCL_REAL
h
alfLength
;
/// @brief Compute AABB
/// @brief Compute AABB
void
computeLocalAABB
();
void
computeLocalAABB
();
...
@@ -219,13 +211,14 @@ public:
...
@@ -219,13 +211,14 @@ public:
FCL_REAL
computeVolume
()
const
FCL_REAL
computeVolume
()
const
{
{
return
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
radius
*
radius
*
lz
/
3
;
return
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
radius
*
radius
*
(
halfLength
*
2
)
/
3
;
}
}
/// \todo verify this formula as it seems different from https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors
Matrix3f
computeMomentofInertia
()
const
Matrix3f
computeMomentofInertia
()
const
{
{
FCL_REAL
V
=
computeVolume
();
FCL_REAL
V
=
computeVolume
();
FCL_REAL
ix
=
V
*
(
0.
1
*
lz
*
lz
+
3
*
radius
*
radius
/
20
);
FCL_REAL
ix
=
V
*
(
0.
4
*
halfLength
*
halfLength
+
3
*
radius
*
radius
/
20
);
FCL_REAL
iz
=
0.3
*
V
*
radius
*
radius
;
FCL_REAL
iz
=
0.3
*
V
*
radius
*
radius
;
return
(
Matrix3f
()
<<
ix
,
0
,
0
,
return
(
Matrix3f
()
<<
ix
,
0
,
0
,
...
@@ -235,7 +228,7 @@ public:
...
@@ -235,7 +228,7 @@ public:
Vec3f
computeCOM
()
const
Vec3f
computeCOM
()
const
{
{
return
Vec3f
(
0
,
0
,
-
0.
2
5
*
lz
);
return
Vec3f
(
0
,
0
,
-
0.5
*
halfLength
);
}
}
};
};
...
@@ -243,20 +236,16 @@ public:
...
@@ -243,20 +236,16 @@ public:
class
Cylinder
:
public
ShapeBase
class
Cylinder
:
public
ShapeBase
{
{
public:
public:
Cylinder
(
FCL_REAL
radius_
,
FCL_REAL
lz_
)
:
ShapeBase
(),
radius
(
radius_
)
,
lz
(
lz_
)
Cylinder
(
FCL_REAL
radius_
,
FCL_REAL
lz_
)
:
ShapeBase
(),
radius
(
radius_
)
{
{
lz
=
lz_
;
halfLength
=
lz_
/
2
;
HalfLength
=
lz_
/
2
;
}
}
/// @brief Radius of the cylinder
/// @brief Radius of the cylinder
FCL_REAL
radius
;
FCL_REAL
radius
;
/// @brief Length along z axis
FCL_REAL
lz
;
/// @brief Half Length along z axis
/// @brief Half Length along z axis
FCL_REAL
H
alfLength
;
FCL_REAL
h
alfLength
;
/// @brief Compute AABB
/// @brief Compute AABB
void
computeLocalAABB
();
void
computeLocalAABB
();
...
@@ -266,13 +255,13 @@ public:
...
@@ -266,13 +255,13 @@ public:
FCL_REAL
computeVolume
()
const
FCL_REAL
computeVolume
()
const
{
{
return
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
radius
*
radius
*
lz
;
return
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
radius
*
radius
*
(
halfLength
*
2
)
;
}
}
Matrix3f
computeMomentofInertia
()
const
Matrix3f
computeMomentofInertia
()
const
{
{
FCL_REAL
V
=
computeVolume
();
FCL_REAL
V
=
computeVolume
();
FCL_REAL
ix
=
V
*
(
3
*
radius
*
radius
+
lz
*
lz
)
/
12
;
FCL_REAL
ix
=
V
*
(
radius
*
radius
/
4
+
halfLength
*
halfLength
/
3
)
;
FCL_REAL
iz
=
V
*
radius
*
radius
/
2
;
FCL_REAL
iz
=
V
*
radius
*
radius
/
2
;
return
(
Matrix3f
()
<<
ix
,
0
,
0
,
return
(
Matrix3f
()
<<
ix
,
0
,
0
,
0
,
ix
,
0
,
0
,
ix
,
0
,
...
...
python/collision-geometries.cc
View file @
3351029d
...
@@ -122,13 +122,13 @@ void exposeShapes ()
...
@@ -122,13 +122,13 @@ void exposeShapes ()
class_
<
Capsule
,
bases
<
ShapeBase
>
,
shared_ptr
<
Capsule
>
>
class_
<
Capsule
,
bases
<
ShapeBase
>
,
shared_ptr
<
Capsule
>
>
(
"Capsule"
,
init
<
FCL_REAL
,
FCL_REAL
>
())
(
"Capsule"
,
init
<
FCL_REAL
,
FCL_REAL
>
())
.
def_readwrite
(
"radius"
,
&
Capsule
::
radius
)
.
def_readwrite
(
"radius"
,
&
Capsule
::
radius
)
.
def_readwrite
(
"
lz
"
,
&
Capsule
::
lz
)
.
def_readwrite
(
"
halfLength
"
,
&
Capsule
::
halfLength
)
;
;
class_
<
Cone
,
bases
<
ShapeBase
>
,
shared_ptr
<
Cone
>
>
class_
<
Cone
,
bases
<
ShapeBase
>
,
shared_ptr
<
Cone
>
>
(
"Cone"
,
init
<
FCL_REAL
,
FCL_REAL
>
())
(
"Cone"
,
init
<
FCL_REAL
,
FCL_REAL
>
())
.
def_readwrite
(
"radius"
,
&
Cone
::
radius
)
.
def_readwrite
(
"radius"
,
&
Cone
::
radius
)
.
def_readwrite
(
"
lz"
,
&
Cone
::
lz
)
.
def_readwrite
(
"
halfLength"
,
&
Cone
::
halfLength
)
;
;
class_
<
ConvexBase
,
bases
<
ShapeBase
>
,
shared_ptr
<
ConvexBase
>
,
noncopyable
>
class_
<
ConvexBase
,
bases
<
ShapeBase
>
,
shared_ptr
<
ConvexBase
>
,
noncopyable
>
...
@@ -148,7 +148,7 @@ void exposeShapes ()
...
@@ -148,7 +148,7 @@ void exposeShapes ()
class_
<
Cylinder
,
bases
<
ShapeBase
>
,
shared_ptr
<
Cylinder
>
>
class_
<
Cylinder
,
bases
<
ShapeBase
>
,
shared_ptr
<
Cylinder
>
>
(
"Cylinder"
,
init
<
FCL_REAL
,
FCL_REAL
>
())
(
"Cylinder"
,
init
<
FCL_REAL
,
FCL_REAL
>
())
.
def_readwrite
(
"radius"
,
&
Cylinder
::
radius
)
.
def_readwrite
(
"radius"
,
&
Cylinder
::
radius
)
.
def_readwrite
(
"
lz
"
,
&
Cylinder
::
lz
)
.
def_readwrite
(
"
halfLength
"
,
&
Cylinder
::
halfLength
)
;
;
class_
<
Halfspace
,
bases
<
ShapeBase
>
,
shared_ptr
<
Halfspace
>
>
class_
<
Halfspace
,
bases
<
ShapeBase
>
,
shared_ptr
<
Halfspace
>
>
...
...
src/distance_capsule_capsule.cpp
View file @
3351029d
...
@@ -41,8 +41,8 @@ namespace fcl {
...
@@ -41,8 +41,8 @@ namespace fcl {
// We assume that capsules are oriented along z-axis.
// We assume that capsules are oriented along z-axis.
Matrix3f
::
ConstColXpr
direction1
=
tf1
.
getRotation
().
col
(
2
);
Matrix3f
::
ConstColXpr
direction1
=
tf1
.
getRotation
().
col
(
2
);
Matrix3f
::
ConstColXpr
direction2
=
tf2
.
getRotation
().
col
(
2
);
Matrix3f
::
ConstColXpr
direction2
=
tf2
.
getRotation
().
col
(
2
);
FCL_REAL
halfLength1
=
0.5
*
c1
->
lz
;
FCL_REAL
halfLength1
=
c1
->
halfLength
;
FCL_REAL
halfLength2
=
0.5
*
c2
->
lz
;
FCL_REAL
halfLength2
=
c2
->
halfLength
;
Vec3f
diff
=
center1
-
center2
;
Vec3f
diff
=
center1
-
center2
;
FCL_REAL
a01
=
-
direction1
.
dot
(
direction2
);
FCL_REAL
a01
=
-
direction1
.
dot
(
direction2
);
...
...
src/narrowphase/details.h
View file @
3351029d
...
@@ -73,8 +73,8 @@ namespace fcl {
...
@@ -73,8 +73,8 @@ namespace fcl {
const
Capsule
&
s2
,
const
Transform3f
&
tf2
,
const
Capsule
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal_
)
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal_
)
{
{
Vec3f
pos1
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
0.5
*
s2
.
lz
)));
// from distance function
Vec3f
pos1
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
s2
.
halfLength
)));
// from distance function
Vec3f
pos2
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
-
0.5
*
s2
.
lz
)));
Vec3f
pos2
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
-
s2
.
halfLength
)));
Vec3f
s_c
=
tf1
.
getTranslation
();
Vec3f
s_c
=
tf1
.
getTranslation
();
Vec3f
segment_point
;
Vec3f
segment_point
;
...
@@ -108,8 +108,8 @@ namespace fcl {
...
@@ -108,8 +108,8 @@ namespace fcl {
const
Capsule
&
s2
,
const
Transform3f
&
tf2
,
const
Capsule
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
dist
,
Vec3f
&
p1
,
Vec3f
&
p2
,
Vec3f
&
normal
)
FCL_REAL
&
dist
,
Vec3f
&
p1
,
Vec3f
&
p2
,
Vec3f
&
normal
)
{
{
Vec3f
pos1
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
0.5
*
s2
.
lz
)));
Vec3f
pos1
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
s2
.
halfLength
)));
Vec3f
pos2
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
-
0.5
*
s2
.
lz
)));
Vec3f
pos2
(
tf2
.
transform
(
Vec3f
(
0.
,
0.
,
-
s2
.
halfLength
)));
Vec3f
s_c
=
tf1
.
getTranslation
();
Vec3f
s_c
=
tf1
.
getTranslation
();
Vec3f
segment_point
;
Vec3f
segment_point
;
...
@@ -143,7 +143,7 @@ namespace fcl {
...
@@ -143,7 +143,7 @@ namespace fcl {
static
const
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
static
const
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
FCL_REAL
r1
(
s1
.
radius
);
FCL_REAL
r1
(
s1
.
radius
);
FCL_REAL
r2
(
s2
.
radius
);
FCL_REAL
r2
(
s2
.
radius
);
FCL_REAL
lz2
(
.5
*
s2
.
lz
);
FCL_REAL
lz2
(
s2
.
halfLength
);
// boundaries of the cylinder axis
// boundaries of the cylinder axis
Vec3f
A
(
tf2
.
transform
(
Vec3f
(
0
,
0
,
-
lz2
)));
Vec3f
A
(
tf2
.
transform
(
Vec3f
(
0
,
0
,
-
lz2
)));
Vec3f
B
(
tf2
.
transform
(
Vec3f
(
0
,
0
,
lz2
)));
Vec3f
B
(
tf2
.
transform
(
Vec3f
(
0
,
0
,
lz2
)));
...
@@ -151,7 +151,7 @@ namespace fcl {
...
@@ -151,7 +151,7 @@ namespace fcl {
Vec3f
S
(
tf1
.
getTranslation
());
Vec3f
S
(
tf1
.
getTranslation
());
// axis of the cylinder
// axis of the cylinder
Vec3f
u
(
tf2
.
getRotation
().
col
(
2
));
Vec3f
u
(
tf2
.
getRotation
().
col
(
2
));
assert
((
B
-
A
-
s2
.
lz
*
u
).
norm
()
<
eps
);
assert
((
B
-
A
-
(
s2
.
halfLength
*
2
)
*
u
).
norm
()
<
eps
);
Vec3f
AS
(
S
-
A
);
Vec3f
AS
(
S
-
A
);
// abscissa of S on cylinder axis with A as the origin
// abscissa of S on cylinder axis with A as the origin
FCL_REAL
s
(
u
.
dot
(
AS
));
FCL_REAL
s
(
u
.
dot
(
AS
));
...
@@ -186,7 +186,7 @@ namespace fcl {
...
@@ -186,7 +186,7 @@ namespace fcl {
dist
=
-
r1
;
dist
=
-
r1
;
}
}
}
}
}
else
if
(
s
<=
s2
.
lz
)
{
}
else
if
(
s
<=
(
s2
.
halfLength
*
2
)
)
{
// 0 < s <= s2.lz
// 0 < s <= s2.lz
normal
=
-
v
;
normal
=
-
v
;
dist
=
dPS
-
r1
-
r2
;
dist
=
dPS
-
r1
-
r2
;
...
@@ -200,7 +200,7 @@ namespace fcl {
...
@@ -200,7 +200,7 @@ namespace fcl {
// lz < s
// lz < s
if
(
dPS
<=
r2
)
{
if
(
dPS
<=
r2
)
{
// closest point on cylinder is on cylinder disc basis
// closest point on cylinder is on cylinder disc basis
dist
=
s
-
s2
.
lz
-
r1
;
p1
=
S
-
r1
*
u
;
p2
=
B
+
dPS
*
v
;
normal
=
-
u
;
dist
=
s
-
(
s2
.
halfLength
*
2
)
-
r1
;
p1
=
S
-
r1
*
u
;
p2
=
B
+
dPS
*
v
;
normal
=
-
u
;
}
else
{
}
else
{
// closest point on cylinder is on cylinder circle basis
// closest point on cylinder is on cylinder circle basis
p2
=
B
+
r2
*
v
;
p2
=
B
+
r2
*
v
;
...
@@ -1605,7 +1605,7 @@ namespace fcl {
...
@@ -1605,7 +1605,7 @@ namespace fcl {
int
sign
=
(
cosa
>
0
)
?
-
1
:
1
;
int
sign
=
(
cosa
>
0
)
?
-
1
:
1
;
// closest capsule vertex to halfspace if no collision,
// closest capsule vertex to halfspace if no collision,
// or deeper inside halspace if collision
// or deeper inside halspace if collision
Vec3f
p
=
T
+
dir_z
*
(
s1
.
lz
*
0.5
*
sign
);
Vec3f
p
=
T
+
dir_z
*
(
s1
.
halfLength
*
sign
);
FCL_REAL
signed_dist
=
new_s2
.
signedDistance
(
p
);
FCL_REAL
signed_dist
=
new_s2
.
signedDistance
(
p
);
distance
=
signed_dist
-
s1
.
radius
;
distance
=
signed_dist
-
s1
.
radius
;
...
@@ -1666,7 +1666,7 @@ namespace fcl {
...
@@ -1666,7 +1666,7 @@ namespace fcl {
int
sign
=
(
cosa
>
0
)
?
-
1
:
1
;
int
sign
=
(
cosa
>
0
)
?
-
1
:
1
;
// deepest point
// deepest point
Vec3f
p
=
T
+
dir_z
*
(
s1
.
lz
*
0.5
*
sign
)
+
C
;
Vec3f
p
=
T
+
dir_z
*
(
s1
.
halfLength
*
sign
)
+
C
;
distance
=
new_s2
.
signedDistance
(
p
);
distance
=
new_s2
.
signedDistance
(
p
);
if
(
distance
>
0
)
{
if
(
distance
>
0
)
{
// TODO: compute closest points
// TODO: compute closest points
...
@@ -1708,7 +1708,7 @@ namespace fcl {
...
@@ -1708,7 +1708,7 @@ namespace fcl {
else
else
{
{
normal
=
-
new_s2
.
n
;
normal
=
-
new_s2
.
n
;
p1
=
p2
=
T
-
dir_z
*
(
s1
.
lz
*
0.5
)
-
p1
=
p2
=
T
-
dir_z
*
(
s1
.
halfLength
)
-
new_s2
.
n
*
(
0.5
*
distance
+
s1
.
radius
);
new_s2
.
n
*
(
0.5
*
distance
+
s1
.
radius
);
return
true
;
return
true
;
}
}
...
@@ -1726,8 +1726,8 @@ namespace fcl {
...
@@ -1726,8 +1726,8 @@ namespace fcl {
C
*=
s
;
C
*=
s
;
}
}
Vec3f
a1
=
T
+
dir_z
*
(
0.5
*
s1
.
lz
);
Vec3f
a1
=
T
+
dir_z
*
(
s1
.
halfLength
);
Vec3f
a2
=
T
-
dir_z
*
(
0.5
*
s1
.
lz
)
+
C
;
Vec3f
a2
=
T
-
dir_z
*
(
s1
.
halfLength
)
+
C
;
FCL_REAL
d1
=
new_s2
.
signedDistance
(
a1
);
FCL_REAL
d1
=
new_s2
.
signedDistance
(
a1
);
FCL_REAL
d2
=
new_s2
.
signedDistance
(
a2
);
FCL_REAL
d2
=
new_s2
.
signedDistance
(
a2
);
...
@@ -2153,8 +2153,8 @@ namespace fcl {
...
@@ -2153,8 +2153,8 @@ namespace fcl {
Vec3f
dir_z
=
R1
.
col
(
2
);
Vec3f
dir_z
=
R1
.
col
(
2
);
// ends of capsule inner segment
// ends of capsule inner segment
Vec3f
a1
=
T1
+
dir_z
*
(
0.5
*
s1
.
lz
)
;
Vec3f
a1
=
T1
+
dir_z
*
s1
.
halfLength
;
Vec3f
a2
=
T1
-
dir_z
*
(
0.5
*
s1
.
lz
)
;
Vec3f
a2
=
T1
-
dir_z
*
s1
.
halfLength
;
FCL_REAL
d1
=
new_s2
.
signedDistance
(
a1
);
FCL_REAL
d1
=
new_s2
.
signedDistance
(
a1
);
FCL_REAL
d2
=
new_s2
.
signedDistance
(
a2
);
FCL_REAL
d2
=
new_s2
.
signedDistance
(
a2
);
...
@@ -2278,8 +2278,8 @@ namespace fcl {
...
@@ -2278,8 +2278,8 @@ namespace fcl {
C
*=
s
;
C
*=
s
;
}
}
Vec3f
a1
=
T
+
dir_z
*
(
0.5
*
s1
.
lz
);
Vec3f
a1
=
T
+
dir_z
*
(
s1
.
halfLength
);
Vec3f
a2
=
T
-
dir_z
*
(
0.5
*
s1
.
lz
);
Vec3f
a2
=
T
-
dir_z
*
(
s1
.
halfLength
);
Vec3f
c1
,
c2
;
Vec3f
c1
,
c2
;
if
(
cosa
>
0
)
if
(
cosa
>
0
)
...
@@ -2346,8 +2346,8 @@ namespace fcl {
...
@@ -2346,8 +2346,8 @@ namespace fcl {
else
else
{
{
if
(
d
<
0
)
normal
=
new_s2
.
n
;
else
normal
=
-
new_s2
.
n
;
if
(
d
<
0
)
normal
=
new_s2
.
n
;
else
normal
=
-
new_s2
.
n
;
p1
=
p2
=
T
-
dir_z
*
(
0.5
*
s1
.
lz
)
+
p1
=
p2
=
T
-
dir_z
*
(
s1
.
halfLength
)
+
dir_z
*
(
-
0.5
*
distance
/
s1
.
radius
*
s1
.
lz
)
-
new_s2
.
n
*
d
;
dir_z
*
(
-
distance
/
s1
.
radius
*
s1
.
halfLength
)
-
new_s2
.
n
*
d
;
return
true
;
return
true
;
}
}
}
}
...
@@ -2365,9 +2365,9 @@ namespace fcl {
...
@@ -2365,9 +2365,9 @@ namespace fcl {
}
}
Vec3f
c
[
3
];
Vec3f
c
[
3
];
c
[
0
]
=
T
+
dir_z
*
(
0.5
*
s1
.
lz
);
c
[
0
]
=
T
+
dir_z
*
(
s1
.
halfLength
);
c
[
1
]
=
T
-
dir_z
*
(
0.5
*
s1
.
lz
)
+
C
;
c
[
1
]
=
T
-
dir_z
*
(
s1
.
halfLength
)
+
C
;
c
[
2
]
=
T
-
dir_z
*
(
0.5
*
s1
.
lz
)
-
C
;
c
[
2
]
=
T
-
dir_z
*
(
s1
.
halfLength
)
-
C
;
FCL_REAL
d
[
3
];
FCL_REAL
d
[
3
];
d
[
0
]
=
new_s2
.
signedDistance
(
c
[
0
]);
d
[
0
]
=
new_s2
.
signedDistance
(
c
[
0
]);
...
...
src/narrowphase/gjk.cpp
View file @
3351029d
...
@@ -119,8 +119,8 @@ inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& suppo
...
@@ -119,8 +119,8 @@ inline void getShapeSupport(const Sphere* sphere, const Vec3f& dir, Vec3f& suppo
inline
void
getShapeSupport
(
const
Capsule
*
capsule
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
inline
void
getShapeSupport
(
const
Capsule
*
capsule
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
{
support
=
capsule
->
radius
*
dir
;
support
=
capsule
->
radius
*
dir
;
if
(
dir
[
2
]
>
0
)
support
[
2
]
+=
capsule
->
lz
/
2
;
if
(
dir
[
2
]
>
0
)
support
[
2
]
+=
capsule
->
halfLength
;
else
support
[
2
]
-=
capsule
->
lz
/
2
;
else
support
[
2
]
-=
capsule
->
halfLength
;
}
}
void
getShapeSupport
(
const
Cone
*
cone
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
void
getShapeSupport
(
const
Cone
*
cone
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
...
@@ -137,7 +137,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
...
@@ -137,7 +137,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
FCL_REAL
len
=
zdist
+
dir
[
2
]
*
dir
[
2
];
FCL_REAL
len
=
zdist
+
dir
[
2
]
*
dir
[
2
];
zdist
=
std
::
sqrt
(
zdist
);
zdist
=
std
::
sqrt
(
zdist
);
len
=
std
::
sqrt
(
len
);
len
=
std
::
sqrt
(
len
);
FCL_REAL
half_h
=
cone
->
lz
*
0.5
;
FCL_REAL
half_h
=
cone
->
halfLength
;
FCL_REAL
radius
=
cone
->
radius
;
FCL_REAL
radius
=
cone
->
radius
;
FCL_REAL
sin_a
=
radius
/
std
::
sqrt
(
radius
*
radius
+
4
*
half_h
*
half_h
);
FCL_REAL
sin_a
=
radius
/
std
::
sqrt
(
radius
*
radius
+
4
*
half_h
*
half_h
);
...
@@ -156,7 +156,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
...
@@ -156,7 +156,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
void
getShapeSupport
(
const
Cylinder
*
cylinder
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
void
getShapeSupport
(
const
Cylinder
*
cylinder
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
{
static
const
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
static
const
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
FCL_REAL
half_h
=
cylinder
->
lz
*
0.5
;
FCL_REAL
half_h
=
cylinder
->
halfLength
;
if
(
dir
[
2
]
>
eps
)
support
[
2
]
=
half_h
;
if
(
dir
[
2
]
>
eps
)
support
[
2
]
=
half_h
;
else
if
(
dir
[
2
]
<
-
eps
)
support
[
2
]
=
-
half_h
;
else
if
(
dir
[
2
]
<
-
eps
)
support
[
2
]
=
-
half_h
;
else
support
[
2
]
=
0
;
else
support
[
2
]
=
0
;
...
...
src/shape/geometric_shapes_utility.cpp
View file @
3351029d
...
@@ -96,7 +96,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& t
...
@@ -96,7 +96,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& t
std
::
vector
<
Vec3f
>
result
(
36
);
std
::
vector
<
Vec3f
>
result
(
36
);
const
FCL_REAL
m
=
(
1
+
sqrt
(
5.0
))
/
2.0
;
const
FCL_REAL
m
=
(
1
+
sqrt
(
5.0
))
/
2.0
;
FCL_REAL
hl
=
capsule
.
lz
*
0.5
;
FCL_REAL
hl
=
capsule
.
halfLength
;
FCL_REAL
edge_size
=
capsule
.
radius
*
6
/
(
sqrt
(
27.0
)
+
sqrt
(
15.0
));
FCL_REAL
edge_size
=
capsule
.
radius
*
6
/
(
sqrt
(
27.0
)
+
sqrt
(
15.0
));
FCL_REAL
a
=
edge_size
;
FCL_REAL
a
=
edge_size
;
FCL_REAL
b
=
m
*
edge_size
;
FCL_REAL
b
=
m
*
edge_size
;
...
@@ -153,7 +153,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
...
@@ -153,7 +153,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf)
{
{
std
::
vector
<
Vec3f
>
result
(
7
);
std
::
vector
<
Vec3f
>
result
(
7
);
FCL_REAL
hl
=
cone
.
lz
*
0.5
;
FCL_REAL
hl
=
cone
.
halfLength
;
FCL_REAL
r2
=
cone
.
radius
*
2
/
sqrt
(
3.0
);
FCL_REAL
r2
=
cone
.
radius
*
2
/
sqrt
(
3.0
);
FCL_REAL
a
=
0.5
*
r2
;
FCL_REAL
a
=
0.5
*
r2
;
FCL_REAL
b
=
cone
.
radius
;
FCL_REAL
b
=
cone
.
radius
;
...
@@ -174,7 +174,7 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f&
...
@@ -174,7 +174,7 @@ std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f&
{
{
std
::
vector
<
Vec3f
>
result
(
12
);
std
::
vector
<
Vec3f
>
result
(
12
);
FCL_REAL
hl
=
cylinder
.
lz
*
0.5
;
FCL_REAL
hl
=
cylinder
.
halfLength
;
FCL_REAL
r2
=
cylinder
.
radius
*
2
/
sqrt
(
3.0
);
FCL_REAL
r2
=
cylinder
.
radius
*
2
/
sqrt
(
3.0
);
FCL_REAL
a
=
0.5
*
r2
;
FCL_REAL
a
=
0.5
*
r2
;
FCL_REAL
b
=
cylinder
.
radius
;
FCL_REAL
b
=
cylinder
.
radius
;
...
@@ -277,7 +277,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
...
@@ -277,7 +277,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
const
Matrix3f
&
R
=
tf
.
getRotation
();
const
Matrix3f
&
R
=
tf
.
getRotation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
Vec3f
v_delta
(
0.5
*
(
R
.
col
(
2
)
*
s
.
lz
)
.
cwiseAbs
()
+
Vec3f
::
Constant
(
s
.
radius
));
Vec3f
v_delta
(
R
.
col
(
2
).
cwiseAbs
()
*
s
.
halfLength
+
Vec3f
::
Constant
(
s
.
radius
));
bv
.
max_
=
T
+
v_delta
;
bv
.
max_
=
T
+
v_delta
;
bv
.
min_
=
T
-
v_delta
;
bv
.
min_
=
T
-
v_delta
;
}
}
...
@@ -288,9 +288,9 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
...
@@ -288,9 +288,9 @@ void computeBV<AABB, Cone>(const Cone& s, const Transform3f& tf, AABB& bv)
const
Matrix3f
&
R
=
tf
.
getRotation
();
const
Matrix3f
&
R
=
tf
.
getRotation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
FCL_REAL
x_range
=
fabs
(
R
(
0
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
0
,
1
)
*
s
.
radius
)
+
0.5
*
fabs
(
R
(
0
,
2
)
*
s
.
lz
);
FCL_REAL
x_range
=
fabs
(
R
(
0
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
0
,
1
)
*
s
.
radius
)
+
fabs
(
R
(
0
,
2
)
*
s
.
halfLength
);
FCL_REAL
y_range
=
fabs
(
R
(
1
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
1
,
1
)
*
s
.
radius
)
+
0.5
*
fabs
(
R
(
1
,
2
)
*
s
.
lz
);
FCL_REAL
y_range
=
fabs
(
R
(
1
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
1
,
1
)
*
s
.
radius
)
+
fabs
(
R
(
1
,
2
)
*
s
.
halfLength
);
FCL_REAL
z_range
=
fabs
(
R
(
2
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
2
,
1
)
*
s
.
radius
)
+
0.5
*
fabs
(
R
(
2
,
2
)
*
s
.
lz
);
FCL_REAL
z_range
=
fabs
(
R
(
2
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
2
,
1
)
*
s
.
radius
)
+
fabs
(
R
(
2
,
2
)
*
s
.
halfLength
);
Vec3f
v_delta
(
x_range
,
y_range
,
z_range
);
Vec3f
v_delta
(
x_range
,
y_range
,
z_range
);
bv
.
max_
=
T
+
v_delta
;
bv
.
max_
=
T
+
v_delta
;
...
@@ -303,9 +303,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& b
...
@@ -303,9 +303,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const Transform3f& tf, AABB& b
const
Matrix3f
&
R
=
tf
.
getRotation
();
const
Matrix3f
&
R
=
tf
.
getRotation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
FCL_REAL
x_range
=
fabs
(
R
(
0
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
0
,
1
)
*
s
.
radius
)
+
0.5
*
fabs
(
R
(
0
,
2
)
*
s
.
lz
);
FCL_REAL
x_range
=
fabs
(
R
(
0
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
0
,
1
)
*
s
.
radius
)
+
fabs
(
R
(
0
,
2
)
*
s
.
halfLength
);
FCL_REAL
y_range
=
fabs
(
R
(
1
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
1
,
1
)
*
s
.
radius
)
+
0.5
*
fabs
(
R
(
1
,
2
)
*
s
.
lz
);
FCL_REAL
y_range
=
fabs
(
R
(
1
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
1
,
1
)
*
s
.
radius
)
+
fabs
(
R
(
1
,
2
)
*
s
.
halfLength
);
FCL_REAL
z_range
=
fabs
(
R
(
2
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
2
,
1
)
*
s
.
radius
)
+
0.5
*
fabs
(
R
(
2
,
2
)
*
s
.
lz
);
FCL_REAL
z_range
=
fabs
(
R
(
2
,
0
)
*
s
.
radius
)
+
fabs
(
R
(
2
,
1
)
*
s
.
radius
)
+
fabs
(
R
(
2
,
2
)
*
s
.
halfLength
);