void predictInterp(double *alpha, double *lambda, double *beta, double *predictPositions, int *NpredictPositions, double *diffPositionj, double *currPositionsj, double *currPositionsjp1, double *thetaj, double *thetajp1, double *predvals) {
// Runs the prediction code when we are interpolating between two positions
int Nd = rpois((*lambda)*(*diffPositionj));
int i;
double depthEvents[Nd];
for(i=0;i<Nd;i++) depthEvents[i] = runif(*currPositionsj,*currPositionsjp1);
R_rsort(depthEvents,Nd);
double timeEventsUnsc[Nd+1],timeEventsSum=0.0;
for(i=0;i<Nd+1;i++) timeEventsUnsc[i] = rgamma(*alpha,1/(*beta));
for(i=0;i<Nd+1;i++) timeEventsSum += timeEventsUnsc[i];
double timeEvents[Nd+1];
for(i=0;i<Nd+1;i++) timeEvents[i] = (*thetajp1-*thetaj)*timeEventsUnsc[i]/timeEventsSum;
double timeEventsCumsum[Nd+1],allTimeEvents[Nd+2];
timeEventsCumsum[0] = 0.0;
for(i=1;i<Nd+1;i++) timeEventsCumsum[i] = timeEventsCumsum[i-1] + timeEvents[i];
for(i=0;i<Nd+1;i++) allTimeEvents[i] = timeEventsCumsum[i]+*thetaj;
allTimeEvents[Nd+1] = *thetajp1;
double allDepthEvents[Nd+2];
allDepthEvents[0] = *currPositionsj;
for(i=1;i<Nd+1;i++) allDepthEvents[i] = depthEvents[i-1];
allDepthEvents[Nd+1] = *currPositionsjp1;
int Ndp2 = Nd+2;
for(i=0;i<*NpredictPositions;i++) {
linInterp(&Ndp2,&predictPositions[i],allDepthEvents,allTimeEvents,&predvals[i]);
}
}
void predictExtrapUp(double *alpha, double *lambda, double *beta, double *predictPositions, int *NpredictPositions, double *currPositions1, double *theta1, int *maxExtrap, double *extractDate, double *predvals) {
// Runs the prediction code when we are extrapolating up beyond the first date
int bad=1,count=0,i;
double depthEvents[*maxExtrap],timeEvents[*maxExtrap];
depthEvents[0] = *currPositions1;
timeEvents[0] = *theta1;
while(bad==1) {
for(i=1;i<*maxExtrap;i++) {
depthEvents[i] = depthEvents[i-1]-rexp(1/(*lambda));
timeEvents[i] = timeEvents[i-1]-rgamma(*alpha,1/(*beta));
}
for(i=0;i<*NpredictPositions;i++) {
linInterp(maxExtrap,&predictPositions[i],depthEvents,timeEvents,&predvals[i]);
}
count+=1;
bad=0;
for(i=0;i<*NpredictPositions;i++) {
if(predvals[i]<*extractDate) bad=1;
}
if(count==50) {
for(i=0;i<*NpredictPositions;i++) {
if(predvals[i]<*extractDate) predvals[i] = *extractDate;
}
bad=0;
warning("Unable to find suitable chronologies for top of core - truncated to date of extraction");
}
}
}
//business function
//--------------------------------------------------------------------------------
//--------------------------------------------------------------------------------
//
// The following function is the workhorse of the delay line
// It uses terms similar to the differential equation for delay
//
// x(n) : the input at sample n
// y(n) : the output at sample n after delay processing
// buffer : the circular buffer used
// readPosA : the read index of the delay buffer
// writePos : the write index of the delay buffer
// MAX_DELAY_SAMPLES : Max size of delay buffer
//
// y(n) = x(n) + x(n - D) 'delay with no feedback
//
// y(n) = x(n - D) + fb*y(n - D) 'delay with feedback
//
// y(n) = x(n) + x(n – D) - fb*x(n-D) + fb*y(n-D) 'feedback with wet/dry mix
//
// MAX_DELAY_SAMPLES = sr * d_ms * .001;
//---------------------------------------------------------------------------------
//---------------------------------------------------------------------------------
float CrossDelayLine::next(const float in){
if(delay_bypass)
return in;
//input
float xn = in;
//output of delay at readPos
float yn = buffer[readPosA];
//if delay < 1 sample, interpolate between x(n) and x(n-1)
if(readPosA == writePos && delay_samples < 1.00)
{
yn = xn;
}
//read location at n-1, one behind yn
int readPos_minus1 = readPosA - 1;
if(readPos_minus1 < 0)
readPos_minus1 = MAX_DELAY_SAMPLES - 1; //MAX_DELAY_SAMPLES - 1 is the last location of the buffer
//get y(n-1)
float yn_minus1 = buffer[readPos_minus1];
//perform linear interpolation of : (0,yn) and (1,yn_minus1) by the ammount of fractional delay(fraction)
float interp = linInterp(0, 1, yn, yn_minus1, fraction);
//if delay value is zero just pass input out
if(delay_samples == 0)
yn = xn;
else
yn = interp;
//write the input to the delay
//check if External Feedback path is enabled
//if enabled then you write the input plus the
//feedbaclIn signal to the delay line instead
if(!useExternalFeedback)
buffer[writePos] = xn + feedback*yn;
else
buffer[writePos] = xn + feedbackIn;
//create wet level and write to output buffer
float out = mixLevel*yn + (1.0 - mixLevel)*xn;
//wrap if bounds exceeded
writePos++;
if(writePos >= MAX_DELAY_SAMPLES)
writePos = 0;
readPosA++;
if(readPosA >= MAX_DELAY_SAMPLES)
readPosA = 0;
return out;
}
void predictExtrapDown(double *alpha, double *lambda, double *beta, double *predictPositions, int *NpredictPositions, double *currPositionsn, double *thetan, int *maxExtrap, double *predvals) {
// Runs the prediction code when we are extrapolating down below the bottom date
double depthEvents[*maxExtrap],timeEvents[*maxExtrap];
int i;
depthEvents[0] = *currPositionsn;
timeEvents[0] = *thetan;
for(i=1;i<*maxExtrap;i++) {
depthEvents[i] = depthEvents[i-1]+rexp(1/(*lambda));
timeEvents[i] = timeEvents[i-1]+rgamma(*alpha,1/(*beta));
}
for(i=0;i<*NpredictPositions;i++) {
linInterp(maxExtrap,&predictPositions[i],depthEvents,timeEvents,&predvals[i]);
}
}
void Node::nextPhase (int interpIndex, int worldIndex, Interpolation interp, int interpCount) {
if (interpCount < 1)
interpCount = 1;
switch (interp) {
case LINEAR:
phase = linInterp(states[wrapi(worldIndex - 1, 0, 2)], states[worldIndex], interpIndex / interpCount);
break;
case COSINE:
phase = cosInterp(states[wrapi(worldIndex - 1, 0, 2)], states[worldIndex], interpIndex / interpCount);
break;
default:
phase = states[worldIndex];
break;
}
}
//business function
//--------------------------------------------------------------------------------
//--------------------------------------------------------------------------------
//
// The following function is the workhorse of the delay line
// It uses terms similar to the differential equation for delay
//
// x(n) : the input at sample n
// y(n) : the output at sample n after delay processing
// buffer : the circular buffer used
// readPosA : the read index of the delay buffer
// readTapX : tap delay 1-4 read indexes set when delay values
// : for these are set
// tapXOutput : output for tap 1-4, delay position* tapXlevel
// writePos : the write index of the delay buffer
// MAX_DELAY_SAMPLES : Max size of delay buffer
//
// y(n) = x(n) + x(n - D) 'delay with no feedback
//
// y(n) = x(n - D) + fb*y(n - D) 'delay with feedback
//
// y(n) = x(n) + x(n – D) - fb*x(n-D) + fb*y(n-D) 'feedback with wet/dry mix
//
// MAX_DELAY_SAMPLES = sr * d_ms * .001;
//---------------------------------------------------------------------------------
//---------------------------------------------------------------------------------
float SyncedTapDelayLine::next(const float in){
if(delay_bypass)
return in;
//input
float xn = in;
//output of delay at readPos
float yn = buffer[readPosA];
//read the 4 individual taps from buffer and scale
float tap1Output = buffer[readTap1] * tap1level;
float tap2Output = buffer[readTap2] * tap2level;
float tap3Output = buffer[readTap3] * tap3level;
float tap4Output = buffer[readTap4] * tap4level;
//if delay < 1 sample, interpolate between x(n) and x(n-1)
if(readPosA == writePos && delay_samples < 1.00)
{
yn = xn;
}
//read location at n-1, one behind yn
int readPos_minus1 = readPosA - 1;
if(readPos_minus1 < 0)
readPos_minus1 = MAX_DELAY_SAMPLES - 1; //MAX_DELAY_SAMPLES - 1 is the last location of the buffer
//get y(n-1)
float yn_minus1 = buffer[readPos_minus1];
//perform linear interpolation of : (0,yn) and (1,yn_minus1) by the ammount of fractional delay(fraction)
float interp = linInterp(0, 1, yn, yn_minus1, fraction);
//if delay value is zero just pass input out
if(delay_samples == 0)
yn = xn;
else
yn = interp;
//write the input to the delay
//check if External Feedback path is enabled
if(!useExternalFeedback)
buffer[writePos] = xn + feedback*yn;
else
buffer[writePos] = xn + feedbackIn;
//add the tap delays that are not zero in length
float tappedOutput = yn;
if(tap1delay){tappedOutput = tappedOutput + tap1Output;}
if(tap2delay){tappedOutput = tappedOutput + tap2Output;}
if(tap3delay){tappedOutput = tappedOutput + tap3Output;}
if(tap4delay){tappedOutput = tappedOutput + tap4Output;}
//create wet level and write to output buffer
float out = mixLevel*tappedOutput + (1.0 - mixLevel)*xn;
//wrap indexes if out of bounds
writePos++;
if(writePos >= MAX_DELAY_SAMPLES)
writePos = 0;
readPosA++;
if(readPosA >= MAX_DELAY_SAMPLES)
readPosA = 0;
readTap1++;
readTap2++;
readTap3++;
readTap4++;
if(readTap1 >= MAX_DELAY_SAMPLES)
readTap1 = 0;
if(readTap2 >= MAX_DELAY_SAMPLES)
readTap2 = 0;
if(readTap3 >= MAX_DELAY_SAMPLES)
readTap3 = 0;
if(readTap4 >= MAX_DELAY_SAMPLES)
readTap4 = 0;
return out;
}
double RobotPosition::speedAtTime(double time)
{
return linInterp(time, _Speed);
}
double RobotPosition::headingAtTime(double time)
{
return linInterp(time, _Heading);
}
double RobotPosition::longAtTime(double time)
{
return linInterp(time, _Long);
}
double RobotPosition::latAtTime(double time)
{
return linInterp(time, _Lat);
}
