C++ Point2D::normalize方法代码示例

float Classifier::classify2D_checkcondnum(const Point2D& P, const Point2D& R, float& condnumber) const
{
	condnumber = 0;
	if (path.size() < 2)
	{
		assert(false);
		return 0;
	}

	// consider each path segment as a mini-classifier
	// the segment PR[Pt-->Refpt] and each path's segment cross
	// iff each one classifies the end point of the other in different classes
	Point2D PR = R-P;
	Point2D u = PR; u.normalize();

	unsigned numcross = 0;

	//we'll also look for the distance between P and the nearest segment
	float closestSquareDist = -1.0f;

	size_t segCount = path.size() - 1;
	for (size_t i=0; i<segCount; ++i)
	{
		//current path segment (or half-line!)
		Point2D AP = P - path[i];
		Point2D AB = path[i+1] - path[i];
		Point2D v = AB; v.normalize();

		condnumber = std::max<float>(condnumber, fabs(v.dot(u)));

		// Compute whether PR[Pt-->Refpt] and that segment cross
		float denom = (u.x*v.y-v.x*u.y);
		if (denom != 0)
		{
			// 1. check whether the given pt and the refpt are on different sides of the classifier line
			//we search for alpha and beta so that
			// P + alpha * PR = A + beta * AB
			float alpha = (AP.y * v.x - AP.x * v.y)/denom;
			bool pathIntersects = (alpha >= 0 && alpha*alpha <= PR.norm2());
			if (pathIntersects)
			{
				float beta = (AP.y * u.x - AP.x * u.y)/denom;

				// first and last lines are projected to infinity
				bool refSegIntersects = ((i == 0 || beta >= 0) && (i+1 == segCount || beta*beta < AB.norm2())); //not "beta*beta <= AB.norm2()" because the equality case will be managed by the next segment!

				// crossing iif each segment/line separates the other
				if (refSegIntersects)
					numcross++;
			}
		}

		// closest distance from the point to that segment
		// 1. projection of the point of the line
		float squareDistToSeg = 0;
		float distAH = v.dot(AP);
		if ((i == 0 || distAH >= 0.0) && (i+1 == segCount || distAH <= AB.norm()))
		{
			// 2. Is the projection within the segment limit? yes => closest
			Point2D PH = (path[i] + v * distAH) - P;
			squareDistToSeg = PH.norm2();
		}
		else
		{
			// 3. otherwise closest is the minimum of the distance to the segment ends
			Point2D BP = P - path[i+1];
			squareDistToSeg = std::min( AP.norm2(), BP.norm2() );
		}

		if (closestSquareDist < 0 || squareDistToSeg < closestSquareDist)
		{
			closestSquareDist = squareDistToSeg;
		}
	}

	assert(closestSquareDist >= 0);
	float deltaNorm = sqrt(closestSquareDist);

	return ((numcross & 1) == 0 ? deltaNorm : -deltaNorm);
}

void test12D() {
  Point2D pt(1.0, 2.0);
  Transform2D trans;
  trans.TransformPoint(pt);

  CHECK_INVARIANT(abs(pt.x - 1.0) < 1.e-8, "");
  CHECK_INVARIANT(abs(pt.y - 2.0) < 1.e-8, "");

  Point2D ref1(randNum(), randNum());
  Point2D ref2(randNum(), randNum());

  std::cout << "ref1: " << ref1 << " ref2: " << ref2 << "\n";

  Point2D pt1(randNum(), randNum());
  Point2D pt2(randNum(), randNum());
  Point2D pt1o = pt1;
  Point2D pt2o = pt2;
  std::cout << "pt1: " << pt1 << " pt2: " << pt2 << "\n";

  Transform2D t2d;
  t2d.SetTransform(ref1, ref2, pt1, pt2);
  t2d.TransformPoint(pt1);
  t2d.TransformPoint(pt2);

  // make sure pt1 overlaps ref1
  Point2D dif1 = pt1 - ref1;
  CHECK_INVARIANT(abs(dif1.x) < 1.e-8, "");
  CHECK_INVARIANT(abs(dif1.y) < 1.e-8, "");

  // now check that the angle between the two vectors (ref2 - ref1) and
  // (pt2 - pt1) is zero
  Point2D rvec = ref2 - ref1;
  Point2D pvec = pt2 - pt1;
  rvec.normalize();
  pvec.normalize();
  double pdot = rvec.dotProduct(pvec);
  CHECK_INVARIANT(abs(pdot - 1.0) < 1.e-8, "");

  // compute the reverse transform and make sure we are basically getting the
  // identity
  Transform2D tdi;
  tdi.SetTransform(pt1o, pt2o, pt1, pt2);
  tdi.TransformPoint(pt1);
  tdi.TransformPoint(pt2);

  CHECK_INVARIANT(ptEq(pt1, pt1o), "");
  CHECK_INVARIANT(ptEq(pt2, pt2o), "");

  // the following product should result in an identity matrix
  tdi *= t2d;

  tdi.TransformPoint(pt1);
  tdi.TransformPoint(pt2);

  CHECK_INVARIANT(ptEq(pt1, pt1o), "");
  CHECK_INVARIANT(ptEq(pt2, pt2o), "");

  Point2D npt1(1.0, 0.0);
  Point2D npt2(5.0, 0.0);
  Point2D opt1 = npt1;
  Point2D opt2(1.0, 4.0);
  Transform2D ntd;
  ntd.SetTransform(npt1, M_PI / 2);
  ntd.TransformPoint(npt1);
  ntd.TransformPoint(npt2);

  CHECK_INVARIANT(ptEq(npt1, opt1), "");
  CHECK_INVARIANT(ptEq(npt2, opt2), "");
}