std::list<WCIntersectionResult> __WILDCAT_NAMESPACE__::GeometricIntersection(WCGeometricLine *left,
WCGeometricLine *right, const WPFloat &tol, unsigned const int &flags) {
std::list<WCIntersectionResult> results;
//Check if self intersecting
if (left == right) return results;
//Evaluate both lines at both ends
WCVector4 p1 = left->Begin();
WCVector4 p2 = left->End();
WCVector4 p3 = right->Begin();
WCVector4 p4 = right->End();
//Get length vectors
WCVector4 p21(p2 - p1);
WCVector4 p43(p4 - p3);
WPFloat dist;
//Check for parallel
WCVector4 cross = p43.CrossProduct(p21);
WPFloat denom = cross.Magnitude();
WPFloat onePlusTol = 1.0 + tol;
if (denom < 0.001) {
//Determine perpendicular distance
WPFloat p21mag = p21.Magnitude();
WCVector4 p13(p1 - p3);
WCVector4 num = p21.CrossProduct(p13);
dist = num.Magnitude() / p21mag;
//If outside of tolerance, no intersection
if (dist <= tol) {
//Project all four points onto opposite lines
p21 /= p21mag;
WPFloat p43mag = p43.Magnitude();
p43 /= p43mag;
WCVector4 p31(p3 - p1);
WCVector4 p41(p4 - p1);
WCVector4 p23(p2 - p3);
WPFloat p1Proj = p43.DotProduct(p13) / p43mag;
WPFloat p2Proj = p43.DotProduct(p23) / p43mag;
WPFloat p3Proj = p21.DotProduct(p31) / p21mag;
WPFloat p4Proj = p21.DotProduct(p41) / p21mag;
//See if each point is in 0.0 to 1.0 +- tolerance
bool p1In = (p1Proj > -tol) && (p1Proj < onePlusTol);
bool p2In = (p2Proj > -tol) && (p2Proj < onePlusTol);
bool p3In = (p3Proj > -tol) && (p3Proj < onePlusTol);
bool p4In = (p4Proj > -tol) && (p4Proj < onePlusTol);
//If none are in then no overlap, return
if ( p1In || p2In || p3In || p4In) {
//Bounds check all of the projections
p1Proj = STDMAX(0.0, STDMIN(1.0, p1Proj));
p2Proj = STDMAX(0.0, STDMIN(1.0, p2Proj));
p3Proj = STDMAX(0.0, STDMIN(1.0, p3Proj));
p4Proj = STDMAX(0.0, STDMIN(1.0, p4Proj));
//Create hit result
WCIntersectionResult hit;
hit.type = IntersectLine;
WCVector4 pStart, pEnd;
//Case 1 - Left is completely within Right
if (p1In && p2In) {
//Set the start and end points
pStart = p1;
pEnd = p2;
//Set the param values
hit.leftParam.I( 0.0 );
hit.leftParam.J( 1.0 );
hit.rightParam.I( STDMIN(p1Proj,p2Proj) );
hit.rightParam.J( STDMAX(p1Proj,p2Proj) );
//Set boundary values
hit.leftBoundary = true;
hit.rightBoundary = false;
}
//Case 2 - Right is completely within Left
else if (p3In && p4In) {
//Set the start and end points
pStart = p3;
pEnd = p4;
//Set the param values
hit.leftParam.I( STDMIN(p3Proj,p4Proj) );
hit.leftParam.J( STDMAX(p3Proj,p4Proj) );
hit.rightParam.I( 0.0 );
hit.rightParam.J( 1.0 );
//Set boundary values
hit.leftBoundary = false;
hit.rightBoundary = true;
}
//Remaining Cases
else {
//Simple sets
if (p1In) { hit.leftParam.I(0.0); pStart = p1; }
if (p2In) { hit.leftParam.J(1.0); pStart = p2; }
if (p3In) { hit.rightParam.I(0.0); pEnd = p3; }
if (p4In) { hit.rightParam.J(1.0); pEnd = p4; }
//Double sets
if (p1In && p3In) { hit.leftParam.J(p3Proj); hit.rightParam.J(p1Proj); }
if (p1In && p4In) { hit.leftParam.J(p4Proj); hit.rightParam.I(p1Proj); }
if (p2In && p3In) { hit.leftParam.I(p3Proj); hit.rightParam.J(p2Proj); }
if (p2In && p4In) { hit.leftParam.I(p4Proj); hit.rightParam.I(p2Proj); }
//Check to see if intersection is a point (think of two lines end to end)
if (pStart.Distance(pEnd) < tol) hit.type = IntersectPoint;
//This is not the best way to check for CULL BOUNDARY, but if pStart and pEnd distance < tol, must be end-to-end
if ((flags & INTERSECT_CULL_BOUNDARY) && (hit.type == IntersectPoint))
return results;
//Set boundary values
hit.leftBoundary = true;
hit.rightBoundary = true;
}
//Overlap forces boundarys to false
//See if genObj
if ((hit.type == IntersectPoint) && (flags & INTERSECT_GEN_POINTS)) {
//Create new point
WCGeometricPoint *newPoint = new WCGeometricPoint(pStart);
//Add to result struct
hit.object = newPoint;
}
else if ((hit.type == IntersectLine) && (flags & INTERSECT_GEN_LINES)) {
//Create new line
WCGeometricLine *newLine = new WCGeometricLine(pStart, pEnd);
//Add to result struct
hit.object = newLine;
}
else hit.object = NULL;
//Add the hit to the list
results.push_back(hit);
}
}
}
//Non-Parallel Case
else {
//Many dot products
WPFloat d121 = p1.DotProduct(p21); // L
WPFloat d2121 = p21.DotProduct(p21); // M
WPFloat d321 = p3.DotProduct(p21); // N
WPFloat d4321 = p43.DotProduct(p21); // O
WPFloat d143 = p1.DotProduct(p43); // Q
WPFloat d343 = p3.DotProduct(p43); // R
WPFloat d4343 = p43.DotProduct(p43); // S
denom = (d4343 * d2121) - (d4321 * d4321);
//What does this correspond to?
if (denom == 0.0) {
CLOGGER_WARN(WCLogManager::RootLogger(), "GeometricIntersection::LineLine - Denominator == 0.0.");
}
//Otherwise
else {
//Calculate parametric intersection values
WPFloat numer = d4343 * (d321 - d121) + d4321 * (d143 - d343);
WPFloat mua = numer / denom;
WPFloat mub = (d143 + d4321 * mua - d343) / d4343;
//Make sure mua and mub are (0.0 +- tol, 1.0 +- tol)
if ( (mua < onePlusTol) && (mua > -tol) && (mub < onePlusTol) && (mub > -tol) ) {
//Bound mua and mub [0, 1]
mua = STDMAX(0.0, STDMIN(1.0, mua));
mub = STDMAX(0.0, STDMIN(1.0, mub));
//Calculate points and distance between them
WCVector4 pointOnFirst = p1 + p21 * mua;
WCVector4 pointOnSecond = p3 + p43 * mub;
//Make sure lines are not > tol apart
dist = pointOnFirst.Distance(pointOnSecond);
if (dist < tol) {
//Create intersection result
WCIntersectionResult hit;
hit.type = IntersectPoint;
hit.leftParam.I(mua);
hit.rightParam.I(mub);
hit.leftBoundary = (fabs(mua) < tol) || (fabs(mua-1.0) < tol);
hit.rightBoundary = (fabs(mub) < tol) || (fabs(mub-1.0) < tol);
//Check for culling end-point intersections
if (!(flags & INTERSECT_CULL_BOUNDARY) || (!hit.leftBoundary && !hit.rightBoundary)) {
//See if genObj
if (flags & INTERSECT_GEN_POINTS) {
//Create new point
WCGeometricPoint *newPoint = new WCGeometricPoint(pointOnFirst);
//Add to result struct
hit.object = newPoint;
}
else hit.object = NULL;
//Add the intersection to the list
// std::cout << hit << pointOnFirst << std::endl;
results.push_back(hit);
}
}
}
}
}
//Return result
return results;
}
int main(int argc, char* argv[])
{
CS325Graphics window(argc, argv);
float delta = 0.1;
Point2D p1(CS325Graphics::X_MIN, CS325Graphics::Y_MAX / 4.5);
Point2D p2(CS325Graphics::X_MAX, CS325Graphics::Y_MAX / 4.5);
Point2D p3(CS325Graphics::X_MIN, CS325Graphics::Y_MIN);
Point2D p4(CS325Graphics::X_MAX, CS325Graphics::Y_MAX);
//Points 41, 42, 45, 46 control the sandbox. DON"T MESS WITH THEM!
Point3D p30(0.5, 0.5,-3.5);
Point3D p31(0.5, -0.5,-3.5);
Point3D p32(-0.5,-0.5,-3.5);
Point3D p33(-0.5, 0.5,-3.5);
Point3D p34(0.5, 0.5,-1.5);
Point3D p35(0.5, -0.5,-1.5);
Point3D p36(-0.5,-0.5,-1.5);
Point3D p37(-0.5, 0.5,-1.5);
Point3D p40( -70.8, 28.8, -50.8);
Point3D p41( 50.8,-2.8, 50.8);
Point3D p42(-50.8,-2.8, 50.8);
Point3D p43(-58.8, 25.8, 50.8);
Point3D p44( 50.8, 50.8, -50.8);
Point3D p45( 50.8,-2.8, -50.8);
Point3D p46(-50.8,-2.8, -50.8);
Point3D p47(-84.8,-2.8, -50.8);
Point3D p49(-8.5,22.0, 50.8);
Point3D p48(70,20,50.8);
Point3D p50(3.5, 0.5,-3.5);
Point3D p51(3.5, -0.5,-3.5);
Point3D p52(2.5,-0.5,-3.5);
Point3D p53(2.5, 0.5,-3.5);
Point3D p54(3.5, 0.5,-1.5);
Point3D p55(3.5, -0.5,-1.5);
Point3D p56(2.5,-0.5,-1.5);
Point3D p57(2.5, 0.5,-1.5);
Point3D p60(3.5, 0.5, 13.5);
Point3D p61(3.5, -0.5, 13.5);
Point3D p62(2.5,-0.5, 13.5);
Point3D p63(2.5, 0.5, 13.5);
Point3D p64(3.5, 0.5, 16.5);
Point3D p65(3.5, -0.5, 16.5);
Point3D p66(2.5,-0.5, 16.5);
Point3D p67(2.5, 0.5, 16.5);
Point2D viewPos;
Vector2D viewDir;
Vector3D deltaV;
viewDir.setAngle(0);
// move view position
for(int i = 0; i < MOVE_TEST; i){
/*window.DrawLineOnScreen(p1, p2);*/
//window.DrawLineOnScreen(p4, p3);
window.DrawLineInSpace(p30, p31);
window.DrawLineInSpace(p31, p32);
window.DrawLineInSpace(p32, p33);
window.DrawLineInSpace(p33, p30);
window.DrawLineInSpace(p34, p35);
window.DrawLineInSpace(p35, p36);
window.DrawLineInSpace(p36, p37);
window.DrawLineInSpace(p37, p34);
window.DrawLineInSpace(p30, p34);
window.DrawLineInSpace(p31, p35);
window.DrawLineInSpace(p32, p36);
window.DrawLineInSpace(p33, p37);
window.DrawLineInSpace(p50, p51);
window.DrawLineInSpace(p51, p52);
window.DrawLineInSpace(p52, p53);
window.DrawLineInSpace(p53, p50);
window.DrawLineInSpace(p54, p55);
window.DrawLineInSpace(p55, p56);
window.DrawLineInSpace(p56, p57);
window.DrawLineInSpace(p57, p54);
window.DrawLineInSpace(p50, p54);
window.DrawLineInSpace(p51, p55);
window.DrawLineInSpace(p52, p56);
window.DrawLineInSpace(p53, p57);
window.DrawLineInSpace(p60, p61);
window.DrawLineInSpace(p61, p62);
window.DrawLineInSpace(p62, p63);
window.DrawLineInSpace(p63, p60);
window.DrawLineInSpace(p64, p65);
window.DrawLineInSpace(p65, p66);
window.DrawLineInSpace(p66, p67);
window.DrawLineInSpace(p67, p64);
window.DrawLineInSpace(p60, p64);
window.DrawLineInSpace(p61, p65);
window.DrawLineInSpace(p62, p66);
window.DrawLineInSpace(p63, p67);
//window.DrawLineInSpace(p40, p41);
window.DrawLineInSpace(p41, p42);
window.DrawLineInSpace(p42, p43);
//window.DrawLineInSpace(p43, p40);
//window.DrawLineInSpace(p44, p45);
window.DrawLineInSpace(p45, p46);
//window.DrawLineInSpace(p46, p47);
//window.DrawLineInSpace(p47, p44);
window.DrawLineInSpace(p40, p45);
window.DrawLineInSpace(p41, p45);
window.DrawLineInSpace(p42, p46);
window.DrawLineInSpace(p43, p47);
window.DrawLineInSpace(p40, p47);
window.DrawLineInSpace(p41, p49);
window.DrawLineInSpace(p42, p49);
window.DrawLineInSpace(p41, p48);
window.DrawLineInSpace(p45, p48);
if(GetAsyncKeyState(VK_DOWN)) // the DOWN arrow was pressed, let's do something
{
delta = -.1;
viewPos.setY(viewPos.getY() + cos(-viewDir.getAngle())*delta);
viewPos.setX(viewPos.getX() + sin(-viewDir.getAngle())*delta);
window.SetViewPosition(viewPos);
cout << "view pos: " << viewPos.toString() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_UP)) // the UP arrow was pressed, let's do something
{
delta = .1;
viewPos.setY(viewPos.getY() + cos(-viewDir.getAngle())*delta);
viewPos.setX(viewPos.getX() + sin(-viewDir.getAngle())*delta);
window.SetViewPosition(viewPos);
cout << "view pos: " << viewPos.toString() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_RIGHT)) // the RIGHT arrow was pressed, let's do something
{
delta = .1;
viewDir.setAngle(viewDir.getAngle()+delta);
window.SetViewDirection(viewDir);
cout << "view dir: " << viewDir.getAngle() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_LEFT)) // the LEFT arrow was pressed, let's do something
{
delta = -.1;
viewDir.setAngle(viewDir.getAngle()+delta);
window.SetViewDirection(viewDir);
cout << "view dir: " << viewDir.getAngle() << endl;
window.DisplayNow();
Sleep(50);
}
if(GetAsyncKeyState(VK_ESCAPE))
{
return 1;
}
}
}
