public BroadPhase(AABB worldAABB, PairCallback callback) {
if (debugPrint) {
System.out.println("BroadPhase()");
}
// array initialization
m_proxyPool = new Proxy[Settings.maxProxies];
pairBuffer = new BufferedPair[Settings.maxPairs];
m_bounds = new Bound[2][2 * Settings.maxProxies];
m_queryResults = new int[Settings.maxProxies];
for (int i = 0; i < 2 * Settings.maxProxies; i++) {
m_bounds[0][i] = new Bound();
m_bounds[1][i] = new Bound();
}
for (int i = 0; i < Settings.maxProxies; i++) {
pairBuffer[i] = new BufferedPair();
}
m_pairManager = new PairManager();
m_pairManager.initialize(this, callback);
assert worldAABB.isValid();
m_worldAABB = new AABB(worldAABB);
m_proxyCount = 0;
Vec2 d = worldAABB.upperBound.sub(worldAABB.lowerBound);
m_quantizationFactor = new Vec2(Integer.MAX_VALUE / d.x, Integer.MAX_VALUE / d.y);
for (int i = 0; i < Settings.maxProxies - 1; ++i) {
m_proxyPool[i] = new Proxy();
m_proxyPool[i].setNext(i + 1);
m_proxyPool[i].timeStamp = 0;
m_proxyPool[i].overlapCount = INVALID;
m_proxyPool[i].userData = null;
}
m_proxyPool[Settings.maxProxies - 1] = new Proxy();
m_proxyPool[Settings.maxProxies - 1].setNext(PairManager.NULL_PROXY);
m_proxyPool[Settings.maxProxies - 1].timeStamp = 0;
m_proxyPool[Settings.maxProxies - 1].overlapCount = INVALID;
m_proxyPool[Settings.maxProxies - 1].userData = null;
m_freeProxy = 0;
m_timeStamp = 1;
m_queryResultCount = 0;
}
public String toString() {
return "Pair: " + pm.getPair() + " checkCounter = " + checkCounter;
}
public void run() {
while (true) {
pm.doTask();
}
}
/**
* Get fingerprint similarity of input fingerprints
*
* @return fingerprint similarity object
*/
public FingerprintSimilarity getFingerprintsSimilarity() {
Map<Integer, Integer> offset_Score_Table =
new HashMap<Integer, Integer>(); // offset_Score_Table<offset,count>
int numFrames = 0;
float score = 0;
int mostSimilarFramePosition = Integer.MIN_VALUE;
// one frame may contain several points, use the shorter one be the
// denominator
if (fingerprint1.length > fingerprint2.length) {
numFrames = FingerprintManager.getNumFrames(fingerprint2);
} else {
numFrames = FingerprintManager.getNumFrames(fingerprint1);
}
// get the pairs
PairManager pairManager = new PairManager();
Map<Integer, List<Integer>> this_Pair_PositionList_Table =
pairManager.getPair_PositionList_Table(fingerprint1);
Map<Integer, List<Integer>> compareWave_Pair_PositionList_Table =
pairManager.getPair_PositionList_Table(fingerprint2);
for (int compareWaveHashNumber : compareWave_Pair_PositionList_Table.keySet()) {
// if the compareWaveHashNumber doesn't exist in both tables, no
// need to compare
if (!this_Pair_PositionList_Table.containsKey(compareWaveHashNumber)
|| !compareWave_Pair_PositionList_Table.containsKey(compareWaveHashNumber)) {
continue;
}
// for each compare hash number, get the positions
List<Integer> wavePositionList = this_Pair_PositionList_Table.get(compareWaveHashNumber);
List<Integer> compareWavePositionList =
compareWave_Pair_PositionList_Table.get(compareWaveHashNumber);
for (int thisPosition : wavePositionList) {
for (int compareWavePosition : compareWavePositionList) {
int offset = thisPosition - compareWavePosition;
if (offset_Score_Table.containsKey(offset)) {
offset_Score_Table.put(offset, offset_Score_Table.get(offset) + 1);
} else {
offset_Score_Table.put(offset, 1);
}
}
}
}
// map rank
MapRankInteger mapRank = new MapRankInteger(offset_Score_Table, false);
// get the most similar positions and scores
List<Integer> orderedKeyList = mapRank.getOrderedKeyList(100, true);
if (orderedKeyList.size() > 0) {
int key = orderedKeyList.get(0);
// get the highest score position
if (mostSimilarFramePosition == Integer.MIN_VALUE) {
mostSimilarFramePosition = key;
score = offset_Score_Table.get(key);
// accumulate the scores from neighbours
if (offset_Score_Table.containsKey(key - 1)) {
score += offset_Score_Table.get(key - 1) / 2;
}
if (offset_Score_Table.containsKey(key + 1)) {
score += offset_Score_Table.get(key + 1) / 2;
}
}
}
/*
* Iterator<Integer> orderedKeyListIterator=orderedKeyList.iterator();
* while (orderedKeyListIterator.hasNext()){ int
* offset=orderedKeyListIterator.next();
* System.out.println(offset+": "+offset_Score_Table.get(offset)); }
*/
score /= numFrames;
float similarity = score;
// similarity >1 means in average there is at least one match in every
// frame
if (similarity > 1) {
similarity = 1;
}
fingerprintSimilarity.setMostSimilarFramePosition(mostSimilarFramePosition);
fingerprintSimilarity.setScore(score);
fingerprintSimilarity.setSimilarity(similarity);
return fingerprintSimilarity;
}
public void commit() {
m_pairManager.commit();
}
// Call MoveProxy as many times as you like, then when you are done
// call Flush to finalized the proxy pairs (for your time step).
void moveProxy(int proxyId, AABB aabb) {
if (debugPrint) {
System.out.println("MoveProxy()");
}
if (proxyId == PairManager.NULL_PROXY || Settings.maxProxies <= proxyId) {
return;
}
assert (aabb.isValid()) : "invalid AABB";
int boundCount = 2 * m_proxyCount;
Proxy proxy = m_proxyPool[proxyId];
// Get new bound values
BoundValues newValues = new BoundValues();
computeBounds(newValues.lowerValues, newValues.upperValues, aabb);
// Get old bound values
BoundValues oldValues = new BoundValues();
for (int axis = 0; axis < 2; ++axis) {
oldValues.lowerValues[axis] = m_bounds[axis][proxy.lowerBounds[axis]].value;
oldValues.upperValues[axis] = m_bounds[axis][proxy.upperBounds[axis]].value;
}
for (int axis = 0; axis < 2; ++axis) {
Bound[] bounds = m_bounds[axis];
int lowerIndex = proxy.lowerBounds[axis];
int upperIndex = proxy.upperBounds[axis];
int lowerValue = newValues.lowerValues[axis];
int upperValue = newValues.upperValues[axis];
int deltaLower = lowerValue - bounds[lowerIndex].value;
int deltaUpper = upperValue - bounds[upperIndex].value;
bounds[lowerIndex].value = lowerValue;
bounds[upperIndex].value = upperValue;
//
// Expanding adds overlaps
//
// Should we move the lower bound down?
if (deltaLower < 0) {
int index = lowerIndex;
while (index > 0 && lowerValue < bounds[index - 1].value) {
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.proxyId;
Proxy prevProxy = m_proxyPool[prevBound.proxyId];
++prevBound.stabbingCount;
if (prevBound.isUpper() == true) {
if (testOverlap(newValues, prevProxy)) {
m_pairManager.addBufferedPair(proxyId, prevProxyId);
}
++prevProxy.upperBounds[axis];
++bound.stabbingCount;
} else {
++prevProxy.lowerBounds[axis];
--bound.stabbingCount;
}
--proxy.lowerBounds[axis];
// b2Swap(*bound, *prevEdge);
Bound tmp = new Bound(bound);
bound.set(prevBound);
prevBound.set(tmp);
--index;
}
}
// Should we move the upper bound up?
if (deltaUpper > 0) {
int index = upperIndex;
while (index < boundCount - 1 && bounds[index + 1].value <= upperValue) {
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.proxyId;
Proxy nextProxy = m_proxyPool[nextProxyId];
++nextBound.stabbingCount;
if (nextBound.isLower() == true) {
if (testOverlap(newValues, nextProxy)) {
m_pairManager.addBufferedPair(proxyId, nextProxyId);
}
--nextProxy.lowerBounds[axis];
++bound.stabbingCount;
} else {
--nextProxy.upperBounds[axis];
--bound.stabbingCount;
}
++proxy.upperBounds[axis];
// b2Swap(*bound, *nextEdge);
// wasn't actually swapping! bounds[index] and
// bounds[index+1] need to be swapped by VALUE
Bound tmp = new Bound(bound);
bound.set(nextBound);
nextBound.set(tmp);
++index;
}
}
//
// Shrinking removes overlaps
//
// Should we move the lower bound up?
if (deltaLower > 0) {
int index = lowerIndex;
while (index < boundCount - 1 && bounds[index + 1].value <= lowerValue) {
Bound bound = bounds[index];
Bound nextBound = bounds[index + 1];
int nextProxyId = nextBound.proxyId;
Proxy nextProxy = m_proxyPool[nextProxyId];
--nextBound.stabbingCount;
if (nextBound.isUpper()) {
if (testOverlap(oldValues, nextProxy)) {
m_pairManager.removeBufferedPair(proxyId, nextProxyId);
}
--nextProxy.upperBounds[axis];
--bound.stabbingCount;
} else {
--nextProxy.lowerBounds[axis];
++bound.stabbingCount;
}
++proxy.lowerBounds[axis];
// b2Swap(*bound, *nextEdge);
// Bound tmp = bound;
// bound = nextEdge;
// nextEdge = tmp;
Bound tmp = new Bound(bound);
bound.set(nextBound);
nextBound.set(tmp);
++index;
}
}
// Should we move the upper bound down?
if (deltaUpper < 0) {
int index = upperIndex;
while (index > 0 && upperValue < bounds[index - 1].value) {
Bound bound = bounds[index];
Bound prevBound = bounds[index - 1];
int prevProxyId = prevBound.proxyId;
Proxy prevProxy = m_proxyPool[prevProxyId];
--prevBound.stabbingCount;
if (prevBound.isLower() == true) {
if (testOverlap(oldValues, prevProxy)) {
m_pairManager.removeBufferedPair(proxyId, prevProxyId);
}
++prevProxy.lowerBounds[axis];
--bound.stabbingCount;
} else {
++prevProxy.upperBounds[axis];
++bound.stabbingCount;
}
--proxy.upperBounds[axis];
// b2Swap(*bound, *prevEdge);
// Bound tmp = bound;
// bound = prevEdge;
// prevEdge = tmp;
Bound tmp = new Bound(bound);
bound.set(prevBound);
prevBound.set(tmp);
--index;
}
}
}
if (s_validate) {
validate();
}
}
public void destroyProxy(int proxyId) {
assert (0 < m_proxyCount && m_proxyCount <= Settings.maxProxies);
Proxy proxy = m_proxyPool[proxyId];
assert (proxy.isValid());
int boundCount = 2 * m_proxyCount;
for (int axis = 0; axis < 2; ++axis) {
Bound[] bounds = m_bounds[axis];
int lowerIndex = proxy.lowerBounds[axis];
int upperIndex = proxy.upperBounds[axis];
int lowerValue = bounds[lowerIndex].value;
int upperValue = bounds[upperIndex].value;
// memmove(bounds + lowerIndex, bounds + lowerIndex + 1,
// (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
// memmove(bounds[lowerIndex + 1], bounds[lowerIndex],
// (upperIndex - lowerIndex) * sizeof(b2Bound));
System.arraycopy(
m_bounds[axis], lowerIndex + 1, m_bounds[axis], lowerIndex, upperIndex - lowerIndex - 1);
for (int i = 0; i < upperIndex - lowerIndex - 1; i++) {
m_bounds[axis][lowerIndex + i] = new Bound(m_bounds[axis][lowerIndex + i]);
}
// memmove(bounds + upperIndex-1, bounds + upperIndex + 1,
// (edgeCount - upperIndex - 1) * sizeof(b2Bound));
System.arraycopy(
m_bounds[axis],
upperIndex + 1,
m_bounds[axis],
upperIndex - 1,
boundCount - upperIndex - 1);
for (int i = 0; i < boundCount - upperIndex - 1; i++) {
m_bounds[axis][upperIndex - 1 + i] = new Bound(m_bounds[axis][upperIndex - 1 + i]);
}
// Fix bound indices.
for (int index = lowerIndex; index < boundCount - 2; ++index) {
Proxy proxyn = m_proxyPool[bounds[index].proxyId];
if (bounds[index].isLower()) {
proxyn.lowerBounds[axis] = index;
} else {
proxyn.upperBounds[axis] = index;
}
}
// Fix stabbing count.
for (int index = lowerIndex; index < upperIndex - 1; ++index) {
--bounds[index].stabbingCount;
}
// Query for pairs to be removed. lowerIndex and upperIndex are not
// needed.
int[] ignored = new int[2];
query(ignored, lowerValue, upperValue, bounds, boundCount - 2, axis);
}
assert (m_queryResultCount < Settings.maxProxies);
for (int i = 0; i < m_queryResultCount; ++i) {
assert (m_proxyPool[m_queryResults[i]].isValid());
m_pairManager.removeBufferedPair(proxyId, m_queryResults[i]);
}
m_pairManager.commit();
// Prepare for next query.
m_queryResultCount = 0;
incrementTimeStamp();
// Return the proxy to the pool.
proxy.userData = null;
proxy.overlapCount = BroadPhase.INVALID;
proxy.lowerBounds[0] = BroadPhase.INVALID;
proxy.lowerBounds[1] = BroadPhase.INVALID;
proxy.upperBounds[0] = BroadPhase.INVALID;
proxy.upperBounds[1] = BroadPhase.INVALID;
// Return the proxy to the pool.
proxy.setNext(m_freeProxy);
m_freeProxy = proxyId;
--m_proxyCount;
if (s_validate) {
validate();
}
}
// Create and destroy proxies. These call Flush first.
int createProxy(
AABB aabb, // int groupIndex, int categoryBits, int maskBits,
Object userData) {
if (debugPrint) {
System.out.println("CreateProxy()");
}
assert (m_proxyCount < Settings.maxProxies);
assert (m_freeProxy != PairManager.NULL_PROXY);
int proxyId = m_freeProxy;
Proxy proxy = m_proxyPool[proxyId];
m_freeProxy = proxy.getNext();
proxy.overlapCount = 0;
proxy.userData = userData;
// proxy.groupIndex = groupIndex;
// proxy.categoryBits = categoryBits;
// proxy.maskBits = maskBits;
// assert m_proxyCount < Settings.maxProxies;
int boundCount = 2 * m_proxyCount;
int lowerValues[] = new int[2];
int upperValues[] = new int[2];
computeBounds(lowerValues, upperValues, aabb);
for (int axis = 0; axis < 2; ++axis) {
Bound[] bounds = m_bounds[axis];
int[] indexes = new int[2];
query(indexes, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
int lowerIndex = indexes[0];
int upperIndex = indexes[1];
// System.out.println(edgeCount + ", "+lowerValues[axis] + ",
// "+upperValues[axis]);
// memmove(bounds[upperIndex + 2], bounds[upperIndex],
// (edgeCount - upperIndex) * sizeof(b2Bound));
System.arraycopy(
m_bounds[axis], upperIndex, m_bounds[axis], upperIndex + 2, boundCount - upperIndex);
for (int i = 0; i < boundCount - upperIndex; i++) {
m_bounds[axis][upperIndex + 2 + i] = new Bound(m_bounds[axis][upperIndex + 2 + i]);
}
// memmove(bounds[lowerIndex + 1], bounds[lowerIndex],
// (upperIndex - lowerIndex) * sizeof(b2Bound));
// System.out.println(lowerIndex+" "+upperIndex);
System.arraycopy(
m_bounds[axis], lowerIndex, m_bounds[axis], lowerIndex + 1, upperIndex - lowerIndex);
for (int i = 0; i < upperIndex - lowerIndex; i++) {
m_bounds[axis][lowerIndex + 1 + i] = new Bound(m_bounds[axis][lowerIndex + 1 + i]);
}
// The upper index has increased because of the lower bound
// insertion.
++upperIndex;
// Copy in the new bounds.
// if (bounds[lowerIndex] == null)
assert (bounds[lowerIndex] != null) : "Null pointer (lower)";
// if (bounds[upperIndex] == null)
assert (bounds[upperIndex] != null) : "Null pointer (upper)";
bounds[lowerIndex].value = lowerValues[axis];
bounds[lowerIndex].proxyId = proxyId;
bounds[upperIndex].value = upperValues[axis];
bounds[upperIndex].proxyId = proxyId;
bounds[lowerIndex].stabbingCount = lowerIndex == 0 ? 0 : bounds[lowerIndex - 1].stabbingCount;
bounds[upperIndex].stabbingCount = bounds[upperIndex - 1].stabbingCount;
// System.out.printf("lv: %d , lid: %d, uv: %d, uid: %d
// \n",lowerValues[axis],proxyId,upperValues[axis],proxyId);
// Adjust the stabbing count between the new bounds.
for (int index = lowerIndex; index < upperIndex; ++index) {
++bounds[index].stabbingCount;
}
// Adjust the all the affected bound indices.
for (int index = lowerIndex; index < boundCount + 2; ++index) {
Proxy proxyn = m_proxyPool[bounds[index].proxyId];
if (bounds[index].isLower()) {
proxyn.lowerBounds[axis] = index;
} else {
proxyn.upperBounds[axis] = index;
}
}
}
++m_proxyCount;
assert m_queryResultCount < Settings.maxProxies;
// Create pairs if the AABB is in range.
for (int i = 0; i < m_queryResultCount; ++i) {
assert (m_queryResults[i] < Settings.maxProxies);
assert (m_proxyPool[m_queryResults[i]].isValid());
m_pairManager.addBufferedPair(proxyId, m_queryResults[i]);
}
m_pairManager.commit();
if (s_validate) {
validate();
}
// Prepare for next query.
m_queryResultCount = 0;
incrementTimeStamp();
return proxyId;
}