int ClosestContact(const ContactPoint& p,const Meshing::TriMesh& mesh,ContactPoint& pclose,Real normalScale)
{
int closest = -1;
Real closestDist2 = Inf;
Triangle3D tri;
Plane3D plane;
for(size_t i=0;i<mesh.tris.size();i++) {
mesh.GetTriangle(i,tri);
//first check distance to supporting plane, since it's a lower bound
tri.getPlane(plane);
Real dxmin = plane.distance(p.x);
Real dn = normalScale*plane.normal.distanceSquared(p.n);
if(dn + Sqr(dxmin) < closestDist2) {
//has potential to be closer than previous
Vector3 cp = tri.closestPoint(p.x);
Real d = cp.distanceSquared(p.x) + dn;
if(d < closestDist2) {
closest = (int)i;
closestDist2 = d;
pclose.x = cp;
pclose.n = plane.normal;
pclose.kFriction = p.kFriction;
}
}
}
return closest;
}
Real Polygon3D::maxDistance(const Plane3D& p) const
{
Real dmax=0;
for(size_t i=0;i<vertices.size();i++) {
Real d = p.distance(vertices[i]);
if(d > dmax) dmax=d;
}
return dmax;
}
bool Circle3D::setIntersection(const Sphere3D& s,const Plane3D& p)
{
Real d = p.distance(s.center);
Real absd=Abs(d);
if(absd > s.radius) return false;
axis = p.normal;
//projection of s.center on plane
center = s.center - d*p.normal;
radius = pythag_leg(absd,s.radius);
return true;
}
bool ConvexPolyhedron3D::planeNeg(const Plane3D& p) const
{
Real vpos;
for(int i=0; i<numVertices; i++)
{
vpos = p.distance(vertices[i]);
if(vpos > Zero)
return false;
}
return true;
}
bool TriMesh::PlaneSplits(const Plane3D& p,Real& dmin,Real& dmax) const
{
dmin=Inf;
dmax=-Inf;
Real d;
for(size_t i=0;i<verts.size();i++) {
d = p.distance(verts[i]);
if(d < dmin) dmin=d;
if(d > dmax) dmax=d;
}
return (dmin <= Zero && dmax >= Zero);
}
void Triangle3D::edgeIntersections(const Triangle3D& T, Real u[3]) const
{
Plane3D PT;
T.getPlane(PT);
Real da,db,dc;
da=PT.distance(a);
db=PT.distance(b);
dc=PT.distance(c);
u[0]=u[1]=u[2]=-One;
//check to see if these points are within T's boundaries
Vector3 x;
Vector2 U;
Real ui;
//edge a,b
ui = SegmentZeroCrossing(da,db);
if(ui >= Zero && ui <= One) {
interpolate(a,b,ui,x);
U = T.planeCoords(x);
if(containsPlaneCoords(U)) u[0] = ui;
}
//edge b,c
ui = SegmentZeroCrossing(db,dc);
if(ui >= Zero && ui <= One) {
interpolate(b,c,ui,x);
U = T.planeCoords(x);
if(containsPlaneCoords(U)) u[1] = ui;
}
//edge c,a
ui = SegmentZeroCrossing(dc,da);
if(ui >= Zero && ui <= One) {
interpolate(c,a,ui,x);
U = T.planeCoords(x);
if(containsPlaneCoords(U)) u[2] = ui;
}
}
bool ConvexPolyhedron3D::planeSplits(const Plane3D& p) const
{
ClosedInterval x;
x.setEmpty();
Real vpos;
for(int i=0; i<numVertices; i++)
{
vpos = p.distance(vertices[i]);
x.expand(vpos);
if(x.contains(Zero))
return true;
}
return false;
}
bool Triangle3D::intersects(const Plane3D& P, Segment3D& S) const
{
Real d[3];
const Point3D* p[3] = {&a,&b,&c};
for(int i=0; i<3; i++) d[i]=P.distance(*p[i]);
//insertion sort
for(int i=1; i<3; i++) {
Real di=d[i];
const Point3D* pi=p[i];
int j=i;
for(; j>0; j--) {
if(d[j-1] <= di) break;
d[j] = d[j-1];
p[j] = p[j-1];
}
d[j] = di;
p[j] = pi;
}
if(!(d[0] <= d[1] && d[1] <= d[2])) {
printf ("AAAACK: %f %f %f\n",d[0],d[1],d[2]);
}
assert(d[0] <= d[1] && d[1] <= d[2]);
if(d[0] > Zero) return false;
if(d[2] < Zero) return false;
Real u;
if(d[1] <= Zero) { //both 0 and 1 are inside p
if(d[0] == d[2]) u = 0;
else u = d[0]/(d[0]-d[2]);
S.a = (One-u)*(*p[0]) + u*(*p[2]);
if(d[1] == d[2]) u = 0;
else u = d[1]/(d[1]-d[2]);
S.b = (One-u)*(*p[1]) + u*(*p[2]);
}
else { //only 0 is inside p
u = d[0]/(d[0]-d[1]);
S.a = (One-u)*(*p[0]) + u*(*p[1]);
u = d[0]/(d[0]-d[2]);
S.b = (One-u)*(*p[0]) + u*(*p[2]);
}
return true;
}
bool TriMesh::Intersects(const Plane3D& p) const
{
vector<Real> d(verts.size());
for(size_t i=0;i<verts.size();i++)
d[i] = p.distance(verts[i]);
Real da,db,dc;
for(size_t i=0;i<tris.size();i++) {
da=d[tris[i].a];
db=d[tris[i].b];
dc=d[tris[i].c];
if(da > Zero) {
if(db <= Zero || dc <= Zero) return true;
}
else if(da < Zero) {
if(db >= Zero || dc >= Zero) return true;
}
else //da==Zero
return true;
}
return false;
}
bool Sphere3D::intersects(const Plane3D& p) const
{
return Abs(p.distance(center)) <= radius;
}
bool ClipLine(const Vector3& x, const Vector3& v, const Plane3D& b, Real& u1, Real& u2)
{
//p is dot(v, normal), q is signed dist to plane
return ClipLine1D(b.distance(x), dot(v,b.normal), u1,u2);
}
