void AISTCollisionDetectorImpl::checkCollisionsOfAssignedPairs
(int pairIndexBegin, int pairIndexEnd, vector<ColdetModelPairEx*>& collidingModelPairs)
{
collidingModelPairs.clear();
for(int i=pairIndexBegin; i < pairIndexEnd; ++i){
ColdetModelPairEx* modelPair;
if(ENABLE_SHUFFLE){
modelPair = modelPairs[shuffledPairIndices[i]].get();
} else {
modelPair = modelPairs[i].get();
}
if(!modelPair->detectCollisions().empty()){
collidingModelPairs.push_back(modelPair);
}
}
}
void AISTCollisionDetectorImpl::extractCollisionsOfAssignedPairs
(int pairIndexBegin, int pairIndexEnd, CollisionPairArray& collisionPairs)
{
collisionPairs.clear();
for(int i=pairIndexBegin; i < pairIndexEnd; ++i){
ColdetModelPairEx* modelPair;
if(ENABLE_SHUFFLE){
modelPair = modelPairs[shuffledPairIndices[i]].get();
} else {
modelPair = modelPairs[i].get();
}
const std::vector<collision_data>& cdata = modelPair->detectCollisions();
if(!cdata.empty()){
collisionPairs.push_back(CollisionPair());
CollisionPair& collisionPair = collisionPairs.back();
collisionPair.geometryId[0] = modelPair->id1();
collisionPair.geometryId[1] = modelPair->id2();
vector<Collision>& collisions = collisionPair.collisions;
const int n = cdata.size();
collisions.reserve(n);
for(int j=0; j < n; ++j){
const collision_data& cd = cdata[j];
for(int k=0; k < cd.num_of_i_points; ++k){
if(cd.i_point_new[k]){
collisions.push_back(Collision());
Collision& collision = collisions.back();
collision.point = cd.i_points[k];
collision.normal = cd.n_vector;
collision.depth = cd.depth;
collision.id1 = cd.id1;
collision.id2 = cd.id2;
}
}
}
if(collisions.empty()){
collisionPairs.pop_back();
}
}
}
}
bool CollisionDetector_impl::detectCollisionsOfLinkPair
(ColdetModelPairEx& coldetPair, CollisionPointSequence& out_collisionPoints, const bool addCollisionPoints)
{
bool detected = false;
vector<collision_data>& cdata = coldetPair.detectCollisions();
int npoints = 0;
for(int i=0; i < cdata.size(); i++) {
for(int j=0; j < cdata[i].num_of_i_points; j++){
if(cdata[i].i_point_new[j]){
npoints ++;
}
}
}
if(npoints > 0){
detected = true;
if(addCollisionPoints){
out_collisionPoints.length(npoints);
int index = 0;
for(int i=0; i < cdata.size(); i++) {
collision_data& cd = cdata[i];
for(int j=0; j < cd.num_of_i_points; j++){
if (cd.i_point_new[j]){
CollisionPoint& point = out_collisionPoints[index];
for(int k=0; k < 3; k++){
point.position[k] = cd.i_points[j][k];
}
for(int k=0; k < 3; k++){
point.normal[k] = cd.n_vector[k];
}
point.idepth = cd.depth;
index++;
}
}
}
}
}
return detected;
}