C++ SUB函数代码示例

//.........这里部分代码省略.........
      }else{
	ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
        LALPrintError( USAGE, *argv );
        return BASICINJECTTESTC_EARG;
      }
    }
    /* Check for unrecognized options. */
    else if ( argv[arg][0] == '-' ) {
      ERROR( BASICINJECTTESTC_EARG, BASICINJECTTESTC_MSGEARG, 0 );
      LALPrintError( USAGE, *argv );
      return BASICINJECTTESTC_EARG;
    }
  } /* End of argument parsing loop. */

  /* Check for redundant or bad argument values. */
  CHECKVAL( npt, 0, 2147483647 );
  CHECKVAL( dt, 0, LAL_REAL4_MAX );


  /*******************************************************************
   * SETUP                                                           *
   *******************************************************************/

  /* Set up output, detector, and random parameter structures. */
  output.data = NULL;
  detector.transfer = (COMPLEX8FrequencySeries *)
    LALMalloc( sizeof(COMPLEX8FrequencySeries) );
  if ( !(detector.transfer) ) {
    ERROR( BASICINJECTTESTC_EMEM, BASICINJECTTESTC_MSGEMEM, 0 );
    return BASICINJECTTESTC_EMEM;
  }
  detector.transfer->data = NULL;
  detector.site = NULL;
  SUB( LALCreateRandomParams( &stat, &params, seed ), &stat );

  /* Set up units. */
  output.sampleUnits = lalADCCountUnit;
  if (XLALUnitDivide( &(detector.transfer->sampleUnits),
                      &lalADCCountUnit, &lalStrainUnit ) == NULL) {
    return LAL_EXLAL;
  }

  /* Read response function. */
  if ( respfile ) {
    REAL4VectorSequence *resp = NULL; /* response as vector sequence */
    COMPLEX8Vector *response = NULL;  /* response as complex vector */
    COMPLEX8Vector *unity = NULL;     /* vector of complex 1's */

    if ( ( fp = fopen( respfile, "r" ) ) == NULL ) {
      ERROR( BASICINJECTTESTC_EFILE, BASICINJECTTESTC_MSGEFILE,
	     respfile );
      return BASICINJECTTESTC_EFILE;
    }

    /* Read header. */
    ok &= ( fscanf( fp, "# epoch = %" LAL_INT8_FORMAT "\n", &epoch ) == 1 );
    I8ToLIGOTimeGPS( &( detector.transfer->epoch ), epoch );
    ok &= ( fscanf( fp, "# f0 = %lf\n", &( detector.transfer->f0 ) )
	    == 1 );
    ok &= ( fscanf( fp, "# deltaF = %lf\n",
		    &( detector.transfer->deltaF ) ) == 1 );
    if ( !ok ) {
      ERROR( BASICINJECTTESTC_EINPUT, BASICINJECTTESTC_MSGEINPUT,
	     respfile );
      return BASICINJECTTESTC_EINPUT;
    }

bool triangle_intersection( const Vector3&   V1,  // Triangle vertices
							const Vector3&   V2,
							const Vector3&   V3,
							const Vector3&    O,
							const Vector3&	  D,
							float& 			out,
							float 			BG[2])
{

	Vector3 e1, e2;  //Edge1, Edge2
	Vector3 P, Q, T;
	float det, inv_det, u, v;
	float t;

	//Find vectors for two edges sharing V1
	
	SUB(e1, V2, V1);
	SUB(e2, V3, V1);

	//Begin calculating determinant - also used to calculate u parameter

	CROSS(P, D, e2);

	//if determinant is near zero, ray lies in plane of triangle

	det = DOT(e1, P);

	//NOT CULLING

	if(det > -EPSILON && det < EPSILON) return false;
	inv_det = 1.f / det;

	//calculate distance from V1 to ray origin

	SUB(T, O, V1);

	//Calculate u parameter and test bound
	u = DOT(T, P) * inv_det;
	//The intersection lies outside of the triangle

	if(u < 0.f || u > 1.f) return false;

	//Prepare to test v parameter

	CROSS(Q, T, e1);

	//Calculate V parameter and test bound

	v = DOT(D, Q) * inv_det;

	//The intersection lies outside of the triangle

	if(u < 0.f || v < 0.f || u + v  > 1.f) return false;


	//u and v are the barycentric coordinates

	t = DOT(e2, Q) * inv_det;

	if(t > EPSILON) 
	{ //ray intersection
		out = t;
		BG[0] = u;
		BG[1] = v;
		return true;
	}

	// No hit, no win
	return false;
}

void updateH(N3 N, P3F3 E, P3F3 H, P3F3 CH) {
	int i,j,k;
	v4sf h, ch, e1, e2, e3, e4; 

	//omp_set_num_threads(8);
	for (i=1;i<N.x;i++){
		for (j=1;j<N.y;j++){
			for (k=0;k<N.z;k+=4){
				h = LOAD(&H.x[i][j][k]);
				ch = LOAD(&CH.x[i][j][k]);
				e1 = LOAD(&E.z[i][j][k]);
				e2 = LOAD(&E.z[i][j-1][k]);
				e3 = LOAD(&E.y[i][j][k]);
				e4 = LOAD(&E.y[i][j][k-1]);
				STORE(&H.x[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4))))); 
			}
		}
	}

	for (i=1;i<N.x;i++){
		for (j=1;j<N.y;j++){
			for (k=0;k<N.z;k+=4){

				h = LOAD(&H.y[i][j][k]);
				ch = LOAD(&CH.y[i][j][k]);
				e1 = LOAD(&E.x[i][j][k]);
				e2 = LOAD(&E.x[i][j][k-1]);
				e3 = LOAD(&E.z[i][j][k]);
				e4 = LOAD(&E.z[i-1][j][k]);
				STORE(&H.y[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4))))); 
			}
		}
	}
	for (i=1;i<N.x;i++){
		for (j=1;j<N.y;j++){
			for (k=0;k<N.z;k+=4){

				h = LOAD(&H.z[i][j][k]);
				ch = LOAD(&CH.z[i][j][k]);
				e1 = LOAD(&E.y[i][j][k]);
				e2 = LOAD(&E.y[i-1][j][k]);
				e3 = LOAD(&E.x[i][j][k]);
				e4 = LOAD(&E.x[i][j-1][k]);
				STORE(&H.z[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4))))); 
			}
		}
	}

	i=0;
	for (j=1;j<N.y;j++){
		for (k=0;k<N.z;k+=4){
			h = LOAD(&H.x[i][j][k]);
			ch = LOAD(&CH.x[i][j][k]);
			e1 = LOAD(&E.z[i][j][k]);
			e2 = LOAD(&E.z[i][j-1][k]);
			e3 = LOAD(&E.y[i][j][k]);
			e4 = LOAD(&E.y[i][j][k-1]);
			STORE(&H.x[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4))))); 
		}
	}

	j=0;
	for (i=1;i<N.x;i++){
		for (k=0;k<N.z;k+=4){
			h = LOAD(&H.y[i][j][k]);	
			ch = LOAD(&CH.y[i][j][k]);
			e1 = LOAD(&E.x[i][j][k]);
			e2 = LOAD(&E.x[i][j][k-1]);
			e3 = LOAD(&E.z[i][j][k]);
			e4 = LOAD(&E.z[i-1][j][k]);
			STORE(&H.y[i][j][k], SUB(h, MUL(ch, SUB( SUB(e1,e2), SUB(e3,e4))))); 
		}
	}
}

bool8 AtiTriBoxMoller( const TBM_FLOAT boxcenter[3], const TBM_FLOAT boxhalfsize[3],
                 const TBM_FLOAT triVert0[3], const TBM_FLOAT triVert1[3],
                 const TBM_FLOAT triVert2[3])
{
   // use separating axis theorem to test overlap between triangle and box
   // need to test for overlap in these directions:
   // 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
   //    we do not even need to test these)
   // 2) normal of the triangle
   // 3) crossproduct(edge from tri, {x,y,z}-directin)
   //    this gives 3x3=9 more tests
   TBM_FLOAT v0[3], v1[3], v2[3];
   TBM_FLOAT min, max, d, p0, p1, p2, rad, fex, fey, fez;
   TBM_FLOAT normal[3], e0[3], e1[3], e2[3];

   // This is the fastest branch on Sun
   // move everything so that the boxcenter is in (0,0,0)
   SUB (v0, triVert0, boxcenter);
   SUB (v1, triVert1, boxcenter);
   SUB (v2, triVert2, boxcenter);

   // compute triangle edges
   SUB (e0, v1, v0);      // tri edge 0
   SUB (e1, v2, v1);      // tri edge 1
   SUB (e2, v0, v2);      // tri edge 2

   // Bullet 3:
   //  test the 9 tests first (this was faster)
   fex = (TBM_FLOAT)fabs (e0[X]);
   fey = (TBM_FLOAT)fabs (e0[Y]);
   fez = (TBM_FLOAT)fabs (e0[Z]);
   AXISTEST_X01 (e0[Z], e0[Y], fez, fey);
   AXISTEST_Y02 (e0[Z], e0[X], fez, fex);
   AXISTEST_Z12 (e0[Y], e0[X], fey, fex);

   fex = (TBM_FLOAT)fabs (e1[X]);
   fey = (TBM_FLOAT)fabs (e1[Y]);
   fez = (TBM_FLOAT)fabs (e1[Z]);
   AXISTEST_X01 (e1[Z], e1[Y], fez, fey);
   AXISTEST_Y02 (e1[Z], e1[X], fez, fex);
   AXISTEST_Z0 (e1[Y], e1[X], fey, fex);

   fex = (TBM_FLOAT)fabs (e2[X]);
   fey = (TBM_FLOAT)fabs (e2[Y]);
   fez = (TBM_FLOAT)fabs (e2[Z]);
   AXISTEST_X2 (e2[Z], e2[Y], fez, fey);
   AXISTEST_Y1 (e2[Z], e2[X], fez, fex);
   AXISTEST_Z12 (e2[Y], e2[X], fey, fex);

   // Bullet 1:
   //  first test overlap in the {x,y,z}-directions
   //  find min, max of the triangle each direction, and test for overlap in
   //  that direction -- this is equivalent to testing a minimal AABB around
   //  the triangle against the AABB

   // test in X-direction
   FINDMINMAX (v0[X], v1[X], v2[X], min, max);
   if (min > boxhalfsize[X] || max < -boxhalfsize[X])
   {
      return FALSE;
   }

   // test in Y-direction
   FINDMINMAX (v0[Y], v1[Y], v2[Y], min, max);
   if (min > boxhalfsize[Y] || max < -boxhalfsize[Y])
   {
      return FALSE;
   }

   // test in Z-direction
   FINDMINMAX (v0[Z], v1[Z], v2[Z], min, max);
   if (min > boxhalfsize[Z] || max < -boxhalfsize[Z])
   {
      return FALSE;
   }

   // Bullet 2:
   //  test if the box intersects the plane of the triangle
   //  compute plane equation of triangle: normal*x+d=0
   CROSS (normal, e0, e1);
   d = -DOT (normal, v0);  // plane eq: normal.x+d=0
   return AtiPlaneBoxOverlap (normal, d, boxhalfsize);
}

//.........这里部分代码省略.........
  if ( f0 < 0 ) {
    ERROR( VALIDATION1_EBAD, VALIDATION1_MSGEBAD, "freq<0:" );
    XLALPrintError( USAGE, *argv  );
    return VALIDATION1_EBAD;
  }

  /******************************************************************/
  /******************************************************************/
  /* create time stamps (for a test) */
  /******************************************************************/
  timeV.time = (LIGOTimeGPS *)LALMalloc(mObsCoh*sizeof(LIGOTimeGPS));
  timeV.time[0].gpsSeconds = T0SEC;
  timeV.time[0].gpsNanoSeconds = T0NSEC;
  for(j=1; j<timeV.length; ++j){
    timeV.time[j].gpsSeconds = timeV.time[j-1].gpsSeconds + TCOH + JUMPTIME;
    timeV.time[j].gpsNanoSeconds = T0NSEC;
  }

  /******************************************************************/
  /* compute detector velocity for those time stamps (for a test) */
  /******************************************************************/

  velV.data = (REAL8Cart3Coor *)LALMalloc(mObsCoh*sizeof(REAL8Cart3Coor));
  velPar.edat = NULL; 
  {
    EphemerisData    *edat=NULL;
   
    /*  ephemeris info */
    edat = (EphemerisData *)LALMalloc(sizeof(EphemerisData));
   (*edat).ephiles.earthEphemeris = earthEphemeris;
   (*edat).ephiles.sunEphemeris = sunEphemeris;

    /* read in ephemeris data */
    SUB( LALInitBarycenter( &status, edat), &status);
    velPar.edat = edat;
    
    for(j=0; j<velV.length; ++j){
      velPar.startTime.gpsSeconds     = timeV.time[j].gpsSeconds;
      velPar.startTime.gpsNanoSeconds = timeV.time[j].gpsNanoSeconds;
    
      SUB( LALAvgDetectorVel ( &status, vel, &velPar), &status );
      velV.data[j].x= vel[0];
      velV.data[j].y= vel[1];
      velV.data[j].z= vel[2];   
    }
    LALFree(edat->ephemE);
    LALFree(edat->ephemS);
    LALFree(edat);
  }

  /******************************************************************/  
  /******************************************************************/
  /* create patch grid */
  /******************************************************************/

  SUB( LALHOUGHComputeNDSizePar( &status, &parSize, &parRes ),  &status );
  
  xSide = parSize.xSide;
  ySide = parSize.ySide;
  maxNBins = parSize.maxNBins;
  maxNBorders = parSize.maxNBorders;

  /* allocate memory based on xSide and ySide */
  patch.xSide = xSide;
  patch.ySide = ySide;
  

bool intersect_triangle(double orig[3], double dir[3],
						double vert0[3], double vert1[3], double vert2[3],
						double *t, double *u, double *v)
{
	bool hit = true;
	double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
	double det,inv_det;
	
	/* find vectors for two edges sharing vert0 */
	SUB(edge1, vert1, vert0);
	SUB(edge2, vert2, vert0);
	
	/* begin calculating determinant - also used to calculate U parameter */
	CROSS(pvec, dir, edge2);
	
	/* if determinant is near zero, ray lies in plane of triangle */
	det = DOT(edge1, pvec);
	
#ifdef TEST_CULL           /* define TEST_CULL if culling is desired */
	if (det < EPSILON)
		hit = false;
	
	/* calculate distance from vert0 to ray origin */
	SUB(tvec, orig, vert0);
	
	/* calculate U parameter and test bounds */
	*u = DOT(tvec, pvec);
	if (*u < 0.0 || *u > det)
		hit = false;
	
	/* prepare to test V parameter */
	CROSS(qvec, tvec, edge1);
	
    /* calculate V parameter and test bounds */
	*v = DOT(dir, qvec);
	if (*v < 0.0 || *u + *v > det)
		hit = false;
	
	/* calculate t, scale parameters, ray intersects triangle */
	*t = DOT(edge2, qvec);
	inv_det = 1.0 / det;
	*t *= inv_det;
	*u *= inv_det;
	*v *= inv_det;
#else                    /* the non-culling branch */
	if (det > -EPSILON && det < EPSILON)
		hit = false;
	inv_det = 1.0 / det;
	
	/* calculate distance from vert0 to ray origin */
	SUB(tvec, orig, vert0);
	
	/* calculate U parameter and test bounds */
	*u = DOT(tvec, pvec) * inv_det;
	if (*u < 0.0 || *u > 1.0)
		hit = false;
	
	/* prepare to test V parameter */
	CROSS(qvec, tvec, edge1);
	
	/* calculate V parameter and test bounds */
	*v = DOT(dir, qvec) * inv_det;
	if (*v < 0.0 || *u + *v > 1.0)
		hit = false;
	
	/* calculate t, ray intersects triangle */
	*t = DOT(edge2, qvec) * inv_det;
#endif
	return hit;
}

int IsTriangleIntersectBox(float boxcenter[3], float boxhalf[3], float triverts[3][3])
{
	/*    use separating axis theorem to test overlap between triangle and box */
	/*    need to test for overlap in these directions: */
	/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
	/*       we do not even need to test these) */
	/*    2) normal of the triangle */
	/*    3) crossproduct(edge from tri, {x,y,z}-directin) */
	/*       this gives 3x3=9 more tests */
	float v0[3],v1[3],v2[3];
	float min,max,d,p0,p1,p2,rad,fex,fey,fez;  
	float normal[3],e0[3],e1[3],e2[3];

	/* 1) first test overlap in the {x,y,z}-directions */
	/*    find min, max of the triangle each direction, and test for overlap in */
	/*    that direction -- this is equivalent to testing a minimal AABB around */
	/*    the triangle against the AABB */

	/*    test in X */
	v0[X]=triverts[0][X]-boxcenter[X];
	v1[X]=triverts[1][X]-boxcenter[X];
	v2[X]=triverts[2][X]-boxcenter[X];
	FINDMINMAX(v0[X],v1[X],v2[X],min,max);
	if(min>boxhalf[X] || max<-boxhalf[X]) return 0;

	/*    test in Y */
	v0[Y]=triverts[0][Y]-boxcenter[Y];
	v1[Y]=triverts[1][Y]-boxcenter[Y];
	v2[Y]=triverts[2][Y]-boxcenter[Y];
	FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
	if(min>boxhalf[Y] || max<-boxhalf[Y]) return 0;

	/*    test in Z */
	v0[Z]=triverts[0][Z]-boxcenter[Z];
	v1[Z]=triverts[1][Z]-boxcenter[Z];
	v2[Z]=triverts[2][Z]-boxcenter[Z];
	FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
	if(min>boxhalf[Z] || max<-boxhalf[Z]) return 0;

	/*    2) */
	/*    test if the box intersects the plane of the triangle */
	/*    compute plane equation of triangle: normal*x+d=0 */
	SUB(e0,v1,v0);      /* tri edge 0 */
	SUB(e1,v2,v1);      /* tri edge 1 */
	CROSS(normal,e0,e1);
	d=-DOT(normal,v0);  /* plane eq: normal.x+d=0 */

	if(!planeBoxOverlap(normal,d,boxhalf)) return 0;

	/*    compute the last triangle edge */
	SUB(e2,v0,v2);

	/*    3) */
	fex = fabsf(e0[X]);
	fey = fabsf(e0[Y]);
	fez = fabsf(e0[Z]);
	AXISTEST_X01(e0[Z], e0[Y], fez, fey);
	AXISTEST_Y02(e0[Z], e0[X], fez, fex);
	AXISTEST_Z12(e0[Y], e0[X], fey, fex);

	fex = fabsf(e1[X]);
	fey = fabsf(e1[Y]);
	fez = fabsf(e1[Z]);
	AXISTEST_X01(e1[Z], e1[Y], fez, fey);
	AXISTEST_Y02(e1[Z], e1[X], fez, fex);
	AXISTEST_Z0(e1[Y], e1[X], fey, fex);

	fex = fabsf(e2[X]);
	fey = fabsf(e2[Y]);
	fez = fabsf(e2[Z]);
	AXISTEST_X2(e2[Z], e2[Y], fez, fey);
	AXISTEST_Y1(e2[Z], e2[X], fez, fex);
	AXISTEST_Z12(e2[Y], e2[X], fey, fex);

	return 1;
}

/*
 * Fast quasi-reduction modulo p384 (FIPS 186-3 D.2.4)
 */
static int ecp_mod_p384(ttls_mpi *N)
{
	INIT(384);

	ADD(12); ADD(21); ADD(20);
	SUB(23);		  NEXT; // A0

	ADD(13); ADD(22); ADD(23);
	SUB(12); SUB(20);		   NEXT; // A2

	ADD(14); ADD(23);
	SUB(13); SUB(21);		   NEXT; // A2

	ADD(15); ADD(12); ADD(20); ADD(21);
	SUB(14); SUB(22); SUB(23);			NEXT; // A3

	ADD(21); ADD(21); ADD(16); ADD(13); ADD(12); ADD(20); ADD(22);
	SUB(15); SUB(23); SUB(23);			NEXT; // A4

	ADD(22); ADD(22); ADD(17); ADD(14); ADD(13); ADD(21); ADD(23);
	SUB(16);		  NEXT; // A5

	ADD(23); ADD(23); ADD(18); ADD(15); ADD(14); ADD(22);
	SUB(17);		  NEXT; // A6

	ADD(19); ADD(16); ADD(15); ADD(23);
	SUB(18);		  NEXT; // A7

	ADD(20); ADD(17); ADD(16);
	SUB(19);		  NEXT; // A8

	ADD(21); ADD(18); ADD(17);
	SUB(20);		  NEXT; // A9

	ADD(22); ADD(19); ADD(18);
	SUB(21);		  NEXT; // A10

	ADD(23); ADD(20); ADD(19);
	SUB(22);		  LAST; // A11

cleanup:
	return ret;
}

static image_t *
render_map (bsp_t *bsp)
{
	long        i = 0, j = 0, k = 0, x = 0;

	dvertex_t  *vertexlist, *vert1, *vert2;
	dedge_t    *edgelist;
	dface_t    *facelist;
	int        *ledges;

	/* edge removal stuff */
	struct edge_extra_t *edge_extra = NULL;
	dvertex_t   v0, v1, vect;
	int         area = 0, usearea;

	float       minX = 0.0, maxX = 0.0, minY = 0.0, maxY = 0.0, minZ = 0.0;
	float       maxZ = 0.0, midZ = 0.0, tempf = 0.0;
	long        Zoffset0 = 0, Zoffset1 = 0;
	long        Z_Xdir = 1, Z_Ydir = -1;

	image_t    *image;
	int         drawcol;

	vertexlist = bsp->vertexes;
	edgelist = bsp->edges;
	facelist = bsp->faces;
	ledges = bsp->surfedges;

	/* Precalc stuff if we're removing edges - - - - - - - - - - - */

	if (options.edgeremove) {
		printf ("Precalc edge removal stuff...\n");
		edge_extra = malloc (sizeof (struct edge_extra_t) * bsp->numedges);
		if (edge_extra == NULL) {
			fprintf (stderr, "Error allocating %ld bytes for extra edge info.",
					 (long) sizeof (struct edge_extra_t) * bsp->numedges);
			exit (2);
		}
		/* initialize the array */
		for (i = 0; i < bsp->numedges; i++) {
			edge_extra[i].num_face_ref = 0;
			for (j = 0; j < MAX_REF_FACES; j++) {
				edge_extra[i].ref_faces[j] = -1;
			}
		}

		for (i = 0; i < bsp->numfaces; i++) {
			/* calculate the normal (cross product) */
			/* starting edge: edgelist[ledges[facelist[i].firstedge]] */
			/* number of edges: facelist[i].numedges; */

			/* quick hack - just take the first 2 edges */
			j = facelist[i].firstedge;
			k = j;
			vect.X = 0.0;
			vect.Y = 0.0;
			vect.Z = 0.0;
			while (vect.X == 0.0 && vect.Y == 0.0 && vect.Z == 0.0
				   && k < (facelist[i].numedges + j)) {
				dedge_t    *e1, *e2;
				/* If the first 2 are parallel edges, go with the next one */
				k++;

				e1 = &edgelist[abs (ledges[j])];
				e2 = &edgelist[abs (ledges[k])];
				//FIXME verify directions
				if (ledges[j] > 0) {
					SUB (vertexlist[e1->v[0]], vertexlist[e1->v[1]], v0);
					SUB (vertexlist[e2->v[0]], vertexlist[e2->v[1]], v1);
				} else {
					/* negative index, therefore walk in reverse order */
					SUB (vertexlist[e1->v[1]], vertexlist[e1->v[0]], v0);
					SUB (vertexlist[e2->v[1]], vertexlist[e2->v[0]], v1);
				}

				/* cross product */
				CROSS (v0, v1, vect);

				/* Okay, it's not the REAL area, but i'm lazy, and since a lot
				   of mapmakers use rectangles anyways... */
				area = sqrt (DOT (v0, v0)) * sqrt (DOT (v1, v1));
			}							/* while */

			/* reduce cross product to a unit vector */
			tempf = sqrt (DOT (vect, vect));
			if (tempf > 0.0) {
				vect.X = vect.X / tempf;
				vect.Y = vect.Y / tempf;
				vect.Z = vect.Z / tempf;
			} else {
				vect.X = 0.0;
				vect.Y = 0.0;
				vect.Z = 0.0;
			}

			/* Now go put ref in all edges... */
			for (j = 0; j < facelist[i].numedges; j++) {
				edge_extra_t *e;
				k = j + facelist[i].firstedge;
				e = &edge_extra[abs (ledges[k])];
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        return GEOCENTRICGEODETICTESTC_EARG;
      }
    }

    /* Parse debug level option. */
    else if ( !strcmp( argv[arg], "-d" ) ) {
      if ( argc > arg + 1 ) {
	arg++;
      } else {
	ERROR( GEOCENTRICGEODETICTESTC_EARG,
	       GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
        LALPrintError( USAGE, *argv );
        return GEOCENTRICGEODETICTESTC_EARG;
      }
    }

    /* Check for unrecognized options. */
    else if ( argv[arg][0] == '-' ) {
      ERROR( GEOCENTRICGEODETICTESTC_EARG,
	     GEOCENTRICGEODETICTESTC_MSGEARG, 0 );
      LALPrintError( USAGE, *argv );
      return GEOCENTRICGEODETICTESTC_EARG;
    }
  } /* End of argument parsing loop. */

  /*******************************************************************
   * SET PARAMETERS                                                  *
   *******************************************************************/

  /* If none of -x, -y, -z were given, generate a random position. */
  if ( !xyz ) {
    REAL4 lon, coslat, sinlat, rad; /* polar coordinates */
    RandomParams *rparams = NULL;   /* pseudorandom sequence parameters */
    SUB( LALCreateRandomParams( &stat, &rparams, 0 ), &stat );
    SUB( LALUniformDeviate( &stat, &lon, rparams ), &stat );
    lon *= LAL_TWOPI;
    SUB( LALUniformDeviate( &stat, &sinlat, rparams ), &stat );
    coslat = sqrt( 1.0 - sinlat*sinlat );
    SUB( LALUniformDeviate( &stat, &rad, rparams ), &stat );
    rad = 1.5*rad + 0.5;
    x_1 = x_2 = rad*coslat*cos( lon );
    y_1 = y_2 = rad*coslat*sin( lon );
    z_1 = z_2 = rad*sinlat;
    SUB( LALDestroyRandomParams( &stat, &rparams ), &stat );
  }

  /* Compute stepsizes. */
  dx = dy = dz = 0.0;
  if ( nx > 1 )
    dx = ( x_2 - x_1 )/( nx - 1.0 );
  if ( ny > 1 )
    dy = ( y_2 - y_1 )/( ny - 1.0 );
  if ( nz > 1 )
    dz = ( z_2 - z_1 )/( nz - 1.0 );

  /*******************************************************************
   * PERFORM TEST                                                    *
   *******************************************************************/

  /* Loop over each direction. */
  for ( i = 0; i < nx; i++ ) {
    x = LAL_REARTH_SI*( x_1 + i*dx );
    for ( j = 0; j < ny; j++ ) {
      y = LAL_REARTH_SI*( y_1 + j*dy );
      for ( k = 0; k < nz; k++ ) {
	z = LAL_REARTH_SI*( z_1 + k*dz );

/*
 * Fast quasi-reduction modulo p256 (FIPS 186-3 D.2.3)
 */
static int ecp_mod_p256(ttls_mpi *N)
{
	INIT(256);

	ADD( 8); ADD( 9);
	SUB(11); SUB(12); SUB(13); SUB(14);			 NEXT; // A0

	ADD( 9); ADD(10);
	SUB(12); SUB(13); SUB(14); SUB(15);			 NEXT; // A1

	ADD(10); ADD(11);
	SUB(13); SUB(14); SUB(15);			NEXT; // A2

	ADD(11); ADD(11); ADD(12); ADD(12); ADD(13);
	SUB(15); SUB( 8); SUB( 9);			NEXT; // A3

	ADD(12); ADD(12); ADD(13); ADD(13); ADD(14);
	SUB( 9); SUB(10);		   NEXT; // A4

	ADD(13); ADD(13); ADD(14); ADD(14); ADD(15);
	SUB(10); SUB(11);		   NEXT; // A5

	ADD(14); ADD(14); ADD(15); ADD(15); ADD(14); ADD(13);
	SUB( 8); SUB( 9);		   NEXT; // A6

	ADD(15); ADD(15); ADD(15); ADD(8);
	SUB(10); SUB(11); SUB(12); SUB(13);			 LAST; // A7

cleanup:
	return ret;
}

/* compute intersection of two convex polyhedrons */
TRI* cvi (double *va, int nva, double *pa, int npa, double *vb, int nvb, double *pb, int npb, CVIKIND kind, int *m, double **pv, int *nv)
{
  double e [6], p [3], q [3], eps, d, *nl, *pt, *nn, *yy;
  PFV *pfv, *v, *w, *z;
  int i, j, k, n;
  TRI *tri, *t;

  /* initialize */
  eps = GEOMETRIC_EPSILON;
  tri = t = NULL;
  pfv = NULL;
  yy = NULL;

  /* compute closest points */
  d = gjk (va, nva, vb, nvb, p, q);
  if (d > GEOMETRIC_EPSILON) { *m = 0; return NULL; }

  /* push 'p' deeper inside only if regularized intersection is sought */
  if (kind == REGULARIZED && !refine_point (pa, npa, pb, npb, p, &eps)) { *m = 0; return NULL; }

  /* vertices extents for a later sanity check */
  vertices_extents (va, nva, vb, nvb, eps, e);

  /* translate base points of planes so that
   * p = q = 0; compute new normals 'yy' */
  ERRMEM (yy = malloc (sizeof (double [3]) * (npa+npb)));
  for (i = 0, nl = pa, pt = pa + 3, nn = yy;
       i < npa; i ++, nl += 6, pt += 6, nn += 3)
  {
    SUB (pt, p, q); /* q => translated point of current plane */
    d = - DOT (nl, q); /* d => zero offset */
    if (d > -GEOMETRIC_EPSILON) d = -eps; /* regularisation (tiny swelling) */
    DIV (nl, -d, nn);  /* <nn, x> <= 1 (yy stores vertices of polar polygon) */
  }
  for (i = 0, nl = pb, pt = pb + 3; i < npb;
       i ++, nl += 6, pt += 6, nn += 3)
  {
    SUB (pt, p, q);
    d = - DOT (nl, q);
    if (d > -GEOMETRIC_EPSILON) d = -eps; /* regularisation (tiny swelling) */
    DIV (nl, -d, nn);
  }

  /* compute and polarise convex
   * hull of new normals 'yy' */
  if (!(tri = hull (yy, npa+npb, &i))) goto error; /* tri = cv (polar (a) U polar (b)) */
  if (!(pfv = TRI_Polarise (tri, i, &j))) goto error; /* pfv = polar (tri) => pfv = a * b */

  /* normals in 'pfv' point to 'yy'; triangulate
   * polar faces and set 'a' or 'b' flags */

  /* count all face vertices */
  for (k = n = 0; k < j; k ++)
  {
    v = &pfv [k]; n ++;
    for (w = v->n; w != v; w = w->n) n ++;
  }
  /* there is (number of face vertices - 2) triangles per face,
   * hence there is n - j * 2 triangles in total; vertex
   * memory in 'pfv' is placed after n PFV items */
#if GEOMDEBUG
  ASSERT_DEBUG (n - j*2 > 3, "Inconsitent polar faces => too few vertices in some faces");
#else
  if (n - j*2 <= 3) goto error;
#endif
  ERRMEM (tri = realloc (tri, sizeof (TRI) * (n-j*2) + sizeof (double [3]) * i)); /* allocate space for triangles and vertices */
  pt = (double*) (tri + (n - j*2)); /* this is where output vertices begin */
  nn = (double*) (pfv + n); /* this is where coords begin in 'pfv' block */
  memcpy (pt, nn, sizeof (double [3]) * i); /* copy vertex data */
  if (pv) *pv = pt;
  if (nv) *nv = i;

  /* shift point coords to the old 'zero' */
  for (k = 0, nl = pt; k < i; k ++, nl += 3)
  { 
    ADD (nl, p, nl); /* 'nl' used as a point */

    if (nl[0] < e [0] || nl[1] < e [1] || nl[2] < e [2] ||
	nl[0] > e [3] || nl[1] > e [4] || nl[2] > e [5])
    {
#if GEOMDEBUG
      printf ("CVI HAS GONE INSANE FOR THE INPUT:\n"), dump_input (va, nva, pa, npa, vb, nvb, pb, npb);
#endif
      goto error;
    }
  }

  for (k = 0, t = tri; k < j; k ++)
  {
    v = &pfv [k]; /* fixed vertex 'v' */
    for (w = v->n, z = w->n; z != v; w = w->n, z = z->n, t ++) /* remaining vertices 'w' & 'z' */
    {
      COPY (v->nl, t->out); /* copy normal */
      NORMALIZE (t->out);
      t->ver [0] = pt + (v->coord - nn); /* map vertices */
      t->ver [1] = pt + (w->coord - nn);
      t->ver [2] = pt + (z->coord - nn);
      t->flg = ((v->nl - yy) / 3) + 1; /* first 'npa' entries in 'yy' come from 'a' => positive 1-based index in 'a' */
      if (t->flg > npa) t->flg = -(t->flg - npa); /* last 'npb' ones come from 'b' => negative 1-based index in 'b' */
//.........这里部分代码省略.........

int main() {

	// distribute bodies in space (randomly)
	for(int i=0; i<N; i++) {
		B[i].m = (i < L)?1:0;
		B[i].pos = triple_rand();
//		B[i].pos = (position) { 0, -10 + 20*(i/2), -10 + 20*(i%2) };		// for debugging!
		B[i].v   = triple_zero();
	}
	// run simulation for M steps
	#pragma omp parallel
	for(int i=0; i<M; i++) {
		
		// set forces to zero
		#pragma omp for
		for(int j=0; j<N; j++) {
			F[j] = triple_zero();
		}

		// reset private copy
		for(int j=0; j<N; j++) {
			pF[j] = triple_zero();
		}

		// compute forces for each body (very naive)
		#pragma omp for
		for(int j=0; j<min(N,L); j++) {
			for(int k=0; k<N; k++) {

				if(j!=k) {
					// comput distance vector
					triple dist = SUB(B[k].pos, B[j].pos);

					// compute absolute distance
					double r = ABS(dist);
				
					// compute strength of force (G = 1 (who cares))
					//			F = G * (m1 * m2) / r^2
					double f = (B[j].m * B[k].m) / (r*r);

					// compute current contribution to force
					//force cur = MULS(NORM(dist), f);
					double s = f / r;
					force cur = MULS(dist,s);

					// accumulate force
					pF[j] = ADD(pF[j], cur);
				}
			}
		}

		// aggregate local data
		#pragma omp critical
		for(int j=0; j<N; j++) {
			F[j] = ADD(pF[j],F[j]);
		}

		// apply forces
		#pragma omp for
		for(int j=0; j<min(N,L); j++) {
			// update speed
			//		F = m * a
			//		a = F / m		// m=1
			//		v' = v + a
			B[j].v = ADD(B[j].v, DIVS(F[j], B[j].m));

			// update position
			//		pos = pos + v * dt		// dt = 1
			B[j].pos = ADD(B[j].pos, B[j].v);
		}

/*		// debug print of positions and speed
		for(int i=0; i<N; i++) {
			printf("%2d - ", i); 
			triple_print(B[i].pos);
			printf(" - ");
			triple_print(B[i].v);
			printf("\n");
		}
		printf("\n");
*/

	}

	// check result (impulse has to be zero)
	impulse sum = triple_zero();
	for(int i=0; i<N; i++) {
		// impulse = m * v
		sum = ADD(sum, MULS(B[i].v,B[i].m));
	}
	int success = EQ(sum, triple_zero());
	printf("Verification: %s\n", ((success)?"OK":"ERR"));
	if (!success) {
		triple_print(sum); printf(" should be (0,0,0)\n");
		return EXIT_FAILURE;
	}
	return EXIT_SUCCESS;
}

void foo()
{
  int add = ADD(505);
  int sub = SUB(525);
}

//.........这里部分代码省略.........
        REV(registro+instruction.op1_value, *(registro+instruction.op2_value));
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"REV16") ==0)
    {
        *codificacion=(11<<12)+(5<<9)+(1<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        REV16(registro+instruction.op1_value,*(registro+instruction.op2_value));
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"REVSH") ==0)
    {
        *codificacion=(11<<12)+(5<<9)+(3<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        REVSH(registro+instruction.op1_value,*(registro+instruction.op2_value));
        registro[15]++;
    }
	if( strcmp(instruction.mnemonic,"RORS") ==0)
    {
        *codificacion=(1<<14)+(7<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        RORS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
        registro[15]++;
	}
    if( strcmp(instruction.mnemonic,"RSBS") ==0)
    {
        *codificacion=(1<<14)+(9<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        RSBS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"SBCS") ==0)
    {
        *codificacion=(1<<14)+(3<<7)+(instruction.op2_value<<3)+instruction.op1_value;
        SBCS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
        registro[15]++;
	}
	if( strcmp(instruction.mnemonic,"SUB") ==0)
    {
        if((instruction.op1_type=='S') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
        {
            SUB(registro+13,*(registro+13),instruction.op3_value);
            *codificacion=(11<<12)+(1<<7)+instruction.op3_value;
        }
        registro[15]++;
    }
	if( strcmp(instruction.mnemonic,"SUBS") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
		{
			SUBS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
			*codificacion=(15<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
        {
            SUBS(registro+instruction.op1_value,*(registro+instruction.op1_value),instruction.op2_value,bandera);
            *codificacion=(7<<11)+(instruction.op1_value<<8)+instruction.op2_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
		{
			SUBS(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value), bandera);
			*codificacion=(13<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        registro[15]++;
	}
    if( strcmp(instruction.mnemonic,"TST") ==0)
    {
        *codificacion=(1<<14)+(1<<9)+(instruction.op2_value<<3)+instruction.op1_value;
        TST(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);	// Como parametros se tienen el contenido de un registro y un valor
        registro[15]++;