C++ (Cpp) Point2f Examples

Example #1

File: Robot.cpp Project: sunweilun/robocup-2013

void Robot::rotateTo(const cv::Point2f &new_dir)
{
    cv::Point2f dir(sin(ori),cos(ori));
    float cross = dir.x*new_dir.y-dir.y*new_dir.x;
    float dot = new_dir.dot(dir);
    dot = dot>1?1:(dot<-1?-1:dot);
    float angle = acos(dot);
    if(cross<0)
        turnRight(angle*180/M_PI);
    else
        turnLeft(angle*180/M_PI);
}

Example #2

File: convexpolygon.cpp Project: tiagojc/IBTSFIF

ConvexPolygon ConvexPolygon::intersect(const ConvexPolygon& poly) const
{
	ConvexPolygon result;

	int a = 0;
	int b = 0;

	int aa = 0;
	int ba = 0;

	enum State {IN_A, IN_B, UNKNOWN};
	State state = UNKNOWN;

	bool firstPoint = true;

	do {
		const int a1 = (a + m_vertices.size() - 1) % m_vertices.size();
		const int b1 = (b + poly.vertices().size() - 1) % poly.vertices().size();

		const cv::Point2f vecA = m_vertices[a] - m_vertices[a1];
		const cv::Point2f vecB = poly.vertices()[b] - poly.vertices()[b1];

		const float cross = signedArea2(cv::Point2f(0, 0), vecA, vecB);
		const float aHB = signedArea2(poly.vertices()[b1], poly.vertices()[b], m_vertices[a]);
		const float bHA = signedArea2(m_vertices[a1], m_vertices[a], poly.vertices()[b]);

		//std::cout << "cross=" << cross << ", aHB=" << aHB << ", bHA=" << bHA << std::endl;

		std::vector<cv::Point2f> intersectionPoints;
		LineSegment::IntersectResult intersectResult = LineSegment(m_vertices[a1], m_vertices[a]).intersect(LineSegment(poly.vertices()[b1], poly.vertices()[b]), intersectionPoints);

		if (intersectResult == LineSegment::INTERESECTING) {
			if (firstPoint && state == UNKNOWN) {
				aa = 0;
				ba = 0;
				firstPoint = false;
			}

			if (aHB > 0) {
				state = IN_A;
			} else if (bHA > 0) {
				state = IN_B;
			}

			assert(intersectionPoints.size() > 0);
			result.addPoint(intersectionPoints[0]);
		}

		if ((intersectResult == LineSegment::COINCIDENT) && (intersectionPoints.size() > 0) && (vecA.dot(vecB) < 0)) {
			result = ConvexPolygon(intersectionPoints);
			return result;
		}

		if ((std::abs(cross) <= std::numeric_limits<float>::epsilon()) && (std::abs(aHB) <= std::numeric_limits<float>::epsilon()) && (std::abs(bHA) <= std::numeric_limits<float>::epsilon())) {
			if (state == IN_A) {
				ba++;
				b = (b + 1) % poly.vertices().size();
			} else {
				aa++;
				a = (a + 1) % m_vertices.size();
			}
		} else if ((std::abs(cross) <= std::numeric_limits<float>::epsilon()) && (aHB < 0) && (bHA < 0)) {
			std::cout << "III" << std::endl;
			result.clear();
			return result;
		} else if (cross >= 0) {
			if (bHA > 0) {
				if (state == IN_A) {
					result.addPoint(m_vertices[a]);
				}
				aa++;
				a = (a + 1) % m_vertices.size();
			} else {
				if (state == IN_B) {
					result.addPoint(poly.vertices()[b]);
				}
				ba++;
				b = (b + 1) % poly.vertices().size();
			}
		} else {
			if (aHB > 0) {
				if (state == IN_B) {
					result.addPoint(poly.vertices()[b]);
				}
				ba++;
				b = (b + 1) % poly.vertices().size();
			} else {
				if (state == IN_A) {
					result.addPoint(m_vertices[a]);
				}
				aa++;
				a = (a + 1) % m_vertices.size();
			}
		}

		//std::cout << "a=" << a << ", b=" << b << ", aa=" << aa << ", ba=" << ba << ", state=" << state << std::endl;
	} while (((aa < (int)m_vertices.size()) || (ba < (int)poly.vertices().size())) && (aa < 2 * (int)m_vertices.size()) && (ba < 2 * (int)poly.vertices().size()));

	return result;
}