@yannseznec I pushed something similar here. This is based on the code that was in the d-box example, but improved. This class works pretty well as far as I can tell. It's not Pd, but this is something that's somehow easier to do in C++. Could add an integration with Bela + Trill + Pd I guess ...
class TouchTracker
{
public:
typedef uint32_t Id;
static constexpr Id kIdInvalid = -1;
typedef CentroidDetection::DATA_T Position;
struct TouchWithId
{
centroid_t touch;
Position startLocation;
Id id = kIdInvalid;
};
private:
static_assert(std::is_signed<Position>::value); // if not signed, distance computation below may get fuzzy
static_assert(std::is_same<decltype(centroid_t::location),Position>::value);
static constexpr size_t kMaxTouches = 5;
size_t numTouches = 0;
size_t newId = 0;
Position maxTrackingDistance = 0.2;
std::array<unsigned int,kMaxTouches> sortedTouchIndices {};
std::array<unsigned int,kMaxTouches> sortedTouchIds {};
std::array<TouchWithId,kMaxTouches> sortedTouches {};
public:
static constexpr TouchWithId kInvalidTouch = {
.id = kIdInvalid,
};
private:
size_t getTouchOrderById(const Id id)
{
for(size_t n = 0; n < sortedTouches.size() && n < numTouches; ++n)
if(id == sortedTouches[n].id)
return n;
return numTouches;
}
// these are stored only so that we can detect frame changes.
// TODO: if there is a guarantee process() is called only on new frames,
// you can save memory by making these local variables there.
std::array<centroid_t,kMaxTouches> touches;
public:
void setMaxTrackingDistance(Position d)
{
maxTrackingDistance = d;
}
void process(CentroidDetection& slider) {
// cache previous readings
std::array<TouchWithId,kMaxTouches> prevSortedTouches = sortedTouches;
size_t prevNumTouches = numTouches;
numTouches = slider.getNumTouches();
bool changed = (numTouches != prevNumTouches);
for(size_t n = 0; n < numTouches; ++n)
{
centroid_t newTouch = centroid_t{ .location = slider.touchLocation(n), .size = slider.touchSize(n) };
changed |= memcmp(&newTouch, &touches[n], sizeof(newTouch));
touches[n] = newTouch;
}
if(!changed)
{
// if we are a repetition of the previous frame, no need to process anything
// NOTE: right now we are already avoiding calling on duplicate frames,
// so we will return early only if two successive, distinct frames happen
// to be _exactly_ the same
return;
}
Id firstNewId = newId;
constexpr size_t kMaxPermutations = kMaxTouches * (kMaxTouches - 1);
Position distances[kMaxPermutations];
size_t permCodes[kMaxPermutations];
// calculate all distance permutations between previous and current touches
for(size_t i = 0; i < numTouches; ++i)
{
for(size_t p = 0; p < prevNumTouches; ++p)
{
size_t index = i * prevNumTouches + p; // permutation code [says between which touches we are calculating distance]
distances[index] = std::abs(touches[i].location - prevSortedTouches[p].touch.location);
permCodes[index] = index;
// sort permCodes and distances by distances from min to max
while(index && (distances[index] < distances[index - 1]))
{
std::swap(permCodes[index], permCodes[index - 1]);
std::swap(distances[index], distances[index - 1]);
index--;
}
}
}
size_t sorted = 0;
bool currAssigned[kMaxTouches] = {false};
bool prevAssigned[kMaxTouches] = {false};
// track touches assigning index according to shortest distance
for(size_t i = 0; i < numTouches * prevNumTouches; ++i)
{
size_t currentIndex = permCodes[i] / prevNumTouches;
size_t prevIndex = permCodes[i] % prevNumTouches;
if(distances[i] > maxTrackingDistance)
{
// if distance is too large, it must be a new touch
// TODO: this heuristic could be improved, e.g.: by tracking
// and considering past velocity
continue;
}
// avoid double assignment
if(!currAssigned[currentIndex] && !prevAssigned[prevIndex])
{
currAssigned[currentIndex] = true;
prevAssigned[prevIndex] = true;
sortedTouchIndices[currentIndex] = prevIndex;
sortedTouchIds[currentIndex] = prevSortedTouches[prevIndex].id;
sorted++;
}
}
// assign a free index to new touches
for(size_t i = 0; i < numTouches; i++)
{
if(!currAssigned[i])
{
sortedTouchIndices[i] = sorted++; // assign next free index
sortedTouchIds[i] = newId++;
}
}
// if some touches have disappeared...
// ...we have to shift all indices...
for(size_t i = prevNumTouches - 1; i != (size_t)-1; --i) // things you do to avoid warnings ...
{
if(!prevAssigned[i])
{
for(size_t j = 0; j < numTouches; ++j)
{
// ...only if touches that disappeared were before the current one
if(sortedTouchIndices[j] > i)
sortedTouchIndices[j]--;
}
}
}
// done! now update
for(size_t i = 0; i < numTouches; ++i)
{
// update tracked value
size_t idx = sortedTouchIndices[i];
const Id id = sortedTouchIds[i];
Position startLocation = -1;
if(id >= firstNewId)
startLocation = touches[i].location;
else {
// find it in the prev arrays
// TODO: cache this value earlier so we can be faster here
for(size_t n = 0; n < prevNumTouches; ++n)
{
if(id == prevSortedTouches[n].id)
{
startLocation = prevSortedTouches[n].startLocation;
break;
}
}
}
assert(-1 != startLocation);
sortedTouches[idx] = TouchWithId {
.touch = touches[i],
.startLocation = startLocation,
.id = id,
};
}
// empty remaining touches. Not that they should ever be accessed...
for(size_t i = numTouches; i < sortedTouches.size(); ++i)
sortedTouches[i].id = kIdInvalid;
#if 0
for(size_t n = 0; n < numTouches; ++n)
{
auto& t = getTouchOrdered(n);
printf("[%u]%lu %.2f %.1f ", n, t.id, t.touch.location, t.startLocation);
}
if(numTouches)
printf("\n\r");
#endif
}
size_t getNumTouches()
{
return numTouches;
}
const TouchWithId& getTouchById(const Id id)
{
size_t n = getTouchOrderById(id);
if(n >= numTouches)
return kInvalidTouch;
else
return sortedTouches[n];
}
void setStartLocationById(const Id id, Position newLocation)
{
size_t n = getTouchOrderById(id);
if(n < numTouches)
sortedTouches[n].startLocation = newLocation;
}
// the last is the most recent
const TouchWithId& getTouchOrdered(size_t n)
{
return sortedTouches[n];
}
const TouchWithId& getTouchMostRecent()
{
return sortedTouches[numTouches - 1];
}
const TouchWithId& getTouchOldest()
{
return sortedTouches[0];
}
};
