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
6eeadecd
Commit
6eeadecd
authored
Jun 03, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Jun 14, 2016
Browse files
Remove Vec3f::length and Vec3f sqrLength
parent
be943c14
Changes
27
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/BV/AABB.h
View file @
6eeadecd
...
...
@@ -190,13 +190,13 @@ public:
/// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
inline
FCL_REAL
size
()
const
{
return
(
max_
-
min_
).
sq
rLength
();
return
(
max_
-
min_
).
sq
uaredNorm
();
}
/// @brief Radius of the AABB
inline
FCL_REAL
radius
()
const
{
return
(
max_
-
min_
).
length
()
/
2
;
return
(
max_
-
min_
).
norm
()
/
2
;
}
/// @brief Center of the AABB
...
...
include/hpp/fcl/BV/BV.h
View file @
6eeadecd
...
...
@@ -75,7 +75,7 @@ public:
static
void
convert
(
const
AABB
&
bv1
,
const
Transform3f
&
tf1
,
AABB
&
bv2
)
{
const
Vec3f
&
center
=
bv1
.
center
();
FCL_REAL
r
=
(
bv1
.
max_
-
bv1
.
min_
).
length
()
*
0.5
;
FCL_REAL
r
=
(
bv1
.
max_
-
bv1
.
min_
).
norm
()
*
0.5
;
Vec3f
center2
=
tf1
.
transform
(
center
);
Vec3f
delta
(
r
,
r
,
r
);
bv2
.
min_
=
center2
-
delta
;
...
...
@@ -180,7 +180,7 @@ public:
static
void
convert
(
const
BV1
&
bv1
,
const
Transform3f
&
tf1
,
AABB
&
bv2
)
{
const
Vec3f
&
center
=
bv1
.
center
();
FCL_REAL
r
=
Vec3f
(
bv1
.
width
(),
bv1
.
height
(),
bv1
.
depth
()).
length
()
*
0.5
;
FCL_REAL
r
=
Vec3f
(
bv1
.
width
(),
bv1
.
height
(),
bv1
.
depth
()).
norm
()
*
0.5
;
Vec3f
delta
(
r
,
r
,
r
);
Vec3f
center2
=
tf1
.
transform
(
center
);
bv2
.
min_
=
center2
-
delta
;
...
...
include/hpp/fcl/BV/OBB.h
View file @
6eeadecd
...
...
@@ -119,7 +119,7 @@ public:
/// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
inline
FCL_REAL
size
()
const
{
return
extent
.
sq
rLength
();
return
extent
.
sq
uaredNorm
();
}
/// @brief Center of the OBB
...
...
include/hpp/fcl/BV/kIOS.h
View file @
6eeadecd
...
...
@@ -58,7 +58,7 @@ class kIOS
static
kIOS_Sphere
encloseSphere
(
const
kIOS_Sphere
&
s0
,
const
kIOS_Sphere
&
s1
)
{
Vec3f
d
=
s1
.
o
-
s0
.
o
;
FCL_REAL
dist2
=
d
.
sq
rLength
();
FCL_REAL
dist2
=
d
.
sq
uaredNorm
();
FCL_REAL
diff_r
=
s1
.
r
-
s0
.
r
;
/** The sphere with the larger radius encloses the other */
...
...
include/hpp/fcl/BVH/BVH_model.h
View file @
6eeadecd
...
...
@@ -236,9 +236,8 @@ public:
const
Vec3f
&
v1
=
vertices
[
tri
[
0
]];
const
Vec3f
&
v2
=
vertices
[
tri
[
1
]];
const
Vec3f
&
v3
=
vertices
[
tri
[
2
]];
FCL_REAL
d_six_vol
=
(
v1
.
cross
(
v2
)).
dot
(
v3
);
Matrix3f
A
(
v1
,
v2
,
v3
);
C
+=
transpose
(
A
)
*
C_canonical
*
A
*
d_six_vol
;
C
+=
A
.
derived
().
transpose
()
*
C_canonical
*
A
*
(
v1
.
cross
(
v2
)).
dot
(
v3
)
;
}
FCL_REAL
trace_C
=
C
(
0
,
0
)
+
C
(
1
,
1
)
+
C
(
2
,
2
);
...
...
include/hpp/fcl/ccd/motion.h
View file @
6eeadecd
...
...
@@ -170,7 +170,7 @@ public:
// R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5
/// 1. compute M(1/2)
Vec3f
Rt0
=
(
Rd
[
0
]
+
Rd
[
1
]
*
23
+
Rd
[
2
]
*
23
+
Rd
[
3
])
*
(
1
/
48.0
);
FCL_REAL
Rt0_len
=
Rt0
.
length
();
FCL_REAL
Rt0_len
=
Rt0
.
norm
();
FCL_REAL
inv_Rt0_len
=
1.0
/
Rt0_len
;
FCL_REAL
inv_Rt0_len_3
=
inv_Rt0_len
*
inv_Rt0_len
*
inv_Rt0_len
;
FCL_REAL
inv_Rt0_len_5
=
inv_Rt0_len_3
*
inv_Rt0_len
*
inv_Rt0_len
;
...
...
@@ -196,7 +196,7 @@ public:
/// 3.1. compute M''(1/2)
Vec3f
ddRt0
=
(
Rd
[
0
]
-
Rd
[
1
]
-
Rd
[
2
]
+
Rd
[
3
])
*
0.5
;
FCL_REAL
Rt0_dot_ddRt0
=
Rt0
.
dot
(
ddRt0
);
FCL_REAL
dRt0_dot_dRt0
=
dRt0
.
sq
rLength
();
FCL_REAL
dRt0_dot_dRt0
=
dRt0
.
sq
uaredNorm
();
FCL_REAL
ddtheta0
=
(
Rt0_dot_ddRt0
+
dRt0_dot_dRt0
)
*
inv_Rt0_len
-
Rt0_dot_dRt0
*
Rt0_dot_dRt0
*
inv_Rt0_len_3
;
Vec3f
ddWt0
=
ddRt0
*
inv_Rt0_len
-
(
dRt0
*
(
2
*
Rt0_dot_dRt0
)
+
Rt0
*
(
Rt0_dot_ddRt0
+
dRt0_dot_dRt0
))
*
inv_Rt0_len_3
+
(
Rt0
*
(
3
*
Rt0_dot_dRt0
*
Rt0_dot_dRt0
))
*
inv_Rt0_len_5
;
Matrix3f
hatddWt0
;
...
...
@@ -367,7 +367,7 @@ protected:
{
angular_vel
=
0
;
axis
=
tf2
.
getTranslation
()
-
tf1
.
getTranslation
();
linear_vel
=
axis
.
length
();
linear_vel
=
axis
.
norm
();
p
=
tf1
.
getTranslation
();
}
else
...
...
include/hpp/fcl/eigen/vec_3fx.h
View file @
6eeadecd
...
...
@@ -322,10 +322,6 @@ public:
return
*
this
;
}
inline
T
length
()
const
{
return
this
->
norm
();
}
// inline T norm() const { return sqrt(details::dot_prod3(data, data)); }
inline
T
sqrLength
()
const
{
return
this
->
squaredNorm
();
}
// inline T squaredNorm() const { return details::dot_prod3(data, data); }
inline
void
setValue
(
T
x
,
T
y
,
T
z
)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
FclMatrix
,
3
);
this
->
m_storage
.
data
()[
0
]
=
x
;
...
...
@@ -548,9 +544,6 @@ public:
return
typename
UnaryReturnType
<
EigenOp
>::
Abs
(
*
this
);
}
inline
Scalar
length
()
const
{
return
this
->
norm
();
}
inline
Scalar
sqrLength
()
const
{
return
this
->
squaredNorm
();
}
template
<
typename
Derived
>
inline
bool
equal
(
const
Eigen
::
MatrixBase
<
Derived
>&
other
,
T
epsilon
=
std
::
numeric_limits
<
T
>::
epsilon
()
*
100
)
const
{
...
...
include/hpp/fcl/math/vec_3fx.h
View file @
6eeadecd
...
...
@@ -127,9 +127,7 @@ public:
return
*
this
;
}
inline
U
length
()
const
{
return
sqrt
(
details
::
dot_prod3
(
data
,
data
));
}
inline
U
norm
()
const
{
return
sqrt
(
details
::
dot_prod3
(
data
,
data
));
}
inline
U
sqrLength
()
const
{
return
details
::
dot_prod3
(
data
,
data
);
}
inline
U
squaredNorm
()
const
{
return
details
::
dot_prod3
(
data
,
data
);
}
inline
void
setValue
(
U
x
,
U
y
,
U
z
)
{
data
.
setValue
(
x
,
y
,
z
);
}
inline
void
setValue
(
U
x
)
{
data
.
setValue
(
x
);
}
...
...
include/hpp/fcl/math/vec_nf.h
deleted
100644 → 0
View file @
be943c14
#ifndef FCL_MATH_VEC_NF_H
#define FCL_MATH_VEC_NF_H
#include
<cmath>
#include
<iostream>
#include
<limits>
#include
<vector>
#include
<boost/array.hpp>
#include
<cstdarg>
#include
<hpp/fcl/data_types.h>
namespace
fcl
{
template
<
typename
T
,
std
::
size_t
N
>
class
Vec_n
{
public:
Vec_n
()
{
data
.
resize
(
N
,
0
);
}
template
<
std
::
size_t
M
>
Vec_n
(
const
Vec_n
<
T
,
M
>&
data_
)
{
std
::
size_t
n
=
std
::
min
(
M
,
N
);
for
(
std
::
size_t
i
=
0
;
i
<
n
;
++
i
)
data
[
i
]
=
data_
[
i
];
}
Vec_n
(
const
std
::
vector
<
T
>&
data_
)
{
data
.
resize
(
N
,
0
);
std
::
size_t
n
=
std
::
min
(
data_
.
size
(),
N
);
for
(
std
::
size_t
i
=
0
;
i
<
n
;
++
i
)
data
[
i
]
=
data_
[
i
];
}
bool
operator
==
(
const
Vec_n
<
T
,
N
>&
other
)
const
{
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
{
if
(
data
[
i
]
!=
other
[
i
])
return
false
;
}
return
true
;
}
bool
operator
!=
(
const
Vec_n
<
T
,
N
>&
other
)
const
{
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
{
if
(
data
[
i
]
!=
other
[
i
])
return
true
;
}
return
false
;
}
std
::
size_t
dim
()
const
{
return
N
;
}
void
setData
(
const
std
::
vector
<
T
>&
data_
)
{
std
::
size_t
n
=
std
::
min
(
data
.
size
(),
N
);
for
(
std
::
size_t
i
=
0
;
i
<
n
;
++
i
)
data
[
i
]
=
data_
[
i
];
}
T
operator
[]
(
std
::
size_t
i
)
const
{
return
data
[
i
];
}
T
&
operator
[]
(
std
::
size_t
i
)
{
return
data
[
i
];
}
Vec_n
<
T
,
N
>
operator
+
(
const
Vec_n
<
T
,
N
>&
other
)
const
{
Vec_n
<
T
,
N
>
result
;
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
result
[
i
]
=
data
[
i
]
+
other
[
i
];
return
result
;
}
Vec_n
<
T
,
N
>&
operator
+=
(
const
Vec_n
<
T
,
N
>&
other
)
{
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
data
[
i
]
+=
other
[
i
];
return
*
this
;
}
Vec_n
<
T
,
N
>
operator
-
(
const
Vec_n
<
T
,
N
>&
other
)
const
{
Vec_n
<
T
,
N
>
result
;
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
result
[
i
]
=
data
[
i
]
-
other
[
i
];
return
result
;
}
Vec_n
<
T
,
N
>&
operator
-=
(
const
Vec_n
<
T
,
N
>&
other
)
{
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
data
[
i
]
-=
other
[
i
];
return
*
this
;
}
Vec_n
<
T
,
N
>
operator
*
(
T
t
)
const
{
Vec_n
<
T
,
N
>
result
;
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
result
[
i
]
=
data
[
i
]
*
t
;
return
result
;
}
Vec_n
<
T
,
N
>&
operator
*=
(
T
t
)
{
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
data
[
i
]
*=
t
;
return
*
this
;
}
Vec_n
<
T
,
N
>
operator
/
(
T
t
)
const
{
Vec_n
<
T
,
N
>
result
;
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
result
[
i
]
=
data
[
i
]
/
5
;
return
result
;
}
Vec_n
<
T
,
N
>&
operator
/=
(
T
t
)
{
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
data
[
i
]
/=
t
;
return
*
this
;
}
Vec_n
<
T
,
N
>&
setZero
()
{
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
data
[
i
]
=
0
;
}
T
dot
(
const
Vec_n
<
T
,
N
>&
other
)
const
{
T
sum
=
0
;
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
sum
+=
data
[
i
]
*
other
[
i
];
return
sum
;
}
std
::
vector
<
T
>
getData
()
const
{
return
data
;
}
protected:
std
::
vector
<
T
>
data
;
};
template
<
typename
T1
,
std
::
size_t
N1
,
typename
T2
,
std
::
size_t
N2
>
void
repack
(
const
Vec_n
<
T1
,
N1
>&
input
,
Vec_n
<
T2
,
N2
>&
output
)
{
std
::
size_t
n
=
std
::
min
(
N1
,
N2
);
for
(
std
::
size_t
i
=
0
;
i
<
n
;
++
i
)
output
[
i
]
=
input
[
i
];
}
template
<
typename
T
,
std
::
size_t
N
>
Vec_n
<
T
,
N
>
operator
*
(
T
t
,
const
Vec_n
<
T
,
N
>&
v
)
{
return
v
*
t
;
}
template
<
typename
T
,
std
::
size_t
N
,
std
::
size_t
M
>
Vec_n
<
T
,
M
+
N
>
combine
(
const
Vec_n
<
T
,
N
>&
v1
,
const
Vec_n
<
T
,
M
>&
v2
)
{
Vec_n
<
T
,
M
+
N
>
v
;
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
v
[
i
]
=
v1
[
i
];
for
(
std
::
size_t
i
=
0
;
i
<
M
;
++
i
)
v
[
i
+
N
]
=
v2
[
i
];
return
v
;
}
template
<
typename
T
,
std
::
size_t
N
>
std
::
ostream
&
operator
<<
(
std
::
ostream
&
o
,
const
Vec_n
<
T
,
N
>&
v
)
{
o
<<
"("
;
for
(
std
::
size_t
i
=
0
;
i
<
N
;
++
i
)
{
if
(
i
==
N
-
1
)
o
<<
v
[
i
]
<<
")"
;
else
o
<<
v
[
i
]
<<
" "
;
}
return
o
;
}
// workaround for template alias
template
<
std
::
size_t
N
>
class
Vecnf
:
public
Vec_n
<
FCL_REAL
,
N
>
{
public:
Vecnf
(
const
Vec_n
<
FCL_REAL
,
N
>&
other
)
:
Vec_n
<
FCL_REAL
,
N
>
(
other
)
{}
Vecnf
()
:
Vec_n
<
FCL_REAL
,
N
>
()
{}
template
<
std
::
size_t
M
>
Vecnf
(
const
Vec_n
<
FCL_REAL
,
M
>&
other
)
:
Vec_n
<
FCL_REAL
,
N
>
(
other
)
{}
Vecnf
(
const
std
::
vector
<
FCL_REAL
>&
data_
)
:
Vec_n
<
FCL_REAL
,
N
>
(
data_
)
{}
};
}
#endif
include/hpp/fcl/narrowphase/narrowphase.h
View file @
6eeadecd
...
...
@@ -615,7 +615,7 @@ struct GJKSolver_indep
w1
+=
shape
.
support
(
-
gjk
.
getSimplex
()
->
c
[
i
]
->
d
,
1
)
*
p
;
}
if
(
distance
)
*
distance
=
(
w0
-
w1
).
length
();
if
(
distance
)
*
distance
=
(
w0
-
w1
).
norm
();
if
(
p1
)
*
p1
=
tf1
.
transform
(
w0
);
if
(
p2
)
*
p2
=
tf1
.
transform
(
w1
);
...
...
@@ -667,7 +667,7 @@ struct GJKSolver_indep
w1
+=
shape
.
support
(
-
gjk
.
getSimplex
()
->
c
[
i
]
->
d
,
1
)
*
p
;
}
if
(
distance
)
*
distance
=
(
w0
-
w1
).
length
();
if
(
distance
)
*
distance
=
(
w0
-
w1
).
norm
();
if
(
p1
)
*
p1
=
w0
;
if
(
p2
)
*
p2
=
shape
.
toshape0
.
transform
(
w1
);
return
true
;
...
...
@@ -717,7 +717,7 @@ struct GJKSolver_indep
w1
+=
shape
.
support
(
-
gjk
.
getSimplex
()
->
c
[
i
]
->
d
,
1
)
*
p
;
}
if
(
distance
)
*
distance
=
(
w0
-
w1
).
length
();
if
(
distance
)
*
distance
=
(
w0
-
w1
).
norm
();
if
(
p1
)
*
p1
=
tf1
.
transform
(
w0
);
if
(
p2
)
*
p2
=
tf1
.
transform
(
w1
);
return
true
;
...
...
include/hpp/fcl/octree.h
View file @
6eeadecd
...
...
@@ -94,7 +94,7 @@ public:
{
aabb_local
=
getRootBV
();
aabb_center
=
aabb_local
.
center
();
aabb_radius
=
(
aabb_local
.
min_
-
aabb_center
).
length
();
aabb_radius
=
(
aabb_local
.
min_
-
aabb_center
).
norm
();
}
/// @brief get the bounding volume for the root
...
...
include/hpp/fcl/shape/geometric_shapes.h
View file @
6eeadecd
...
...
@@ -369,9 +369,8 @@ public:
int
e_second
=
index
[(
j
+
1
)
%*
points_in_poly
];
const
Vec3f
&
v1
=
points
[
e_first
];
const
Vec3f
&
v2
=
points
[
e_second
];
FCL_REAL
d_six_vol
=
(
v1
.
cross
(
v2
)).
dot
(
v3
);
Matrix3f
A
(
v1
,
v2
,
v3
);
// this is A' in the original document
C
+=
transpose
(
A
)
*
C_canonical
*
A
*
d_six_vol
;
// change accordingly
C
+=
A
.
derived
().
transpose
()
*
C_canonical
*
A
*
(
v1
.
cross
(
v2
)).
dot
(
v3
);
}
points_in_poly
+=
(
*
points_in_poly
+
1
);
...
...
src/BV/OBB.cpp
View file @
6eeadecd
...
...
@@ -599,7 +599,7 @@ OBB OBB::operator + (const OBB& other) const
Vec3f
center_diff
=
To
-
other
.
To
;
FCL_REAL
max_extent
=
std
::
max
(
std
::
max
(
extent
[
0
],
extent
[
1
]),
extent
[
2
]);
FCL_REAL
max_extent2
=
std
::
max
(
std
::
max
(
other
.
extent
[
0
],
other
.
extent
[
1
]),
other
.
extent
[
2
]);
if
(
center_diff
.
length
()
>
2
*
(
max_extent
+
max_extent2
))
if
(
center_diff
.
norm
()
>
2
*
(
max_extent
+
max_extent2
))
{
return
merge_largedist
(
*
this
,
other
);
}
...
...
src/BV/RSS.cpp
View file @
6eeadecd
...
...
@@ -212,7 +212,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -241,7 +241,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -269,7 +269,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -297,7 +297,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -365,7 +365,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -393,7 +393,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -423,7 +423,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -451,7 +451,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -519,7 +519,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -547,7 +547,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -575,7 +575,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -603,7 +603,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -664,7 +664,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -692,7 +692,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -721,7 +721,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -749,7 +749,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
*
Q
=
S
+
(
*
P
);
}
return
S
.
length
();
return
S
.
norm
();
}
}
...
...
@@ -889,20 +889,20 @@ bool RSS::contain(const Vec3f& p) const
{
FCL_REAL
y
=
(
proj1
>
0
)
?
l
[
1
]
:
0
;
Vec3f
v
(
proj0
,
y
,
0
);
return
((
proj
-
v
).
sq
rLength
()
<
r
*
r
);
return
((
proj
-
v
).
sq
uaredNorm
()
<
r
*
r
);
}
else
if
((
proj1
<
l
[
1
])
&&
(
proj1
>
0
)
&&
((
proj0
<
0
)
||
(
proj0
>
l
[
0
])))
{
FCL_REAL
x
=
(
proj0
>
0
)
?
l
[
0
]
:
0
;
Vec3f
v
(
x
,
proj1
,
0
);
return
((
proj
-
v
).
sq
rLength
()
<
r
*
r
);
return
((
proj
-
v
).
sq
uaredNorm
()
<
r
*
r
);
}
else
{
FCL_REAL
x
=
(
proj0
>
0
)
?
l
[
0
]
:
0
;
FCL_REAL
y
=
(
proj1
>
0
)
?
l
[
1
]
:
0
;
Vec3f
v
(
x
,
y
,
0
);
return
((
proj
-
v
).
sq
rLength
()
<
r
*
r
);
return
((
proj
-
v
).
sq
uaredNorm
()
<
r
*
r
);
}
}
...
...
@@ -934,7 +934,7 @@ RSS& RSS::operator += (const Vec3f& p)
{
FCL_REAL
y
=
(
proj1
>
0
)
?
l
[
1
]
:
0
;
Vec3f
v
(
proj0
,
y
,
0
);
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
sq
rLength
();
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
sq
uaredNorm
();
if
(
new_r_sqr
<
r
*
r
)
;
// do nothing
else
...
...
@@ -964,7 +964,7 @@ RSS& RSS::operator += (const Vec3f& p)
{
FCL_REAL
x
=
(
proj0
>
0
)
?
l
[
0
]
:
0
;
Vec3f
v
(
x
,
proj1
,
0
);
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
sq
rLength
();
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
sq
uaredNorm
();
if
(
new_r_sqr
<
r
*
r
)
;
// do nothing
else
...
...
@@ -995,7 +995,7 @@ RSS& RSS::operator += (const Vec3f& p)
FCL_REAL
x
=
(
proj0
>
0
)
?
l
[
0
]
:
0
;
FCL_REAL
y
=
(
proj1
>
0
)
?
l
[
1
]
:
0
;
Vec3f
v
(
x
,
y
,
0
);
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
sq
rLength
();
FCL_REAL
new_r_sqr
=
(
proj
-
v
).
sq
uaredNorm
();
if
(
new_r_sqr
<
r
*
r
)
;
// do nothing
else
...
...
src/BV/kIOS.cpp
View file @
6eeadecd
...
...
@@ -51,7 +51,7 @@ bool kIOS::overlap(const kIOS& other) const
{
for
(
unsigned
int
j
=
0
;
j
<
other
.