C++ Bezier类代码示例

double Bernsteins::secant(Bezier bz) {
    double s = 0, t = 1;
    double e = 1e-14;
    int side = 0;
    double r, fr, fs = bz.at0(), ft = bz.at1();

    for (size_t n = 0; n < 100; ++n)
    {
        r = (fs*t - ft*s) / (fs - ft);
        if (fabs(t-s) < e * fabs(t+s)) {
            debug(std::cout << "error small " << fabs(t-s) 
                      << ", accepting solution " << r 
                  << "after " << n << "iterations\n");
            return r;
        }

        fr = horner(bz, r);

        if (fr * ft > 0)
        {
            t = r; ft = fr;
            if (side == -1) fs /= 2;
            side = -1;
        }
        else if (fs * fr > 0)
        {
            s = r;  fs = fr;
            if (side == +1) ft /= 2;
            side = +1;
        }
        else break;
    }
    return r;
}

void draw_surface(float width, float height)
{
	/* pseudo
	1. calculate p3-p2
	2. set new bezier p0 = old bezier p3
	3. new beier p1 = p0 + old(p3-p2)*/

	Bezier tl = Bezier(width, height, angle);
	Vector4 d1 = tl.p3 - tl.p2;
	Vector4 d2 = tl.p7 - tl.p6;
	Vector4 d3 = tl.p11 - tl.p10;
	Vector4 d4 = tl.p15 - tl.p14;
	Vector4 g1 = tl.p3 + d1;
	Vector4 g2 = tl.p7 + d2;
	Vector4 g3 = tl.p11 + d3;
	Vector4 g4 = tl.p15 + d4;
	cout << "tlp7: " << tl.p7.y << endl;
	Bezier tr = Bezier(tl.p3, g1, tl.p7, g2,
						tl.p11, g3, tl.p15, g4, angle);
	//cout << "p0: " << tr.p0.y << " p3: " << tr.p3.y << " p12: " << tr.p12.y << " p15: " << tr.p15.y << endl;
	cout << "p4: " << tr.p4.y << " p8: " << tr.p8.y << " p7: " << tr.p7.y << " p11: " << tr.p11.y << endl;
	cout << "p5: " << tr.p5.y << endl;
	for (float t1 = -0.5; t1 < 0.49; t1 += 0.01)
	{
		for (float t2 = -0.5; t2 < 0.49; t2 += 0.01)
		{
			glColor3f(1, 0, 0);
			tl.tessellate(t1, t2, .1);
			glColor3f(0, 0, 1);
			tr.tessellate(t1, t2, .1);
		}
	}
}

	void ShapeMaker::cubicTo (const Bezier &cubic)
	{
		const double sqrt3 = 1.7320508075688772935274463415059;
		const double precision = 600 * 18 / (60 * factorx * sqrt3);

		// distance between control points of quadratic approximations of either end
		double D01 = (cubic.p1 - (cubic.c1 - cubic.c0) * 3 - cubic.p0).magnitude () / 2;

		double tMax3 = precision / D01;
		if (tMax3 >= 1)
		{
			curveTo (cubic.quadraticCtrl(), cubic.p1);
			return;
		}

		Bezier end(cubic);

		if (tMax3 >= 0.5 * 0.5 * 0.5)
		{
			Bezier start (end.split (0.5));
			curveTo (start.quadraticCtrl (), start.p1);
		}
		else
		{
			double tMax = pow (tMax3, 1.0/3.0);
			Bezier start (end.split (tMax));
			Bezier middle (end.split (1 - tMax / (1 - tMax)));

			curveTo (start.quadraticCtrl (), start.p1);
			cubicTo (middle);
		}

		curveTo (end.quadraticCtrl (), end.p1);
	}

CubicSpline::CubicSpline(vector<Vector3> controlPoints, float tightness, int subDivisions) : m_length(0.f)
{
	Vector3 endTangent;
	for (int i = 0; i < controlPoints.size()-1; i++) {
		vector<Vector3> path;
		Vector3 start = controlPoints[i];
		Vector3 end = controlPoints[i+1];
		path.push_back(start);
		if (endTangent != Vector3::Zero) {
			// start tangent becomes the negative of the previous end tangent
			path.push_back(-endTangent + start);
		}
		
		
		if (i < controlPoints.size() - 2) {
			Vector3 next = controlPoints[i + 2];

			Vector3 line1 = start - end;
			Vector3 line2 = end - next;

			line1.Normalize();
			line2.Normalize();
			// average the two vectors to get the normal of reflection
			endTangent = Vector3((line1.x + line2.x) / 2.f, (line1.y + line2.y) / 2.f, (line1.z + line2.z) / 2.f);
			//endTangent.Normalize();
			endTangent *= tightness;
			path.push_back(endTangent + end);
		}
		path.push_back(end);
		Bezier bezier = Bezier(path);
		float length = bezier.Length(subDivisions);
		m_length += length;
		m_bezierSections.push_back(std::make_pair(Bezier(bezier), length));
	}
}

CtrlSliderBg::CtrlSliderBg( intf_thread_t *pIntf,
                            const Bezier &rCurve, VarPercent &rVariable,
                            int thickness, GenericBitmap *pBackground,
                            int nbHoriz, int nbVert, int padHoriz, int padVert,
                            VarBool *pVisible, const UString &rHelp ):
    CtrlGeneric( pIntf, rHelp, pVisible ), m_pCursor( NULL ),
    m_rVariable( rVariable ), m_thickness( thickness ), m_rCurve( rCurve ),
    m_width( rCurve.getWidth() ), m_height( rCurve.getHeight() ),
    m_pImgSeq( pBackground ), m_nbHoriz( nbHoriz ), m_nbVert( nbVert ),
    m_padHoriz( padHoriz ), m_padVert( padVert ), m_bgWidth( 0 ),
    m_bgHeight( 0 ), m_position( 0 )
{
    if( pBackground )
    {
        // Build the background image sequence
        // Note: we suppose that the last padding is not included in the
        // given image
        // TODO: we should probably change this assumption, as it would make
        // the code a bit simpler and it would be more natural for the skins
        // designers
        m_bgWidth = (pBackground->getWidth() + m_padHoriz) / nbHoriz;
        m_bgHeight = (pBackground->getHeight() + m_padVert) / nbVert;

        // Observe the position variable
        m_rVariable.addObserver( this );

        // Initial position
        m_position = (int)( m_rVariable.get() * (m_nbHoriz * m_nbVert - 1) );
    }
}

void Bezier::attach(Bezier& other, bool reverse) {
	nextPatch = &other;

	// Calculate road radius as the connection of this and the next patch
	MathVector<float, 3> a = surfCoord(0.5, 0.0), b = surfCoord(0.5, 1.0), c = other.surfCoord(0.5, 1.0);
	if (reverse) { a = surfCoord(0.5, 1.0); b = surfCoord(0.5, 0.0); c = other.surfCoord(0.5, 0.0); }

	MathVector<float, 3> d1 = a - b, d2 = c - b;
	float diff = d2.magnitude() - d1.magnitude();
	float dd = ((d1.magnitude() < 0.0001) || (d2.magnitude() < 0.0001)) ?
			     0.f : d1.normalized().dot(d2.normalized());
	float angle = acosf((dd >= 1.f) ? 1.f :(dd <= -1.f) ? -1.f : dd);
	float d1d2mag = d1.magnitude() + d2.magnitude();
	float alpha = (d1d2mag < 0.0001) ? 0.f : (float) (M_PI * diff + 2.f * d1.magnitude() * angle) / d1d2mag / 2.f;

	if (fabs(alpha - M_PI/2.0) < 0.001) roadRadius = 10000.0;
	else roadRadius = d1.magnitude() / 2.f / cosf(alpha);

	if (d1.magnitude() < 0.0001) roadCurvature = 0.0;
	else roadCurvature = 2.f * cosf(alpha) / d1.magnitude();

	// Determine it's a left or right turn at the connection
	MathVector<float, 3> d = d1.cross(d2);
	if (fabs(d[0]) < 0.1 && fabs(d[1]) < 0.1 && fabs(d[2]) < 0.1) turn = 0; // Straight road ahead
	else if (d[1] > 0.0) turn = -1; // Left turn ahead
	else turn = 1; // Right turn ahead

	// Calculate distance from start of the road
	length = d1.magnitude();
}

float AiCarStandard::GetHorizontalDistanceAlongPatch(const Bezier & patch, Vec3 carposition)
{
	Vec3 leftside = (patch.GetPoint(0,0) + patch.GetPoint(3,0))*0.5;
	Vec3 rightside = (patch.GetPoint(0,3) + patch.GetPoint(3,3))*0.5;
	Vec3 patchwidthvector = rightside - leftside;
	return patchwidthvector.Normalize().dot(carposition-leftside);
}

void convex_hull_marching(Bezier src_bz, Bezier bz,
                          std::vector<double> &solutions,
                          double left_t,
                          double right_t)
{
    while(bz.order() > 0 and bz[0] == 0) {
        std::cout << "deflate\n";
        bz = bz.deflate();
        solutions.push_back(left_t);
    }
    if (bz.order() > 0) {
    
        int old_sign = SGN(bz[0]);
    
        int sign;
        double left_bound = 0;
        double dt = 0;
        for (size_t i = 1; i < bz.size(); i++)
        {
            sign = SGN(bz[i]);
            if (sign != old_sign)
            {
                dt = double(i) / bz.order();
                left_bound = dt * bz[0] / (bz[0] - bz[i]);
                break;
            }
            old_sign = sign;
        }
        if (dt == 0) return;
        std::cout << bz << std::endl;
        std::cout << "dt = " << dt << std::endl;
        std::cout << "left_t = " << left_t << std::endl;
        std::cout << "right_t = " << right_t << std::endl;
        std::cout << "left bound = " << left_bound 
                  << " = " << bz(left_bound) << std::endl; 
        double new_left_t = left_bound * (right_t - left_t) + left_t;
        std::cout << "new_left_t = " << new_left_t << std::endl;
        Bezier bzr = subRight(src_bz, new_left_t);
        while(bzr.order() > 0 and bzr[0] == 0) {
            std::cout << "deflate\n";
            bzr = bzr.deflate();
            solutions.push_back(new_left_t);
        }
        if (left_t < new_left_t) {
            convex_hull_marching(src_bz, bzr,
                                 solutions,
                                 new_left_t, right_t); 
        } else {
            std::cout << "epsilon reached\n";
            while(bzr.order() > 0 and fabs(bzr[0]) <= 1e-10) {
                std::cout << "deflate\n";
                bzr = bzr.deflate();
                std::cout << bzr << std::endl;
                solutions.push_back(new_left_t);
            }

        }
    }
}

bool Bezier::operator!=(const Bezier & other) const
{
	if (m_isEmpty != other.isEmpty()) return true;
	if (m_cp0 != other.cp0()) return true;
	if (m_cp1 != other.cp1()) return true;

	return false;
}

CtrlSliderBg::CtrlSliderBg( intf_thread_t *pIntf, CtrlSliderCursor &rCursor,
                            const Bezier &rCurve, VarPercent &rVariable,
                            int thickness, VarBool *pVisible,
                            const UString &rHelp ):
    CtrlGeneric( pIntf, rHelp, pVisible ), m_rCursor( rCursor ),
    m_rVariable( rVariable ), m_thickness( thickness ), m_rCurve( rCurve ),
    m_width( rCurve.getWidth() ), m_height( rCurve.getHeight() )
{
}

bool are_equal(Bezier A, Bezier B) {
    int maxSize = max(A.size(), B.size());
    double t = 0., dt = 1./maxSize;
    
    for(int i = 0; i <= maxSize; i++) {
        EXPECT_FLOAT_EQ(A.valueAt(t), B.valueAt(t));// return false;
        t += dt;
    }
    return true;
}

/** Changes the basis of p to be bernstein.
 \param p the Symmetric basis polynomial
 \returns the Bernstein basis polynomial

 if the degree is even q is the order in the symmetrical power basis,
 if the degree is odd q is the order + 1
 n is always the polynomial degree, i. e. the Bezier order
 sz is the number of bezier handles.
*/
void sbasis_to_bezier (Bezier & bz, SBasis const& sb, size_t sz)
{
    if (sb.size() == 0) {
        THROW_RANGEERROR("size of sb is too small");
    }

    size_t q, n;
    bool even;
    if (sz == 0)
    {
        q = sb.size();
        if (sb[q-1][0] == sb[q-1][1])
        {
            even = true;
            --q;
            n = 2*q;
        }
        else
        {
            even = false;
            n = 2*q-1;
        }
    }
    else
    {
        q = (sz > 2*sb.size()-1) ?  sb.size() : (sz+1)/2;
        n = sz-1;
        even = false;
    }
    bz.clear();
    bz.resize(n+1);
    double Tjk;
    for (size_t k = 0; k < q; ++k)
    {
        for (size_t j = k; j < n-k; ++j) // j <= n-k-1
        {
            Tjk = binomial(n-2*k-1, j-k);
            bz[j] += (Tjk * sb[k][0]);
            bz[n-j] += (Tjk * sb[k][1]); // n-k <-> [k][1]
        }
    }
    if (even)
    {
        bz[q] += sb[q][0];
    }
    // the resulting coefficients are with respect to the scaled Bernstein
    // basis so we need to divide them by (n, j) binomial coefficient
    for (size_t j = 1; j < n; ++j)
    {
        bz[j] /= binomial(n, j);
    }
    bz[0] = sb[0][0];
    bz[n] = sb[0][1];
}

// suggested by Sederberg.
double Bernsteins::horner(Bezier bz, double t)
{
    double u, tn, tmp;
    u = 1.0 - t;
    tn = 1.0;
    tmp = bz.at0() * u;
    for(size_t i = 1; i < bz.degree(); ++i)
    {
        tn *= t;
        tmp = (tmp + tn*choose<double>(bz.order(), (unsigned)i)*bz[i]) * u;
    }
    return (tmp + tn*t*bz.at1());
}

/** Find all t s.t s(t) = 0
 \param a sbasis function
 \returns vector of zeros (roots)

*/
std::vector<double> roots(SBasis const & s) {
    switch(s.size()) {
        case 0:
            return std::vector<double>();
        case 1:
            return roots1(s);
        default:
        {
            Bezier bz;
            sbasis_to_bezier(bz, s);
            return bz.roots();
        }
    }
}

Bezier Bezier::join(const Bezier * other) const
{
	Bezier bezier;
	bool otherIsEmpty = (other == NULL || other->isEmpty());

	if (isEmpty() && otherIsEmpty) {
		return bezier;
	}
	else {
		if (isEmpty()) {
			bezier.set_cp0(m_endpoint0);
			bezier.set_cp1(other->cp1());
		}
		else if (other->isEmpty()) {
			bezier.set_cp1(other->m_endpoint1);
			bezier.set_cp0(cp0());
		}
		else {
			bezier.set_cp0(cp0());
			bezier.set_cp1(other->cp1());
		}
	}

	return bezier;
}