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
03f773bd
Commit
03f773bd
authored
Oct 01, 2019
by
Lucile Remigy
Browse files
Merge branch 'devel' into refactoring
parents
6884d050
2d4578cd
Changes
31
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
03f773bd
...
...
@@ -128,13 +128,10 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/math/types.h
include/hpp/fcl/math/transform.h
include/hpp/fcl/data_types.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h
include/hpp/fcl/BVH/BVH_model.h
include/hpp/fcl/BVH/BV_fitter.h
include/hpp/fcl/BVH/BVH_front.h
include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/intersect.h
include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h
include/hpp/fcl/octree.h
...
...
include/hpp/fcl/BV/AABB.h
View file @
03f773bd
...
...
@@ -39,7 +39,7 @@
#define HPP_FCL_AABB_H
#include
<stdexcept>
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
namespace
hpp
{
...
...
@@ -81,16 +81,6 @@ public:
{
}
//start API in common test
/// @name Bounding volume API
/// Common API to BVs.
/// @{
...
...
@@ -189,19 +179,6 @@ public:
}
/// @}
//End API in common test
/// @brief Check whether the AABB contains another AABB
inline
bool
contain
(
const
AABB
&
other
)
const
...
...
include/hpp/fcl/BV/BV.h
View file @
03f773bd
...
...
@@ -125,7 +125,7 @@ class Converter<RSS, OBB>
public:
static
void
convert
(
const
RSS
&
bv1
,
const
Transform3f
&
tf1
,
OBB
&
bv2
)
{
bv2
.
extent
.
noalias
()
=
Vec3f
(
bv1
.
l
[
0
]
*
0.5
+
bv1
.
r
,
bv1
.
l
[
1
]
*
0.5
+
bv1
.
r
,
bv1
.
r
);
bv2
.
extent
.
noalias
()
=
Vec3f
(
bv1
.
l
ength
[
0
]
*
0.5
+
bv1
.
r
adius
,
bv1
.
l
ength
[
1
]
*
0.5
+
bv1
.
r
adius
,
bv1
.
r
adius
);
bv2
.
To
.
noalias
()
=
tf1
.
transform
(
bv1
.
Tr
);
bv2
.
axes
.
noalias
()
=
tf1
.
getRotation
()
*
bv1
.
axes
;
}
...
...
@@ -168,9 +168,9 @@ public:
bv2
.
Tr
=
tf1
.
transform
(
bv1
.
To
);
bv2
.
axes
.
noalias
()
=
tf1
.
getRotation
()
*
bv1
.
axes
;
bv2
.
r
=
bv1
.
extent
[
2
];
bv2
.
l
[
0
]
=
2
*
(
bv1
.
extent
[
0
]
-
bv2
.
r
);
bv2
.
l
[
1
]
=
2
*
(
bv1
.
extent
[
1
]
-
bv2
.
r
);
bv2
.
r
adius
=
bv1
.
extent
[
2
];
bv2
.
l
ength
[
0
]
=
2
*
(
bv1
.
extent
[
0
]
-
bv2
.
r
adius
);
bv2
.
l
ength
[
1
]
=
2
*
(
bv1
.
extent
[
1
]
-
bv2
.
r
adius
);
}
};
...
...
@@ -183,9 +183,9 @@ public:
bv2
.
Tr
.
noalias
()
=
tf1
.
transform
(
bv1
.
Tr
);
bv2
.
axes
.
noalias
()
=
tf1
.
getRotation
()
*
bv1
.
axes
;
bv2
.
r
=
bv1
.
r
;
bv2
.
l
[
0
]
=
bv1
.
l
[
0
];
bv2
.
l
[
1
]
=
bv1
.
l
[
1
];
bv2
.
r
adius
=
bv1
.
r
adius
;
bv2
.
l
ength
[
0
]
=
bv1
.
l
ength
[
0
];
bv2
.
l
ength
[
1
]
=
bv1
.
l
ength
[
1
];
}
};
...
...
@@ -232,9 +232,9 @@ public:
}
Vec3f
extent
=
(
bv1
.
max_
-
bv1
.
min_
)
*
0.5
;
bv2
.
r
=
extent
[
id
[
2
]];
bv2
.
l
[
0
]
=
(
extent
[
id
[
0
]]
-
bv2
.
r
)
*
2
;
bv2
.
l
[
1
]
=
(
extent
[
id
[
1
]]
-
bv2
.
r
)
*
2
;
bv2
.
r
adius
=
extent
[
id
[
2
]];
bv2
.
l
ength
[
0
]
=
(
extent
[
id
[
0
]]
-
bv2
.
r
adius
)
*
2
;
bv2
.
l
ength
[
1
]
=
(
extent
[
id
[
1
]]
-
bv2
.
r
adius
)
*
2
;
const
Matrix3f
&
R
=
tf1
.
getRotation
();
bool
left_hand
=
(
id
[
0
]
==
(
id
[
1
]
+
1
)
%
3
);
...
...
include/hpp/fcl/BV/BV_node.h
View file @
03f773bd
...
...
@@ -39,7 +39,7 @@
#ifndef HPP_FCL_BV_NODE_H
#define HPP_FCL_BV_NODE_H
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
#include
<hpp/fcl/BV/BV.h>
#include
<iostream>
...
...
include/hpp/fcl/BV/OBB.h
View file @
03f773bd
...
...
@@ -38,7 +38,7 @@
#ifndef HPP_FCL_OBB_H
#define HPP_FCL_OBB_H
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
namespace
hpp
{
...
...
@@ -60,9 +60,6 @@ public:
/// @brief Half dimensions of OBB
Vec3f
extent
;
/// @brief Check whether the OBB contains a point.
bool
contain
(
const
Vec3f
&
p
)
const
;
...
...
include/hpp/fcl/BV/OBBRSS.h
View file @
03f773bd
...
...
@@ -59,10 +59,6 @@ public:
/// @brief RSS member, for distance
RSS
rss
;
/// @brief Check whether the OBBRSS contains a point
inline
bool
contain
(
const
Vec3f
&
p
)
const
{
...
...
@@ -151,11 +147,6 @@ public:
}
};
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
inline
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
OBBRSS
&
b1
,
const
OBBRSS
&
b2
)
{
...
...
include/hpp/fcl/BV/RSS.h
View file @
03f773bd
...
...
@@ -39,7 +39,7 @@
#define HPP_FCL_RSS_H
#include
<stdexcept>
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
#include
<boost/math/constants/constants.hpp>
namespace
hpp
...
...
@@ -60,10 +60,10 @@ public:
Vec3f
Tr
;
/// @brief Side lengths of rectangle
FCL_REAL
l
[
2
];
FCL_REAL
l
ength
[
2
];
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL
r
;
FCL_REAL
r
adius
;
/// @brief Check whether the RSS contains a point
...
...
@@ -101,7 +101,7 @@ public:
/// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
inline
FCL_REAL
size
()
const
{
return
(
std
::
sqrt
(
l
[
0
]
*
l
[
0
]
+
l
[
1
]
*
l
[
1
])
+
2
*
r
);
return
(
std
::
sqrt
(
l
ength
[
0
]
*
l
ength
[
0
]
+
l
ength
[
1
]
*
l
ength
[
1
])
+
2
*
r
adius
);
}
/// @brief The RSS center
...
...
@@ -113,25 +113,25 @@ public:
/// @brief Width of the RSS
inline
FCL_REAL
width
()
const
{
return
l
[
0
]
+
2
*
r
;
return
l
ength
[
0
]
+
2
*
r
adius
;
}
/// @brief Height of the RSS
inline
FCL_REAL
height
()
const
{
return
l
[
1
]
+
2
*
r
;
return
l
ength
[
1
]
+
2
*
r
adius
;
}
/// @brief Depth of the RSS
inline
FCL_REAL
depth
()
const
{
return
2
*
r
;
return
2
*
r
adius
;
}
/// @brief Volume of the RSS
inline
FCL_REAL
volume
()
const
{
return
(
l
[
0
]
*
l
[
1
]
*
2
*
r
+
4
*
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
r
*
r
*
r
);
return
(
l
ength
[
0
]
*
l
ength
[
1
]
*
2
*
r
adius
+
4
*
boost
::
math
::
constants
::
pi
<
FCL_REAL
>
()
*
r
adius
*
radius
*
radius
);
}
/// @brief Check collision between two RSS and return the overlap part.
...
...
@@ -161,4 +161,4 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
}
// namespace hpp
#endif
#endif
\ No newline at end of file
include/hpp/fcl/BV/kDOP.h
View file @
03f773bd
...
...
@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include
<stdexcept>
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
namespace
hpp
{
...
...
include/hpp/fcl/BVH/BVH_model.h
View file @
03f773bd
...
...
@@ -41,8 +41,8 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/BVH/BVH_internal.h>
#include
<hpp/fcl/BV/BV_node.h>
#include
<hpp/fcl
/BVH/BV_splitter.h
>
#include
<hpp/fcl
/BVH/BV_fitter.h
>
#include
"../../src
/BVH/BV_splitter.h
"
#include
"../../src
/BVH/BV_fitter.h
"
#include
<vector>
#include
<boost/shared_ptr.hpp>
#include
<boost/noncopyable.hpp>
...
...
include/hpp/fcl/collision.h
View file @
03f773bd
...
...
@@ -39,7 +39,7 @@
#ifndef HPP_FCL_COLLISION_H
#define HPP_FCL_COLLISION_H
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/collision_data.h>
...
...
include/hpp/fcl/collision_data.h
View file @
03f773bd
...
...
@@ -41,7 +41,7 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
#include
<vector>
#include
<set>
#include
<limits>
...
...
include/hpp/fcl/data_types.h
View file @
03f773bd
...
...
@@ -41,16 +41,37 @@
#include
<cstddef>
#include
<boost/cstdint.hpp>
#include
<Eigen/Core>
#include
<Eigen/Geometry>
namespace
hpp
{
#ifdef HPP_FCL_HAVE_OCTOMAP
#define OCTOMAP_VERSION_AT_LEAST(x,y,z) \
(OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \
(OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \
OCTOMAP_PATCH_VERSION >= z))))
#define OCTOMAP_VERSION_AT_MOST(x,y,z) \
(OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \
(OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \
OCTOMAP_PATCH_VERSION <= z))))
#endif // HPP_FCL_HAVE_OCTOMAP
}
namespace
hpp
{
namespace
fcl
{
typedef
double
FCL_REAL
;
typedef
boost
::
uint64_t
FCL_INT64
;
typedef
boost
::
int64_t
FCL_UINT64
;
typedef
boost
::
uint32_t
FCL_UINT32
;
typedef
boost
::
int32_t
FCL_INT32
;
typedef
Eigen
::
Matrix
<
FCL_REAL
,
3
,
1
>
Vec3f
;
typedef
Eigen
::
Matrix
<
FCL_REAL
,
3
,
3
>
Matrix3f
;
/// @brief Triangle with 3 indices for points
class
Triangle
...
...
@@ -84,4 +105,5 @@ public:
}
// namespace hpp
#endif
include/hpp/fcl/math/transform.h
View file @
03f773bd
...
...
@@ -39,7 +39,7 @@
#ifndef HPP_FCL_TRANSFORM_H
#define HPP_FCL_TRANSFORM_H
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
#include
<boost/thread/mutex.hpp>
namespace
hpp
...
...
include/hpp/fcl/math/types.h
View file @
03f773bd
...
...
@@ -38,50 +38,9 @@
#ifndef HPP_FCL_MATH_TYPES_H
#define HPP_FCL_MATH_TYPES_H
#
i
nclude
<hpp/fcl/data_types.h>
#
warning "This file is deprecated. I
nclude <hpp/fcl/data_types.h>
instead."
#include
<Eigen/Core>
#include
<Eigen/Geometry>
namespace
hpp
{
#ifdef HPP_FCL_HAVE_OCTOMAP
#define OCTOMAP_VERSION_AT_LEAST(x,y,z) \
(OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \
(OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \
OCTOMAP_PATCH_VERSION >= z))))
#define OCTOMAP_VERSION_AT_MOST(x,y,z) \
(OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \
(OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \
OCTOMAP_PATCH_VERSION <= z))))
#endif // HPP_FCL_HAVE_OCTOMAP
}
namespace
hpp
{
namespace
fcl
{
typedef
Eigen
::
Matrix
<
FCL_REAL
,
3
,
1
>
Vec3f
;
typedef
Eigen
::
Matrix
<
FCL_REAL
,
3
,
3
>
Matrix3f
;
/// @brief Class for variance matrix in 3d
class
Variance3f
{
public:
/// @brief Variation matrix
Matrix3f
Sigma
;
/// @brief Variations along the eign axes
Matrix3f
::
Scalar
sigma
[
3
];
/// @brief Eigen axes of the variation matrix
Vec3f
axis
[
3
];
};
}
}
// namespace hpp
// List of equivalent includes.
# include <hpp/fcl/data_types.h>
#endif
\ No newline at end of file
include/hpp/fcl/mesh_loader/loader.h
View file @
03f773bd
...
...
@@ -40,7 +40,7 @@
#include
<boost/shared_ptr.hpp>
#include
<hpp/fcl/fwd.hh>
#include
<hpp/fcl/config.hh>
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
#include
<hpp/fcl/collision_object.h>
namespace
hpp
...
...
include/hpp/fcl/shape/geometric_shapes.h
View file @
03f773bd
...
...
@@ -42,7 +42,7 @@
#include
<boost/math/constants/constants.hpp>
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/
m
at
h/
types.h>
#include
<hpp/fcl/
d
at
a_
types.h>
#include
<string.h>
namespace
hpp
...
...
@@ -151,12 +151,17 @@ public:
{
}
// Capsule::Capsule() : HalfLength(lz/2), lz(0){}
/// @brief Radius of capsule
FCL_REAL
radius
;
/// @brief Length along z axis
FCL_REAL
lz
;
/// @brief Half Length along z axis
FCL_REAL
HalfLength
;
/// @brief Compute AABB
void
computeLocalAABB
();
...
...
@@ -191,12 +196,17 @@ public:
{
}
//Cone::Cone() : HalfLength(lz/2), lz(0){}
/// @brief Radius of the cone
FCL_REAL
radius
;
/// @brief Length along z axis
FCL_REAL
lz
;
/// @brief Half Length along z axis
FCL_REAL
HalfLength
;
/// @brief Compute AABB
void
computeLocalAABB
();
...
...
@@ -233,6 +243,7 @@ public:
{
}
// Cylinder::Cylinder() : HalfLength(lz/2), lz(0){}
/// @brief Radius of the cylinder
FCL_REAL
radius
;
...
...
@@ -240,6 +251,9 @@ public:
/// @brief Length along z axis
FCL_REAL
lz
;
/// @brief Half Length along z axis
FCL_REAL
HalfLength
;
/// @brief Compute AABB
void
computeLocalAABB
();
...
...
src/BV/RSS.cpp
View file @
03f773bd
...
...
@@ -844,8 +844,8 @@ bool RSS::overlap(const RSS& other) const
/// Now compute R1'R2
Matrix3f
R
(
axes
.
transpose
()
*
other
.
axes
);
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
l
,
other
.
l
);
return
(
dist
<=
(
r
+
other
.
r
));
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
l
ength
,
other
.
l
ength
);
return
(
dist
<=
(
r
adius
+
other
.
r
adius
));
}
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
RSS
&
b1
,
const
RSS
&
b2
)
...
...
@@ -859,8 +859,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
Vec3f
T
(
b1
.
axes
.
transpose
()
*
Ttemp
);
Matrix3f
R
(
b1
.
axes
.
transpose
()
*
R0
*
b2
.
axes
);
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
b1
.
l
,
b2
.
l
);
return
(
dist
<=
(
b1
.
r
+
b2
.
r
));
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
b1
.
l
ength
,
b2
.
l
ength
);
return
(
dist
<=
(
b1
.
r
adius
+
b2
.
r
adius
));
}
bool
overlap
(
const
Matrix3f
&
R0
,
const
Vec3f
&
T0
,
const
RSS
&
b1
,
const
RSS
&
b2
,
...
...
@@ -876,7 +876,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
Vec3f
T
(
b1
.
axes
.
transpose
()
*
Ttemp
);
Matrix3f
R
(
b1
.
axes
.
transpose
()
*
R0
*
b2
.
axes
);
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
b1
.
l
,
b2
.
l
)
-
b1
.
r
-
b2
.
r
;
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
b1
.
l
ength
,
b2
.
l
ength
)
-
b1
.
r
adius
-
b2
.
r
adius
;
if
(
dist
<=
0
)
return
true
;
sqrDistLowerBound
=
dist
*
dist
;
return
false
;
...
...
@@ -893,28 +893,28 @@ bool RSS::contain(const Vec3f& p) const
Vec3f
proj
(
proj0
,
proj1
,
proj2
);
/// projection is within the rectangle
if
((
proj0
<
l
[
0
])
&&
(
proj0
>
0
)
&&
(
proj1
<
l
[
1
])
&&
(
proj1
>
0
))
if
((
proj0
<
l
ength
[
0
])
&&
(
proj0
>
0
)
&&
(
proj1
<
l
ength
[
1
])
&&
(
proj1
>
0
))
{
return
(
abs_proj2
<
r
);
return
(
abs_proj2
<
r
adius
);
}
else
if
((
proj0
<
l
[
0
])
&&
(
proj0
>
0
)
&&
((
proj1
<
0
)
||
(
proj1
>
l
[
1
])))
else
if
((
proj0
<
l
ength
[
0
])
&&
(
proj0
>
0
)
&&
((
proj1
<
0
)
||
(
proj1
>
l
ength
[
1
])))
{
FCL_REAL
y
=
(
proj1
>
0
)
?
l
[
1
]
:
0
;
FCL_REAL
y
=
(
proj1
>
0
)
?
l
ength
[
1
]
:
0
;
Vec3f
v
(
proj0
,
y
,
0
);
return
((
proj
-
v
).
squaredNorm
()
<
r
*
r
);
return
((
proj
-
v
).
squaredNorm
()
<
r
adius
*
radius
);
}
else
if
((
proj1
<
l
[
1
])
&&
(
proj1
>
0
)
&&
((
proj0
<
0
)
||
(
proj0
>
l
[
0
])))
else
if
((
proj1
<
l
ength
[
1
])
&&
(
proj1
>
0
)
&&
((
proj0
<
0
)
||
(
proj0
>
l
ength
[
0
])))
{
FCL_REAL
x
=
(
proj0
>
0
)
?
l
[
0
]
:
0
;
FCL_REAL
x
=
(
proj0
>
0
)
?
l
ength
[
0
]
:
0
;
Vec3f
v
(
x
,
proj1
,
0
);
return
((
proj
-
v
).
squaredNorm
()
<
r
*
r
);
return
((
proj
-
v
).
squaredNorm
()
<
r
adius
*
radius
);
}
else
{
FCL_REAL
x
=
(
proj0
>
0
)
?
l
[
0
]
:
0
;
FCL_REAL
y
=
(
proj1
>
0
)
?
l
[
1
]
:
0
;
FCL_REAL
x
=
(
proj0
>
0
)
?
l
ength
[
0
]
:
0
;
FCL_REAL
y
=
(
proj1
>
0
)
?
l
ength
[
1
]
:
0
;
Vec3f
v
(
x
,
y
,
0
);
return
((
proj
-
v
).
squaredNorm
()
<
r
*
r
);
return
((
proj
-
v
).
squaredNorm
()
<
r
adius
*
radius
);
}
}
...
...
@@ -928,99 +928,99 @@ RSS& RSS::operator += (const Vec3f& p)
Vec3f
proj
(
proj0
,
proj1
,
proj2
);
// projection is within the rectangle
if
((
proj0
<
l
[
0
])
&&
(
proj0
>
0
)
&&
(
proj1
<
l
[
1
])
&&
(
proj1
>
0
))
if
((
proj0
<
l
ength
[
0
])
&&
(
proj0
>
0
)
&&
(
proj1
<
l
ength
[
1
])
&&
(
proj1
>
0
))
{
if
(
abs_proj2
<
r
)
if
(
abs_proj2
<
r
adius
)
;
// do nothing
else
{
r
=
0.5
*
(
r
+
abs_proj2
);
// enlarge the r
r
adius
=
0.5
*
(
r
adius
+
abs_proj2
);
// enlarge the r
// change RSS origin position
if
(
proj2
>
0
)
Tr
[
2
]
+=
0.5
*
(
abs_proj2
-
r
);
Tr
[
2
]
+=
0.5
*
(
abs_proj2
-
r
adius
);
else
Tr
[
2
]
-=
0.5
*
(
abs_proj2
-
r
);
Tr
[
2
]
-=
0.5
*
(
abs_proj2
-
r
adius
);
}
}
else
if
((
proj0
<
l
[
0
])
&&
(
proj0
>
0
)
&&
((
proj1
<
0
)
||
(
proj1
>
l
[
1
])))
else
if
((
proj0
<
l
ength
[
0
])
&&
(
proj0
>
0
)
&&
((
proj1
<
0
)
||
(
proj1
>
l
ength
[
1
])))
{
FCL_REAL
y
=
(
proj1
>
0
)
?
l
[
1
]
:
0
;
FCL_REAL
y
=
(
proj1
>
0
)
?
l
ength
[
1
]
:
0
;
Vec3f
v
(
proj0
,
y
,
0
);
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
squaredNorm
();
if
(
new_r_sqr
<
r
*
r
)
if
(
new_r_sqr
<
r
adius
*
radius
)
;
// do nothing
else
{
if
(
abs_proj2
<
r
)
if
(
abs_proj2
<
r
adius
)
{
FCL_REAL
delta_y
=
-
std
::
sqrt
(
r
*
r
-
proj2
*
proj2
)
+
fabs
(
proj1
-
y
);
l
[
1
]
+=
delta_y
;
FCL_REAL
delta_y
=
-
std
::
sqrt
(
r
adius
*
radius
-
proj2
*
proj2
)
+
fabs
(
proj1
-
y
);
l
ength
[
1
]
+=
delta_y
;
if
(
proj1
<
0
)
Tr
[
1
]
-=
delta_y
;
}
else
{
FCL_REAL
delta_y
=
fabs
(
proj1
-
y
);
l
[
1
]
+=
delta_y
;
l
ength
[
1
]
+=
delta_y
;
if
(
proj1
<
0
)
Tr
[
1
]
-=
delta_y
;
if
(
proj2
>
0
)
Tr
[
2
]
+=
0.5
*
(
abs_proj2
-
r
);
Tr
[
2
]
+=
0.5
*
(
abs_proj2
-
r
adius
);
else
Tr
[
2
]
-=
0.5
*
(
abs_proj2
-
r
);
Tr
[
2
]
-=
0.5
*
(
abs_proj2
-
r
adius
);
}
}
}
else
if
((
proj1
<
l
[
1
])
&&
(
proj1
>
0
)
&&
((
proj0
<
0
)
||
(
proj0
>
l
[
0
])))
else
if
((
proj1
<
l
ength
[
1
])
&&
(
proj1
>
0
)
&&
((
proj0
<
0
)
||
(
proj0
>
l
ength
[
0
])))
{
FCL_REAL
x
=
(
proj0
>
0
)
?
l
[
0
]
:
0
;
FCL_REAL
x
=
(
proj0
>
0
)
?
l
ength
[
0
]
:
0
;
Vec3f
v
(
x
,
proj1
,
0
);
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
squaredNorm
();
if
(
new_r_sqr
<
r
*
r
)
if
(
new_r_sqr
<
r
adius
*
radius
)
;
// do nothing
else
{
if
(
abs_proj2
<
r
)
if
(
abs_proj2
<
r
adius
)
{
FCL_REAL
delta_x
=
-
std
::
sqrt
(
r
*
r
-
proj2
*
proj2
)
+
fabs
(
proj0
-
x
);
l
[
0
]
+=
delta_x
;
FCL_REAL
delta_x
=
-
std
::
sqrt
(
r
adius
*
radius
-
proj2
*
proj2
)
+
fabs
(
proj0
-
x
);
l
ength
[
0
]
+=
delta_x
;
if
(
proj0
<
0
)
Tr
[
0
]
-=
delta_x
;
}
else
{
FCL_REAL
delta_x
=
fabs
(
proj0
-
x
);
l
[
0
]
+=
delta_x
;
l
ength
[
0
]
+=
delta_x
;
if
(
proj0
<
0
)
Tr
[
0
]
-=
delta_x
;
if
(
proj2
>
0
)
Tr
[
2
]
+=
0.5
*
(
abs_proj2
-
r
);
Tr
[
2
]
+=
0.5
*
(
abs_proj2
-
r
adius
);