/*! Interpolate linear between the robot joint vectors \a joint_state1 and \a joint_state2
* using the given \a ratio.
* Using values out of [0.0, 1.0] will extrapolate, a value of 0.5 will interpolate in the
* middle.
*/
std::vector<double> interpolateLinear(const std::vector<double>& joint_state1,
const std::vector<double>& joint_state2, double ratio)
{
assert(joint_state1.size() == joint_state2.size());
std::vector<double> result(joint_state1.size());
for (std::size_t i=0; i<joint_state1.size(); ++i)
{
result.at(i) = interpolateLinear(joint_state1.at(i), joint_state2.at(i), ratio);
}
return result;
}
/*! Interpolate linear between the robot JointValueMaps \a joint_state1 and \a joint_state2
* using the given \a ratio.
* Using values out of [0.0, 1.0] will extrapolate, a value of 0.5 will interpolate in the
* middle.
*/
std::map<std::string, float> interpolateLinear(const std::map<std::string, float>& joint_state1,
const std::map<std::string, float>& joint_state2, float ratio)
{
assert(joint_state1.size() == joint_state2.size());
std::map<std::string, float> result(joint_state1);
for (std::map<std::string, float>::const_iterator it=joint_state1.begin();
it!=joint_state1.end(); ++it)
{
result[it->first] = interpolateLinear(joint_state1.at(it->first),
joint_state2.at(it->first), ratio);
}
return result;
}
Real32 interpolatedNoise(Real32 t, UInt32 octave, UInt32 UInt32, bool Smoothing)
{
Real32 intT(osgFloor(t));
Real32 fractionT = t - intT;
Real32 v1,v2;
if(Smoothing)
{
v1 = getNoise(intT,octave)/2.0f + getNoise(intT - 1.0f, octave)/4.0f + getNoise(intT + 1.0f, octave)/4.0f;
intT += 1.0f;
v2 = getNoise(intT,octave)/2.0f + getNoise(intT - 1.0f, octave)/4.0f + getNoise(intT + 1.0f, octave)/4.0f;
} else
{
v1 = getNoise(intT,octave);
v2 = getNoise(intT + 1.0f,octave);
}
Real32 returnValue(0.0);
if(UInt32 == PERLIN_INTERPOLATE_COSINE) returnValue = interpolateCosine(v1 , v2 , fractionT);
else if(UInt32 == PERLIN_INTERPOLATE_LINEAR) returnValue = interpolateLinear(v1 , v2 , fractionT);
return returnValue;
}
Real32 interpolatedNoise(const Pnt2f& t, UInt32 & octave, UInt32 UInt32, bool Smoothing)
{
Real32 intX(osgFloor(t[0])), intY(osgFloor(t[1]));
Real32 fractionX = t[0] - intX;
Real32 fractionY = t[1] - intY;
Real32 i1(0.0f), i2(0.0f), returnValue(0.0f);
if(Smoothing)
{
if(UInt32 == PERLIN_INTERPOLATE_COSINE)
{
i1 = interpolateCosine(smoothNoise(intX, intY, octave),smoothNoise(intX + 1.0f, intY, octave), fractionX);
intY += 1.0f;
i2 = interpolateCosine(smoothNoise(intX, intY, octave),smoothNoise(intX + 1.0f, intY, octave), fractionX);
returnValue = interpolateCosine(i1 , i2 , fractionY);
}
else if (UInt32 == PERLIN_INTERPOLATE_LINEAR)
{
i1 = interpolateLinear(smoothNoise(intX, intY, octave),smoothNoise(intX + 1.0f, intY, octave), fractionX);
intY += 1.0f;
i2 = interpolateLinear(smoothNoise(intX, intY, octave),smoothNoise(intX + 1.0f, intY, octave), fractionX);
returnValue = interpolateLinear(i1 , i2 , fractionY);
}
} else
{
if(UInt32 == PERLIN_INTERPOLATE_COSINE)
{
i1 = interpolateCosine(getNoise(intX, intY, octave),getNoise(intX + 1.0f, intY, octave), fractionX);
intY += 1.0f;
i2 = interpolateCosine(getNoise(intX, intY, octave),getNoise(intX + 1.0f, intY, octave), fractionX);
returnValue = interpolateCosine(i1 , i2 , fractionY);
}
else if (UInt32 == PERLIN_INTERPOLATE_LINEAR)
{
i1 = interpolateLinear(getNoise(intX, intY, octave),getNoise(intX + 1.0f, intY, octave), fractionX);
intY += 1.0f;
i2 = interpolateLinear(getNoise(intX, intY, octave),getNoise(intX + 1.0f, intY, octave), fractionX);
returnValue = interpolateLinear(i1 , i2 , fractionY);
}
}
return returnValue;
}
void ChorusAudioProcessor::processBlock (AudioSampleBuffer& buffer, MidiBuffer& midiMessages)
{
// In case we have more outputs than inputs, this code clears any output
// channels that didn't contain input data, (because these aren't
// guaranteed to be empty - they may contain garbage).
// I've added this to avoid people getting screaming feedback
// when they first compile the plugin, but obviously you don't need to
// this code if your algorithm already fills all the output channels.
for (int i = getNumInputChannels(); i < getNumOutputChannels(); ++i)
buffer.clear (i, 0, buffer.getNumSamples());
// If the sample rate has changed resize the buffers
if (FS != getSampleRate())resizeBuffers(depthOSC, delayOSC, delayBufferL, delayBufferR);
// If the modulation rate or the depth have been changed recreate LFO
if (freq != modRate || Amp != depth)LFOBuffer();
// get current write pointers
depthOSCwp = depthOSC.getWritePointer(0);
delayOSCwp = delayOSC.getWritePointer(0);
delayBufferLwp = delayBufferL.getWritePointer(0);
delayBufferRwp = delayBufferR.getWritePointer(0);
// get current read pointers
depthOSCrp = depthOSC.getReadPointer(0);
delayOSCrp = delayOSC.getReadPointer(0);
delayBufferLrp = delayBufferL.getReadPointer(0);
delayBufferRrp = delayBufferR.getReadPointer(0);
// Calculate delay in samples
int delay = (int)(delayTime*FS);
// create input and output channels to read from and write to
const float *inL = buffer.getReadPointer(0);
const float *inR = buffer.getReadPointer(1);
float *outL = {};
outL = buffer.getWritePointer(0);
float *outR = {};
outR = buffer.getWritePointer(1);
// Step through each sample in the current buffer
for (int i = 0; i < buffer.getNumSamples(); i++)
{
// Calculate the delay point, and write to delay buffer
double dSamp = i + bidx - delay;
// Current sample for the delay buffer
int ridx = i + bidx;
// Wrap ridx if it is larger than the size of the delay buffer (equal to the sample rate)
if (ridx >= FS)ridx -= FS;
// Calculate delay after modulation has been applied
double modDelay = (delay * 0.3 * depthOSCrp[ridx]);
double dSampMod = dSamp - modDelay;
// If the delay time exceeds or is under the buffer length wrap around
if ((int)dSampMod < 0)dSampMod += FS;
if ((int)dSampMod >= FS)dSampMod -= FS;
double delaySampleL = 0;
double delaySampleR = 0;
// write current input data to delayBuffer
delayBufferRwp[ridx] = inR[i];
delayBufferLwp[ridx] = inL[i];
// Check whether the delay is fractional
double dSampModround = round(dSampMod);
double check = dSampMod - dSampModround;
int inc = 0;
// If the fractional content of the delay is > 0 then interpolate against the next sample
// If the fractional ocntent of the delay is < 0 then interpolate against the previous sample
// Else take the current sample without interpolation
if (check > 0)
{
inc = (int)dSampModround+1;
if (inc >= FS)
{
delaySampleL = interpolateLinear(delayBufferLrp[int(dSampMod)], delayBufferLrp[inc-FS], dSampMod, round(dSampMod));
delaySampleR = interpolateLinear(delayBufferRrp[int(dSampMod)], delayBufferRrp[inc - FS], dSampMod, round(dSampMod));
}
else
{
delaySampleL = interpolateLinear(delayBufferLrp[int(dSampMod)], delayBufferLrp[inc], dSampMod, round(dSampMod));
delaySampleR = interpolateLinear(delayBufferRrp[int(dSampMod)], delayBufferRrp[inc], dSampMod, round(dSampMod));
}
}
else if (check < 0)
{
inc = (int)dSampModround-1;
if (inc < 0)
{
delaySampleL = interpolateLinear(delayBufferLrp[inc + FS], delayBufferLrp[int(dSampMod)], dSampMod, inc);
delaySampleR = interpolateLinear(delayBufferRrp[inc + FS], delayBufferRrp[int(dSampMod)], dSampMod, inc);
}
else
{
delaySampleL = interpolateLinear(delayBufferLrp[inc], delayBufferLrp[int(dSampMod)], dSampMod, inc);
delaySampleR = interpolateLinear(delayBufferRrp[inc], delayBufferRrp[int(dSampMod)], dSampMod, inc);
}
}
else
{
delaySampleL = delayBufferLrp[int(dSampMod)];
delaySampleR = delayBufferRrp[int(dSampMod)];
}
// Add the delayed sample to the current sample
// Weight each sample against how much of the wet/dry mix has been select
outL[i] = ((1 - wetDry)*inL[i] + wetDry*delaySampleL)*0.5;
outR[i] = ((1 - wetDry)*inR[i] + wetDry*delaySampleR)*0.5;
}
// Increment index in relation to where the current buffer will be placed within the delay buffer
// wrap bidx if greater than sample rate
bidx += buffer.getNumSamples();
if (bidx >= FS)bidx -= FS;
// store current freqency and modulation rates
freq = modRate;
Amp = depth;
}
void CircularCam::drawTexturedLine(const Point &p0, const Point &p1, const Texture &texture)
{
bool invertTextureIndex = false;
// Express p0 and p1 in the camera coordinate system.
// In cam coord sys, x axis is the optical axis.
const Matrix22 rot(-absOrientation);
Vector p0c = rot * (p0 - absPos);
Vector p1c = rot * (p1 - absPos);
// Find angle of interest. Here we order p0 and p1 so that
// p0 is the point with the smallest angle (in the [-pi;pi]
// range).
double p0dir = p0c.angle(); // [-pi;pi]
double p1dir = p1c.angle(); // [-pi;pi]
if (p0dir > p1dir)
{
std::swap(p0dir, p1dir);
std::swap(p0c, p1c);
invertTextureIndex = !invertTextureIndex;
}
const double beginAperture = -halfFieldOfView; // [-pi/2;0]
const double endAperture = halfFieldOfView; // [0; pi/2]
// check if the line is going "behind us"
if (p1dir - p0dir > M_PI)
{
// dismiss line if not in field of view.
if (p0dir < beginAperture && p1dir > endAperture)
return;
std::swap(p0dir, p1dir);
std::swap(p0c, p1c);
if (p1dir < -halfFieldOfView)
p1dir += 2*M_PI;
else
p0dir -= 2*M_PI;
invertTextureIndex = !invertTextureIndex;
}
// TODO: understand why this happens
if (!(p1dir > p0dir))
return;
assert(p1dir > p0dir);
// dismiss line if not in field of view.
if ((p1dir < beginAperture) || (p0dir > endAperture))
return;
const size_t pixelCount = zbuffer.size();
const double beginAngle = std::max(p0dir, beginAperture);
const double endAngle = std::min(p1dir, endAperture);
const double dAngle = 2*halfFieldOfView / (pixelCount - 1);
// align begin and end angle to our sampled angles
const double beginIndex = ceil((beginAngle-beginAperture) / dAngle);
const double endIndex = floor((endAngle-beginAperture) / dAngle);
const double alignedBeginAngle = beginAperture + beginIndex * dAngle;
const double alignedEndAngle = beginAperture + endIndex * dAngle;
const double beginPixel = round(interpolateLinear(beginAperture, endAperture, alignedBeginAngle, 0, pixelCount-1));
const double endPixel = round(interpolateLinear(beginAperture, endAperture, alignedEndAngle, 0, pixelCount-1));
// Optimization stuff
const double x10 = p1c.x - p0c.x;
const double y01 = p0c.y - p1c.y;
const Vector p10c = p1c - p0c;
double tanAngle;
bool tanDirty = true;
const double tanDelta = tan(dAngle);
const size_t beginPixelIndex = static_cast<size_t>(beginPixel);
const size_t endPixelIndex = static_cast<size_t>(endPixel);
double angle = alignedBeginAngle;
for (size_t i = beginPixelIndex; i <= endPixelIndex; i++)
{
double lambda = 0;
if (fabs(angle) == M_PI/2)
{
lambda = - p0c.x / x10;
tanDirty = true;
}
else
{
// OPTIMIZATION: we compute tan(angle+n*dAngle) recursively, using
// the formula tan(a+b) = (tan(a) + tan(b))/(1 - tan(a)*tan(b)
if(tanDirty)
{
tanAngle = tan(angle);
tanDirty = false;
}
else
tanAngle = (tanAngle + tanDelta) / (1 - tanAngle * tanDelta);
lambda = (p0c.y - p0c.x * tanAngle) / (tanAngle * x10 + y01);
}
assert(i < image.size());
// Compute zbuffer and texture index.
size_t texIndex;
Vector p;
if (lambda < 0)
{
p = p0c;
texIndex = 0;
}
else if (lambda >= 1)
{
p = p1c;
texIndex = texture.size() - 1;
}
else
{
p = p0c + p10c * lambda;
texIndex = static_cast<size_t>(floor(lambda * texture.size()));
}
assert(texIndex < texture.size());
// apply pixel only if distance is inferior to the current one
const double z = p.norm2();
if (zbuffer[i] > z)
{
if (invertTextureIndex)
texIndex = texture.size() - texIndex - 1;
image[i] = texture[texIndex];
zbuffer[i] = z;
}
angle += dAngle;
}
}
Real32 interpolatedNoise(const Pnt3f& t, UInt32 & octave, UInt32 UInt32, bool Smoothing)
{
Real32 intX(osgFloor(t[0])), intY(osgFloor(t[1])), intZ(osgFloor(t[2]));
Real32 fractionX = t[0] - intX;
Real32 fractionY = t[1] - intY;
Real32 fractionZ = t[2] - intZ;
Real32 v1,v2,v3,v4,v5,v6,v7,v8,i1,i2,i3,i4, returnValue(0.0f);
if(Smoothing)
{
v1 = smoothNoise(intX,intY,intZ,octave);
v2 = smoothNoise(intX + 1.0f,intY,intZ,octave);
v3 = smoothNoise(intX,intY + 1.0f,intZ,octave);
v4 = smoothNoise(intX + 1.0f,intY + 1.0f,intZ,octave);
v5 = smoothNoise(intX,intY,intZ + 1.0f,octave);
v6 = smoothNoise(intX + 1.0f,intY,intZ + 1.0f,octave);
v7 = smoothNoise(intX,intY + 1.0f,intZ + 1.0f,octave);
v8 = smoothNoise(intX + 1.0f,intY + 1.0f,intZ + 1.0f,octave);
if(UInt32 == PERLIN_INTERPOLATE_COSINE)
{
i1 = interpolateCosine(v1,v2,fractionX);
i2 = interpolateCosine(v3,v4,fractionX);
i3 = interpolateCosine(v5,v6,fractionX);
i4 = interpolateCosine(v7,v8,fractionX);
i1 = interpolateCosine(i1,i2,fractionY);
i2 = interpolateCosine(i3,i4,fractionY);
returnValue = interpolateCosine(i1,i2,fractionZ);
} else if (UInt32 == PERLIN_INTERPOLATE_LINEAR)
{
i1 = interpolateLinear(v1,v2,fractionX);
i2 = interpolateLinear(v3,v4,fractionX);
i3 = interpolateLinear(v5,v6,fractionX);
i4 = interpolateLinear(v7,v8,fractionX);
i1 = interpolateLinear(i1,i2,fractionY);
i2 = interpolateLinear(i3,i4,fractionY);
returnValue = interpolateLinear(i1,i2,fractionZ);
}
} else
{
v1 = getNoise(intX,intY,intZ,octave);
v2 = getNoise(intX + 1.0f,intY,intZ,octave);
v3 = getNoise(intX,intY + 1.0f,intZ,octave);
v4 = getNoise(intX + 1.0f,intY + 1.0f,intZ,octave);
v5 = getNoise(intX,intY,intZ + 1.0f,octave);
v6 = getNoise(intX + 1.0f,intY,intZ + 1.0f,octave);
v7 = getNoise(intX,intY + 1.0f,intZ + 1.0f,octave);
v8 = getNoise(intX + 1.0f,intY + 1.0f,intZ + 1.0f,octave);
if(UInt32 == PERLIN_INTERPOLATE_COSINE)
{
i1 = interpolateCosine(v1,v2,fractionX);
i2 = interpolateCosine(v3,v4,fractionX);
i3 = interpolateCosine(v5,v6,fractionX);
i4 = interpolateCosine(v7,v8,fractionX);
i1 = interpolateCosine(i1,i2,fractionY);
i2 = interpolateCosine(i3,i4,fractionY);
returnValue = interpolateCosine(i1,i2,fractionZ);
} else if (UInt32 == PERLIN_INTERPOLATE_LINEAR)
{
i1 = interpolateLinear(v1,v2,fractionX);
i2 = interpolateLinear(v3,v4,fractionX);
i3 = interpolateLinear(v5,v6,fractionX);
i4 = interpolateLinear(v7,v8,fractionX);
i1 = interpolateLinear(i1,i2,fractionY);
i2 = interpolateLinear(i3,i4,fractionY);
returnValue = interpolateLinear(i1,i2,fractionZ);
}
}
return returnValue;
}
void ColorTable::interpolateLinearHSV(float h1, float s1, float v1, float h2, float s2, float v2, unsigned int offset, unsigned int length) {
Color c1 = Color::fromHSV(h1, s1, v1), c2 = Color::fromHSV(h2, s2, v2);
interpolateLinear(c1.r, c1.g, c1.b, c2.r, c2.g, c2.b, offset, length);
}
