int LocalMinDistance::computeIntersection(Ray &t1, Triangle &t2, OutputVector* contacts)
{
const double alarmDist = getAlarmDistance() + t1.getProximity() + t2.getProximity();
if (fabs(t2.n() * t1.direction()) < 0.000001)
return false; // no intersection for edges parallel to the triangle
Vector3 A = t1.origin();
Vector3 B = A + t1.direction() * t1.l();
Vector3 P,Q,PQ;
static DistanceSegTri proximitySolver;
proximitySolver.NewComputation( t2.p1(), t2.p2(), t2.p3(), A,B,P,Q);
PQ = Q-P;
if (PQ.norm2() >= alarmDist*alarmDist)
return 0;
const double contactDist = alarmDist;
contacts->resize(contacts->size()+1);
DetectionOutput *detection = &*(contacts->end()-1);
detection->elem = std::pair<core::CollisionElementIterator, core::CollisionElementIterator>(t1, t2);
detection->id = t1.getIndex();
detection->point[1]=P;
detection->point[0]=Q;
#ifdef DETECTIONOUTPUT_FREEMOTION
detection->freePoint[1] = P;
detection->freePoint[0] = Q;
#endif
detection->normal=-t2.n();
detection->value = PQ.norm();
detection->value -= contactDist;
return 1;
}
int FFDDistanceGridDiscreteIntersection::computeIntersection(Ray& e2, FFDDistanceGridCollisionElement& e1, OutputVector* contacts)
{
Vector3 rayOrigin(e2.origin());
Vector3 rayDirection(e2.direction());
const double rayLength = e2.l();
DistanceGrid* grid1 = e1.getGrid();
FFDDistanceGridCollisionModel::DeformedCube& c1 = e1.getCollisionModel()->getDeformCube(e1.getIndex());
// Center of the sphere
const Vector3 center1 = c1.center;
// Radius of the sphere
const double radius1 = c1.radius;
const Vector3 tmp = center1 - rayOrigin;
double rayPos = tmp*rayDirection;
const double dist2 = tmp.norm2() - (rayPos*rayPos);
if (dist2 >= (radius1*radius1))
return 0;
double l0 = rayPos - sqrt(radius1*radius1 - dist2);
double l1 = rayPos + sqrt(radius1*radius1 - dist2);
if (l0 < 0) l0 = 0;
if (l1 > rayLength) l1 = rayLength;
if (l0 > l1) return 0; // outside of ray
//const double dist = sqrt(dist2);
//double epsilon = grid1->getCellWidth().norm()*0.1f;
c1.updateFaces();
DistanceGrid::Coord p1;
const SReal cubesize = c1.invDP.norm();
for(int i=0; i<100; i++)
{
rayPos = l0 + (l1-l0)*(i*0.01);
p1 = rayOrigin + rayDirection*rayPos;
// estimate the barycentric coordinates
DistanceGrid::Coord b = c1.undeform0(p1);
// refine the estimate until we are very close to the p2 or we are sure p2 cannot intersect with the object
int iter;
//SReal err1 = 1000.0f;
bool found = false;
for(iter=0; iter<5; ++iter)
{
DistanceGrid::Coord pdeform = c1.deform(b);
DistanceGrid::Coord diff = p1-pdeform;
SReal err = diff.norm();
//if (iter>3)
// sout << "Iter"<<iter<<": "<<err1<<" -> "<<err<<" b = "<<b<<" diff = "<<diff<<" d = "<<grid1->interp(c1.initpos(b))<<""<<sendl;
SReal berr = err*cubesize; if (berr>0.5f) berr=0.5f;
if (b[0] < -berr || b[0] > 1+berr
|| b[1] < -berr || b[1] > 1+berr
|| b[2] < -berr || b[2] > 1+berr)
break; // far from the cube
if (err < 0.001f)
{
// we found the corresponding point, but is is only valid if inside the current cube
if (b[0] > -0.1f && b[0] < 1.1f
&& b[1] > -0.1f && b[1] < 1.1f
&& b[2] > -0.1f && b[2] < 1.1f)
{
found = true;
}
break;
}
//err1 = err;
b += c1.undeformDir( b, diff );
}
if (found)
{
SReal d = grid1->interp(c1.initpos(b));
if (d < 0)
{
// intersection found
contacts->resize(contacts->size()+1);
DetectionOutput *detection = &*(contacts->end()-1);
detection->point[0] = e2.origin() + e2.direction()*rayPos;
detection->point[1] = c1.initpos(b);
detection->normal = e2.direction(); // normal in global space from p1's surface
detection->value = d;
detection->elem.first = e2;
detection->elem.second = e1;
detection->id = e2.getIndex();
return 1;
}
}
// else move along the ray
//if (dot(Vector3(grid1->grad(c1.initpos(b))),rayDirection) < 0)
// rayPos += 0.5*d;
//else
// rayPos -= 0.5*d;
}
return 0;
}
