C++ B2函数代码示例

void Bezier::recalc(QPointF p)
{
	// http://www.flong.com/texts/code/shapers_bez/
	// http://www.lemoda.net/maths/bezier-length/index.html,

	// arbitrary but reasonable t-values for interior control points
	double t0 = 0.3;
	double t1 = 0.7;

	if (m_drag_cp0) {
		double x = (p.x() - m_endpoint0.x() * B0(t0) - m_cp1.x() * B2(t0) - m_endpoint1.x() * B3(t0)) / B1(t0);
		double y = (p.y() - m_endpoint0.y() * B0(t0) - m_cp1.y() * B2(t0) - m_endpoint1.y() * B3(t0)) / B1(t0);
		m_cp0 = QPointF(x, y);
	}
	else {
		double x = (p.x() - m_endpoint0.x() * B0(t1) - m_cp0.x() * B1(t1) - m_endpoint1.x() * B3(t1)) /  B2(t1);
		double y = (p.y() - m_endpoint0.y() * B0(t1) - m_cp0.y() * B1(t1) - m_endpoint1.y() * B3(t1)) /  B2(t1);
		m_cp1 = QPointF(x, y);
	}
	
	/*
	DebugDialog::debug(QString("ix:%1 p0x:%2,p0y:%3 p1x:%4,p1y:%5 px:%6,py:%7")
							.arg(m_drag_cp0)
							.arg(m_endpoint0.x())
							.arg(m_endpoint0.y())
							.arg(m_endpoint1.x())
							.arg(m_endpoint1.y())
							.arg(p.x())
							.arg(p.y())
							);
	*/

	m_isEmpty = false;
}

mat ls_solve_od(const mat &A, const mat &B)
{
    int m=A.rows(), n=A.cols(), N=B.cols(), j;
    double beta;
    mat A2(A), B2(B), B3(n,N), submat, submat2;
    vec tmp(n), v;

//    it_assert1(m >= n, "The system is under-determined!");
    //it_assert1(m == B.rows(), "The number of rows in A must equal the number of rows in B!");

    // Perform a Householder QR factorization
    for (j=0; j<n; j++) {
	house(rvectorize(A2(j, m-1, j, j)), v, beta);
	v *= sqrt(beta);
	// 	submat.ref(A2, j,m-1, j,n-1);
 	submat = A2(j,m-1,j,n-1);
	sub_v_vT_m(submat, v);
	// 	submat.ref(B2, j,m-1, 0,N-1);
 	submat = B2(j,m-1,0,N-1);
	sub_v_vT_m(submat, v);
    }

    //    submat.ref(A2, 0,n-1,0,n-1);
    //    submat2.ref(B2, 0,n-1,0,N-1);
    submat = A2(0,n-1,0,n-1);
    submat2 = B2(0,n-1,0,N-1);
    for (j=0; j<N; j++) {
	backward_substitution(submat, submat2.get_col(j), tmp);
	B3.set_col(j, tmp);
    }
    
    return B3;
}

static int reallyroutespline (Pedge_t *edges, int edgen,
        Ppoint_t *inps, int inpn, Ppoint_t ev0, Ppoint_t ev1) {
    Ppoint_t p1, p2, cp1, cp2, p;
    Pvector_t v1, v2, splitv, splitv1, splitv2;
    double maxd, d, t;
    int maxi, i, spliti;

    static tna_t *tnas;
    static int tnan;

    if (tnan < inpn) {
        if (!tnas) {
            if (!(tnas = (tna_t *)malloc (sizeof (tna_t) * inpn)))
                return -1;
        } else {
            if (!(tnas = (tna_t *)realloc (tnas, sizeof (tna_t) * inpn)))
                return -1;
        }
        tnan = inpn;
    }
    tnas[0].t = 0;
    for (i = 1; i < inpn; i++)
        tnas[i].t = tnas[i - 1].t + dist (inps[i], inps[i - 1]);
    for (i = 1; i < inpn; i++)
        tnas[i].t /= tnas[inpn - 1].t;
    for (i = 0; i < inpn; i++) {
        tnas[i].a[0] = scale (ev0, B1 (tnas[i].t));
        tnas[i].a[1] = scale (ev1, B2 (tnas[i].t));
    }
    if (mkspline (inps, inpn, tnas, ev0, ev1, &p1, &v1, &p2, &v2) == -1)
        return -1;
    if (splinefits (edges, edgen, p1, v1, p2, v2, (inpn == 2 ? 1 : 0)))
        return 0;
    cp1 = add (p1, scale (v1, 1 / 3.0));
    cp2 = sub (p2, scale (v2, 1 / 3.0));
    for (maxd = -1, maxi = -1, i = 1; i < inpn - 1; i++) {
        t = tnas[i].t;
        p.x = B0 (t) * p1.x + B1 (t) * cp1.x +
                B2 (t) * cp2.x + B3 (t) * p2.x;
        p.y = B0 (t) * p1.y + B1 (t) * cp1.y +
                B2 (t) * cp2.y + B3 (t) * p2.y;
        if ((d = dist (p, inps[i])) > maxd)
            maxd = d, maxi = i;
    }
    spliti = maxi;
    splitv1 = normv (sub (inps[spliti], inps[spliti - 1]));
    splitv2 = normv (sub (inps[spliti + 1], inps[spliti]));
    splitv = normv (add (splitv1, splitv2));
    reallyroutespline (edges, edgen, inps, spliti + 1, ev0, splitv);
    reallyroutespline (edges, edgen, &inps[spliti], inpn - spliti, splitv, ev1);
    return 0;
}

// Shortest cubic spline through 4 on-curve points(chord approximation)
void Bezier::FitSpline(Vec3 p[])
{
	// use chord length for shortest(best) cubic spline approximation
	float c3 = (p[1] - p[0]).Magnitude();
	float c2 = (p[2] - p[1]).Magnitude();
	float c1 = (p[3] - p[2]).Magnitude();

	// cases where p[1] is close to p[2] might lead to instabilities(need some heuristic)
	if (50 * c2 < c1 + c3)
	{
		p[1] = p[0] + (p[1] - p[0]) * 0.98;
		p[2] = p[3] + (p[2] - p[3]) * 0.98;
		c3 = c3 * 0.98;
		c2 = (p[2] - p[1]).Magnitude();
		c1 = c1 * 0.98;
	}

	float t1 = c1 / (c1 + c2 + c3);
	float t2 = (c1 + c2) / (c1 + c2 + c3);

	// Solve M * x = y
	float m00 = B1(t1);
	float m01 = B2(t1);
	float m10 = B1(t2);
	float m11 = B2(t2);

	float detM = m00 * m11 - m01 * m10;
	if (fabs(detM) > 1E-3)
	{
		// y = p - p0 * B0(t) - p3 * B3(t)
		Vec3 y1 = p[1] - p[0] * B0(t1) - p[3] * B3(t1);
		Vec3 y2 = p[2] - p[0] * B0(t2) - p[3] * B3(t2);

		// Minv
		float s = 1 / detM;
		float n00 = s * m11;
		float n01 = -s * m01;
		float n10 = -s * m10;
		float n11 = s * m00;

		// x = Minv * y
		Vec3 x1 = y1 * n00 + y2 * n01;
		Vec3 x2 = y1 * n10 + y2 * n11;

		p[1] = x1;
		p[2] = x2;
	}
}

int read_toc(unsigned char *buf, int *buflen, int mode)
{
  int result;
  int len,i,o;


  result=scsi_request("read_toc",buf,buflen,10,0,SCSIR_READ,
		      READTOC,0,0,0,0,0,
		      0,
		      B2(*buflen),
		      0);

  if (result || !mode) return result;

  len=V2(&buf[0]); 
  printf("\nTracks: %d \t (first=%02d last=%02d)\n",
	(buf[3]-buf[2])+1,buf[2],buf[3]);
  
  for (i=0;i<((len-2)/8)-1;i++) {
    o=4+i*8; /* offset to track descriptor */
    printf("Track %02d: %s (adr/ctrl=%02xh) begin=%06d end=%06d  "
           "length<=%06d\n",
	   i+1,(buf[o+1]&DATA_TRACK?"data ":"audio"),buf[o+1],V4(&buf[o+4]),
	   V4(&buf[o+4+8]),V4(&buf[o+4+8])-V4(&buf[o+4]) );

  }

  return result;
}

void rubikStep(char *step)
{
	u8 m=0;
	for(m=0;step[m]!=0;m++)
	{
		switch(step[m])
		{
			case 7:allright90();break;
			case 11:F1();break;
			case 12:F2();break;
			case 13:F3();break;
			case 21:B1();break;
			case 22:B2();break;
			case 23:B3();break;
			case 31:R1();break;
			case 32:R2();break;
			case 33:R3();break;
			case 41:L1();break;
			case 42:L2();break;
			case 43:L3();break;
			case 51:U1();break;
			case 52:U2();break;
			case 53:U3();break;
			case 61:D1();break;
			case 62:D2();break;
			case 63:D3();break;
			default:break;
		}
	}
}

void f(void) {
	int b1 = B1();
	if (b1) {
		int b2 = B2();
		if (b2) {
			B3();
		} else {
			B4();
		}
	}
	B5();
	while (B6()) {
		B12();
		int b14;
		do {
			B13();
			b14 = B14();
		} while(b14);
		B15();
	}
	int b7 = B7();
	if (b7 || B8()) {
		B9();
	}
	B10();
	B11();
}

int mode_sense10(unsigned char *buf, int *buflen)
{
  int len = *buflen;
  if (len >65000) len=65000;
  return scsi_request("mode_sense(10)",buf,buflen,10,0,SCSIR_READ,
		      MODESENSE10,0,0x01,0,0,0,0,B2(len),0);
}

inline CPLSafeInt<unsigned> operator*( const CPLSafeInt<unsigned>& A,
                                       const CPLSafeInt<unsigned>& B )
{
#ifdef BUILTIN_OVERFLOW_CHECK_AVAILABLE
    unsigned res;
    if( __builtin_umul_overflow(A.v(), B.v(), &res) )
        throw CPLSafeIntOverflow();
    return CPLSM(res);
#elif defined(_MSC_VER)
    msl::utilities::SafeInt<unsigned, CPLMSVCSafeIntException> A2(A.v());
    msl::utilities::SafeInt<unsigned, CPLMSVCSafeIntException> B2(B.v());
    return CPLSM(static_cast<unsigned>(A2 * B2));
#elif defined(CPL_HAS_GINT64)
    const unsigned a = A.v();
    const unsigned b = B.v();
    const GUInt64 res = static_cast<GUInt64>(a) * b;
    if( res > std::numeric_limits<unsigned>::max() )
    {
        throw CPLSafeIntOverflow();
    }
    return CPLSM(static_cast<unsigned>(res));
#else
    const unsigned a = A.v();
    const unsigned b = B.v();
    if( b > 0 && a > std::numeric_limits<unsigned>::max() / b )
        throw CPLSafeIntOverflow();
    return CPLSM(a*b);
#endif
}

void	inter_cone(t_caster *caster, t_object *cone)
{
  double	a;
  double	b;
  double	c;
  double	delt;

  init_temp_pos(caster, cone);
  a = A2(caster->temp_vec.x, caster->temp_vec.y, caster->temp_vec.z,
	 cone->data.angle);
  b = B2(caster->temp_vec.x, caster->temp_pos.x, caster->temp_vec.y,
	 caster->temp_pos.y, caster->temp_vec.z, caster->temp_pos.z,
	 cone->data.angle);
  c = C2(caster->temp_pos.x, caster->temp_pos.y, caster->temp_pos.z,
	 cone->data.angle);
  delt = (pow(b, 2.0) - 4.0 * (a * c));
  if (delt >= 0.0)
    {
      cone->dist = get_nearest((-b - sqrt(delt)) / (2.0 * a),
			       (-b + sqrt(delt)) / (2.0 * a));
      if (cone->dist > 0.0 && cone->dist < caster->intersection.dist)
	{
	  caster->intersection.brightness = cone->brightness;
	  init_intersection(caster, cone);
	  rotate_caster(caster, cone);
	}
    }
}

static int
rvec_priv_inst( int dummy_rvec, ulong inst )
{
	int op, op_ext, b1, b2, b3;

	/* unhandled privileged instruction in supervisor mode */
	/* IMPORTANT: The GPRs are not available here! */

	op = OPCODE_PRIM( inst );
	op_ext = OPCODE_EXT( inst );
	b1 = B1( inst );	/* bit 6-10 */
	b2 = B2( inst );	/* bit 11-15 */
	b3 = B3( inst );	/* bit 16-20 */

	switch( OPCODE(op,op_ext) ) {
	case OPCODE( 31, 370 ):	/* tlbia (opt.) */
		/* not implemented on the 601,603,604,G3 (G4?) */
		break;
	case OPCODE( 31, 470 ):  /* dcbi rA,rB  -- rA=b2 rB=b3 */
		printm("dcbi treated as nop\n");
		mregs->nip += 4;
		return 0;
	default:
		printm("Unknown privileged instruction, opcode %lX\n", inst);
		stop_emulation();
		break;
	}
	mac_exception( 0x700, MOL_BIT(13) );
	return 0;
}

bool PGCicrcleTaskPt::CrossPoint(const ProjPt& prev, const ProjPt& next, ProjPt& optimized) {
    ProjPt A = prev - m_Center;
    ProjPt B = next - m_Center;
    ProjPt A2(A.m_X * A.m_X, A.m_Y * A.m_Y);
    ProjPt B2(B.m_X * B.m_X, B.m_Y * B.m_Y);
    double R2 = (m_Radius * m_Radius);

    bool PrevOutside = (A2.m_X + A2.m_Y) > R2;
    bool NextOutside = (B2.m_X + B2.m_Y) > R2;

    if (!PrevOutside && !NextOutside) {
        return false; // no cross point
    }

    ProjPt AB = B - A;

    double a = (AB.m_X * AB.m_X) + (AB.m_Y * AB.m_Y);
    double b = 2 * ((AB.m_X * A.m_X) + (AB.m_Y * A.m_Y));
    double c = A2.m_X + A2.m_Y - R2;

    double bb4ac = (b * b) -(4 * a * c);
    if (bb4ac < 0.0) {
        return false;
    }

    bool bCrossPoint = false;
    double k = 0.0;
    if (bb4ac == 0.0) {
        LKASSERT(a);
        // one point
        k = -b / (2 * a);
        bCrossPoint = true;
    }

    if (bb4ac > 0.0) {
        // Two point,
        if ((PrevOutside && m_bExit) || (!PrevOutside && NextOutside)) {
            LKASSERT(a);
            k = (-b + sqrt(bb4ac)) / (2 * a); // ouput : prev ouside && Exit TP || prev inside && next outside
            bCrossPoint = true;
        } else {
            LKASSERT(a);
            k = (-b - sqrt(bb4ac)) / (2 * a); // input : prev outside && Enter TP
            bCrossPoint = true;
        }
    }

    if (bCrossPoint) {
        ProjPt O = prev + ((next - prev) * k);
        if (dot_product((next - prev), O - prev) > 0.0 &&
                dot_product((prev - next), O - next) > 0.0) {
            optimized = O;
            return true;
        }
    }

    // no point
    return false;
}

Matrix genA0(Matrix &A,vector<long>&JROW,vector<long>&JCOL)
{
  long M = A.size();
  Matrix B(M), B2(M);
  forMatrix(A,i,j) B[JROW[i]-1][j]  = A[i][j];
  forMatrix(B,i,j) B2[i][JCOL[j]-1] = B[i][j];
  return B2;
}

int main()
{
	Rat a1(-1,2);
	Rat a2(2,9);
	Rat b2(3,2);
	Rat c1(-1,9);

	DoubleInterval A11(a2,b2);
	DoubleInterval A12(a1,0);
	DoubleInterval A21(-1,c1);
	DoubleInterval A22(a2,b2);

	DoubleMatrix A(2,2);
	A(0,0) = A11;
	A(0,1) = A12;
	A(1,0) = A21;
	A(1,1) = A22;

	DoubleInterval B1(1,3);
	DoubleInterval B2(3,4);

	DoubleVector b(2, (DoubleInterval)0);
	b[0] = B1;
	b[1] = B2;

	std::cout << A << std::endl;
	std::cout << b << std::endl;

	DoubleVector x(2, (DoubleInterval)0);
	x = A * b;

	std::cout << x << std::endl;

	//Need to sort this out
	DoubleInterval test(-1,2);
	DoubleInterval testb(5,100);

	DoubleInterval ans;
	ans = testb/test;

	std::cout << ans << std::endl;

	DoubleInterval k00(0,2);
	DoubleInterval k01(1,3);
	DoubleInterval k10(3,5);
	DoubleInterval k11(5,7);

	std::cout << "INVERSE IS " << std::endl;
	std::cout << boost::numeric::Doubleinterval_lib::multiplicative_inverse(k11) << std::endl;

	std::cout << boost::numeric::norm(k11) << std::endl;

	std::cout << k00 * k11 << std::endl;
	std::cout << k01 * k10 << std::endl;
	std::cout << (k00 * k11) - (k01 * k10) << std::endl;

	return 0;
}

uint8 IRsense(void){

    uint8 tmp;
    DIRIN(TRISD,BIT2);
    Nop();Nop();Nop();Nop();
    tmp=PORTD;
    DIROUT(TRISC,BIT2);


    switch(irsta){
   
        case 1:
            psta=0;
            B2(LATC)=0;
            if(irsense==1) {oncnt=0;irsta=2;}
          //  else {irsta=1;}
            break;

        case 2:
            oncnt++;
            if(oncnt>t1000) {irsta=3;}
            if(irsense==0) {irsta=1;}
            break;


        case 3:
            psta=1;
            B2(LATC)=1;
            if(irsense==0) {irsta=4;offcnt=0;}
            break;

        case 4:
            offcnt++;
            if(offcnt>t1000) {irsta=1;}
            if(irsense==1) {irsta=3;}
           
            break;

        default: irsta=1; break;

     
    }
    return irsta;
}