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
fdd9b54d
Commit
fdd9b54d
authored
Jun 08, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Jun 14, 2016
Browse files
Replace setValue by operator<< and remove normalize(bool)
parent
48ba15ac
Changes
18
Expand all
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/ccd/motion.h
View file @
fdd9b54d
...
...
@@ -268,7 +268,7 @@ public:
ScrewMotion
()
:
MotionBase
()
{
// Default angular velocity is zero
axis
.
setValue
(
1
,
0
,
0
)
;
axis
<<
1
,
0
,
0
;
angular_vel
=
0
;
// Default reference point is local zero point
...
...
include/hpp/fcl/eigen/vec_3fx.h
View file @
fdd9b54d
...
...
@@ -236,7 +236,7 @@ public:
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
zz
)
:
Base
()
{
setValue
(
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
)
;
*
this
<<
xx
,
xy
,
xz
,
yx
,
yy
,
yz
,
zx
,
zy
,
zz
;
}
template
<
typename
Vector
>
...
...
@@ -303,42 +303,12 @@ public:
return
*
this
;
}
inline
FclMatrix
&
normalize
(
bool
*
signal
)
{
T
sqr_length
=
this
->
squaredNorm
();
if
(
sqr_length
>
0
)
{
this
->
operator
/=
((
T
)
std
::
sqrt
(
sqr_length
));
*
signal
=
true
;
}
else
*
signal
=
false
;
return
*
this
;
}
inline
FclMatrix
&
abs
()
{
*
this
=
this
->
cwiseAbs
();
return
*
this
;
}
inline
void
setValue
(
T
x
,
T
y
,
T
z
)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
FclMatrix
,
3
);
this
->
m_storage
.
data
()[
0
]
=
x
;
this
->
m_storage
.
data
()[
1
]
=
y
;
this
->
m_storage
.
data
()[
2
]
=
z
;
}
inline
void
setValue
(
T
xx
,
T
xy
,
T
xz
,
T
yx
,
T
yy
,
T
yz
,
T
zx
,
T
zy
,
T
zz
)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
(
FclMatrix
,
3
,
3
);
this
->
operator
()(
0
,
0
)
=
xx
;
this
->
operator
()(
0
,
1
)
=
xy
;
this
->
operator
()(
0
,
2
)
=
xz
;
this
->
operator
()(
1
,
0
)
=
yx
;
this
->
operator
()(
1
,
1
)
=
yy
;
this
->
operator
()(
1
,
2
)
=
yz
;
this
->
operator
()(
2
,
0
)
=
zx
;
this
->
operator
()(
2
,
1
)
=
zy
;
this
->
operator
()(
2
,
2
)
=
zz
;
}
inline
void
setValue
(
T
x
)
{
this
->
setConstant
(
x
);
}
// inline void setZero () {data.setValue (0); }
inline
bool
equal
(
const
FclMatrix
&
other
,
T
epsilon
=
std
::
numeric_limits
<
T
>::
epsilon
()
*
100
)
const
{
return
((
*
this
-
other
).
cwiseAbs
().
array
()
<
epsilon
).
all
();
...
...
@@ -440,9 +410,9 @@ public:
Scalar
sc
=
si
*
ch
;
Scalar
ss
=
si
*
sh
;
setValue
(
cj
*
ch
,
sj
*
sc
-
cs
,
sj
*
cc
+
ss
,
*
this
<<
cj
*
ch
,
sj
*
sc
-
cs
,
sj
*
cc
+
ss
,
cj
*
sh
,
sj
*
ss
+
cc
,
sj
*
cs
-
sc
,
-
sj
,
cj
*
si
,
cj
*
ci
)
;
-
sj
,
cj
*
si
,
cj
*
ci
;
}
...
...
@@ -672,7 +642,7 @@ void generateCoordinateSystem(
template
<
typename
Matrix
,
typename
Vector
>
void
hat
(
Matrix
&
mat
,
const
Vector
&
vec
)
{
mat
.
setValue
(
0
,
-
vec
[
2
],
vec
[
1
],
vec
[
2
],
0
,
-
vec
[
0
],
-
vec
[
1
],
vec
[
0
],
0
)
;
mat
<<
0
,
-
vec
[
2
],
vec
[
1
],
vec
[
2
],
0
,
-
vec
[
0
],
-
vec
[
1
],
vec
[
0
],
0
;
}
template
<
typename
Matrix
,
typename
Vector
>
...
...
@@ -715,9 +685,9 @@ void eigen(const FclType<Matrix>& m, typename Matrix::Scalar dout[3], Vector* vo
sm
+=
std
::
abs
(
R
(
ip
,
iq
));
if
(
sm
==
0.0
)
{
vout
[
0
]
.
setValue
(
v
[
0
][
0
],
v
[
0
][
1
],
v
[
0
][
2
]
)
;
vout
[
1
]
.
setValue
(
v
[
1
][
0
],
v
[
1
][
1
],
v
[
1
][
2
]
)
;
vout
[
2
]
.
setValue
(
v
[
2
][
0
],
v
[
2
][
1
],
v
[
2
][
2
]
)
;
vout
[
0
]
<<
v
[
0
][
0
],
v
[
0
][
1
],
v
[
0
][
2
];
vout
[
1
]
<<
v
[
1
][
0
],
v
[
1
][
1
],
v
[
1
][
2
];
vout
[
2
]
<<
v
[
2
][
0
],
v
[
2
][
1
],
v
[
2
][
2
];
dout
[
0
]
=
d
[
0
];
dout
[
1
]
=
d
[
1
];
dout
[
2
]
=
d
[
2
];
return
;
}
...
...
include/hpp/fcl/math/transform.h
View file @
fdd9b54d
...
...
@@ -362,7 +362,7 @@ public:
inline
void
setIdentity
()
{
R
.
setIdentity
();
T
.
set
Value
(
0
);
T
.
set
Zero
(
);
q
=
Quaternion3f
();
matrix_set
=
true
;
}
...
...
include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
View file @
fdd9b54d
...
...
@@ -55,14 +55,14 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f&
double
c
=
shape
.
side
[
2
];
std
::
vector
<
Vec3f
>
points
(
8
);
std
::
vector
<
Triangle
>
tri_indices
(
12
);
points
[
0
]
.
setValue
(
0.5
*
a
,
-
0.5
*
b
,
0.5
*
c
)
;
points
[
1
]
.
setValue
(
0.5
*
a
,
0.5
*
b
,
0.5
*
c
)
;
points
[
2
]
.
setValue
(
-
0.5
*
a
,
0.5
*
b
,
0.5
*
c
)
;
points
[
3
]
.
setValue
(
-
0.5
*
a
,
-
0.5
*
b
,
0.5
*
c
)
;
points
[
4
]
.
setValue
(
0.5
*
a
,
-
0.5
*
b
,
-
0.5
*
c
)
;
points
[
5
]
.
setValue
(
0.5
*
a
,
0.5
*
b
,
-
0.5
*
c
)
;
points
[
6
]
.
setValue
(
-
0.5
*
a
,
0.5
*
b
,
-
0.5
*
c
)
;
points
[
7
]
.
setValue
(
-
0.5
*
a
,
-
0.5
*
b
,
-
0.5
*
c
)
;
points
[
0
]
<<
0.5
*
a
,
-
0.5
*
b
,
0.5
*
c
;
points
[
1
]
<<
0.5
*
a
,
0.5
*
b
,
0.5
*
c
;
points
[
2
]
<<
-
0.5
*
a
,
0.5
*
b
,
0.5
*
c
;
points
[
3
]
<<
-
0.5
*
a
,
-
0.5
*
b
,
0.5
*
c
;
points
[
4
]
<<
0.5
*
a
,
-
0.5
*
b
,
-
0.5
*
c
;
points
[
5
]
<<
0.5
*
a
,
0.5
*
b
,
-
0.5
*
c
;
points
[
6
]
<<
-
0.5
*
a
,
0.5
*
b
,
-
0.5
*
c
;
points
[
7
]
<<
-
0.5
*
a
,
-
0.5
*
b
,
-
0.5
*
c
;
tri_indices
[
0
].
set
(
0
,
4
,
1
);
tri_indices
[
1
].
set
(
1
,
4
,
5
);
...
...
src/BV/OBB.cpp
View file @
fdd9b54d
...
...
@@ -100,8 +100,8 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2)
else
{
mid
=
2
;
}
R1
.
setValue
(
E
[
0
][
max
],
E
[
1
][
max
],
E
[
2
][
max
]
)
;
R2
.
setValue
(
E
[
0
][
mid
],
E
[
1
][
mid
],
E
[
2
][
mid
]
)
;
R1
<<
E
[
0
][
max
],
E
[
1
][
max
],
E
[
2
][
max
];
R2
<<
E
[
0
][
mid
],
E
[
1
][
mid
],
E
[
2
][
mid
];
// set obb centers and extensions
Vec3f
center
,
extent
;
...
...
@@ -588,7 +588,7 @@ OBB& OBB::operator += (const Vec3f& p)
bvp
.
axis
[
0
]
=
axis
[
0
];
bvp
.
axis
[
1
]
=
axis
[
1
];
bvp
.
axis
[
2
]
=
axis
[
2
];
bvp
.
extent
.
set
Value
(
0
);
bvp
.
extent
.
set
Zero
(
);
*
this
+=
bvp
;
return
*
this
;
...
...
src/BV/RSS.cpp
View file @
fdd9b54d
...
...
@@ -208,7 +208,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
a
[
0
],
t
,
0
)
;
*
P
<<
a
[
0
],
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -237,7 +237,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
a
[
0
],
t
,
0
)
;
*
P
<<
a
[
0
],
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -265,7 +265,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
0
,
t
,
0
)
;
*
P
<<
0
,
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -293,7 +293,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
0
,
t
,
0
)
;
*
P
<<
0
,
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -361,7 +361,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
a
[
0
],
t
,
0
)
;
*
P
<<
a
[
0
],
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -389,7 +389,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
a
[
0
],
t
,
0
)
;
*
P
<<
a
[
0
],
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -418,7 +418,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
0
,
t
,
0
)
;
*
P
<<
0
,
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -447,7 +447,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
0
,
t
,
0
)
;
*
P
<<
0
,
t
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -515,7 +515,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
a
[
1
],
0
)
;
*
P
<<
t
,
a
[
1
],
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -543,7 +543,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
a
[
1
],
0
)
;
*
P
<<
t
,
a
[
1
],
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -571,7 +571,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
0
,
0
)
;
*
P
<<
t
,
0
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -599,7 +599,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
0
,
0
)
;
*
P
<<
t
,
0
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -660,7 +660,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
a
[
1
],
0
)
;
*
P
<<
t
,
a
[
1
],
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -688,7 +688,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
a
[
1
],
0
)
;
*
P
<<
t
,
a
[
1
],
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -717,7 +717,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
0
,
0
)
;
*
P
<<
t
,
0
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -745,7 +745,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
P
&&
Q
)
{
P
->
setValue
(
t
,
0
,
0
)
;
*
P
<<
t
,
0
,
0
;
*
Q
=
S
+
(
*
P
);
}
...
...
@@ -786,14 +786,14 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
if
(
sep1
>=
sep2
&&
sep1
>=
0
)
{
if
(
Tab
[
2
]
>
0
)
S
.
setValue
(
0
,
0
,
sep1
)
;
S
<<
0
,
0
,
sep1
;
else
S
.
setValue
(
0
,
0
,
-
sep1
)
;
S
<<
0
,
0
,
-
sep1
;
if
(
P
&&
Q
)
{
*
Q
=
S
;
P
->
set
Value
(
0
);
P
->
set
Zero
(
);
}
}
...
...
@@ -1094,11 +1094,11 @@ RSS RSS::operator + (const RSS& other) const
else
{
mid
=
2
;
}
// column first matrix, as the axis in RSS
bv
.
axis
[
0
]
.
setValue
(
E
[
0
][
max
],
E
[
1
][
max
],
E
[
2
][
max
]
)
;
bv
.
axis
[
1
]
.
setValue
(
E
[
0
][
mid
],
E
[
1
][
mid
],
E
[
2
][
mid
]
)
;
bv
.
axis
[
2
]
.
setValue
(
E
[
1
][
max
]
*
E
[
2
][
mid
]
-
E
[
1
][
mid
]
*
E
[
2
][
max
],
E
[
0
][
mid
]
*
E
[
2
][
max
]
-
E
[
0
][
max
]
*
E
[
2
][
mid
],
E
[
0
][
max
]
*
E
[
1
][
mid
]
-
E
[
0
][
mid
]
*
E
[
1
][
max
]
)
;
bv
.
axis
[
0
]
<<
E
[
0
][
max
],
E
[
1
][
max
],
E
[
2
][
max
];
bv
.
axis
[
1
]
<<
E
[
0
][
mid
],
E
[
1
][
mid
],
E
[
2
][
mid
];
bv
.
axis
[
2
]
<<
E
[
1
][
max
]
*
E
[
2
][
mid
]
-
E
[
1
][
mid
]
*
E
[
2
][
max
],
E
[
0
][
mid
]
*
E
[
2
][
max
]
-
E
[
0
][
max
]
*
E
[
2
][
mid
],
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
.
axis
,
bv
.
Tr
,
bv
.
l
,
bv
.
r
);
...
...
src/BVH/BVH_model.cpp
View file @
fdd9b54d
...
...
@@ -712,7 +712,7 @@ int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_pri
FCL_REAL
x
=
(
p1
[
0
]
+
p2
[
0
]
+
p3
[
0
])
/
3.0
;
FCL_REAL
y
=
(
p1
[
1
]
+
p2
[
1
]
+
p3
[
1
])
/
3.0
;
FCL_REAL
z
=
(
p1
[
2
]
+
p2
[
2
]
+
p3
[
2
])
/
3.0
;
p
.
setValue
(
x
,
y
,
z
)
;
p
<<
x
,
y
,
z
;
}
else
{
...
...
src/BVH/BVH_utility.cpp
View file @
fdd9b54d
...
...
@@ -520,9 +520,9 @@ static inline void getExtentAndCenter_pointcloud(Vec3f* ps, Vec3f* ps2, unsigned
center
=
axis
[
0
]
*
o
[
0
]
+
axis
[
1
]
*
o
[
1
]
+
axis
[
2
]
*
o
[
2
];
extent
.
setValue
(
(
max_coord
[
0
]
-
min_coord
[
0
])
/
2
,
(
max_coord
[
1
]
-
min_coord
[
1
])
/
2
,
(
max_coord
[
2
]
-
min_coord
[
2
])
/
2
)
;
extent
<<
(
max_coord
[
0
]
-
min_coord
[
0
])
/
2
,
(
max_coord
[
1
]
-
min_coord
[
1
])
/
2
,
(
max_coord
[
2
]
-
min_coord
[
2
])
/
2
;
}
...
...
@@ -589,9 +589,9 @@ static inline void getExtentAndCenter_mesh(Vec3f* ps, Vec3f* ps2, Triangle* ts,
center
=
axis
[
0
]
*
o
[
0
]
+
axis
[
1
]
*
o
[
1
]
+
axis
[
2
]
*
o
[
2
];
extent
.
setValue
(
(
max_coord
[
0
]
-
min_coord
[
0
])
/
2
,
(
max_coord
[
1
]
-
min_coord
[
1
])
/
2
,
(
max_coord
[
2
]
-
min_coord
[
2
])
/
2
)
;
extent
<<
(
max_coord
[
0
]
-
min_coord
[
0
])
/
2
,
(
max_coord
[
1
]
-
min_coord
[
1
])
/
2
,
(
max_coord
[
2
]
-
min_coord
[
2
])
/
2
;
}
...
...
src/BVH/BV_fitter.cpp
View file @
fdd9b54d
...
...
@@ -58,11 +58,11 @@ static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::U eigenS[3], Vec3f a
else
if
(
eigenS
[
2
]
>
eigenS
[
max
])
{
mid
=
max
;
max
=
2
;
}
else
{
mid
=
2
;
}
axis
[
0
]
.
setValue
(
eigenV
[
0
][
max
],
eigenV
[
1
][
max
],
eigenV
[
2
][
max
]
)
;
axis
[
1
]
.
setValue
(
eigenV
[
0
][
mid
],
eigenV
[
1
][
mid
],
eigenV
[
2
][
mid
]
)
;
axis
[
2
]
.
setValue
(
eigenV
[
1
][
max
]
*
eigenV
[
2
][
mid
]
-
eigenV
[
1
][
mid
]
*
eigenV
[
2
][
max
],
eigenV
[
0
][
mid
]
*
eigenV
[
2
][
max
]
-
eigenV
[
0
][
max
]
*
eigenV
[
2
][
mid
],
eigenV
[
0
][
max
]
*
eigenV
[
1
][
mid
]
-
eigenV
[
0
][
mid
]
*
eigenV
[
1
][
max
]
)
;
axis
[
0
]
<<
eigenV
[
0
][
max
],
eigenV
[
1
][
max
],
eigenV
[
2
][
max
];
axis
[
1
]
<<
eigenV
[
0
][
mid
],
eigenV
[
1
][
mid
],
eigenV
[
2
][
mid
];
axis
[
2
]
<<
eigenV
[
1
][
max
]
*
eigenV
[
2
][
mid
]
-
eigenV
[
1
][
mid
]
*
eigenV
[
2
][
max
],
eigenV
[
0
][
mid
]
*
eigenV
[
2
][
max
]
-
eigenV
[
0
][
max
]
*
eigenV
[
2
][
mid
],
eigenV
[
0
][
max
]
*
eigenV
[
1
][
mid
]
-
eigenV
[
0
][
mid
]
*
eigenV
[
1
][
max
];
}
namespace
OBB_fit_functions
...
...
@@ -71,10 +71,10 @@ namespace OBB_fit_functions
void
fit1
(
Vec3f
*
ps
,
OBB
&
bv
)
{
bv
.
To
=
ps
[
0
];
bv
.
axis
[
0
]
.
setValue
(
1
,
0
,
0
)
;
bv
.
axis
[
1
]
.
setValue
(
0
,
1
,
0
)
;
bv
.
axis
[
2
]
.
setValue
(
0
,
0
,
1
)
;
bv
.
extent
.
set
Value
(
0
);
bv
.
axis
[
0
]
<<
1
,
0
,
0
;
bv
.
axis
[
1
]
<<
0
,
1
,
0
;
bv
.
axis
[
2
]
<<
0
,
0
,
1
;
bv
.
extent
.
set
Zero
(
);
}
void
fit2
(
Vec3f
*
ps
,
OBB
&
bv
)
...
...
@@ -88,10 +88,10 @@ void fit2(Vec3f* ps, OBB& bv)
bv
.
axis
[
0
]
=
p1p2
;
generateCoordinateSystem
(
bv
.
axis
[
0
],
bv
.
axis
[
1
],
bv
.
axis
[
2
]);
bv
.
extent
.
setValue
(
len_p1p2
*
0.5
,
0
,
0
)
;
bv
.
To
.
setValue
(
0.5
*
(
p1
[
0
]
+
p2
[
0
]),
0.5
*
(
p1
[
1
]
+
p2
[
1
]),
0.5
*
(
p1
[
2
]
+
p2
[
2
])
)
;
bv
.
extent
<<
len_p1p2
*
0.5
,
0
,
0
;
bv
.
To
<<
0.5
*
(
p1
[
0
]
+
p2
[
0
]),
0.5
*
(
p1
[
1
]
+
p2
[
1
]),
0.5
*
(
p1
[
2
]
+
p2
[
2
]);
}
void
fit3
(
Vec3f
*
ps
,
OBB
&
bv
)
...
...
@@ -156,9 +156,9 @@ namespace RSS_fit_functions
void
fit1
(
Vec3f
*
ps
,
RSS
&
bv
)
{
bv
.
Tr
=
ps
[
0
];
bv
.
axis
[
0
]
.
setValue
(
1
,
0
,
0
)
;
bv
.
axis
[
1
]
.
setValue
(
0
,
1
,
0
)
;
bv
.
axis
[
2
]
.
setValue
(
0
,
0
,
1
)
;
bv
.
axis
[
0
]
<<
1
,
0
,
0
;
bv
.
axis
[
1
]
<<
0
,
1
,
0
;
bv
.
axis
[
2
]
<<
0
,
0
,
1
;
bv
.
l
[
0
]
=
0
;
bv
.
l
[
1
]
=
0
;
bv
.
r
=
0
;
...
...
@@ -245,10 +245,10 @@ void fit1(Vec3f* ps, kIOS& bv)
bv
.
spheres
[
0
].
o
=
ps
[
0
];
bv
.
spheres
[
0
].
r
=
0
;
bv
.
obb
.
axis
[
0
]
.
setValue
(
1
,
0
,
0
)
;
bv
.
obb
.
axis
[
1
]
.
setValue
(
0
,
1
,
0
)
;
bv
.
obb
.
axis
[
2
]
.
setValue
(
0
,
0
,
1
)
;
bv
.
obb
.
extent
.
set
Value
(
0
);
bv
.
obb
.
axis
[
0
]
<<
1
,
0
,
0
;
bv
.
obb
.
axis
[
1
]
<<
0
,
1
,
0
;
bv
.
obb
.
axis
[
2
]
<<
0
,
0
,
1
;
bv
.
obb
.
extent
.
set
Zero
(
);
bv
.
obb
.
To
=
ps
[
0
];
}
...
...
@@ -267,7 +267,7 @@ void fit2(Vec3f* ps, kIOS& bv)
generateCoordinateSystem
(
axis
[
0
],
axis
[
1
],
axis
[
2
]);
FCL_REAL
r0
=
len_p1p2
*
0.5
;
bv
.
obb
.
extent
.
setValue
(
r0
,
0
,
0
)
;
bv
.
obb
.
extent
<<
r0
,
0
,
0
;
bv
.
obb
.
To
=
(
p1
+
p2
)
*
0.5
;
bv
.
spheres
[
0
].
o
=
bv
.
obb
.
To
;
...
...
src/ccd/motion.cpp
View file @
fdd9b54d
...
...
@@ -441,7 +441,7 @@ FCL_REAL TriangleMotionBoundVisitor::visit(const InterpMotion& motion) const
InterpMotion
::
InterpMotion
()
:
MotionBase
()
{
// Default angular velocity is zero
angular_axis
.
setValue
(
1
,
0
,
0
)
;
angular_axis
<<
1
,
0
,
0
;
angular_vel
=
0
;
// Default reference point is local zero point
...
...
src/intersect.cpp
View file @
fdd9b54d
...
...
@@ -1111,11 +1111,10 @@ FCL_REAL Intersect::distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v)
bool
Intersect
::
buildTrianglePlane
(
const
Vec3f
&
v1
,
const
Vec3f
&
v2
,
const
Vec3f
&
v3
,
Vec3f
*
n
,
FCL_REAL
*
t
)
{
Vec3f
n_
=
(
v2
-
v1
).
cross
(
v3
-
v1
);
bool
can_normalize
=
false
;
n_
.
normalize
(
&
can_normalize
);
if
(
can_normalize
)
FCL_REAL
norm2
=
n_
.
squaredNorm
();
if
(
n
>
0
)
{
*
n
=
n_
;
*
n
=
n_
/
sqrt
(
norm2
)
;
*
t
=
n_
.
dot
(
v1
);
return
true
;
}
...
...
@@ -1126,11 +1125,10 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f
bool
Intersect
::
buildEdgePlane
(
const
Vec3f
&
v1
,
const
Vec3f
&
v2
,
const
Vec3f
&
tn
,
Vec3f
*
n
,
FCL_REAL
*
t
)
{
Vec3f
n_
=
(
v2
-
v1
).
cross
(
tn
);
bool
can_normalize
=
false
;
n_
.
normalize
(
&
can_normalize
);
if
(
can_normalize
)
FCL_REAL
norm2
=
n_
.
squaredNorm
();
if
(
n
>
0
)
{
*
n
=
n_
;
*
n
=
n_
/
sqrt
(
norm2
)
;
*
t
=
n_
.
dot
(
v1
);
return
true
;
}
...
...
src/math/transform.cpp
View file @
fdd9b54d
...
...
@@ -103,9 +103,9 @@ void Quaternion3f::toRotation(Matrix3f& R) const
FCL_REAL
twoYZ
=
twoZ
*
data
[
2
];
FCL_REAL
twoZZ
=
twoZ
*
data
[
3
];
R
.
setValue
(
1.0
-
(
twoYY
+
twoZZ
),
twoXY
-
twoWZ
,
twoXZ
+
twoWY
,
twoXY
+
twoWZ
,
1.0
-
(
twoXX
+
twoZZ
),
twoYZ
-
twoWX
,
twoXZ
-
twoWY
,
twoYZ
+
twoWX
,
1.0
-
(
twoXX
+
twoYY
)
)
;
R
<<
1.0
-
(
twoYY
+
twoZZ
),
twoXY
-
twoWZ
,
twoXZ
+
twoWY
,
twoXY
+
twoWZ
,
1.0
-
(
twoXX
+
twoZZ
),
twoYZ
-
twoWX
,
twoXZ
-
twoWY
,
twoYZ
+
twoWX
,
1.0
-
(
twoXX
+
twoYY
);
}
...
...
@@ -169,9 +169,9 @@ void Quaternion3f::toAxes(Vec3f axis[3]) const
FCL_REAL
twoYZ
=
twoZ
*
data
[
2
];
FCL_REAL
twoZZ
=
twoZ
*
data
[
3
];
axis
[
0
]
.
setValue
(
1.0
-
(
twoYY
+
twoZZ
),
twoXY
+
twoWZ
,
twoXZ
-
twoWY
)
;
axis
[
1
]
.
setValue
(
twoXY
-
twoWZ
,
1.0
-
(
twoXX
+
twoZZ
),
twoYZ
+
twoWX
)
;
axis
[
2
]
.
setValue
(
twoXZ
+
twoWY
,
twoYZ
-
twoWX
,
1.0
-
(
twoXX
+
twoYY
)
)
;
axis
[
0
]
<<
1.0
-
(
twoYY
+
twoZZ
),
twoXY
+
twoWZ
,
twoXZ
-
twoWY
;
axis
[
1
]
<<
twoXY
-
twoWZ
,
1.0
-
(
twoXX
+
twoZZ
),
twoYZ
+
twoWX
;
axis
[
2
]
<<
twoXZ
+
twoWY
,
twoYZ
-
twoWX
,
1.0
-
(
twoXX
+
twoYY
);
}
...
...
src/narrowphase/gjk_libccd.cpp
View file @
fdd9b54d
...
...
@@ -780,9 +780,9 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
res
=
ccdMPRPenetration
(
obj1
,
obj2
,
&
ccd
,
&
depth
,
&
dir
,
&
pos
);
if
(
res
==
0
)
{
contact_points
->
setValue
(
ccdVec3X
(
&
pos
),
ccdVec3Y
(
&
pos
),
ccdVec3Z
(
&
pos
)
)
;
*
contact_points
<<
ccdVec3X
(
&
pos
),
ccdVec3Y
(
&
pos
),
ccdVec3Z
(
&
pos
);
*
penetration_depth
=
depth
;
normal
->
setValue
(
ccdVec3X
(
&
dir
),
ccdVec3Y
(
&
dir
),
ccdVec3Z
(
&
dir
)
)
;
*
normal
<<
ccdVec3X
(
&
dir
),
ccdVec3Y
(
&
dir
),
ccdVec3Z
(
&
dir
);
return
true
;
}
...
...
@@ -808,8 +808,8 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1,
ccd_vec3_t
p1_
,
p2_
;
dist
=
libccd_extension
::
ccdGJKDist2
(
obj1
,
obj2
,
&
ccd
,
&
p1_
,
&
p2_
);
if
(
p1
)
p1
->
setValue
(
ccdVec3X
(
&
p1_
),
ccdVec3Y
(
&
p1_
),
ccdVec3Z
(
&
p1_
)
)
;
if
(
p2
)
p2
->
setValue
(
ccdVec3X
(
&
p2_
),
ccdVec3Y
(
&
p2_
),
ccdVec3Z
(
&
p2_
)
)
;
if
(
p1
)
*
p1
<<
ccdVec3X
(
&
p1_
),
ccdVec3Y
(
&
p1_
),
ccdVec3Z
(
&
p1_
);
if
(
p2
)
*
p2
<<
ccdVec3X
(
&
p2_
),
ccdVec3Y
(
&
p2_
),
ccdVec3Z
(
&
p2_
);
if
(
res
)
*
res
=
dist
;
if
(
dist
<
0
)
return
false
;
else
return
true
;
...
...
src/shape/geometric_shapes.cpp
View file @
fdd9b54d
...
...
@@ -109,7 +109,7 @@ void Halfspace::unitNormalTest()
}
else
{
n
.
setValue
(
1
,
0
,
0
)
;
n
<<
1
,
0
,
0
;
d
=
0
;
}
}
...
...
@@ -125,7 +125,7 @@ void Plane::unitNormalTest()
}
else
{
n
.
setValue
(
1
,
0
,
0
)
;
n
<<
1
,
0
,
0
;
d
=
0
;
}
}
...
...
src/shape/geometric_shapes_utility.cpp
View file @
fdd9b54d
...
...
@@ -424,10 +424,10 @@ void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
const
Vec3f
&
T
=
tf
.
getTranslation
();
bv
.
To
=
T
;
bv
.
axis
[
0
]
.
setValue
(
1
,
0
,
0
)
;
bv
.
axis
[
1
]
.
setValue
(
0
,
1
,
0
)
;
bv
.
axis
[
2
]
.
setValue
(
0
,
0
,
1
)
;
bv
.
extent
.
set
Value
(
s
.
radius
);
bv
.
axis
[
0
]
<<
1
,
0
,
0
;
bv
.
axis
[
1
]
<<
0
,
1
,
0
;
bv
.
axis
[
2
]
<<
0
,
0
,
1
;
bv
.
extent
.
set
Constant
(
s
.
radius
);
}
template
<
>
...
...
@@ -440,7 +440,7 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
bv
.
axis
[
0
]
=
R
.
col
(
0
);
bv
.
axis
[
1
]
=
R
.
col
(
1
);
bv
.
axis
[
2
]
=
R
.
col
(
2
);
bv
.
extent
.
setValue
(
s
.
radius
,
s
.
radius
,
s
.
lz
/
2
+
s
.
radius
)
;
bv
.
extent
<<
s
.
radius
,
s
.
radius
,
s
.
lz
/
2
+
s
.
radius
;
}
template
<
>
...
...
@@ -453,7 +453,7 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
bv
.
axis
[
0
]
=
R
.
col
(
0
);
bv
.
axis
[
1
]
=
R
.
col
(
1
);
bv
.
axis
[
2
]
=
R
.
col
(
2
);
bv
.
extent
.
setValue
(
s
.
radius
,
s
.
radius
,
s
.
lz
/
2
)
;