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
c845c9a6
Commit
c845c9a6
authored
Sep 23, 2012
by
Jia Pan
Browse files
add the interface for continuous collision
parent
0f0a2d18
Changes
19
Hide whitespace changes
Inline
Side-by-side
include/fcl/broadphase/broadphase.h
View file @
c845c9a6
...
...
@@ -146,6 +146,86 @@ protected:
};
/// @brief Callback for continuous collision between two objects. Return value is whether can stop now.
typedef
bool
(
*
ContinuousCollisionCallBack
)(
ContinuousCollisionObject
*
o1
,
ContinuousCollisionObject
*
o2
,
void
*
cdata
);
/// @brief Callback for continuous distance between two objects, Return value is whether can stop now, also return the minimum distance till now.
typedef
bool
(
*
ContinuousDistanceCallBack
)(
ContinuousCollisionObject
*
o1
,
ContinuousCollisionObject
*
o2
,
void
*
cdata
,
FCL_REAL
&
dist
);
/// @brief Base class for broad phase continuous collision. It helps to accelerate the continuous collision/distance between N objects. Also support self collision, self distance and collision/distance with another M objects.
class
BroadPhaseContinuousCollisionManager
{
public:
BroadPhaseContinuousCollisionManager
()
{
}
virtual
~
BroadPhaseContinuousCollisionManager
()
{}
/// @brief add objects to the manager
virtual
void
registerObjects
(
const
std
::
vector
<
ContinuousCollisionObject
*>&
other_objs
)
{
for
(
size_t
i
=
0
;
i
<
other_objs
.
size
();
++
i
)
registerObject
(
other_objs
[
i
]);
}
/// @brief add one object to the manager
virtual
void
registerObject
(
ContinuousCollisionObject
*
obj
)
=
0
;
/// @brief remove one object from the manager
virtual
void
unregisterObject
(
ContinuousCollisionObject
*
obj
)
=
0
;
/// @brief initialize the manager, related with the specific type of manager
virtual
void
setup
()
=
0
;
/// @brief update the condition of manager
virtual
void
update
()
=
0
;
/// @brief update the manager by explicitly given the object updated
virtual
void
update
(
ContinuousCollisionObject
*
updated_obj
)
{
update
();
}
/// @brief update the manager by explicitly given the set of objects update
virtual
void
update
(
const
std
::
vector
<
ContinuousCollisionObject
*>&
updated_objs
)
{
update
();
}
/// @brief clear the manager
virtual
void
clear
()
=
0
;
/// @brief return the objects managed by the manager
virtual
void
getObjects
(
std
::
vector
<
ContinuousCollisionObject
*>&
objs
)
const
=
0
;
/// @brief perform collision test between one object and all the objects belonging to the manager
virtual
void
collide
(
ContinuousCollisionObject
*
obj
,
void
*
cdata
,
CollisionCallBack
callback
)
const
=
0
;
/// @brief perform distance computation between one object and all the objects belonging to the manager
virtual
void
distance
(
ContinuousCollisionObject
*
obj
,
void
*
cdata
,
DistanceCallBack
callback
)
const
=
0
;
/// @brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision)
virtual
void
collide
(
void
*
cdata
,
CollisionCallBack
callback
)
const
=
0
;
/// @brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance)
virtual
void
distance
(
void
*
cdata
,
DistanceCallBack
callback
)
const
=
0
;
/// @brief perform collision test with objects belonging to another manager
virtual
void
collide
(
BroadPhaseContinuousCollisionManager
*
other_manager
,
void
*
cdata
,
CollisionCallBack
callback
)
const
=
0
;
/// @brief perform distance test with objects belonging to another manager
virtual
void
distance
(
BroadPhaseContinuousCollisionManager
*
other_manager
,
void
*
cdata
,
DistanceCallBack
callback
)
const
=
0
;
/// @brief whether the manager is empty
virtual
bool
empty
()
const
=
0
;
/// @brief the number of objects managed by the manager
virtual
size_t
size
()
const
=
0
;
};
}
...
...
include/fcl/ccd/conservative_advancement.h
View file @
c845c9a6
...
...
@@ -48,9 +48,9 @@ namespace fcl
template
<
typename
BV
,
typename
ConservativeAdvancementNode
,
typename
CollisionNode
>
int
conservativeAdvancement
(
const
CollisionGeometry
*
o1
,
MotionBase
*
motion1
,
const
MotionBase
*
motion1
,
const
CollisionGeometry
*
o2
,
MotionBase
*
motion2
,
const
MotionBase
*
motion2
,
const
CollisionRequest
&
request
,
CollisionResult
&
result
,
FCL_REAL
&
toc
);
...
...
include/fcl/ccd/motion.h
View file @
c845c9a6
...
...
@@ -56,7 +56,7 @@ public:
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
bool
integrate
(
double
dt
);
bool
integrate
(
double
dt
)
const
;
/// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
FCL_REAL
computeMotionBound
(
const
BVMotionBoundVisitor
&
mb_visitor
)
const
...
...
@@ -92,6 +92,84 @@ public:
tf_
=
tf
;
}
void
getTaylorModel
(
TMatrix3
&
tm
,
TVector3
&
tv
)
const
{
// set tv
Vec3f
c
[
4
];
c
[
0
]
=
(
Td
[
0
]
+
Td
[
1
]
*
4
+
Td
[
2
]
+
Td
[
3
])
*
(
1
/
6.0
);
c
[
1
]
=
(
-
Td
[
0
]
+
Td
[
2
])
*
(
1
/
2.0
);
c
[
2
]
=
(
Td
[
0
]
-
Td
[
1
]
*
2
+
Td
[
2
])
*
(
1
/
2.0
);
c
[
3
]
=
(
-
Td
[
0
]
+
Td
[
1
]
*
3
-
Td
[
2
]
*
3
+
Td
[
3
])
*
(
1
/
6.0
);
tv
.
setTimeInterval
(
getTimeInterval
());
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
{
for
(
std
::
size_t
j
=
0
;
j
<
4
;
++
j
)
{
tv
[
i
].
coeff
(
j
)
=
c
[
j
][
i
];
}
}
// set tm
Matrix3f
I
(
1
,
0
,
0
,
0
,
1
,
0
,
0
,
0
,
1
);
// 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
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
;
FCL_REAL
theta0
=
Rt0_len
;
FCL_REAL
costheta0
=
cos
(
theta0
);
FCL_REAL
sintheta0
=
sin
(
theta0
);
Vec3f
Wt0
=
Rt0
*
inv_Rt0_len
;
Matrix3f
hatWt0
;
hat
(
hatWt0
,
Wt0
);
Matrix3f
hatWt0_sqr
=
hatWt0
*
hatWt0
;
Matrix3f
Mt0
=
I
+
hatWt0
*
sintheta0
+
hatWt0_sqr
*
(
1
-
costheta0
);
/// 2. compute M'(1/2)
Vec3f
dRt0
=
(
-
Rd
[
0
]
-
Rd
[
1
]
*
5
+
Rd
[
2
]
*
5
+
Rd
[
3
])
*
(
1
/
8.0
);
FCL_REAL
Rt0_dot_dRt0
=
Rt0
.
dot
(
dRt0
);
FCL_REAL
dtheta0
=
Rt0_dot_dRt0
*
inv_Rt0_len
;
Vec3f
dWt0
=
dRt0
*
inv_Rt0_len
-
Rt0
*
(
Rt0_dot_dRt0
*
inv_Rt0_len_3
);
Matrix3f
hatdWt0
;
hat
(
hatdWt0
,
dWt0
);
Matrix3f
dMt0
=
hatdWt0
*
sintheta0
+
hatWt0
*
(
costheta0
*
dtheta0
)
+
hatWt0_sqr
*
(
sintheta0
*
dtheta0
)
+
(
hatWt0
*
hatdWt0
+
hatdWt0
*
hatWt0
)
*
(
1
-
costheta0
);
/// 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
.
sqrLength
();
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
;
hat
(
hatddWt0
,
ddWt0
);
Matrix3f
ddMt0
=
hatddWt0
*
sintheta0
+
hatWt0
*
(
costheta0
*
dtheta0
-
sintheta0
*
dtheta0
*
dtheta0
+
costheta0
*
ddtheta0
)
+
hatdWt0
*
(
costheta0
*
dtheta0
)
+
(
hatWt0
*
hatdWt0
+
hatdWt0
*
hatWt0
)
*
(
sintheta0
*
dtheta0
*
2
)
+
hatdWt0
*
hatdWt0
*
(
2
*
(
1
-
costheta0
))
+
hatWt0
*
hatWt0
*
(
costheta0
*
dtheta0
*
dtheta0
+
sintheta0
*
ddtheta0
)
+
(
hatWt0
*
hatddWt0
+
hatddWt0
+
hatWt0
)
*
(
1
-
costheta0
);
tm
.
setTimeInterval
(
getTimeInterval
());
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
{
for
(
std
::
size_t
j
=
0
;
j
<
3
;
++
j
)
{
tm
(
i
,
j
).
coeff
(
0
)
=
Mt0
(
i
,
j
)
-
dMt0
(
i
,
j
)
*
0.5
+
ddMt0
(
i
,
j
)
*
0.25
*
0.5
;
tm
(
i
,
j
).
coeff
(
1
)
=
dMt0
(
i
,
j
)
-
ddMt0
(
i
,
j
)
*
0.5
;
tm
(
i
,
j
).
coeff
(
2
)
=
ddMt0
(
i
,
j
)
*
0.5
;
tm
(
i
,
j
).
coeff
(
3
)
=
0
;
tm
(
i
,
j
).
remainder
()
=
Interval
(
-
1
/
48.0
,
1
/
48.0
);
/// not correct, should fix
}
}
}
protected:
void
computeSplineParameter
()
{
...
...
@@ -110,10 +188,10 @@ protected:
FCL_REAL
Rd0Rd0
,
Rd0Rd1
,
Rd0Rd2
,
Rd0Rd3
,
Rd1Rd1
,
Rd1Rd2
,
Rd1Rd3
,
Rd2Rd2
,
Rd2Rd3
,
Rd3Rd3
;
//// @brief The transformation at current time t
Transform3f
tf
;
mutable
Transform3f
tf
;
/// @brief The time related with tf
FCL_REAL
tf_t
;
mutable
FCL_REAL
tf_t
;
public:
FCL_REAL
computeTBound
(
const
Vec3f
&
n
)
const
;
...
...
@@ -163,12 +241,12 @@ public:
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
bool
integrate
(
double
dt
)
bool
integrate
(
double
dt
)
const
{
if
(
dt
>
1
)
dt
=
1
;
tf
.
setQuatRotation
(
absoluteRotation
(
dt
));
Quaternion3f
delta_rot
=
deltaRotation
(
dt
);
tf
.
setTranslation
(
p
+
axis
*
(
dt
*
linear_vel
)
+
delta_rot
.
transform
(
tf1
.
getTranslation
()
-
p
));
...
...
@@ -210,6 +288,29 @@ public:
tf_
=
tf
;
}
void
getTaylorModel
(
TMatrix3
&
tm
,
TVector3
&
tv
)
const
{
Matrix3f
hat_axis
;
hat
(
hat_axis
,
axis
);
TaylorModel
cos_model
(
getTimeInterval
());
generateTaylorModelForCosFunc
(
cos_model
,
angular_vel
,
0
);
TaylorModel
sin_model
(
getTimeInterval
());
generateTaylorModelForSinFunc
(
sin_model
,
angular_vel
,
0
);
TMatrix3
delta_R
=
hat_axis
*
sin_model
-
hat_axis
*
hat_axis
*
(
cos_model
-
1
)
+
Matrix3f
(
1
,
0
,
0
,
0
,
1
,
0
,
0
,
0
,
1
);
TaylorModel
a
(
getTimeInterval
()),
b
(
getTimeInterval
()),
c
(
getTimeInterval
());
generateTaylorModelForLinearFunc
(
a
,
0
,
linear_vel
*
axis
[
0
]);
generateTaylorModelForLinearFunc
(
b
,
0
,
linear_vel
*
axis
[
1
]);
generateTaylorModelForLinearFunc
(
c
,
0
,
linear_vel
*
axis
[
2
]);
TVector3
delta_T
=
p
-
delta_R
*
p
+
TVector3
(
a
,
b
,
c
);
tm
=
delta_R
*
tf1
.
getRotation
();
tv
=
delta_R
*
tf1
.
getTranslation
()
+
delta_T
;
}
protected:
void
computeScrewParameter
()
{
...
...
@@ -256,7 +357,7 @@ protected:
Transform3f
tf2
;
/// @brief The transformation at current time t
Transform3f
tf
;
mutable
Transform3f
tf
;
/// @brief screw axis
Vec3f
axis
;
...
...
@@ -322,7 +423,7 @@ public:
/// @brief Integrate the motion from 0 to dt
/// We compute the current transformation from zero point instead of from last integrate time, for precision.
bool
integrate
(
double
dt
);
bool
integrate
(
double
dt
)
const
;
/// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor
FCL_REAL
computeMotionBound
(
const
BVMotionBoundVisitor
&
mb_visitor
)
const
...
...
@@ -358,6 +459,28 @@ public:
tf_
=
tf
;
}
void
getTaylorModel
(
TMatrix3
&
tm
,
TVector3
&
tv
)
const
{
Matrix3f
hat_angular_axis
;
hat
(
hat_angular_axis
,
angular_axis
);
TaylorModel
cos_model
(
getTimeInterval
());
generateTaylorModelForCosFunc
(
cos_model
,
angular_vel
,
0
);
TaylorModel
sin_model
(
getTimeInterval
());
generateTaylorModelForSinFunc
(
sin_model
,
angular_vel
,
0
);
TMatrix3
delta_R
=
hat_angular_axis
*
sin_model
-
hat_angular_axis
*
hat_angular_axis
*
(
cos_model
-
1
)
+
Matrix3f
(
1
,
0
,
0
,
0
,
1
,
0
,
0
,
0
,
1
);
TaylorModel
a
(
getTimeInterval
()),
b
(
getTimeInterval
()),
c
(
getTimeInterval
());
generateTaylorModelForLinearFunc
(
a
,
0
,
linear_vel
[
0
]);
generateTaylorModelForLinearFunc
(
b
,
0
,
linear_vel
[
1
]);
generateTaylorModelForLinearFunc
(
c
,
0
,
linear_vel
[
2
]);
TVector3
delta_T
(
a
,
b
,
c
);
tm
=
delta_R
*
tf1
.
getRotation
();
tv
=
tf1
.
transform
(
reference_p
)
+
delta_T
-
delta_R
*
tf1
.
getQuatRotation
().
transform
(
reference_p
);
}
protected:
void
computeVelocity
();
...
...
@@ -373,7 +496,7 @@ protected:
Transform3f
tf2
;
/// @brief The transformation at current time t
Transform3f
tf
;
mutable
Transform3f
tf
;
/// @brief Linear velocity
Vec3f
linear_vel
;
...
...
include/fcl/ccd/motion_base.h
View file @
c845c9a6
...
...
@@ -40,6 +40,8 @@
#include
"fcl/math/transform.h"
#include
"fcl/ccd/taylor_matrix.h"
#include
"fcl/ccd/taylor_vector.h"
#include
"fcl/BV/RSS.h"
namespace
fcl
...
...
@@ -108,10 +110,14 @@ protected:
class
MotionBase
{
public:
MotionBase
()
:
time_interval_
(
boost
::
shared_ptr
<
TimeInterval
>
(
new
TimeInterval
(
0
,
1
)))
{
}
virtual
~
MotionBase
()
{}
/** \brief Integrate the motion from 0 to dt */
virtual
bool
integrate
(
double
dt
)
=
0
;
virtual
bool
integrate
(
double
dt
)
const
=
0
;
/** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects */
virtual
FCL_REAL
computeMotionBound
(
const
BVMotionBoundVisitor
&
mb_visitor
)
const
=
0
;
...
...
@@ -127,6 +133,17 @@ public:
virtual
void
getCurrentTranslation
(
Vec3f
&
T
)
const
=
0
;
virtual
void
getCurrentTransform
(
Transform3f
&
tf
)
const
=
0
;
virtual
void
getTaylorModel
(
TMatrix3
&
tm
,
TVector3
&
tv
)
const
=
0
;
const
boost
::
shared_ptr
<
TimeInterval
>&
getTimeInterval
()
const
{
return
time_interval_
;
}
protected:
boost
::
shared_ptr
<
TimeInterval
>
time_interval_
;
};
...
...
include/fcl/ccd/taylor_matrix.h
View file @
c845c9a6
...
...
@@ -45,10 +45,11 @@
namespace
fcl
{
struct
TMatrix3
class
TMatrix3
{
TVector3
v_
[
3
];
public:
TMatrix3
();
TMatrix3
(
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
);
TMatrix3
(
TaylorModel
m
[
3
][
3
]);
...
...
@@ -75,23 +76,45 @@ struct TMatrix3
TMatrix3
operator
+
(
const
TMatrix3
&
m
)
const
;
TMatrix3
&
operator
+=
(
const
TMatrix3
&
m
);
TMatrix3
operator
+
(
const
Matrix3f
&
m
)
const
;
TMatrix3
&
operator
+=
(
const
Matrix3f
&
m
);
TMatrix3
operator
-
(
const
TMatrix3
&
m
)
const
;
TMatrix3
&
operator
-=
(
const
TMatrix3
&
m
);
TMatrix3
operator
-
(
const
Matrix3f
&
m
)
const
;
TMatrix3
&
operator
-=
(
const
Matrix3f
&
m
);
TMatrix3
operator
-
()
const
;
IMatrix3
getBound
()
const
;
IMatrix3
getBound
(
FCL_REAL
l
,
FCL_REAL
r
)
const
;
IMatrix3
getBound
(
FCL_REAL
t
)
const
;
IMatrix3
getTightBound
()
const
;
IMatrix3
getTightBound
(
FCL_REAL
l
,
FCL_REAL
r
)
const
;
void
print
()
const
;
void
setIdentity
();
void
setZero
();
FCL_REAL
diameter
()
const
;
void
setTimeInterval
(
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
);
void
setTimeInterval
(
FCL_REAL
l
,
FCL_REAL
r
);
const
boost
::
shared_ptr
<
TimeInterval
>&
getTimeInterval
()
const
;
TMatrix3
&
rotationConstrain
();
};
TMatrix3
rotationConstrain
(
const
TMatrix3
&
m
);
TMatrix3
operator
*
(
const
Matrix3f
&
m
,
const
TaylorModel
&
a
);
TMatrix3
operator
*
(
const
TaylorModel
&
a
,
const
Matrix3f
&
m
);
TMatrix3
operator
*
(
const
TaylorModel
&
a
,
const
TMatrix3
&
m
);
TMatrix3
operator
*
(
FCL_REAL
d
,
const
TMatrix3
&
m
);
TMatrix3
operator
+
(
const
Matrix3f
&
m1
,
const
TMatrix3
&
m2
);
TMatrix3
operator
-
(
const
Matrix3f
&
m1
,
const
TMatrix3
&
m2
);
}
#endif
include/fcl/ccd/taylor_model.h
View file @
c845c9a6
...
...
@@ -52,12 +52,28 @@ struct TimeInterval
Interval
t4_
;
// [t1, t2]^4
Interval
t5_
;
// [t1, t2]^5
Interval
t6_
;
// [t1, t2]^6
TimeInterval
()
{}
TimeInterval
(
FCL_REAL
l
,
FCL_REAL
r
)
{
setValue
(
l
,
r
);
}
void
setValue
(
FCL_REAL
l
,
FCL_REAL
r
)
{
t_
.
setValue
(
l
,
r
);
t2_
.
setValue
(
l
*
t_
[
0
],
r
*
t_
[
1
]);
t3_
.
setValue
(
l
*
t2_
[
0
],
r
*
t2_
[
1
]);
t4_
.
setValue
(
l
*
t3_
[
0
],
r
*
t3_
[
1
]);
t5_
.
setValue
(
l
*
t4_
[
0
],
r
*
t4_
[
1
]);
t6_
.
setValue
(
l
*
t5_
[
0
],
r
*
t5_
[
1
]);
}
};
/// @brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
/// over a time interval, with an interval remainder.
/// All the operations on two Taylor models assume their time intervals are the same.
struct
TaylorModel
class
TaylorModel
{
/// @brief time interval
boost
::
shared_ptr
<
TimeInterval
>
time_interval_
;
...
...
@@ -68,12 +84,29 @@ struct TaylorModel
/// @brief interval remainder
Interval
r_
;
void
setTimeInterval
(
FCL_REAL
l
,
FCL_REAL
r
);
void
setTimeInterval
(
const
boost
::
shared_ptr
<
TimeInterval
>
time_interval
)
public:
void
setTimeInterval
(
FCL_REAL
l
,
FCL_REAL
r
)
{
time_interval_
->
setValue
(
l
,
r
);
}
void
setTimeInterval
(
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
)
{
time_interval_
=
time_interval
;
}
const
boost
::
shared_ptr
<
TimeInterval
>&
getTimeInterval
()
const
{
return
time_interval_
;
}
FCL_REAL
coeff
(
std
::
size_t
i
)
const
{
return
coeffs_
[
i
];
}
FCL_REAL
&
coeff
(
std
::
size_t
i
)
{
return
coeffs_
[
i
];
}
const
Interval
&
remainder
()
const
{
return
r_
;
}
Interval
&
remainder
()
{
return
r_
;
}
TaylorModel
();
TaylorModel
(
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
);
TaylorModel
(
FCL_REAL
coeff
,
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
);
...
...
@@ -89,6 +122,9 @@ struct TaylorModel
TaylorModel
operator
+
(
FCL_REAL
d
)
const
;
TaylorModel
&
operator
+=
(
FCL_REAL
d
);
TaylorModel
operator
-
(
FCL_REAL
d
)
const
;
TaylorModel
&
operator
-=
(
FCL_REAL
d
);
TaylorModel
operator
*
(
const
TaylorModel
&
other
)
const
;
TaylorModel
operator
*
(
FCL_REAL
d
)
const
;
TaylorModel
&
operator
*=
(
const
TaylorModel
&
other
);
...
...
@@ -109,8 +145,17 @@ struct TaylorModel
void
setZero
();
};
TaylorModel
operator
*
(
FCL_REAL
d
,
const
TaylorModel
&
a
);
TaylorModel
operator
+
(
FCL_REAL
d
,
const
TaylorModel
&
a
);
TaylorModel
operator
-
(
FCL_REAL
d
,
const
TaylorModel
&
a
);
/// @brief Generate Taylor model for cos(w t + q0)
void
generateTaylorModelForCosFunc
(
TaylorModel
&
tm
,
FCL_REAL
w
,
FCL_REAL
q0
);
/// @brief Generate Taylor model for sin(w t + q0)
void
generateTaylorModelForSinFunc
(
TaylorModel
&
tm
,
FCL_REAL
w
,
FCL_REAL
q0
);
/// @brief Generate Taylor model for p + v t
void
generateTaylorModelForLinearFunc
(
TaylorModel
&
tm
,
FCL_REAL
p
,
FCL_REAL
v
);
}
...
...
include/fcl/ccd/taylor_vector.h
View file @
c845c9a6
...
...
@@ -43,25 +43,32 @@
namespace
fcl
{
struct
TVector3
class
TVector3
{
TaylorModel
i_
[
3
];
public:
TVector3
();
TVector3
(
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
);
TVector3
(
TaylorModel
v
[
3
]);
TVector3
(
const
TaylorModel
&
v0
,
const
TaylorModel
&
v1
,
const
TaylorModel
&
v2
);
TVector3
(
const
Vec3f
&
v
,
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
);
TVector3
operator
+
(
const
TVector3
&
other
)
const
;
TVector3
&
operator
+=
(
const
TVector3
&
other
);
TVector3
operator
+
(
FCL_REAL
d
)
const
;
TVector3
&
operator
+=
(
FCL_REAL
d
);
TVector3
operator
+
(
const
Vec3f
&
other
)
const
;
TVector3
&
operator
+=
(
const
Vec3f
&
other
);
TVector3
operator
-
(
const
TVector3
&
other
)
const
;
TVector3
&
operator
-=
(
const
TVector3
&
other
);
TVector3
operator
-
(
const
Vec3f
&
other
)
const
;
TVector3
&
operator
-=
(
const
Vec3f
&
other
);
TVector3
operator
-
()
const
;
TVector3
operator
*
(
const
TaylorModel
&
d
)
const
;
TVector3
&
operator
*=
(
const
TaylorModel
&
d
);
TVector3
operator
*
(
FCL_REAL
d
)
const
;
...
...
@@ -76,8 +83,12 @@ struct TVector3
TVector3
cross
(
const
Vec3f
&
other
)
const
;
IVector3
getBound
()
const
;
IVector3
getBound
(
FCL_REAL
l
,
FCL_REAL
r
)
const
;
IVector3
getBound
(
FCL_REAL
t
)
const
;
IVector3
getTightBound
()
const
;
IVector3
getTightBound
(
FCL_REAL
l
,
FCL_REAL
r
)
const
;
void
print
()
const
;
FCL_REAL
volumn
()
const
;
void
setZero
();
...
...
@@ -85,11 +96,18 @@ struct TVector3
TaylorModel
squareLength
()
const
;
void
setTimeInterval
(
const
boost
::
shared_ptr
<
TimeInterval
>&
time_interval
);
void
setTimeInterval
(
FCL_REAL
l
,
FCL_REAL
r
);
const
boost
::
shared_ptr
<
TimeInterval
>&
getTimeInterval
()
const
;
};
void
generateTVector3ForLinearFunc
(
TVector3
&
v
,
const
Vec3f
&
position
,
const
Vec3f
&
velocity
);
TVector3
operator
*
(
const
Vec3f
&
v
,
const
TaylorModel
&
a
);
TVector3
operator
+
(
const
Vec3f
&
v1
,
const
TVector3
&
v2
);
TVector3
operator
-
(
const
Vec3f
&
v1
,
const
TVector3
&
v2
);
}
#endif
include/fcl/collision.h
View file @
c845c9a6
...
...
@@ -71,6 +71,15 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const
CollisionGeometry
*
o2
,
const
Transform3f
&
tf2
,
const
CollisionRequest
&
request
,
CollisionResult
&
result
);
std
::
size_t
collide
(
const
ContinuousCollisionObject
*
o1
,
const
ContinuousCollisionObject
*
o2
,
const
CollisionRequest
&
request
,
CollisionResult
&
result
);
std
::
size_t
collide
(
const
CollisionGeometry
*
o1
,
const
MotionBase
*
motion1
,
const
CollisionGeometry
*
o2
,
const
MotionBase
*
motion2
,
const
CollisionRequest
&
request
,
CollisionResult
&
result
);
}
#endif
include/fcl/collision_object.h
View file @
c845c9a6
...
...
@@ -40,6 +40,7 @@
#include
"fcl/BV/AABB.h"
#include
"fcl/math/transform.h"
#include
"fcl/ccd/motion_base.h"
#include
<boost/shared_ptr.hpp>
namespace
fcl
...
...
@@ -314,6 +315,118 @@ protected:
void
*
user_data
;
};
/// @brief the object for continuous collision or distance computation, contains the geometry and the motion information
class
ContinuousCollisionObject
{
public:
ContinuousCollisionObject
(
const
boost
::
shared_ptr
<
CollisionGeometry
>&
cgeom_
)
:
cgeom
(
cgeom_
)
{
}