/**
* Sorts the maximum end point for specified axis and adds overlapping pair if necessary.
*/
void SweepAndPrune::sortMaxUp(unsigned int axis, unsigned int endPointIndex, bool testOverlap)
{
BodyEndPoint *endPoint = &sortedEndPoints[axis][endPointIndex];
BodyEndPoint *nextEndPoint = &sortedEndPoints[axis][endPointIndex+1];
while(endPoint->getValue() > nextEndPoint->getValue())
{
//update indexes in bodies
if(nextEndPoint->isMin())
{
const int axis1 = (1 << axis) & 3;
const int axis2 = (1 << axis1) & 3;
if(testOverlap && isOverlap(endPoint->getBodyBox(), nextEndPoint->getBodyBox(), axis1, axis2))
{
createOverlappingPair(endPoint->getBodyBox(), nextEndPoint->getBodyBox());
}
nextEndPoint->getBodyBox()->min[axis]--;
}else
{
nextEndPoint->getBodyBox()->max[axis]--;
}
endPoint->getBodyBox()->max[axis]++;
//swap end points
BodyEndPoint tmp = *endPoint;
*endPoint = *nextEndPoint;
*nextEndPoint = tmp;
//increment
endPoint++;
nextEndPoint++;
}
}
/**
* Sorts the minimum end point for specified axis and removes overlapping pair if necessary.
*/
void SweepAndPrune::sortMaxDown(unsigned int axis, unsigned int endPointIndex, bool testOverlap)
{
BodyEndPoint *endPoint = &sortedEndPoints[axis][endPointIndex];
BodyEndPoint *prevEndPoint = &sortedEndPoints[axis][endPointIndex-1];
while(endPoint->getValue() < prevEndPoint->getValue())
{
//update indexes in bodies
if(prevEndPoint->isMin())
{
const int axis1 = (1 << axis) & 3;
const int axis2 = (1 << axis1) & 3;
if(testOverlap && isOverlap(endPoint->getBodyBox(), prevEndPoint->getBodyBox(), axis1, axis2))
{
removeOverlappingPair(endPoint->getBodyBox(), prevEndPoint->getBodyBox());
}
prevEndPoint->getBodyBox()->min[axis]++;
}else
{
prevEndPoint->getBodyBox()->max[axis]++;
}
endPoint->getBodyBox()->max[axis]--;
//swap end points
BodyEndPoint tmp = *endPoint;
*endPoint = *prevEndPoint;
*prevEndPoint = tmp;
//decrement
endPoint--;
prevEndPoint--;
}
}
/**
* @version 0.5-1 (Marek Gagolewski, 2015-02-14)
*/
StriByteSearchMatcher* StriContainerByteSearch::getMatcher(R_len_t i) {
if (i >= n && matcher && matcher->getPatternStr() == get(i).c_str()) {
// matcher reuse
}
else {
if (matcher) {
delete matcher;
matcher = NULL;
}
if (isCaseInsensitive())
matcher = new StriByteSearchMatcherKMPci(get(i).c_str(), get(i).length(), isOverlap());
else if (get(i).length() == 1)
matcher = new StriByteSearchMatcher1(get(i).c_str(), get(i).length(), isOverlap());
else if (get(i).length() < 16)
matcher = new StriByteSearchMatcherShort(get(i).c_str(), get(i).length(), isOverlap());
else
matcher = new StriByteSearchMatcherKMP(get(i).c_str(), get(i).length(), isOverlap());
}
return matcher;
}
static bool checkEdge(const CCPoint& edgePointA,const CCPoint& edgePointB,const Range& one,const Range& other)
{
//check top
CCPoint edge = ccp(edgePointB.x-edgePointA.x,edgePointB.y-edgePointA.y);
CCPoint prep=edge.getPerp();
float selfMin=0,selfMax=0,otherMin=0,otherMax=0;
projectRange(prep, one, &selfMin, &selfMax);
projectRange(prep, other, &otherMin, &otherMax);
return isOverlap(selfMin,selfMax,otherMin,otherMax);
}
bool Shape::isOverlap(const Shape& cShape, const Coordinate& cCoordinate) const {
//Check if any of the vertices in the given shape have the same values as ours
for (std::vector<Vertex*>::const_iterator iter = cShape.internals().begin() ; iter != cShape.internals().end() ; iter++) {
//Check to see if the current vertex overlaps
if (isOverlap(**iter + cCoordinate))
return true;
}
return false;
}
void Delaunay::rebucket(VHandle vh, VHandleVec& vhvec)
{
// get all outgoing half_edge around the vertex
HHandleVec hhvec;
// for (auto vhit = mesh.voh_begin(vh); vhit != mesh.voh_end(vh); vhit++ )
//{
// hhvec.push_back(*vhit);
//}
for(auto& hh : mesh.voh_range(vh))
{
hhvec.push_back(hh);
}
//for(auto& hh : mesh)
// get all face handles around the vertex
FHandleVec fhvec;
//for(auto &hh : hhvec)
//{
// fhvec.push_back(mesh.face_handle(hh));
//}
//for(auto& fh : mesh.vf_range(vh))
//{
// fhvec.push_back(fh);
//}
for(auto fit = mesh.vf_begin(vh); fit != mesh.vf_end(vh); fit++)
{
fhvec.push_back(*fit);
}
// following code lead to compiler crash:
//for(auto& fhi : mesh.vf_range(vh))
// fhvec.push_back(fhi);
// clear the incident faces' property
for(auto& fh : fhvec)
{
mesh.property(FaceToVertices, fh).clear();
}
// what's the diff up and down
// reset vertex's property
for (auto& vh : vhvec)
{
mesh.property(VertexToFace, vh).invalidate();
mesh.property(VertexToHEdge, vh).invalidate();
}
// for every vertex influenced, find new Face/Edge it belongs to
for(auto& vhi : vhvec)
{
// ENSURE(vh != vhi) and deal with points OVERLAP
if (isOverlap(vhi, vh))
{
continue;
}
// find new face it belongs to
for(auto& fh : fhvec)
{
if (isInTriangle(mesh.point(vhi), fh))
{
mesh.property(FaceToVertices, fh).push_back(vhi);
mesh.property(VertexToFace, vhi) = fh;
break;
}
// if (isOnTriangleEdges)
}
// the vertex is not IN any triangle
// check whether lies ON an edge of the triangles
if (!mesh.property(VertexToFace, vhi).is_valid())
{
for (auto &hhi : hhvec)
{
// find new edge it belongs to
HHandle hh_on;
HHandle hh_next = mesh.next_halfedge_handle(hhi);
if (isOnEdge(mesh.point(vhi), hhi))
{
hh_on = hhi;
}
else if (isOnEdge(mesh.point(vhi),hh_next))
{
hh_on = hh_next;
}
if (hh_on.is_valid())
{
mesh.property(VertexToHEdge, vhi) = hh_on;
FHandle fh = mesh.face_handle(hh_on);
mesh.property(FaceToVertices, fh).push_back(vhi);
mesh.property(VertexToFace, vhi) = fh;
break;
}
}
}
}
}
// Update rectangle set to avoid collision
// Input R is split and output as R1 and R2
void updateR(Rset& rect, Rset& R, const short m, const short n, Rset& R1, Rset& R2) {
short* B1lo = &R.lo[0]; short* B1hi = &R.hi[0];
short* E1lo = &R.lo[1]; short* E1hi = &R.hi[1];
short* B2lo = &R.lo[2]; short* B2hi = &R.hi[2];
short* E2lo = &R.lo[3]; short* E2hi = &R.hi[3];
short* l = &rect.lo[0]; short* r = &rect.lo[1];
short* t = &rect.lo[2]; short* b = &rect.lo[3];
copyR(R, R1);
copyR(R, R2);
// upper right corner of rect is in the bottom left region of R
if (ismember<short>(*r, *B1lo, *B1hi) && ismember<short>(*t, *E2lo, *E2hi)) {
if (*r==m) // r reaches the last frame
R1.setValid(false);
else // Update B1(1) to r+1
R1.lo[0] = *r+1;
if (*t==1) // t is the 1st frame
R2.setValid(false);
else // Update E2(2) to t-1
R2.hi[3] = *t-1;
}
// bottom right corner of rect is in the top left region of R
else if (ismember<short>(*r, *B1lo, *B1hi) && ismember<short>(*t, *B2lo, *B2hi)) {
if (*r==m) // r reaches the last frame
R1.setValid(false);
else // Update B1(1) to r+1
R1.lo[0] = *r+1;
if (*b==n) // b reaches the last frame
R2.setValid(false);
else // Update B2(1) to b+1
R2.lo[2] = *b+1;
}
// bottom left corner of rect is in the top right region of R
else if (ismember<short>(*l, *E1lo, *E1hi) && ismember<short>(*b, *B2lo, *B2hi)) {
if (*l==1) // l is the first frame
R1.setValid(false);
else // Update E1(2) to l-1
R1.hi[1] = *l-1;
if (*b==n) // b reaches the last frame
R2.setValid(false);
else // Update B2(1) to b+1
R2.lo[2] = *b+1;
}
// top left corner of rect is in the bottom right region of R
else if (ismember<short>(*l, *E1lo, *E1hi) && ismember<short>(*t, *E2lo, *E2hi)) {
if (*l==1) // l is the first frame
R1.setValid(false);
else // Update E1(2) to l-1
R1.hi[1] = *l-1;
if (*t==1) // t is the first frame
R2.setValid(false);
else // Update E2(2) to t-1
R2.hi[3] = *t-1;
}
else if (ismember<short>(*b, *B2lo, *B2hi)) {
R2.setValid(false);
if (*b==n) // b reaches the last frame
R1.setValid(false);
else // Update B2(1) to b+1
R1.lo[2] = *b+1;
}
else if (ismember<short>(*r, *B1lo, *B1hi)) {
R2.setValid(false);
if (*r==m) // r reaches the last frame
R1.setValid(false);
else // Update B1(1) to r+1
R1.lo[0] = *r+1;
}
else if (ismember<short>(*t, *E2lo, *E2hi)) {
R2.setValid(false);
if (*t==1) // t is the first frame
R1.setValid(false);
else // Update E2(2) to t-1
R1.hi[3] = *t-1;
}
else if (ismember<short>(*l, *E1lo, *E1hi)) {
R2.setValid(false);
if (*l==1) // l is the first frame
R1.setValid(false);
else // Update E1(2) to l-1
R1.hi[1] = *l-1;
}
else if (*l>*E1hi || *t>*E2hi || *r<*B1lo || *b<*B2lo) {// keep R1
R2.setValid(false);
}
else {
R1.setValid(false);
R2.setValid(false);
}
// Double check the overlap
if (isOverlap(rect, R1) && R1.isValid()) {
mexPrintf("rect: ");rect.disp();
mexPrintf("R1: ");R1.disp();
return;
}
if (isOverlap(rect, R2) && R2.isValid()) {
mexPrintf("rect: ");rect.disp();
mexPrintf("R2: ");R2.disp();
return;
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
// Input checks
if (nrhs < 3) {
printUsg();
return;
}
#define S1 prhs[0]
#define S2 prhs[1]
if(!IS_REAL_2D(S1)) // check S1
mexErrMsgTxt("S1 must be a real 2D matrix.");
if(!IS_REAL_2D(S2)) // check S2
mexErrMsgTxt("S2 must be a real 2D matrix.");
// init
float* S1r = (float*)mxGetPr(S1);
float* S2r = (float*)mxGetPr(S2);
short B = static_cast<short>(mxGetM(S1)); // num of bins
short m = static_cast<short>(mxGetN(S1))-1; // length of Seq1
short n = static_cast<short>(mxGetN(S2))-1; // length of Seq2
// Get parameters
unsigned int maxIter;
short nRes;
short L;
string dist;
bool isVerbose;
getParams(nrhs, prhs, L, maxIter, nRes, dist, isVerbose);
Rset R1, R2; // Rectangle sets for splitting
float lb, ub; // lower and upper bounds
State topS; // top state
priority_queue<State, vector<State>, CompareState> Q; // priority queue
priority_queue<State, vector<State>, CompareState> Qtmp; // priority queue
if (dist.compare("l1")!=0 && dist.compare("l2")!=0 && dist.compare("X2")!=0 && dist.compare("int")!=0) {
mexPrintf("!!! We do not support \"%s\" distance. Please read the usage carefully!\n",dist.c_str());
mexPrintf("!!! Please select from these distance options: [\"l1\",\"l2\",\"X2\",\"int\"]\n");
return;
}
// Init output structure
int dimOut1[2] = {1,nRes};
int numFields1 = 3;
const char* fieldNames1[] = { "lo", "hi", "lb" };
plhs[0] = mxCreateStructArray(2, dimOut1, numFields1, fieldNames1); // creates fields
int dimOut2[2] = {1,nRes};
int numFields2 = 1;
const char* fieldNames2[] = { "nIter" };
plhs[1] = mxCreateStructArray(2, dimOut2, numFields2, fieldNames2); // creates fields
// Main loop below
mexPrintf("===============================================\n");
mexPrintf("+ Running TCD w/ %s distance\n", dist.c_str());
mexPrintf("===============================================\n");
for (short iRes=0; iRes<nRes; iRes++) {
// init
unsigned int nIter = 0; // counter for #iteration
bool isConv = false; // flag for convergence
Rset Rout = { {0,0,0,0}, {0,0,0,0}, false };
// Update Q
Rset R;
if (iRes == 0) {
Rset R0 = { {1,1,1,1}, {m,m,n,n}, true }; // init R with everything
R = R0;
} else {
Rout = topS.R;
mexPrintf("+ Updating Q (size=%d -> ", Q.size());
// Clean Qtmp
while (!Qtmp.empty())
Qtmp.pop();
// Copy Q into Qtmp and clean Q
while(!Q.empty()) {
Qtmp.push(Q.top());
Q.pop();
}
// Being to update Q
State tmpS;
while(!Qtmp.empty()) {
tmpS = Qtmp.top();
Qtmp.pop();
// Update R into R1 and R2
Rset R1, R2;
updateR(topS.R, tmpS.R, m, n, R1, R2);
// Push {bound(R1), R1} to Q
getBounds(S1r, S2r, B, R1, lb, ub, dist);
if (R1.isValid()) {
State St1 = {lb, R1};
Q.push(St1);
}
// Push {bound(R2), R2} to Q
getBounds(S1r, S2r, B, R2, lb, ub, dist);
if (R2.isValid()) {
State St2 = {lb, R2};
Q.push(St2);
}
}
mexPrintf("%d)\n",Q.size());
// Get topS
topS = Q.top();
R = topS.R;
Q.pop();
}
while (!isConv) {
nIter++;
// Split rect set into R1 and R2
split(R, R1, R2);
// Push R1 into queue if valid
if (isvalid(R1, L)) {
getBounds(S1r, S2r, B, R1, lb, ub, dist);
State S = {lb, R1};
Q.push(S);
}
// Push R2 into queue if valid
if (isvalid(R2, L)) {
getBounds(S1r, S2r, B, R2, lb, ub, dist);
State S = {lb, R2};
Q.push(S);
}
// Update R by top state
topS = Q.top();
R = topS.R;
Q.pop();
if (isOverlap(Rout, R)) {
mexPrintf("Rout: "); Rout.disp();
mexPrintf("R: "); R.disp();
return;
}
// Check stopping criterion
if (nIter % 100 == 0 && isVerbose) {
mexPrintf(" # Iter%4d: ",nIter);
topS.disp();
}
if (nIter >= maxIter) {
mexPrintf(" # MaxIter%d reached: ", maxIter);
topS.disp();
isConv = true;
}
if (isUniqR(R)) {
mexPrintf(" # Iter%4d: ",nIter);
topS.disp();
isConv = true;
}
} // end of main loop
// Assign output structure
setField<short>(&plhs[0], iRes, "lo", 4, mxINT16_CLASS, topS.R.lo); // set lo
setField<short>(&plhs[0], iRes, "hi", 4, mxINT16_CLASS, topS.R.hi); // set hi
setField<float>(&plhs[0], iRes, "lb", 1, mxSINGLE_CLASS, &topS.bound); // set lb
setField<unsigned int>(&plhs[1], iRes, "nIter", 1, mxUINT16_CLASS, &nIter); // set info.nIter
} // end of iRes
return;
}
