// add the entire signal b to this signal, at the subpixel destination offset.
//
void MLSignal::add2D(const MLSignal& b, const Vec2& destOffset)
{
MLSignal& a = *this;
Vec2 iDestOffset, fDestOffset;
destOffset.getIntAndFracParts(iDestOffset, fDestOffset);
int destX = iDestOffset[0];
int destY = iDestOffset[1];
float srcPosFX = fDestOffset[0];
float srcPosFY = fDestOffset[1];
MLRect srcRect(0, 0, b.getWidth() + 1, b.getHeight() + 1); // add (1, 1) for interpolation
MLRect destRect = srcRect.translated(iDestOffset).intersect(getBoundsRect());
for(int j=destRect.top(); j<destRect.bottom(); ++j)
{
for(int i=destRect.left(); i<destRect.right(); ++i)
{
a(i, j) += b.getInterpolatedLinear(i - destX - srcPosFX, j - destY - srcPosFY);
}
}
setConstant(false);
}
// TODO SSE
void MLSignal::sigLerp(const MLSignal& b, const MLSignal& mix)
{
int n = min(mSize, b.getSize());
n = min(n, mix.getSize());
for(int i = 0; i < n; ++i)
{
mDataAligned[i] = lerp(mDataAligned[i], b.mDataAligned[i], mix.mDataAligned[i]);
}
setConstant(false);
}
// note: order of properties is important! delays property will set
// the number of delays and clear other peoperties.
void FDN::setProperty(Symbol name, MLProperty value)
{
MLSignal sigVal = value.getSignalValue();
int currentSize = mDelays.size();
int newSize = sigVal.getWidth();
if(name == "delays")
{
// setting number of delays outside of bounds will turn object into a passthru
if(!within(newSize, 3, 17))
{
// TODO report error
newSize = 0;
}
// resize if needed.
if(newSize != currentSize)
{
mDelays.resize(newSize);
mFilters.resize(newSize);
mDelayInputVectors.resize(newSize);
mFeedbackGains.setDims(newSize);
}
// set default feedbacks.
for(int n=0; n<newSize; ++n)
{
mFeedbackGains[n] = 1.f;
}
// set delay times.
for(int n=0; n<newSize; ++n)
{
// we have one DSPVector feedback latency, so delay times can't be smaller than that
int len = sigVal[n] - kFloatsPerDSPVector;
len = max(1, len);
mDelays[n].setDelayInSamples(len);
}
clear();
}
else if(name == "cutoffs")
{
// compute coefficients from cutoffs
int newValues = min(currentSize, newSize);
for(int n=0; n<newValues; ++n)
{
mFilters[n].setCoeffs(biquadCoeffs::onePole(sigVal[n]));
}
}
else if(name == "gains")
{
mFeedbackGains.copy(sigVal);
}
}
// TODO SSE
void MLSignal::sigClamp(const MLSignal& a, const MLSignal& b)
{
int n = min(mSize, a.getSize());
n = min(n, b.getSize());
for(int i = 0; i < n; ++i)
{
MLSample f = mDataAligned[i];
mDataAligned[i] = clamp(f, a.mDataAligned[i], b.mDataAligned[i]);
}
setConstant(false);
}
// set up coefficient signals
MLProc::err MLProcBiquad::resize()
{
MLProc::err e = OK;
int b = getContextVectorSize();
mA0.setDims(b);
mA1.setDims(b);
mA2.setDims(b);
mB1.setDims(b);
mB2.setDims(b);
// TODO check
return e;
}
void TouchTracker::Calibrator::setCalibration(const MLSignal& v)
{
if((v.getHeight() == kTemplateSize) && (v.getWidth() == kTemplateSize))
{
mCalibrateSignal = v;
mHasCalibration = true;
}
else
{
MLConsole() << "TouchTracker::Calibrator::setCalibration: bad size, restoring default.\n";
mHasCalibration = false;
}
}
void TouchTracker::Calibrator::setNormalizeMap(const MLSignal& v)
{
if((v.getHeight() == mSrcHeight) && (v.getWidth() == mSrcWidth))
{
mNormalizeMap = v;
mHasNormalizeMap = true;
}
else
{
MLConsole() << "TouchTracker::Calibrator::setNormalizeMap: restoring default.\n";
mNormalizeMap.fill(1.f);
mHasNormalizeMap = false;
}
}
void MLSignal::copy(const MLSignal& b)
{
const bool kb = b.isConstant();
if (kb)
{
setToConstant(b.mDataAligned[0]);
}
else
{
const int n = min(mSize, b.getSize());
std::copy(b.mDataAligned, b.mDataAligned + n, mDataAligned);
setConstant(false);
}
}
// add the entire signal b to this signal, at the integer destination offset.
//
void MLSignal::add2D(const MLSignal& b, int destX, int destY)
{
MLSignal& a = *this;
MLRect srcRect(0, 0, b.getWidth(), b.getHeight());
MLRect destRect = srcRect.translated(Vec2(destX, destY)).intersect(getBoundsRect());
for(int j=destRect.top(); j<destRect.bottom(); ++j)
{
for(int i=destRect.left(); i<destRect.right(); ++i)
{
a(i, j) += b(i - destX, j - destY);
}
}
setConstant(false);
}
void TouchTracker::Calibrator::normalizeInput(MLSignal& in)
{
if(mHasNormalizeMap)
{
in.multiply(mNormalizeMap);
}
}
// setFrame() - set the 2D frame i to the incoming signal.
void MLSignal::setFrame(int i, const MLSignal& src)
{
// only valid for 3D signals
assert(is3D());
// source must be 2D
assert(src.is2D());
// src signal should match our dimensions
if((src.getWidth() != mWidth) || (src.getHeight() != mHeight))
{
return;
}
MLSample* pDestFrame = mDataAligned + plane(i);
const MLSample* pSrc = src.getConstBuffer();
std::copy(pSrc, pSrc + src.getSize(), pDestFrame);
}
// a simple pixel-by-pixel measure of the distance between two signals.
//
float rmsDifference2D(const MLSignal& a, const MLSignal& b)
{
int w = min(a.getWidth(), b.getWidth());
int h = min(a.getHeight(), b.getHeight());
float sum = 0.;
float d;
for(int j=0; j<h; ++j)
{
for(int i=0; i<w; ++i)
{
d = a(i, j) - b(i, j);
sum += d*d;
}
}
sum /= w*h;
sum = sqrtf(sum);
return sum;
}
void MLSignal::sigMax(const MLSignal& b)
{
int n = min(mSize, b.getSize());
for(int i = 0; i < n; ++i)
{
MLSample f = mDataAligned[i];
mDataAligned[i] = max(f, b.mDataAligned[i]);
}
setConstant(false);
}
// TODO SSE
void MLSignal::divide(const MLSignal& b)
{
const bool ka = isConstant();
const bool kb = b.isConstant();
if (ka && kb)
{
setToConstant(mDataAligned[0] + b.mDataAligned[0]);
}
else
{
const int n = min(mSize, b.getSize());
if (ka && !kb)
{
MLSample fa = mDataAligned[0];
for(int i = 0; i < n; ++i)
{
mDataAligned[i] = fa / b[i];
}
}
else if (!ka && kb)
{
MLSample fb = b[0];
for(int i = 0; i < n; ++i)
{
mDataAligned[i] /= fb;
}
}
else
{
for(int i = 0; i < n; ++i)
{
mDataAligned[i] /= b.mDataAligned[i];
}
}
setConstant(false);
}
}
void TouchTracker::addPeakToKeyState(const MLSignal& in)
{
mTemp.copy(in);
for(int i=0; i<kMaxPeaksPerFrame; ++i)
{
// get highest peak from temp.
Vec3 peak = mTemp.findPeak();
float z = peak.z();
// add peak to key state, or bail
if (z > mOnThreshold)
{
Vec2 pos = in.correctPeak(peak.x(), peak.y());
int key = getKeyIndexAtPoint(pos);
if(within(key, 0, mNumKeys))
{
// send peak energy to key under peak.
KeyState& keyState = mKeyStates[key];
MLRange kdzRange(mOnThreshold, mMaxForce*0.5, 0.001f, 1.f);
float iirCoeff = kdzRange.convertAndClip(z);
float dt = mCalibrator.differenceFromTemplateTouch(in, pos);
//debug() << "new PEAK dt: " << dt << "\n";
keyState.mK = iirCoeff;
keyState.zIn = z;
keyState.dtIn = dt;
if(mQuantizeToKey)
{
keyState.posIn = keyState.mKeyCenter;
}
else
{
keyState.posIn = pos;
}
}
}
else
{
break;
}
}
}
// constructor for making loops. only one type for now. we could loop in different directions and dimensions.
MLSignal::MLSignal(MLSignal other, eLoopType loopType, int loopSize) :
mData(0),
mDataAligned(0),
mCopy(0),
mCopyAligned(0)
{
switch(loopType)
{
case kLoopType1DEnd:
default:
{
int w = other.getWidth();
int loopWidth = clamp(loopSize, 0, w);
setDims(w + loopWidth, 1, 1);
mRate = other.mRate;
std::copy(other.mDataAligned, other.mDataAligned + w, mDataAligned);
std::copy(other.mDataAligned, other.mDataAligned + loopWidth, mDataAligned + w);
}
break;
}
}
void MLProcBiquad::calcCoeffs(const int frames)
{
static MLSymbol modeSym("mode");
int mode = (int)getParam(modeSym);
const MLSignal& frequency = getInput(2);
const MLSignal& q = getInput(3);
int coeffFrames;
float twoPiOverSr = kMLTwoPi*getContextInvSampleRate();
bool paramSignalsAreConstant = frequency.isConstant() && q.isConstant();
if (paramSignalsAreConstant)
{
coeffFrames = 1;
}
else
{
coeffFrames = frames;
}
// set proper constant state for coefficient signals
mA0.setConstant(paramSignalsAreConstant);
mA1.setConstant(paramSignalsAreConstant);
mA2.setConstant(paramSignalsAreConstant);
mB1.setConstant(paramSignalsAreConstant);
mB2.setConstant(paramSignalsAreConstant);
float a0, a1, a2, b0, b1, b2;
float qm1, omega, alpha, sinOmega, cosOmega;
float highLimit = getContextSampleRate() * 0.33f;
// generate coefficient signals
// TODO SSE
for(int n=0; n<coeffFrames; ++n)
{
qm1 = 1.f/(q[n] + 0.05f);
omega = clamp(frequency[n], kLowFrequencyLimit, highLimit) * twoPiOverSr;
sinOmega = fsin1(omega);
cosOmega = fcos1(omega);
alpha = sinOmega * 0.5f * qm1;
b0 = 1.f/(1.f + alpha);
switch (mode)
{
default:
case kLowpass:
a0 = ((1.f - cosOmega) * 0.5f) * b0;
a1 = (1.f - cosOmega);
a2 = a0;
b1 = (-2.f * cosOmega);
b2 = (1.f - alpha);
break;
case kHighpass:
a0 = ((1.f + cosOmega) * 0.5f);
a1 = -(1.f + cosOmega);
a2 = a0;
b1 = (-2.f * cosOmega);
b2 = (1.f - alpha);
break;
case kBandpass:
a0 = alpha;
a1 = 0.f;
a2 = -alpha;
b1 = -2.f * cosOmega;
b2 = (1.f - alpha);
break;
case kNotch:
a0 = 1;
a1 = -2.f * cosOmega;
a2 = 1;
b1 = -2.f * cosOmega;
b2 = (1.f - alpha);
break;
}
mA0[n] = a0*b0;
mA1[n] = a1*b0;
mA2[n] = a2*b0;
mB1[n] = b1*b0;
mB2[n] = b2*b0;
}
}
// read a ring buffer into the given row of the destination signal.
//
int MLProcRingBuffer::readToSignal(MLSignal& outSig, int samples, int row)
{
int lastRead = 0;
int skipped = 0;
int available = 0;
MLSample * outBuffer = outSig.getBuffer() + outSig.row(row);
void * trashBuffer = (void *)mTrashSignal.getBuffer();
MLSample * trashbufferAsSamples = reinterpret_cast<MLSample*>(trashBuffer);
static MLSymbol modeSym("mode");
int mode = (int)getParam(modeSym);
bool underTrigger = false;
MLSample triggerVal = 0.f;
samples = min(samples, (int)outSig.getWidth());
available = (int)PaUtil_GetRingBufferReadAvailable( &mBuf );
// return if we have not accumulated enough signal.
if (available < samples) return 0;
// depending on trigger mode, trash samples up to the ones we will return.
switch(mode)
{
default:
case eMLRingBufferNoTrash:
break;
case eMLRingBufferUpTrig:
while (available >= samples+1)
{
// read buffer
lastRead = (int)PaUtil_ReadRingBuffer( &mBuf, trashBuffer, 1 );
skipped += lastRead;
available = (int)PaUtil_GetRingBufferReadAvailable( &mBuf );
if(trashbufferAsSamples[0] < triggerVal)
{
underTrigger = true;
}
else
{
if (underTrigger == true) break;
underTrigger = false;
}
}
break;
case eMLRingBufferMostRecent:
if (available > samples)
{
// TODO modify pa ringbuffer instead of reading to trash buffer.
lastRead = (int)PaUtil_ReadRingBuffer( &mBuf, trashBuffer, available - samples );
// skipped += lastRead;
}
break;
}
lastRead = (int)PaUtil_ReadRingBuffer( &mBuf, outBuffer, samples );
// DEBUG
/*
const MLSymbol& myName = getName();
if (!myName.compare("body_position_x_out"))
{
available = PaUtil_GetRingBufferReadAvailable( &mBuf );
if ((skipped == 0) && (lastRead == 0))
{
debug() << "-";
}
else
{
debug() << getName() << " requested " << samples << " read " << lastRead << ", skipped " << skipped << ", avail. " << available << "\n";
}
}
*/
return lastRead;
}
// expire or move existing touches based on new signal input.
//
void TouchTracker::updateTouches(const MLSignal& in)
{
// copy input signal to border land
int width = in.getWidth();
int height = in.getHeight();
mTemp.copy(in);
mTemplateMask.clear();
// sort active touches by Z
// copy into sorting container, referring back to unsorted touches
int activeTouches = 0;
for(int i = 0; i < mMaxTouchesPerFrame; ++i)
{
Touch& t = mTouches[i];
if (t.isActive())
{
t.unsortedIdx = i;
mTouchesToSort[activeTouches++] = t;
}
}
std::sort(mTouchesToSort.begin(), mTouchesToSort.begin() + activeTouches, compareTouchZ());
// update active touches in sorted order, referring to existing touches in place
for(int i = 0; i < activeTouches; ++i)
{
int refIdx = mTouchesToSort[i].unsortedIdx;
Touch& t = mTouches[refIdx];
Vec2 pos(t.x, t.y);
Vec2 newPos = pos;
float newX = t.x;
float newY = t.y;
float newZ = in.getInterpolatedLinear(pos);
// if not preparing to remove, update position.
if (t.releaseCtr == 0)
{
Vec2 minPos(0, 0);
Vec2 maxPos(width, height);
int ix = floor(pos.x() + 0.5f);
int iy = floor(pos.y() + 0.5f);
// move to any higher neighboring integer value
Vec2 newPeak;
newPeak = adjustPeak(mTemp, ix, iy);
Vec2 newPeakI, newPeakF;
newPeak.getIntAndFracParts(newPeakI, newPeakF);
int newPx = newPeakI.x();
int newPy = newPeakI.y();
// get exact location and new key
Vec2 correctPos = mTemp.correctPeak(newPx, newPy);
newPos = correctPos;
int newKey = getKeyIndexAtPoint(newPos);
// move the touch.
if((newKey == t.key) || !keyIsOccupied(newKey))
{
// This must be the only place a touch can move from key to key.
pos = newPos;
newX = pos.x();
newY = pos.y();
t.key = newKey;
}
}
// look for reasons to release
newZ = in.getInterpolatedLinear(newPos);
bool thresholdTest = (newZ > mOffThreshold);
float inhibit = getInhibitThreshold(pos);
bool inhibitTest = (newZ > inhibit);
t.tDist = mCalibrator.differenceFromTemplateTouchWithMask(mTemp, pos, mTemplateMask);
bool templateTest = (t.tDist < mTemplateThresh);
bool overrideTest = (newZ > mOverrideThresh);
t.age++;
// handle release
// TODO get releaseDetect: evidence that touch has been released.
// from matching release curve over ~ 50 samples.
if (!thresholdTest || (!templateTest && !overrideTest) || (!inhibitTest))
{
/* debug
if(!thresholdTest && (t.releaseCtr == 0))
{
debug() << refIdx << " REL thresholdFail: " << newZ << " at " << pos << "\n";
}
if(!inhibitTest && (t.releaseCtr == 0))
{
debug() << refIdx << " REL inhibitFail: " << newZ << " < " << inhibit << "\n";
}
if(!templateTest && (t.releaseCtr == 0))
{
debug() << refIdx << " REL templateFail: " << t.tDist << " at " << pos << "\n";
}
*/
if(t.releaseCtr == 0)
{
t.releaseSlope = t.z / (float)kTouchReleaseFrames;
}
t.releaseCtr++;
newZ = t.z - t.releaseSlope;
}
else
{
// reset off counter
t.releaseCtr = 0;
}
// filter position and assign new touch values
const float e = 2.718281828;
float xyCutoff = (newZ - mOnThreshold) / (mMaxForce*0.25);
xyCutoff = clamp(xyCutoff, 0.f, 1.f);
xyCutoff *= xyCutoff;
xyCutoff *= xyCutoff;
xyCutoff = xyCutoff*100.;
xyCutoff = clamp(xyCutoff, 1.f, 100.f);
float x = powf(e, -kMLTwoPi * xyCutoff / (float)mSampleRate);
float a0 = 1.f - x;
float b1 = -x;
t.dz = newZ - t.z;
// these can't be filtered too much or updateTouches will not work
// for fast movements. Revisit when we rewrite the touch tracker.
t.x = a0*newX - b1*t.x;
t.y = a0*newY - b1*t.y;
t.z = newZ;
// filter z based on user lowpass setting and touch age
float lp = mLopass;
lp -= t.age*(mLopass*0.75f/kAttackFrames);
lp = clamp(lp, mLopass, mLopass*0.25f); // WTF???
float xz = powf(e, -kMLTwoPi * lp / (float)mSampleRate);
float a0z = 1.f - xz;
float b1z = -xz;
t.zf = a0z*(newZ - mOnThreshold) - b1z*t.zf;
// remove touch if filtered z is below threshold
if(t.zf < 0.)
{
// debug() << refIdx << " OFF with z:" << t.z << " td:" << t.tDist << "\n";
removeTouchAtIndex(refIdx);
}
// subtract updated touch from the input sum.
// mTemplateScaled is scratch space.
mTemplateScaled.clear();
mTemplateScaled.add2D(mCalibrator.getTemplate(pos), 0, 0);
mTemplateScaled.scale(-t.z*mCalibrator.getZAdjust(pos));
mTemp.add2D(mTemplateScaled, Vec2(pos - Vec2(kTemplateRadius, kTemplateRadius)));
mTemp.sigMax(0.0);
// add touch neighborhood to template mask. This allows crowded touches
// to pass the template test by ignoring areas shared with other touches.
Vec2 maskPos(t.x, t.y);
const MLSignal& tmplate = mCalibrator.getTemplate(maskPos);
mTemplateMask.add2D(tmplate, maskPos - Vec2(kTemplateRadius, kTemplateRadius));
}
}
// if any neighbors of the input coordinates are higher, move the coordinates there.
//
Vec2 TouchTracker::adjustPeak(const MLSignal& in, int xp, int yp)
{
int width = in.getWidth();
int height = in.getHeight();
int x = clamp(xp, 1, width - 2);
int y = clamp(yp, 1, height - 2);
float t = 0.0;
int rx = x;
int ry = y;
float a = in(x - 1, y - 1);
float b = in(x, y - 1);
float c = in(x + 1, y - 1);
float d = in(x - 1, y);
float e = in(x, y);
float f = in(x + 1, y);
float g = in(x - 1, y + 1);
float h = in(x, y + 1);
float i = in(x + 1, y + 1);
float fmax = e;
if (y > 0)
{
if (a > fmax + t)
{
// debug() << "<^ ";
fmax = a; rx = x - 1; ry = y - 1;
}
if (b > fmax + t)
{
// debug() << "^ ";
fmax = b; rx = x; ry = y - 1;
}
if (c > fmax + t)
{
// debug() << ">^ ";
fmax = c; rx = x + 1; ry = y - 1;
}
}
if (d > fmax + t)
{
// debug() << "<- ";
fmax = d; rx = x - 1; ry = y;
}
if (f > fmax + t)
{
// debug() << "-> ";
fmax = f; rx = x + 1; ry = y;
}
if (y < in.getHeight() - 1)
{
if (g > fmax + t)
{
// debug() << "<_ ";
fmax = g; rx = x - 1; ry = y + 1;
}
if (h > fmax + t)
{
// debug() << "_ ";
fmax = h; rx = x; ry = y + 1;
}
if (i > fmax + t)
{
// debug() << ">_ ";
fmax = i; rx = x + 1; ry = y + 1;
}
}
return Vec2(rx, ry);
}
float TouchTracker::Calibrator::differenceFromTemplateTouchWithMask(const MLSignal& in, Vec2 pos, const MLSignal& mask)
{
static float maskThresh = 0.001f;
static MLSignal a2(kTemplateSize, kTemplateSize);
static MLSignal b(kTemplateSize, kTemplateSize);
static MLSignal b2(kTemplateSize, kTemplateSize);
float r = 0.f;
int height = in.getHeight();
int width = in.getWidth();
MLRect boundsRect(0, 0, width, height);
// use linear interpolated z value from input
float linearZ = in.getInterpolatedLinear(pos)*getZAdjust(pos);
linearZ = clamp(linearZ, 0.00001f, 1.f);
float z1 = 1./linearZ;
const MLSignal& a = getTemplate(pos);
// get normalized input values surrounding touch
int tr = kTemplateRadius;
b.clear();
for(int j=0; j < kTemplateSize; ++j)
{
for(int i=0; i < kTemplateSize; ++i)
{
Vec2 vInPos = pos + Vec2((float)i - tr,(float)j - tr);
if (boundsRect.contains(vInPos) && (mask.getInterpolatedLinear(vInPos) < maskThresh))
{
float inVal = in.getInterpolatedLinear(vInPos);
inVal *= z1;
b(i, j) = inVal;
}
}
}
int tests = 0;
float sum = 0.;
// add differences in z from template
a2.copy(a);
b2.copy(b);
a2.partialDiffX();
b2.partialDiffX();
for(int j=0; j < kTemplateSize; ++j)
{
for(int i=0; i < kTemplateSize; ++i)
{
if(b(i, j) > 0.)
{
float d = a2(i, j) - b2(i, j);
sum += d*d;
tests++;
}
}
}
// get RMS difference
if(tests > 0)
{
r = sqrtf(sum / tests);
}
return r;
}
