/*
* 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;
}
static float filter_func_gaussian(float v, float width)
{
v *= 6.0f / width;
return expf(-2.0f * v * v);
}
/**
* 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);
}
}
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) );
}
static GMQCC_INLINE ast_expression *fold_intrin_exp(fold_t *fold, ast_value *a) {
return fold_constgen_float(fold, expf(fold_immvalue_float(a)));
}
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;
}
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
}
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;
}
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;
}
}
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);
}
}
static float raise_fpe_underflow() { return expf( -88.8f ); }
static float raise_fpe_overflow() { return expf( 88.8f ); }
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);
}
float gauss3d(float dx, float dy, float dz, float s) {
return expf(-(dx*dx + dy*dy + dz*dz)/(s*s));
}
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)));
}
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);
}
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;
}
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;
}
}
float coshf(float x)
{
return (expf(x) + expf(-x)) / 2;
}
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;
}
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) );
}
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;
}
}
}
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);
}
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;
}
}
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;
}
}
__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);
}
// 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
}