/*
 * Reduces the input array (in place)
 * length specifies the length of the array
 */
int 
BinomialOption::binomialOptionCPUReference()
{
    refOutput = (float*)malloc(numSamples * sizeof(cl_float4));
    CHECK_ALLOCATION(refOutput, "Failed to allocate host memory. (refOutput)");

    float* stepsArray = (float*)malloc((numSteps + 1) * sizeof(cl_float4));
    CHECK_ALLOCATION(stepsArray, "Failed to allocate host memory. (stepsArray)");

    // Iterate for all samples
    for(int bid = 0; bid < numSamples; ++bid)
    {
        float s[4];
        float x[4];
        float vsdt[4];
        float puByr[4];
        float pdByr[4];
        float optionYears[4];

        float inRand[4];

        for(int i = 0; i < 4; ++i)
        {
            inRand[i] = randArray[bid + i];
            s[i] = (1.0f - inRand[i]) * 5.0f + inRand[i] * 30.f;
            x[i] = (1.0f - inRand[i]) * 1.0f + inRand[i] * 100.f;
            optionYears[i] = (1.0f - inRand[i]) * 0.25f + inRand[i] * 10.f; 
            float dt = optionYears[i] * (1.0f / (float)numSteps);
            vsdt[i] = VOLATILITY * sqrtf(dt);
            float rdt = RISKFREE * dt;
            float r = expf(rdt);
            float rInv = 1.0f / r;
            float u = expf(vsdt[i]);
            float d = 1.0f / u;
            float pu = (r - d)/(u - d);
            float pd = 1.0f - pu;
            puByr[i] = pu * rInv;
            pdByr[i] = pd * rInv;
        }
        /**
         * Compute values at expiration date:
         * Call option value at period end is v(t) = s(t) - x
         * If s(t) is greater than x, or zero otherwise...
         * The computation is similar for put options...
         */
        for(int j = 0; j <= numSteps; j++)
        {
            for(int i = 0; i < 4; ++i)
            {
                float profit = s[i] * expf(vsdt[i] * (2.0f * j - numSteps)) - x[i];
                stepsArray[j * 4 + i] = profit > 0.0f ? profit : 0.0f;
            }
        }

        /**
         * walk backwards up on the binomial tree of depth numSteps
         * Reduce the price step by step
         */
        for(int j = numSteps; j > 0; --j)
        {
            for(int k = 0; k <= j - 1; ++k)
            {
                for(int i = 0; i < 4; ++i)
                {
                    stepsArray[k * 4 + i] = pdByr[i] * stepsArray[(k + 1) * 4 + i] + puByr[i] * stepsArray[k * 4 + i];
                }
            }   
        }

        //Copy the root to result
        refOutput[bid] = stepsArray[0];
    }
    
    free(stepsArray);

    return SDK_SUCCESS;
}
Esempio n. 2
0
static float filter_func_gaussian(float v, float width)
{
  v *= 6.0f / width;
  return expf(-2.0f * v * v);
}
Esempio n. 3
0
/**
 * Module task
 */
static void stabilizationTask(void* parameters)
{
    UAVObjEvent ev;

    uint32_t timeval = PIOS_DELAY_GetRaw();

    ActuatorDesiredData actuatorDesired;
    StabilizationDesiredData stabDesired;
    RateDesiredData rateDesired;
    AttitudeActualData attitudeActual;
    GyrosData gyrosData;
    FlightStatusData flightStatus;

    float *stabDesiredAxis = &stabDesired.Roll;
    float *actuatorDesiredAxis = &actuatorDesired.Roll;
    float *rateDesiredAxis = &rateDesired.Roll;
    float horizonRateFraction = 0.0f;

    // Force refresh of all settings immediately before entering main task loop
    SettingsUpdatedCb((UAVObjEvent *) NULL);

    // Settings for system identification
    uint32_t iteration = 0;
    const uint32_t SYSTEM_IDENT_PERIOD = 75;
    uint32_t system_ident_timeval = PIOS_DELAY_GetRaw();

    float dT_filtered = 0;

    // Main task loop
    zero_pids();
    while(1) {
        iteration++;

        PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);

        // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
        if (PIOS_Queue_Receive(queue, &ev, FAILSAFE_TIMEOUT_MS) != true)
        {
            AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
            continue;
        }

        calculate_pids();

        float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
        timeval = PIOS_DELAY_GetRaw();

        // exponential moving averaging (EMA) of dT to reduce jitter; ~200points
        // to have more or less equivalent noise reduction to a normal N point moving averaging:  alpha = 2 / (N + 1)
        // run it only at the beginning for the first samples, to reduce CPU load, and the value should converge to a constant value

        if (iteration < 100) {
            dT_filtered = dT;
        } else if (iteration < 2000) {
            dT_filtered = 0.01f * dT + (1.0f - 0.01f) * dT_filtered;
        } else if (iteration == 2000) {
            gyro_filter_updated = true;
        }

        if (gyro_filter_updated) {
            if (settings.GyroCutoff < 1.0f) {
                gyro_alpha = 0;
            } else {
                gyro_alpha = expf(-2.0f * (float)(M_PI) *
                                  settings.GyroCutoff * dT_filtered);
            }

            // Compute time constant for vbar decay term
            if (settings.VbarTau < 0.001f) {
                vbar_decay = 0;
            } else {
                vbar_decay = expf(-dT_filtered / settings.VbarTau);
            }

            gyro_filter_updated = false;
        }

        FlightStatusGet(&flightStatus);
        StabilizationDesiredGet(&stabDesired);
        AttitudeActualGet(&attitudeActual);
        GyrosGet(&gyrosData);
        ActuatorDesiredGet(&actuatorDesired);
#if defined(RATEDESIRED_DIAGNOSTICS)
        RateDesiredGet(&rateDesired);
#endif

        struct TrimmedAttitudeSetpoint {
            float Roll;
            float Pitch;
            float Yaw;
        } trimmedAttitudeSetpoint;

        // Mux in level trim values, and saturate the trimmed attitude setpoint.
        trimmedAttitudeSetpoint.Roll = bound_min_max(
                                           stabDesired.Roll + trimAngles.Roll,
                                           -settings.RollMax + trimAngles.Roll,
                                           settings.RollMax + trimAngles.Roll);
        trimmedAttitudeSetpoint.Pitch = bound_min_max(
                                            stabDesired.Pitch + trimAngles.Pitch,
                                            -settings.PitchMax + trimAngles.Pitch,
                                            settings.PitchMax + trimAngles.Pitch);
        trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw;

        // For horizon mode we need to compute the desire attitude from an unscaled value and apply the
        // trim offset. Also track the stick with the most deflection to choose rate blending.
        horizonRateFraction = 0.0f;
        if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
            trimmedAttitudeSetpoint.Roll = bound_min_max(
                                               stabDesired.Roll * settings.RollMax + trimAngles.Roll,
                                               -settings.RollMax + trimAngles.Roll,
                                               settings.RollMax + trimAngles.Roll);
            horizonRateFraction = fabsf(stabDesired.Roll);
        }
        if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
            trimmedAttitudeSetpoint.Pitch = bound_min_max(
                                                stabDesired.Pitch * settings.PitchMax + trimAngles.Pitch,
                                                -settings.PitchMax + trimAngles.Pitch,
                                                settings.PitchMax + trimAngles.Pitch);
            horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Pitch));
        }
        if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
            trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw * settings.YawMax;
            horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Yaw));
        }

        // For weak leveling mode the attitude setpoint is the trim value (drifts back towards "0")
        if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
            trimmedAttitudeSetpoint.Roll = trimAngles.Roll;
        }
        if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
            trimmedAttitudeSetpoint.Pitch = trimAngles.Pitch;
        }
        if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
            trimmedAttitudeSetpoint.Yaw = 0;
        }

        // Note we divide by the maximum limit here so the fraction ranges from 0 to 1 depending on
        // how much is requested.
        horizonRateFraction = bound_sym(horizonRateFraction, HORIZON_MODE_MAX_BLEND) / HORIZON_MODE_MAX_BLEND;

        // Calculate the errors in each axis. The local error is used in the following modes:
        //  ATTITUDE, HORIZON, WEAKLEVELING
        float local_attitude_error[3];
        local_attitude_error[0] = trimmedAttitudeSetpoint.Roll - attitudeActual.Roll;
        local_attitude_error[1] = trimmedAttitudeSetpoint.Pitch - attitudeActual.Pitch;
        local_attitude_error[2] = trimmedAttitudeSetpoint.Yaw - attitudeActual.Yaw;

        // Wrap yaw error to [-180,180]
        local_attitude_error[2] = circular_modulus_deg(local_attitude_error[2]);

        static float gyro_filtered[3];
        gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
        gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
        gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);

        // A flag to track which stabilization mode each axis is in
        static uint8_t previous_mode[MAX_AXES] = {255,255,255};
        bool error = false;

        //Run the selected stabilization algorithm on each axis:
        for(uint8_t i=0; i< MAX_AXES; i++)
        {
            // Check whether this axis mode needs to be reinitialized
            bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]);
            // The unscaled input (-1,1)
            float *raw_input = &stabDesired.Roll;
            previous_mode[i] = stabDesired.StabilizationMode[i];
            // Apply the selected control law
            switch(stabDesired.StabilizationMode[i])
            {
            case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
                if(reinit)
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;

                // Store to rate desired variable for storing to UAVO
                rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);

                // Compute the inner loop
                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

                break;

            case STABILIZATIONDESIRED_STABILIZATIONMODE_ACROPLUS:
                // this implementation is based on the Openpilot/Librepilot Acro+ flightmode
                // and our existing rate & MWRate flightmodes
                if(reinit)
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;

                // The factor for gyro suppression / mixing raw stick input into the output; scaled by raw stick input
                float factor = fabsf(raw_input[i]) * settings.AcroInsanityFactor / 100;

                // Store to rate desired variable for storing to UAVO
                rateDesiredAxis[i] = bound_sym(raw_input[i] * settings.ManualRate[i], settings.ManualRate[i]);

                // Zero integral for aggressive maneuvers, like it is done for MWRate
                if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) ||
                        (i == 0 && fabsf(raw_input[i]) > 0.2f)) {
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
                    pids[PID_RATE_ROLL + i].i = 0;
                }

                // Compute the inner loop
                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], rateDesiredAxis[i], gyro_filtered[i], dT);
                actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i];
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f);

                break;
            case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
                if(reinit) {
                    pids[PID_ATT_ROLL + i].iAccumulator = 0;
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
                }

                // Compute the outer loop
                rateDesiredAxis[i] = pid_apply(&pids[PID_ATT_ROLL + i], local_attitude_error[i], dT);
                rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);

                // Compute the inner loop
                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

                break;

            case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
                // Store for debugging output
                rateDesiredAxis[i] = stabDesiredAxis[i];

                // Run a virtual flybar stabilization algorithm on this axis
                stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &pids[PID_VBAR_ROLL + i], &settings);

                break;
            case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
            {
                if (reinit)
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;

                float weak_leveling = local_attitude_error[i] * weak_leveling_kp;
                weak_leveling = bound_sym(weak_leveling, weak_leveling_max);

                // Compute desired rate as input biased towards leveling
                rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

                break;
            }
            case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
                if (reinit)
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;

                if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
                    // While getting strong commands act like rate mode
                    rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);

                    // Reset accumulator
                    axis_lock_accum[i] = 0;
                } else {
                    // For weaker commands or no command simply lock (almost) on no gyro change
                    axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
                    axis_lock_accum[i] = bound_sym(axis_lock_accum[i], max_axis_lock);

                    // Compute the inner loop
                    float tmpRateDesired = pid_apply(&pids[PID_ATT_ROLL + i], axis_lock_accum[i], dT);
                    rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]);
                }

                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

                break;

            case STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON:
                if(reinit) {
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
                }

                // Do not allow outer loop integral to wind up in this mode since the controller
                // is often disengaged.
                pids[PID_ATT_ROLL + i].iAccumulator = 0;

                // Compute the outer loop for the attitude control
                float rateDesiredAttitude = pid_apply(&pids[PID_ATT_ROLL + i], local_attitude_error[i], dT);
                // Compute the desire rate for a rate control
                float rateDesiredRate = raw_input[i] * settings.ManualRate[i];

                // Blend from one rate to another. The maximum of all stick positions is used for the
                // amount so that when one axis goes completely to rate the other one does too. This
                // prevents doing flips while one axis tries to stay in attitude mode.
                rateDesiredAxis[i] = rateDesiredAttitude * (1.0f-horizonRateFraction) + rateDesiredRate * horizonRateFraction;
                rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]);

                // Compute the inner loop
                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

                break;

            case STABILIZATIONDESIRED_STABILIZATIONMODE_MWRATE:
            {
                if(reinit) {
                    pids[PID_MWR_ROLL + i].iAccumulator = 0;
                }

                /*
                 Conversion from MultiWii PID settings to our units.
                	Kp = Kp_mw * 4 / 80 / 500
                	Kd = Kd_mw * looptime * 1e-6 * 4 * 3 / 32 / 500
                	Ki = Ki_mw * 4 / 125 / 64 / (looptime * 1e-6) / 500

                	These values will just be approximate and should help
                	you get started.
                */

                // The unscaled input (-1,1) - note in MW this is from (-500,500)
                float *raw_input = &stabDesired.Roll;

                // dynamic PIDs are scaled both by throttle and stick position
                float scale = (i == 0 || i == 1) ? mwrate_settings.RollPitchRate : mwrate_settings.YawRate;
                float pid_scale = (100.0f - scale * fabsf(raw_input[i])) / 100.0f;
                float dynP8 = pids[PID_MWR_ROLL + i].p * pid_scale;
                float dynD8 = pids[PID_MWR_ROLL + i].d * pid_scale;
                // these terms are used by the integral loop this proportional term is scaled by throttle (this is different than MW
                // that does not apply scale
                float cfgP8 = pids[PID_MWR_ROLL + i].p;
                float cfgI8 = pids[PID_MWR_ROLL + i].i;

                // Dynamically adjust PID settings
                struct pid mw_pid;
                mw_pid.p = 0;      // use zero Kp here because of strange setpoint. applied later.
                mw_pid.d = dynD8;
                mw_pid.i = cfgI8;
                mw_pid.iLim = pids[PID_MWR_ROLL + i].iLim;
                mw_pid.iAccumulator = pids[PID_MWR_ROLL + i].iAccumulator;
                mw_pid.lastErr = pids[PID_MWR_ROLL + i].lastErr;
                mw_pid.lastDer = pids[PID_MWR_ROLL + i].lastDer;

                // Zero integral for aggressive maneuvers
                if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) ||
                        (i == 0 && fabsf(raw_input[i]) > 0.2f)) {
                    mw_pid.iAccumulator = 0;
                    mw_pid.i = 0;
                }

                // Apply controller as if we want zero change, then add stick input afterwards
                actuatorDesiredAxis[i] = pid_apply_setpoint(&mw_pid,  raw_input[i] / cfgP8,  gyro_filtered[i], dT);
                actuatorDesiredAxis[i] += raw_input[i];             // apply input
                actuatorDesiredAxis[i] -= dynP8 * gyro_filtered[i]; // apply Kp term
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

                // Store PID accumulators for next cycle
                pids[PID_MWR_ROLL + i].iAccumulator = mw_pid.iAccumulator;
                pids[PID_MWR_ROLL + i].lastErr = mw_pid.lastErr;
                pids[PID_MWR_ROLL + i].lastDer = mw_pid.lastDer;
            }
            break;
            case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
                if(reinit) {
                    pids[PID_ATT_ROLL + i].iAccumulator = 0;
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
                }

                static uint32_t ident_iteration = 0;
                static float ident_offsets[3] = {0};

                if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) {
                    ident_iteration++;
                    system_ident_timeval = PIOS_DELAY_GetRaw();

                    SystemIdentData systemIdent;
                    SystemIdentGet(&systemIdent);

                    const float SCALE_BIAS = 7.1f;
                    float roll_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_ROLL]);
                    float pitch_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_PITCH]);
                    float yaw_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_YAW]);

                    if (roll_scale > 0.25f)
                        roll_scale = 0.25f;
                    if (pitch_scale > 0.25f)
                        pitch_scale = 0.25f;
                    if (yaw_scale > 0.25f)
                        yaw_scale = 0.2f;

                    switch(ident_iteration & 0x07) {
                    case 0:
                        ident_offsets[0] = 0;
                        ident_offsets[1] = 0;
                        ident_offsets[2] = yaw_scale;
                        break;
                    case 1:
                        ident_offsets[0] = roll_scale;
                        ident_offsets[1] = 0;
                        ident_offsets[2] = 0;
                        break;
                    case 2:
                        ident_offsets[0] = 0;
                        ident_offsets[1] = 0;
                        ident_offsets[2] = -yaw_scale;
                        break;
                    case 3:
                        ident_offsets[0] = -roll_scale;
                        ident_offsets[1] = 0;
                        ident_offsets[2] = 0;
                        break;
                    case 4:
                        ident_offsets[0] = 0;
                        ident_offsets[1] = 0;
                        ident_offsets[2] = yaw_scale;
                        break;
                    case 5:
                        ident_offsets[0] = 0;
                        ident_offsets[1] = pitch_scale;
                        ident_offsets[2] = 0;
                        break;
                    case 6:
                        ident_offsets[0] = 0;
                        ident_offsets[1] = 0;
                        ident_offsets[2] = -yaw_scale;
                        break;
                    case 7:
                        ident_offsets[0] = 0;
                        ident_offsets[1] = -pitch_scale;
                        ident_offsets[2] = 0;
                        break;
                    }
                }

                if (i == ROLL || i == PITCH) {
                    // Compute the outer loop
                    rateDesiredAxis[i] = pid_apply(&pids[PID_ATT_ROLL + i], local_attitude_error[i], dT);
                    rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);

                    // Compute the inner loop
                    actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                    actuatorDesiredAxis[i] += ident_offsets[i];
                    actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
                } else {
                    // Get the desired rate. yaw is always in rate mode in system ident.
                    rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);

                    // Compute the inner loop only for yaw
                    actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                    actuatorDesiredAxis[i] += ident_offsets[i];
                    actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
                }

                break;

            case STABILIZATIONDESIRED_STABILIZATIONMODE_COORDINATEDFLIGHT:
                switch (i) {
                case YAW:
                    if (reinit) {
                        pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
                        pids[PID_RATE_YAW].iAccumulator = 0;
                        axis_lock_accum[YAW] = 0;
                    }

                    //If we are not in roll attitude mode, trigger an error
                    if (stabDesired.StabilizationMode[ROLL] != STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
                    {
                        error = true;
                        break ;
                    }

                    if (fabsf(stabDesired.Yaw) < COORDINATED_FLIGHT_MAX_YAW_THRESHOLD) { //If yaw is within the deadband...
                        if (fabsf(stabDesired.Roll) > COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting more roll than the threshold
                            float accelsDataY;
                            AccelsyGet(&accelsDataY);

                            //Reset integral if we have changed roll to opposite direction from rudder. This implies that we have changed desired turning direction.
                            if ((stabDesired.Roll > 0 && actuatorDesiredAxis[YAW] < 0) ||
                                    (stabDesired.Roll < 0 && actuatorDesiredAxis[YAW] > 0)) {
                                pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
                            }

                            // Coordinate flight can simply be seen as ensuring that there is no lateral acceleration in the
                            // body frame. As such, we use the (noisy) accelerometer data as our measurement. Ideally, at
                            // some point in the future we will estimate acceleration and then we can use the estimated value
                            // instead of the measured value.
                            float errorSlip = -accelsDataY;

                            float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip, dT);
                            actuatorDesiredAxis[YAW] = bound_sym(command ,1.0);

                            // Reset axis-lock integrals
                            pids[PID_RATE_YAW].iAccumulator = 0;
                            axis_lock_accum[YAW] = 0;
                        } else if (fabsf(stabDesired.Roll) <= COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting less roll than the threshold
                            // Axis lock on no gyro change
                            axis_lock_accum[YAW] += (0 - gyro_filtered[YAW]) * dT;

                            rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW], dT);
                            rateDesiredAxis[YAW] = bound_sym(rateDesiredAxis[YAW], settings.MaximumRate[YAW]);

                            actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW],  rateDesiredAxis[YAW],  gyro_filtered[YAW], dT);
                            actuatorDesiredAxis[YAW] = bound_sym(actuatorDesiredAxis[YAW],1.0f);

                            // Reset coordinated-flight integral
                            pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
                        }
                    } else { //... yaw is outside the deadband. Pass the manual input directly to the actuator.
                        actuatorDesiredAxis[YAW] = bound_sym(stabDesiredAxis[YAW], 1.0);

                        // Reset all integrals
                        pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
                        pids[PID_RATE_YAW].iAccumulator = 0;
                        axis_lock_accum[YAW] = 0;
                    }
                    break;
                case ROLL:
                case PITCH:
                default:
                    //Coordinated Flight has no effect in these modes. Trigger a configuration error.
                    error = true;
                    break;
                }

                break;

            case STABILIZATIONDESIRED_STABILIZATIONMODE_POI:
                // The sanity check enforces this is only selectable for Yaw
                // for a gimbal you can select pitch too.
                if(reinit) {
                    pids[PID_ATT_ROLL + i].iAccumulator = 0;
                    pids[PID_RATE_ROLL + i].iAccumulator = 0;
                }

                float error;
                float angle;
                if (CameraDesiredHandle()) {
                    switch(i) {
                    case PITCH:
                        CameraDesiredDeclinationGet(&angle);
                        error = circular_modulus_deg(angle - attitudeActual.Pitch);
                        break;
                    case ROLL:
                    {
                        uint8_t roll_fraction = 0;
#ifdef GIMBAL
                        if (BrushlessGimbalSettingsHandle()) {
                            BrushlessGimbalSettingsRollFractionGet(&roll_fraction);
                        }
#endif /* GIMBAL */

                        // For ROLL POI mode we track the FC roll angle (scaled) to
                        // allow keeping some motion
                        CameraDesiredRollGet(&angle);
                        angle *= roll_fraction / 100.0f;
                        error = circular_modulus_deg(angle - attitudeActual.Roll);
                    }
                    break;
                    case YAW:
                        CameraDesiredBearingGet(&angle);
                        error = circular_modulus_deg(angle - attitudeActual.Yaw);
                        break;
                    default:
                        error = true;
                    }
                } else
                    error = true;

                // Compute the outer loop
                rateDesiredAxis[i] = pid_apply(&pids[PID_ATT_ROLL + i], error, dT);
                rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]);

                // Compute the inner loop
                actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i],  rateDesiredAxis[i],  gyro_filtered[i], dT);
                actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

                break;
            case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
                actuatorDesiredAxis[i] = bound_sym(stabDesiredAxis[i],1.0f);
                break;
            default:
                error = true;
                break;
            }
        }

        if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE)
            stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);

#if defined(RATEDESIRED_DIAGNOSTICS)
        RateDesiredSet(&rateDesired);
#endif

        // Save dT
        actuatorDesired.UpdateTime = dT * 1000;
        actuatorDesired.Throttle = stabDesired.Throttle;

        if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_MANUAL) {
            ActuatorDesiredSet(&actuatorDesired);
        } else {
            // Force all axes to reinitialize when engaged
            for(uint8_t i=0; i< MAX_AXES; i++)
                previous_mode[i] = 255;
        }

        if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
                (lowThrottleZeroIntegral && stabDesired.Throttle < 0))
        {
            // Force all axes to reinitialize when engaged
            for(uint8_t i=0; i< MAX_AXES; i++)
                previous_mode[i] = 255;
        }

        // Clear or set alarms.  Done like this to prevent toggling each cycle
        // and hammering system alarms
        if (error)
            AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR);
        else
            AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
    }
}
Esempio n. 4
0
void pdsp::EnvelopeFollower::prepareUnit( int expectedBufferSize, double sampleRate ){
    this->sampleRate = sampleRate;
    envelopeOutput = 0.0f;
    attackCoeff  = expf( TC / (50.0f * sampleRate * 0.001f) );
    releaseCoeff = expf( TC / (50.0f * sampleRate * 0.001f) );
}
Esempio n. 5
0
static GMQCC_INLINE ast_expression *fold_intrin_exp(fold_t *fold, ast_value *a) {
    return fold_constgen_float(fold, expf(fold_immvalue_float(a)));
}
Esempio n. 6
0
int get_Emission(HMMER_PROFILE *hmm, const char* hmm_Path)
{
  char* BEGIN = "  COMPO   ";
  int i = 0;
  int j = 0;
  int q = 0;

  FilePointer = fopen(hmm_Path, "r");
  if(FilePointer == NULL) {
    printf("Fatal error: Cannot open or find <.hmm> file\n");
    getchar();
    return fileERROR;
  } else {
    //printf(".hmm file open well\n");
  }

  /* 1. locate the 'COMPO' */
  char* locate = (char*)malloc(11 * sizeof(char));  //why 11? since we need use 'fgets' to search BEGIN
  do{
    fgets(locate, 11, FilePointer);
    if(strcmp(locate, BEGIN))
      nextLine(FilePointer, 1);
  }while(strcmp(locate, BEGIN) && !feof(FilePointer));

  /* 2. Process node 0 for I (no M here) */
  char* m_Temp = (char*)malloc(179 * sizeof(char)); //why 179? since each line has 178 chars
  char* match;

  char* i_Temp = (char*)malloc(179 * sizeof(char));
  char* insert;

  nextLine(FilePointer, 1);
  moveCursor(FilePointer, 10);

  fgets(i_Temp, 179, FilePointer);
  insert = strtok(i_Temp, "  ");
  while(insert) {   
    //printf("%s\n", insert);
    hmm->ins_32bits[0][i] = expf(-1.0 * (float)atof(insert));       //这个要放在这里,因为你放在p赋值语句后,最后一次循环p会被赋值NULL,这里就错了
    i++;   
    insert = strtok(NULL, "  "); 
  }

  nextLine(FilePointer, 2);
  moveCursor(FilePointer, 10);

  /* 3. Process node 1 to M (both I and M) */
  
  j = j + 1;  //above, we read Node 0 of INSERT, but we wont use them since we will set all Node 0 of MATCH and INSERT into -infinity
              //Thus, here we j++, for both INSERT and MATCH, to make start from Node 1;  

  do{
    //Match Emission
    i = 0;
    fgets(m_Temp, 179, FilePointer);          //记住用 fgets的时候,要多出一个长度 num,因为他只截取 num-1个字符(这里情况特殊多出两个无所谓)
    match = strtok(m_Temp, "  ");
    //mat[i][j] = (float)atof(p);
    while(match) {   
      //printf("%s\n", match);
      hmm->mat_32bits[j][i] = expf(-1.0 * (float)atof(match)); //这个要放在这里,因为你放在p赋值语句后,最后一次循环p会被赋值NULL,这里就错了
      i++;   
      match = strtok(NULL, "  ");   
    }

    //Insert Emission
    i = 0;
    nextLine(FilePointer, 1);
    moveCursor(FilePointer, 10);

    fgets(i_Temp, 179, FilePointer);
    insert = strtok(i_Temp, "  ");
    while(insert) {   
      //printf("%s\n", insert);
      hmm->ins_32bits[j][i] = expf(-1.0 * (float)atof(insert)); //这个要放在这里,因为你放在p赋值语句后,最后一次循环p会被赋值NULL,这里就错了
      i++;   
      insert = strtok(NULL, "  "); 
    }

    //Finish one node..
    nextLine(FilePointer, 2);
    moveCursor(FilePointer, 10);

    j++;

  }while(j <= hmm->M);      //ensure that we can fill-up Mat/Ins[HMM_SIZE + 1][PROTEIN_TYPE]

  /* 4. free */
  fclose(FilePointer);
  free(locate);
  free(m_Temp);
  free(i_Temp);

  return fileOK;
}
Esempio n. 7
0
	void MayaCamera::Update(float timestep)
	{
		(void)timestep;

		// Track mouse motion
		int2 mousePos;
		GetCursorPos((POINT *)&mousePos);
		int2 mouseMove = mousePos - m_mousePosPrev;
		m_mousePosPrev = mousePos;

		// Handle mouse rotation
		if (m_mbuttonCur == MBUTTON_Left)
		{
			m_yaw -= m_rotateSpeed * mouseMove.x;
			m_yaw = modPositive(m_yaw, 2.0f*pi);
			
			m_pitch -= m_rotateSpeed * mouseMove.y;
			m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
		}

		// Retrieve controller state
		XINPUT_STATE controllerState = {};
		float2 controllerLeftStick(0.0f), controllerRightStick(0.0f);
		float controllerLeftTrigger = 0.0f, controllerRightTrigger = 0.0f;
		if (m_controllerPresent)
		{
			// Look out for disconnection
			if (XInputGetState(0, &controllerState) == ERROR_SUCCESS)
			{
				// Decode axes and apply dead zones

				controllerLeftTrigger = float(max(0, controllerState.Gamepad.bLeftTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);
				controllerRightTrigger = float(max(0, controllerState.Gamepad.bRightTrigger - XINPUT_GAMEPAD_TRIGGER_THRESHOLD)) / float(255 - XINPUT_GAMEPAD_TRIGGER_THRESHOLD);

				controllerLeftStick = float2(controllerState.Gamepad.sThumbLX, controllerState.Gamepad.sThumbLY);
				float lengthLeft = length(controllerLeftStick);
				if (lengthLeft > XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE)
					controllerLeftStick = (controllerLeftStick / lengthLeft) * (lengthLeft - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_LEFT_THUMB_DEADZONE);
				else
					controllerLeftStick = float2(0.0f);

				controllerRightStick = float2(controllerState.Gamepad.sThumbRX, controllerState.Gamepad.sThumbRY);
				float lengthRight = length(controllerRightStick);
				if (lengthRight > XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE)
					controllerRightStick = (controllerRightStick / lengthRight) * (lengthRight - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE) / float(32768 - XINPUT_GAMEPAD_RIGHT_THUMB_DEADZONE);
				else
					controllerRightStick = float2(0.0f);
			}
			else
			{
				m_controllerPresent = false;
			}
		}

		// Handle controller rotation
		if (m_controllerPresent)
		{
			m_yaw -= m_controllerRotateSpeed * controllerRightStick.x * abs(controllerRightStick.x) * timestep;
			m_yaw = modPositive(m_yaw, 2.0f*pi);

			m_pitch += m_controllerRotateSpeed * controllerRightStick.y * abs(controllerRightStick.y) * timestep;
			m_pitch = clamp(m_pitch, -0.5f*pi, 0.5f*pi);
		}

		UpdateOrientation();

		// Handle zoom
		if (m_mbuttonCur == MBUTTON_Right)
		{
			m_radius *= expf(mouseMove.y * m_zoomSpeed);
		}
		m_radius *= expf(-m_wheelDelta * m_zoomWheelSpeed);
		m_wheelDelta = 0;

		// Handle controller zoom
		if (m_controllerPresent && !(controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
		{
			m_radius *= expf(-controllerLeftStick.y * abs(controllerLeftStick.y) * m_controllerZoomSpeed * timestep);
		}

		// Handle motion of target point
		if (m_mbuttonCur == MBUTTON_Middle)
		{
			m_posTarget -= m_rotateSpeed * mouseMove.x * m_radius * m_viewToWorld[0].xyz;
			m_posTarget += m_rotateSpeed * mouseMove.y * m_radius * m_viewToWorld[1].xyz;
		}

		// Handle controller motion of target point
		if (m_controllerPresent && (controllerState.Gamepad.wButtons & XINPUT_GAMEPAD_RIGHT_SHOULDER))
		{
			float3 localVelocity(0.0f);
			localVelocity.x = controllerLeftStick.x * abs(controllerLeftStick.x);
			localVelocity.y = square(controllerRightTrigger) - square(controllerLeftTrigger);
			localVelocity.z = -controllerLeftStick.y * abs(controllerLeftStick.y);
			m_posTarget += xfmVector(localVelocity, m_viewToWorld) * (m_radius * m_controllerMoveSpeed * timestep);
		}

		// Calculate remaining matrices
		m_pos = m_posTarget + m_radius * m_viewToWorld[2].xyz;
		setTranslation(&m_viewToWorld, m_pos);
		UpdateWorldToClip();
	}
void intro_do(long t, long delta_time)
{
    char errorText[MAX_ERROR_LENGTH + 1];
    float ftime = 0.001f*(float)t;
    float fdelta_time = 0.001f * (float)(delta_time);
    GLuint textureID;

    glDisable(GL_BLEND);
    glDisable(GL_DEPTH_TEST);

    // Those are key-Press indicators. I only act on 0-to-1.
    for (int i = 0; i < maxNumParameters; i++)
    {
        float destination_value = params.getParam(i, defaultParameters[i]);
        interpolatedParameters[i] = expf(-2.5f*fdelta_time) * interpolatedParameters[i] +
            (1.0f - expf(-2.5f*fdelta_time)) * destination_value;
    }
    // Update key press events.
    for (int i = 0; i < NUM_KEYS; i++)
    {
        if (params.getParam(i, 0.0) > 0.5f) keyPressed[i]++;
        else keyPressed[i] = 0;
    }

    // BPM => spike calculation
    float BPS = BPM / 60.0f;
    float jumpsPerSecond = BPS / 1.0f; // Jump on every fourth beat.
    static float phase = 0.0f;
    float jumpTime = (ftime * jumpsPerSecond) + phase;
    jumpTime -= (float)floor(jumpTime);
    if (keyPressed[41] == 1)
    {
        phase -= jumpTime;
        jumpTime = 0.0f;
        if (phase < 0.0f) phase += 1.0;
    }
    jumpTime = jumpTime * jumpTime;
    // spike is between 0.0 and 1.0 depending on the position within whatever.
    float spike = 0.5f * cosf(jumpTime * 3.1415926f * 1.5f) + 0.5f;
    // blob is growing down from 1. after keypress
    static float lastFTime = 0.f;
    blob *= (float)exp(-(float)(ftime - lastFTime) * BLOB_FADE_SPEED);
    lastFTime = ftime;

    // Set the program uniforms
    GLuint programID;
    shaderManager.getProgramID(usedProgram[usedIndex], &programID, errorText);
    glUseProgram(programID);
#if 1
    GLuint loc = glGetUniformLocation(programID, "aspectRatio");
    glUniform1f(loc, aspectRatio);
    loc = glGetUniformLocation(programID, "time");
    glUniform1f(loc, (float)(t * 0.001f));
    // For now I am just sending the spike to the shader. I might need something better...
    loc = glGetUniformLocation(programID, "spike");
    glUniform1f(loc, spike);
    loc = glGetUniformLocation(programID, "blob");
    glUniform1f(loc, blob);
    loc = glGetUniformLocation(programID, "knob1");
    glUniform1f(loc, interpolatedParameters[14]);
    loc = glGetUniformLocation(programID, "knob2");
    glUniform1f(loc, interpolatedParameters[15]);
    loc = glGetUniformLocation(programID, "knob3");
    glUniform1f(loc, interpolatedParameters[16]);
    loc = glGetUniformLocation(programID, "knob4");
    glUniform1f(loc, interpolatedParameters[17]);
    loc = glGetUniformLocation(programID, "knob5");
    glUniform1f(loc, interpolatedParameters[18]);
    loc = glGetUniformLocation(programID, "knob6");
    glUniform1f(loc, interpolatedParameters[19]);
    loc = glGetUniformLocation(programID, "knob7");
    glUniform1f(loc, interpolatedParameters[20]);
    loc = glGetUniformLocation(programID, "knob8");
    glUniform1f(loc, interpolatedParameters[21]);
    loc = glGetUniformLocation(programID, "knob9");
    glUniform1f(loc, interpolatedParameters[22]);
    loc = glGetUniformLocation(programID, "slider1");
    glUniform1f(loc, interpolatedParameters[2]);
    loc = glGetUniformLocation(programID, "slider2");
    glUniform1f(loc, interpolatedParameters[3]);
    loc = glGetUniformLocation(programID, "slider3");
    glUniform1f(loc, interpolatedParameters[4]);
    loc = glGetUniformLocation(programID, "slider4");
    glUniform1f(loc, interpolatedParameters[5]);
    loc = glGetUniformLocation(programID, "slider5");
    glUniform1f(loc, interpolatedParameters[6]);
    loc = glGetUniformLocation(programID, "slider6");
    glUniform1f(loc, interpolatedParameters[8]);
    loc = glGetUniformLocation(programID, "slider7");
    glUniform1f(loc, interpolatedParameters[9]);
    loc = glGetUniformLocation(programID, "slider8");
    glUniform1f(loc, interpolatedParameters[12]);
    loc = glGetUniformLocation(programID, "slider9");
    glUniform1f(loc, interpolatedParameters[13]);
#endif

    // Set texture identifiers
    GLint texture_location;
    texture_location = glGetUniformLocation(programID, "Noise3DTexture");
    glUniform1i(texture_location, 0);
    texture_location = glGetUniformLocation(programID, "DepthSensorTexture");
    glUniform1i(texture_location, 1);
    texture_location = glGetUniformLocation(programID, "BGTexture");
    glUniform1i(texture_location, 2);

    // render to larger offscreen texture
    glActiveTexture(GL_TEXTURE2);
    textureManager.getTextureID("hermaniak.tga", &textureID, errorText);
    glBindTexture(GL_TEXTURE_2D, textureID);
    glActiveTexture(GL_TEXTURE1);
    //textureManager.getTextureID(TM_DEPTH_SENSOR_NAME, &textureID, errorText);
    glBindTexture(GL_TEXTURE_2D, textureID);
    glActiveTexture(GL_TEXTURE0);
    textureManager.getTextureID(TM_NOISE3D_NAME, &textureID, errorText);
    glBindTexture(GL_TEXTURE_3D, textureID);

#if 1
    glViewport(0, 0, X_HIGHLIGHT, Y_HIGHLIGHT);
#endif

    // TODO: Here is the rendering done!
    interpolatedParameters[6] = 0.4f;

    shaderManager.getProgramID("SimpleTexture.gprg", &programID, errorText);
    glUseProgram(programID);
    glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
    //glDisable(GL_BLEND);
    glDisable(GL_CULL_FACE);
    glEnable(GL_BLEND);
    glBlendFunc(GL_DST_COLOR, GL_ZERO);

    // Texture for first pass is simply black
    textureManager.getTextureID("black.tga", &textureID, errorText);
    glActiveTexture(GL_TEXTURE0);
    glBindTexture(GL_TEXTURE_2D, textureID);

    const int num_passes = 4;

    // TODO: Make the first 1 or 2 passes to the smaller backbuffer
    for (int pass = 0; pass < num_passes; pass++) {
        // Set small
        
        // In the first pass, use highlight
        if (pass < num_passes - 3) {
            glViewport(0, 0, X_HIGHLIGHT, Y_HIGHLIGHT);
        } else if (pass < num_passes - 1) {
            glViewport(0, 0, X_OFFSCREEN, Y_OFFSCREEN);
        } else {
            // Set the whole screen as viewport so that it is used in the last pass
            int xres = windowRect.right - windowRect.left;
            int yres = windowRect.bottom - windowRect.top;
            glViewport(0, 0, xres, yres);
        }

        float red = 1.0f;
        float green = 1.0f;
        float blue = 1.0f;
        glClearColor(red, green, blue, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT);

        // Draw one iteration of the IFS
        float transformation[2][3];
        for (int i = 0; i < 20; i++) {
            transformation[0][0] = 0.5f * sinf(ftime * 0.133f + 0.3f + 1.3f * i) * sinf(ftime * 0.051f + 2.8f + 4.2f * i);
            transformation[0][1] = 0.5f * sinf(ftime * 0.051f + 2.8f + 4.2f * i) * sinf(ftime * 0.087f + 4.1f + 2.3f * i);
            transformation[0][2] = 0.6f * sinf(ftime * 0.087f + 4.1f + 2.3f * i) * sinf(ftime * 0.077f + 3.2f + 6.1f * i);
            transformation[1][0] = 0.5f * sinf(ftime * 0.077f + 3.2f + 6.1f * i) * sinf(ftime * 0.028f + 7.1f + 1.9f * i);
            transformation[1][1] = 0.5f * sinf(ftime * 0.028f + 7.1f + 1.9f * i) * sinf(ftime * 0.095f + 2.3f + 0.7f * i);
            transformation[1][2] = 0.6f * sinf(ftime * 0.095f + 2.3f + 0.7f * i) * sinf(ftime * 0.133f + 0.3f + 1.3f * i);
            DrawQuad(transformation, 1.0f);
        }

        // Copy backbuffer to texture
        if (pass < num_passes - 3) {
            textureManager.getTextureID(TM_HIGHLIGHT_NAME, &textureID, errorText);
            glBindTexture(GL_TEXTURE_2D, textureID);
            glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, X_HIGHLIGHT, Y_HIGHLIGHT);
            glActiveTexture(GL_TEXTURE0);
            glBindTexture(GL_TEXTURE_2D, textureID);
        } else if (pass < num_passes - 1) {
            textureManager.getTextureID(TM_OFFSCREEN_NAME, &textureID, errorText);
            glBindTexture(GL_TEXTURE_2D, textureID);
            glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, X_OFFSCREEN, Y_OFFSCREEN);
            glActiveTexture(GL_TEXTURE0);
            glBindTexture(GL_TEXTURE_2D, textureID);
        }
    }

#if 0
	// Copy backbuffer to front (so far no improvement)
	int xres = windowRect.right - windowRect.left;
	int yres = windowRect.bottom - windowRect.top;
	glViewport(0, 0, xres, yres);
    shaderManager.getProgramID("SimpleTexture.gprg", &programID, errorText);
	glUseProgram(programID);
    loc = glGetUniformLocation(programID, "time");
	glUniform1f(loc, (float)(t * 0.001f));
	glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
	glDisable(GL_BLEND);
	glBegin(GL_QUADS);
	glTexCoord2f(0.0f, 0.0f);
	glVertex2f(-1.0f, -1.0f);
	glTexCoord2f(1.0f, 0.0f);
	glVertex2f(1.0f, -1.0f);
	glTexCoord2f(1.0f, 1.0f);
	glVertex2f(1.0f, 1.0f);
	glTexCoord2f(0.0f, 1.0f);
	glVertex2f(-1.0f, 1.0f);
	glEnd();
#endif
}
Esempio n. 9
0
int main(int argc, const char * argv[])
{
    FILE * pFile;
    FILE * outputFile;
    bool debug = false;
    bool debugReg = false;
    bool timeLoops = false;
    
    //Defaults (for my mac)
    const char * rootDirectory = "/Users/AdamDossa/Documents/Columbia/GRA/Babel/";
    const char * currentDirectory = "/Users/AdamDossa/Documents/XCode/Babel_SGD/Babel_SGD/";
    float regParam = REG_PARAM;
    float initialLR = INITIAL_LR;
    
    //Get root directory from args or use default
    if (argc > 1)
    {
        rootDirectory = argv[1];
    }
    if (argc > 2)
    {
        currentDirectory = argv[2];
    }
    if (argc > 3)
    {
        regParam = (float) atof(argv[3]);
    }
    if (argc > 4)
    {
        initialLR = (float) atof(argv[4]);
    }
    
    //We want to find an unused log file name (to some limit)
    char logFileName[200];
    for (int i = 0; i < 100; i++)
    {
        snprintf(logFileName, sizeof(char) * 200,"%s/log-%d-%d-%f-%f.txt.%d", currentDirectory, NUM_EPOCHS, NUM_FILES, regParam, initialLR, i);
        outputFile = fopen(logFileName,"r");
        if (outputFile)
        {
            continue;
        } else {
            outputFile = fopen(logFileName,"w");
            break;
        }
    }
    fprintf(outputFile,"Log file: %s\n", logFileName);
    
    //First calculate the number of training examples (since not every file is guaranteed to have 250 rows)
    fprintf(outputFile, "Calculating number of training examples\n");
    int noOfTrainingExamples = 0;
    for (int i = 0; i < NUM_FILES; i++) {
        char fName[200];
        snprintf(fName, sizeof(char) * 200,"%s/labels/train.%d.lab", rootDirectory, i);
        pFile = fopen(fName,"rb");
        int n;
        fread(&n,4,1,pFile);
        n = ntohl(n);
        noOfTrainingExamples += n;
        //printf("File: %d No: %f\n",i,((float) noOfTrainingExamples) / 250.0f);
        fclose(pFile);
    }
    fprintf(outputFile, "No of training examples: %d\n", noOfTrainingExamples);
    
    //First read in the labels
    fprintf(outputFile, "Reading in labels\n");
    int readSoFar = 0;
    int * labels = (int *) malloc(sizeof(int) * noOfTrainingExamples);
    for (int i = 0; i < NUM_FILES; i++) {
        char fName[200];
        snprintf(fName, sizeof(char) * 200,"%s/labels/train.%d.lab", rootDirectory, i);
        pFile = fopen(fName,"rb");
        int n;
        fread(&n,4,1,pFile);
        n = ntohl(n);
        fread(&labels[readSoFar],4,n,pFile);
        for (int j = 0; j < n; j++)
        {
            labels[readSoFar + j] = ntohl(labels[readSoFar + j]);
        }
        readSoFar += n;
        fclose(pFile);
    }
    if (debug) {
        for (int i = 0; i < noOfTrainingExamples; i++)
        {
            fprintf(outputFile,"Train: Label number: %d has value: %d\n",i,labels[i]);
        }
    }
    fflush(outputFile);
    
    //Now read in the features - hold these in an array of arrays - assume same number as training examples
    fprintf(outputFile,"Reading in features\n");
    readSoFar = 0;
    float ** features = (float **) malloc(sizeof(float *) * noOfTrainingExamples);
    for (int i = 0; i < NUM_FILES; i++) {
        char fName[200];
        snprintf(fName, sizeof(char) * 200,"%s/features/train.%d.fea", rootDirectory, i);
        pFile = fopen(fName,"rb");
        int n;
        fread(&n,4,1,pFile);
        n = ntohl(n);
        for (int j = 0; j < n; j++)
        {
            int m;
            fread(&m,4,1,pFile);
            m = ntohl(m);
            features[readSoFar + j] = (float *) malloc(sizeof(float) * (m + 1));
            fread(features[readSoFar + j],sizeof(float),m,pFile);
            for (int k = 0; k < m; k++)
            {
                features[readSoFar + j][k] = bin2flt(&features[readSoFar + j][k]);
            }
            features[readSoFar + j][360] = 1.0f;
        }
        readSoFar += n;
        fclose(pFile);
    }
    if (debug) {
        for (int i = 0; i < noOfTrainingExamples; i++)
        {
            for (int j = 0; j<(FEATURE_COLS + 1); j++)
            {
                fprintf(outputFile,"Train: Feature coordinate: %d, %d has value: %f\n",i,j,features[i][j]);
            }
        }
    }
    fflush(outputFile);
    
    //Now do the same for the heldout data
    //First calculate the number of training examples (since not every file is guaranteed to have 250 rows)
    fprintf(outputFile, "Calculating number of heldout examples\n");
    int noOfHeldoutExamples = 0;
    for (int i = 0; i < NUM_HELDOUT_FILES; i++) {
        char fName[200];
        snprintf(fName, sizeof(char) * 200,"%s/labels/heldout.%d.lab", rootDirectory, i);
        pFile = fopen(fName,"rb");
        int n;
        fread(&n,4,1,pFile);
        n = ntohl(n);
        noOfHeldoutExamples += n;
        fclose(pFile);
    }
    fprintf(outputFile, "No of heldout examples: %d\n", noOfHeldoutExamples);
    
    //First read in the held out labels
    fprintf(outputFile,"Reading in heldout labels\n");
    readSoFar = 0;
    int * heldLabels = (int *) malloc(sizeof(int) * noOfHeldoutExamples);
    for (int i = 0; i<NUM_HELDOUT_FILES; i++) {
        char fName[200];
        snprintf(fName, sizeof(char) * 200,"%s/labels/heldout.%d.lab", rootDirectory, i);
        pFile = fopen(fName,"rb");
        int n;
        fread(&n,4,1,pFile);
        n = ntohl(n);
        fread(&heldLabels[readSoFar],4,n,pFile);
        for (int j = 0; j < n; j++)
        {
            heldLabels[readSoFar + j] = ntohl(heldLabels[readSoFar + j]);
        }
        readSoFar += n;
        fclose(pFile);
    }
    if (debug) {
        for (int i = 0; i < noOfHeldoutExamples; i++)
        {
            fprintf(outputFile,"Heldout: Label number: %d has value: %d\n",i,heldLabels[i]);
        }
    }
    fflush(outputFile);
    
    //Now read in the features - hold these in an array of arrays
    fprintf(outputFile,"Reading in heldout features\n");
    readSoFar = 0;
    float ** heldFeatures = (float **) malloc(sizeof(float *) * noOfHeldoutExamples);
    for (int i = 0; i < NUM_HELDOUT_FILES; i++) {
        char fName[200];
        snprintf(fName, sizeof(char) * 200,"%s/features/heldout.%d.fea", rootDirectory, i);
        pFile = fopen(fName,"rb");
        int n;
        fread(&n,4,1,pFile);
        n = ntohl(n);
        for (int j = 0; j < n; j++)
        {
            int m;
            fread(&m,4,1,pFile);
            m = ntohl(m);
            heldFeatures[readSoFar + j] = (float *) malloc(sizeof(float) * (m + 1));
            fread(heldFeatures[readSoFar + j],sizeof(float),m,pFile);
            for (int k = 0; k < m; k++)
            {
                heldFeatures[readSoFar + j][k] = bin2flt(&heldFeatures[readSoFar + j][k]);
            }
            heldFeatures[readSoFar + j][360] = 1.0f;
        }
        readSoFar += n;
        fclose(pFile);
    }
    if (debug) {
        for (int i = 0; i < noOfHeldoutExamples; i++)
        {
            for (int j = 0; j < (FEATURE_COLS + 1); j++)
            {
                fprintf(outputFile,"Heldout: Feature coordinate: %d, %d has value: %f\n",i,j,heldFeatures[i][j]);
            }
        }
    }
    fflush(outputFile);
    
    //Now run SGD
    fprintf(outputFile,"Running SGD\n");
    fflush(outputFile);
    
    //Initialize beta parameters, one for each class
    float ** beta = (float **) malloc(sizeof(float *) * NUM_CLASSES);
    for (int i = 0; i < NUM_CLASSES; i++)
    {
        beta[i] = (float *) malloc(sizeof(float) * (FEATURE_COLS + 1));
        for (int j = 0; j < (FEATURE_COLS + 1); j++)
        {
            beta[i][j] = 0.0f;
        }
    }
    
    //Create timing variables, used if timeLoops = true
    time_t t0, t1;
    clock_t c0, c1;
    if (timeLoops) {
        t0 = time(NULL);
        c0 = clock();
    }
    
    //Add new resume functionality
    //Check for any beta* files of appropriate name, and initialize using latest epoch
    int startingEpoch = 0;
    char lsName[200];
    snprintf(lsName, sizeof(char) * 200,"ls %s/beta-%s-%d-%f-%f.out | grep 'beta' | cut -d'-' -f2", currentDirectory, "*", NUM_FILES, regParam, initialLR);
    FILE *lsFile = popen(lsName, "r" );
    sleep(1);
    if (lsFile == 0 ) {
        fprintf(outputFile, "Could not execute ls command\n" );
        return 1;
    }
    const int BUFSIZE = 1000;
    char lsBuffer[BUFSIZE];
    while(fgets(lsBuffer, BUFSIZE,  lsFile))
    {
        fprintf(outputFile, "Found file for resume: %s", lsBuffer);
        startingEpoch = 1 + atoi(lsBuffer);
    }
    pclose(lsFile);
    
    //Now read in beta files as needed
    if (startingEpoch > 0)
    {
        //Initialize using beta file
        snprintf(lsName, sizeof(char) * 200,"%s/beta-%d-%d-%f-%f.out", currentDirectory, startingEpoch - 1, NUM_FILES, regParam, initialLR);
        fprintf(outputFile,"Initializing from file: %s\n", lsName);
        lsFile = fopen(lsName,"rb");
        for (int k = 0; k < NUM_CLASSES; k++)
        {
            fread(beta[k],sizeof(float),FEATURE_COLS + 1, lsFile);
        }

    }
    fflush(outputFile);
    
    //Start looping for NUM_EPOCHS
    for (int e = startingEpoch; e < NUM_EPOCHS; e++)
    {
        fprintf(outputFile,"SGD Epoch: %d\n", e);
        fflush(outputFile);
        //Calculate learning rate for epoch
        //Loop over examples
        float epochExamples = (e * noOfTrainingExamples);
        for (int j = 0; j < noOfTrainingExamples; j++)
        {
            float learningRate = initialLR/(1 + ((epochExamples + j)/(float) noOfTrainingExamples));
            if (debug)
            {
                fprintf(outputFile,"Train: Iterating on example: %d\n",j);
                fprintf(outputFile,"Learning Rate: %f\n", learningRate);
            }
            
            //Generate loop times (every 1000 loops)
            if (timeLoops) {
                float batchCounter = ((float) j) / 1000.0f;
                double integral;
                double fractional = modf(batchCounter, &integral);
                if (fractional == 0.0f) {
                    fprintf(outputFile,"Train: Loop: %d\n",j);
                    t1 = time(NULL);
                    c1 = clock();
                    fprintf(outputFile,"Train: Elapsed wall clock time: %ld\n", (long) (t1 - t0));
                    fprintf (outputFile, "Train: Elapsed CPU time: %f\n", (float) (c1 - c0)/CLOCKS_PER_SEC);
                    t0 = time(NULL);
                    c0 = clock();
                    fflush(outputFile);
                }
            }
            //Initialize z (the denominator of our probability function:
            //sum_i(exp(beta_i.x_i))
            float z = 0.0f;
            //We store each dot product of beta_i.x_i as we go
            float dotProductV[NUM_CLASSES];
            //In order to avoid overflows, we use the identity:
            //log(sum_i(exp(beta_i.x_i))) = m + log(sum_i(exp(beta_i.x_i - m)))
            //where m is the maxDotProduct
            float maxDotProduct;
            for (int k = 0; k < NUM_CLASSES; k++)
            {
                float dotProduct = 0.0f;
                for (int d = 0; d < (FEATURE_COLS + 1); d++)
                {
                    dotProduct += beta[k][d] * features[j][d];
                }
                dotProductV[k] = dotProduct;
                if (k==0) maxDotProduct = dotProduct;
                if (maxDotProduct < dotProduct) {
                    maxDotProduct = dotProduct;
                }
            }
            //Now calculate z as above
            for (int k = 0; k < NUM_CLASSES; k++)
            {
                z += expf(dotProductV[k] - maxDotProduct);
            }
            z = maxDotProduct + logf(z);
            //Update each beta
            for (int c = 0; c < NUM_CLASSES; c++)
            {
                //Calculate p(c_c|x_j, beta_c)
                float p_c_x_c = expf(dotProductV[c] - z);

                //Update rule depends on indicator function (Gaussian Prior)
                if (c == labels[j])
                {
                    for (int d = 0; d < (FEATURE_COLS + 1); d++)
                    {
                        beta[c][d] += learningRate*(features[j][d]*(1.0f - p_c_x_c) - (regParam * beta[c][d])/noOfTrainingExamples);
                    }
                } else {
                    for (int d = 0; d < (FEATURE_COLS + 1); d++)
                    {
                        beta[c][d] += learningRate*(features[j][d]*(0.0f - p_c_x_c) - (regParam * beta[c][d])/noOfTrainingExamples);
                    }                    
                }
            }
        }
        
        //Now do the log-likelihood calculation (on training data)
        fprintf(outputFile,"Train: Calculating Likelihood\n");
        fflush(outputFile);

        //Initialize logLikelihood to 0
        float logLikelihood = 0.0f;
        if (timeLoops) {
            t0 = time(NULL);
            c0 = clock();
        }
        
        //Loop over each example
        for (int j = 0; j < noOfTrainingExamples; j++)
        {
            if (debug) fprintf(outputFile,"Train Likelihood: Iterating on example: %d\n",j);
            if (timeLoops) {
                float batchCounter = ((float) j) / 1000.0f;
                double integral;
                double fractional = modf(batchCounter, &integral);
                if (fractional == 0.0f) {
                    fprintf(outputFile,"Train Likelihood Loop: %d\n",j);
                    t1 = time(NULL);
                    c1 = clock();
                    fprintf(outputFile,"Train Likelihood: Elapsed wall clock time: %ld\n", (long) (t1 - t0));
                    fprintf(outputFile, "Train Likelihood: Elapsed CPU time: %f\n", (float) (c1 - c0)/CLOCKS_PER_SEC);
                    t0 = time(NULL);
                    c0 = clock();
                    fflush(outputFile);
                }
            }
            float z = 0.0f;
            float dotProductV[NUM_CLASSES];
            float maxDotProduct;
            for (int k = 0; k < NUM_CLASSES; k++)
            {
                float dotProduct = 0.0f;
                for (int d = 0; d < (FEATURE_COLS + 1); d++)
                {
                    dotProduct += beta[k][d] * features[j][d];
                }
                dotProductV[k] = dotProduct;
                if (k==0) maxDotProduct = dotProduct;
                if (maxDotProduct < dotProduct) {
                    maxDotProduct = dotProduct;
                }
            }
            for (int k = 0; k < NUM_CLASSES; k++)
            {
                z += expf(dotProductV[k] - maxDotProduct);
            }
            float regTerm = 0.0f;
            for (int d = 0; d < (FEATURE_COLS + 1); d++)
            {
                regTerm += powf(beta[labels[j]][d],2);
            }
            logLikelihood += dotProductV[labels[j]] - (maxDotProduct + logf(z)) - ((0.5f * regParam * regTerm)/(float) noOfTrainingExamples);
        }
        fprintf(outputFile,"Train: LogLikelihood: %f\n", logLikelihood);
        
        //Now do the log-likelihood calculation on held out data
        fprintf(outputFile,"Heldout: Calculating Likelihood\n");
        fflush(outputFile);
        //Initialize logLikelihood to 0
        float heldLogLikelihood = 0.0f;
        if (timeLoops) {
            t0 = time(NULL);
            c0 = clock();
        }
        
        //Loop over each example
        for (int j = 0; j < noOfHeldoutExamples; j++)
        {
            if (debug) fprintf(outputFile,"Heldout Likelihood - Iterating on example: %d\n",j);
            if (timeLoops) {
                float batchCounter = ((float) j) / 1000.0f;
                double integral;
                double fractional = modf(batchCounter, &integral);
                if (fractional == 0.0f) {
                    fprintf(outputFile,"Heldout Likelihood Loop: %d\n",j);
                    t1 = time(NULL);
                    c1 = clock();
                    fprintf(outputFile,"Heldout Likelihood: Elapsed wall clock time: %ld\n", (long) (t1 - t0));
                    fprintf(outputFile, "Heldout Likelihood: Elapsed CPU time: %f\n", (float) (c1 - c0)/CLOCKS_PER_SEC);
                    t0 = time(NULL);
                    c0 = clock();
                    fflush(outputFile);
                }
            }
            float z = 0.0f;
            float dotProductV[NUM_CLASSES];
            float maxDotProduct;
            for (int k = 0; k < NUM_CLASSES; k++)
            {
                float dotProduct = 0.0f;
                for (int d = 0; d < (FEATURE_COLS + 1); d++)
                {
                    dotProduct += beta[k][d] * heldFeatures[j][d];
                }
                dotProductV[k] = dotProduct;
                if (k==0) maxDotProduct = dotProduct;
                if (maxDotProduct < dotProduct) {
                    maxDotProduct = dotProduct;
                }
            }
            for (int k = 0; k < NUM_CLASSES; k++)
            {
                z += expf(dotProductV[k] - maxDotProduct);
            }
            heldLogLikelihood += dotProductV[heldLabels[j]] - (maxDotProduct + logf(z));
        }
        fprintf(outputFile,"Heldout: LogLikelihood: %f\n", heldLogLikelihood);
       
        
        //Check Regularization
        if (debugReg)
        {
            float totalBeta = 0.0f;
            for (int k = 0; k < NUM_CLASSES; k++)
            {
                for (int j = 0; j < FEATURE_COLS + 1; j++)
                {
                    totalBeta += beta[k][j];
                }
            }
            fprintf(outputFile,"Total beta: %f", totalBeta);
        }
        
        //Now write to a file named for the epoch etc.
        char fName[200];
        snprintf(fName, sizeof(char) * 200,"%s/beta-%d-%d-%f-%f.out", currentDirectory, e, NUM_FILES, regParam, initialLR);
        pFile = fopen(fName,"wb");
        for (int k = 0; k < NUM_CLASSES; k++)
        {
            fwrite(beta[k],sizeof(float),FEATURE_COLS + 1,pFile);
        }
        fclose(pFile);
        fflush(outputFile);
    }
    
    free(labels);
    free(features);
    free(heldLabels);
    free(heldFeatures);
    return 0;
}
Esempio n. 10
0
void calc_grad_c(		int i1, 		// i index правой молекулы
					int j1,			// j index правой молекулы

					int i2,			// i index левой молекулы

					bit type, 		// dimer type: 0 - 'D', 1 - 'T'
					bit pos,		// monomer position in dimer: 0 - bottom, 1 - top

					float x_1,		// правая молекула		mol1
					float y_1,
					float teta_1,

					float x_2,		// левая молекула		mol2
					float y_2,
					float teta_2,

					float x_3,		// верхняя молекула 	mol3
					float y_3,
					float teta_3,

					float *grad_lat_x_1,			// left component of mol1
					float *grad_lat_y_1,
					float *grad_lat_teta_1,

					float *grad_lat_x_2,			// right component of mol2
					float *grad_lat_y_2,
					float *grad_lat_teta_2,

					float *grad_long_x_1,			// up component of mol1
					float *grad_long_y_1,
					float *grad_long_teta_1,

					float *grad_long_x_3,			// down component of mol3
					float *grad_long_y_3,
					float *grad_long_teta_3

							)


{

	// теперь PE_left - это индекс i2, а PF_right - индекс i1

	float cos_t_A = cosf(teta_2);
	float sin_t_A = sinf(teta_2);
	float cos_t_B = cosf(teta_1);
	float sin_t_B = sinf(teta_1);


	float cos_t_1 = cos_t_B;
	float sin_t_1 = sin_t_B;

	float cos_t_3 = cosf(teta_3);
	float sin_t_3 = sinf(teta_3);

	// swap i1 <=> i2

	float Ax_left = Ax_1[i2]*cos_t_A + Ax_3[i2]*sin_t_A - Ax_2[i2] +
				(x_2 + R_MT) * A_Bx_4[i2];

	float Ay_left = Ay_1[i2]*cos_t_A + Ay_2[i2]*sin_t_A + Ay_3[i2] +
				(x_2 + R_MT) * A_By_4[i2];

	float Az_left = -Az_1*sin_t_A + Az_2*cos_t_A + y_2;


	float Bx_right = Bx_1[i1]*cos_t_B + Bx_3[i1]*sin_t_B - Bx_2[i1] +
				(x_1 + R_MT) * A_Bx_4[i1];

	float By_right = By_1[i1]*cos_t_B + By_2[i1]*sin_t_B + By_3[i1] +
				(x_1 + R_MT) * A_By_4[i1];

	float Bz_right = -Bz_1*sin_t_B + Bz_2*cos_t_B + y_1;

	float Dx = Ax_left - Bx_right;
	float Dy = Ay_left - By_right;
	float Dz = Az_left - Bz_right;


	float dist = sqrtf(( pow(Dx, 2) + pow(Dy, 2) + pow(Dz, 2) ));


	if (dist <=1e-7 ){
		dist = 1e-5;
	}

	float inv_dist = 1/dist;

	float drdAx = Dx * inv_dist;
	float drdAy = Dy * inv_dist;
	float drdAz = Dz * inv_dist;
	float drdBx = -drdAx;
	float drdBy = -drdAy;
	float drdBz = -drdAz;

	float dA_X_dteta = -sin_t_A*Ax_1[i2] + cos_t_A*Ax_3[i2];
	float dA_Y_dteta = -sin_t_A*Ay_1[i2] + cos_t_A*Ay_2[i2];
	float dA_Z_dteta = -cos_t_A*Az_1 - sin_t_A*Az_2;

	float drdx_A = drdAx*A_Bx_4[i2] + drdAy*A_By_4[i2];
	float drdy_A = drdAz;
	float drdteta_A = drdAx*dA_X_dteta + drdAy*dA_Y_dteta + drdAz*dA_Z_dteta;

	//================================================
	float dB_X_dteta = -sin_t_B*Bx_1[i1] + cos_t_B*Bx_3[i1];
	float dB_Y_dteta = -sin_t_B*By_1[i1] + cos_t_B*By_2[i1];
	float dB_Z_dteta = -cos_t_B*Bz_1 - sin_t_B*Bz_2;

	float drdx_B = drdBx*A_Bx_4[i1] + drdBy*A_By_4[i1];
	float drdy_B = drdBz;
	float drdteta_B = drdBx*dB_X_dteta + drdBy*dB_Y_dteta + drdBz*dB_Z_dteta;


	float Grad_U_tmp = (b_lat* dist *expf(-dist*inv_ro0)*(2.0f - dist*inv_ro0) +
				dist* clat_dlat_ro0 * expf( - (dist*dist) * d_lat_ro0 )  ) * A_Koeff;



	if ((i1==12)&&(j1>=(N_d-3))) {

		*grad_lat_x_2 = 0.0f;
		*grad_lat_y_2 = 0.0f;
		*grad_lat_teta_2 = 0.0f;

		*grad_lat_x_1 = 0.0f;
		*grad_lat_y_1 = 0.0f;
		*grad_lat_teta_1 = 0.0f;

	} else {

		*grad_lat_x_2 = Grad_U_tmp * drdx_A;
		*grad_lat_y_2 = Grad_U_tmp * drdy_A;
		*grad_lat_teta_2 = Grad_U_tmp * drdteta_A;

		*grad_lat_x_1 = Grad_U_tmp * drdx_B;
		*grad_lat_y_1 = Grad_U_tmp * drdy_B;
		*grad_lat_teta_1 = Grad_U_tmp * drdteta_B;

	}



	//	[nd] 	- 	mol3
	//	[nd-1] 	- 	mol1


	// longitudinal gradient

	float r_long_x = (x_3 - x_1) - Rad*(sin_t_1 + sin_t_3);
	float r_long_y = (y_3 - y_1) - Rad*(cos_t_1 + cos_t_3);
	float r_long = sqrtf( r_long_x*r_long_x + r_long_y*r_long_y);

	if (r_long <=1e-15 ){
		r_long = 1e-7;
	}

	float drdx_long = - r_long_x/r_long;
	float drdy_long = - r_long_y/r_long;

	float dUdr_C;

	if (pos==0) {		// bottom monomer (interaction inside dimer)
		dUdr_C = C_Koeff*r_long;
	} else {			// top monomer (interaction with upper dimer)

		float tmp1 = r_long *  expf(-r_long*inv_ro0_long)*(2 - r_long*inv_ro0_long);
		float tmp2	= r_long * clong_dlong_ro0 * expf(-(r_long*r_long) * d_long_ro0 );

		if (type==0)	// dimer type 'D'
			dUdr_C = (tmp1*b_long_D + tmp2) * A_long_D;
		else 			// dimer type 'T'
			dUdr_C = (tmp1*b_long_T + tmp2) * A_long_T;
	}



	float Grad_tmp_x = drdx_long * dUdr_C;
	float Grad_tmp_y = drdy_long * dUdr_C;

	float GradU_C_teta_1 = -dUdr_C*( drdx_long*(-Rad*cos_t_1) + drdy_long*(Rad*sin_t_1));
	float GradU_C_teta_3 =  dUdr_C*(-drdx_long*(-Rad*cos_t_3) - drdy_long*(Rad*sin_t_3));

	float Grad_tmp;
	if (type==0)		// dimer type 'D'
		Grad_tmp = B_Koeff*(teta_3 - teta_1 - teta0_D);
	else				// dimer type 'T'
		Grad_tmp = B_Koeff*(teta_3 - teta_1 - teta0_T);

	// поменял тут знак - все заработало!
	float GradU_B_teta_1 = - Grad_tmp;
	float GradU_B_teta_3 = + Grad_tmp;


	if (j1 == (N_d-1)) {

		*grad_long_x_1 		= 0.0f;
		*grad_long_y_1 		= 0.0f;
		*grad_long_teta_1	= 0.0f;

		*grad_long_x_3 		= 0.0f;
		*grad_long_y_3 		= 0.0f;
		*grad_long_teta_3	= 0.0f;

	} else {

		*grad_long_x_1 		= Grad_tmp_x;
		*grad_long_y_1 		= Grad_tmp_y;
		*grad_long_teta_1	= GradU_C_teta_1 + GradU_B_teta_1;

		*grad_long_x_3 		= - Grad_tmp_x;
		*grad_long_y_3 		= - Grad_tmp_y;
		*grad_long_teta_3	= GradU_C_teta_3 + GradU_B_teta_3;


	}


}
Esempio n. 11
0
static void settingsUpdatedCb(UAVObjEvent * objEv) {
    AttitudeSettingsData attitudeSettings;
    AttitudeSettingsGet(&attitudeSettings);
    SensorSettingsGet(&sensorSettings);


    accelKp = attitudeSettings.AccelKp;
    accelKi = attitudeSettings.AccelKi;
    yawBiasRate = attitudeSettings.YawBiasRate;

    // Calculate accel filter alpha, in the same way as for gyro data in stabilization module.
    const float fakeDt = 0.0025f;
    if(attitudeSettings.AccelTau < 0.0001f) {
        accel_alpha = 0;   // not trusting this to resolve to 0
        accel_filter_enabled = false;
    } else {
        accel_alpha = expf(-fakeDt  / attitudeSettings.AccelTau);
        accel_filter_enabled = true;
    }

    zero_during_arming = attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE;
    bias_correct_gyro = attitudeSettings.BiasCorrectGyro == ATTITUDESETTINGS_BIASCORRECTGYRO_TRUE;

    gyro_correct_int[0] = 0;
    gyro_correct_int[1] = 0;
    gyro_correct_int[2] = 0;

    // Indicates not to expend cycles on rotation
    if(attitudeSettings.BoardRotation[0] == 0 && attitudeSettings.BoardRotation[1] == 0 &&
            attitudeSettings.BoardRotation[2] == 0) {
        rotate = 0;

        // Shouldn't be used but to be safe
        float rotationQuat[4] = {1,0,0,0};
        Quaternion2R(rotationQuat, Rsb);
    } else {
        float rotationQuat[4];
        const float rpy[3] = {attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL] / 100.0f,
                              attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH] / 100.0f,
                              attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] / 100.0f
                             };
        RPY2Quaternion(rpy, rotationQuat);
        Quaternion2R(rotationQuat, Rsb);
        rotate = 1;
    }

    if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_START) {
        trim_accels[0] = 0;
        trim_accels[1] = 0;
        trim_accels[2] = 0;
        trim_samples = 0;
        trim_requested = true;
    } else if (attitudeSettings.TrimFlight == ATTITUDESETTINGS_TRIMFLIGHT_LOAD) {
        trim_requested = false;

        // Get sensor data  mean
        float a_body[3] = { trim_accels[0] / trim_samples,
                            trim_accels[1] / trim_samples,
                            trim_accels[2] / trim_samples
                          };

        // Inverse rotation of sensor data, from body frame into sensor frame
        float a_sensor[3];
        rot_mult(Rsb, a_body, a_sensor, false);

        // Temporary variables
        float psi, theta, phi;

        psi = attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_YAW] * DEG2RAD / 100.0f;

        float cP = cosf(psi);
        float sP = sinf(psi);

        // In case psi is too small, we have to use a different equation to solve for theta
        if (fabsf(psi) > PI / 2)
            theta = atanf((a_sensor[1] + cP * (sP * a_sensor[0] -
                                               cP * a_sensor[1])) / (sP * a_sensor[2]));
        else
            theta = atanf((a_sensor[0] - sP * (sP * a_sensor[0] -
                                               cP * a_sensor[1])) / (cP * a_sensor[2]));

        phi = atan2f((sP * a_sensor[0] - cP * a_sensor[1]) / GRAVITY,
                     (a_sensor[2] / cosf(theta) / GRAVITY));

        attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_ROLL] = phi * RAD2DEG * 100.0f;
        attitudeSettings.BoardRotation[ATTITUDESETTINGS_BOARDROTATION_PITCH] = theta * RAD2DEG * 100.0f;

        attitudeSettings.TrimFlight = ATTITUDESETTINGS_TRIMFLIGHT_NORMAL;
        AttitudeSettingsSet(&attitudeSettings);

    }
}
Esempio n. 12
0
static float raise_fpe_underflow() { return expf( -88.8f ); }
Esempio n. 13
0
static float raise_fpe_overflow()  { return expf(  88.8f ); }
Esempio n. 14
0
static float RdIntegral(float alphap, float A) {
    float sqrtTerm = sqrtf(3.f * (1.f - alphap));
    return alphap / 2.f * (1.f + expf(-4.f/3.f * A * sqrtTerm)) *
        expf(-sqrtTerm);
}
Esempio n. 15
0
float gauss3d(float dx, float dy, float dz, float s) {
  return expf(-(dx*dx + dy*dy + dz*dz)/(s*s));
}
Esempio n. 16
0
float App::compute_gaussian(float n, float theta) // theta = Blur Amount
{
	return (float)((1.0f / sqrtf(2 * (float)clan::PI * theta)) * expf(-(n * n) / (2.0f * theta * theta)));
}
Esempio n. 17
0
int    GCI_Phasor(float xincr, float y[], int fit_start, int fit_end,
							  float *Z, float *U, float *V, float *taup, float *taum, float *tau, float *fitted, float *residuals,
							  float *chisq)
{
    // Z must contain a bg estimate
	// fitted and residuals must be arrays big enough to hold possibly fit_end floats.

	int   i, ret = PHASOR_ERR_NO_ERROR, nBins;
	float *data, u, v, A, w, I, Ifit, bg, chisq_local, res, sigma2;

	data = &(y[fit_start]);	
	nBins = (fit_end - fit_start);
	bg = *Z;
    if (!data)
        return (PHASOR_ERR_INVALID_DATA);
    if (nBins<0)
        return (PHASOR_ERR_INVALID_WINDOW);

	// rep frequency, lets use the period of the measurement, but we can stay in the units of bins
	w = 2.0f*3.1415926535897932384626433832795028841971f/(float)nBins; //2.0*PI/(float)nBins;
	setPhasorPeriod((float)nBins*xincr); // store the real phasor period used for future external use.

	// integral over data
	for (i=0, I=0.0f; i<nBins; i++) 
		I += (data[i]-bg);

	// Phasor coords
    // Take care that values correspond to the centre of the bin, hence i+0.5
	for (i=0, u=0.0f; i<nBins; i++) 
		u += (data[i]-bg) * cosf(w*((float)i+0.5f));
	u /= I;

	for (i=0, v=0.0f; i<nBins; i++) 
		v += (data[i]-bg) * sinf(w*((float)i+0.5f));
	v /= I;

	// taus, convert now to real time with xincr
	*taup = (xincr/w) * (v/u);
	*taum = (xincr/w) * sqrtf(1.0f/(u*u + v*v) - 1.0f);

	*tau = ((*taup) + (*taum))/2.0f;

	*U = u;
	*V = v;

	/* Now calculate the fitted curve and chi-squared if wanted. */
	if (fitted == NULL)
		return 0;

	memset(fitted, 0, (size_t)fit_end * sizeof(float));
	memset(residuals, 0, (size_t)fit_end * sizeof(float));
	// Madison report some "memory issue", and replaced the 2 line above with new arrays.
	// Not sure what that was but I breaks the filling of the fitted array, probably not allocating the arrays before calling

	// integral over nominal fit data
	for (Ifit=0.0f, i=fit_start; i<fit_end; i++) 
		Ifit += expf((float)(-(i-fit_start))*xincr/(*tau));
	// Estimate A
	A = I / Ifit;

	// Calculate fit
	for (i=fit_start; i<fit_end; i++){
		fitted[i] = bg + A * expf((float)(-(i-fit_start))*xincr/(*tau));
	}
	// OK, so now fitted contains our data for the timeslice of interest.
	// We can calculate a chisq value and plot the graph, along with
	// the residuals.

	if (residuals == NULL && chisq == NULL)
		return 0;

	chisq_local = 0.0f;
	for (i=0; i<fit_start; i++) {
		res = y[i]-fitted[i];
		if (residuals != NULL)
			residuals[i] = res;
	}


//	case NOISE_POISSON_FIT:
		/* Summation loop over all data */
		for (i=fit_start ; i<fit_end; i++) {
			res = y[i] - fitted[i];
			if (residuals != NULL)
				residuals[i] = res;
			/* don't let variance drop below 1 */
			sigma2 = (fitted[i] > 1 ? 1.0f/fitted[i] : 1.0f);
			chisq_local += res * res * sigma2;
		}

	if (chisq != NULL)
		*chisq = chisq_local;

	return (ret);
}
Esempio n. 18
0
	void MsrVAD::process(float* fft_ptr){
		const float a00 = 1.0f - m_VAD_A01;
		const float a11 = 1.0f - m_VAD_A10;
		const float a00f = 1.0f - m_VAD_A01f;
		const float a11f = 1.0f - m_VAD_A10f;
		// per bin and per frame soft VAD
		float likemean = 0.0f;
		float likelogmean = 0.0f;
		for (unsigned int u = m_MecBegBin; u <= m_MecEndBin; u++) { // optimized for non-zero frequency bins
			// calculate signal power
			float MicRe = fft_ptr[u];
			float MicIm = fft_ptr[TWO_FRAME_SIZE - u];
			SignalPower[u] = MicRe * MicRe + MicIm * MicIm;

			// update the prior and posterios SNRs
			posteriorSNR_NS[u] = SignalPower[u] / (NoiseModel[u] + DIV_BY_ZERO_PREVENTION);
			float MLPriorSNR = SpeechModel[u] / (NoiseModel[u] + DIV_BY_ZERO_PREVENTION) - 1.0f;
			if (MLPriorSNR < 0.0f) MLPriorSNR = 0.0f;
			priorSNR[u] = m_VAD_SNR_SMOOTHER * priorSNR[u] + (1.0f - m_VAD_SNR_SMOOTHER) * MLPriorSNR;

			// Speech presence likelihood ratio
			float vRatio = posteriorSNR_NS[u] * priorSNR[u] / (priorSNR[u] + 1.0f);
			if (vRatio > 700.0f) vRatio = 700.0f;
			float likelihoodratio = 1.0f / (priorSNR[u] + 1.0f) * expf(vRatio);
			if (likelihoodratio > 1000.0f) likelihoodratio = 1000.0f;
			else if (likelihoodratio < LOG_LIKELIHOOD_MINVAL) likelihoodratio = LOG_LIKELIHOOD_MINVAL;

			// Smooth speech presence probability per bin
			speechPresenceLR[u] = likelihoodratio * (m_VAD_A01 + a11 * speechPresenceLR[u]) / (a00 + m_VAD_A10 * speechPresenceLR[u]); // HMM for changing the state
			speechPresenceProb[u] = speechPresenceLR[u] / (1.0f + speechPresenceLR[u]);
			if (speechPresenceProb[u] < 0.0f) speechPresenceProb[u] = 0.0f;
			else if (speechPresenceProb[u] > 1.0f) speechPresenceProb[u] = 1.0f;

			// Note that likelogmean is only calculated on a subset of the frequency bins
			if (u >= m_LogLikelihoodBegBin && u <= m_LogLikelihoodEndBin) {
				likemean += likelihoodratio;
				likelogmean += logf(likelihoodratio);
			}
		}
		likemean /= (m_LogLikelihoodEndBin - m_LogLikelihoodBegBin + 1);
		likelogmean /= (m_LogLikelihoodEndBin - m_LogLikelihoodBegBin + 1);

		// Speech presence likelihood ratio for the frame
		float curFrameLR = expf(likelogmean);
		curFrameLR = 0.8f * curFrameLR + (1 - 0.8f) * likemean;
		if (curFrameLR > 1000.0f) curFrameLR = 1000.0f;

		// Smooth speech presence probability per frame
		FrameLR = curFrameLR * (m_VAD_A01f + a11f * FrameLR) / (a00f + m_VAD_A10f * FrameLR); // HMM for changing the state
		float framePresProb = FrameLR / (1.0f + FrameLR);
		if (framePresProb < 0.0f) framePresProb = 0.0f;
		else if (framePresProb > 1.0f) framePresProb = 1.0f;
		m_fFramePresProb = framePresProb;

		// precise noise model
		if (nFrame > 1) {
			if (nFrame < m_VAD_TAUN / m_MecFrameDuration) {
				for (unsigned int u = m_MecBegBin; u <= m_MecEndBin; u++) {
					float alphaN = 1.0f / nFrame;
					NoiseModel[u] = (1.0f - alphaN) * NoiseModel[u] + alphaN * SignalPower[u];
				}
			}
			else {
				for (unsigned int u = m_MecBegBin; u <= m_MecEndBin; u++) {
					float alphaN = (1.0f - speechPresenceProb[u]) * (1.0f - framePresProb) * m_MecFrameDuration / m_VAD_TAUN;
					NoiseModel[u] = (1.0f - alphaN) * NoiseModel[u] + alphaN * SignalPower[u];
				}
			}

			// update the speech model
			for (unsigned int u = m_MecBegBin; u <= m_MecEndBin; u++) {
				float alphaS = speechPresenceProb[u] * framePresProb * m_MecFrameDuration / m_VAD_TAUS;
				SpeechModel[u] = (1.0f - alphaS) * SpeechModel[u] + alphaS * SignalPower[u];
			}
		}
		nFrame++;

		// Update the prior SNR
		m_fEnergy = 0.0f;
		float fSNRam = 0.0f;
		float fSNR = 0.0f;
		for (unsigned int u = m_MecBegBin; u <= m_MecEndBin; u++) {
			if (u >= m_LogLikelihoodBegBin && u <= m_LogLikelihoodEndBin) {
				fSNR = SignalPower[u] / (NoiseModel[u] + 1e-10f);
				fSNR = std::max(1.0f, std::min(20000.0f, fSNR));
				fSNRam += fSNR;
			}

			// Compute the suppression rule - simple Wiener
			float Gain = priorSNR[u] / (priorSNR[u] + 1.0f);
			// update the prior SNR
			MasterSignalPowerOverNoiseModel[u] = SignalPower[u] / (NoiseModel[u] + DIV_BY_ZERO_PREVENTION);
			float alphaS = speechPresenceProb[u] * framePresProb * m_MecFrameDuration / m_VAD_TAUS;
			priorSNR[u] = (1.0f - alphaS) * priorSNR[u] + alphaS *  Gain * Gain * MasterSignalPowerOverNoiseModel[u];
			m_fEnergy += SignalPower[u];
			q30MasterSpeechPresenceProb[u] = (int)((float)(1u << 30) * framePresProb * speechPresenceProb[u]);
		}
		m_fEnergy /= (m_MecEndBin - m_MecBegBin + 1);
		fSNRam /= (m_MecEndBin - m_MecBegBin + 1);
		float beta = m_MecFrameDuration*framePresProb / 10.0f;  // time constant in seconds
		m_fSNR = (1.0f - beta) * m_fSNR + beta * fSNRam;
	}
Esempio n. 19
0
File: math.c Progetto: DeforaOS/libc
float sinhf(float x)
{
	return (expf(x) - expf(-x)) / 2;
}
void AudioEffectDistortionInstance::process(const AudioFrame *p_src_frames, AudioFrame *p_dst_frames, int p_frame_count) {

	const float *src = (const float *)p_src_frames;
	float *dst = (float *)p_dst_frames;

	//float lpf_c=expf(-2.0*Math_PI*keep_hf_hz.get()/(mix_rate*(float)OVERSAMPLE));
	float lpf_c = expf(-2.0 * Math_PI * base->keep_hf_hz / (AudioServer::get_singleton()->get_mix_rate()));
	float lpf_ic = 1.0 - lpf_c;

	float drive_f = base->drive;
	float pregain_f = Math::db2linear(base->pre_gain);
	float postgain_f = Math::db2linear(base->post_gain);

	float atan_mult = pow(10, drive_f * drive_f * 3.0) - 1.0 + 0.001;
	float atan_div = 1.0 / (atanf(atan_mult) * (1.0 + drive_f * 8));

	float lofi_mult = powf(2.0, 2.0 + (1.0 - drive_f) * 14); //goes from 16 to 2 bits

	for (int i = 0; i < p_frame_count * 2; i++) {

		float out = undenormalise(src[i] * lpf_ic + lpf_c * h[i & 1]);
		h[i & 1] = out;
		float a = out;
		float ha = src[i] - out; //high freqs
		a *= pregain_f;

		switch (base->mode) {

			case AudioEffectDistortion::MODE_CLIP: {

				a = powf(a, 1.0001 - drive_f);
				if (a > 1.0)
					a = 1.0;
				else if (a < (-1.0))
					a = -1.0;

			} break;
			case AudioEffectDistortion::MODE_ATAN: {

				a = atanf(a * atan_mult) * atan_div;

			} break;
			case AudioEffectDistortion::MODE_LOFI: {

				a = floorf(a * lofi_mult + 0.5) / lofi_mult;

			} break;
			case AudioEffectDistortion::MODE_OVERDRIVE: {

				const double x = a * 0.686306;
				const double z = 1 + exp(sqrt(fabs(x)) * -0.75);
				a = (expf(x) - expf(-x * z)) / (expf(x) + expf(-x));
			} break;
			case AudioEffectDistortion::MODE_WAVESHAPE: {
				float x = a;
				float k = 2 * drive_f / (1.00001 - drive_f);

				a = (1.0 + k) * x / (1.0 + k * fabsf(x));

			} break;
		}

		dst[i] = a * postgain_f + ha;
	}
}
Esempio n. 21
0
File: math.c Progetto: DeforaOS/libc
float coshf(float x)
{
	return (expf(x) + expf(-x)) / 2;
}
Esempio n. 22
0
int get_transition(HMMER_PROFILE *hmm, const char* hmm_Path) 
{
  int i;

  char* BEGIN = "  COMPO   ";
  FilePointer = fopen(hmm_Path, "r");
  if(FilePointer == NULL) {
    printf("Fatal error: Cannot open or find <.hmm> file\n");
    getchar();
    return fileERROR;
  } else {
    //printf(".hmm file open well_trans\n");
  }

  /* 1. locate the line of 1st stage */
  char* locate = (char*)malloc(11 * sizeof(char));  //why 11? since we need use 'fgets' to search BEGIN
  do{
    fgets(locate, 11, FilePointer);
    if(strcmp(locate, BEGIN))
      nextLine(FilePointer, 1);
  }while(strcmp(locate, BEGIN) && !feof(FilePointer));

  nextLine(FilePointer, 2);                         //move to the 'transition' line...

  char* t_Temp = (char*)malloc(72 * sizeof(char));  //why 72? since we need use 'fgets' to get TRAN line
  char* p;

  /* 2. Process node 0 */
  fgets(t_Temp, 72, FilePointer);

  p = strtok(t_Temp, "  ");
  hmm->tran_32bits[0][MM] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");
  hmm->tran_32bits[0][MI] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");
  hmm->tran_32bits[0][MD] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");
  hmm->tran_32bits[0][IM] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");
  hmm->tran_32bits[0][II] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");
  hmm->tran_32bits[0][DM] = 1.0f;

  hmm->tran_32bits[0][DD] = 0;

  nextLine(FilePointer, 3);             //move to next 'transition' line... 

  /* 3. Process node 1 to M-1 */
  for(i = 1; i < hmm->M; i++) {         //不去用 i = 0 (B) 和 i = length + 1 (E), 其中 E 的空间应该是永远都用不上的 

    fgets(t_Temp, 72, FilePointer);     //需要测试一下:p 设置在rolling外是不是可以? 答案是肯定的,可以!!下一次用fgets时会覆盖掉原来的 72 个空间

    p = strtok(t_Temp, "  ");
    hmm->tran_32bits[i][MM] = expf(-1.0 * (float)atof(p));
    p = strtok(NULL, "  ");
    hmm->tran_32bits[i][MI] = expf(-1.0 * (float)atof(p));
    p = strtok(NULL, "  ");
    hmm->tran_32bits[i][MD] = expf(-1.0 * (float)atof(p));
    p = strtok(NULL, "  ");
    hmm->tran_32bits[i][IM] = expf(-1.0 * (float)atof(p));
    p = strtok(NULL, "  ");
    hmm->tran_32bits[i][II] = expf(-1.0 * (float)atof(p));
    p = strtok(NULL, "  ");
    hmm->tran_32bits[i][DM] = expf(-1.0 * (float)atof(p));
    p = strtok(NULL, "  ");
    hmm->tran_32bits[i][DD] = expf(-1.0 * (float)atof(p));

    nextLine(FilePointer, 3); //move to next 'transition' line...
  }

  /* 4. Process node M */
  fgets(t_Temp, 72, FilePointer);

  p = strtok(t_Temp, "  ");
  hmm->tran_32bits[hmm->M][MM] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");
  hmm->tran_32bits[hmm->M][MI] = expf(-1.0 * (float)atof(p));

  hmm->tran_32bits[hmm->M][MD] = expf(-1.0 * (float)atof(p));

  p = strtok(NULL, "        *  ");
  hmm->tran_32bits[hmm->M][IM] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");
  hmm->tran_32bits[hmm->M][II] = expf(-1.0 * (float)atof(p));
  p = strtok(NULL, "  ");

  hmm->tran_32bits[hmm->M][DM] = 1.0f;
    
  hmm->tran_32bits[hmm->M][DD] = 0;

  /* 3. free memory */
  fclose(FilePointer);
  free(locate);
  free(t_Temp);

  return fileOK;
}
Esempio n. 23
0
float gaussian2d(float A, float c, std::array<float,2> x_0, std::array<float,2> x) {
	return A * expf( (-1.f)*
		( (x[0]-x_0[0])*(x[0]-x_0[0]) + (x[1]-x_0[1])*(x[1]-x_0[1])   ) /  
			(2.f * c*c) );
}
Esempio n. 24
0
void problem_migration_forces(){
    if(mig_forces==1){
        int print = 1;
        struct particle com = particles[0]; // calculate migration forces with respect to center of mass;
        for(int i=1;i<N;i++){ // N = _N + 1 = total number of planets + star
                double t_migend = t_mig[i] + t_damp[i]; //when migration ends
                if (t > t_mig[i] && t < t_migend) { //ramp down the migration force (by increasing the migration timescale)
                    tau_a[i] *= expf(dt/expmigfac[i]);
                    tau_e[i] = tau_a[i]/K;
                } else if(t > t_migend){
                    tau_a[i]=0.;
                    tau_e[i]=0.;
                    double sum = 0;
                    for(int j=0;j<N;j++) sum += tau_a[j];
                    if(sum < 0.1){
                        mig_forces = 0; //turn migration loop off altogether, save calcs
                        if(p_suppress == 0 && print == 1) printf("\n\n **migration loop off at t=%f** \n\n",t);
                        print = 0;
                    }
                }
            
            if (tau_e[i]!=0||tau_a[i]!=0){
                struct particle* p = &(particles[i]);
                const double dvx = p->vx-com.vx;
                const double dvy = p->vy-com.vy;
                const double dvz = p->vz-com.vz;
                
                if (tau_a[i]!=0){ 	// Migration
                    double const term = 1/(2.*tau_a[i]);
                    p->ax -=  dvx*term;
                    p->ay -=  dvy*term;
                    p->az -=  dvz*term;
                }
                
                if (tau_e[i]!=0){ 	// Eccentricity damping
                    const double mu = G*(com.m + p->m);
                    const double dx = p->x-com.x;
                    const double dy = p->y-com.y;
                    const double dz = p->z-com.z;
                    
                    const double hx = dy*dvz - dz*dvy;
                    const double hy = dz*dvx - dx*dvz;
                    const double hz = dx*dvy - dy*dvx;
                    const double h = sqrt ( hx*hx + hy*hy + hz*hz );
                    const double v = sqrt ( dvx*dvx + dvy*dvy + dvz*dvz );
                    const double r = sqrt ( dx*dx + dy*dy + dz*dz );
                    const double vr = (dx*dvx + dy*dvy + dz*dvz)/r;
                    const double ex = 1./mu*( (v*v-mu/r)*dx - r*vr*dvx );
                    const double ey = 1./mu*( (v*v-mu/r)*dy - r*vr*dvy );
                    const double ez = 1./mu*( (v*v-mu/r)*dz - r*vr*dvz );
                    const double e = sqrt( ex*ex + ey*ey + ez*ez );		// eccentricity
                    const double a = -mu/( v*v - 2.*mu/r );			// semi major axis, AU
                    
                    //TESTP5m*********************** to get them all starting off in line together
                    /**
                    //if(a < 0.091432 && t > 500 && i==2){
                    if(a < 0.078 && t > 500 && i==2){
                        mig_forces = 0;
                        if(p_suppress == 0) printf("\n\n **migration loop off (abrupt) at t=%f** \n\n",t);
                    }
                    **/
                    const double prefac1 = 1./(1.-e*e) /tau_e[i]/1.5;
                    const double prefac2 = 1./(r*h) * sqrt(mu/a/(1.-e*e))  /tau_e[i]/1.5;
                    p->ax += -dvx*prefac1 + (hy*dz-hz*dy)*prefac2;
                    p->ay += -dvy*prefac1 + (hz*dx-hx*dz)*prefac2;
                    p->az += -dvz*prefac1 + (hx*dy-hy*dx)*prefac2;
                }
            }
            com = tools_get_center_of_mass(com,particles[i]);
        }
    }
    
    //This calculates the tau_e for tides as forces, now that we have the resonance eccentricities
    if(tautide_force_calc == 0 && mig_forces == 0. && tide_force == 1 && tides_on == 1){
        tautide_force_calc = 1; //only do this process once
        struct particle com = particles[0];
        for(int i=1;i<N;i++){
            struct particle* par = &(particles[i]);
            const double m = par->m;
            const double mu = G*(com.m + m);
            const double rp = par->r*0.00464913;       //Rp from Solar Radii to AU
            const double Qp = par->k2/(par->Q);
            
            const double dvx = par->vx-com.vx;
            const double dvy = par->vy-com.vy;
            const double dvz = par->vz-com.vz;
            const double dx = par->x-com.x;
            const double dy = par->y-com.y;
            const double dz = par->z-com.z;
            
            const double v = sqrt ( dvx*dvx + dvy*dvy + dvz*dvz );
            const double r = sqrt ( dx*dx + dy*dy + dz*dz );
            const double vr = (dx*dvx + dy*dvy + dz*dvz)/r;
            const double ex = 1./mu*( (v*v-mu/r)*dx - r*vr*dvx );
            const double ey = 1./mu*( (v*v-mu/r)*dy - r*vr*dvy );
            const double ez = 1./mu*( (v*v-mu/r)*dz - r*vr*dvz );
            const double e = sqrt( ex*ex + ey*ey + ez*ez );   // eccentricity
            const double a = -mu/( v*v - 2.*mu/r );			// semi major axis, AU
            double a5r5 = pow(a/rp, 5);
            
            tidetauinv_e[i] = 1.0/(2./(9*M_PI)*(1./Qp)*sqrt(a*a*a/com.m/com.m/com.m)*a5r5*m);
            //tidetau_a[i] = tidetau_e[i]*K; //Dan uses a K factor instead.
            
            if(p_suppress == 0) printf("planet %i: tau_e,=%.1f Myr, a=%f,e=%f \n",i,1.0/(tidetauinv_e[i]*1e6),a,e);
        }
    }
    
    if(tides_on == 1 && tide_force == 1 && t > tide_delay){
        struct particle com = particles[0]; // calculate add. forces w.r.t. center of mass
        for(int i=1;i<N;i++){
            struct particle* p = &(particles[i]);
            const double dvx = p->vx - com.vx;
            const double dvy = p->vy - com.vy;
            const double dvz = p->vz - com.vz;
            
          //if(i==1){
                /*Papaloizou & Larwood (2000) - Unlike the orbital elements version of tides,
                 tidetau_e already induces an a' (Gold&Schlich2015), and so the tidetau_a is
                 an additional migration on top... don't think I need it. */
                /*
                if (tidetau_a[i] != 0.){
                    p->ax +=  -dvx/(2.*tidetau_a[i]);
                    p->ay +=  -dvy/(2.*tidetau_a[i]);
                    p->az +=  -dvz/(2.*tidetau_a[i]);
                }
                 */
                 
                //Papaloizou & Larwood (2000)
                if (tidetauinv_e[i] != 0. ){ 	// need h and e vectors for both types
                    const double dx = p->x-com.x;
                    const double dy = p->y-com.y;
                    const double dz = p->z-com.z;
                
                    const double rinv = 1/sqrt ( dx*dx + dy*dy + dz*dz );
                    const double vr = (dx*dvx + dy*dvy + dz*dvz)*rinv;
                    const double term = -2.*tidetauinv_e[i]*vr*rinv;
                    
                    p->ax += term*dx;
                    p->ay += term*dy;
                    p->az += term*dz;

                }
                com = tools_get_center_of_mass(com,particles[i]);
            //}
        }
        
        //print message
        if(tide_print == 0 && p_suppress == 0){
            printf("\n\n ***Tides (forces!) have just been turned on at t=%f years***\n\n",t);
            tide_print = 1;
        }
    }
}
Esempio n. 25
0
void init(char *filename) {
    GLfloat cloud_color[4] = { 1., 1., 1., 0., };
    GLfloat fog_color[4], fog_density = 0.05, density, far_cull;
    unsigned *image;
    int width, height, components;
    if (filename) {
	image = read_texture(filename, &width, &height, &components);
	if (image == NULL) {
	    fprintf(stderr, "Error: Can't load image file \"%s\".\n",
		    filename);
	    exit(EXIT_FAILURE);
	} else {
	    printf("%d x %d image loaded\n", width, height);
	}
	if (components != 1) {
	    printf("must be a bw image\n");
	    exit(EXIT_FAILURE);
	}
    } else {
	int i, j;
	unsigned char *img;
	components = 4; width = height = 512;
	image = (unsigned *) malloc(width*height*sizeof(unsigned));
	img = (unsigned char *)image;
	for (j = 0; j < height; j++)
	    for (i = 0; i < width; i++) {
		int w2 = width/2, h2 = height/2;
		if (i & 32)
		    img[4*(i+j*width)+0] = 0xff;
		else
		    img[4*(i+j*width)+1] = 0xff;
		if (j&32)
		    img[4*(i+j*width)+2] = 0xff;
		if ((i-w2)*(i-w2) + (j-h2)*(j-h2) > 64*64 &&
		    (i-w2)*(i-w2) + (j-h2)*(j-h2) < 300*300) img[4*(i+j*width)+3] = 0xff;
	    }

    }
    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
    glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_BLEND);
    glTexEnvfv(GL_TEXTURE_ENV, GL_TEXTURE_ENV_COLOR, cloud_color);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
    glTexImage2D(GL_TEXTURE_2D, 0, components, width,
                 height, 0, GL_RGBA, GL_UNSIGNED_BYTE,
                 image);
    glEnable(GL_TEXTURE_2D);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(50.,1.,.1,far_cull = 10.);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glTranslatef(0.,0.,-5.5);

    density = 1.- expf(-5.5 * fog_density * fog_density *
			      far_cull * far_cull);

#define MAX(a,b) ((a) > (b) ? (a) : (b))
#define MIN(a,b) ((a) < (b) ? (a) : (b))
    density = MAX(MIN(density, 1.), 0.);

    fog_color[0] = .23 + density *.57;
    fog_color[1] = .35 + density *.45;
    fog_color[2] = .78 + density *.22;

    glClearColor(fog_color[0], fog_color[1], fog_color[2], 1.f);

    glFogi(GL_FOG_MODE, GL_EXP2);
    glFogf(GL_FOG_DENSITY, fog_density);
    glFogfv(GL_FOG_COLOR, fog_color);
    if (fog_density > 0)
	glEnable(GL_FOG);
}
Esempio n. 26
0
void waveShapeSmps(int n,
                   float *smps,
                   unsigned char type,
                   unsigned char drive)
{
    int   i;
    float ws = drive / 127.0f;
    float tmpv;

    switch(type) {
        case 1:
            ws = powf(10, ws * ws * 3.0f) - 1.0f + 0.001f; //Arctangent
            for(i = 0; i < n; ++i)
                smps[i] = atanf(smps[i] * ws) / atanf(ws);
            break;
        case 2:
            ws = ws * ws * 32.0f + 0.0001f; //Asymmetric
            if(ws < 1.0f)
                tmpv = sinf(ws) + 0.1f;
            else
                tmpv = 1.1f;
            for(i = 0; i < n; ++i)
                smps[i] = sinf(smps[i] * (0.1f + ws - ws * smps[i])) / tmpv;
            ;
            break;
        case 3:
            ws = ws * ws * ws * 20.0f + 0.0001f; //Pow
            for(i = 0; i < n; ++i) {
                smps[i] *= ws;
                if(fabs(smps[i]) < 1.0f) {
                    smps[i] = (smps[i] - powf(smps[i], 3.0f)) * 3.0f;
                    if(ws < 1.0f)
                        smps[i] /= ws;
                }
                else
                    smps[i] = 0.0f;
            }
            break;
        case 4:
            ws = ws * ws * ws * 32.0f + 0.0001f; //Sine
            if(ws < 1.57f)
                tmpv = sinf(ws);
            else
                tmpv = 1.0f;
            for(i = 0; i < n; ++i)
                smps[i] = sinf(smps[i] * ws) / tmpv;
            break;
        case 5:
            ws = ws * ws + 0.000001f; //Quantisize
            for(i = 0; i < n; ++i)
                smps[i] = floor(smps[i] / ws + 0.5f) * ws;
            break;
        case 6:
            ws = ws * ws * ws * 32 + 0.0001f; //Zigzag
            if(ws < 1.0f)
                tmpv = sinf(ws);
            else
                tmpv = 1.0f;
            for(i = 0; i < n; ++i)
                smps[i] = asinf(sinf(smps[i] * ws)) / tmpv;
            break;
        case 7:
            ws = powf(2.0f, -ws * ws * 8.0f); //Limiter
            for(i = 0; i < n; ++i) {
                float tmp = smps[i];
                if(fabs(tmp) > ws) {
                    if(tmp >= 0.0f)
                        smps[i] = 1.0f;
                    else
                        smps[i] = -1.0f;
                }
                else
                    smps[i] /= ws;
            }
            break;
        case 8:
            ws = powf(2.0f, -ws * ws * 8.0f); //Upper Limiter
            for(i = 0; i < n; ++i) {
                float tmp = smps[i];
                if(tmp > ws)
                    smps[i] = ws;
                smps[i] *= 2.0f;
            }
            break;
        case 9:
            ws = powf(2.0f, -ws * ws * 8.0f); //Lower Limiter
            for(i = 0; i < n; ++i) {
                float tmp = smps[i];
                if(tmp < -ws)
                    smps[i] = -ws;
                smps[i] *= 2.0f;
            }
            break;
        case 10:
            ws = (powf(2.0f, ws * 6.0f) - 1.0f) / powf(2.0f, 6.0f); //Inverse Limiter
            for(i = 0; i < n; ++i) {
                float tmp = smps[i];
                if(fabs(tmp) > ws) {
                    if(tmp >= 0.0f)
                        smps[i] = tmp - ws;
                    else
                        smps[i] = tmp + ws;
                }
                else
                    smps[i] = 0;
            }
            break;
        case 11:
            ws = powf(5, ws * ws * 1.0f) - 1.0f; //Clip
            for(i = 0; i < n; ++i)
                smps[i] = smps[i]
                          * (ws + 0.5f) * 0.9999f - floor(
                    0.5f + smps[i] * (ws + 0.5f) * 0.9999f);
            break;
        case 12:
            ws = ws * ws * ws * 30 + 0.001f; //Asym2
            if(ws < 0.3f)
                tmpv = ws;
            else
                tmpv = 1.0f;
            for(i = 0; i < n; ++i) {
                float tmp = smps[i] * ws;
                if((tmp > -2.0f) && (tmp < 1.0f))
                    smps[i] = tmp * (1.0f - tmp) * (tmp + 2.0f) / tmpv;
                else
                    smps[i] = 0.0f;
            }
            break;
        case 13:
            ws = ws * ws * ws * 32.0f + 0.0001f; //Pow2
            if(ws < 1.0f)
                tmpv = ws * (1 + ws) / 2.0f;
            else
                tmpv = 1.0f;
            for(i = 0; i < n; ++i) {
                float tmp = smps[i] * ws;
                if((tmp > -1.0f) && (tmp < 1.618034f))
                    smps[i] = tmp * (1.0f - tmp) / tmpv;
                else
                if(tmp > 0.0f)
                    smps[i] = -1.0f;
                else
                    smps[i] = -2.0f;
            }
            break;
        case 14:
            ws = powf(ws, 5.0f) * 80.0f + 0.0001f; //sigmoid
            if(ws > 10.0f)
                tmpv = 0.5f;
            else
                tmpv = 0.5f - 1.0f / (expf(ws) + 1.0f);
            for(i = 0; i < n; ++i) {
                float tmp = smps[i] * ws;
                if(tmp < -10.0f)
                    tmp = -10.0f;
                else
                if(tmp > 10.0f)
                    tmp = 10.0f;
                tmp     = 0.5f - 1.0f / (expf(tmp) + 1.0f);
                smps[i] = tmp / tmpv;
            }
            break;
    }
}
Esempio n. 27
0
void ParticleSystem::update(float dt)
{
	float grav = gravity.getValue(manim, mtime);
	float deaccel = deacceleration.getValue(manim, mtime);

	// spawn new particles
	if (emitter) {
		float frate = rate.getValue(manim, mtime);
		float flife = 1.0f;
		flife = lifespan.getValue(manim, mtime);

		float ftospawn = (dt * frate / flife) + rem;
		if (ftospawn < 1.0f) {
			rem = ftospawn;
			if (rem<0)
				rem = 0;
		}
		else {
			int tospawn = (int)ftospawn;

			if ((tospawn + particles.size()) > MAX_PARTICLES) // Error check to prevent the program from trying to load insane amounts of particles.
				tospawn = particles.size() - MAX_PARTICLES;

			rem = ftospawn - static_cast<float>(tospawn);


			float w = areal.getValue(manim, mtime) * 0.5f;
			float l = areaw.getValue(manim, mtime) * 0.5f;
			float spd = speed.getValue(manim, mtime);
			float var = variation.getValue(manim, mtime);
			float spr = spread.getValue(manim, mtime);
			float spr2 = lat.getValue(manim, mtime);
			bool en = true;
			if (enabled.uses(manim))
				en = enabled.getValue(manim, mtime) != 0;

			//rem = 0;
			if (en) {
				for (int i = 0; i<tospawn; ++i) {
					Particle p = emitter->newParticle(manim, mtime, w, l, spd, var, spr, spr2);
					// sanity check:
					//if (particles.size() < MAX_PARTICLES) // No need to check this every loop iteration. Already checked above.
					particles.push_back(p);
				}
			}
		}
	}

	float mspeed = 1.0f;

	for (ParticleList::iterator it = particles.begin(); it != particles.end();) {
		Particle &p = *it;
		p.speed += p.down * grav * dt - p.dir * deaccel * dt;

		if (slowdown>0) {
			mspeed = expf(-1.0f * slowdown * p.life);
		}
		p.pos += p.speed * mspeed * dt;

		p.life += dt;
		float rlife = p.life / p.maxlife;
		// calculate size and color based on lifetime
		p.size = lifeRamp<float>(rlife, mid, sizes[0], sizes[1], sizes[2]);
		p.color = lifeRamp<Vec4D>(rlife, mid, colors[0], colors[1], colors[2]);

		// kill off old particles
		if (rlife >= 1.0f)
			particles.erase(it);

		++it;
	}
}
Esempio n. 28
0
__global__ void FloatMathPrecise() {
    int iX;
    float fX, fY;

    acosf(1.0f);
    acoshf(1.0f);
    asinf(0.0f);
    asinhf(0.0f);
    atan2f(0.0f, 1.0f);
    atanf(0.0f);
    atanhf(0.0f);
    cbrtf(0.0f);
    fX = ceilf(0.0f);
    fX = copysignf(1.0f, -2.0f);
    cosf(0.0f);
    coshf(0.0f);
    cospif(0.0f);
    cyl_bessel_i0f(0.0f);
    cyl_bessel_i1f(0.0f);
    erfcf(0.0f);
    erfcinvf(2.0f);
    erfcxf(0.0f);
    erff(0.0f);
    erfinvf(1.0f);
    exp10f(0.0f);
    exp2f(0.0f);
    expf(0.0f);
    expm1f(0.0f);
    fX = fabsf(1.0f);
    fdimf(1.0f, 0.0f);
    fdividef(0.0f, 1.0f);
    fX = floorf(0.0f);
    fmaf(1.0f, 2.0f, 3.0f);
    fX = fmaxf(0.0f, 0.0f);
    fX = fminf(0.0f, 0.0f);
    fmodf(0.0f, 1.0f);
    frexpf(0.0f, &iX);
    hypotf(1.0f, 0.0f);
    ilogbf(1.0f);
    isfinite(0.0f);
    fX = isinf(0.0f);
    fX = isnan(0.0f);
    j0f(0.0f);
    j1f(0.0f);
    jnf(-1.0f, 1.0f);
    ldexpf(0.0f, 0);
    lgammaf(1.0f);
    llrintf(0.0f);
    llroundf(0.0f);
    log10f(1.0f);
    log1pf(-1.0f);
    log2f(1.0f);
    logbf(1.0f);
    logf(1.0f);
    lrintf(0.0f);
    lroundf(0.0f);
    modff(0.0f, &fX);
    fX = nanf("1");
    fX = nearbyintf(0.0f);
    nextafterf(0.0f, 0.0f);
    norm3df(1.0f, 0.0f, 0.0f);
    norm4df(1.0f, 0.0f, 0.0f, 0.0f);
    normcdff(0.0f);
    normcdfinvf(1.0f);
    fX = 1.0f;
    normf(1, &fX);
    powf(1.0f, 0.0f);
    rcbrtf(1.0f);
    remainderf(2.0f, 1.0f);
    remquof(1.0f, 2.0f, &iX);
    rhypotf(0.0f, 1.0f);
    fY = rintf(1.0f);
    rnorm3df(0.0f, 0.0f, 1.0f);
    rnorm4df(0.0f, 0.0f, 0.0f, 1.0f);
    fX = 1.0f;
    rnormf(1, &fX);
    fY = roundf(0.0f);
    rsqrtf(1.0f);
    scalblnf(0.0f, 1);
    scalbnf(0.0f, 1);
    signbit(1.0f);
    sincosf(0.0f, &fX, &fY);
    sincospif(0.0f, &fX, &fY);
    sinf(0.0f);
    sinhf(0.0f);
    sinpif(0.0f);
    sqrtf(0.0f);
    tanf(0.0f);
    tanhf(0.0f);
    tgammaf(2.0f);
    fY = truncf(0.0f);
    y0f(1.0f);
    y1f(1.0f);
    ynf(1, 1.0f);
}
Esempio n. 29
0
// check for an event, and queue it if found.  Return TRUE if an event
// was generated.
int Spaceball::check_event(void) {
  int tx, ty, tz, rx, ry, rz, buttons;
  int buttonchanged;
  int win_event=FALSE;
  int direct_event=FALSE;
  // for use in UserKeyEvent() calls
  DisplayDevice::EventCodes keydev=DisplayDevice::WIN_KBD;

  // explicitly initialize event state variables
  rx=ry=rz=tx=ty=tz=buttons=0;

#if defined(VMDTDCONNEXION) && defined(__APPLE__)
  if (tdx_getstatus(tx, ty, tz, rx, ry, rz, buttons))
    win_event = TRUE;
#else
  if (app->display->spaceball(&rx, &ry, &rz, &tx, &ty, &tz, &buttons)) 
    win_event = TRUE;
#endif


#if defined(VMDLIBSBALL)
  // combine direct spaceball events together with window-system events
  if (sball != NULL) {
    int rx2, ry2, rz2, tx2, ty2, tz2, buttons2;
    if (sball_getstatus(sball, &tx2, &ty2, &tz2, &rx2, &ry2, &rz2, &buttons2)) {
      direct_event = TRUE;
      rx += rx2;
      ry += ry2;
      rz += rz2;
      tx += tx2; 
      ty += ty2; 
      tz += tz2; 
      buttons |= buttons2;
    }
  }
#endif

  if (!win_event && !direct_event)
    return FALSE; // no events to report

  // find which buttons changed state
  buttonchanged = buttons ^ buttonDown; 

  // if the user presses button 1, reset the view, a very very very
  // important feature to have implemented early on... ;-)
#if defined(VMDLIBSBALL)  && !defined(VMDSPACEWARE)
  if (((buttonchanged & SBALL_BUTTON_1) && (buttons & SBALL_BUTTON_1)) ||
      ((buttonchanged & SBALL_BUTTON_LEFT) && (buttons & SBALL_BUTTON_LEFT))){
#else 
// #elif!defined(VMDLIBSBALL) && defined(VMDSPACEWARE)
  if ((buttonchanged & 2) && (buttons & 2)) {
#endif

    app->scene_resetview();
    msgInfo << "Spaceball reset view orientation" << sendmsg;
  }

  // Toggle between the different modes
#if   defined(VMDLIBSBALL)  &&  !defined(VMDSPACEWARE)
  if (((buttonchanged & SBALL_BUTTON_2) && (buttons & SBALL_BUTTON_2)) ||
      ((buttonchanged & SBALL_BUTTON_RIGHT) && (buttons & SBALL_BUTTON_RIGHT))) {
#else
//#elif !defined(VMDLIBSBALL) &&  defined(VMDSPACEWARE)
  if ((buttonchanged & 4) && (buttons & 4)) {
#endif

    switch (moveMode) {
      case NORMAL:
        move_mode(MAXAXIS);
        msgInfo << "Spaceball set to dominant axis rotation/translation mode" << sendmsg;
        break;   

      case MAXAXIS:
        move_mode(SCALING);
        msgInfo << "Spaceball set to scaling mode" << sendmsg;
        break;   

      case SCALING:
        move_mode(ANIMATE);
        msgInfo << "Spaceball set to animate mode" << sendmsg;
        break;   

      case ANIMATE:
        move_mode(TRACKER);
        msgInfo << "Spaceball set to tracker mode" << sendmsg;
        break;   

      case TRACKER:
        move_mode(USER);
        msgInfo << "Spaceball set to user mode" << sendmsg;
        break;   

      default: 
        move_mode(NORMAL);
        msgInfo << "Spaceball set to rotation/translation mode" << sendmsg;
        break;
    }
  }

  // if the user presses button 3 through N, run a User command
#if defined(VMDLIBSBALL)  &&  !defined(VMDSPACEWARE)
  if ((buttonchanged & SBALL_BUTTON_3) && (buttons & SBALL_BUTTON_3)) {
    runcommand(new UserKeyEvent(keydev, '3', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & SBALL_BUTTON_4) && (buttons & SBALL_BUTTON_4)) {
    runcommand(new UserKeyEvent(keydev, '4', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & SBALL_BUTTON_5) && (buttons & SBALL_BUTTON_5)) {
    runcommand(new UserKeyEvent(keydev, '5', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & SBALL_BUTTON_6) && (buttons & SBALL_BUTTON_6)) {
    runcommand(new UserKeyEvent(keydev, '6', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & SBALL_BUTTON_7) && (buttons & SBALL_BUTTON_7)) {
    runcommand(new UserKeyEvent(keydev, '7', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & SBALL_BUTTON_8) && (buttons & SBALL_BUTTON_8)) {
    runcommand(new UserKeyEvent(keydev, '8', (int) DisplayDevice::AUX));
  }
//#elif !defined(VMDLIBSBALL) &&  defined(VMDSPACEWARE)
#else
  if ((buttonchanged & 8) && (buttons & 8)) {
    runcommand(new UserKeyEvent(keydev, '3', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & 16) && (buttons & 16)) {
    runcommand(new UserKeyEvent(keydev, '4', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & 32) && (buttons & 32)) {
    runcommand(new UserKeyEvent(keydev, '5', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & 64) && (buttons & 64)) {
    runcommand(new UserKeyEvent(keydev, '6', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & 128) && (buttons & 128)) {
    runcommand(new UserKeyEvent(keydev, '7', (int) DisplayDevice::AUX));
  }
  if ((buttonchanged & 256) && (buttons & 256)) {
    runcommand(new UserKeyEvent(keydev, '8', (int) DisplayDevice::AUX));
  }
#endif

  // get absolute values of axis forces for use in 
  // null region processing and min/max comparison tests
  int atx, aty, atz, arx, ary, arz;
  atx = abs(tx);
  aty = abs(ty);
  atz = abs(tz);
  arx = abs(rx);
  ary = abs(ry);
  arz = abs(rz);


  // perform null region processing
  if (atx > null_region) {
    tx = ((tx > 0) ? (tx - null_region) : (tx + null_region));
  } else {
    tx = 0;
  }
  if (aty > null_region) {
    ty = ((ty > 0) ? (ty - null_region) : (ty + null_region));
  } else {
    ty = 0;
  }
  if (atz > null_region) {
    tz = ((tz > 0) ? (tz - null_region) : (tz + null_region));
  } else {
    tz = 0;
  }
  if (arx > null_region) {
    rx = ((rx > 0) ? (rx - null_region) : (rx + null_region));
  } else {
    rx = 0;
  }
  if (ary > null_region) {
    ry = ((ry > 0) ? (ry - null_region) : (ry + null_region));
  } else {
    ry = 0;
  }
  if (arz > null_region) {
    rz = ((rz > 0) ? (rz - null_region) : (rz + null_region));
  } else {
    rz = 0;
  }


  // Ignore null motion events since some versions of the Windows 
  // Spaceball driver emit a constant stream of null motion event
  // packets which would otherwise cause continuous redraws, pegging the 
  // CPU and GPU at maximum load.
  if ((arx+ary+arz+atx+aty+atz) > 0) {
    float ftx = tx * sensitivity;
    float fty = ty * sensitivity;
    float ftz = tz * sensitivity;
    float frx = rx * sensitivity;
    float fry = ry * sensitivity;
    float frz = rz * sensitivity;
    char rmaxaxis = 'x';
    float rmaxval = 0.0f;
    float tmaxval = 0.0f;
    float tmaxvec[3] = { 0.0f, 0.0f, 0.0f };
    tmaxvec[0] = tmaxvec[1] = tmaxvec[2] = 0.0f;

    switch(moveMode) {
      case NORMAL:
        // Z-axis rotation/trans have to be negated in order to please VMD...
        app->scene_rotate_by(frx * rotInc, 'x');
        app->scene_rotate_by(fry * rotInc, 'y');
        app->scene_rotate_by(-frz * rotInc, 'z');
        if (app->display_projection_is_perspective()) {
          app->scene_translate_by(ftx * transInc, fty * transInc, -ftz * transInc);
        } else {
          app->scene_scale_by((1.0f + scaleInc * -ftz > 0.0f) ? 
                               1.0f + scaleInc * -ftz : 0.0f);
          app->scene_translate_by(ftx * transInc, fty * transInc, 0);
        }
 
        break;

      case MAXAXIS:
        // Z-axis rotation/trans have to be negated in order to please VMD...
        // find dominant rotation axis
        if (arx > ary) {
          if (arx > arz) {
            rmaxaxis = 'x';
            rmaxval = frx; 
          } else {
            rmaxaxis = 'z';
            rmaxval = -frz; 
          }
        } else {     
          if (ary > arz) {
            rmaxaxis = 'y';
            rmaxval = fry; 
          } else {
            rmaxaxis = 'z';
            rmaxval = -frz; 
          }
        }

        // find dominant translation axis
        if (atx > aty) {
          if (atx > atz) {
            tmaxval = ftx;
            tmaxvec[0] = ftx; 
          } else {
            tmaxval = ftz;
            tmaxvec[2] = ftz; 
          }
        } else {     
          if (aty > atz) {
            tmaxval = fty;
            tmaxvec[1] = fty; 
          } else {
            tmaxval = ftz;
            tmaxvec[2] = ftz; 
          }
       }

       // determine whether to rotate or translate
       if (fabs(rmaxval) > fabs(tmaxval)) {
         app->scene_rotate_by(rmaxval * rotInc, rmaxaxis);
       } else {
         app->scene_translate_by(tmaxvec[0] * transInc, 
                                 tmaxvec[1] * transInc, 
                                -tmaxvec[2] * transInc);
       }
       break;

      case SCALING:
        app->scene_scale_by((1.0f + scaleInc * ftz > 0.0f) ? 
                             1.0f + scaleInc * ftz : 0.0f);
        break;

      case ANIMATE:
        // if we got a non-zero input, update the VMD animation state
        if (abs(ry) > 0) {
#if 1
          // exponential input scaling
          float speed = fabsf(expf(fabsf((fabsf(fry) * animInc) / 1.7f))) - 1.0f;
#else
          // linear input scaling
          float speed = fabsf(fry) * animInc;
#endif

          if (speed > 0) {
            if (speed < 1.0) 
              app->animation_set_speed(speed);
            else
              app->animation_set_speed(1.0f);
 
            int stride = 1;
            if (fabs(speed - 1.0) > (double) maxstride)
              stride = maxstride;
            else
              stride = 1 + (int) fabs(speed-1.0);
            if (stride < 1)
              stride = 1; 
            app->animation_set_stride(stride);
 
            // -ry is turned to the right, like a typical shuttle/jog control
            if (fry < 0) 
              app->animation_set_dir(Animation::ANIM_FORWARD1);
            else
              app->animation_set_dir(Animation::ANIM_REVERSE1);
          } else {
            app->animation_set_dir(Animation::ANIM_PAUSE);
            app->animation_set_speed(1.0f);
          }
        } else {
          app->animation_set_dir(Animation::ANIM_PAUSE);
          app->animation_set_speed(1.0f);
        }
        break;

      case TRACKER:
        trtx = ftx;
        trty = fty;
        trtz = ftz;
        trrx = frx;
        trry = fry;
        trrz = frz;
        trbuttons = buttons;
        break;

      case USER:
        // inform TCL
        app->commandQueue->runcommand(new SpaceballEvent(ftx, fty, ftz, 
                                                         frx, fry, frz, 
                                                         buttons));
        break;
    }
  }

  // update button status for next time through
  buttonDown = buttons;

  return TRUE;
}


///////////// public routines for use by text commands etc

const char* Spaceball::get_mode_str(MoveMode mm) {
  const char* modestr;

  switch (mm) {
    default:
    case NORMAL:      modestr = "rotate";     break;
    case MAXAXIS:     modestr = "maxaxis";    break;
    case SCALING:     modestr = "scale";      break;
    case ANIMATE:     modestr = "animate";    break;
    case TRACKER:     modestr = "tracker";    break;
    case USER:        modestr = "user";       break;
  }

  return modestr;
}


void Spaceball::get_tracker_status(float &tx, float &ty, float &tz,
                                   float &rx, float &ry, float &rz, 
                                   int &buttons) {
  tx =  trtx * transInc;
  ty =  trty * transInc;
  tz = -trtz * transInc;
  rx =  trrx * rotInc;
  ry =  trry * rotInc;
  rz = -trrz * rotInc;
  buttons = trbuttons;
}


// set the Spaceball move mode to the given state; return success
int Spaceball::move_mode(MoveMode mm) {
  // change the mode now
  moveMode = mm;

  /// clear out any remaining tracker event data if we're not in that mode
  if (moveMode != TRACKER) {
    trtx=trty=trtz=trrx=trry=trrz=0.0f; 
    trbuttons=0;
  }

  return TRUE; // report success
}