コード例 #1
0
ファイル: simu.cpp プロジェクト: 702nADOS/speed-dreams
/*
 * Check the input control from robots
 */
static void
ctrlCheck(tCar *car)
{
    tTransmission	*trans = &(car->transmission);
    tClutch		*clutch = &(trans->clutch);

    /* sanity check */
#ifndef WIN32
    if (isnan(car->ctrl->accelCmd) || isinf(car->ctrl->accelCmd)) car->ctrl->accelCmd = 0;
    if (isnan(car->ctrl->brakeCmd) || isinf(car->ctrl->brakeCmd)) car->ctrl->brakeCmd = 0;
    if (isnan(car->ctrl->clutchCmd) || isinf(car->ctrl->clutchCmd)) car->ctrl->clutchCmd = 0;
    if (isnan(car->ctrl->steer) || isinf(car->ctrl->steer)) car->ctrl->steer = 0;
    if (isnan(car->ctrl->gear) || isinf(car->ctrl->gear)) car->ctrl->gear = 0;
#else
    if (isnan(car->ctrl->accelCmd)) car->ctrl->accelCmd = 0;
    if (isnan(car->ctrl->brakeCmd)) car->ctrl->brakeCmd = 0;
    if (isnan(car->ctrl->clutchCmd)) car->ctrl->clutchCmd = 0;
    if (isnan(car->ctrl->steer)) car->ctrl->steer = 0;
    if (isnan(car->ctrl->gear)) car->ctrl->gear = 0;
#endif

    /* When the car is broken try to send it on the track side */
    if (car->carElt->_state & RM_CAR_STATE_BROKEN) {
	car->ctrl->accelCmd = 0.0f;
	car->ctrl->brakeCmd = 0.1f;
	car->ctrl->gear = 0;
	if (car->trkPos.toRight >  car->trkPos.seg->width / 2.0) {
	    car->ctrl->steer = 0.1f;
	} else {
	    car->ctrl->steer = -0.1f;
	}
    } else if (car->carElt->_state & RM_CAR_STATE_ELIMINATED) {
	car->ctrl->accelCmd = 0.0f;
	car->ctrl->brakeCmd = 0.1f;
	car->ctrl->gear = 0;
	if (car->trkPos.toRight >  car->trkPos.seg->width / 2.0) {
	    car->ctrl->steer = 0.1f;
	} else {
	    car->ctrl->steer = -0.1f;
	}
    } else if (car->carElt->_state & RM_CAR_STATE_FINISH) {
	/* when the finish line is passed, continue at "slow" pace */
	car->ctrl->accelCmd = (tdble) MIN(car->ctrl->accelCmd, 0.20);
	if (car->DynGC.vel.x > 30.0) {
	    car->ctrl->brakeCmd = (tdble) MAX(car->ctrl->brakeCmd, 0.05);
	}
    }

    /* check boundaries */
    if (car->ctrl->accelCmd > 1.0) {
	car->ctrl->accelCmd = 1.0;
    } else if (car->ctrl->accelCmd < 0.0) {
	car->ctrl->accelCmd = 0.0;
    }
    if (car->ctrl->brakeCmd > 1.0) {
	car->ctrl->brakeCmd = 1.0;
    } else if (car->ctrl->brakeCmd < 0.0) {
	car->ctrl->brakeCmd = 0.0;
    }
    if (car->ctrl->clutchCmd > 1.0) {
	car->ctrl->clutchCmd = 1.0;
    } else if (car->ctrl->clutchCmd < 0.0) {
	car->ctrl->clutchCmd = 0.0;
    }
    if (car->ctrl->steer > 1.0) {
	car->ctrl->steer = 1.0;
    } else if (car->ctrl->steer < -1.0) {
	car->ctrl->steer = -1.0;
    }

    clutch->transferValue = 1.0f - car->ctrl->clutchCmd;
}
コード例 #2
0
ファイル: echo.c プロジェクト: msherry/kodama-orig
static float nlms_pw(echo *e, float tx, float rx, int update)
{
    int j = e->j;

    e->x[j] = rx;
    e->xf[j] = iir_highpass(e->Fx, rx); /* pre-whitening of x */

    float dotp_w_x = dotp(e->w, e->x+j);
    float err = tx - dotp_w_x;
    float ef = iir_highpass(e->Fe, err); /* pre-whitening of err */
    if (isnan(ef))
    {
        DEBUG_LOG("%s\n", "ef went NaN");
        stack_trace(1);
    }

    /* Iterative update */
    e->dotp_xf_xf += (e->xf[j] * e->xf[j] -
        e->xf[j+NLMS_LEN-1] * e->xf[j+NLMS_LEN-1]);

    if (e->dotp_xf_xf == 0.0)
    {
        DEBUG_LOG("%s\n", "dotp_xf_xf went to zero");
        int i;
        for (i = 0; i < NLMS_LEN; i++)
        {
            DEBUG_LOG("%.02f ", e->xf[j+i]);
        }
        DEBUG_LOG("%s\n\n", "");
        stack_trace(1);
    }

    if (update)
    {
        float u_ef = STEPSIZE * ef / e->dotp_xf_xf;
        if (isinf(u_ef))
        {
            DEBUG_LOG("%s\n", "u_ef went infinite");
            DEBUG_LOG("ef: %f\tdotp_xf_xf: %f\n", ef, e->dotp_xf_xf);
            stack_trace(1);
        }

        /* Update tap weights */
        int i;
        for (i = 0; i < NLMS_LEN; i += 2)
        {
            e->w[i] += u_ef*e->xf[j+i];
            e->w[i+1] += u_ef*e->xf[j+i+1];
        }
    }

    /* Keep us within our sample buffers */
    if (--e->j < 0)
    {
        e->j = NLMS_EXT;
        memmove(e->x+e->j+1, e->x, (NLMS_LEN-1)*sizeof(float));
        memmove(e->xf+e->j+1, e->xf, (NLMS_LEN-1)*sizeof(float));
    }

    return err;
}
コード例 #3
0
ファイル: Note_map.c プロジェクト: cyberixae/kunquat
const Sample_entry* Note_map_get_entry(
        const Note_map* map,
        double cents,
        double force,
        Random* random)
{
    assert(map != NULL);
    assert(isfinite(cents));
    assert(isfinite(force) || (isinf(force) && force < 0));
    assert(random != NULL);

    Random_list* key =
        &(Random_list){ .force = force, .freq = NAN, .cents = cents };
    Random_list* estimate_low = AAiter_get_at_most(map->iter, key);
    Random_list* choice = NULL;
    double choice_d = INFINITY;

    if (estimate_low != NULL)
    {
        choice = estimate_low;
        choice_d = distance(choice, key);
        double min_tone = key->cents - choice_d;
        Random_list* candidate = AAiter_get_prev(map->iter);
        while (candidate != NULL && candidate->cents >= min_tone)
        {
            double d = distance(candidate, key);
            if (d < choice_d)
            {
                choice = candidate;
                choice_d = d;
                min_tone = key->cents - choice_d;
            }
            candidate = AAiter_get_prev(map->iter);
        }
    }

    Random_list* estimate_high = AAiter_get_at_least(map->iter, key);
    if (estimate_high != NULL)
    {
        double d = distance(estimate_high, key);
        if (choice == NULL || choice_d > d)
        {
            choice = estimate_high;
            choice_d = d;
        }

        double max_tone = key->cents + choice_d;
        Random_list* candidate = AAiter_get_next(map->iter);
        while (candidate != NULL && candidate->cents <= max_tone)
        {
            d = distance(candidate, key);
            if (d < choice_d)
            {
                choice = candidate;
                choice_d = d;
                max_tone = key->cents + choice_d;
            }
            candidate = AAiter_get_next(map->iter);
        }
    }

    if (choice == NULL)
    {
//        fprintf(stderr, "empty map\n");
        return NULL;
    }

    assert(choice->entry_count > 0);
    assert(choice->entry_count < NOTE_MAP_RANDOMS_MAX);
//    state->middle_tone = choice->freq;
    int index = Random_get_index(random, choice->entry_count);
    assert(index >= 0);
//    fprintf(stderr, "%d\n", index);

    return &choice->entries[index];
}
コード例 #4
0
ファイル: isnan.c プロジェクト: FluidityProject/multifluids
//to call floating-point check from the Fortran routine
void fpcheck_c_( double *a, int *flag ) 
{ 
   *flag = isnan(*a) && !isinf(*a); 
}
コード例 #5
0
ファイル: mavlink_parameters.c プロジェクト: 0919061/mavlink
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg)
{
	switch (msg->msgid)
	{
		case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
			{
				// Start sending parameters
				pm.next_param = 0;
				mavlink_missionlib_send_gcs_string("PM SENDING LIST");
			}
			break;
		case MAVLINK_MSG_ID_PARAM_SET:
		{
			mavlink_param_set_t set;
			mavlink_msg_param_set_decode(msg, &set);
			
			// Check if this message is for this system
			if (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid)
			{
				char* key = set.param_id;
				
				for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++)
				{
					bool match = true;
					for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++)
					{
						// Compare
						if (pm.param_names[i][j] != key[j])
						{
							match = false;
						}
						
						// End matching if null termination is reached
						if (pm.param_names[i][j] == '\0')
						{
							break;
						}
					}
					
					// Check if matched
					if (match)
					{
						// Only write and emit changes if there is actually a difference
						// AND only write if new value is NOT "not-a-number"
						// AND is NOT infinity
						if (pm.param_values[i] != set.param_value
							&& !isnan(set.param_value)
							&& !isinf(set.param_value))
						{
							pm.param_values[i] = set.param_value;
							// Report back new value
#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS
							mavlink_message_t tx_msg;
							mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
															  mavlink_system.compid,
															  MAVLINK_COMM_0,
															  &tx_msg,
															  pm.param_names[i],
															  pm.param_values[i],
															  MAVLINK_TYPE_FLOAT,
															  pm.size,
															  i);
							mavlink_missionlib_send_message(&tx_msg);
#else
							mavlink_msg_param_value_send(MAVLINK_COMM_0,
														 pm.param_names[i],
														 pm.param_values[i],
														 MAVLINK_TYPE_FLOAT,
														 pm.size,
														 i);
#endif
							
							mavlink_missionlib_send_gcs_string("PM received param");
						} // End valid value check
					} // End match check
				} // End for loop
		} // End system ID check
		
	} // End case
			break;
			
	} // End switch
	
}
コード例 #6
0
ファイル: integrator_wh.c プロジェクト: dchandan/rebound
static int wh_check_normal(struct reb_particle* p){
	if (isnan(p->vx) || isnan(p->vy)) return 1;
	if (isinf(p->vx) || isinf(p->vy)) return 2;
	return 0;
}
コード例 #7
0
bool LogisticRegression::train(LabelledRegressionData &trainingData){
    
    const unsigned int M = trainingData.getNumSamples();
    const unsigned int N = trainingData.getNumInputDimensions();
    const unsigned int K = trainingData.getNumTargetDimensions();
    trained = false;
    trainingResults.clear();
    
    if( M == 0 ){
        errorMessage = "train(LabelledRegressionData &trainingData) - Training data has zero samples!";
        errorLog << errorMessage << endl;
        return false;
    }
    
    if( K == 0 ){
        errorMessage = "train(LabelledRegressionData &trainingData) - The number of target dimensions is not 1!";
        errorLog << errorMessage << endl;
        return false;
    }
    
    numFeatures = N;
    numOutputDimensions = 1; //Logistic Regression will have 1 output
    inputVectorRanges.clear();
    targetVectorRanges.clear();
    
    //Scale the training and validation data, if needed
	if( useScaling ){
		//Find the ranges for the input data
        inputVectorRanges = trainingData.getInputRanges();
        
        //Find the ranges for the target data
		targetVectorRanges = trainingData.getTargetRanges();
        
		//Scale the training data
		trainingData.scale(inputVectorRanges,targetVectorRanges,0.0,1.0);
	}
    
    //Reset the weights
    Random rand;
    w0 = rand.getRandomNumberUniform(-0.1,0.1);
    w.resize(N);
    for(UINT j=0; j<N; j++){
        w[j] = rand.getRandomNumberUniform(-0.1,0.1);
    }

    double error = 0;
    double squaredError = 0;
    double lastSquaredError = 0;
    double delta = 0;
    UINT iter = 0;
    bool keepTraining = true;
    Random random;
    vector< UINT > randomTrainingOrder(M);
    TrainingResult result;
    trainingResults.reserve(M);
    
    //In most cases, the training data is grouped into classes (100 samples for class 1, followed by 100 samples for class 2, etc.)
    //This can cause a problem for stochastic gradient descent algorithm. To avoid this issue, we randomly shuffle the order of the
    //training samples. This random order is then used at each epoch.
    for(UINT i=0; i<M; i++){
        randomTrainingOrder[i] = i;
    }
    std::random_shuffle(randomTrainingOrder.begin(), randomTrainingOrder.end());
    
    //Run the main stochastic gradient descent training algorithm
    while( keepTraining ){
        
        //Run one epoch of training using stochastic gradient descent
        squaredError = 0;
        for(UINT m=0; m<M; m++){
            
            //Select the random sample
            UINT i = randomTrainingOrder[m];
            
            //Compute the error, given the current weights
            VectorDouble x = trainingData[i].getInputVector();
            VectorDouble y = trainingData[i].getTargetVector();
            double h = w0;
            for(UINT j=0; j<N; j++){
                h += x[j] * w[j];
            }
            error = y[0] - sigmoid( h );
            squaredError += SQR(error);
            
            //Update the weights
            for(UINT j=0; j<N; j++){
                w[j] += learningRate  * error * x[j];
            }
            w0 += learningRate * error;
        }
        
        //Compute the error
        delta = fabs( squaredError-lastSquaredError );
        lastSquaredError = squaredError;
        
        //Check to see if we should stop
        if( delta <= minChange ){
            keepTraining = false;
        }
        
        if( ++iter >= maxNumIterations ){
            keepTraining = false;
        }
        
        if( isinf( squaredError ) || isnan( squaredError ) ){
            errorMessage = "train(LabelledRegressionData &trainingData) - Training failed! Total squared error is NAN. If scaling is not enabled then you should try to scale your data and see if this solves the issue.";
            errorLog << errorMessage << endl;
            return false;
        }
        
        //Store the training results
        result.setRegressionResult(iter,totalSquaredTrainingError,rootMeanSquaredTrainingError);
        trainingResults.push_back( result );
        
        //Notify any observers of the new training data
        trainingResultsObserverManager.notifyObservers( result );
        
        trainingLog << "Epoch: " << iter << " SSE: " << squaredError << " Delta: " << delta << endl;
    }
    
    //Flag that the algorithm has been trained
    regressionData.resize(1,0);
    trained = true;
    return trained;
}
コード例 #8
0
ファイル: volume.c プロジェクト: KimT/pulseaudiokt
pa_volume_t pa_sw_volume_from_dB(double dB) {
    if (isinf(dB) < 0 || dB <= PA_DECIBEL_MININFTY)
        return PA_VOLUME_MUTED;

    return pa_sw_volume_from_linear(dB_to_linear(dB));
}
コード例 #9
0
ファイル: e_gammal_r.c プロジェクト: JamesLinus/glibc-mips
long double
__ieee754_gammal_r (long double x, int *signgamp)
{
  u_int32_t es, hx, lx;
  long double ret;

  GET_LDOUBLE_WORDS (es, hx, lx, x);

  if (__glibc_unlikely (((es & 0x7fff) | hx | lx) == 0))
    {
      /* Return value for x == 0 is Inf with divide by zero exception.  */
      *signgamp = 0;
      return 1.0 / x;
    }
  if (__glibc_unlikely (es == 0xffffffff && ((hx & 0x7fffffff) | lx) == 0))
    {
      /* x == -Inf.  According to ISO this is NaN.  */
      *signgamp = 0;
      return x - x;
    }
  if (__glibc_unlikely ((es & 0x7fff) == 0x7fff))
    {
      /* Positive infinity (return positive infinity) or NaN (return
	 NaN).  */
      *signgamp = 0;
      return x + x;
    }
  if (__builtin_expect ((es & 0x8000) != 0, 0) && __rintl (x) == x)
    {
      /* Return value for integer x < 0 is NaN with invalid exception.  */
      *signgamp = 0;
      return (x - x) / (x - x);
    }

  if (x >= 1756.0L)
    {
      /* Overflow.  */
      *signgamp = 0;
      return LDBL_MAX * LDBL_MAX;
    }
  else
    {
      SET_RESTORE_ROUNDL (FE_TONEAREST);
      if (x > 0.0L)
	{
	  *signgamp = 0;
	  int exp2_adj;
	  ret = gammal_positive (x, &exp2_adj);
	  ret = __scalbnl (ret, exp2_adj);
	}
      else if (x >= -LDBL_EPSILON / 4.0L)
	{
	  *signgamp = 0;
	  ret = 1.0L / x;
	}
      else
	{
	  long double tx = __truncl (x);
	  *signgamp = (tx == 2.0L * __truncl (tx / 2.0L)) ? -1 : 1;
	  if (x <= -1766.0L)
	    /* Underflow.  */
	    ret = LDBL_MIN * LDBL_MIN;
	  else
	    {
	      long double frac = tx - x;
	      if (frac > 0.5L)
		frac = 1.0L - frac;
	      long double sinpix = (frac <= 0.25L
				    ? __sinl (M_PIl * frac)
				    : __cosl (M_PIl * (0.5L - frac)));
	      int exp2_adj;
	      ret = M_PIl / (-x * sinpix
			     * gammal_positive (-x, &exp2_adj));
	      ret = __scalbnl (ret, -exp2_adj);
	      math_check_force_underflow_nonneg (ret);
	    }
	}
    }
  if (isinf (ret) && x != 0)
    {
      if (*signgamp < 0)
	return -(-__copysignl (LDBL_MAX, ret) * LDBL_MAX);
      else
	return __copysignl (LDBL_MAX, ret) * LDBL_MAX;
    }
  else if (ret == 0)
    {
      if (*signgamp < 0)
	return -(-__copysignl (LDBL_MIN, ret) * LDBL_MIN);
      else
	return __copysignl (LDBL_MIN, ret) * LDBL_MIN;
    }
  else
    return ret;
}
コード例 #10
0
ファイル: trsm.c プロジェクト: qq332982511/libxsmm
int main(int argc, char* argv[])
{
  unsigned int m=8, n=8, lda=8, ldb=8, nerrs, num, nmat, nmats, nmatd, ntest;
  unsigned int layout, asize, VLEND=4, VLENS=8, bsize;
  unsigned int ncorr;
  int i, j;
  char side, uplo, trans, diag;
  unsigned int typesize8 = 8;
  unsigned int typesize4 = 4;
  float  *sa, *sb, *sc, *sd;
  double *da, *db, *dc, *dd, *tmpbuf;
  double dalpha = 1.0;
  float  salpha;
  double dtmp;
  const unsigned char *cptr;
  unsigned long op_count;
  const libxsmm_trsm_descriptor* desc8 = NULL;
  const libxsmm_trsm_descriptor* desc4 = NULL;
  libxsmm_descriptor_blob blob;
  union {
    libxsmm_xtrsmfunction dp;
    libxsmm_xtrsmfunction sp;
    const void* pv;
  } mykernel = { 0 };
#ifdef USE_KERNEL_GENERATION_DIRECTLY
  void (*opcode_routine)();
#endif
#ifdef USE_KERNEL_GENERATION_DIRECTLY
  #include <unistd.h>
  #include <signal.h>
  #include <malloc.h>
  #include <sys/mman.h>
  #include "../../src/generator_packed_trsm_avx_avx512.h"
  unsigned char *routine_output;
  libxsmm_generated_code io_generated_code;
  int pagesize = sysconf(_SC_PAGE_SIZE);
  if (pagesize == -1) fprintf(stderr,"sysconf pagesize\n");
  routine_output = (unsigned char *) mmap(NULL,
                      BUFSIZE2, PROT_READ|PROT_WRITE,
                      MAP_PRIVATE|MAP_ANONYMOUS, 0,0);
  if (mprotect(routine_output, BUFSIZE2,
                PROT_EXEC | PROT_READ | PROT_WRITE ) == -1)
      fprintf(stderr,"mprotect\n");
  printf("Routine ready\n");
  io_generated_code.generated_code = &routine_output[0];
  io_generated_code.buffer_size = BUFSIZE2;
  io_generated_code.code_size = 0;
  io_generated_code.code_type = 2;
  io_generated_code.last_error = 0;
#endif

  if ( argc <= 3 )
  {
     printf("\nUSAGE: %s m n lda ldb nmat side uplo trans diag layout ntest alpha\n",argv[0]);
     printf("Compact TRSM a mxn matrix of leading dimension ldb\n");
     printf("This will test the jit of 1 VLEN work of nmat at a time\n");
     printf("Defaults: m=n=lda=ldb=nmat=8, alpha=1.0, side=uplo='L',trans=diag='N',layout=102,ntest=1\n");
  }
  if ( argc > 1 ) m = atoi(argv[1]); else m = 8;
  if ( argc > 2 ) n = atoi(argv[2]); else n = 8;
  if ( argc > 3 ) lda= atoi(argv[3]); else lda = 8;
  if ( argc > 4 ) ldb = atoi(argv[4]); else ldb = 8;
  if ( argc > 5 ) nmat = atoi(argv[5]); else nmat = 8;
  if ( argc > 6 ) side = argv[6][0]; else side = 'L';
  if ( argc > 7 ) uplo = argv[7][0]; else uplo = 'L';
  if ( argc > 8 ) trans = argv[8][0]; else trans = 'N';
  if ( argc > 9 ) diag = argv[9][0]; else diag = 'N';
  if ( argc > 10 ) layout = atoi(argv[10]); else layout=102;
  if ( argc > 11 ) ntest = atoi(argv[11]); else ntest = 1;
  if ( argc > 12 ) dalpha = atof(argv[12]); else dalpha = 1.0;
  salpha = (float)dalpha;

  m = LIBXSMM_MAX(m,1);
  n = LIBXSMM_MAX(n,1);
  /* A is either mxm or nxn depending on side */
  if ( (side == 'L') || (side=='l') ) asize = m; else asize = n;

  lda = LIBXSMM_MAX(lda,asize);
  if ( layout == 102 )
  {
      /* Column major: B is mxn, and stored in B format */
      ldb = LIBXSMM_MAX(ldb,m);
      bsize = ldb*n;
  } else {
      /* Row major: B is mxn, and stored in B^T format */
      ldb = LIBXSMM_MAX(ldb,n);
      bsize = ldb*m;
  }
  nmats = LIBXSMM_MAX(VLENS,nmat - (nmat%VLENS));
  nmatd = LIBXSMM_MAX(VLEND,nmat - (nmat%VLEND));
  nmat = LIBXSMM_MAX(nmats,nmatd);

  op_count = n * m * asize;

  printf("This is a real*%u tester for JIT compact TRSM kernels! (%c%c%c%c m=%u n=%u lda=%u ldb=%u layout=%u nmat=%u)\n",typesize8,side,uplo,trans,diag,m,n,lda,ldb,layout,nmat);
#ifdef USE_XSMM_GENERATED
  printf("This code tests the LIBXSMM generated kernels\n");
#endif
#ifdef USE_PREDEFINED_ASSEMBLY
  printf("This code tests some predefined assembly kenrel\n");
#endif
#ifdef USE_KERNEL_GENERATION_DIRECTLY
  printf("This code tests kernel generation directly\n");
#endif
#ifdef TIME_MKL
  printf("This code tests MKL compact batch directly\n");
#endif

  desc8 = libxsmm_trsm_descriptor_init(&blob, typesize8, m, n, lda, ldb, &dalpha, trans, diag, side, uplo, layout);
  desc4 = libxsmm_trsm_descriptor_init(&blob, typesize4, m, n, lda, ldb, &salpha, trans, diag, side, uplo, layout);
#ifdef USE_XSMM_GENERATED
  printf("calling libxsmm_dispatch_trsm: typesize8=%u\n",typesize8);
  mykernel.dp = libxsmm_dispatch_trsm(desc8);
  printf("done calling libxsmm_dispatch_trsm: typesize8=%u\n",typesize8);
  if ( mykernel.dp == NULL ) printf("R8 Kernel after the create call is null\n");
  mykernel.sp = libxsmm_dispatch_trsm(desc4);
  if ( mykernel.sp == NULL ) printf("R4 kernel after the create call is null\n");
#endif

#ifdef USE_KERNEL_GENERATION_DIRECTLY
  libxsmm_generator_trsm_kernel ( &io_generated_code, &desc8, "hsw" );
#endif

#ifndef NO_ACCURACY_CHECK
  printf("mallocing matrices\n");
#endif
  sa  = (float  *) malloc ( lda*asize*nmats*sizeof(float) );
  da  = (double *) malloc ( lda*asize*nmatd*sizeof(double) );
  sb  = (float  *) malloc ( bsize*nmats*sizeof(float) );
  db  = (double *) malloc ( bsize*nmatd*sizeof(double) );
  sc  = (float  *) malloc ( bsize*nmats*sizeof(float) );
  dc  = (double *) malloc ( bsize*nmatd*sizeof(double) );
  sd  = (float  *) malloc ( bsize*nmats*sizeof(float) );
  dd  = (double *) malloc ( bsize*nmatd*sizeof(double) );
  tmpbuf = (double *) malloc ( asize*VLEND*sizeof(double) );

#ifndef NO_ACCURACY_CHECK
  printf("filling matrices\n");
#endif
  sfill_matrix ( sa, lda, asize, asize*nmats );
#ifdef TRIANGLE_IS_IDENTITY
  printf("Warning: setting triangular matrix to identity. Not good for accuracy testing\n");
  dfill_identity ( da, lda, asize, asize, VLEND, nmatd/VLEND );
#else
  dfill_matrix ( da, lda, asize, asize*nmatd );
#endif
  sfill_matrix ( sb, bsize, bsize, nmats );
  dfill_matrix ( db, bsize, bsize, nmatd );

#ifndef NO_ACCURACY_CHECK
  for ( i = 0 ; i < (int)(bsize*nmats) ; i++ ) sc[i]=sb[i];
  for ( i = 0 ; i < (int)(bsize*nmatd) ; i++ ) dc[i]=db[i];
  for ( i = 0 ; i < (int)(bsize*nmats) ; i++ ) sd[i]=sb[i];
  for ( i = 0 ; i < (int)(bsize*nmatd) ; i++ ) dd[i]=db[i];
  printf("Pointing at the kernel now\n");
#endif

#ifdef USE_XSMM_GENERATED
  cptr = (const unsigned char*) mykernel.pv;
#endif
#ifdef USE_PREDEFINED_ASSEMBLY
  cptr = (const unsigned char*) trsm_xct_;
#endif
#ifdef USE_KERNEL_GENERATION_DIRECTLY
  cptr = (const unsigned char*) &routine_output[0];
  opcode_routine = (void *) &cptr[0];
#endif

#ifndef TIME_MKL
  #define DUMP_ASSEMBLY_FILE
#endif

#ifdef DUMP_ASSEMBLY_FILE
  printf("Dumping assembly file\n");
  FILE *fp = fopen("foo.s","w");
  char buffer[80];
  fputs("\t.text\n",fp);
  fputs("\t.align 256\n",fp);
  fputs("\t.globl trsm_xct_\n",fp);
  fputs("trsm_xct_:\n",fp);
  for (i = 0 ; i < 4000; i+=4 )
  {
     sprintf(buffer,".byte 0x%02x, 0x%02x, 0x%02x, 0x%02x\n",cptr[i],cptr[i+1],cptr[i+2],cptr[i+3]);
     fputs(buffer,fp);
  }
  fputs("\tretq\n",fp);
  fputs("\t.type trsm_xct_,@function\n",fp);
  fputs("\t.size trsm_xct_,.-trsm_xct_\n",fp);
  fclose(fp);
#endif

#if defined(USE_MKL_FOR_REFERENCE) || defined(TIME_MKL)
  #include "mkl.h"
  MKL_LAYOUT CLAYOUT = (layout == 101) ? MKL_ROW_MAJOR : MKL_COL_MAJOR;
  MKL_SIDE SIDE = (side == 'R' || side == 'r') ? MKL_RIGHT : MKL_LEFT;
  MKL_UPLO UPLO = (uplo == 'U' || uplo == 'u') ? MKL_UPPER : MKL_LOWER;
  MKL_TRANSPOSE TRANSA = (trans == 'N' || trans == 'n') ? MKL_NOTRANS : MKL_TRANS;
  MKL_DIAG DIAG = (diag == 'N' || diag == 'n') ? MKL_NONUNIT : MKL_UNIT;
  MKL_COMPACT_PACK CMP_FORMAT = mkl_get_format_compact();
#if 0
  MKL_COMPACT_PACK CMP_FORMAT = MKL_COMPACT_AVX;
#endif
#endif

#ifndef NO_ACCURACY_CHECK
  printf("Before routine, initial B(1,1)=%g B[256]=%g\n",db[0],db[256]);
#endif
#ifdef USE_PREDEFINED_ASSEMBLY
  double one = 1.0;
#endif
  double timer;
#ifdef MKL_TIMER
  double tmptimer;
  tmptimer = dsecnd_();
#else
  unsigned long long l_start, l_end;
#endif

  timer = 0.0;
  for ( j = 0 ; j < (int)ntest ; j++ )
  {
#ifndef TRIANGLE_IS_IDENTITY
  for ( i = 0 ; i < (int)(bsize*nmatd) ; i++ ) db[i]=dd[i];
#endif
  for ( i = 0 , num = 0; i < (int)nmatd ; i+= (int)VLEND, num++ )
  {
     double *Ap = &da[num*lda*asize*VLEND];
     double *Bp = &db[num*bsize*VLEND];
#ifdef MKL_TIMER
     tmptimer = dsecnd_();
#else
     l_start = libxsmm_timer_tick();
#endif

#ifdef USE_XSMM_GENERATED
     mykernel.dp ( Ap, Bp, tmpbuf );
#endif
#ifdef USE_PREDEFINED_ASSEMBLY
     trsm_xct_ ( Ap, Bp, &one );
#endif
#ifdef USE_KERNEL_GENERATION_DIRECTLY
     (*opcode_routine)( Ap, Bp );
#endif
#ifdef TIME_MKL
     mkl_dtrsm_compact ( CLAYOUT, SIDE, UPLO, TRANSA, DIAG, m, n, dalpha, da, lda, db, ldb, CMP_FORMAT, nmatd );
     i+=nmatd; /* Because MKL will do everything */
#endif
#ifdef MKL_TIMER
     timer += dsecnd_() - tmptimer;
#else
     l_end = libxsmm_timer_tick();
     timer += libxsmm_timer_duration(l_start,l_end);
#endif
  }
  }
  timer /= ((double)ntest);

#ifndef NO_ACCURACY_CHECK
  printf("Average time to get through %u matrices: %g\n",nmatd,timer);
  printf("Gflops: %g\n",(double)(op_count*nmatd)/(timer*1.0e9));
  printf("after routine, new      B(1,1)=%g B[256]=%g\n",db[0],db[256]);
#endif

#ifdef TEST_SINGLE
  printf("Before r4 routine, initial B(1,1)=%g B[256]=%g\n",sb[0],sb[256]);
  for ( i = 0 , num = 0; i < nmats ; i+= VLENS, num++ )
  {
     float *Ap = &sa[num*lda*asize*VLENS];
     float *Bp = &sb[num*bsize*VLENS];
#ifdef USE_XSMM_GENERATED
     mykernel.sp ( Ap, Bp, NULL );
#endif
  }
  printf("after r4 routine, new      B(1,1)=%g B]256]=%g\n",db[0],db[256]);
#endif

#ifndef NO_ACCURACY_CHECK
  /* Call some reference code now on a copy of the B matrix (C) */
  double timer2 = 0.0;
  for ( j = 0 ; j < (int)ntest ; j++ )
  {
#ifndef TRIANGLE_IS_IDENTITY
  for ( i = 0 ; i < (int)(bsize*nmatd) ; i++ ) dc[i]=dd[i];
#endif

#ifdef MKL_TIMER
  tmptimer = dsecnd_();
#else
  l_start = libxsmm_timer_tick();
#endif

#ifdef USE_MKL_FOR_REFERENCE
  mkl_dtrsm_compact ( CLAYOUT, SIDE, UPLO, TRANSA, DIAG, m, n, dalpha, da, lda, dc, ldb, CMP_FORMAT, nmatd );
#elif !defined(LIBXSMM_NOFORTRAN)
  if ( (layout == 101) && (nmatd!=VLEND) )
  {
     unsigned int lay = 102, m1 = n, n1 = m;
     char side1='L', uplo1='L';
     if ( side == 'L' || side == 'l' ) side1 = 'R';
     if ( uplo == 'L' || uplo == 'l' ) uplo1 = 'U';
     compact_dtrsm_ ( &lay, &side1, &uplo1, &trans, &diag, &m1, &n1, &dalpha, da, &lda, dc, &ldb, &nmatd, &VLEND );
  } else {
     compact_dtrsm_ ( &layout, &side, &uplo, &trans, &diag, &m, &n, &dalpha, da, &lda, dc, &ldb, &nmatd, &VLEND );
  }
#endif

#ifdef MKL_TIMER
  timer2 += dsecnd_() - tmptimer;
#else
  l_end = libxsmm_timer_tick();
  timer2 += libxsmm_timer_duration(l_start,l_end);
#endif

  }
  timer2 /= ((double)ntest);
  printf("Reference time=%g Reference Gflops=%g\n",timer2,(op_count*nmatd)/(timer2*1.0e9));

  /* Compute the residual between B and C */
  dtmp = residual_d ( dc, bsize, bsize, nmatd, db, bsize, &nerrs, &ncorr );
  printf("R8 %c%c%c%c m=%u n=%u lda=%u ldb=%u error: %g number of errors: %u corrects: %u",side,uplo,trans,diag,m,n,lda,ldb,dtmp,nerrs,ncorr);
  if ( nerrs > 0 ) printf(" ->FAILED at %ux%u real*8 %u case",m,n,layout);
  printf("\n");

#ifdef TEST_SINGLE
  /* Call some reference code now on a copy of the B matrix (C) */
  compact_strsm_ ( &layout, &side, &uplo, &trans, &diag, &m, &n, &salpha, sa, &lda, sc, &ldb, &nmats, &VLENS );
  /* Compute the residual between B and C */
  dtmp = residual_s ( sc, bsize, bsize, nmats, sb, bsize, &nerrs, &ncorr );
  printf("R4 %c%c%c%c m=%u n=%u lda=%u ldb=%u error: %g number of errors: %u corrects: %u\n",side,uplo,trans,diag,m,n,lda,ldb,dtmp,nerrs,ncorr);
  if ( nerrs > 0 ) printf(" ->FAILED at %ux%u real*4 case",m,n);
  printf("\n");
#endif

#else
  for ( j = 0, nerrs = 0 ; j < bsize*nmatd ; j++ )
  {
     if ( isnan(db[j]) || isinf(db[j]) )
     {
        if ( ++nerrs < 10 )
        {
           printf("WARNING: db[%d]=%g\n",j,db[j]);
        }
     }
  }
  printf("%g,real*8 %c%c%c%c m=%u n=%u lda=%u ldb=%u Denormals=%u Time=%g Gflops=%g",(op_count*nmatd)/(timer*1.0e9),side,uplo,trans,diag,m,n,lda,ldb,nerrs,timer,(op_count*nmatd)/(timer*1.0e9));
  if ( nerrs > 0 ) printf(" -> FAILED at %ux%u real*8 case",m,n);
  printf("\n");
#endif

  free(dd);
  free(sd);
  free(dc);
  free(sc);
  free(db);
  free(sb);
  free(da);
  free(sa);

  return 0;
}
コード例 #11
0
ファイル: zend-printf.cpp プロジェクト: 2bj/hhvm
inline static void appenddouble(char **buffer, int *pos,
                                int *size, double number,
                                int width, char padding,
                                int alignment, int precision,
                                int adjust, char fmt,
                                int always_sign) {
  char num_buf[NUM_BUF_SIZE];
  char *s = nullptr;
  int s_len = 0, is_negative = 0;

  if ((adjust & ADJ_PRECISION) == 0) {
    precision = FLOAT_PRECISION;
  } else if (precision > MAX_FLOAT_PRECISION) {
    precision = MAX_FLOAT_PRECISION;
  }

  if (isnan(number)) {
    is_negative = (number<0);
    appendstring(buffer, pos, size, "NaN", 3, 0, padding,
                 alignment, 3, is_negative, 0, always_sign);
    return;
  }

  if (isinf(number)) {
    is_negative = (number<0);
    appendstring(buffer, pos, size, "INF", 3, 0, padding,
                 alignment, 3, is_negative, 0, always_sign);
    return;
  }

  switch (fmt) {
  case 'e':
  case 'E':
  case 'f':
  case 'F':
    s = php_conv_fp((fmt == 'f')?'F':fmt, number, 0, precision, '.',
                    &is_negative, &num_buf[1], &s_len);
    if (is_negative) {
      num_buf[0] = '-';
      s = num_buf;
      s_len++;
    } else if (always_sign) {
      num_buf[0] = '+';
      s = num_buf;
      s_len++;
    }
    break;

  case 'g':
  case 'G':
    if (precision == 0)
      precision = 1;
    /*
     * * We use &num_buf[ 1 ], so that we have room for the sign
     */
    s = php_gcvt(number, precision, '.', (fmt == 'G')?'E':'e', &num_buf[1]);
    is_negative = 0;
    if (*s == '-') {
      is_negative = 1;
      s = &num_buf[1];
    } else if (always_sign) {
      num_buf[0] = '+';
      s = num_buf;
    }

    s_len = strlen(s);
    break;
  }

  appendstring(buffer, pos, size, s, width, 0, padding,
               alignment, s_len, is_negative, 0, always_sign);
}
コード例 #12
0
/**
   Check for infinity.

   @param number An arbitrary floating-point number.
   @return 1 if positive infinity, -1 if negative infinity, 0 otherwise.
*/
TRIO_PUBLIC int
trio_isinf(TRIO_VOLATILE double number)
{
#if defined(TRIO_COMPILER_DECC)
  /*
   * DECC has an isinf() macro, but it works differently than that
   * of C99, so we use the fp_class() function instead.
   */
  return ((fp_class(number) == FP_POS_INF)
	  ? 1
	  : ((fp_class(number) == FP_NEG_INF) ? -1 : 0));
  
#elif defined(isinf)
  /*
   * C99 defines isinf() as a macro.
   */
  return isinf(number);
  
#elif defined(TRIO_COMPILER_MSVC)
  /*
   * MSVC has an _fpclass() function that can be used to detect infinity.
   */
  return ((_fpclass(number) == _FPCLASS_PINF)
	  ? 1
	  : ((_fpclass(number) == _FPCLASS_NINF) ? -1 : 0));

#elif defined(USE_IEEE_754)
  /*
   * Examine IEEE 754 bit-pattern. Infinity must have a special exponent
   * pattern, and an empty mantissa.
   */
  int has_mantissa;
  int is_special_quantity;

  is_special_quantity = trio_is_special_quantity(number, &has_mantissa);
  
  return (is_special_quantity && !has_mantissa)
    ? ((number < 0.0) ? -1 : 1)
    : 0;

#else
  /*
   * Fallback solution.
   */
  int status;
  
# if defined(TRIO_PLATFORM_UNIX)
  void (*signal_handler)(int) = signal(SIGFPE, SIG_IGN);
# endif
  
  double infinity = trio_pinf();
  
  status = ((number == infinity)
	    ? 1
	    : ((number == -infinity) ? -1 : 0));
  
# if defined(TRIO_PLATFORM_UNIX)
  signal(SIGFPE, signal_handler);
# endif
  
  return status;
  
#endif
}
コード例 #13
0
ファイル: igi.cpp プロジェクト: jwzhang/pbrt-v2
Spectrum IGIIntegrator::Li(const Scene *scene, const Renderer *renderer,
        const RayDifferential &ray, const Intersection &isect,
        const Sample *sample, MemoryArena &arena) const {
    Spectrum L(0.);
    Vector wo = -ray.d;
    // Compute emitted light if ray hit an area light source
    L += isect.Le(wo);

    // Evaluate BSDF at hit point
    BSDF *bsdf = isect.GetBSDF(ray, arena);
    const Point &p = bsdf->dgShading.p;
    const Normal &n = bsdf->dgShading.nn;
    L += UniformSampleAllLights(scene, renderer, arena, p, n,
                    wo, isect.rayEpsilon, bsdf, sample,
                    lightSampleOffsets, bsdfSampleOffsets);
    // Compute indirect illumination with virtual lights
    u_int lSet = min(u_int(sample->oneD[vlSetOffset][0] * nLightSets),
                     nLightSets-1);
    for (u_int i = 0; i < virtualLights[lSet].size(); ++i) {
        const VirtualLight &vl = virtualLights[lSet][i];
        // Compute virtual light's tentative contribution _Llight_
        float d2 = DistanceSquared(p, vl.p);
        Vector wi = Normalize(vl.p - p);
        float G = AbsDot(wi, n) * AbsDot(wi, vl.n) / d2;
        Spectrum f = bsdf->f(wo, wi);
        G = min(G, gLimit);
        if (G == 0.f || f.IsBlack()) continue;
        Spectrum Llight = f * G * vl.pathContrib / virtualLights[lSet].size();
        RayDifferential connectRay(p, wi, ray, isect.rayEpsilon,
            sqrtf(d2) * (1.f - vl.rayEpsilon));
        Llight *= renderer->Transmittance(scene, connectRay, NULL, arena,
                                          sample->rng);

        // Possibly skip virtual light shadow ray with Russian roulette
        if (Llight.y() < rrThreshold) {
            float continueProbability = .1f;
            if (sample->rng->RandomFloat() > continueProbability)
                continue;
            Llight /= continueProbability;
        }

        // Add contribution from _VirtualLight_ _vl_
        if (!scene->IntersectP(connectRay))
            L += Llight;
    }
    if (ray.depth < maxSpecularDepth) {
        // Do bias compensation for bounding geoemtry term
        int nSamples = (ray.depth == 0) ? nGatherSamples : 1;
        for (int i = 0; i < nSamples; ++i) {
            Vector wi;
            float pdf;
            BSDFSample bsdfSample = (ray.depth == 0) ?
                                    BSDFSample(sample, gatherSampleOffset, i) :
                                    BSDFSample(*sample->rng);
            Spectrum f = bsdf->Sample_f(wo, &wi, bsdfSample,
                                        &pdf, BxDFType(BSDF_ALL & ~BSDF_SPECULAR));
            if (!f.IsBlack() && pdf > 0.f) {
                // Trace ray for bias compensation gather sample
                float maxDist = sqrtf(AbsDot(wi, n) / gLimit);
                RayDifferential gatherRay(p, wi, ray, isect.rayEpsilon, maxDist);
                Intersection gatherIsect;
                Spectrum Li = renderer->Li(scene, gatherRay, sample, arena, &gatherIsect);
                if (Li.IsBlack()) continue;
                float Ggather = AbsDot(wi, n) * AbsDot(-wi, gatherIsect.dg.nn) /
                    DistanceSquared(p, gatherIsect.dg.p);
                if (Ggather - gLimit > 0.f && !isinf(Ggather))
                    L += f * Li * ((Ggather - gLimit) / Ggather * AbsDot(wi, n) /
                        (nSamples * pdf));
            }
        }
    }
    if (ray.depth + 1 < maxSpecularDepth) {
        Vector wi;
        // Trace rays for specular reflection and refraction
        L += SpecularReflect(ray, bsdf, *sample->rng, isect, renderer,
                             scene, sample, arena);
        L += SpecularTransmit(ray, bsdf, *sample->rng, isect, renderer,
                              scene, sample, arena);
    }
    return L;
}
コード例 #14
0
ファイル: wcs.c プロジェクト: bpon/wine
/*********************************************************************
 * _wcstod_l - not exported in native msvcrt
 */
double CDECL MSVCRT__wcstod_l(const MSVCRT_wchar_t* str, MSVCRT_wchar_t** end,
        MSVCRT__locale_t locale)
{
    MSVCRT_pthreadlocinfo locinfo;
    unsigned __int64 d=0, hlp;
    unsigned fpcontrol;
    int exp=0, sign=1;
    const MSVCRT_wchar_t *p;
    double ret;
    BOOL found_digit = FALSE;

    if (!MSVCRT_CHECK_PMT(str != NULL)) {
        *MSVCRT__errno() = MSVCRT_EINVAL;
        return 0;
    }

    if(!locale)
        locinfo = get_locinfo();
    else
        locinfo = locale->locinfo;

    p = str;
    while(isspaceW(*p))
        p++;

    if(*p == '-') {
        sign = -1;
        p++;
    } else  if(*p == '+')
        p++;

    while(isdigitW(*p)) {
        found_digit = TRUE;
        hlp = d*10+*(p++)-'0';
        if(d>MSVCRT_UI64_MAX/10 || hlp<d) {
            exp++;
            break;
        } else
            d = hlp;
    }
    while(isdigitW(*p)) {
        exp++;
        p++;
    }
    if(*p == *locinfo->lconv->decimal_point)
        p++;

    while(isdigitW(*p)) {
        found_digit = TRUE;
        hlp = d*10+*(p++)-'0';
        if(d>MSVCRT_UI64_MAX/10 || hlp<d)
            break;

        d = hlp;
        exp--;
    }
    while(isdigitW(*p))
        p++;

    if(!found_digit) {
        if(end)
            *end = (MSVCRT_wchar_t*)str;
        return 0.0;
    }

    if(*p=='e' || *p=='E' || *p=='d' || *p=='D') {
        int e=0, s=1;

        p++;
        if(*p == '-') {
            s = -1;
            p++;
        } else if(*p == '+')
            p++;

        if(isdigitW(*p)) {
            while(isdigitW(*p)) {
                if(e>INT_MAX/10 || (e=e*10+*p-'0')<0)
                    e = INT_MAX;
                p++;
            }
            e *= s;

            if(exp<0 && e<0 && exp+e>=0) exp = INT_MIN;
            else if(exp>0 && e>0 && exp+e<0) exp = INT_MAX;
            else exp += e;
        } else {
            if(*p=='-' || *p=='+')
                p--;
            p--;
        }
    }

    fpcontrol = _control87(0, 0);
    _control87(MSVCRT__EM_DENORMAL|MSVCRT__EM_INVALID|MSVCRT__EM_ZERODIVIDE
            |MSVCRT__EM_OVERFLOW|MSVCRT__EM_UNDERFLOW|MSVCRT__EM_INEXACT, 0xffffffff);

    if(exp>0)
        ret = (double)sign*d*pow(10, exp);
    else
        ret = (double)sign*d/pow(10, -exp);

    _control87(fpcontrol, 0xffffffff);

    if((d && ret==0.0) || isinf(ret))
        *MSVCRT__errno() = MSVCRT_ERANGE;

    if(end)
        *end = (MSVCRT_wchar_t*)p;

    return ret;
}
コード例 #15
0
ファイル: GCS_Common.cpp プロジェクト: MarkMote/ardupilot
void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *DataFlash)
{
    AP_Param *vp;
    enum ap_var_type var_type;

    mavlink_param_set_t packet;
    mavlink_msg_param_set_decode(msg, &packet);

    // set parameter
    char key[AP_MAX_NAME_SIZE+1];
    strncpy(key, (char *)packet.param_id, AP_MAX_NAME_SIZE);
    key[AP_MAX_NAME_SIZE] = 0;

    // find the requested parameter
    vp = AP_Param::find(key, &var_type);
    if ((NULL != vp) &&                                 // exists
        !isnan(packet.param_value) &&                       // not nan
        !isinf(packet.param_value)) {                       // not inf

        // add a small amount before casting parameter values
        // from float to integer to avoid truncating to the
        // next lower integer value.
        float rounding_addition = 0.01;
        
        // handle variables with standard type IDs
        if (var_type == AP_PARAM_FLOAT) {
            ((AP_Float *)vp)->set_and_save(packet.param_value);
        } else if (var_type == AP_PARAM_INT32) {
            if (packet.param_value < 0) rounding_addition = -rounding_addition;
            float v = packet.param_value+rounding_addition;
            v = constrain_float(v, -2147483648.0, 2147483647.0);
            ((AP_Int32 *)vp)->set_and_save(v);
        } else if (var_type == AP_PARAM_INT16) {
            if (packet.param_value < 0) rounding_addition = -rounding_addition;
            float v = packet.param_value+rounding_addition;
            v = constrain_float(v, -32768, 32767);
            ((AP_Int16 *)vp)->set_and_save(v);
        } else if (var_type == AP_PARAM_INT8) {
            if (packet.param_value < 0) rounding_addition = -rounding_addition;
            float v = packet.param_value+rounding_addition;
            v = constrain_float(v, -128, 127);
            ((AP_Int8 *)vp)->set_and_save(v);
        } else {
            // we don't support mavlink set on this parameter
            return;
        }

        // Report back the new value if we accepted the change
        // we send the value we actually set, which could be
        // different from the value sent, in case someone sent
        // a fractional value to an integer type
        mavlink_msg_param_value_send_buf(
            msg,
            chan,
            key,
            vp->cast_to_float(var_type),
            mav_var_type(var_type),
            _count_parameters(),
            -1);     // XXX we don't actually know what its index is...
        if (DataFlash != NULL) {
            DataFlash->Log_Write_Parameter(key, vp->cast_to_float(var_type));
        }
    }
}
コード例 #16
0
ファイル: LateProfessor.cpp プロジェクト: kuno4n/TopCoder
	static const double MAX_DOUBLE_ERROR = 1e-9; static bool topcoder_fequ(double expected, double result) { if (isnan(expected)) { return isnan(result); } else if (isinf(expected)) { if (expected > 0) { return result > 0 && isinf(result); } else { return result < 0 && isinf(result); } } else if (isnan(result) || isinf(result)) { return false; } else if (fabs(result - expected) < MAX_DOUBLE_ERROR) { return true; } else { double mmin = min(expected * (1.0 - MAX_DOUBLE_ERROR), expected * (1.0 + MAX_DOUBLE_ERROR)); double mmax = max(expected * (1.0 - MAX_DOUBLE_ERROR), expected * (1.0 + MAX_DOUBLE_ERROR)); return result > mmin && result < mmax; } }
コード例 #17
0
ファイル: vector3.cpp プロジェクト: ArmaJo/ardupilot
bool Vector3<T>::is_inf(void) const
{
    return isinf(x) || isinf(y) || isinf(z);
}
コード例 #18
0
ファイル: LateProfessor.cpp プロジェクト: kuno4n/TopCoder
	double moj_relative_error(double expected, double result) { if (isnan(expected) || isinf(expected) || isnan(result) || isinf(result) || expected == 0) return 0; return fabs(result-expected) / fabs(expected); }
コード例 #19
0
int __dtostr(double d,char *buf,unsigned int maxlen,unsigned int prec,unsigned int prec2) {
#if 1
	union {
		unsigned long long l;
		double d;
	} u = { .d=d };
	/* step 1: extract sign, mantissa and exponent */
	signed long e=((u.l>>52)&((1<<11)-1))-1023;
#else
	signed long e=(((((unsigned long*)&d)[1])>>20)&((1<<11)-1))-1023;
#endif
/*  unsigned long long m=u.l & ((1ull<<52)-1); */
	/* step 2: exponent is base 2, compute exponent for base 10 */
	signed long e10;
	/* step 3: calculate 10^e10 */
	unsigned int i;
	double backup=d;
	double tmp;
	char *oldbuf=buf;

	if ((i=isinf(d))) return copystring(buf,maxlen,i>0?"inf":"-inf");
	if (isnan(d)) return copystring(buf,maxlen,"nan");
	e10=1+(long)(e*0.30102999566398119802); /* log10(2) */
	/* Wir iterieren von Links bis wir bei 0 sind oder maxlen erreicht
	 * ist.  Wenn maxlen erreicht ist, machen wir das nochmal in
	 * scientific notation.  Wenn dann von prec noch was übrig ist, geben
	 * wir einen Dezimalpunkt aus und geben prec2 Nachkommastellen aus.
	 * Wenn prec2 Null ist, geben wir so viel Stellen aus, wie von prec
	 * noch übrig ist. */
	if (d==0.0) {
		prec2=prec2==0?1:prec2+2;
		prec2=prec2>maxlen?8:prec2;
		i=0;
		if (prec2 && (long long)u.l<0) { buf[0]='-'; ++i; }
		for (; i<prec2; ++i) buf[i]='0';
		buf[buf[0]=='0'?1:2]='.'; buf[i]=0;
		return i;
	}

	if (d < 0.0) { d=-d; *buf='-'; --maxlen; ++buf; }

	/*
      Perform rounding. It needs to be done before we generate any
      digits as the carry could propagate through the whole number.
	*/

	tmp = 0.5;
	for (i = 0; i < prec2; i++) { tmp *= 0.1; }
	d += tmp;

	if (d < 1.0) { *buf='0'; --maxlen; ++buf; }
/*  printf("e=%d e10=%d prec=%d\n",e,e10,prec); */
	if (e10>0) {
		int first=1;	/* are we about to write the first digit? */
		tmp = 10.0;
		i=e10;
		while (i>10) { tmp=tmp*1e10; i-=10; }
		while (i>1) { tmp=tmp*10; --i; }
		/* the number is greater than 1. Iterate through digits before the
		 * decimal point until we reach the decimal point or maxlen is
		 * reached (in which case we switch to scientific notation). */
		while (tmp>0.9) {
			char digit;
			double fraction=d/tmp;
			digit=(int)(fraction);		/* floor() */
			if (!first || digit) {
				first=0;
				*buf=digit+'0'; ++buf;
				if (!maxlen) {
					/* use scientific notation */
					int len=__dtostr(backup/tmp,oldbuf,maxlen,prec,prec2);
					int initial=1;
					if (len==0) return 0;
					maxlen-=len; buf+=len;
					if (maxlen>0) {
						*buf='e';
						++buf;
					}
					--maxlen;
					for (len=1000; len>0; len/=10) {
						if (e10>=len || !initial) {
							if (maxlen>0) {
								*buf=(e10/len)+'0';
								++buf;
							}
							--maxlen;
							initial=0;
							e10=e10%len;
						}
					}
					if (maxlen>0) goto fini;
					return 0;
				}
				d-=digit*tmp;
				--maxlen;
			}
			tmp/=10.0;
		}
	}
	else
	{
		tmp = 0.1;
	}

	if (buf==oldbuf) {
		if (!maxlen) return 0; --maxlen;
		*buf='0'; ++buf;
	}
	if (prec2 || prec>(unsigned int)(buf-oldbuf)+1) {	/* more digits wanted */
		if (!maxlen) return 0; --maxlen;
		*buf='.'; ++buf;
		prec-=buf-oldbuf-1;
		if (prec2) prec=prec2;
		if (prec>maxlen) return 0;
		while (prec>0) {
			char digit;
			double fraction=d/tmp;
			digit=(int)(fraction);		/* floor() */
			*buf=digit+'0'; ++buf;
			d-=digit*tmp;
			tmp/=10.0;
			--prec;
		}
	}
fini:
	*buf=0;
	return buf-oldbuf;
}
コード例 #20
0
ファイル: port.c プロジェクト: John-Colvin/dmd
int Port::isInfinity(double r)
{
    return isinf(r);
}
コード例 #21
0
ファイル: LinearSystem_driver.c プロジェクト: bremond/siconos
/* return 0 if ok
* otherwise return !=0
*/
int solveLeastSquareProblem(LinearSystemProblem* problem, double *z ,  SolverOptions* options)
{


  /* Checks inputs */
  if (problem == NULL || z == NULL)
    numericsError("EqualityProblem", "null input for EqualityProblem and/or unknowns (z)");

  /* Output info. : 0: ok -  >0: problem (depends on solver) */
  int info = -1;
  int n = problem->size;
  int n2 = n * n;

  double * Maux = 0;
  int * ipiv = 0;
  if (options && options->dWork)
    Maux = options->dWork;
  else
    Maux = (double *) malloc(LinearSystem_getNbDwork(problem, options) * sizeof(double));

  if (options && options->iWork)
    ipiv = options->iWork;
  else
    ipiv = (int *) malloc(LinearSystem_getNbIwork(problem, options) * sizeof(int));
  int LAinfo = 0;
  //displayLS(problem);

  assert(problem->M->matrix0);
  assert(problem->q);

  memcpy(Maux, problem->M->matrix0, n2 * sizeof(double));
  //  memcpy(z,problem->q,n*sizeof(double));
  for (int ii = 0; ii < n; ii++)
    z[ii] = -problem->q[ii];


  printf("LinearSystem : solveLeastSquareProblem LWORK is :%d\n", LWORK);


  DGELS(LA_NOTRANS,n, n, 1, Maux, n, z, n, &LAinfo);
  if (LAinfo)
  {
    printf("LinearSystem_driver: DGELS  failed:\n");
    goto __fin;
  }

  int ii;
  for (ii = 0; ii < n; ii++)
  {
    if (isnan(z[ii]) || isinf(z[ii]))
    {
      printf("DGELS FAILED\n");
      goto __fin;
    }
  }
  info = 0;
  printf("LinearSystem_driver: computeError of LinearSystem : %e\n", LinearSystem_computeError(problem, z));
__fin:
  if (!(options && options->dWork))
    free(Maux);

  if (!(options && options->iWork))
    free(ipiv);

  return info;


}
コード例 #22
0
ファイル: Table.c プロジェクト: yngman/libsoc
int so_Table_xml(so_Table *self, xmlTextWriterPtr writer, char *element_name)
{
    int rc;
    rc = xmlTextWriterStartElement(writer, BAD_CAST element_name);
    if (rc < 0) return 1;

    if (self->superclass_func) {
       rc = (*(self->superclass_func))(self->superclass, writer);
       if (rc != 0) return rc;
    }

    char temp_colnum[10];

    if (self->numcols > 0) {
        rc = xmlTextWriterStartElement(writer, BAD_CAST "ds:Definition");
        if (rc < 0) return 1;
        for (int i = 0; i < self->numcols; i++) {
            rc = xmlTextWriterStartElement(writer, BAD_CAST "ds:Column");
            if (rc < 0) return 1;
            rc = xmlTextWriterWriteAttribute(writer, BAD_CAST "columnId", BAD_CAST self->columns[i]->columnId);
            if (rc < 0) return 1;
            rc = xmlTextWriterWriteAttribute(writer, BAD_CAST "columnType", BAD_CAST pharmml_columnType_to_string(self->columns[i]->columnType));
            if (rc < 0) return 1;
            rc = xmlTextWriterWriteAttribute(writer, BAD_CAST "valueType", BAD_CAST pharmml_valueType_to_string(self->columns[i]->valueType));
            if (rc < 0) return 1;

            snprintf(temp_colnum, 10, "%d", i + 1);
            rc = xmlTextWriterWriteAttribute(writer, BAD_CAST "columnNum", BAD_CAST temp_colnum);
            if (rc < 0) return 1;
            rc = xmlTextWriterEndElement(writer);
            if (rc < 0) return 1;
        }
        rc = xmlTextWriterEndElement(writer);
        if (rc < 0) return 1;
    }

    if (!self->ExternalFile) {
        rc = xmlTextWriterStartElement(writer, BAD_CAST "ds:Table");
        if (rc < 0) return 1;
        for (int i = 0; i < self->numrows; i++) {
            rc = xmlTextWriterStartElement(writer, BAD_CAST "ds:Row");
            for (int j = 0; j < self->numcols; j++) {
                char *value_string;
                char *special_string = NULL;
                if (self->columns[j]->valueType == PHARMML_VALUETYPE_REAL) {
                    double *ptr = (double *) self->columns[j]->column;
                    double number = ptr[i];
                    if (pharmml_is_na(number)) {
                        special_string = "ct:NA";
                    } else if (isnan(number)) {
                        special_string = "ct:NaN";
                    } else if (isinf(number)) {
                        if (number > 0) {
                            special_string = "ct:plusInf";
                        } else {
                            special_string = "ct:minusInf";
                        }
                    } else {
                        value_string = pharmml_double_to_string(number);
                    }
                } else if (self->columns[j]->valueType == PHARMML_VALUETYPE_INT) {
                    int *ptr = (int *) self->columns[j]->column;
                    value_string = pharmml_int_to_string(ptr[i]);
                } else if (self->columns[j]->valueType == PHARMML_VALUETYPE_STRING || self->columns[j]->valueType == PHARMML_VALUETYPE_ID) {
                    char **ptr = (char **) self->columns[j]->column;
                    value_string = ptr[i];
                }
                if (self->columns[j]->valueType == PHARMML_VALUETYPE_BOOLEAN) {
                    bool *ptr = (bool *) self->columns[j]->column;
                    if (ptr[i]) {
                        rc = xmlTextWriterWriteElement(writer, BAD_CAST "ct:True", NULL);
                        if (rc < 0) return 1;
                    } else {
                        rc = xmlTextWriterWriteElement(writer, BAD_CAST "ct:False", NULL);
                        if (rc < 0) return 1;
                    }
                } else if (special_string) {
                    rc = xmlTextWriterWriteElement(writer, BAD_CAST special_string, NULL);
                    if (rc < 0) return 1;
                } else {
                    rc = xmlTextWriterWriteElement(writer, BAD_CAST pharmml_valueType_to_element(self->columns[j]->valueType), BAD_CAST value_string);
                    if (self->columns[j]->valueType == PHARMML_VALUETYPE_REAL || self->columns[j]->valueType == PHARMML_VALUETYPE_INT) {
                        free(value_string);
                    }
                    if (rc < 0) return 1;
                }
            }
            rc = xmlTextWriterEndElement(writer);
            if (rc < 0) return 1;
        }
        rc = xmlTextWriterEndElement(writer);
        if (rc < 0) return 1;
    } else {
        rc = so_ExternalFile_xml(self->ExternalFile, writer, "ds:ExternalFile");
        if (rc) return rc;

        if (self->write_external_file) {
            char *delimiter_string;
            if (strcmp(self->ExternalFile->delimiter, "COMMA") == 0) {
                delimiter_string = ",";
            } else if (strcmp(self->ExternalFile->delimiter, "SPACE") == 0) {
                delimiter_string = " ";
            } else if (strcmp(self->ExternalFile->delimiter, "TAB")  == 0) {
                delimiter_string = "\t";
            } else if (strcmp(self->ExternalFile->delimiter, "SEMICOLON") == 0) {
                delimiter_string = ";";
            }
            FILE *fp = fopen(self->ExternalFile->path, "w");

            for (int i = 0; i < self->numrows; i++) {
                for (int j = 0; j < self->numcols; j++) {
                    char *value_string;
                    if (self->columns[j]->valueType == PHARMML_VALUETYPE_REAL) {
                        double *ptr = (double *) self->columns[j]->column;
                        value_string = pharmml_double_to_string(ptr[i]);
                    } else if (self->columns[j]->valueType == PHARMML_VALUETYPE_INT) {
                        int *ptr = (int *) self->columns[j]->column;
                        value_string = pharmml_int_to_string(ptr[i]);
                    } else if (self->columns[j]->valueType == PHARMML_VALUETYPE_STRING || self->columns[j]->valueType == PHARMML_VALUETYPE_ID) {
                        char **ptr = (char **) self->columns[j]->column;
                        value_string = ptr[i];
                    } else if (self->columns[j]->valueType == PHARMML_VALUETYPE_BOOLEAN) {
                        bool *ptr = (bool *) self->columns[j]->column;
                        if (ptr[i]) {
                            value_string = "True";
                        } else {
                            value_string = "False";
                        }
                    }

                    fprintf(fp, "%s", value_string);
                    if (j != self->numcols - 1) {
                        fprintf(fp, "%s", delimiter_string);
                    }

                    if (self->columns[j]->valueType == PHARMML_VALUETYPE_REAL || self->columns[j]->valueType == PHARMML_VALUETYPE_INT) {
                        free(value_string);
                    }
                }
                fprintf(fp, "\n");
            }

            fclose(fp);
        }
    }

    rc = xmlTextWriterEndElement(writer);
    if (rc < 0) return 1;

    return 0;
}
コード例 #23
0
ファイル: lib_libdtoa.c プロジェクト: a1ien/nuttx
static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
                     uint8_t flags, double value)
{
    FAR char *digits;     /* String returned by __dtoa */
    FAR char *rve;        /* Points to the end of the return value */
    int  expt;            /* Integer value of exponent */
    int  numlen;          /* Actual number of digits returned by cvt */
    int  nchars;          /* Number of characters to print */
    int  dsgn;            /* Unused sign indicator */
    int  i;

    /* Special handling for NaN and Infinity */

    if (isnan(value))
    {
        lib_dtoa_string(obj, "NaN");
        return;
    }

    if (isinf(value))
    {
        if (value < 0.0)
        {
            obj->put(obj, '-');
        }

        lib_dtoa_string(obj, "Infinity");
        return;
    }

    /* Non-zero... positive or negative */

    if (value < 0)
    {
        value = -value;
        SET_NEGATE(flags);
    }

    /* Perform the conversion */

    digits   = __dtoa(value, 3, prec, &expt, &dsgn, &rve);
    numlen   = rve - digits;

    /* Avoid precision error from missing trailing zeroes */

    numlen = MAX(expt, numlen);

    if (IS_NEGATE(flags))
    {
        obj->put(obj, '-');
    }
    else if (IS_SHOWPLUS(flags))
    {
        obj->put(obj, '+');
    }

    /* Special case exact zero or the case where the number is smaller than
     * the print precision.
     */

    if (value == 0 || expt < -prec)
    {
        /* kludge for __dtoa irregularity */

        obj->put(obj, '0');

        /* A decimal point is printed only in the alternate form or if a
         * particular precision is requested.
         */

        if (prec > 0 || IS_ALTFORM(flags))
        {
            obj->put(obj, '.');

            /* Always print at least one digit to the right of the decimal point. */

            prec = MAX(1, prec);
        }
    }

    /* A non-zero value will be printed */

    else
    {

        /* Handle the case where the value is less than 1.0 (in magnitude) and
         * will need a leading zero.
         */

        if (expt <= 0)
        {
            /* Print a single zero to the left of the decimal point */

            obj->put(obj, '0');

            /* Print the decimal point */

            obj->put(obj, '.');

            /* Print any leading zeros to the right of the decimal point */

            if (expt < 0)
            {
                nchars = MIN(-expt, prec);
                zeroes(obj, nchars);
                prec -= nchars;
            }
        }

        /* Handle the general case where the value is greater than 1.0 (in
         * magnitude).
         */

        else
        {
            /* Print the integer part to the left of the decimal point */

            for (i = expt; i > 0; i--)
            {
                if (*digits != '\0')
                {
                    obj->put(obj, *digits);
                    digits++;
                }
                else
                {
                    obj->put(obj, '0');
                }
            }

            /* Get the length of the fractional part */

            numlen -= expt;

            /* If there is no fractional part, then a decimal point is printed
             * only in the alternate form or if a particular precision is
             * requested.
             */

            if (numlen > 0 || prec > 0 || IS_ALTFORM(flags))
            {
                /* Print the decimal point */

                obj->put(obj, '.');

                /* Always print at least one digit to the right of the decimal
                 * point.
                 */

                prec = MAX(1, prec);
            }
        }

        /* If a precision was specified, then limit the number digits to the
         * right of the decimal point.
         */

        if (prec > 0)
        {
            nchars = MIN(numlen, prec);
        }
        else
        {
            nchars = numlen;
        }

        /* Print the fractional part to the right of the decimal point */

        for (i = nchars; i > 0; i--)
        {
            obj->put(obj, *digits);
            digits++;
        }

        /* Decrement to get the number of trailing zeroes to print */

        prec -= nchars;
    }

    /* Finally, print any trailing zeroes */

    zeroes(obj, prec);
}
コード例 #24
0
ファイル: knh_bytes_t.c プロジェクト: matsuu/konoha
int knh_bytes_parsefloat(knh_bytes_t t, knh_float_t *value)
{
	if(t.buf[0] == '0' && (t.buf[1] == 'x' || t.buf[1] == 'b')) {
		knh_int_t n = 0;
		int res = knh_bytes_parseint(t, &n);
		*value = (knh_float_t)n;
		return res;
	}

	knh_index_t loc = knh_bytes_index(t, 'E');
	if(loc == -1) loc = knh_bytes_index(t, 'e');
	if(loc != -1) {
		t = knh_bytes_first(t, loc);
	}

	size_t i = 0;
	knh_float_t v = 0.0, prev = 0.0, c = 1.0;

	if(t.buf[0] == '-') i = 1;

	for(;i < t.len; i++) {
		if('0' <= t.buf[i] && t.buf[i] <= '9') {
			prev = v;
			v = v * 10 + (t.buf[i] - '0');
#if defined(KNH_USING_MATH) && !defined(KONOHA_ON_WINDOWS)
			if(isinf(v)||isnan(v)) {
				*value = 0.0;
				return 1;
			}
#endif
		}
		else if(t.buf[i] == '.') {
			i++;
			break;
		}
		else {
			*value = (t.buf[0] == '-') ? -v : v;
			return 1;
		}
	}

	for(; i < t.len; i++) {
		if('0' <= t.buf[i] && t.buf[i] <= '9') {
			prev = v;
			c *= 10;
#if defined(KNH_USING_MATH) && !defined(KONOHA_ON_WINDOWS)
			if(isinf(c)||isnan(c)) {
				break;
			}
#endif
			v = v + ((t.buf[i] - '0') / c);
		}else {
			break;
		}
	}

	v = (t.buf[0] == '-') ? -v : v ;

	if(loc != -1) {
		knh_bytes_t t2 = knh_bytes_last(t, loc + 1);
		knh_intptr_t scale = knh_bytes_toint(t2);
		int j;
		if(scale > 0) {
			for(j = 0; j < scale; j++) {
				v *= 10;
			}
		}
		else if(scale < 0) {
			scale = -scale;
			for(j = 0; j < scale; j++) {
				v /= 10;
			}
		}
	}
	*value = v;
	return 1;
}
コード例 #25
0
ファイル: lf.cpp プロジェクト: garethcmurphy/mpi-mhd-fv
/* 
currently no entropy fix is in place 
*/
int
lf (double *leftstate,
	 double *rightstate,
	 double *roe_flux,
	 double *res_state, int time_step, double *max_speed, int idir, int * loc)
{
  double gammag = PhysConsts::gamma;

  double pmin = 1e-10;
  ofstream outFile;
  char outfilename[50] = "output/riemann.log";

  int rc = 0;
  int ii = 0;
  int jj = 0;
  int kk = 0;
  int hh = 0;

  double av_state[8];
  double rl = leftstate[0];
  double ul = leftstate[1];
  double vl = leftstate[2];
  double wl = leftstate[3];
  double pl = leftstate[4];
  double bul = leftstate[5];
  double bvl = leftstate[6];
  double bwl = leftstate[7];

  double rr = rightstate[0];
  double ur = rightstate[1];
  double vr = rightstate[2];
  double wr = rightstate[3];
  double pr = rightstate[4];
  double bur = rightstate[5];
  double bvr = rightstate[6];
  double bwr = rightstate[7];
  double cslow = 0;
  double cslow2 = 0;
  double calfven = 0;
  double calfven2 = 0;
  double cfast = 0;
  double cfast2 = 0;
  double bsquared = 0;
  double gammam1 = gammag - 1;
  double gammam1i = 1.0 / gammam1;


  double rho_rl = 0;
  double u_rl = 0;
  double v_rl = 0;
  double w_rl = 0;
  double p_rl = 0;
  double bu_rl = 0;
  double bv_rl = 0;
  double bw_rl = 0;

  double lambda[7];
  double lstate[8];
  double rstate[8];
  double lrsp[7];
  double rrsp[7];
  double fl[7], fr[7], fx[7];
  double eigenwt[7];
  double temp;

  double rho = 0;
  double mass = 0;
  double rhoi = 0;
  double srhoi = 0;
  double u = 0;
  double v = 0;
  double w = 0;
  double bu = 0;
  double bv = 0;
  double bw = 0;
  double p = 0;
  double csound2 = 0;
  double term = 0;
  double a_star2 = 0;
  double vv2 = 0;
  double kinetic = 0;
  double p_magnetic = 0;
  double internal_energy = 0;
  double energy = 0;
  double v2 = 0;
  double vdotb = 0;

  double cc[7];
  double dvy = 0, dvz = 0;

  if (rl < 0 || rr < 0)
    {
      cerr << "Negative density in roe solver!" << endl;
      exit (0);
    }
  outFile.open (outfilename, ofstream::app);
  if (!outFile)
    {
      cerr << "Unable to open file" << endl;
    }

  if (idir == 1)
    {
      /* Keep fluxes */
    }
  else if (idir == 2)
    {
      rl = leftstate[0];
      ul = leftstate[2];
      vl = leftstate[3];
      wl = leftstate[1];
      pl = leftstate[4];
      pl = std::max (pl, pmin);
      bul = leftstate[6];
      bvl = leftstate[7];
      bwl = leftstate[5];

      rr = rightstate[0];
      ur = rightstate[2];
      vr = rightstate[3];
      wr = rightstate[1];
      pr = rightstate[4];
      pr = std::max (pr, pmin);
      bur = rightstate[6];
      bvr = rightstate[7];
      bwr = rightstate[5];
      /* Rotate fluxes */
    }
  else if (idir == 3)
    {
      /* Rotate fluxes */
      rl = leftstate[0];
      ul = leftstate[3];
      vl = leftstate[1];
      wl = leftstate[2];
      pl = leftstate[4];
      bul = leftstate[7];
      bvl = leftstate[5];
      bwl = leftstate[6];

      rr = rightstate[0];
      ur = rightstate[3];
      vr = rightstate[1];
      wr = rightstate[2];
      pr = rightstate[4];
      bur = rightstate[7];
      bvr = rightstate[5];
      bwr = rightstate[6];
    }
  /* Convert conserved to primitives */

  lrsp[0] = rl;
  lrsp[1] = ul;
  lrsp[2] = vl;
  lrsp[3] = wl;
  lrsp[4] = pl;
  lrsp[5] = bvl;
  lrsp[6] = bwl;

  rrsp[0] = rr;
  rrsp[1] = ur;
  rrsp[2] = vr;
  rrsp[3] = wr;
  rrsp[4] = pr;
  rrsp[5] = bvr;
  rrsp[6] = bwr;

  for (ii = 0; ii < 7; ii++)
    {
      lstate[ii] = lrsp[ii];
      rstate[ii] = rrsp[ii];
    }
  cc[0] = (rr - rl);
  cc[1] = (ur - ul);
  cc[2] = (vr - vl);
  dvy = vr - vl;
  cc[3] = (wr - wl);
  dvz = wr - wl;
  cc[4] = (pr - pl);
  if (fabs (cc[4]) < 1e-10)
    cc[4] = 0;
  cc[5] = (bvr - bvl);
  cc[6] = (bwr - bwl);

  /* compute the averaged quantities */
  rho_rl = 0.5 * (rr + rl);
  u_rl = 0.5 * (ul + ur);
  if (u_rl == 0)
    {
      /* work out res state and flux and exit? */
    }
  v_rl = 0.5 * (vl + vr);
  w_rl = 0.5 * (wl + wr);
  p_rl = 0.5 * (pl + pr);
  bu_rl = 0.5 * (bul + bur);
  bv_rl = 0.5 * (bvl + bvr);
  bw_rl = 0.5 * (bwl + bwr);

  av_state[0] = rho_rl;
  av_state[1] = u_rl;
  av_state[2] = v_rl;
  av_state[3] = w_rl;
  av_state[4] = p_rl;
  av_state[5] = bu_rl;
  av_state[6] = bv_rl;
  av_state[7] = bw_rl;

  /* Allocte memory for eigenvectors */
  Array2D < double >levec (7, 7);
  Array2D < double >revec (7, 7);
  Array2D < double >levc (7, 7);
  Array2D < double >revc (7, 7);

  /* Compute fast and slow speeds */
  rho = rl;
  rhoi = 1 / rho;
  srhoi = 1 / sqrt (rho);
  u = ul;
  v = vl;
  w = wl;
  bu = bul * sqrt (rhoi);
  bv = bvl * sqrt (rhoi);
  bw = bwl * sqrt (rhoi);
  bsquared = (bu * bu + bv * bv + bw * bw);
  vv2 = (u * u + v * v + w * w);
  p = pl;
  calfven2 = fabs (bu * bu);
  calfven = sqrt (calfven2);
  csound2 = gammag * p * rhoi;
  a_star2 = csound2 + bsquared;
  term = sqrt (a_star2 * a_star2 - 4.0 * csound2 * calfven2);
  term = fabs (term);
  cslow2 = 0.5 * (a_star2 - term);
  if (cfast2 < 0)
    {
      if (cfast2 < -1e-10)
	{
	  cout << "Fast speed went negative!! " << cslow2 << endl;
	  cout
	    << " csound2 " << csound2
	    << " calfven2 " << calfven2
	    << " bsquared " << bsquared
	    << " bx " << bu << " by " << bv << " bz " << bw << endl;
	  return 1;
	}
      else
	{
	  cslow2 = 0;
	}
    }
  cfast2 = 0.5 * (a_star2 + term);
  cslow = sqrt (cslow2);
  double cfast_l = sqrt (cfast2);
  if (isnan (cfast_l))
  {
	  
	  std::cout << "gggg"
	  << std::endl;
	  exit(0);
	  }
  


  /* Compute fast and slow speeds */
  rho = rho_rl;
  rhoi = 1 / rho;
  srhoi = 1 / sqrt (rho);
  u = u_rl;
  v = v_rl;
  w = w_rl;
  bu = bu_rl * sqrt (rhoi);
  bv = bv_rl * sqrt (rhoi);
  bw = bw_rl * sqrt (rhoi);
  bsquared = (bu * bu + bv * bv + bw * bw);
  vv2 = (u * u + v * v + w * w);
  p = p_rl;
  calfven2 = fabs (bu * bu);
  calfven = sqrt (calfven2);
  csound2 = gammag * p * rhoi;
  a_star2 = csound2 + bsquared;
  term = sqrt (a_star2 * a_star2 - 4.0 * csound2 * calfven2);
  term = fabs (term);
  cslow2 = 0.5 * (a_star2 - term);
  cfast2 = 0.5 * (a_star2 + term);
  if (cfast2 < 0)
    {
      if (cfast2 < -1e-10)
	{
	  cout << "Fast speed went negative!! " << cslow2 << endl;
	  cout
	    << " csound2 " << csound2
	    << " calfven2 " << calfven2
	    << " bsquared " << bsquared
	    << " bx " << bu << " by " << bv << " bz " << bw << endl;
	  return 1;
	}
      else
	{
	  cslow2 = 0;
	}
    }
  cslow = sqrt (cslow2);
  cfast = sqrt (cfast2);
  if (isnan (cfast))
  {
	  std::cout << "gggg"
	  << std::endl;
	  exit(0);
	}
  
  if (isinf (cfast))
  {
	  std::cout << "cfast inf"
	  << "p_rl  " << p_rl 
	    << " csound2 " << csound2
	    << " calfven2 " << calfven2
	    << " bsquared " << bsquared
	    << " bul " << bul
	    << " bur " << bur
	    << " bu_rl " << bu_rl
	    << " bv_rl " << bv_rl
	    << " bw_rl " << bw_rl
	  << std::endl;
	  exit(0);
	  //cfast=0;
	}
  

  double clax = std::max( fabs( u_rl+ cfast), fabs(u_rl- cfast) );



  mass = leftstate[0];
  u = leftstate[1];
  v = leftstate[2];
  w = leftstate[3];
  p = leftstate[4];
  bu = leftstate[5];
  bv = leftstate[6];
  bw = leftstate[7];


  v2 = u * u + v * v + w * w;
  kinetic = 0.5 * mass * v2;
  bsquared = bu * bu + bv * bv + bw * bw;
  p_magnetic = 0.5 * bsquared;
  internal_energy = p * gammam1i;
  energy = kinetic + p_magnetic + internal_energy;
  vdotb = u * bu + v * bv + w * bw;


  fl[0] = mass * u;
  fl[1] = mass * u * u + p + p_magnetic - bu * bu;
  fl[2] = mass * u * v - bu * bv;
  fl[3] = mass * u * w - bu * bw;
  fl[4] = (energy + p + p_magnetic) * u - bu * (vdotb);
  fl[5] = u * bv - v * bu;
  fl[6] = u * bw - w * bu;

	lstate[0] = mass;
	lstate[1] = mass*u;
	lstate[2] = mass*v;
	lstate[3] = mass*w;
	lstate[4] = energy;
	lstate[5] = bu;
	lstate[6] = bv;
	lstate[7] = bw;

  mass = rightstate[0];
  u = rightstate[1];
  v = rightstate[2];
  w = rightstate[3];
  p = rightstate[4];
  bu = rightstate[5];
  bv = rightstate[6];
  bw = rightstate[7];


  v2 = u * u + v * v + w * w;
  kinetic = 0.5 * mass * v2;
  bsquared = bu * bu + bv * bv + bw * bw;
  p_magnetic = 0.5 * bsquared;
  internal_energy = p * gammam1i;
  energy = kinetic + p_magnetic + internal_energy;
  vdotb = u * bu + v * bv + w * bw;


  fr[0] = mass * u;
  fr[1] = mass * u * u + p + p_magnetic - bu * bu;
  fr[2] = mass * u * v - bu * bv;
  fr[3] = mass * u * w - bu * bw;
  fr[4] = (energy + p + p_magnetic) * u - bu * (vdotb);
  fr[5] = u * bv - v * bu;
  fr[6] = u * bw - w * bu;


	rstate[0] = mass;
	rstate[1] = mass*u;
	rstate[2] = mass*v;
	rstate[3] = mass*w;
	rstate[4] = energy;
	rstate[5] = bu;
	rstate[6] = bv;
	rstate[7] = bw;

  roe_flux[0] = 0.5 *( fl[0]+ fr[0] - fabs(clax ) * ( rstate[0] - lstate[0]) ); 
  roe_flux[1] = 0.5 *( fl[1]+ fr[1] - fabs(clax ) * ( rstate[1] - lstate[1]) );  
  roe_flux[2] = 0.5 *( fl[2]+ fr[2] - fabs(clax ) * ( rstate[2] - lstate[2]) ); 
  roe_flux[3] = 0.5 *( fl[3]+ fr[3] - fabs(clax ) * ( rstate[3] - lstate[3]) ); 
  roe_flux[4] = 0.5 *( fl[4]+ fr[4] - fabs(clax ) * ( rstate[4] - lstate[4]) ); 
  roe_flux[5] = 0.; 
  roe_flux[6] =  0.5* ( fl[5]+ fr[5] - fabs(clax ) * ( rstate[6] - lstate[6]) ); 
  roe_flux[7] = 0.5 *( fl[6]+ fr[6] - fabs(clax ) * ( rstate[7] - lstate[7]) ); 

  if (  isnan(roe_flux[0]) )
  {

		std::cout <<
		" bomb "
     << " clax= " << clax 
     << " cfast= " << cfast 
     << " u_rl= " << u_rl 
		<< std::endl;
		exit(0);
  }

return 0;
}
コード例 #26
0
ファイル: GammaDD.c プロジェクト: 010001111/darling
static long double GammaCommon( double dhead, double dtail, int igamma )
{
	int signgam, ireflect, increase, i;
	
	double int1, int2, arg, arg2, arg3, argt, arg2t, anew;
	double factor, factor1, factor2, factor3, f1, f2, f3;
	double prod1, prod2, pextra, pextra1, pextra2, sum1, sum2, sextra;
	double sum21, sum22, sextra2, sum31, sum32, sextra3;
	double diff1, diff2, dextra, sarg1, sarg2, saextra;
	double denom1, denom2, sum, suml, sumt, recip1, recip2, recipsq1, recipsq2;
	double extra, extra2, extra3, extrax, extray, extralg, gmextra;
	double temp, eexp, diff, z, z2, y, y2, y3, asize, c;
	
	DblDbl gamdd, tempdd, intdd, xhintdd, sinargdd;
	
	DD *bern = (DD *)(bernData - 4);
	DD *zeta = (DD *)(zetaData - 8);

	signgam = 1;

	if (dhead <= 0) {					// special for neg arguments
		tempdd.d[0] = dhead;
		tempdd.d[1] = dtail;
		intdd.f = rintl(tempdd.f);		// get integer part of argument
		int1 = intdd.d[0];
		int2 = intdd.d[1];
		
		if (isinf(dhead) || (dhead - int1 + dtail - int2) == 0) {
			
			//*********************************************************************
			//    Argument is a non-positive integer.  Gamma is not defined.      *
			//    Return a NaN. 		                                          *
			//*********************************************************************
			gamdd.d[0] = INFINITY; // per IEC, [was: zero/zero;]
			gamdd.d[1] = zero;
			return gamdd.f;
		}
		if (dhead > -20.0) {
			ireflect = 0;				// reflection formula not used for
										// modest sized neg numbers
		}
		else {
			ireflect = 1;				// signal use of reflection formula
			dhead = -dhead;				// change sign of argument
			dtail = -dtail;
		}
	}
	else								// else, positive arguments --
		ireflect = 0;					// signal reflection formula not used
	
	if (isinf(dhead) || (dhead + dtail) != (dhead + dtail)) {
		gamdd.d[0] = dhead + dtail;
		gamdd.d[1] = zero;
		return gamdd.f;
	}
	
	arg  = dhead;						// working copy of argument
	arg2 = dtail;
	arg3 = 0.0;
	factor1 = 1.0;
	factor2 = 0.0;
	factor3 = 0.0;
	
	if (arg > 18.0) {					// use asymptotic formula
		while (arg < 31.0) {
			_d3mul( factor1, factor2, factor3, arg, arg2, arg3,
						&factor1, &factor2, &factor3 );
			arg = arg + 1.0;
		}								// argument in range for asympt. formula
		while (arg < 35.0) {
			_d3mul( factor1, factor2, factor3, arg, arg2, arg3,
						&factor1, &factor2, &factor3 );
			argt = __FADD( arg, 1.0 );
			arg2t = __FADD( (arg - argt + 1.0), arg2 );
			arg3 = ((arg - argt + 1.0) - arg2t + arg2) + arg3;
			arg = argt;
			arg2 = arg2t;
		}
		tempdd.f = _LogInnerLD(arg, arg2, 0.0, &f3, 1);		// ln x
		f3 = f3 + arg3/arg;
		f1 = tempdd.d[0];
		f2 = tempdd.d[1];

		_d3mul(f1, f2, f3, (arg - 0.5), arg2, arg3,
					&prod1, &prod2, &c);					// (x-.5)ln x
		_d3add(prod1, prod2, c, -arg, -arg2, -arg3,
					&sum1, &sum2, &sextra);					// (x-.5)ln x-x
		_d3add(sum1, sum2, sextra, bump.f, bump2.f, bump3.f,
					&sum21, &sum22, &sextra2);
		
		//**************************************************************
		//   sum21, sum22, sextra2 represent (x-.5)ln x-x+.5 ln(2 Pi)  *
		//**************************************************************
		
		_d3div(1.0, 0.0, 0.0, arg, arg2, arg3, &recip1, &recip2, &extra);
		
		// argument for asymptotic formula
		_d3mul(recip1, recip2, extra, recip1, recip2, extra,
					&recipsq1, &recipsq2, &extra2);
		
		sum  = bern[11][0];
		suml = bern[11][1];
		for (i = 10; i >= 1; --i) {
			temp = __FMADD( sum, recipsq1, bern[i][0] ); // bern[i][0] + sum*recipsq1;
			/* suml = bern[i][0] - temp + sum*recipsq1 +
			         (bern[i][1] + sum*recipsq2 + suml*recipsq1); */
			suml = __FMADD( sum, recipsq1, bern[i][0] - temp ) +
					__FMADD( suml, recipsq1, __FMADD( sum, recipsq2, bern[i][1] ) );
			sum = temp;
		}								// finish summation of asymptotic series

		_d3mul(sum, suml, 0.0, recip1, recip2, extra, &prod1, &prod2, &extra3);
		_d3add(sum21, sum22, sextra2, prod1, prod2, extra3, &sum31, &sum32, &sextra3);
		
		//******************************************************************************
		//   At this point, log(gamma*factor) is computed as (sum31, sum32, sextra3).  *
		//******************************************************************************
		
		if ((igamma == 1) && (ireflect != 1))  {
			// Gamma entry point (without use of reflection formula)?
			tempdd.f = _ExpInnerLD(sum31, sum32, sextra3, &eexp, 4);
			if (factor1 == 1.0) {
				gamdd.f = tempdd.f;
				gmextra = eexp;
			}
			else {
				_d3div(tempdd.d[0], tempdd.d[1], eexp, factor1, factor2, factor3,
							&(gamdd.d[0]), &(gamdd.d[1]), &gmextra);
			}
		}
		else {							// Computing log(gamma(x))
			factor = factor1;
			if (factor == 1.0) {
				gamdd.d[0] = sum31;
				gamdd.d[1] = sum32;
				gmextra = sextra3;
			}
			else {
				tempdd.f = _LogInnerLD(factor1, factor2, 0.0, &extrax, 1);
				// computing log gamma.
				extray = -(extrax + factor3/factor);
				_d3add(sum31, sum32, sextra3, -tempdd.d[0], -tempdd.d[1], extray,
							&(gamdd.d[0]), &(gamdd.d[1]), &gmextra);
			}
		}
	}
	else {								// use formula for interval [0.5,1.5]
		arg = dhead;
		arg2 = dtail;					// working copy of argument
		arg3 = 0.0;
		increase = 0;					// signal-> no scaling for result
		if (arg < 1.5) {				// scale up argument by recursion formula
			increase = -1;				// signal-> result to be divided by factor
			factor1 = arg;
			factor2 = arg2;
			factor3 = 0.0;				// factor is the old argument
			while (arg < 1.5) {
				_d3add(arg, arg2, arg3, 1.0, 0.0, 0.0, &arg, &arg2, &arg3);
				if (arg < 1.5) {
					_d3mul(factor1, factor2, factor3, arg, arg2, arg3,
								&factor1, &factor2, &factor3);
				}
			}
			if (factor1 < 0.0) {
				signgam = -1;			// special case of negative arguments
			}
		}
		else if (arg > 2.5) {
			increase = +1;				// signal-> result must be mult by factor
			factor1 = 1.0;
			factor2 = 0.0;
			factor3 = 0.0;
			while (arg > 2.5) {						// reduce argument by 1
				arg = arg - 1.0;					// there may be room for bits to
				anew = __FADD( arg, arg2 );			// shift from low order word to
				arg2 = arg - anew + arg2;			// high order word
				arg = anew;
				_d3mul(factor1, factor2, factor3, arg, arg2, 0.0,
								&factor1, &factor2, &factor3);
			}
			arg3 = 0.0;
		}
		diff = __FSUB( arg, 2.0 );					// series in terms of
		z = __FADD( (arg - (diff + 2.0)), arg2 );	// (diff,z,x2)=arg-2
		z2 = (arg - (diff + 2.0)) - z + arg2 + arg3;
		y = __FADD( diff, z );
		y2 = (diff - y + z) + z2;
		y3 = (diff - y + z) - y2 + z2;
		sum  = zeta[53][0];
		suml = zeta[53][1];
		for (i = 52; i >= 40; --i) {
			sum = __FMADD( sum, y, zeta[i][0] ); // zeta[i][0] + sum*y;
		}
		sumt = __FMADD( sum, y, zeta[39][0] ); // zeta[39][0] + sum*y;
		suml = __FMADD( sum, y, zeta[39][0] - sumt) + __FMADD( sum, y2, zeta[39][1] ); // zeta[39][0] - sumt + sum*y + (zeta[39][1] + sum*y2);
		sum = sumt;
		for (i = 38; i >= 3; --i) {
			temp = __FMADD( sum, y, zeta[i][0] ); // zeta[i][0] + sum*y;
			// suml = (zeta[i][0] - temp + sum*y) + zeta[i][1] + (sum*y2 + suml*y);
			suml = __FMADD( sum, y, zeta[i][0] - temp ) +  zeta[i][1] + __FMADD( sum, y2, suml*y ); 
			sum = temp;
		}
		_d3mul(sum, suml, 0.0, y, y2, y3, &prod1, &prod2, &pextra2);
		_d3add(prod1, prod2, pextra2, zeta[2][0], zeta[2][1], zeta32.f,
					&sum1, &sum2, &sextra);
		_d3mul(sum1, sum2, sextra, y, y2, y3,
					&prod1, &prod2, &pextra1);  			// multiply sum by z
		_d3add(prod1, prod2, pextra1, ec.f, ec2.f, ec3.f,
					&sum1, &sum2, &sextra);	 				// add linear term
		_d3mul(sum1, sum2, sextra, y, y2, y3,
					&prod1, &prod2, &pextra);				// final mult. by z.
		
		if (igamma == 1) {			 // a Gamma entry
			tempdd.f = _ExpInnerLD(prod1, prod2, pextra, &eexp, 4);
			if (increase == 1) {
				_d3mul(tempdd.d[0], tempdd.d[1], eexp, factor1, factor2, factor3,
								&(gamdd.d[0]), &(gamdd.d[1]), &gmextra);
			}
			else if (increase == -1) {
				_d3div(tempdd.d[0], tempdd.d[1], eexp, factor1, factor2, factor3,
								&(gamdd.d[0]), &(gamdd.d[1]), &gmextra);
			}
			else {
				gamdd.f = tempdd.f;
				gmextra = eexp;
			}
		}
		else {				// entry was for log gamma
			if (increase == 0) {
				gamdd.d[0] = prod1;
				gamdd.d[1] = prod2;
				gmextra = pextra;
			}
			else {
				if (signgam < 0) {
					factor1 = -factor1;
					factor2 = -factor2;
					factor3 = -factor3;
				}
				factor = factor1;
				tempdd.f = _LogInnerLD(factor1, factor2, 0.0, &extra, 1);
				extra = extra + factor3/factor;
				if (increase == -1) {				// change sign of log
					tempdd.d[0] = -tempdd.d[0];
					tempdd.d[1] = -tempdd.d[1];
					extra = -extra;
				}
				_d3add(prod1, prod2, pextra, tempdd.d[0], tempdd.d[1], extra,
							&(gamdd.d[0]), &(gamdd.d[1]), &gmextra);
			}
		}
	}
	
	if (gamdd.d[0] != gamdd.d[0]) {
		gamdd.d[0] = infinity.f;
		gamdd.d[1] = zero;
		gmextra = 0.0;
	}
	
	if (ireflect == 1) {							// original argument less than 0.0
		arg = -dhead;								// recover original argument
		arg2 = -dtail;
													// reduce argument for computation
		_d3add(arg, arg2, 0.0, -int1, -int2, 0.0, &diff1, &diff2, &dextra);
		asize = fabs(diff1);

		if (asize < tiny.f) {  				 		// For arguments very close
			diff1  *= scaleup.f;					// to an integer, rescale,
			diff2  *= scaleup.f;            		// so that sin can be computed
			dextra *= scaleup.f;					// to a full 107+ bits
		}											// of sin (Pi x)

		_d3mul(diff1, diff2, dextra, pi.f, pib.f, pic.f, &sarg1, &sarg2, &saextra);
		xhintdd.f = 2.0*rintl(0.5*intdd.f);
		
		tempdd.d[0] = sarg1;
		tempdd.d[1] = sarg2;
		sinargdd.f = sinl(tempdd.f);				// sin of argument

		if ((xhintdd.d[0] - intdd.d[0] + xhintdd.d[1] - intdd.d[1]) != 0.0) {
			sinargdd.f = -sinargdd.f;
		}
			
		if (sinargdd.d[0] < 0.0) {
			sinargdd.f = -sinargdd.f;				// force sin(pi x) to have plus sign
			signgam = -signgam;						// show gamma(x) has negative sign
		}

		if (fabs(gamdd.d[0]) == infinity.f){		// result will underflow
			if(igamma == 1) {
				gamdd.d[0] = zero;
				gamdd.d[1] = zero;
			}
			else {
				gamdd.d[0] = -infinity.f;			//  gamma underflows, so
				gamdd.d[1] = zero;					//  lgamma overflows to -INF
			}
			return gamdd.f;
		}
		_d3mul(dhead, dtail, 0.0, sinargdd.d[0], sinargdd.d[1], 0.0,
					&prod1, &prod2, &pextra);		//  x sin(pi x)
		
		tempdd.f = _LogInnerLD(prod1, prod2, 0.0, &extralg, 1);	// log (x sin(pi x))
		extralg = extralg + pextra/prod1;
		_d3add(tempdd.d[0], tempdd.d[1], extralg, gamdd.d[0], gamdd.d[1], gmextra,
					&denom1, &denom2, &dextra);
		_d3add(piln.f, pilnb.f, pilnc.f, -denom1, -denom2, -dextra,
					&(gamdd.d[0]), &(gamdd.d[1]), &gmextra);
		
		if (asize < tiny.f) {
			// Compensate for scaling of argument to sin(pi x)
			_d3add(gamdd.d[0], gamdd.d[1], gmextra, sclog.f, sclog2.f, sclog3.f,
						&(gamdd.d[0]), &(gamdd.d[1]), &gmextra);
		}
		if (igamma == 1) {    // we really want gamma itself ?
			tempdd.f = _ExpInnerLD(gamdd.d[0], gamdd.d[1], gmextra, &eexp, 4);
			
			if (signgam == 1) {
				gamdd.f = tempdd.f;
			}
			else {
				gamdd.f = -tempdd.f;
			}
		}
	}
	return gamdd.f;
}	
コード例 #27
0
	void EXPORT_API ComputeVorticity(btVector3* predictedPositions, btVector3* velocities, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float KPOLY, float SPIKY, float H, float EPSILON_VORTICITY, float dt)
	{

		#pragma omp parallel
		{

			#pragma omp for
			for (int idA = 0; idA < pointsCount; idA++)
			{
				btVector3 deltaP(0, 0, 0);

				btVector3 predPosA = predictedPositions[idA];
				btVector3 velA = velocities[idA];
				btVector3 omega = btVector3(0.0f, 0.0f, 0.0f);
				btVector3 eta = btVector3(0.0f, 0.0f, 0.0f);

				for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
				{
					int idB = neighbours[idA * maxNeighboursPerPoint + nId];

					btVector3 predPosB = predictedPositions[idB];
					btVector3 velB = velocities[idB];
					btVector3 dir = predPosA - predPosB;
					

					btVector3 velocityDiff = velB - velA;
					btVector3 spiky = WSpiky(dir, SPIKY, H);
					btVector3 gradient = spiky;

					omega += btCross(velocityDiff, gradient);

					eta += spiky;

				}

				//float omegaLength = length(omega);
				float omegaLength = btDot(omega, omega);
				if (omegaLength == 0.0f)
				{
					//No direction for eta
					temp[idA] = btVector3(0.0f, 0.0f, 0.0f);
					continue;
				}

				//	float3 eta = eta(p, omegaLength);
				eta *= omegaLength;

				//	if (eta == float3(0.0f, 0.0f, 0.0f)) 
				if (btDot(eta, eta) == 0.0f)
				{
					//Particle is isolated or net force is 0
					temp[idA] = btVector3(0.0f, 0.0f, 0.0f);
					continue;
				}

				btVector3 etaNormal = eta.normalized();
				if (isinf(etaNormal.x()) || isinf(etaNormal.y()) || isinf(etaNormal.z()))
				{
					temp[idA] = btVector3(0.0f, 0.0f, 0.0f);
					//return;
					continue;
				}

				temp[idA] = btCross(etaNormal, omega) * EPSILON_VORTICITY;


			}

			#pragma omp for
			for (int i = 0; i < pointsCount; i++)
			{
				velocities[i] += temp[i] * dt;
			}
		}
	}
コード例 #28
0
ファイル: String.cpp プロジェクト: Hyzoran/Eoverse
wchar_t* DoubleToWStr( double D )
{
	wchar_t* s = new wchar_t[60];

	if(isnan( D )) {
		wcscpy( s, L"nan" );
	}
	else if(isinf( D )) {
		wcscpy( s, L"inf" );
	}
	else if(D == 0.0) {
		wcscpy( s, L"0" );
	}
	else {
		int digit, m, m1;
		wchar_t *c = s;
		int neg = ( D < 0 );
		if(neg)
			D = -D;
		// calculate magnitude
		m = log10( D );
		int useExp = ( m >= 14 || ( neg && m >= 9 ) || m <= -9 );
		if(neg)
			*( c++ ) = L'-';
		// set up for scientific notation
		if(useExp) {
			if(m < 0)
				m -= 1.0;
			D = D / pow( 10.0, m );
			m1 = m;
			m = 0;
		}
		if(m < 1.0) {
			m = 0;
		}
		// convert the number
		while(D > 0.00000000000001 || m >= 0) {
			double weight = pow( 10.0, m );
			if(weight > 0 && !isinf( weight )) {
				digit = floor( D / weight );
				D -= ( digit * weight );
				*( c++ ) = L'0' + digit;
			}
			if(m == 0 && D > 0)
				*( c++ ) = L'.';
			m--;
		}
		if(useExp) {
			// convert the exponent
			int i, j;
			*( c++ ) = L'e';
			if(m1 > 0) {
				*( c++ ) = L'+';
			}
			else {
				*( c++ ) = L'-';
				m1 = -m1;
			}
			m = 0;
			while(m1 > 0) {
				*( c++ ) = L'0' + m1 % 10;
				m1 /= 10;
				m++;
			}
			c -= m;
			for(i = 0, j = m - 1; i<j; i++, j--) {
				// swap without temporary
				c[i] ^= c[j];
				c[j] ^= c[i];
				c[i] ^= c[j];
			}
			c += m;
		}
		*( c ) = L'\0';
	}
	return s;
}
コード例 #29
0
ファイル: rrd_xport.c プロジェクト: baiyunping333/rrdtool-1.x
int rrd_xport_format_xmljson(int flags,stringbuffer_t *buffer,image_desc_t *im,time_t start, time_t end, unsigned long step, unsigned long col_cnt, char **legend_v, rrd_value_t* data) {

  /* define some other stuff based on flags */
  int json=0;
  if (flags &1) { json=1; }
  int showtime=0;
  if (flags &2) { showtime=1;}
  int enumds=0;
  if (flags &4) { enumds=1;}

  /* define the time format */
  char* timefmt=NULL;
  /* unfortunatley we have to do it this way, 
     as when no --x-graph argument is given,
     then the xlab_user is not in a clean state (e.g. zero-filled) */
  if (im->xlab_user.minsec!=-1) { timefmt=im->xlab_user.stst; }

  /* row count */
  unsigned long row_cnt=(end-start)/step;

  /* estimate buffer size (to avoid multiple allocations) */
  /* better estimates are needed here */
  buffer->allocated=
    1024 /* bytes of overhead /header/footer */
    +(12+19*col_cnt) /* 12 bytes overhead/line plus 19 bytes per column*/
    *(1+row_cnt) /* number of columns + 1 (for header) */
    ;
  char buf[256];
  char dbuf[1024];

  rrd_value_t *ptr = data;
  if (json == 0){
    snprintf(buf,sizeof(buf),
	     "<?xml version=\"1.0\" encoding=\"%s\"?>\n\n<%s>\n  <%s>\n",
	     XML_ENCODING,ROOT_TAG,META_TAG);
    addToBuffer(buffer,buf,0);
  }
  else {
    addToBuffer(buffer,"{ \"about\": \"RRDtool graph JSON output\",\n  \"meta\": {\n",0);
  }
  
  /* calculate start time */
  if (timefmt) {
    struct tm loc;
    time_t ti=start+step;
    localtime_r(&ti,&loc);
    strftime(dbuf,sizeof(dbuf),timefmt,&loc);
    if (json) {
      snprintf(buf,sizeof(buf),"    \"%s\": \"%s\",\n",META_START_TAG,dbuf);
    } else {
      snprintf(buf,sizeof(buf),"    <%s>%s</%s>\n",META_START_TAG,dbuf,META_START_TAG);
    }
  } else {
    if (json) {
      snprintf(buf,sizeof(buf),"    \"%s\": %lld,\n",META_START_TAG,(long long int)start+step);
    } else {
      snprintf(buf,sizeof(buf),"    <%s>%lld</%s>\n",META_START_TAG,(long long int)start+step,META_START_TAG);
    }
  }
  addToBuffer(buffer,buf,0);

  /* calculate end time */
  if (timefmt) {
    struct tm loc;
    time_t ti=end;
    localtime_r(&ti,&loc);
    strftime(dbuf,sizeof(dbuf),timefmt,&loc);
    if (json) {
      snprintf(buf,sizeof(buf),"    \"%s\": \"%s\",\n",META_END_TAG,dbuf);
    } else {
      snprintf(buf,sizeof(buf),"    <%s>%s</%s>\n",META_END_TAG,dbuf,META_END_TAG);
    }
  } else {
    if (json) {
      snprintf(buf,sizeof(buf),"    \"%s\": %lld,\n",META_END_TAG,(long long int)end);
    } else {
      snprintf(buf,sizeof(buf),"    <%s>%lld</%s>\n",META_END_TAG,(long long int)end,META_END_TAG);
    }
  }
  addToBuffer(buffer,buf,0);
  /* print other info */
  if (json) {
    snprintf(buf,sizeof(buf),"    \"%s\": %lld,\n",META_STEP_TAG,(long long int)step);
    addToBuffer(buffer,buf,0);
  } else {
    snprintf(buf,sizeof(buf),"    <%s>%lld</%s>\n",META_STEP_TAG,(long long int)step,META_STEP_TAG);
    addToBuffer(buffer,buf,0);
    snprintf(buf,sizeof(buf),"    <%s>%lu</%s>\n",META_ROWS_TAG,row_cnt,META_ROWS_TAG);
    addToBuffer(buffer,buf,0);
    snprintf(buf,sizeof(buf),"    <%s>%lu</%s>\n",META_COLS_TAG,col_cnt,META_COLS_TAG);
    addToBuffer(buffer,buf,0);
  }
  
  /* start legend */
  if (json){
    snprintf(buf,sizeof(buf),"    \"%s\": [\n", LEGEND_TAG);
  }
  else {
    snprintf(buf,sizeof(buf),"    <%s>\n", LEGEND_TAG);
  }
  addToBuffer(buffer,buf,0);
  /* add legend entries */
  for (unsigned long j = 0; j < col_cnt; j++) {
    char *entry = legend_v[j];
    /* I do not know why the legend is "spaced", but let us skip it */
    while(isspace(*entry)){entry++;}
    /* now output it */
    if (json){
      snprintf(buf,sizeof(buf),"      \"%s\"", entry);
      addToBuffer(buffer,buf,0);
      if (j < col_cnt -1){
	addToBuffer(buffer,",",1);
      }
      addToBuffer(buffer,"\n",1);
    }
    else {
      snprintf(buf,sizeof(buf),"      <%s>%s</%s>\n", LEGEND_ENTRY_TAG, entry,LEGEND_ENTRY_TAG);
      addToBuffer(buffer,buf,0);
    }
  }
  /* end legend */
  if (json){
    snprintf(buf,sizeof(buf),"          ]\n");
  }
  else {
    snprintf(buf,sizeof(buf),"    </%s>\n", LEGEND_TAG);
  }
  addToBuffer(buffer,buf,0);
  
  /* add graphs and prints */
  if (rrd_xport_format_addprints(json,buffer,im)) {return -1;}

  /* if we have got a trailing , then kill it */
  if (buffer->data) {
    if (buffer->data[buffer->len-2]==',') { 
      buffer->data[buffer->len-2]=buffer->data[buffer->len-1];
      buffer->len--;
    }
  }

  /* end meta */
  if (json){
    snprintf(buf,sizeof(buf),"     },\n");
  } else {
    snprintf(buf,sizeof(buf),"  </%s>\n", META_TAG);
  }
  addToBuffer(buffer,buf,0);

  
  /* start data */
  if (json){
    snprintf(buf,sizeof(buf),"  \"%s\": [\n",DATA_TAG);
  } else {
    snprintf(buf,sizeof(buf),"  <%s>\n", DATA_TAG);
  }
  addToBuffer(buffer,buf,0);
  /* iterate over data */
  for (time_t ti = start + step; ti < end; ti += step) {
    if (timefmt) {
      struct tm loc;
      localtime_r(&ti,&loc);
      strftime(dbuf,sizeof(dbuf),timefmt,&loc);
    } else {
      snprintf(dbuf,sizeof(dbuf),"%lld",(long long int)ti);
    }
    if (json){
      addToBuffer(buffer,"    [ ",0);
      if(showtime){
	addToBuffer(buffer,"\"",1);
	addToBuffer(buffer,dbuf,0);
	addToBuffer(buffer,"\",",2);
      }
    }
    else {
      if (showtime) {
        snprintf(buf,sizeof(buf),
                 "    <%s><%s>%s</%s>", DATA_ROW_TAG,COL_TIME_TAG, dbuf, COL_TIME_TAG);
      } else {
        snprintf(buf,sizeof(buf),
                 "    <%s>", DATA_ROW_TAG);
      }
      addToBuffer(buffer,buf,0);
    }
    for (unsigned long j = 0; j < col_cnt; j++) {
      rrd_value_t newval = DNAN;
      newval = *ptr;
      if (json){
	if (isnan(newval) || isinf(newval)){
	  addToBuffer(buffer,"null",0);                        
	} else {
          rrd_snprintf(buf,sizeof(buf),"%0.10e",newval);
	  addToBuffer(buffer,buf,0);
	}
	if (j < col_cnt -1){
	  addToBuffer(buffer,", ",0);
	}
      }
      else {
	if (isnan(newval)) {
	  if (enumds) {
            snprintf(buf,sizeof(buf),"<%s%lu>NaN</%s%lu>", COL_DATA_TAG,j,COL_DATA_TAG,j);
	  } else {
            snprintf(buf,sizeof(buf),"<%s>NaN</%s>", COL_DATA_TAG,COL_DATA_TAG);
	  }
	} else {
	  if (enumds) {
            rrd_snprintf(buf,sizeof(buf),"<%s%lu>%0.10e</%s%lu>", COL_DATA_TAG,j,newval,COL_DATA_TAG,j);
	  } else {
            rrd_snprintf(buf,sizeof(buf),"<%s>%0.10e</%s>", COL_DATA_TAG,newval,COL_DATA_TAG);
	  }
	}
	addToBuffer(buffer,buf,0);
      }
      ptr++;
    }                
    if (json){
      addToBuffer(buffer,(ti < end-(time_t)step ? " ],\n" : " ]\n"),0);
    }
    else {                
      snprintf(buf,sizeof(buf),"</%s>\n", DATA_ROW_TAG);
      addToBuffer(buffer,buf,0);
    }
  }
  /* end data */
  if (json){
    addToBuffer(buffer,"  ]\n",0);
  }
  else {
    snprintf(buf,sizeof(buf),"  </%s>\n",DATA_TAG);
    addToBuffer(buffer,buf,0);
  }

  /* end all */
  if (json){
    addToBuffer(buffer,"}\n",0);
  } else {
    snprintf(buf,sizeof(buf),"</%s>\n", ROOT_TAG);
    addToBuffer(buffer,buf,0);
  }
  return 0;
}
コード例 #30
0
ファイル: isfinite.c プロジェクト: Logout22/Escape
int isfinite(double x) {
	return !isnan(x) && !isinf(x);
}