void testCG(){
  int N = 100;
  //read n=100 spd matrix from file
  float A[N*N];
  readMatrix(A, N);
  
  //run cg on it to see if it converges
  
  
  //When porting cg to opencl  only the mtx-vec multiply and the
  //dot-product(reduction) are run on the GPU everything is on the GPU
  int i = 0;
  int imax = 1000;
  float tol = 0.000001f;
  float r[N];
  float b[N];
  float d[N];
  float x[N];
  float q[N];
 
  for(int i = 0; i < N; i++){
    x[i] = 0.0f;
    b[i] = rand()/(float)RAND_MAX * 100.0f;
    r[i] = b[i];
    d[i] = r[i];
  }
  
  float rnew = dot(r,r,N);
  float rold = 0.0f;

  float r0 = rnew;
  //while(i < imax && rnew > 0.0000001*r0) {
  while(i < imax && rnew > tol) {
    mtx_times_vec(q,A,d,N);
    float alpha = rnew/(dot(d,q,N));
    
    for(int j = 0; j < N; j++){
      x[j] += alpha*d[j];
    }
    
    for(int j = 0; j < N; j++){
      r[j] -= alpha*q[j];
    }
    
    rold = rnew;
    rnew = dot(r,r,N);
    
    float beta = rnew/rold;
    
    for(int j = 0; j < N; j++){
      d[j] = r[j] + beta*d[j];
    }
    
    i++;
  }
  
  //Check the answer
  float ax[N];
  int goodMatrix = 1;
  mtx_times_vec(ax,A,x,N);
  for(int i = 0; i < N; i++){
    float diff = ax[i] - b[i];
    if(fabs(diff) > 0.01f){
      goodMatrix = 0;
      printf("CG Result mismatch at idx %d ax[%d] = %3.6f != b[%d] = %3.6f\n", i,i,ax[i],i,b[i]);
    }
  }
  if(goodMatrix){
    printf("CG Converged and Solved Ax=b\n");
  }else{
    printf("CG did not solve Ax=b\n");
  }

  printf("CG Terminated with iterations %d, and rnew %3.6f\n",i, rnew);
  
  
  
}

	/// Helper function: reflect \c wi with respect to a given surface normal
	inline Vector reflect(const Vector &wi, const Normal &m) const {
		return 2 * dot(wi, m) * Vector(m) - wi;
	}

//.........这里部分代码省略.........
      j1=0;
      k1=0;
      i2=1;
      j2=0;
      k2=1;
    }
    else
    {
      i1=0;  // Z X Y order
      j1=0;
      k1=1;
      i2=1;
      j2=0;
      k2=1;
    }
  }
  else   // x0<y0
  {
    if(y0<z0)
    {
      i1=0;  // Z Y X order
      j1=0;
      k1=1;
      i2=0;
      j2=1;
      k2=1;
    }
    else if(x0<z0)
    {
      i1=0;  // Y Z X order
      j1=1;
      k1=0;
      i2=0;
      j2=1;
      k2=1;
    }
    else
    {
      i1=0;  // Y X Z order
      j1=1;
      k1=0;
      i2=1;
      j2=1;
      k2=0;
    }
  }
//  A step of (1,0,0) in (i,j,k) means a step of (1-c,-c,-c) in (x,y,z),
//  a step of (0,1,0) in (i,j,k) means a step of (-c,1-c,-c) in (x,y,z), and
//  a step of (0,0,1) in (i,j,k) means a step of (-c,-c,1-c) in (x,y,z), where
//  c = 1/6.
  double   x1 = x0 - i1 + G3; // Offsets for second corner in (x,y,z) coords
  double   y1 = y0 - j1 + G3;
  double   z1 = z0 - k1 + G3;
  double   x2 = x0 - i2 + 2.0*G3; // Offsets for third corner in (x,y,z) coords
  double   y2 = y0 - j2 + 2.0*G3;
  double   z2 = z0 - k2 + 2.0*G3;
  double   x3 = x0 - 1.0 + 3.0*G3; // Offsets for last corner in (x,y,z) coords
  double   y3 = y0 - 1.0 + 3.0*G3;
  double   z3 = z0 - 1.0 + 3.0*G3;
  // Work out the hashed gradient indices of the four simplex corners
  int ii = i & 255;
  int jj = j & 255;
  int kk = k & 255;
  int gi0 = perm[ii+perm[jj+perm[kk]]] % 12;
  int gi1 = perm[ii+i1+perm[jj+j1+perm[kk+k1]]] % 12;
  int gi2 = perm[ii+i2+perm[jj+j2+perm[kk+k2]]] % 12;
  int gi3 = perm[ii+1+perm[jj+1+perm[kk+1]]] % 12;
  // Calculate the contribution from the four corners
  double t0 = 0.6 - x0*x0 - y0*y0 - z0*z0;
  if(t0<0) n0 = 0.0;
  else
  {
    t0 *= t0;
    n0 = t0 * t0 * dot(grad3[gi0], x0, y0, z0);
  }
  double t1 = 0.6 - x1*x1 - y1*y1 - z1*z1;
  if(t1<0) n1 = 0.0;
  else
  {
    t1 *= t1;
    n1 = t1 * t1 * dot(grad3[gi1], x1, y1, z1);
  }
  double t2 = 0.6 - x2*x2 - y2*y2 - z2*z2;
  if(t2<0) n2 = 0.0;
  else
  {
    t2 *= t2;
    n2 = t2 * t2 * dot(grad3[gi2], x2, y2, z2);
  }
  double t3 = 0.6 - x3*x3 - y3*y3 - z3*z3;
  if(t3<0) n3 = 0.0;
  else
  {
    t3 *= t3;
    n3 = t3 * t3 * dot(grad3[gi3], x3, y3, z3);
  }
  // Add contributions from each corner to get the final noise value.
  // The result is scaled to stay just inside [-1,1]
  return 32.0*(n0 + n1 + n2 + n3);
}

// 2D raw Simplex noise
double raw_noise_2d( const double x, const double y ) {
    // Noise contributions from the three corners
    double n0, n1, n2;

    // Skew the input space to determine which simplex cell we're in
    double F2 = 0.5 * (sqrt(3.0) - 1.0);
    // Hairy factor for 2D
    double s = (x + y) * F2;
    int i = fastfloor( x + s );
    int j = fastfloor( y + s );

    double G2 = (3.0 - sqrt(3.0)) / 6.0;
    double t = (i + j) * G2;
    // Unskew the cell origin back to (x,y) space
    double X0 = i-t;
    double Y0 = j-t;
    // The x,y distances from the cell origin
    double x0 = x-X0;
    double y0 = y-Y0;

    // For the 2D case, the simplex shape is an equilateral triangle.
    // Determine which simplex we are in.
    int i1, j1; // Offsets for second (middle) corner of simplex in (i,j) coords
    if(x0>y0) {i1=1; j1=0;} // lower triangle, XY order: (0,0)->(1,0)->(1,1)
    else {i1=0; j1=1;} // upper triangle, YX order: (0,0)->(0,1)->(1,1)

    // A step of (1,0) in (i,j) means a step of (1-c,-c) in (x,y), and
    // a step of (0,1) in (i,j) means a step of (-c,1-c) in (x,y), where
    // c = (3-sqrt(3))/6
    double x1 = x0 - i1 + G2; // Offsets for middle corner in (x,y) unskewed coords
    double y1 = y0 - j1 + G2;
    double x2 = x0 - 1.0 + 2.0 * G2; // Offsets for last corner in (x,y) unskewed coords
    double y2 = y0 - 1.0 + 2.0 * G2;

    // Work out the hashed gradient indices of the three simplex corners
    int ii = i & 255;
    int jj = j & 255;
    int gi0 = perm[ii+perm[jj]] % 12;
    int gi1 = perm[ii+i1+perm[jj+j1]] % 12;
    int gi2 = perm[ii+1+perm[jj+1]] % 12;

    // Calculate the contribution from the three corners
    double t0 = 0.5 - x0*x0-y0*y0;
    if(t0<0) n0 = 0.0;
    else {
        t0 *= t0;
        n0 = t0 * t0 * dot(grad3[gi0], x0, y0); // (x,y) of grad3 used for 2D gradient
    }

    double t1 = 0.5 - x1*x1-y1*y1;
    if(t1<0) n1 = 0.0;
    else {
        t1 *= t1;
        n1 = t1 * t1 * dot(grad3[gi1], x1, y1);
    }

    double t2 = 0.5 - x2*x2-y2*y2;
    if(t2<0) n2 = 0.0;
    else {
        t2 *= t2;
        n2 = t2 * t2 * dot(grad3[gi2], x2, y2);
    }

    // Add contributions from each corner to get the final noise value.
    // The result is scaled to return values in the interval [-1,1].
    return 70.0 * (n0 + n1 + n2);
}

//-----------------------------------------------------------------------------
// LLKeyframeStandMotion::onUpdate()
//-----------------------------------------------------------------------------
BOOL LLKeyframeStandMotion::onUpdate(F32 time, U8* joint_mask)
{
	//-------------------------------------------------------------------------
	// let the base class update the cycle
	//-------------------------------------------------------------------------
	BOOL status = LLKeyframeMotion::onUpdate(time, joint_mask);
	if (!status)
	{
		return FALSE;
	}

	LLVector3 root_world_pos = mPelvisState->getJoint()->getParent()->getWorldPosition();

	// have we received a valid world position for this avatar?
	if (root_world_pos.isExactlyZero())
	{
		return TRUE;
	}

	//-------------------------------------------------------------------------
	// Stop tracking (start locking) ankles once ease in is done.
	// Setting this here ensures we track until we get valid foot position.
	//-------------------------------------------------------------------------
	if (dot(mPelvisState->getJoint()->getWorldRotation(), mLastGoodPelvisRotation) < ROTATION_THRESHOLD)
	{
		mLastGoodPelvisRotation = mPelvisState->getJoint()->getWorldRotation();
		mLastGoodPelvisRotation.normalize();
		mTrackAnkles = TRUE;
	}
	else if ((mCharacter->getCharacterPosition() - mLastGoodPosition).magVecSquared() > POSITION_THRESHOLD)
	{
		mLastGoodPosition = mCharacter->getCharacterPosition();
		mTrackAnkles = TRUE;
	}
	else if (mPose.getWeight() < 1.f)
	{
		mTrackAnkles = TRUE;
	}


	//-------------------------------------------------------------------------
	// propagate joint positions to internal versions
	//-------------------------------------------------------------------------
	mPelvisJoint.setPosition(
			root_world_pos +
			mPelvisState->getPosition() );

	mHipLeftJoint.setPosition( mHipLeftState->getJoint()->getPosition() );
	mKneeLeftJoint.setPosition( mKneeLeftState->getJoint()->getPosition() );
	mAnkleLeftJoint.setPosition( mAnkleLeftState->getJoint()->getPosition() );

	mHipLeftJoint.setScale( mHipLeftState->getJoint()->getScale() );
	mKneeLeftJoint.setScale( mKneeLeftState->getJoint()->getScale() );
	mAnkleLeftJoint.setScale( mAnkleLeftState->getJoint()->getScale() );

	mHipRightJoint.setPosition( mHipRightState->getJoint()->getPosition() );
	mKneeRightJoint.setPosition( mKneeRightState->getJoint()->getPosition() );
	mAnkleRightJoint.setPosition( mAnkleRightState->getJoint()->getPosition() );

	mHipRightJoint.setScale( mHipRightState->getJoint()->getScale() );
	mKneeRightJoint.setScale( mKneeRightState->getJoint()->getScale() );
	mAnkleRightJoint.setScale( mAnkleRightState->getJoint()->getScale() );
	//-------------------------------------------------------------------------
	// propagate joint rotations to internal versions
	//-------------------------------------------------------------------------
	mPelvisJoint.setRotation( mPelvisState->getJoint()->getWorldRotation() );

#if GO_TO_KEY_POSE
	mHipLeftJoint.setRotation( mHipLeftState->getRotation() );
	mKneeLeftJoint.setRotation( mKneeLeftState->getRotation() );
	mAnkleLeftJoint.setRotation( mAnkleLeftState->getRotation() );

	mHipRightJoint.setRotation( mHipRightState->getRotation() );
	mKneeRightJoint.setRotation( mKneeRightState->getRotation() );
	mAnkleRightJoint.setRotation( mAnkleRightState->getRotation() );
#else
	mHipLeftJoint.setRotation( mHipLeftState->getJoint()->getRotation() );
	mKneeLeftJoint.setRotation( mKneeLeftState->getJoint()->getRotation() );
	mAnkleLeftJoint.setRotation( mAnkleLeftState->getJoint()->getRotation() );

	mHipRightJoint.setRotation( mHipRightState->getJoint()->getRotation() );
	mKneeRightJoint.setRotation( mKneeRightState->getJoint()->getRotation() );
	mAnkleRightJoint.setRotation( mAnkleRightState->getJoint()->getRotation() );
#endif

	// need to wait for underlying keyframe motion to affect the skeleton
	if (mFrameNum == 2)
	{
		mIKLeft.setupJoints( &mHipLeftJoint, &mKneeLeftJoint, &mAnkleLeftJoint, &mTargetLeft );
		mIKRight.setupJoints( &mHipRightJoint, &mKneeRightJoint, &mAnkleRightJoint, &mTargetRight );
	}
	else if (mFrameNum < 2)
	{
		mFrameNum++;
		return TRUE;
	}

//.........这里部分代码省略.........

/* task that renders a single screen tile */
Vec3fa renderPixelStandard(float x, float y, const Vec3fa& vx, const Vec3fa& vy, const Vec3fa& vz, const Vec3fa& p)
{
  float weight = 1.0f;
  Vec3fa color = Vec3fa(0.0f);

  /* initialize ray */
  RTCRay2 primary;
  primary.org = p;
  primary.dir = normalize(x*vx + y*vy + vz);
  primary.tnear = 0.0f;
  primary.tfar = inf;
  primary.geomID = RTC_INVALID_GEOMETRY_ID;
  primary.primID = RTC_INVALID_GEOMETRY_ID;
  primary.mask = -1;
  primary.time = 0;
  primary.transparency = 0.0f;

  while (true)
  {
    /* intersect ray with scene */
    rtcIntersect(g_scene,*((RTCRay*)&primary)); // FIXME: use (RTCRay&) cast
    
    /* shade pixels */
    if (primary.geomID == RTC_INVALID_GEOMETRY_ID) 
      break;

    float opacity = 1.0f-primary.transparency;
    Vec3fa diffuse = colors[primary.primID];
    Vec3fa La = diffuse*0.5f;
    color = color + weight*opacity*La; // FIXME: +=
    Vec3fa lightDir = normalize(Vec3fa(-1,-1,-1));
      
    /* initialize shadow ray */
    RTCRay2 shadow;
    shadow.org = primary.org + primary.tfar*primary.dir;
    shadow.dir = neg(lightDir);
    shadow.tnear = 0.001f;
    shadow.tfar = inf;
    shadow.geomID = RTC_INVALID_GEOMETRY_ID;
    shadow.primID = RTC_INVALID_GEOMETRY_ID;
    shadow.mask = -1;
    shadow.time = 0;
    shadow.transparency = 1.0f;
    
    /* trace shadow ray */
    rtcOccluded(g_scene,*((RTCRay*)&shadow)); // FIXME: use (RTCRay&) cast
    
    /* add light contribution */
    if (shadow.geomID) {
      Vec3fa Ll = diffuse*shadow.transparency*clamp(-dot(lightDir,normalize(primary.Ng)),0.0f,1.0f);
      color = color + weight*opacity*Ll; // FIXME: +=
    }

    /* shoot transmission ray */
    weight *= primary.transparency;
    primary.tnear = 1.001f*primary.tfar;
    primary.tfar = inf;
    primary.geomID = RTC_INVALID_GEOMETRY_ID;
    primary.primID = RTC_INVALID_GEOMETRY_ID;
    primary.transparency = 0.0f;
  }
  return color;
}

/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // ----------------------------
    DifferentialState    x("", 10, 1);    // a differential state vector with dimension 10. (vector)
    DifferentialState    y    ;    // another differential state y                   (scalar)
    Control              u("", 2, 1);    // a control input with dimension 2.              (vector)
    Parameter            p    ;    // a parameter (here a scalar).                   (scalar)

    DifferentialEquation f    ;    // the differential equation

    const double t_start =  0.0;
    const double t_end   =  1.0;


    // READ A MATRIX "A" FROM A FILE:
    // ------------------------------
    DMatrix A; A.read( "matrix_vector_ocp_A.txt" );
    DMatrix B; B.read( "matrix_vector_ocp_B.txt" );


    // READ A VECTOR "x0" FROM A FILE:
    // -------------------------------
    DVector x0; x0.read( "matrix_vector_ocp_x0.txt" );


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x) == -(A*x) + B*u;                           // matrix vector notation for a linear equation
    f << dot(y) == x.transpose()*x + 2.0*u.transpose()*u;  // matrix vector notation:  x^x  = scalar product = ||x||_2^2
                                                           //                          u^u  = scalar product = ||u||_2^2


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 20 );
    ocp.minimizeMayerTerm( y );
    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x == x0  );
    ocp.subjectTo( AT_START, y == 0.0 );


    GnuplotWindow window;
        window.addSubplot( x(0),"x0" );
        window.addSubplot( x(6),"x6" );
        window.addSubplot( u(0),"u0" );
        window.addSubplot( u(1),"u1" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

    algorithm.set( MAX_NUM_ITERATIONS, 20 );
    algorithm.set( KKT_TOLERANCE, 1e-10 );

    algorithm << window;
    algorithm.solve();

    return 0;
}

 //================== Plane ============
 Plane::Plane(const float3 &A,const float3 &B,const float3 &C)
 {
     normal = normalize( cross((B-A),(C-A)) );
     d = -dot(normal, A);
 }

    void DistanceRayToSegment(const Ray& ray, const LineSeg& segment,
                                float* out_distTo, float* out_distBetween, float3* out_pos, float3* out_nor)
    {
        // Find closest distance between ray and segment:
        //      Construct seg ray:
        //          seg.direction = normal(segment.B - segment.A)
        //          seg.pos = segment.A
        //
        //      Construct plane through ray.pos that contains both ray.direction and segment.direction
        //      Construct second plane through segment.pos that contains both ray.direction and segment.direction
        //      The normal of both these planes will be:
        //          planeNormal = normal(cross(ray.direction, seg.direction))
        //          planeR . planeNormal = ray.pos . planeNormal
        //          planeS . planeNormal = seg.pos . planeNormal
        //
        //      The distance between these planes will be ray-to-seg projected on the planeNormal:
        //          distRayPosToSegment = (seg.pos - ray.pos) . planeNormal
        //
        // Now we need to find where this closest point occurs to return distanceToClosest and check if
        // it actually occurs between segment.A and segment.B:
        //          rayLine = ray.pos + tRay * ray.direction
        //          segLine = seg.pos + tSeg * seg.direction
        //
        //      Since we already have the normal and distance between ray and segment:
        //          rayLine + distBetweenRaySegment * planeNormal = segLine
        //
        //      By setting components equal we can solve for tRay and tSeg:
        //                                 (B * seg.direction.x + A * seg.direction.y)
        //          tRay =  ---------------------------------------------------------------------------
        //                  (ray.direction.y * seg.direction.x - ray.direction.x * seg.direction.y)
        //
        //          where
        //          A = ray.pos.x - seg.pos.x + distBetweenRaySegment * planeNormal.x
        //          B = seg.pos.y - ray.pos.y - distBetweenRaySegment * planeNormal.y
        //
        //          tSeg = (A + tRay * ray.direction.x) / seg.direction.x
        //
        //      if      tSeg < 0                                =>  use point-to-ray-distance(segment.A, ray)
        //      else if tSeg > length(segment.B - segment.A)    =>  use point-to-ray-distance(segment.B, ray)
        //      else                                            =>  distToClosest = tRay
        //                                                          posClosest = tRay * ray.direction + ray.position
        //

        Ray seg(segment.A, segment.B - segment.A);

        // check for parallel seg and ray

        float3 planeNormal = normalize(cross(ray.direction, seg.direction));
        float distBetweenRaySegment = dot(seg.pos - ray.pos, planeNormal);
        float A = ray.pos.x - seg.pos.x + distBetweenRaySegment * planeNormal.x;
        float B = seg.pos.y - ray.pos.y - distBetweenRaySegment * planeNormal.y;
        float tRay = (B * seg.direction.x + A * seg.direction.y) /
                        (ray.direction.y * seg.direction.x - ray.direction.x * seg.direction.y);
        float tSeg = (A + tRay * ray.direction.x) / seg.direction.x;

        float segmentLength = length(segment.B - segment.A);

        if (tRay < 0) // the segment is behind us
        {
            DistancePointToSegment(segment, ray.pos, out_distTo, out_distBetween, out_pos, out_nor);
        }
        else if (tSeg < 0) // closest is before segment.A
        {
            DistanceRayToPoint(ray, segment.A, out_distTo, out_distBetween, out_pos, out_nor);
        }
        else if (tSeg > segmentLength) // closest is after segment.B
        {
            DistanceRayToPoint(ray, segment.B, out_distTo, out_distBetween, out_pos, out_nor);
        }
        else
        {
            *out_distTo = tRay;
            *out_distBetween = fabsf(distBetweenRaySegment);
            *out_pos = ray.pos + tRay * ray.direction;
            *out_nor = planeNormal;
        }
    }

 inline Vector3D reflect(const Vector3D &v, const Vector3D &n) {
   return v - 2*dot(n, v)*n;
 }

int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE FIXED PARAMETERS:
    // ---------------------------
    #define  v		0.1
    #define  L		1.0
    #define  Beta	0.2
    #define  Delta	0.25
    #define  E		11250.0
    #define  k0		1E+06
    #define  R		1.986
    #define  K1		250000.0
    #define  Cin	0.02
    #define  Tin	340.0


    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState     x1,x2;
    Control               u    ;
    DifferentialEquation  f( 0.0, L );


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    double Alpha, Gamma;
    Alpha = k0*exp(-E/(R*Tin));
    Gamma = E/(R*Tin);

    f << dot(x1) ==  Alpha       /v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2));
    f << dot(x2) == (Alpha*Delta)/v * (1.0-x1) * exp((Gamma*x2)/(1.0+x2)) + Beta/v * (u-x2);


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, L, 50 );
    ocp.minimizeMayerTerm( 0, Cin*(1.0-x1)            ); // Solve conversion optimal problem
    ocp.minimizeMayerTerm( 1, (pow((Tin*x2),2.0)/K1) + 0.005*Cin*(1.0-x1) ); // Solve energy optimal problem (perturbed by small conversion cost; 
									     // otherwise the problem is ill-defined.)

    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x1 ==  0.0 );
    ocp.subjectTo( AT_START, x2 ==  0.0 );

    ocp.subjectTo(  0.0            <= x1 <=  1.0             );
    ocp.subjectTo( (280.0-Tin)/Tin <= x2 <= (400.0-Tin)/Tin  );
    ocp.subjectTo( (280.0-Tin)/Tin <= u  <= (400.0-Tin)/Tin  );


    // DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE OCP:
    // -----------------------------------------------------
    MultiObjectiveAlgorithm algorithm(ocp);

    algorithm.set( INTEGRATOR_TYPE, INT_BDF );
    algorithm.set( KKT_TOLERANCE, 1e-7 );

    algorithm.set( PARETO_FRONT_GENERATION    , PFG_NORMAL_BOUNDARY_INTERSECTION );
    algorithm.set( PARETO_FRONT_DISCRETIZATION, 11 );

    // Minimize individual objective function
    algorithm.solveSingleObjective(0);
    
    // Minimize individual objective function
    algorithm.solveSingleObjective(1);
    
    // Generate Pareto set 
    algorithm.solve();

    algorithm.getWeights("plug_flow_reactor_nbi_weights.txt");
    algorithm.getAllDifferentialStates("plug_flow_reactor_nbi_states.txt");
    algorithm.getAllControls("plug_flow_reactor_nbi_controls.txt");


    // VISUALIZE THE RESULTS IN A GNUPLOT WINDOW:
    // ------------------------------------------
    VariablesGrid paretoFront;
    algorithm.getParetoFront( paretoFront );

    GnuplotWindow window1;
    window1.addSubplot( paretoFront, "Pareto Front (conversion versus energy)", "OUTLET CONCENTRATION", "ENERGY", PM_POINTS );
    window1.plot( );


    // PRINT INFORMATION ABOUT THE ALGORITHM:
    // --------------------------------------
    algorithm.printInfo();


    // SAVE INFORMATION:
    // -----------------
    paretoFront.print();

    return 0;
}

// Returns "distance" between target destination and resulting xfrom
F32 LLDrawable::updateXform(BOOL undamped)
{
	BOOL damped = !undamped;

	// Position
	LLVector3 old_pos(mXform.getPosition());
	LLVector3 target_pos;
	if (mXform.isRoot())
	{
		// get root position in your agent's region
		target_pos = mVObjp->getPositionAgent();
	}
	else
	{
		// parent-relative position
		target_pos = mVObjp->getPosition();
	}
	
	// Rotation
	LLQuaternion old_rot(mXform.getRotation());
	LLQuaternion target_rot = mVObjp->getRotation();
	//scaling
	LLVector3 target_scale = mVObjp->getScale();
	LLVector3 old_scale = mCurrentScale;
	
	// Damping
	F32 dist_squared = 0.f;
	F32 camdist2 = (mDistanceWRTCamera * mDistanceWRTCamera);

	if (damped && isVisible())
	{
		F32 lerp_amt = llclamp(LLCriticalDamp::getInterpolant(OBJECT_DAMPING_TIME_CONSTANT), 0.f, 1.f);
		LLVector3 new_pos = lerp(old_pos, target_pos, lerp_amt);
		dist_squared = dist_vec_squared(new_pos, target_pos);

		LLQuaternion new_rot = nlerp(lerp_amt, old_rot, target_rot);
		dist_squared += (1.f - dot(new_rot, target_rot)) * 10.f;

		LLVector3 new_scale = lerp(old_scale, target_scale, lerp_amt);
		dist_squared += dist_vec_squared(new_scale, target_scale);

		if ((dist_squared >= MIN_INTERPOLATE_DISTANCE_SQUARED * camdist2) &&
			(dist_squared <= MAX_INTERPOLATE_DISTANCE_SQUARED))
		{
			// interpolate
			target_pos = new_pos;
			target_rot = new_rot;
			target_scale = new_scale;
		}
		else if (mVObjp->getAngularVelocity().isExactlyZero())
		{
			// snap to final position (only if no target omega is applied)
			dist_squared = 0.0f;
			if (getVOVolume() && !isRoot())
			{ //child prim snapping to some position, needs a rebuild
				gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE);
			}
		}
	}

	if ((mCurrentScale != target_scale) ||
		(!isRoot() &&
		(dist_squared >= MIN_INTERPOLATE_DISTANCE_SQUARED ||
		!mVObjp->getAngularVelocity().isExactlyZero() ||
		target_pos != mXform.getPosition() ||
		target_rot != mXform.getRotation())))
	{ //child prim moving or scale change requires immediate rebuild
		mCurrentScale = target_scale;
		gPipeline.markRebuild(this, LLDrawable::REBUILD_POSITION, TRUE);
	}
	else if (!getVOVolume() && !isAvatar())
	{
		movePartition();
	}

	// Update
	mXform.setPosition(target_pos);
	mXform.setRotation(target_rot);
	mXform.setScale(LLVector3(1,1,1)); //no scale in drawable transforms (IT'S A RULE!)
	mXform.updateMatrix();

	if (mSpatialBridge)
	{
		gPipeline.markMoved(mSpatialBridge, FALSE);
	}
	return dist_squared;
}

unsigned int trace(const vec p, const vec d) {
	static const vec light = { 0.408248, 0.816497, -0.408248 };
	vec o;
	int i;
	float t = 0, a = 0;
	for (i = 0; i < 75; i++) {

		mov(o, d);
		mul(o, t);
		add(o, p);

		if (f(o) < 0) break;
		a = t;
		t += 0.125;
	}
	if (i == 75) return 0x000000;

	float b = t;
	for (i = 0; i < 10; i++) {
		t = (a + b) * 0.5;

		mov(o, d);
		mul(o, t);
		add(o, p);

		if (f(o) < 0) b = t;
		else a = t;
	}

	mov(o, d);
	mul(o, t);
	add(o, p);

	vec o2, o3;
	mov(o2, o);
	mov(o3, o);
	o[0] += 0.1;
	o2[1] += 0.1;
	o3[2] += 0.1;
	vec n = { f(o), f(o2), f(o2) };
	normalize(n);
	float diffuse = -dot(n, light);
	if (diffuse < 0) diffuse = 0;

	mul(n, dot(d, n) * -2);
	add(n, d);
	float specular = dot(n, light);
	if (specular < 0) specular = 0;
	specular = pow(specular, 60);

	float fog = 1 - t / 11;
	fog *= fog;

	float pattern = (
		(fmod(o[0], 0.2) < 0.1) +
		(fmod(o[1], 0.2) < 0.1) +
		(fmod(o[2], 0.2) < 0.1)) * 50;

	return COLOR_RGB(
		(255 * diffuse + specular * 200) * fog,
		(pattern * diffuse + specular * 200) * fog,
		(130 * diffuse + specular * 200) * fog);
}

	Spectrum Li(const RayDifferential &r, RadianceQueryRecord &rRec) const {
		/* Some aliases and local variables */
		const Scene *scene = rRec.scene;
		Intersection &its = rRec.its;
		RayDifferential ray(r);
		Spectrum Li(0.0f);

		/* Perform the first ray intersection (or ignore if the 
		   intersection has already been provided). */
		rRec.rayIntersect(ray);
		ray.mint = Epsilon;

		Spectrum pathThroughput(1.0f);

		while (rRec.depth <= m_maxDepth || m_maxDepth < 0) {
			if (!its.isValid()) {
				/* If no intersection could be found, potentially return 
				   radiance from a background luminaire if it exists */
				if (rRec.type & RadianceQueryRecord::EEmittedRadiance)
					Li += pathThroughput * scene->LeBackground(ray);
				break;
			}

			const BSDF *bsdf = its.getBSDF(ray);

			if (EXPECT_NOT_TAKEN(bsdf == NULL)) {
				/* The MI path tracer doesn't support
				   surfaces without a BSDF (e.g. medium transitions)
				   -- give up. */
				break;
			}

			/* Possibly include emitted radiance if requested */
			if (its.isLuminaire() && (rRec.type & RadianceQueryRecord::EEmittedRadiance))
				Li += pathThroughput * its.Le(-ray.d);

			/* Include radiance from a subsurface integrator if requested */
			if (its.hasSubsurface() && (rRec.type & RadianceQueryRecord::ESubsurfaceRadiance))
				Li += pathThroughput * its.LoSub(scene, rRec.sampler, -ray.d, rRec.depth);

			if (m_maxDepth > 0 && rRec.depth >= m_maxDepth)
				break;

			/* ==================================================================== */
			/*                          Luminaire sampling                          */
			/* ==================================================================== */

			/* Prevent light leaks due to the use of shading normals */
			Float wiDotGeoN = -dot(its.geoFrame.n, ray.d),
				  wiDotShN  = Frame::cosTheta(its.wi);
			if (wiDotGeoN * wiDotShN < 0 && m_strictNormals) 
				break;

			/* Estimate the direct illumination if this is requested */
			LuminaireSamplingRecord lRec;
			if (rRec.type & RadianceQueryRecord::EDirectSurfaceRadiance && 
				scene->sampleLuminaire(its.p, ray.time, lRec, rRec.nextSample2D())) {
				/* Allocate a record for querying the BSDF */
				const Vector wo = -lRec.d;
				const BSDFQueryRecord bRec(its, its.toLocal(wo));
	
				/* Evaluate BSDF * cos(theta) */
				const Spectrum bsdfVal = bsdf->fCos(bRec);

				Float woDotGeoN = dot(its.geoFrame.n, wo);

				/* Prevent light leaks due to the use of shading normals */
				if (!bsdfVal.isZero() && (!m_strictNormals
						|| woDotGeoN * Frame::cosTheta(bRec.wo) > 0)) {
					/* Calculate prob. of having sampled that direction
					   using BSDF sampling */
					Float bsdfPdf = (lRec.luminaire->isIntersectable() 
							|| lRec.luminaire->isBackgroundLuminaire()) ? 
						bsdf->pdf(bRec) : 0;

					/* Weight using the power heuristic */
					const Float weight = miWeight(lRec.pdf, bsdfPdf);
					Li += pathThroughput * lRec.value * bsdfVal * weight;
				}
			}

			/* ==================================================================== */
			/*                            BSDF sampling                             */
			/* ==================================================================== */

			/* Sample BSDF * cos(theta) */
			BSDFQueryRecord bRec(its);
			Float bsdfPdf;
			Spectrum bsdfVal = bsdf->sampleCos(bRec, bsdfPdf, rRec.nextSample2D());
			if (bsdfVal.isZero()) 
				break;
			bsdfVal /= bsdfPdf;
	
			/* Prevent light leaks due to the use of shading normals */
			const Vector wo = its.toWorld(bRec.wo);
			Float woDotGeoN = dot(its.geoFrame.n, wo);
			if (woDotGeoN * Frame::cosTheta(bRec.wo) <= 0 && m_strictNormals)
				break;

			/* Trace a ray in this direction */
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    // calcul du delta= pondere par les longueurs des segments
    double delta = 0;
    {
        double worstD = 0;
        worstP = -1;
        Geom::Point prevAppP;
    Geom::Point   prevP;
    double      prevDist;
    prevP[0] = Xk[0];
    prevP[1] = Yk[0];
    prevAppP = prevP; // le premier seulement
    prevDist = 0;
#ifdef with_splotch_killer
    if ( N <= 20 ) {
      for (int i = 1; i < N - 1; i++)
      {
        Geom::Point curAppP;
        Geom::Point curP;
        double    curDist;
        Geom::Point midAppP;
        Geom::Point midP;
        double    midDist;
        
        curAppP[0] = N13 (tk[i]) * cp1[0] + N23 (tk[i]) * cp2[0] + N03 (tk[i]) * Xk[0] + N33 (tk[i]) * Xk[N - 1];
        curAppP[1] = N13 (tk[i]) * cp1[1] + N23 (tk[i]) * cp2[1] + N03 (tk[i]) * Yk[0] + N33 (tk[i]) * Yk[N - 1];
        curP[0] = Xk[i];
        curP[1] = Yk[i];
        midAppP[0] = N13 (0.5*(tk[i]+tk[i-1])) * cp1[0] + N23 (0.5*(tk[i]+tk[i-1])) * cp2[0] + N03 (0.5*(tk[i]+tk[i-1])) * Xk[0] + N33 (0.5*(tk[i]+tk[i-1])) * Xk[N - 1];
        midAppP[1] = N13 (0.5*(tk[i]+tk[i-1])) * cp1[1] + N23 (0.5*(tk[i]+tk[i-1])) * cp2[1] + N03 (0.5*(tk[i]+tk[i-1])) * Yk[0] + N33 (0.5*(tk[i]+tk[i-1])) * Yk[N - 1];
        midP=0.5*(curP+prevP);
        
        Geom::Point diff;
        diff = curAppP-curP;
        curDist = dot(diff,diff);

        diff = midAppP-midP;
        midDist = dot(diff,diff);
        
        delta+=0.3333*(curDist+prevDist+midDist)/**lk[i]*/;

        if ( curDist > worstD ) {
          worstD = curDist;
          worstP = i;
        } else if ( fk[i] && 2*curDist > worstD ) {
          worstD = 2*curDist;
          worstP = i;
        }
        prevP = curP;
        prevAppP = curAppP;
        prevDist = curDist;
      }
      delta/=totLen;
    } else {
#endif
      for (int i = 1; i < N - 1; i++)
      {
        Geom::Point curAppP;
        Geom::Point curP;
        double    curDist;
        
        curAppP[0] = N13 (tk[i]) * cp1[0] + N23 (tk[i]) * cp2[0] + N03 (tk[i]) * Xk[0] + N33 (tk[i]) * Xk[N - 1];
        curAppP[1] = N13 (tk[i]) * cp1[1] + N23 (tk[i]) * cp2[1] + N03 (tk[i]) * Yk[0] + N33 (tk[i]) * Yk[N - 1];
        curP[0] = Xk[i];
        curP[1] = Yk[i];
        
        Geom::Point diff;

 Plane::Plane(const float3 &P,const float3 &n)
 {
     normal = n;
     d = -dot(normal, P);
 }

inline Vec3fa face_forward(const Vec3fa& dir, const Vec3fa& _Ng) {
  const Vec3fa Ng = _Ng;
  return dot(dir,Ng) < 0.0f ? Ng : neg(Ng);
}

    //-----------------------------------------------------------------------------
    // Frustum Triangle intersection  using separating axis test.
    // note: the frustum and the triangle must be in the same space.
    //       optimization needed
    bool FrustumTriangleIntersect(const Frustum& fr, const Triangle& tri)
    {            
        bool allInside = true;        
        for(int i = 0; i < 6; ++i)
        {
            const Plane& plane = fr[i];
            float d1 = plane.Eval(tri.A);
            float d2 = plane.Eval(tri.B);
            float d3 = plane.Eval(tri.C); 

            // if all outside a single plane, then there is
            // no intersection.
            if( d1 < 0 && d2 < 0 && d3 < 0) 
                return false;
            allInside = allInside && ( d1 > 0 && d2 > 0 && d3 > 0);            
        }

        // the tri is completely inside the frustum.
        if( allInside ) 
            return true; 

        // separating axis test.
        // Triangle:  3 edges  1 face normal.
        // Frustum:   6 edges, 5 face normal.
        // for total of 24 separating axis.
        // note this test can be optimized.
                        
        // tri edges
        float3 triEdges[3];
        triEdges[0] = tri.B - tri.A;
        triEdges[1] = tri.C - tri.A;
        triEdges[2] = tri.C - tri.B;

        // frustum edges
        float3 FrEdges[6];
        FrEdges[0] = fr.Corner(Frustum::NearTopLeft) - fr.Corner(Frustum::NearTopRight);
        FrEdges[1] = fr.Corner(Frustum::NearBottomRight) - fr.Corner(Frustum::NearTopRight);
        FrEdges[2] = (fr.Corner(Frustum::FarBottonLeft) - fr.Corner(Frustum::NearBottonLeft));
        FrEdges[3] = (fr.Corner(Frustum::FarBottomRight) - fr.Corner(Frustum::NearBottomRight));
        FrEdges[4] = (fr.Corner(Frustum::FarTopRight) - fr.Corner(Frustum::NearTopRight));
        FrEdges[5] = (fr.Corner(Frustum::FarTopLeft) - fr.Corner(Frustum::NearTopLeft));

        int k = 0;        
        float3 Axis[24];                
        Axis[k++] = fr.TopPlane().normal;
        Axis[k++] = fr.BottomPlane().normal;
        Axis[k++] = fr.LeftPlane().normal;
        Axis[k++] = fr.RightPlane().normal;
        Axis[k++] = fr.NearPlane().normal;
        Axis[k++] = normalize(cross(triEdges[0], triEdges[1]));

        for(int te = 0;  te < 3; te++)
        {
            for(int fe = 0; fe < 6; fe++)
            {
                float3 axis = cross( triEdges[te], FrEdges[fe]); 
                Axis[k++] = normalize(axis);                
            }
        }
                
        for(int n = 0; n < 24; n++)
        {
            float3 axis = Axis[n];
            if( lengthsquared(axis) < Epsilon) 
                continue;
            float trid1 = dot(tri.A,axis);
            float trid2 = dot(tri.B,axis);
            float trid3 = dot(tri.C,axis);

            float trMin = std::min(trid1,trid2);
            trMin = std::min(trMin, trid3);
            float trMax = std::max(trid1,trid2);
            trMax = std::max(trMax,trid3);

            float frMin = dot(fr.Corner(0),axis);
            float frMax = frMin;
            for(int c = 1; c < 8; c++)
            {
                float fdist = dot(fr.Corner(c), axis);
                frMin = std::min(frMin,fdist);
                frMax = std::max(frMax,fdist);
            }
           
            if( (trMax < frMin) || (frMax < trMin))
            {               
                return false;                
            }
        }

        // must be intersecting.
        return true;
               
    }

 inline Vector2 project(const Vector2& other) const { return other * (dot(other) / other.dot(other)); }

	double distance(const Vector3 &p, uint32_t tidx) const
	{
		const