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
Humanoid Path Planner
hpp-fcl
Commits
5d20d8c9
Commit
5d20d8c9
authored
Sep 21, 2020
by
Joseph Mirabel
Browse files
Fix halfspaceDistance + add tiny unit-test.
parent
7ad91cce
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/narrowphase/details.h
View file @
5d20d8c9
...
...
@@ -1955,11 +1955,10 @@ namespace fcl {
Vec3f
n_2
(
tf2
.
getRotation
().
transpose
()
*
n_w
);
int
hint
=
0
;
p2
=
getSupport
(
&
s
,
-
n_2
,
true
,
hint
);
dist
=
p2
.
dot
(
n_2
)
-
h
.
d
;
p1
=
p2
-
dist
*
n_2
;
p1
=
tf2
.
transform
(
p1
);
p2
=
tf2
.
transform
(
p2
);
dist
=
(
p2
-
tf1
.
getTranslation
()).
dot
(
n_w
)
-
h
.
d
;
p1
=
p2
-
dist
*
n_w
;
normal
=
n_w
;
return
dist
<=
0
;
...
...
test/python_unit/CMakeLists.txt
View file @
5d20d8c9
SET
(
${
PROJECT_NAME
}
_PYTHON_TESTS
geometric_shapes
api
collision
)
FOREACH
(
TEST
${${
PROJECT_NAME
}
_PYTHON_TESTS
}
)
...
...
test/python_unit/collision.py
0 → 100644
View file @
5d20d8c9
import
unittest
from
test_case
import
TestCase
import
hppfcl
hppfcl
.
switchToNumpyArray
()
import
numpy
as
np
def
tetahedron
():
pts
=
hppfcl
.
StdVec_Vec3f
()
pts
.
append
(
np
.
array
((
0
,
0
,
0
))
)
pts
.
append
(
np
.
array
((
0
,
1
,
0
))
)
pts
.
append
(
np
.
array
((
1
,
0
,
0
))
)
pts
.
append
(
np
.
array
((
0
,
0
,
1
))
)
tri
=
hppfcl
.
StdVec_Triangle
()
tri
.
append
(
hppfcl
.
Triangle
(
0
,
1
,
2
))
tri
.
append
(
hppfcl
.
Triangle
(
0
,
1
,
3
))
tri
.
append
(
hppfcl
.
Triangle
(
0
,
2
,
3
))
tri
.
append
(
hppfcl
.
Triangle
(
1
,
2
,
3
))
return
hppfcl
.
Convex
(
pts
,
tri
)
class
TestMainAPI
(
TestCase
):
def
test_convex_halfspace
(
self
):
convex
=
tetahedron
()
halfspace
=
hppfcl
.
Halfspace
(
np
.
array
((
0
,
0
,
1
)),
0
)
req
=
hppfcl
.
CollisionRequest
()
res
=
hppfcl
.
CollisionResult
()
M1
=
hppfcl
.
Transform3f
()
M2
=
hppfcl
.
Transform3f
(
np
.
eye
(
3
),
np
.
array
([
0
,
0
,
-
0.1
]))
res
.
clear
()
hppfcl
.
collide
(
convex
,
M1
,
halfspace
,
M2
,
req
,
res
)
self
.
assertFalse
(
hppfcl
.
collide
(
convex
,
M1
,
halfspace
,
M2
,
req
,
res
))
M1
=
hppfcl
.
Transform3f
()
M2
=
hppfcl
.
Transform3f
(
np
.
eye
(
3
),
np
.
array
([
0
,
0
,
0.1
]))
res
.
clear
()
self
.
assertTrue
(
hppfcl
.
collide
(
convex
,
M1
,
halfspace
,
M2
,
req
,
res
))
M1
=
hppfcl
.
Transform3f
()
M2
=
hppfcl
.
Transform3f
(
np
.
eye
(
3
),
np
.
array
([
0
,
0
,
2
]))
res
.
clear
()
self
.
assertTrue
(
hppfcl
.
collide
(
convex
,
M1
,
halfspace
,
M2
,
req
,
res
))
if
__name__
==
'__main__'
:
unittest
.
main
()
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