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
a2268fe0
Commit
a2268fe0
authored
Aug 10, 2012
by
jpan
Browse files
collision for plane and halfspace, not finished yet.
git-svn-id:
https://kforge.ros.org/fcl/fcl_ros@161
253336fb-580f-4252-a368-f3cef5a2a82b
parent
d7d66c14
Changes
10
Hide whitespace changes
Inline
Side-by-side
trunk/fcl/include/fcl/BV/kDOP.h
View file @
a2268fe0
...
...
@@ -46,6 +46,15 @@ namespace fcl
/// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24
/// The KDOP structure is defined by some pairs of parallel planes defined by some axes.
/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 8
/// (0,-1,0) and (0,1,0) -> indices 1 and 9
/// (0,0,-1) and (0,0,1) -> indices 2 and 10
/// (-1,-1,0) and (1,1,0) -> indices 3 and 11
/// (-1,0,-1) and (1,0,1) -> indices 4 and 12
/// (0,-1,-1) and (0,1,1) -> indices 5 and 13
/// (-1,1,0) and (1,-1,0) -> indices 6 and 14
/// (-1,0,1) and (1,0,-1) -> indices 7 and 15
/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 9
/// (0,-1,0) and (0,1,0) -> indices 1 and 10
...
...
@@ -56,6 +65,19 @@ namespace fcl
/// (-1,1,0) and (1,-1,0) -> indices 6 and 15
/// (-1,0,1) and (1,0,-1) -> indices 7 and 16
/// (0,-1,1) and (0,1,-1) -> indices 8 and 17
/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 12
/// (0,-1,0) and (0,1,0) -> indices 1 and 13
/// (0,0,-1) and (0,0,1) -> indices 2 and 14
/// (-1,-1,0) and (1,1,0) -> indices 3 and 15
/// (-1,0,-1) and (1,0,1) -> indices 4 and 16
/// (0,-1,-1) and (0,1,1) -> indices 5 and 17
/// (-1,1,0) and (1,-1,0) -> indices 6 and 18
/// (-1,0,1) and (1,0,-1) -> indices 7 and 19
/// (0,-1,1) and (0,1,-1) -> indices 8 and 20
/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
template
<
size_t
N
>
class
KDOP
{
...
...
trunk/fcl/include/fcl/BV/kIOS.h
View file @
a2268fe0
...
...
@@ -139,9 +139,6 @@ public:
/// @brief The distance between two kIOS
FCL_REAL
distance
(
const
kIOS
&
other
,
Vec3f
*
P
=
NULL
,
Vec3f
*
Q
=
NULL
)
const
;
private:
};
...
...
trunk/fcl/include/fcl/collision_object.h
View file @
a2268fe0
...
...
@@ -50,7 +50,7 @@ enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
enum
NODE_TYPE
{
BV_UNKNOWN
,
BV_AABB
,
BV_OBB
,
BV_RSS
,
BV_kIOS
,
BV_OBBRSS
,
BV_KDOP16
,
BV_KDOP18
,
BV_KDOP24
,
GEOM_BOX
,
GEOM_SPHERE
,
GEOM_CAPSULE
,
GEOM_CONE
,
GEOM_CYLINDER
,
GEOM_CONVEX
,
GEOM_PLANE
,
GEOM_TRIANGLE
,
GEOM_OCTREE
,
NODE_COUNT
};
GEOM_BOX
,
GEOM_SPHERE
,
GEOM_CAPSULE
,
GEOM_CONE
,
GEOM_CYLINDER
,
GEOM_CONVEX
,
GEOM_PLANE
,
GEOM_HALFSPACE
,
GEOM_TRIANGLE
,
GEOM_OCTREE
,
NODE_COUNT
};
/// @brief The geometry for the object for collision or distance computation
class
CollisionGeometry
...
...
trunk/fcl/include/fcl/math/transform.h
View file @
a2268fe0
...
...
@@ -130,6 +130,10 @@ public:
inline
FCL_REAL
&
getY
()
{
return
data
[
2
];
}
inline
FCL_REAL
&
getZ
()
{
return
data
[
3
];
}
Vec3f
getColumn
(
std
::
size_t
i
)
const
;
Vec3f
getRow
(
std
::
size_t
i
)
const
;
private:
FCL_REAL
data
[
4
];
...
...
trunk/fcl/include/fcl/shape/geometric_shapes.h
View file @
a2268fe0
...
...
@@ -258,6 +258,43 @@ protected:
void
fillEdges
();
};
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space
class
Halfspace
:
public
ShapeBase
{
public:
/// @brief Construct a half space with normal direction and offset
Halfspace
(
const
Vec3f
&
n_
,
FCL_REAL
d_
)
:
ShapeBase
(),
n
(
n_
),
d
(
d_
)
{
unitNormalTest
();
}
/// @brief Construct a plane with normal direction and offset
Halfspace
(
FCL_REAL
a
,
FCL_REAL
b
,
FCL_REAL
c
,
FCL_REAL
d_
)
:
n
(
a
,
b
,
c
),
d
(
d_
)
{
unitNormalTest
();
}
/// @brief Compute AABB
void
computeLocalAABB
();
/// @brief Get node type: a half space
NODE_TYPE
getNodeType
()
const
{
return
GEOM_HALFSPACE
;
}
/// @brief Plane normal
Vec3f
n
;
/// @brief Plane offset
FCL_REAL
d
;
protected:
/// @brief Turn non-unit normal into unit
void
unitNormalTest
();
};
/// @brief Infinite plane
class
Plane
:
public
ShapeBase
{
...
...
trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
View file @
a2268fe0
...
...
@@ -89,8 +89,9 @@ void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv);
template
<
>
void
computeBV
<
AABB
,
Triangle2
>
(
const
Triangle2
&
s
,
const
Transform3f
&
tf
,
AABB
&
bv
);
template
<
>
void
computeBV
<
AABB
,
Halfspace
>
(
const
Halfspace
&
s
,
const
Transform3f
&
tf
,
AABB
&
bv
);
/// @brief the bounding volume for half space back of plane for OBB, it is the plane itself
template
<
>
void
computeBV
<
AABB
,
Plane
>
(
const
Plane
&
s
,
const
Transform3f
&
tf
,
AABB
&
bv
);
...
...
@@ -114,9 +115,13 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
template
<
>
void
computeBV
<
OBB
,
Convex
>
(
const
Convex
&
s
,
const
Transform3f
&
tf
,
OBB
&
bv
);
template
<
>
void
computeBV
<
OBB
,
Halfspace
>
(
const
Halfspace
&
s
,
const
Transform3f
&
tf
,
OBB
&
bv
);
template
<
>
void
computeBV
<
OBB
,
Plane
>
(
const
Plane
&
s
,
const
Transform3f
&
tf
,
OBB
&
bv
);
template
<
>
void
computeBV
<
RSS
,
Plane
>
(
const
Plane
&
s
,
const
Transform3f
&
tf
,
RSS
&
bv
);
...
...
@@ -169,6 +174,9 @@ void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transf
void
constructBox
(
const
KDOP
<
24
>&
bv
,
const
Transform3f
&
tf_bv
,
Box
&
box
,
Transform3f
&
tf
);
Halfspace
transform
(
const
Halfspace
&
a
,
const
Transform3f
&
tf
);
Plane
transform
(
const
Plane
&
a
,
const
Transform3f
&
tf
);
}
...
...
trunk/fcl/src/math/transform.cpp
View file @
a2268fe0
...
...
@@ -327,6 +327,49 @@ Quaternion3f inverse(const Quaternion3f& q)
return
res
.
inverse
();
}
Vec3f
Quaternion3f
::
getColumn
(
std
::
size_t
i
)
const
{
switch
(
i
)
{
case
0
:
return
Vec3f
(
data
[
0
]
*
data
[
0
]
+
data
[
1
]
*
data
[
1
]
-
data
[
2
]
*
data
[
2
]
-
data
[
3
]
*
data
[
3
],
2
*
(
-
data
[
0
]
*
data
[
3
]
+
data
[
1
]
*
data
[
2
]),
2
*
(
data
[
1
]
*
data
[
3
]
+
data
[
0
]
*
data
[
2
]));
case
1
:
return
Vec3f
(
2
*
(
data
[
1
]
*
data
[
2
]
+
data
[
0
]
*
data
[
3
]),
data
[
0
]
*
data
[
0
]
-
data
[
1
]
*
data
[
1
]
+
data
[
2
]
*
data
[
2
]
-
data
[
3
]
*
data
[
3
],
2
*
(
data
[
2
]
*
data
[
3
]
-
data
[
0
]
*
data
[
1
]));
case
2
:
return
Vec3f
(
2
*
(
data
[
1
]
*
data
[
3
]
-
data
[
0
]
*
data
[
2
]),
2
*
(
data
[
2
]
*
data
[
3
]
+
data
[
0
]
*
data
[
1
]),
data
[
0
]
*
data
[
0
]
-
data
[
1
]
*
data
[
1
]
-
data
[
2
]
*
data
[
2
]
+
data
[
3
]
*
data
[
3
]);
default:
return
Vec3f
();
}
}
Vec3f
Quaternion3f
::
getRow
(
std
::
size_t
i
)
const
{
switch
(
i
)
{
case
0
:
return
Vec3f
(
data
[
0
]
*
data
[
0
]
+
data
[
1
]
*
data
[
1
]
-
data
[
2
]
*
data
[
2
]
-
data
[
3
]
*
data
[
3
],
2
*
(
data
[
0
]
*
data
[
3
]
+
data
[
1
]
*
data
[
2
]),
2
*
(
data
[
1
]
*
data
[
3
]
-
data
[
0
]
*
data
[
2
]));
case
1
:
return
Vec3f
(
2
*
(
data
[
1
]
*
data
[
2
]
-
data
[
0
]
*
data
[
3
]),
data
[
0
]
*
data
[
0
]
-
data
[
1
]
*
data
[
1
]
+
data
[
2
]
*
data
[
2
]
-
data
[
3
]
*
data
[
3
],
2
*
(
data
[
2
]
*
data
[
3
]
+
data
[
0
]
*
data
[
1
]));
case
2
:
return
Vec3f
(
2
*
(
data
[
1
]
*
data
[
3
]
+
data
[
0
]
*
data
[
2
]),
2
*
(
data
[
2
]
*
data
[
3
]
-
data
[
0
]
*
data
[
1
]),
data
[
0
]
*
data
[
0
]
-
data
[
1
]
*
data
[
1
]
-
data
[
2
]
*
data
[
2
]
+
data
[
3
]
*
data
[
3
]);
default:
return
Vec3f
();
}
}
const
Matrix3f
&
Transform3f
::
getRotationInternal
()
const
{
boost
::
mutex
::
scoped_lock
slock
(
const_cast
<
boost
::
mutex
&>
(
lock_
));
...
...
trunk/fcl/src/narrowphase/narrowphase.cpp
View file @
a2268fe0
...
...
@@ -35,6 +35,7 @@
/** \author Jia Pan */
#include
"fcl/narrowphase/narrowphase.h"
#include
"fcl/shape/geometric_shapes_utility.h"
#include
<boost/math/constants/constants.hpp>
#include
<vector>
...
...
@@ -1242,6 +1243,543 @@ bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
bool
sphereHalfspaceIntersect
(
const
Sphere
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
{
Halfspace
new_s2
=
transform
(
s2
,
tf2
);
FCL_REAL
k
=
tf1
.
getTranslation
().
dot
(
new_s2
.
n
);
FCL_REAL
depth
=
new_s2
.
d
-
k
+
s1
.
radius
;
if
(
depth
>=
0
)
{
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
// pointing from s1 to s2
if
(
penetration_depth
)
*
penetration_depth
=
depth
;
if
(
contact_points
)
*
contact_points
=
tf1
.
getTranslation
()
-
new_s2
.
n
*
s1
.
radius
+
new_s2
.
n
*
(
depth
*
0.5
);
return
true
;
}
else
return
false
;
}
bool
boxHalfspaceIntersect
(
const
Box
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
{
Halfspace
new_s2
=
transform
(
s2
,
tf2
);
/// project sides lengths along normal vector, get absolue values
const
Matrix3f
&
R
=
tf1
.
getRotation
();
const
Vec3f
&
T
=
tf1
.
getTranslation
();
const
Quaternion3f
&
q
=
tf1
.
getQuatRotation
();
Vec3f
Q
=
conj
(
q
).
transform
(
new_s2
.
n
);
Vec3f
A
(
Q
[
0
]
*
s1
.
side
[
0
],
Q
[
1
]
*
s1
.
side
[
1
],
Q
[
2
]
*
s1
.
side
[
2
]);
Vec3f
B
=
abs
(
A
);
FCL_REAL
depth
=
new_s2
.
d
+
0.5
*
(
B
[
0
]
+
B
[
1
]
+
B
[
2
])
-
new_s2
.
n
.
dot
(
tf1
.
getTranslation
());
if
(
depth
<
0
)
return
false
;
/// find deepest point
Vec3f
p
=
tf1
.
getTranslation
();
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
{
if
(
A
[
i
]
>
0
)
p
-=
R
.
getColumn
(
i
)
*
(
0.5
*
s1
.
side
[
i
]);
else
p
+=
R
.
getColumn
(
i
)
*
(
0.5
*
s1
.
side
[
i
]);
}
/// compute the contact point from the deepest point
if
(
penetration_depth
)
*
penetration_depth
=
depth
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
p
+
new_s2
.
n
*
(
depth
*
0.5
);
return
true
;
}
bool
capsuleHalfspaceIntersect
(
const
Capsule
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
{
Halfspace
new_s2
=
transform
(
s2
,
tf2
);
const
Matrix3f
&
R
=
tf1
.
getRotation
();
const
Vec3f
&
T
=
tf1
.
getTranslation
();
Vec3f
dir_z
=
R
.
getColumn
(
2
);
Vec3f
Q
=
R
.
transposeTimes
(
new_s2
.
n
);
FCL_REAL
sign
=
(
Q
[
2
]
>
0
)
?
-
1
:
1
;
Vec3f
p
=
tf1
.
getTranslation
()
+
dir_z
*
(
s1
.
lz
*
0.5
*
sign
);
FCL_REAL
k
=
p
.
dot
(
new_s2
.
n
);
FCL_REAL
depth
=
new_s2
.
d
-
k
+
s1
.
radius
;
if
(
depth
<
0
)
return
false
;
if
(
penetration_depth
)
*
penetration_depth
=
depth
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
{
Vec3f
c
=
p
-
new_s2
.
n
*
s1
.
radius
;
// deepest
c
+=
dir_z
*
(
0.5
*
depth
/
Q
[
2
]);
*
contact_points
=
c
;
}
return
true
;
}
template
<
typename
T
>
T
cylinderHalfspaceIntersectTolerance
()
{
}
template
<
>
double
cylinderHalfspaceIntersectTolerance
()
{
return
0.0000001
;
}
template
<
>
float
cylinderHalfspaceIntersectTolerance
()
{
return
0.0001
;
}
bool
cylinderHalfspaceIntersect
(
const
Cylinder
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
{
Halfspace
new_s2
=
transform
(
s2
,
tf2
);
const
Matrix3f
&
R
=
tf1
.
getRotation
();
const
Vec3f
&
T
=
tf1
.
getTranslation
();
Vec3f
dir_z
=
R
.
getColumn
(
2
);
Vec3f
p1
=
tf1
.
getTranslation
()
+
dir_z
*
(
0.5
*
s1
.
lz
);
Vec3f
p2
=
tf1
.
getTranslation
()
-
dir_z
*
(
0.5
*
s1
.
lz
);
FCL_REAL
s
=
dir_z
.
dot
(
new_s2
.
n
);
if
(
s
<
0
)
s
+=
1
;
else
s
-=
1
;
if
(
std
::
abs
(
s
)
<
cylinderHalfspaceIntersectTolerance
<
FCL_REAL
>
())
{
FCL_REAL
d1
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
p1
);
FCL_REAL
d2
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
p2
);
if
(
d1
>=
d2
)
{
if
(
d1
>=
0
)
{
if
(
contact_points
)
*
contact_points
=
p1
+
new_s2
.
n
*
(
0.5
*
d1
);
if
(
penetration_depth
)
*
penetration_depth
=
d1
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
return
true
;
}
else
return
false
;
}
else
{
if
(
d2
>=
0
)
{
if
(
contact_points
)
*
contact_points
=
p2
+
new_s2
.
n
*
(
0.5
*
d2
);
if
(
penetration_depth
)
*
penetration_depth
=
d2
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
return
true
;
}
else
return
false
;
}
}
else
{
FCL_REAL
t
=
(
new_s2
.
n
).
dot
(
dir_z
);
Vec3f
C
=
dir_z
*
t
-
new_s2
.
n
;
FCL_REAL
s
=
C
.
length
();
s
=
s1
.
radius
/
s
;
C
*=
s
;
// deepest point of disc1
Vec3f
c1
=
C
+
p1
;
FCL_REAL
depth1
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
c1
);
Vec3f
c2
=
C
+
p2
;
FCL_REAL
depth2
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
c2
);
if
(
depth1
>=
0
&&
depth2
>=
0
)
{
if
(
depth1
>
depth2
)
{
if
(
penetration_depth
)
*
penetration_depth
=
depth1
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c1
+
dir_z
*
(
0.5
*
depth1
/
s
);
}
else
{
if
(
penetration_depth
)
*
penetration_depth
=
depth2
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c2
+
dir_z
*
(
0.5
*
depth2
/
s
);
}
return
true
;
}
else
if
(
depth1
>=
0
)
{
if
(
penetration_depth
)
*
penetration_depth
=
depth1
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c1
+
dir_z
*
(
0.5
*
depth1
/
s
);
return
true
;
}
else
if
(
depth2
>=
0
)
{
if
(
penetration_depth
)
*
penetration_depth
=
depth2
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c2
+
dir_z
*
(
0.5
*
depth2
/
s
);
return
true
;
}
else
return
false
;
}
}
bool
coneHalfspaceIntersect
(
const
Cone
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
{
Halfspace
new_s2
=
transform
(
s2
,
tf2
);
const
Matrix3f
&
R
=
tf1
.
getRotation
();
const
Vec3f
&
T
=
tf1
.
getTranslation
();
Vec3f
dir_z
=
R
.
getColumn
(
2
);
Vec3f
p1
=
tf1
.
getTranslation
()
+
dir_z
*
(
0.5
*
s1
.
lz
);
Vec3f
p2
=
tf1
.
getTranslation
()
-
dir_z
*
(
0.5
*
s1
.
lz
);
FCL_REAL
s
=
dir_z
.
dot
(
new_s2
.
n
);
if
(
s
<
0
)
s
+=
1
;
else
s
-=
1
;
if
(
std
::
abs
(
s
)
<
cylinderHalfspaceIntersectTolerance
<
FCL_REAL
>
())
{
FCL_REAL
d1
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
p1
);
FCL_REAL
d2
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
p2
);
if
(
d1
>=
d2
)
{
if
(
d1
>=
0
)
{
if
(
contact_points
)
*
contact_points
=
p1
+
new_s2
.
n
*
(
0.5
*
d1
);
if
(
penetration_depth
)
*
penetration_depth
=
d1
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
return
true
;
}
else
return
false
;
}
else
{
if
(
d2
>=
0
)
{
if
(
contact_points
)
*
contact_points
=
p2
+
new_s2
.
n
*
(
0.5
*
d2
);
if
(
penetration_depth
)
*
penetration_depth
=
d2
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
return
true
;
}
else
return
false
;
}
}
else
{
FCL_REAL
t
=
(
new_s2
.
n
).
dot
(
dir_z
);
Vec3f
C
=
dir_z
*
t
-
new_s2
.
n
;
FCL_REAL
s
=
C
.
length
();
s
=
s1
.
radius
/
s
;
C
*=
s
;
// deepest point of disc1
Vec3f
c1
=
p1
;
FCL_REAL
depth1
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
c1
);
Vec3f
c2
=
C
+
p2
;
FCL_REAL
depth2
=
new_s2
.
d
-
(
new_s2
.
n
).
dot
(
c2
);
if
(
depth1
>=
0
&&
depth2
>=
0
)
{
if
(
depth1
>
depth2
)
{
if
(
penetration_depth
)
*
penetration_depth
=
depth1
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c1
+
dir_z
*
(
0.5
*
depth1
/
s
);
}
else
{
if
(
penetration_depth
)
*
penetration_depth
=
depth2
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c2
+
dir_z
*
(
0.5
*
depth2
/
s
);
}
return
true
;
}
else
if
(
depth1
>=
0
)
{
if
(
penetration_depth
)
*
penetration_depth
=
depth1
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c1
+
dir_z
*
(
0.5
*
depth1
/
s
);
return
true
;
}
else
if
(
depth2
>=
0
)
{
if
(
penetration_depth
)
*
penetration_depth
=
depth2
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
if
(
contact_points
)
*
contact_points
=
c2
+
dir_z
*
(
0.5
*
depth2
/
s
);
return
true
;
}
else
return
false
;
}
}
bool
convexHalfspaceIntersect
(
const
Convex
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
{
Halfspace
new_s2
=
transform
(
s2
,
tf2
);
Vec3f
v
;
FCL_REAL
depth
=
1
;
for
(
std
::
size_t
i
=
0
;
i
<
s1
.
num_points
;
++
i
)
{
Vec3f
p
=
s1
.
points
[
i
];
p
=
tf1
.
transform
(
p
);
FCL_REAL
d
=
(
new_s2
.
n
).
dot
(
p
)
-
new_s2
.
d
;
if
(
d
<
depth
)
{
depth
=
d
;
v
=
p
;
}
}
if
(
depth
<=
0
)
{
if
(
contact_points
)
*
contact_points
=
v
;
if
(
penetration_depth
)
*
penetration_depth
=
depth
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
return
true
;
}
else
return
false
;
}
bool
triangleHalfspaceIntersect
(
const
Triangle2
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
{
Halfspace
new_s2
=
transform
(
s2
,
tf2
);
Vec3f
v
=
tf1
.
transform
(
s1
.
a
);
FCL_REAL
depth
=
(
new_s2
.
n
).
dot
(
v
)
-
new_s2
.
d
;
Vec3f
p
=
tf1
.
transform
(
s1
.
b
);
FCL_REAL
d
=
(
new_s2
.
n
).
dot
(
p
)
-
new_s2
.
d
;
if
(
d
<
depth
)
{
depth
=
d
;
v
=
p
;
}
p
=
tf1
.
transform
(
s1
.
c
);
d
=
(
new_s2
.
n
).
dot
(
p
)
-
new_s2
.
d
;
if
(
d
<
depth
)
{
depth
=
d
;
v
=
p
;
}
if
(
depth
<=
0
)
{
if
(
contact_points
)
*
contact_points
=
v
;
if
(
penetration_depth
)
*
penetration_depth
=
depth
;
if
(
normal
)
*
normal
=
-
new_s2
.
n
;
return
true
;
}
else
return
false
;
}