//==============================================================================
bool DARTCollisionDetector::collide(
CollisionGroup* group1,
CollisionGroup* group2,
const CollisionOption& option, CollisionResult& result)
{
result.clear();
if ((this != group1->getCollisionDetector().get())
|| (this != group2->getCollisionDetector().get()))
{
dterr << "[DARTCollisionDetector::detect] Attempting to check collision "
<< "for a collision group that is created from a different collision "
<< "detector instance.\n";
return false;
}
auto casted1 = static_cast<DARTCollisionGroup*>(group1);
auto casted2 = static_cast<DARTCollisionGroup*>(group2);
const auto& objects1 = casted1->mCollisionObjects;
const auto& objects2 = casted2->mCollisionObjects;
if (objects1.empty() || objects2.empty())
return false;
auto done = false;
const auto& filter = option.collisionFilter;
for (auto i = 0u; i < objects1.size(); ++i)
{
auto collObj1 = objects1[i];
for (auto j = 0u; j < objects2.size(); ++j)
{
auto collObj2 = objects2[j];
if (filter && !filter->needCollision(collObj1, collObj2))
continue;
checkPair(collObj1, collObj2, option, result);
if (result.getNumContacts() >= option.maxNumContacts)
{
done = true;
break;
}
}
if (done)
break;
}
return result.isCollision();
}
static std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
if(request.enable_cost && request.use_approximate_cost)
{
CollisionRequest no_cost_request(request);
no_cost_request.enable_cost = false;
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, no_cost_request, result);
FCL_REAL sqrDistance;
fcl::collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
Box box;
Transform3f box_tf;
constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
box.cost_density = obj1->cost_density;
box.threshold_occupied = obj1->threshold_occupied;
box.threshold_free = obj1->threshold_free;
CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
ShapeShapeCollide<Box, T_SH>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
}
else
{
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, request, result);
FCL_REAL sqrDistance;
fcl::collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
}
return result.numContacts();
}
int collideSphereSphere(CollisionObject* o1, CollisionObject* o2, const double& _r0, const Eigen::Isometry3d& c0,
const double& _r1, const Eigen::Isometry3d& c1,
CollisionResult& result)
{
double r0 = _r0;
double r1 = _r1;
double rsum = r0 + r1;
Eigen::Vector3d normal = c0.translation() - c1.translation();
double normal_sqr = normal.squaredNorm();
if ( normal_sqr > rsum * rsum )
{
return 0;
}
r0 /= rsum;
r1 /= rsum;
Eigen::Vector3d point = r1 * c0.translation() + r0 * c1.translation();
double penetration;
if (normal_sqr < DART_COLLISION_EPS)
{
normal.setZero();
penetration = rsum;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
normal_sqr = sqrt(normal_sqr);
normal *= (1.0/normal_sqr);
penetration = rsum - normal_sqr;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
if (request.enable_distance_lower_bound) {
DistanceResult distanceResult;
DistanceRequest distanceRequest (request.enable_contact);
FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2, NarrowPhaseSolver>
(o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
if (distance <= 0) {
Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
contact.pos = .5*(p1+p2);
contact.normal = (p2-p1)/(p2-p1).length ();
result.addContact (contact);
return 1;
}
result.distance_lower_bound = distance;
return 0;
}
ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
if(request.enable_cached_gjk_guess)
{
nsolver->enableCachedGuess(true);
nsolver->setCachedGuess(request.cached_gjk_guess);
}
else
{
nsolver->enableCachedGuess(true);
}
initialize(node, *obj1, tf1, *obj2, tf2, nsolver, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
if(request.enable_cached_gjk_guess)
result.cached_gjk_guess = nsolver->getCachedGuess();
return result.numContacts();
}
std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OrientedMeshCollisionTraversalNode node (request.enable_distance_lower_bound);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
initialize(node, *obj1, tf1, *obj2, tf2, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
return result.numContacts();
}
int conservativeAdvancement(const CollisionGeometry* o1,
const MotionBase* motion1,
const CollisionGeometry* o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
typedef ConservativeAdvancement<BV, ConservativeAdvancementNode, CollisionNode>
ConservativeAdvancementType;
boost::shared_ptr<ConservativeAdvancementType> advancement =
boost::make_shared<ConservativeAdvancementType>(o1, motion1, o2, motion2);
ContinuousCollisionRequest continuous_request;
ContinuousCollisionResult continuous_result;
continuous_request.assign(request);
advancement->collide(continuous_request, continuous_result);
result = continuous_result;
toc = continuous_result.getTimeOfContact();
return static_cast<int>(result.numContacts() );
}
std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
return result.numContacts();
}
std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
if(request.enable_cost && request.use_approximate_cost)
{
CollisionRequest no_cost_request(request); // request remove cost to avoid the exact but expensive cost computation between mesh and octree
no_cost_request.enable_cost = false; // disable cost computation
MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, no_cost_request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
Box box;
Transform3f box_tf;
constructBox(obj1->getBV(0).bv, tf1, box, box_tf);
box.cost_density = obj1->cost_density;
box.threshold_occupied = obj1->threshold_occupied;
box.threshold_free = obj1->threshold_free;
CollisionRequest only_cost_request(result.numContacts(), false, request.num_max_cost_sources, true, false);
ShapeOcTreeCollide<Box, NarrowPhaseSolver>(&box, box_tf, o2, tf2, nsolver, only_cost_request, result);
}
else
{
MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
OcTreeSolver<NarrowPhaseSolver> otsolver(nsolver);
initialize(node, *obj1, tf1, *obj2, tf2, &otsolver, request, result);
FCL_REAL sqrDistance = 0;
collide(&node, sqrDistance);
}
return result.numContacts();
}
std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
if(request.isSatisfied(result)) return result.numContacts();
MeshCollisionTraversalNode<T_BVH> node (request.enable_distance_lower_bound);
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
Transform3f tf1_tmp = tf1;
BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
Transform3f tf2_tmp = tf2;
initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, request, result);
FCL_REAL sqrDistance;
fcl::collide(&node, sqrDistance);
result.distance_lower_bound = sqrt (sqrDistance);
delete obj1_tmp;
delete obj2_tmp;
return result.numContacts();
}
/**
* Perform explosion
* @param sqrRadius The squared radius that affect the bomb
*/
void Bomb::performExplosion(float sqrRadius, float power)
{
// perform an aabb query
sm::AABB query;
query.setSize(2.0f * sqrRadius, 2.0f * sqrRadius);
// set the actual position
ASSERT(mNode);
const Ogre::Vector3 &pos = mNode->getPosition();
query.setPosition(sm::Vector2(pos.x, pos.z));
static CollisionResult objects;
mCollMngr->getCollisionObjects(query, BOMB_AFFECTABLE_MASK, objects);
debugRED("Tenemos que poner el power real aca, estamos"
" usando directamente una fuerza lineal, tenemos que disminuir"
" el poder a medida que nos alejamos del centro de explosion\n");
// iterate over all the objects affected
const CollisionObject *co;
Hit_t hit;
hit.shooter = 0;
for(int i = objects.size()-1; i >= 0; --i){
co = objects[i];
if(checkObjectBetween(query.pos, co)) {
// not affect that object..
continue;
}
// else we affect the object with the corresponding power
GameObject *go = static_cast<GameObject *>(co->userDefined);
ASSERT(go);
hit.power = power;
hit.hitDir = query.pos - co->getPosition();
go->beenHit(hit);
}
}
int conservativeAdvancement(const CollisionGeometry* o1,
const MotionBase* motion1,
const CollisionGeometry* o2,
const MotionBase* motion2,
const CollisionRequest& request,
CollisionResult& result,
FCL_REAL& toc)
{
if(request.num_max_contacts == 0)
{
std::cerr << "Warning: should stop early as num_max_contact is " << request.num_max_contacts << " !" << std::endl;
return 0;
}
const OBJECT_TYPE object_type1 = o1->getObjectType();
const NODE_TYPE node_type1 = o1->getNodeType();
const OBJECT_TYPE object_type2 = o2->getObjectType();
const NODE_TYPE node_type2 = o2->getNodeType();
if(object_type1 != OT_BVH || object_type2 != OT_BVH)
return 0;
if(node_type1 != BV_RSS || node_type2 != BV_RSS)
return 0;
const BVHModel<BV>* model1 = static_cast<const BVHModel<BV>*>(o1);
const BVHModel<BV>* model2 = static_cast<const BVHModel<BV>*>(o2);
Transform3f tf1, tf2;
motion1->getCurrentTransform(tf1);
motion2->getCurrentTransform(tf2);
// whether the first start configuration is in collision
CollisionNode cnode;
if(!initialize(cnode, *model1, tf1, *model2, tf2, request, result))
std::cout << "initialize error" << std::endl;
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), cnode.R, cnode.T);
cnode.enable_statistics = false;
cnode.request = request;
collide(&cnode);
int b = result.numContacts();
if(b > 0)
{
toc = 0;
// std::cout << "zero collide" << std::endl;
return b;
}
ConservativeAdvancementNode node;
initialize(node, *model1, tf1, *model2, tf2);
node.motion1 = motion1;
node.motion2 = motion2;
do
{
Matrix3f R1_t, R2_t;
Vec3f T1_t, T2_t;
node.motion1->getCurrentTransform(R1_t, T1_t);
node.motion2->getCurrentTransform(R2_t, T2_t);
// compute the transformation from 1 to 2
relativeTransform(R1_t, T1_t, R2_t, T2_t, node.R, node.T);
node.delta_t = 1;
node.min_distance = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t <= node.t_err)
{
// std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
}
node.toc += node.delta_t;
if(node.toc > 1)
{
node.toc = 1;
break;
}
node.motion1->integrate(node.toc);
node.motion2->integrate(node.toc);
}
while(1);
toc = node.toc;
if(node.toc < 1)
return 1;
return 0;
}
int collideSphereBox(CollisionObject* o1, CollisionObject* o2,
const double& r0, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
CollisionResult& result)
{
Eigen::Vector3d size = 0.5 * size1;
bool inside_box = true;
// clipping a center of the sphere to a boundary of the box
Eigen::Vector3d c0 = T0.translation();
Eigen::Vector3d p = T1.inverse() * c0;
if (p[0] < -size[0]) { p[0] = -size[0]; inside_box = false; }
if (p[0] > size[0]) { p[0] = size[0]; inside_box = false; }
if (p[1] < -size[1]) { p[1] = -size[1]; inside_box = false; }
if (p[1] > size[1]) { p[1] = size[1]; inside_box = false; }
if (p[2] < -size[2]) { p[2] = -size[2]; inside_box = false; }
if (p[2] > size[2]) { p[2] = size[2]; inside_box = false; }
Eigen::Vector3d normal(0.0, 0.0, 0.0);
double penetration;
if ( inside_box )
{
// find nearest side from the sphere center
double min = size[0] - std::abs(p[0]);
double tmin = size[1] - std::abs(p[1]);
int idx = 0;
if ( tmin < min )
{
min = tmin;
idx = 1;
}
tmin = size[2] - std::abs(p[2]);
if ( tmin < min )
{
min = tmin;
idx = 2;
}
normal[idx] = (p[idx] > 0.0 ? 1.0 : -1.0);
normal = T1.linear() * normal;
penetration = min + r0;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = c0;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
Eigen::Vector3d contactpt = T1 * p;
normal = c0 - contactpt;
double mag = normal.norm();
penetration = r0 - mag;
if (penetration < 0.0)
{
return 0;
}
if (mag > DART_COLLISION_EPS)
{
normal *= (1.0/mag);
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = contactpt;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
}
else
{
double min = size[0] - std::abs(p[0]);
double tmin = size[1] - std::abs(p[1]);
int idx = 0;
if ( tmin < min )
{
min = tmin;
idx = 1;
}
tmin = size[2] - std::abs(p[2]);
if ( tmin < min )
{
min = tmin;
idx = 2;
}
normal.setZero();
normal[idx] = (p[idx] > 0.0 ? 1.0 : -1.0);
normal = T1.linear() * normal;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = contactpt;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
}
return 1;
}
// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and
// generate contact points. this returns 0 if there is no contact otherwise
// it returns the number of contacts generated.
// `normal' returns the contact normal.
// `depth' returns the maximum penetration depth along that normal.
// `return_code' returns a number indicating the type of contact that was
// detected:
// 1,2,3 = box 2 intersects with a face of box 1
// 4,5,6 = box 1 intersects with a face of box 2
// 7..15 = edge-edge contact
// `maxc' is the maximum number of contacts allowed to be generated, i.e.
// the size of the `contact' array.
// `contact' and `skip' are the contact array information provided to the
// collision functions. this function only fills in the position and depth
// fields.
int dBoxBox(CollisionObject* o1, CollisionObject* o2,
const dVector3 p1, const dMatrix3 R1, const dVector3 side1,
const dVector3 p2, const dMatrix3 R2, const dVector3 side2,
CollisionResult& result)
{
const double fudge_factor = 1.05;
dVector3 p,pp,normalC = {0.0, 0.0, 0.0, 0.0};
const double *normalR = 0;
double A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
int i,j,invert_normal,code;
// get vector from centers of box 1 to box 2, relative to box 1
p[0] = p2[0] - p1[0];
p[1] = p2[1] - p1[1];
p[2] = p2[2] - p1[2];
dMULTIPLY1_331 (pp,R1,p); // get pp = p relative to body 1
// get side lengths / 2
A[0] = side1[0];
A[1] = side1[1];
A[2] = side1[2];
B[0] = side2[0];
B[1] = side2[1];
B[2] = side2[2];
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
R11 = Inner44(R1+0,R2+0); R12 = Inner44(R1+0,R2+1); R13 = Inner44(R1+0,R2+2);
R21 = Inner44(R1+1,R2+0); R22 = Inner44(R1+1,R2+1); R23 = Inner44(R1+1,R2+2);
R31 = Inner44(R1+2,R2+0); R32 = Inner44(R1+2,R2+1); R33 = Inner44(R1+2,R2+2);
Q11 = std::abs(R11); Q12 = std::abs(R12); Q13 = std::abs(R13);
Q21 = std::abs(R21); Q22 = std::abs(R22); Q23 = std::abs(R23);
Q31 = std::abs(R31); Q32 = std::abs(R32); Q33 = std::abs(R33);
// for all 15 possible separating axes:
// * see if the axis separates the boxes. if so, return 0.
// * find the depth of the penetration along the separating axis (s2)
// * if this is the largest depth so far, record it.
// the normal vector will be set to the separating axis with the smallest
// depth. note: normalR is set to point to a column of R1 or R2 if that is
// the smallest depth normal so far. otherwise normalR is 0 and normalC is
// set to a vector relative to body 1. invert_normal is 1 if the sign of
// the normal should be flipped.
#define TST(expr1,expr2,norm,cc) \
s2 = std::abs(expr1) - (expr2); \
if (s2 > s) { \
s = s2; \
normalR = norm; \
invert_normal = ((expr1) < 0); \
code = (cc); \
}
s = -1E12;
invert_normal = 0;
code = 0;
// separating axis = u1,u2,u3
TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);
// separating axis = v1,v2,v3
TST (Inner41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
TST (Inner41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
TST (Inner41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);
// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
s2 = std::abs(expr1) - (expr2); \
l = sqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
if (l > 0) { \
s2 /= l; \
if (s2*fudge_factor > s) { \
s = s2; \
normalR = 0; \
normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
invert_normal = ((expr1) < 0); \
code = (cc); \
} \
}
// separating axis = u1 x (v1,v2,v3)
TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);
// separating axis = u2 x (v1,v2,v3)
TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);
// separating axis = u3 x (v1,v2,v3)
TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
if (!code) return 0;
if (s > 0.0) return 0;
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
Eigen::Vector3d normal;
Eigen::Vector3d point_vec;
double penetration;
if (normalR) {
normal << normalR[0],normalR[4],normalR[8];
}
else {
normal << Inner((R1),(normalC)), Inner((R1+4),(normalC)), Inner((R1+8),(normalC));
//dMULTIPLY0_331 (normal,R1,normalC);
}
if (invert_normal) {
normal *= -1.0;
}
// compute contact point(s)
// single point
if (code > 6) {
// an edge from box 1 touches an edge from box 2.
// find a point pa on the intersecting edge of box 1
dVector3 pa;
double sign;
for (i=0; i<3; i++) pa[i] = p1[i];
for (j=0; j<3; j++)
{
#define TEMP_INNER14(a,b) (a[0]*(b)[0] + a[1]*(b)[4] + a[2]*(b)[8])
sign = (TEMP_INNER14(normal,R1+j) > 0) ? 1.0 : -1.0;
//sign = (Inner14(normal,R1+j) > 0) ? 1.0 : -1.0;
for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
}
// find a point pb on the intersecting edge of box 2
dVector3 pb;
for (i=0; i<3; i++) pb[i] = p2[i];
for (j=0; j<3; j++) {
sign = (TEMP_INNER14(normal,R2+j) > 0) ? -1.0 : 1.0;
#undef TEMP_INNER14
for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
}
double alpha,beta;
dVector3 ua,ub;
for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
for (i=0; i<3; i++) pb[i] += ub[i]*beta;
{
point_vec << 0.5*(pa[0]+pb[0]), 0.5*(pa[1]+pb[1]), 0.5*(pa[2]+pb[2]);
penetration = -s;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
}
return 1;
}
// okay, we have a face-something intersection (because the separating
// axis is perpendicular to a face). define face 'a' to be the reference
// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
// the incident face (the closest face of the other box).
const double *Ra,*Rb,*pa,*pb,*Sa,*Sb;
if (code <= 3) {
Ra = R1;
Rb = R2;
pa = p1;
pb = p2;
Sa = A;
Sb = B;
}
else {
Ra = R2;
Rb = R1;
pa = p2;
pb = p1;
Sa = B;
Sb = A;
}
// nr = normal vector of reference face dotted with axes of incident box.
// anr = absolute values of nr.
dVector3 normal2,nr,anr;
if (code <= 3) {
normal2[0] = normal[0];
normal2[1] = normal[1];
normal2[2] = normal[2];
}
else {
normal2[0] = -normal[0];
normal2[1] = -normal[1];
normal2[2] = -normal[2];
}
dMULTIPLY1_331 (nr,Rb,normal2);
anr[0] = fabs (nr[0]);
anr[1] = fabs (nr[1]);
anr[2] = fabs (nr[2]);
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
// are stored in a1,a2.
int lanr,a1,a2;
if (anr[1] > anr[0]) {
if (anr[1] > anr[2]) {
a1 = 0;
lanr = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
else {
if (anr[0] > anr[2]) {
lanr = 0;
a1 = 1;
a2 = 2;
}
else {
a1 = 0;
a2 = 1;
lanr = 2;
}
}
// compute center point of incident face, in reference-face coordinates
dVector3 center;
if (nr[lanr] < 0) {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
}
else {
for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
}
// find the normal and non-normal axis numbers of the reference box
int codeN,code1,code2;
if (code <= 3) codeN = code-1; else codeN = code-4;
if (codeN==0) {
code1 = 1;
code2 = 2;
}
else if (codeN==1) {
code1 = 0;
code2 = 2;
}
else {
code1 = 0;
code2 = 1;
}
// find the four corners of the incident face, in reference-face coordinates
double quad[8]; // 2D coordinate of incident face (x,y pairs)
double c1,c2,m11,m12,m21,m22;
c1 = Inner14 (center,Ra+code1);
c2 = Inner14 (center,Ra+code2);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
m11 = Inner44 (Ra+code1,Rb+a1);
m12 = Inner44 (Ra+code1,Rb+a2);
m21 = Inner44 (Ra+code2,Rb+a1);
m22 = Inner44 (Ra+code2,Rb+a2);
{
double k1 = m11*Sb[a1];
double k2 = m21*Sb[a1];
double k3 = m12*Sb[a2];
double k4 = m22*Sb[a2];
quad[0] = c1 - k1 - k3;
quad[1] = c2 - k2 - k4;
quad[2] = c1 - k1 + k3;
quad[3] = c2 - k2 + k4;
quad[4] = c1 + k1 + k3;
quad[5] = c2 + k2 + k4;
quad[6] = c1 + k1 - k3;
quad[7] = c2 + k2 - k4;
}
// find the size of the reference face
double rect[2];
rect[0] = Sa[code1];
rect[1] = Sa[code2];
// intersect the incident and reference faces
double ret[16];
int n = intersectRectQuad (rect,quad,ret);
if (n < 1) return 0; // this should never happen
// convert the intersection points into reference-face coordinates,
// and compute the contact position and depth for each point. only keep
// those points that have a positive (penetrating) depth. delete points in
// the 'ret' array as necessary so that 'point' and 'ret' correspond.
//real point[3*8]; // penetrating contact points
double point[24]; // penetrating contact points
double dep[8]; // depths for those points
double det1 = dRecip(m11*m22 - m12*m21);
m11 *= det1;
m12 *= det1;
m21 *= det1;
m22 *= det1;
int cnum = 0; // number of penetrating contact points found
for (j=0; j < n; j++) {
double k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
double k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
for (i=0; i<3; i++)
{
point[cnum*3+i] = center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
}
dep[cnum] = Sa[codeN] - Inner(normal2,point+cnum*3);
if (dep[cnum] >= 0) {
ret[cnum*2] = ret[j*2];
ret[cnum*2+1] = ret[j*2+1];
cnum++;
}
}
if (cnum < 1) return 0; // this should never happen
// we can't generate more contacts than we actually have
int maxc = 4;
if (maxc > cnum) maxc = cnum;
//if (maxc < 1) maxc = 1;
if (cnum <= maxc) {
// we have less contacts than we need, so we use them all
for (j=0; j < cnum; j++)
{
point_vec << point[j*3+0] + pa[0], point[j*3+1] + pa[1], point[j*3+2] + pa[2];
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[j];
result.addContact(contact);
}
}
else {
// we have more contacts than are wanted, some of them must be culled.
// find the deepest point, it is always the first contact.
int i1 = 0;
double maxdepth = dep[0];
for (i=1; i<cnum; i++) {
if (dep[i] > maxdepth) {
maxdepth = dep[i];
i1 = i;
}
}
int iret[8];
cullPoints (cnum,ret,maxc,i1,iret);
cnum = maxc;
for (j=0; j < cnum; j++)
{
point_vec << point[iret[j]*3+0] + pa[0], point[iret[j]*3+1] + pa[1], point[iret[j]*3+2] + pa[2];
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point_vec;
contact.normal = normal;
contact.penetrationDepth = dep[iret[j]];
result.addContact(contact);
}
}
return cnum;
}
int collideCylinderPlane(CollisionObject* o1, CollisionObject* o2, const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1,
CollisionResult& result)
{
Eigen::Vector3d normal = T1.linear() * plane_normal;
Eigen::Vector3d Rx = T0.linear().rightCols(1);
Eigen::Vector3d Ry = normal - normal.dot(Rx) * Rx;
double mag = Ry.norm();
Ry.normalize();
if (mag < DART_COLLISION_EPS)
{
if (std::abs(Rx[2]) > 1.0 - DART_COLLISION_EPS)
Ry = Eigen::Vector3d::UnitX();
else
Ry = (Eigen::Vector3d(Rx[1], -Rx[0], 0.0)).normalized();
}
Eigen::Vector3d Rz = Rx.cross(Ry);
Eigen::Isometry3d T;
T.linear().col(0) = Rx;
T.linear().col(1) = Ry;
T.linear().col(2) = Rz;
T.translation() = T0.translation();
Eigen::Vector3d nn = T.linear().transpose() * normal;
Eigen::Vector3d pn = T.inverse() * T1.translation();
// four corners c0 = ( -h/2, -r ), c1 = ( +h/2, -r ), c2 = ( +h/2, +r ), c3 = ( -h/2, +r )
Eigen::Vector3d c[4] = {
Eigen::Vector3d(-half_height, -cyl_rad, 0.0),
Eigen::Vector3d(+half_height, -cyl_rad, 0.0),
Eigen::Vector3d(+half_height, +cyl_rad, 0.0),
Eigen::Vector3d(-half_height, +cyl_rad, 0.0) };
double depth[4] = { (pn - c[0]).dot(nn), (pn - c[1]).dot(nn), (pn - c[2]).dot(nn), (pn - c[3]).dot(nn) };
double penetration = -1.0;
int found = -1;
for (int i = 0; i < 4; i++)
{
if (depth[i] > penetration)
{
penetration = depth[i];
found = i;
}
}
Eigen::Vector3d point;
if (std::abs(depth[found] - depth[(found+1)%4]) < DART_COLLISION_EPS)
point = T * (0.5 * (c[found] + c[(found+1)%4]));
else if (std::abs(depth[found] - depth[(found+3)%4]) < DART_COLLISION_EPS)
point = T * (0.5 * (c[found] + c[(found+3)%4]));
else
point = T * c[found];
if (penetration > 0.0)
{
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
return 0;
}
int collideCylinderSphere(CollisionObject* o1, CollisionObject* o2, const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0,
const double& sphere_rad, const Eigen::Isometry3d& T1,
CollisionResult& result)
{
Eigen::Vector3d center = T0.inverse() * T1.translation();
double dist = sqrt(center[0] * center[0] + center[1] * center[1]);
if ( dist < cyl_rad && std::abs(center[2]) < half_height + sphere_rad )
{
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.penetrationDepth = 0.5 * (half_height + sphere_rad - math::sign(center[2]) * center[2]);
contact.point = T0 * Eigen::Vector3d(center[0], center[1], half_height - contact.penetrationDepth);
contact.normal = T0.linear() * Eigen::Vector3d(0.0, 0.0, math::sign(center[2]));
result.addContact(contact);
return 1;
}
else
{
double penetration = 0.5 * (cyl_rad + sphere_rad - dist);
if ( penetration > 0.0 )
{
if ( std::abs(center[2]) > half_height )
{
Eigen::Vector3d point = (Eigen::Vector3d(center[0], center[1], 0.0).normalized());
point *= cyl_rad;
point[2] = math::sign(center[2]) * half_height;
Eigen::Vector3d normal = point - center;
penetration = sphere_rad - normal.norm();
normal = (T0.linear() * normal).normalized();
point = T0 * point;
if (penetration > 0.0)
{
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
}
else // if( center[2] >= -half_height && center[2] <= half_height )
{
Eigen::Vector3d point = (Eigen::Vector3d(center[0], center[1], 0.0)).normalized();
Eigen::Vector3d normal = -(T0.linear() * point);
point *= (cyl_rad - penetration);
point[2] = center[2];
point = T0 * point;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
}
}
return 0;
}
bool CollisionRequest::isSatisfied(const CollisionResult& result) const
{
return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts());
}
