//returns the number of resulting intervals
//if 1, returns it in p
//if 2, returns them in p and q
int IntervalSubtract(const ClosedInterval& a,const OpenInterval& b,ClosedInterval& p,ClosedInterval& q)
{
if(a.intersects(b)) {
if(a.contains(b.a) && a.contains(b.b)) {
p.a=a.a; p.b=b.a;
q.a=b.b; q.b=a.b;
return 2;
}
else if(a.contains(b.a)) {
p.a=a.a; p.b=b.a;
return 1;
}
else if(a.contains(b.b)) {
p.a=b.b; p.b=a.b;
return 1;
}
else {
Assert(b.contains(a.a) && b.contains(a.b));
return 0;
}
}
else {
p=a;
return 1;
}
}
bool HollowBall::containsSphere(const Sphere3D& s) const
{
Real d = s.center.distance(center);
ClosedInterval i;
i.a = Max(innerRadius,Zero);
i.b = outerRadius;
return i.contains(d-s.radius) && i.contains(d+s.radius);
}
void RegionOfRows::ShiftRegion(int RowDisplacement,int ColumnDisplacement)
{
if (!IsDefined()) return;
if (RowDisplacement != 0)
{
// Shift rows
int StartRow = (int) Rows.GetStart();
StartRow += RowDisplacement;
if (StartRow < 0)
{
// Can't shift => invalidate !
Reset();
return;
}
int EndRow = (int) Rows.GetEnd();
EndRow += RowDisplacement;
Rows.Set((unsigned int)StartRow,(unsigned int)EndRow);
}
if (ColumnDisplacement != 0)
{
// Shift Columns
ClosedInterval<unsigned int>* MyColumns = GetMyColumns();
unsigned int NumberOfRows = Rows.GetSpan() + 1;
for (unsigned int i=0; i < NumberOfRows;++i)
{
int StartColumn = (int) MyColumns->GetStart();
StartColumn += ColumnDisplacement;
if (StartColumn < 0)
{
// Can't shift => invalidate !
Reset();
return;
}
int EndColumn = (int) MyColumns->GetEnd();
EndColumn += ColumnDisplacement;
MyColumns->Set((unsigned int)StartColumn,(unsigned int)EndColumn);
MyColumns++;
}
}
}
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;
}
void make(const ClosedInterval& i)
{
if (i.pointCount() > 1)
{
if (mostInternal == NULL) { mostInternal = &i; }
fprintf(output, (mostInternal == &i) ? "\n" : "\n\n###########\n");
}
}
bool RegionOfRows::GetIntersection(const RegionOfRows& RegionOI,RegionOfRows& Intersection) const
{
Intersection.Reset();
// Regions are defined ?
if (!IsDefined()) return false;
if (!RegionOI.IsDefined()) return false;
// Regions are defined, do the have common rows ?
const ClosedInterval<unsigned int>* Region1Rows = GetRows();
const ClosedInterval<unsigned int>* Region2Rows = RegionOI.GetRows();
ClosedInterval<unsigned int> RowsOI;
if (!Region1Rows->GetOverlap(*Region2Rows,RowsOI)) return false; // no rows in common
unsigned int RowStart = RowsOI.GetStart();
unsigned int RowEnd = RowsOI.GetEnd();
const ClosedInterval<unsigned int>* Columns1 = GetColumns();
const ClosedInterval<unsigned int>* Columns2 = RegionOI.GetColumns();
// Adjust offets
Columns1 += RowStart - GetFirstRow();
Columns2 += RowStart - RegionOI.GetFirstRow();
for (unsigned int Row = RowStart;Row <= RowEnd;Row++)
{
ClosedInterval<unsigned int> ColumnsOI;
if (Columns1->GetOverlap(*Columns2,ColumnsOI))
{
// Rows and columns in common => intersection
Intersection.Add(Row,ColumnsOI.GetStart(),ColumnsOI.GetEnd());
} else if (Intersection.IsDefined()) break; // Convex region !
Columns1++;
Columns2++;
}
// Any intersection ?
return Intersection.IsDefined();
}
bool Segment2D::intersects(const Vector2& A,const Vector2& B,Vector2& p) const
{
//find (u,v) s.t. a+u(b-a) = A+v(B-A)
//=> (b-a | A-B)*(u,v)^T = (A-a)
Matrix2 M;
Vector2 res,uv;
M.setCol1(b-a);
M.setCol2(A-B);
res = A-a;
if(Math::FuzzyZero(M.determinant())) {
//they're parallel
Vector2 t = b-a;
Vector2 n; n.setPerpendicular(t);
Real D = dot(n,A);
Real d = dot(n,a);
if(Math::FuzzyEquals(d,D)) { //they overlap
ClosedInterval U,u;
u.a = 0;
u.b = t.normSquared();
U.a = dot(t,A-a);
U.b = dot(t,B-a);
if(U.intersects(u)) {
ClosedInterval i;
i.setIntersection(u,U);
Real param=0.5*(i.a+i.b);
p = a + t*(param/u.b);
return true;
}
}
return false;
}
M.inplaceInverse();
M.mul(res,uv);
if(uv.x>=Zero && uv.x<=One &&
uv.y>=Zero && uv.y<=One) {
interpolate(a,b,uv.x,p);
Vector2 temp;
interpolate(A,B,uv.y,temp);
if(temp.distance(p) > 1e-3) {
cout<<"Error: intersection points are too far away "<<endl;
cout<<A<<" -> "<<B<<endl;
cout<<a<<" -> "<<b<<endl;
cout<<"u,v "<<uv<<endl;
cout<<"inverse basis "<<endl<<M<<endl;
cout<<"p1,p2 "<<p<<", "<<temp<<endl;
abort();
}
return true;
}
/*
if(intersects(a,b)) {
if(Math::FuzzyZero(uv.x)) { p=a; return true; }
if(Math::FuzzyEquals(uv.x,One)) { p=b; return true; }
if(Math::FuzzyZero(uv.y)) { p=a; return true; }
if(Math::FuzzyEquals(uv.y,One)) { p=b; return true; }
cout<<"Error! segment is supposed to intersect, but we don't have that in the basis!"<<endl;
cout<<A<<" -> "<<B<<endl;
cout<<a<<" -> "<<b<<endl;
cout<<"u,v "<<uv<<endl;
cout<<"inverse basis "<<endl<<M<<endl;
abort();
}
*/
return false;
}
