AA_API void
aa_rx_cl_allow( struct aa_rx_cl *cl,
aa_rx_frame_id id0,
aa_rx_frame_id id1,
int allowed )
{
aa_rx_cl_set_set( cl->allowed, id0, id1, allowed );
}
AA_API void
aa_rx_mp_allow_collision( struct aa_rx_mp *mp,
aa_rx_frame_id id0, aa_rx_frame_id id1, int allowed )
{
amino::sgSpaceInformation::Ptr &si = mp->space_information;
amino::sgStateSpace *ss = si->getTypedStateSpace();
aa_rx_cl_set_set( ss->allowed, id0, id1, allowed );
mp->validity_checker->allow();
}
static bool
cl_check_callback( ::fcl::CollisionObject *o1,
::fcl::CollisionObject *o2,
void *data_ )
{
struct cl_check_data *data = (struct cl_check_data*)data_;
aa_rx_frame_id id1 = (intptr_t) o1->getUserData();
aa_rx_frame_id id2 = (intptr_t) o2->getUserData();
//const struct aa_rx_sg *sg = data->cl->sg;
//const char *name1 = aa_rx_sg_frame_name(sg, id1);
//const char *name2 = aa_rx_sg_frame_name(sg, id2);
/* Skip geometry in the same frame */
if( id1 == id2 ) return false;
/* Check if allowed collision */
if( aa_rx_cl_set_get(data->cl->allowed,id1,id2) ) {
return false;
}
fcl::CollisionRequest request;
fcl::CollisionResult result;
fcl::collide(o1, o2, request, result);
if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts)) {
//printf("collide: %s x %s\n", name1, name2 );
/* In Collision */
data->result = 1;
/* Short Circuit? */
if( data->cl_set ) {
aa_rx_cl_set_set( data->cl_set, id1, id2, 1 );
} else {
return true;
}
}
return false;
}
