double IK::Update(double max_timestep, double min_timestep, double max_integ_error)
{
double ret;
double new_timestep = max_timestep;
for(int i=0; i<2; i++)
{
CalcPosition();
calc_jacobian();
calc_feedback();
ret = solve_ik();
IntegrateAdaptive(new_timestep, i, min_timestep, max_integ_error);
}
// cerr << new_timestep << endl;
CalcPosition();
return ret;
}
double IK::Update(double timestep)
{
double ret;
CalcPosition();
#ifdef MEASURE_TIME
clock_t t1 = clock();
#endif
calc_jacobian();
calc_feedback();
#ifdef MEASURE_TIME
clock_t t2 = clock();
calc_jacobian_time += (double)(t2 - t1) / (double)CLOCKS_PER_SEC;
#endif
ret = solve_ik();
#ifdef MEASURE_TIME
solve_ik_time += (double)(clock() - t2) / (double)CLOCKS_PER_SEC;
#endif
Integrate(timestep);
CalcPosition();
return ret;
}
static void runComb_c(LADSPA_Handle instance, unsigned long sample_count) {
Comb_c *plugin_data = (Comb_c *)instance;
/* Input (array of floats of length sample_count) */
const LADSPA_Data * const in = plugin_data->in;
/* Output (array of floats of length sample_count) */
LADSPA_Data * const out = plugin_data->out;
/* Max Delay (s) (float value) */
const LADSPA_Data max_delay = *(plugin_data->max_delay);
/* Delay Time (s) (float value) */
const LADSPA_Data delay_time = *(plugin_data->delay_time);
/* Decay Time (s) (float value) */
const LADSPA_Data decay_time = *(plugin_data->decay_time);
LADSPA_Data * buffer = plugin_data->buffer;
unsigned int buffer_mask = plugin_data->buffer_mask;
LADSPA_Data delay_samples = plugin_data->delay_samples;
LADSPA_Data feedback = plugin_data->feedback;
LADSPA_Data last_decay_time = plugin_data->last_decay_time;
LADSPA_Data last_delay_time = plugin_data->last_delay_time;
unsigned int sample_rate = plugin_data->sample_rate;
long write_phase = plugin_data->write_phase;
#line 79 "comb_1887.xml"
int i;
i = max_delay;
if (write_phase == 0) {
plugin_data->last_delay_time = delay_time;
plugin_data->last_decay_time = decay_time;
plugin_data->delay_samples = delay_samples = CALC_DELAY (delay_time);
plugin_data->feedback = feedback = calc_feedback (delay_time, decay_time);
}
if (delay_time == last_delay_time && decay_time == last_decay_time) {
long idelay_samples = (long)delay_samples;
LADSPA_Data frac = delay_samples - idelay_samples;
for (i=0; i<sample_count; i++) {
long read_phase = write_phase - (long)delay_samples;
LADSPA_Data read = cube_interp (frac,
buffer[(read_phase-1) & buffer_mask],
buffer[read_phase & buffer_mask],
buffer[(read_phase+1) & buffer_mask],
buffer[(read_phase+2) & buffer_mask]);
buffer[write_phase++ & buffer_mask] = read * feedback + in[i];
buffer_write(out[i], read);
}
} else {
float next_delay_samples = CALC_DELAY (delay_time);
float delay_samples_slope = (next_delay_samples - delay_samples) / sample_count;
float next_feedback = calc_feedback (delay_time, decay_time);
float feedback_slope = (next_feedback - feedback) / sample_count;
for (i=0; i<sample_count; i++) {
long read_phase, idelay_samples;
LADSPA_Data read, frac;
delay_samples += delay_samples_slope;
write_phase++;
read_phase = write_phase - (long)delay_samples;
idelay_samples = (long)delay_samples;
frac = delay_samples - idelay_samples;
read = cube_interp (frac,
buffer[(read_phase-1) & buffer_mask],
buffer[read_phase & buffer_mask],
buffer[(read_phase+1) & buffer_mask],
buffer[(read_phase+2) & buffer_mask]);
buffer[write_phase & buffer_mask] = read * feedback + in[i];
buffer_write(out[i], read);
feedback += feedback_slope;
}
plugin_data->last_delay_time = delay_time;
plugin_data->last_decay_time = decay_time;
plugin_data->feedback = feedback;
plugin_data->delay_samples = delay_samples;
}
plugin_data->write_phase = write_phase;
}
static void runAddingComb_n(LADSPA_Handle instance, unsigned long sample_count) {
Comb_n *plugin_data = (Comb_n *)instance;
LADSPA_Data run_adding_gain = plugin_data->run_adding_gain;
/* Input (array of floats of length sample_count) */
const LADSPA_Data * const in = plugin_data->in;
/* Output (array of floats of length sample_count) */
LADSPA_Data * const out = plugin_data->out;
/* Max Delay (s) (float value) */
const LADSPA_Data max_delay = *(plugin_data->max_delay);
/* Delay Time (s) (float value) */
const LADSPA_Data delay_time = *(plugin_data->delay_time);
/* Decay Time (s) (float value) */
const LADSPA_Data decay_time = *(plugin_data->decay_time);
LADSPA_Data * buffer = plugin_data->buffer;
unsigned int buffer_mask = plugin_data->buffer_mask;
LADSPA_Data delay_samples = plugin_data->delay_samples;
LADSPA_Data feedback = plugin_data->feedback;
LADSPA_Data last_decay_time = plugin_data->last_decay_time;
LADSPA_Data last_delay_time = plugin_data->last_delay_time;
unsigned int sample_rate = plugin_data->sample_rate;
long write_phase = plugin_data->write_phase;
#line 79 "comb_1887.xml"
int i;
i = max_delay; /* stop gcc complaining */
if (write_phase == 0) {
plugin_data->last_delay_time = delay_time;
plugin_data->last_decay_time = decay_time;
plugin_data->delay_samples = delay_samples = CALC_DELAY (delay_time);
plugin_data->feedback = feedback = calc_feedback (delay_time, decay_time);
}
if (delay_time == last_delay_time) {
long read_phase = write_phase - (long)delay_samples;
LADSPA_Data *readptr = buffer + (read_phase & buffer_mask);
LADSPA_Data *writeptr = buffer + (write_phase & buffer_mask);
LADSPA_Data *lastptr = buffer + buffer_mask + 1;
if (decay_time == last_decay_time) {
long remain = sample_count;
while (remain) {
long read_space = lastptr - readptr;
long write_space = lastptr - writeptr;
long to_process = MIN (MIN (read_space, remain), write_space);
if (to_process == 0)
return; // buffer not allocated.
remain -= to_process;
for (i=0; i<to_process; i++) {
LADSPA_Data read = *(readptr++);
*(writeptr++) = read * feedback + in[i];
buffer_write(out[i], read);
}
if (readptr == lastptr) readptr = buffer;
if (writeptr == lastptr) writeptr = buffer;
}
} else {
float next_feedback = calc_feedback (delay_time, decay_time);
float feedback_slope = (next_feedback - feedback) / sample_count;
long remain = sample_count;
while (remain) {
long read_space = lastptr - readptr;
long write_space = lastptr - writeptr;
long to_process = MIN (MIN (read_space, remain), write_space);
if (to_process == 0)
return; // buffer not allocated.
remain -= to_process;
for (i=0; i<to_process; i++) {
LADSPA_Data read = *(readptr++);
*(writeptr++) = read * feedback + in[i];
buffer_write(out[i], read);
feedback += feedback_slope;
}
if (readptr == lastptr) readptr = buffer;
if (writeptr == lastptr) writeptr = buffer;
}
plugin_data->last_decay_time = decay_time;
plugin_data->feedback = feedback;
}
write_phase += sample_count;
} else {
float next_delay_samples = CALC_DELAY (delay_time);
float delay_samples_slope = (next_delay_samples - delay_samples) / sample_count;
float next_feedback = calc_feedback (delay_time, decay_time);
float feedback_slope = (next_feedback - feedback) / sample_count;
for (i=0; i<sample_count; i++) {
long read_phase;
LADSPA_Data read;
delay_samples += delay_samples_slope;
write_phase++;
read_phase = write_phase - (long)delay_samples;
read = buffer[read_phase & buffer_mask];
buffer[write_phase & buffer_mask] = read * feedback + in[i];
buffer_write(out[i], read);
feedback += feedback_slope;
}
plugin_data->last_delay_time = delay_time;
plugin_data->last_decay_time = decay_time;
plugin_data->feedback = feedback;
plugin_data->delay_samples = delay_samples;
}
plugin_data->write_phase = write_phase;
}
static void runAddingAllpass_l(LADSPA_Handle instance, unsigned long sample_count) {
Allpass_l *plugin_data = (Allpass_l *)instance;
LADSPA_Data run_adding_gain = plugin_data->run_adding_gain;
/* Input (array of floats of length sample_count) */
const LADSPA_Data * const in = plugin_data->in;
/* Output (array of floats of length sample_count) */
LADSPA_Data * const out = plugin_data->out;
/* Max Delay (s) (float value) */
const LADSPA_Data max_delay = *(plugin_data->max_delay);
/* Delay Time (s) (float value) */
const LADSPA_Data delay_time = *(plugin_data->delay_time);
/* Decay Time (s) (float value) */
const LADSPA_Data decay_time = *(plugin_data->decay_time);
LADSPA_Data * buffer = plugin_data->buffer;
unsigned int buffer_mask = plugin_data->buffer_mask;
LADSPA_Data delay_samples = plugin_data->delay_samples;
LADSPA_Data feedback = plugin_data->feedback;
LADSPA_Data last_decay_time = plugin_data->last_decay_time;
LADSPA_Data last_delay_time = plugin_data->last_delay_time;
unsigned int sample_rate = plugin_data->sample_rate;
long write_phase = plugin_data->write_phase;
#line 82 "allpass_1895.xml"
int i;
ignore(max_delay);
if (write_phase == 0) {
plugin_data->last_delay_time = delay_time;
plugin_data->last_decay_time = decay_time;
plugin_data->delay_samples = delay_samples = CALC_DELAY (delay_time);
plugin_data->feedback = feedback = calc_feedback (delay_time, decay_time);
}
if (delay_time == last_delay_time && decay_time == last_decay_time) {
long idelay_samples = (long)delay_samples;
LADSPA_Data frac = delay_samples - idelay_samples;
for (i=0; i<sample_count; i++) {
long read_phase = write_phase - (long)delay_samples;
LADSPA_Data r1 = buffer[read_phase & buffer_mask];
LADSPA_Data r2 = buffer[(read_phase-1) & buffer_mask];
LADSPA_Data read = LIN_INTERP (frac, r1, r2);
LADSPA_Data written = read * feedback + in[i];
buffer[write_phase++ & buffer_mask] = written;
buffer_write(out[i], read - feedback * written);
write_phase++;
}
} else {
float next_delay_samples = CALC_DELAY (delay_time);
float delay_samples_slope = (next_delay_samples - delay_samples) / sample_count;
float next_feedback = calc_feedback (delay_time, decay_time);
float feedback_slope = (next_feedback - feedback) / sample_count;
for (i=0; i<sample_count; i++) {
long read_phase, idelay_samples;
LADSPA_Data read, written, frac;
delay_samples += delay_samples_slope;
write_phase++;
read_phase = write_phase - (long)delay_samples;
idelay_samples = (long)delay_samples;
frac = delay_samples - idelay_samples;
read = LIN_INTERP (frac,
buffer[read_phase & buffer_mask],
buffer[(read_phase-1) & buffer_mask]);
written = read * feedback + in[i];
buffer[write_phase & buffer_mask] = written;
buffer_write(out[i], read - feedback * written);
feedback += feedback_slope;
}
plugin_data->last_delay_time = delay_time;
plugin_data->last_decay_time = decay_time;
plugin_data->feedback = feedback;
plugin_data->delay_samples = delay_samples;
}
plugin_data->write_phase = write_phase;
}
