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; }
bool Surface::collapse(Edge& e) { timespec t0,t1,t; clock_gettime(CLOCK_REALTIME,&t0); Point* p1 = e.p1; Point* p2 = e.p2; assert(p1); assert(p2); if(p1->faces.size() ==0 || p2->faces.size()==0) //Do not simplify boundary corners { //cerr << "*ERROR: EMPTY VERTEX.\n"; failed_collapses++; //cout << redtty << "failed.\n" << deftty; return false; } //Iterating faces vector<Face*> for_removal; //clock_gettime(CLOCK_REALTIME,&t0); for(vector<Face*>::iterator fit = p1->faces.begin(); fit!=p1->faces.end(); ++fit) { bool removeface = false; vector<Point*>::iterator auxp1; for(vector<Point*>::iterator pit = (*fit)->points.begin(); pit != (*fit)->points.end() ; ++pit) { if((*pit) == p2) { removeface = true; break; } else if ((*pit) == p1) { auxp1 = pit; } } if(removeface) //p1 and p2 share face, remove it. { for_removal.push_back((*fit)); } else //Swap p1 for p2 for this face { //Face is about to be modified. Checking intersection. Point3 p30((*fit)->points[0]->x,(*fit)->points[0]->y,(*fit)->points[0]->z); Point3 p31((*fit)->points[1]->x,(*fit)->points[1]->y,(*fit)->points[1]->z); Point3 p32((*fit)->points[2]->x,(*fit)->points[2]->y,(*fit)->points[2]->z); Triangle3 face(p30,p31,p32); for(face_vec_it fit2 = m_faces.begin(); fit2 != m_faces.end(); ++fit2) { Face* f = (*fit2); Point3 pf0(f->points[0]->x,f->points[0]->y,f->points[0]->z); Point3 pf1(f->points[1]->x,f->points[1]->y,f->points[1]->z); Point3 pf2(f->points[2]->x,f->points[2]->y,f->points[2]->z); Triangle3 face2(pf0,pf1,pf2); if(CGAL::do_intersect(face,face2)) { cerr << "***Faces " << (*fit)->id << " X " << f->id << endl; } } (*auxp1) = p2; p2->faces.push_back((*fit)); } } //cerr << "Removing faces: "; for(vector<Face*>::iterator it = for_removal.begin(); it != for_removal.end(); ++it) { //cerr << (*it)->id << " "; removeFace(*it); (*it)->removed = true; //delete (*it); } //Set position of p2 as midpoint between p1-p2 p2->x = e.placement->x; p2->y = e.placement->y; p2->z = e.placement->z; clock_gettime(CLOCK_REALTIME,&t1); t = diff(t0,t1); time_faces+= getNanoseconds(t); //TODO: more efficient to use std::remove_if clock_gettime(CLOCK_REALTIME,&t0); removeEdge(m_edges[e.id]); vector<Edge*> edges_to_remove; edge_vec_it eit = p1->to.begin(); //Remove double edges to while(eit != p1->to.end()) { Edge* e = (*eit); bool found = false; edge_vec_it it = p2->to.begin(); while(it != p2->to.end()) { Edge* e2 = (*it); if(e->p1->id == e2->p1->id) { //cerr << "Found " << e->id << " " << e->p1->id << " " << e->p2->id << endl; edges_to_remove.push_back(e); found = true; break; } it++; } if(!found) { e->p2 = p2; if(e->p1->id == e->p2->id) edges_to_remove.push_back(e); } eit++; } //Remove double edges from eit = p1->from.begin(); while(eit!=p1->from.end()) { Edge* e = (*eit); bool found = false; edge_vec_it it = p2->from.begin(); while(it != p2->from.end()) { Edge* e2 = (*it); if(e->p2->id == e2->p2->id) { //cerr << "Found from " << e->id << " " << e->p1->id << " " <<e->p2->id << endl; edges_to_remove.push_back(e); found =true; break; } it++; } if(!found) { e->p1=p2; if(e->p1->id == e->p2->id) edges_to_remove.push_back(e); } eit++; } eit = edges_to_remove.begin(); while(eit != edges_to_remove.end()) { removeEdge(*eit); eit++; } //Append p1 edges to p2 p2->to.insert(p2->to.end(), p1->to.begin(), p1->to.end()); p2->from.insert(p2->from.end(),p1->from.begin(), p1->from.end()); clock_gettime(CLOCK_REALTIME,&t1); t = diff(t0,t1); time_edges += getNanoseconds(t); //Can remove point p1 clock_gettime(CLOCK_REALTIME,&t0); removePoint(p1); clock_gettime(CLOCK_REALTIME,&t1); t = diff(t0,t1); time_point+=getNanoseconds(t); return true; }
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; } } }