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
67635545
Commit
67635545
authored
Jun 10, 2016
by
Joseph Mirabel
Committed by
Joseph Mirabel
Jun 14, 2016
Browse files
Update Quaternion3f API
parent
405a4b83
Changes
10
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/math/transform.h
View file @
67635545
...
...
@@ -58,14 +58,6 @@ public:
data
[
3
]
=
0
;
}
Quaternion3f
(
FCL_REAL
a
,
FCL_REAL
b
,
FCL_REAL
c
,
FCL_REAL
d
)
{
data
[
0
]
=
a
;
data
[
1
]
=
b
;
data
[
2
]
=
c
;
data
[
3
]
=
d
;
}
/// @brief Whether the rotation is identity
bool
isIdentity
()
const
{
...
...
@@ -127,20 +119,6 @@ public:
/// @brief rotate a vector
Vec3f
transform
(
const
Vec3f
&
v
)
const
;
inline
const
FCL_REAL
&
getW
()
const
{
return
data
[
0
];
}
inline
const
FCL_REAL
&
getX
()
const
{
return
data
[
1
];
}
inline
const
FCL_REAL
&
getY
()
const
{
return
data
[
2
];
}
inline
const
FCL_REAL
&
getZ
()
const
{
return
data
[
3
];
}
inline
FCL_REAL
&
getW
()
{
return
data
[
0
];
}
inline
FCL_REAL
&
getX
()
{
return
data
[
1
];
}
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
;
bool
operator
==
(
const
Quaternion3f
&
other
)
const
{
for
(
std
::
size_t
i
=
0
;
i
<
4
;
++
i
)
...
...
@@ -161,8 +139,26 @@ public:
return
data
[
i
];
}
inline
FCL_REAL
&
w
()
{
return
data
[
0
];
}
inline
FCL_REAL
&
x
()
{
return
data
[
1
];
}
inline
FCL_REAL
&
y
()
{
return
data
[
2
];
}
inline
FCL_REAL
&
z
()
{
return
data
[
3
];
}
inline
const
FCL_REAL
&
w
()
const
{
return
data
[
0
];
}
inline
const
FCL_REAL
&
x
()
const
{
return
data
[
1
];
}
inline
const
FCL_REAL
&
y
()
const
{
return
data
[
2
];
}
inline
const
FCL_REAL
&
z
()
const
{
return
data
[
3
];
}
private:
Quaternion3f
(
FCL_REAL
a
,
FCL_REAL
b
,
FCL_REAL
c
,
FCL_REAL
d
)
{
data
[
0
]
=
a
;
data
[
1
]
=
b
;
data
[
2
]
=
c
;
data
[
3
]
=
d
;
}
FCL_REAL
data
[
4
];
};
...
...
src/math/transform.cpp
View file @
67635545
...
...
@@ -316,8 +316,8 @@ Quaternion3f& Quaternion3f::inverse()
Vec3f
Quaternion3f
::
transform
(
const
Vec3f
&
v
)
const
{
Vec3f
u
(
getX
(),
getY
(),
getZ
());
double
s
=
getW
();
Vec3f
u
(
x
(),
y
(),
z
());
double
s
=
w
();
Vec3f
vprime
=
2
*
u
.
dot
(
v
)
*
u
+
(
s
*
s
-
u
.
dot
(
u
))
*
v
+
2
*
s
*
u
.
cross
(
v
);
return
vprime
;
}
...
...
@@ -366,49 +366,6 @@ void Quaternion3f::toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const
}
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_
));
...
...
src/narrowphase/gjk_libccd.cpp
View file @
67635545
...
...
@@ -509,7 +509,7 @@ static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
const
Quaternion3f
&
q
=
tf
.
getQuatRotation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
ccdVec3Set
(
&
o
->
pos
,
T
[
0
],
T
[
1
],
T
[
2
]);
ccdQuatSet
(
&
o
->
rot
,
q
.
getX
(),
q
.
getY
(),
q
.
getZ
(),
q
.
getW
());
ccdQuatSet
(
&
o
->
rot
,
q
.
x
(),
q
.
y
(),
q
.
z
(),
q
.
w
());
ccdQuatInvert2
(
&
o
->
rot_inv
,
&
o
->
rot
);
}
...
...
@@ -1016,7 +1016,7 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
const
Quaternion3f
&
q
=
tf
.
getQuatRotation
();
const
Vec3f
&
T
=
tf
.
getTranslation
();
ccdVec3Set
(
&
o
->
pos
,
T
[
0
],
T
[
1
],
T
[
2
]);
ccdQuatSet
(
&
o
->
rot
,
q
.
getX
(),
q
.
getY
(),
q
.
getZ
(),
q
.
getW
());
ccdQuatSet
(
&
o
->
rot
,
q
.
x
(),
q
.
y
(),
q
.
z
(),
q
.
w
());
ccdQuatInvert2
(
&
o
->
rot_inv
,
&
o
->
rot
);
return
o
;
...
...
test/CMakeLists.txt
View file @
67635545
...
...
@@ -32,11 +32,11 @@ add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp)
#add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test
(
test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp
)
add_fcl_test
(
test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp
)
add_fcl_test
(
test_fcl_box_box_distance test_fcl_box_box_distance.cpp
)
add_fcl_test
(
test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp
test_fcl_utility.cpp
)
add_fcl_test
(
test_fcl_box_box_distance test_fcl_box_box_distance.cpp
test_fcl_utility.cpp
)
add_fcl_test
(
test_fcl_simple test_fcl_simple.cpp
)
add_fcl_test
(
test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp
)
add_fcl_test
(
test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp
)
add_fcl_test
(
test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp
test_fcl_utility.cpp
)
add_fcl_test
(
test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp
test_fcl_utility.cpp
)
#add_fcl_test(test_fcl_obb test_fcl_obb.cpp)
add_fcl_test
(
test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp
)
...
...
test/test_fcl_box_box_distance.cpp
View file @
67635545
...
...
@@ -46,10 +46,11 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
"test_fcl_utility.h"
typedef
boost
::
shared_ptr
<
fcl
::
CollisionGeometry
>
CollisionGeometryPtr_t
;
using
fcl
::
Transform3f
;
using
fcl
::
Quaternion3f
;
using
fcl
::
Vec3f
;
using
fcl
::
CollisionObject
;
using
fcl
::
DistanceResult
;
...
...
@@ -104,7 +105,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2)
CollisionGeometryPtr_t
s2
(
new
fcl
::
Box
(
2
,
2
,
2
));
static
double
pi
=
M_PI
;
Transform3f
tf1
;
Transform3f
tf2
(
Quaternion3f
(
cos
(
pi
/
8
),
sin
(
pi
/
8
)
/
sqrt
(
3
),
Transform3f
tf2
(
fcl
::
makeQuat
(
cos
(
pi
/
8
),
sin
(
pi
/
8
)
/
sqrt
(
3
),
sin
(
pi
/
8
)
/
sqrt
(
3
),
sin
(
pi
/
8
)
/
sqrt
(
3
)),
Vec3f
(
0
,
0
,
10
));
...
...
@@ -144,9 +145,9 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
CollisionGeometryPtr_t
s1
(
new
fcl
::
Box
(
1
,
1
,
1
));
CollisionGeometryPtr_t
s2
(
new
fcl
::
Box
(
1
,
1
,
1
));
static
double
pi
=
M_PI
;
Transform3f
tf1
(
Quaternion3f
(
cos
(
pi
/
8
),
0
,
0
,
sin
(
pi
/
8
)),
Transform3f
tf1
(
fcl
::
makeQuat
(
cos
(
pi
/
8
),
0
,
0
,
sin
(
pi
/
8
)),
Vec3f
(
-
2
,
1
,
.5
));
Transform3f
tf2
(
Quaternion3f
(
cos
(
pi
/
8
),
0
,
sin
(
pi
/
8
),
0
),
Transform3f
tf2
(
fcl
::
makeQuat
(
cos
(
pi
/
8
),
0
,
sin
(
pi
/
8
),
0
),
Vec3f
(
2
,
.5
,
.5
));
CollisionObject
o1
(
s1
,
tf1
);
...
...
@@ -182,7 +183,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
BOOST_CHECK_CLOSE
(
p2
[
2
],
p2Ref
[
2
],
1e-4
);
// Apply the same global transform to both objects and recompute
Transform3f
tf3
(
Quaternion3f
(
0.435952844074
,
-
0.718287018243
,
Transform3f
tf3
(
fcl
::
makeQuat
(
0.435952844074
,
-
0.718287018243
,
0.310622451066
,
0.444435113443
),
Vec3f
(
4
,
5
,
6
));
tf1
=
tf3
*
tf1
;
...
...
test/test_fcl_capsule_box_1.cpp
View file @
67635545
...
...
@@ -46,6 +46,8 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
"test_fcl_utility.h"
BOOST_AUTO_TEST_CASE
(
distance_capsule_box
)
{
typedef
boost
::
shared_ptr
<
fcl
::
CollisionGeometry
>
CollisionGeometryPtr_t
;
...
...
@@ -96,7 +98,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
// Rotate capsule around y axis by pi/2 and move it behind box
tf1
.
setTranslation
(
fcl
::
Vec3f
(
-
10.
,
0.
,
0.
));
tf1
.
setQuatRotation
(
fcl
::
Quat
ernion3f
(
sqrt
(
2
)
/
2
,
0
,
sqrt
(
2
)
/
2
,
0
));
tf1
.
setQuatRotation
(
fcl
::
make
Quat
(
sqrt
(
2
)
/
2
,
0
,
sqrt
(
2
)
/
2
,
0
));
capsule
.
setTransform
(
tf1
);
// test distance
...
...
test/test_fcl_capsule_box_2.cpp
View file @
67635545
...
...
@@ -39,6 +39,8 @@
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include
<boost/test/included/unit_test.hpp>
#include
"test_fcl_utility.h"
#include
<cmath>
#include
<hpp/fcl/distance.h>
#include
<hpp/fcl/math/transform.h>
...
...
@@ -59,7 +61,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
fcl
::
DistanceResult
distanceResult
;
// Rotate capsule around y axis by pi/2 and move it behind box
fcl
::
Transform3f
tf1
(
fcl
::
Quat
ernion3f
(
sqrt
(
2
)
/
2
,
0
,
sqrt
(
2
)
/
2
,
0
),
fcl
::
Transform3f
tf1
(
fcl
::
make
Quat
(
sqrt
(
2
)
/
2
,
0
,
sqrt
(
2
)
/
2
,
0
),
fcl
::
Vec3f
(
-
10.
,
0.8
,
1.5
));
fcl
::
Transform3f
tf2
;
fcl
::
CollisionObject
capsule
(
capsuleGeometry
,
tf1
);
...
...
test/test_fcl_capsule_capsule.cpp
View file @
67635545
...
...
@@ -47,6 +47,8 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
"test_fcl_utility.h"
using
namespace
fcl
;
typedef
boost
::
shared_ptr
<
CollisionGeometry
>
CollisionGeometryPtr_t
;
...
...
@@ -139,7 +141,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
CollisionGeometryPtr_t
s2
(
new
Capsule
(
5
,
10
));
Transform3f
tf1
;
Transform3f
tf2
(
Quat
ernion3f
(
sqrt
(
2
)
/
2
,
0
,
sqrt
(
2
)
/
2
,
0
),
Transform3f
tf2
(
make
Quat
(
sqrt
(
2
)
/
2
,
0
,
sqrt
(
2
)
/
2
,
0
),
Vec3f
(
0
,
0
,
25.1
));
CollisionObject
o1
(
s1
,
tf1
);
...
...
test/test_fcl_utility.cpp
View file @
67635545
...
...
@@ -454,4 +454,14 @@ std::string getGJKSolverName(GJKSolverType solver_type)
return
std
::
string
(
"invalid"
);
}
Quaternion3f
makeQuat
(
FCL_REAL
w
,
FCL_REAL
x
,
FCL_REAL
y
,
FCL_REAL
z
)
{
Quaternion3f
q
;
q
.
w
()
=
w
;
q
.
x
()
=
x
;
q
.
y
()
=
y
;
q
.
z
()
=
z
;
return
q
;
}
}
test/test_fcl_utility.h
View file @
67635545
...
...
@@ -187,6 +187,8 @@ std::string getNodeTypeName(NODE_TYPE node_type);
std
::
string
getGJKSolverName
(
GJKSolverType
solver_type
);
Quaternion3f
makeQuat
(
FCL_REAL
w
,
FCL_REAL
x
,
FCL_REAL
y
,
FCL_REAL
z
);
}
#endif
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