2D point projection onto cubic (Catmull-Rom) curve

gistfile1.txt

	//
	// CatmullRomCurveProjectPt()
	//
	//
	//*******************************************************************************************
	template <typename T>
	T CatmullRomCurveProjectPt(	const CVector2<T> &p0, 
								const CVector2<T> &p1, 
								const CVector2<T> &p2, 
								const CVector2<T> &p3, 
								const CVector2<T> &p)
	{
		typedef CVector2<T> T_Vec;

		const T_Vec	A(T(1.5) * p1 - T(1.5) * p2 + p3 * T(0.5) - p0 * T(0.5));
		const T_Vec B(p0 - T(2.5) * p1 + T(2.0) * p2 - p3 * T(0.5));
		const T_Vec C((p2 - p0) * T(0.5));
		const T_Vec D(p1);

		
		static const T oneOverGR	= T(0.61803398874989484820);

		//
		// Find interval where nearest point is located using Golden-section search (https://en.wikipedia.org/wiki/Golden-section_search)
		//
		// For further info see:
		// http://www.paulwrightapps.com/blog/2014/9/19/projecting-a-point-onto-a-bzier-curve-in-objective-c-using-minimisation 
		//

		T	mint	= 0.0f;
		T	maxt	= 1.0f;

		{
			static const uint NUM_INITIAL_BOUNDS_SEARCH_ITERS = 7;

			const T		offs	= (maxt - mint) * oneOverGR;
			const T_Vec	E		= D - p;

			T	t0	= maxt - offs;
			T	t1	= mint + offs;

			for (uint i = 0; i < NUM_INITIAL_BOUNDS_SEARCH_ITERS; i++)
			{
				// diff0: point on curve at t0 minus p
				// diff1: point on curve at t1 minus p

				const T_Vec diff0 = ((A * t0 + B) * t0 + C) * t0 + E;
				const T_Vec diff1 = ((A * t1 + B) * t1 + C) * t1 + E;

				// d0 / d1: squared distance from p to point on curve at t0 / t1

				const T	d0	= diff0[0] * diff0[0] + diff0[1] * diff0[1];
				const T	d1	= diff1[0] * diff1[0] + diff1[1] * diff1[1];

				// Refine search interval towards minimum

				maxt = d0 < d1 ? t1 : maxt;
				mint = d0 < d1 ? mint : t0;

				const T newOffs = (maxt - mint) * oneOverGR;

				t0	= maxt - newOffs;
				t1	= mint + newOffs;
			}
		}

		//
		// Perform non-linear refinement over interval found by golden-section search
		//
		// For further info see:
		// https://www.shadertoy.com/view/4sXyDr
		//

		T	t = (mint + maxt) * 0.5f;

		{
			static const int NUM_REFINE_ITERS = 1;

			const T	a5 = T(6)  * A.Dot(A);
			const T	a4 = T(10) * A.Dot(B);
			const T	a3 = T(8)  * A.Dot(C) + T(4) * B.Dot(B);
			const T	a2 = T(6)  * A.Dot(D - p) + T(6) * B.Dot(C);
			const T	a1 = T(4)  * B.Dot(D - p) + T(2) * C.Dot(C);
			const T	a0 = T(2)  * C.Dot(D - p);

			for (int i = 0; i < NUM_REFINE_ITERS; i++)
			{
				// Halley's method

				const T f	= ((((a5 * t + a4) * t + a3) * t + a2) * t + a1) * t + a0;
				const T df	= (((5 * a5 * t + 4 * a4) * t + 3 * a3) * t + 2 * a2) * t + a1;
				const T ddf	= ((20 * a5 * t + 12 * a4) * t + 6 * a3) * t + 2 * a2;
				const T denom	= 2 * df * df - f * ddf;

				t = t - (2 * f * df) / denom;
			}
		}

		return Clamp<T>(t, 0, 1);
	}
	//*******************************************************************************************