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
7c60ab49
Commit
7c60ab49
authored
Sep 27, 2019
by
Lucile Remigy
Browse files
changement nom de variable pour rendre le code plus claire
parent
3211b8ff
Changes
6
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
7c60ab49
...
...
@@ -132,7 +132,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/BVH/BVH_model.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/BV.h
View file @
7c60ab49
...
...
@@ -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/RSS.h
View file @
7c60ab49
...
...
@@ -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.
...
...
src/BV/RSS.cpp
View file @
7c60ab49
...
...
@@ -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
);
else
Tr
[
2
]
-=
0.5
*
(
abs_proj2
-
r
);
Tr
[
2
]
-=
0.5
*
(
abs_proj2
-
r
adius
);
}
}
}
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
);
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
diag
=
std
::
sqrt
(
new_r_sqr
-
proj2
*
proj2
);
FCL_REAL
delta_diag
=
-
std
::
sqrt
(
r
*
r
-
proj2
*
proj2
)
+
diag
;
FCL_REAL
delta_diag
=
-
std
::
sqrt
(
r
adius
*
radius
-
proj2
*
proj2
)
+
diag
;
FCL_REAL
delta_x
=
delta_diag
/
diag
*
fabs
(
proj0
-
x
);
FCL_REAL
delta_y
=
delta_diag
/
diag
*
fabs
(
proj1
-
y
);
l
[
0
]
+=
delta_x
;
l
[
1
]
+=
delta_y
;
l
ength
[
0
]
+=
delta_x
;
l
ength
[
1
]
+=
delta_y
;
if
(
proj0
<
0
&&
proj1
<
0
)
{
...
...
@@ -1033,8 +1033,8 @@ RSS& RSS::operator += (const Vec3f& p)
FCL_REAL
delta_x
=
fabs
(
proj0
-
x
);
FCL_REAL
delta_y
=
fabs
(
proj1
-
y
);
l
[
0
]
+=
delta_x
;
l
[
1
]
+=
delta_y
;
l
ength
[
0
]
+=
delta_x
;
l
ength
[
1
]
+=
delta_y
;
if
(
proj0
<
0
&&
proj1
<
0
)
{
...
...
@@ -1043,9 +1043,9 @@ RSS& RSS::operator += (const Vec3f& p)
}
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
);
}
}
}
...
...
@@ -1058,12 +1058,12 @@ RSS RSS::operator + (const RSS& other) const
RSS
bv
;
Vec3f
v
[
16
];
Vec3f
d0_pos
(
other
.
axes
.
col
(
0
)
*
(
other
.
l
[
0
]
+
other
.
r
));
Vec3f
d1_pos
(
other
.
axes
.
col
(
1
)
*
(
other
.
l
[
1
]
+
other
.
r
));
Vec3f
d0_neg
(
other
.
axes
.
col
(
0
)
*
(
-
other
.
r
));
Vec3f
d1_neg
(
other
.
axes
.
col
(
1
)
*
(
-
other
.
r
));
Vec3f
d2_pos
(
other
.
axes
.
col
(
2
)
*
other
.
r
);
Vec3f
d2_neg
(
other
.
axes
.
col
(
2
)
*
(
-
other
.
r
));
Vec3f
d0_pos
(
other
.
axes
.
col
(
0
)
*
(
other
.
l
ength
[
0
]
+
other
.
r
adius
));
Vec3f
d1_pos
(
other
.
axes
.
col
(
1
)
*
(
other
.
l
ength
[
1
]
+
other
.
r
adius
));
Vec3f
d0_neg
(
other
.
axes
.
col
(
0
)
*
(
-
other
.
r
adius
));
Vec3f
d1_neg
(
other
.
axes
.
col
(
1
)
*
(
-
other
.
r
adius
));
Vec3f
d2_pos
(
other
.
axes
.
col
(
2
)
*
other
.
r
adius
);
Vec3f
d2_neg
(
other
.
axes
.
col
(
2
)
*
(
-
other
.
r
adius
));
v
[
0
].
noalias
()
=
other
.
Tr
+
d0_pos
+
d1_pos
+
d2_pos
;
v
[
1
].
noalias
()
=
other
.
Tr
+
d0_pos
+
d1_pos
+
d2_neg
;
...
...
@@ -1074,12 +1074,12 @@ RSS RSS::operator + (const RSS& other) const
v
[
6
].
noalias
()
=
other
.
Tr
+
d0_neg
+
d1_neg
+
d2_pos
;
v
[
7
].
noalias
()
=
other
.
Tr
+
d0_neg
+
d1_neg
+
d2_neg
;
d0_pos
.
noalias
()
=
axes
.
col
(
0
)
*
(
l
[
0
]
+
r
);
d1_pos
.
noalias
()
=
axes
.
col
(
1
)
*
(
l
[
1
]
+
r
);
d0_neg
.
noalias
()
=
axes
.
col
(
0
)
*
(
-
r
);
d1_neg
.
noalias
()
=
axes
.
col
(
1
)
*
(
-
r
);
d2_pos
.
noalias
()
=
axes
.
col
(
2
)
*
r
;
d2_neg
.
noalias
()
=
axes
.
col
(
2
)
*
(
-
r
);
d0_pos
.
noalias
()
=
axes
.
col
(
0
)
*
(
l
ength
[
0
]
+
r
adius
);
d1_pos
.
noalias
()
=
axes
.
col
(
1
)
*
(
l
ength
[
1
]
+
r
adius
);
d0_neg
.
noalias
()
=
axes
.
col
(
0
)
*
(
-
r
adius
);
d1_neg
.
noalias
()
=
axes
.
col
(
1
)
*
(
-
r
adius
);
d2_pos
.
noalias
()
=
axes
.
col
(
2
)
*
r
adius
;
d2_neg
.
noalias
()
=
axes
.
col
(
2
)
*
(
-
r
adius
);
v
[
8
].
noalias
()
=
Tr
+
d0_pos
+
d1_pos
+
d2_pos
;
v
[
9
].
noalias
()
=
Tr
+
d0_pos
+
d1_pos
+
d2_neg
;
...
...
@@ -1113,7 +1113,7 @@ RSS RSS::operator + (const RSS& other) const
E
[
0
][
max
]
*
E
[
1
][
mid
]
-
E
[
0
][
mid
]
*
E
[
1
][
max
];
// set rss origin, rectangle size and radius
getRadiusAndOriginAndRectangleSize
(
v
,
NULL
,
NULL
,
NULL
,
16
,
bv
.
axes
,
bv
.
Tr
,
bv
.
l
,
bv
.
r
);
getRadiusAndOriginAndRectangleSize
(
v
,
NULL
,
NULL
,
NULL
,
16
,
bv
.
axes
,
bv
.
Tr
,
bv
.
l
ength
,
bv
.
r
adius
);
return
bv
;
}
...
...
@@ -1126,8 +1126,8 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
Matrix3f
R
(
axes
.
transpose
()
*
other
.
axes
);
Vec3f
T
(
axes
.
transpose
()
*
(
other
.
Tr
-
Tr
));
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
l
,
other
.
l
,
P
,
Q
);
dist
-=
(
r
+
other
.
r
);
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
l
ength
,
other
.
l
ength
,
P
,
Q
);
dist
-=
(
r
adius
+
other
.
r
adius
);
return
(
dist
<
(
FCL_REAL
)
0.0
)
?
(
FCL_REAL
)
0.0
:
dist
;
}
...
...
@@ -1138,8 +1138,8 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
Vec3f
T
(
b1
.
axes
.
transpose
()
*
Ttemp
);
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
b1
.
l
,
b2
.
l
,
P
,
Q
);
dist
-=
(
b1
.
r
+
b2
.
r
);
FCL_REAL
dist
=
rectDistance
(
R
,
T
,
b1
.
l
ength
,
b2
.
l
ength
,
P
,
Q
);
dist
-=
(
b1
.
r
adius
+
b2
.
r
adius
);
return
(
dist
<
(
FCL_REAL
)
0.0
)
?
(
FCL_REAL
)
0.0
:
dist
;
}
...
...
src/BVH/BV_fitter.cpp
View file @
7c60ab49
...
...
@@ -150,9 +150,9 @@ void fit1(Vec3f* ps, RSS& bv)
{
bv
.
Tr
.
noalias
()
=
ps
[
0
];
bv
.
axes
.
setIdentity
();
bv
.
l
[
0
]
=
0
;
bv
.
l
[
1
]
=
0
;
bv
.
r
=
0
;
bv
.
l
ength
[
0
]
=
0
;
bv
.
l
ength
[
1
]
=
0
;
bv
.
r
adius
=
0
;
}
void
fit2
(
Vec3f
*
ps
,
RSS
&
bv
)
...
...
@@ -164,11 +164,11 @@ void fit2(Vec3f* ps, RSS& bv)
bv
.
axes
.
col
(
0
)
/=
len_p1p2
;
generateCoordinateSystem
(
bv
.
axes
.
col
(
0
),
bv
.
axes
.
col
(
1
),
bv
.
axes
.
col
(
2
));
bv
.
l
[
0
]
=
len_p1p2
;
bv
.
l
[
1
]
=
0
;
bv
.
l
ength
[
0
]
=
len_p1p2
;
bv
.
l
ength
[
1
]
=
0
;
bv
.
Tr
=
p2
;
bv
.
r
=
0
;
bv
.
r
adius
=
0
;
}
void
fit3
(
Vec3f
*
ps
,
RSS
&
bv
)
...
...
@@ -193,7 +193,7 @@ void fit3(Vec3f* ps, RSS& bv)
bv
.
axes
.
col
(
0
).
noalias
()
=
e
[
imax
].
normalized
();
bv
.
axes
.
col
(
1
).
noalias
()
=
bv
.
axes
.
col
(
2
).
cross
(
bv
.
axes
.
col
(
0
));
getRadiusAndOriginAndRectangleSize
(
ps
,
NULL
,
NULL
,
NULL
,
3
,
bv
.
axes
,
bv
.
Tr
,
bv
.
l
,
bv
.
r
);
getRadiusAndOriginAndRectangleSize
(
ps
,
NULL
,
NULL
,
NULL
,
3
,
bv
.
axes
,
bv
.
Tr
,
bv
.
l
ength
,
bv
.
r
adius
);
}
void
fit6
(
Vec3f
*
ps
,
RSS
&
bv
)
...
...
@@ -215,7 +215,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
axisFromEigen
(
E
,
s
,
bv
.
axes
);
// set rss origin, rectangle size and radius
getRadiusAndOriginAndRectangleSize
(
ps
,
NULL
,
NULL
,
NULL
,
n
,
bv
.
axes
,
bv
.
Tr
,
bv
.
l
,
bv
.
r
);
getRadiusAndOriginAndRectangleSize
(
ps
,
NULL
,
NULL
,
NULL
,
n
,
bv
.
axes
,
bv
.
Tr
,
bv
.
l
ength
,
bv
.
r
adius
);
}
}
...
...
@@ -542,9 +542,9 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
getRadiusAndOriginAndRectangleSize
(
vertices
,
prev_vertices
,
tri_indices
,
primitive_indices
,
num_primitives
,
bv
.
rss
.
axes
,
origin
,
l
,
r
);
bv
.
rss
.
Tr
=
origin
;
bv
.
rss
.
l
[
0
]
=
l
[
0
];
bv
.
rss
.
l
[
1
]
=
l
[
1
];
bv
.
rss
.
r
=
r
;
bv
.
rss
.
l
ength
[
0
]
=
l
[
0
];
bv
.
rss
.
l
ength
[
1
]
=
l
[
1
];
bv
.
rss
.
r
adius
=
r
;
return
bv
;
}
...
...
@@ -568,9 +568,9 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
getRadiusAndOriginAndRectangleSize
(
vertices
,
prev_vertices
,
tri_indices
,
primitive_indices
,
num_primitives
,
bv
.
axes
,
origin
,
l
,
r
);
bv
.
Tr
=
origin
;
bv
.
l
[
0
]
=
l
[
0
];
bv
.
l
[
1
]
=
l
[
1
];
bv
.
r
=
r
;
bv
.
l
ength
[
0
]
=
l
[
0
];
bv
.
l
ength
[
1
]
=
l
[
1
];
bv
.
r
adius
=
r
;
return
bv
;
...
...
src/shape/geometric_shapes_utility.cpp
View file @
7c60ab49
...
...
@@ -482,7 +482,7 @@ void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv)
/// Half space can only have very rough RSS
bv
.
axes
.
setIdentity
();
bv
.
Tr
.
setZero
();
bv
.
l
[
0
]
=
bv
.
l
[
1
]
=
bv
.
r
=
std
::
numeric_limits
<
FCL_REAL
>::
max
();
bv
.
l
ength
[
0
]
=
bv
.
l
ength
[
1
]
=
bv
.
r
adius
=
std
::
numeric_limits
<
FCL_REAL
>::
max
();
}
template
<
>
...
...
@@ -716,10 +716,10 @@ void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
generateCoordinateSystem
(
n
,
bv
.
axes
.
col
(
1
),
bv
.
axes
.
col
(
2
));
bv
.
axes
.
col
(
0
).
noalias
()
=
n
;
bv
.
l
[
0
]
=
std
::
numeric_limits
<
FCL_REAL
>::
max
();
bv
.
l
[
1
]
=
std
::
numeric_limits
<
FCL_REAL
>::
max
();
bv
.
l
ength
[
0
]
=
std
::
numeric_limits
<
FCL_REAL
>::
max
();
bv
.
l
ength
[
1
]
=
std
::
numeric_limits
<
FCL_REAL
>::
max
();
bv
.
r
=
0
;
bv
.
r
adius
=
0
;
Vec3f
p
=
s
.
n
*
s
.
d
;
bv
.
Tr
=
tf
.
transform
(
p
);
...
...
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