// calculate the median of 3
int med(void **A, int a, int b, int c,
int (*compareXY ) (const void *, const void * ) ) {
return
compareXY( A[a], A[b] ) < 0 ?
( compareXY( A[b], A[c] ) < 0 ? b : compareXY( A[a], A[c] ) < 0 ? c : a)
: compareXY( A[b], A[c] ) > 0 ? b : compareXY( A[a], A[c] ) > 0 ? c : a;
} // end med
void siftDown(void **a, int start, int end) {
// input: end represents the limit of how far down the heap to sift.
int root = start;
int child, swapi;
// (While the root has at least one child)
while ( root * 2 + 1 <= end ) {
child = root * 2 + 1; // (root*2 + 1 points to the left child)
swapi = root; // (keeps track of child to swap with)
// (check if root is smaller than left child)
// if ( a[swapi] < a[child] )
if ( compareXY(a[swapi], a[child]) < 0 )
swapi = child;
// (check if right child exists, and if it's bigger
// than what we're currently swapping with)
// if ( child+1 <= end && a[swapi] < a[child+1] )
if ( child+1 <= end && compareXY(a[swapi],a[child+1]) < 0 )
swapi = child + 1;
// (check if we need to swap at all)
if ( swapi != root ) {
// swap(a[root], a[swapi]);
iswap(root, swapi, a);
root = swapi; // (repeat to continue sifting down the child now)
} else return;
}
} // end siftDown
infoActionType clapChezNousJaune(int option) {
static infoActionType infoAction = {ACTION_PAS_COMMENCEE, // statut de l'action
0, // Etape actuelle
4, // Nombre d'étapes
ACTION_ACTIONNEURS, // Type de l'étape
{0, 0, 0}}; // Destination de l'étape (si pertinent)
actionStatutType trajStatut;
enum {
GOTO_CLAP1_START = 0,
DEPLOIEMENT_BRAS = 1,
GOTO_CLAP2_END = 2,
REPLIEMENT_BRAS = 3,
GOTO_CLAP2_START = 4,
GOTO_CLAP1_END = 5,
FINI = 255
} etape = GOTO_CLAP1_START;
infoAction.statut = ACTION_PAS_COMMENCEE;
switch (option) {
case RESET_ACTION:
rentrerServoClap();
etape = FINI;
infoAction.statut = ACTION_FINIE;
break;
case DEFAULT_ACTION:
etape = GOTO_CLAP1_START;
infoAction.statut = ACTION_EN_COURS;
break;
case CLAP1_ONLY:
etape = GOTO_CLAP1_START;
infoAction.statut = ACTION_EN_COURS;
break;
case CLAP2_ONLY:
etape = GOTO_CLAP2_START;
infoAction.statut = ACTION_EN_COURS;
break;
default:
break;
}
while (etape != FINI) {
switch (etape) {
case GOTO_CLAP1_START:
trajStatut = trajectoryBasicAction(clap1StartPos);
if (trajStatut == ACTION_FINIE) {
etape = DEPLOIEMENT_BRAS;
} else if (trajStatut == ACTION_ERREUR) {
infoAction.statut = ACTION_REMISE;
etape = FINI;
}
break;
case GOTO_CLAP2_START:
trajStatut = trajectoryBasicAction(clap2StartPos);
if (trajStatut == ACTION_FINIE) {
etape = DEPLOIEMENT_BRAS;
} else if (trajStatut == ACTION_ERREUR) {
infoAction.statut = ACTION_REMISE;
etape = FINI;
}
break;
case DEPLOIEMENT_BRAS:
sortirServoClap();
// on ne met pas de délai, on suppose que le bras sera sorti à temps pdt le mouvment
if (option == CLAP1_ONLY) {
etape = GOTO_CLAP1_END;
} else {
etape = GOTO_CLAP2_END;
}
break;
case GOTO_CLAP1_END:
trajStatut = trajectoryBasicAction(clap1EndPos);
if (trajStatut == ACTION_FINIE) {
etape = REPLIEMENT_BRAS;
} else if (trajStatut == ACTION_ERREUR) {
infoAction.statut = ACTION_REMISE;
etape = FINI;
}
break;
case GOTO_CLAP2_END:
trajStatut = trajectoryBasicAction(clap2EndPos);
if (trajStatut == ACTION_FINIE) {
etape = REPLIEMENT_BRAS;
} else if (trajStatut == ACTION_ERREUR) {
infoAction.statut = ACTION_REMISE;
etape = FINI;
}
if (compareXY(propulsionGetPosition(), Fish_bordure)) { // on rentre le bras pour ne pas toucher le clap de l'adversaire
rentrerServoClap();
}
if (compareXY(propulsionGetPosition(), clap2StartPos)) { // on ressort le bras pour notre 2ème clap
sortirServoClap();
}
break;
case REPLIEMENT_BRAS:
rentrerServoClap();
infoAction.statut = ACTION_FINIE;
etape = FINI;
break;
default:
break;
}
}
return (infoAction);
}
// insertionsort for small segments where the loop has been unrolled
void insertionsort(int N, int M) {
if ( M <= N ) return;
// M <= N + 10
register int i, s;
int minimumPosition;
void *minimum, *next, *ai;
minimumPosition = N;
minimum = A[N];
for ( i = N+1; i <= M; i++ ) {
ai = A[i];
// if ( ai < minimum ) {
if ( compareXY(ai, minimum) < 0 ) {
minimum = ai;
minimumPosition = i;
}
}
if ( N != minimumPosition ) {
A[minimumPosition] = A[N];
A[N] = minimum;
}
s = N+1;
// if ( M == s ) return;
while ( s < M ) {
s=s+1;
next=A[s];
ai = A[s-1];
// if ( ai <= next ) continue;
if ( compareXY(ai, next) <= 0 ) continue;
A[s] = ai;
ai = A[s-2];
if ( compareXY(ai, next) <= 0 ) { A[s-1] = next; continue; }
A[s-1] = ai;
ai = A[s-3];
if ( compareXY(ai, next) <= 0 ) { A[s-2] = next; continue; }
A[s-2] = ai;
ai = A[s-4];
if ( compareXY(ai, next) <= 0 ) { A[s-3] = next; continue; }
A[s-3] = ai;
ai = A[s-5];
if ( compareXY(ai, next) <= 0 ) { A[s-4] = next; continue; }
A[s-4] = ai;
ai = A[s-6];
if ( compareXY(ai, next) <= 0 ) { A[s-5] = next; continue; }
A[s-5] = ai;
ai = A[s-7];
if ( compareXY(ai, next) <= 0 ) { A[s-6] = next; continue; }
A[s-6] = ai;
ai = A[s-8];
if ( compareXY(ai, next) <= 0 ) { A[s-7] = next; continue; }
A[s-7] = ai;
ai = A[s-9];
if ( compareXY(ai, next) <= 0 ) { A[s-8] = next; continue; }
A[s-8] = ai;
ai = A[s-10];
if ( compareXY(ai, next) <= 0 ) { A[s-9] = next; continue; }
A[s-9] = ai;
ai = A[s-11];
if ( compareXY(ai, next) <= 0 ) { A[s-10] = next; continue; }
A[s-10] = ai;
ai = A[s-12];
if ( compareXY(ai, next) <= 0 ) { A[s-11] = next; continue; }
fprintf(stderr, "FourSort/ insertionsort() ");
fprintf(stderr, "N: %d M: %d s: %d\n", N, M, s);
exit(1);
}
return;
} // end insertionsort
/* Three partition sorting with a single pivot p
Left elements are less than the pivot
Middle elements are equal to the pivot
Right elements are greater than the pivot
Only middle elements are guaranteed to be non-empty
This module is invoked on 'pathological' inputs */
void cut3duplicates(int N, int M, int pivotx, void (*cut)(), int depthLimit) {
// pivotx is pivot index inside [N,M] of pivot: p
register void *p = A[pivotx];
// indices
register int i, j, lw, up;
// values
register void *x, *y, *AM;
// initialize end points
x = A[N]; AM = A[M];
A[N] = A[M] = p;
/* We employ again whack-a-mole. We know in which partition element x
should be. Find a close, undecided position where x should go.
Store its content in y. Drop x (simplified). Set x to y and repeat.
*/
// initialize running indices
i = N;
j = M;
// if ( x < p ) goto StartL;
if ( compareXY(x, p) < 0 ) goto StartL;
// if ( p < x ) goto StartR;
if ( compareXY(p, x) < 0 ) goto StartR;
goto CreateMiddle;
StartL:
/*
|o---]-----------------------------[---o|
N i j M
x -> L
*/
i++;
y = A[i];
if ( compareXY(y, p) < 0 ) goto StartL;
// if ( p < y ) {
if ( compareXY(p, y ) < 0 ) {
if ( i < j ) {
A[i] = x;
x = y;
goto StartR;
}
// gap closed; i==j
/*
|o-----------][----------------------o|
N i M
x -> L j
*/
A[N] = x;
// AM must be p
A[M] = A[j];
A[j] = AM;
goto Finish;
}
// y = p -> M
if ( i < j ) {
A[i] = x;
x = y;
goto CreateMiddle;
}
/* We have:
x -> L
y -> M
i = j
N+i <= k < i -> A[k] < p
i = j <= k < M -> p < A[k]
Therefore:
x must go to A[N] and
p = A[M], the only location where y can go.
Hence: i = j = M
This cannot happen under normal use because there must be
more than one element equal to p when used by the other
members of Par/FourSort
|o------------------------------------]o|
N M=i=j
x -> L
*/
A[N] = x;
// AM must be p hence the next assignment is defensive
A[M] = AM;
i--;
goto FinishL;
StartR:
/*
|o---]-----------------------------[---o|
N i j M
x -> R
*/
j--;
y = A[j];
if ( compareXY(p, y ) < 0 ) goto StartR;
if ( compareXY(y, p) < 0 ) {
if ( i < j ) {
A[j] = x;
x = y;
goto StartL;
}
// gap closed; i==j
/*
|o-----------][----------------------o|
N i M
x -> R j
*/
A[M] = x;
// AM must be p
A[N] = A[i]; A[i] = AM;
goto Finish;
}
// y -> M
if ( i < j ) {
A[j] = x;
x = y;
goto CreateMiddle;
}
/* We have:
x -> R
y -> M
i = j
N+i <= k <= i -> A[k] < p
i = j < k < M -> p < A[k]
Therefore:
x must go to A[M] and
p = A[N], the only location where y can go.
Hence: i = j = N
This cannot happen under normal use because there must be
more than one element equal to p when used by the other
members of Par/FourSort.
|o][-----------------------------------o|
N=i=j M
x -> R
*/
A[M] = x;
// AM must be p
A[N] = AM;
j++;
goto FinishR;
CreateMiddle:
/*
|o---]-----------------------------[---o|
N i j M
x -> M
*/
/* This test can NOT be true:
all elements <= i are less than p
all elements >= j are greater than p
Thus: contradiction
if ( i+1 == j ) {
/////
|o--------------][---------------------o|
N ij M
x -> M
/////
// if ( AM < p ) {
if ( compareXY(AM, p) < 0 ) {
A[N] = AM;
A[M] = A[j]; A[j] = x;
i++;
goto Finish;
}
// if ( p < AM ) {
if ( compareXY(p, AM) < 0 ) {
A[M] = AM;
A[N] = A[i]; A[i] = x;
j--;
goto Finish;
}
// p = AM
A[N] = A[i]; A[i] = x;
A[M] = A[j]; A[j] = AM;
goto Finish;
}
*/
lw = (i+j)/ 2;
up = lw+1;
y = A[lw];
A[lw] = x;
x = y;
lw--;
/*
|o---]-----------+[-]+-------------[---o|
N i lw up j M
x -> ?
*/
// if ( x < p ){
if ( compareXY(x, p) < 0 ) {
goto LLgMgRL;
}
// if ( p < x ) {
if ( compareXY(p, x) < 0 ) {
goto LLgMgRR;
}
goto LLgMgRM1;
LLgMgRL:
/*
|o---]-----------+[-]+-------------[---o|
N i lw up j M
x -> L
*/
i++;
y = A[i];
if ( compareXY(y, p) < 0 ) goto LLgMgRL;
if ( compareXY(p, y ) < 0 ) {
A[i] = x;
x = y;
goto LLgMgRR;
}
// y -> M
if ( i <= lw ) {
A[i] = x;
x = y;
goto LLgMgRM1;
}
// left gap closed
/*
|o---------][------]+-------------[---o|
N i up j M
x -> L
*/
goto LLMgRL;
LLgMgRR:
/*
|o---]-----------+[-]+-------------[---o|
N i lw up j M
x -> R
*/
j--;
y = A[j];
if ( compareXY(p, y ) < 0 ) goto LLgMgRR;
if ( compareXY(y, p) < 0 ) {
A[j] = x;
x = y;
goto LLgMgRL;
}
// y -> M
if ( up <= j ) {
A[j] = x;
x = y;
goto LLgMgRM2;
}
// right gap closed
/*
|o---]-----------+[---------][---------o|
N i lw j M
x -> R
*/
goto LLgMRR;
LLgMgRM1:
/*
|o---]-----------+[-]+-------------[---o|
N i lw up j M
x -> M
*/
y = A[lw];
if ( compareXY(p, y ) < 0 ) {
A[lw] = x;
x = y;
lw--;
goto LLgMgRR;
}
if ( compareXY(p, y) == 0 ) {
if ( i < lw ) {
lw--;
goto LLgMgRM2;
}
goto LMgRM;
}
// y -> L
if ( i < lw ) {
A[lw] = x;
x = y;
lw--;
goto LLgMgRL;
}
// left gap closed
i++;
goto LLMgRM;
LLgMgRM2:
/*
|o---]-----------+[-]+-------------[---o|
N i lw up j M
x -> M
*/
y = A[up];
if ( compareXY(y, p) < 0 ) {
A[up] = x;
x = y;
up++;
goto LLgMgRL;
}
if ( compareXY(p, y) == 0 ) {
if ( up < j ) {
up++;
goto LLgMgRM1;
}
goto LLgMM;
}
// y -> R
if ( up < j ) {
A[up] = x;
x = y;
up++;
goto LLgMgRR;
}
// right gap closed
j--;
goto LLgMRM;
// left gap closed
LLMgRL:
/*
|o---------][------]+-------------[---o|
N i up j M
x -> L
*/
y = A[up];
if ( compareXY(y, p) < 0 ) {
A[up] = A[i]; A[i] = y;
up++; i++;
goto LLMgRL;
}
// if ( y == p ) {
if ( compareXY(p, y) == 0 ) {
if ( up == M ) {
/*
|o---------][-------------------]o|
N i M
x -> L
*/
A[N] = x;
if ( compareXY(p, AM) <= 0 ) {
A[M] = AM;
i--;
goto FinishL;
}
/*
// if ( p == AM ) {
if ( compareXY(p, AM) == 0 ) {
A[M] = AM;
i--;
goto FinishL;
}
*/
// compareXY(AM, p) < 0
A[M] = A[i]; A[i] = AM;
goto FinishL;
}
up++;
goto LLMgRL;
}
// y -> R
if ( up < j ) {
A[up] = A[i]; A[i] = x;
x = y;
i++; up++;
goto LLMgRR;
}
// right gap closed also
/*
|o---------][------][-----------------o|
N i j M
x -> L
*/
// up == j; A[j] == y
if ( compareXY(p, AM) < 0 ) {
A[N] = x; A[M] = AM;
j--;
goto Finish;
}
if ( compareXY(p, AM) == 0 ) {
A[N] = x; A[M] = y; A[j] = AM;
goto Finish;
}
// AM -> L and x -> L
A[M] = y; A[j] = A[i];
// if ( x <= AM ) {
if ( compareXY(x, AM ) <= 0 ) {
A[N] = x;
A[i] = AM;
} else {
A[N] = AM;
A[i] = x;
}
i++;
goto Finish;
LLMgRM:
/*
|o---------][------]+-------------[---o|
N i up j M
x -> M
*/
y = A[up];
if ( compareXY(y, p) < 0 ) {
A[up] = A[i]; A[i] = y;
i++; up++;
goto LLMgRM;
}
if ( compareXY(p, y) == 0 ) {
if ( up == M ) { // R empty
/*
|o---------][-------------------]o|
N i M
x -> M
*/
if ( compareXY(AM, p) < 0 ) {
A[N] = AM;
A[M] = x;
i--;
goto FinishL;
}
/*
if ( compareXY(p, AM) < 0 ) {
A[M] = AM;
i--;
A[N] = A[i]; A[i] = x;
i--;
goto FinishL;
}
*/
A[M] = AM;
i--;
A[N] = A[i]; A[i] = x;
i--;
goto FinishL;
}
up++;
goto LLMgRM;
}
// y -> R
if ( up < j ) {
A[up] = x;
x = y;
up++;
goto LLMgRR;
}
// right gap closed also
/*
|o---------][------][-----------------o|
N i j M
x -> M
*/
// up == j; A[j] == y
if ( compareXY(p, AM) < 0 ) {
A[M] = AM;
i--;
A[N] = A[i]; A[i] = x;
j--;
goto Finish;
}
if ( compareXY(AM, p) < 0 ) {
A[N] = AM; A[M] = y; A[j] = x;
goto Finish;
}
// AM -> M
i--;
A[N] = A[i]; A[i] = x;
A[M] = y; A[j] = AM;
goto Finish;
LLMgRR:
/*
|o---------][------]+-------------[---o|
N i up j M
x -> R
*/
j--;
y = A[j];
if ( compareXY(p, y ) < 0 ) goto LLMgRR;
if ( compareXY(y, p) < 0 ) {
A[j] = x;
x = y;
goto LLMgRL;
}
// y -> M
if ( up <= j ) {
A[j] = x;
x = y;
goto LLMgRM;
}
// right gap closed also y = A[j]
/*
|o---------][------][-----------------o|
N i j M
x -> R
*/
A[M] = x;
if ( compareXY(AM, p) < 0 ) {
A[N] = AM;
goto Finish;
}
if ( compareXY(p, AM) == 0 ) {
i--;
A[N] = A[i]; A[i] = AM;
goto Finish;
}
// AM -> R
i--;
A[N] = A[i]; A[i] = y; A[j] = AM;
j--;
goto Finish;
// right gap closed
LLgMRR:
/*
|o---]-----------+[---------][---------o|
N i lw j M
x -> R
*/
y = A[lw];
if ( compareXY(p, y ) < 0 ) {
A[lw] = A[j]; A[j] = y;
lw--; j--;
goto LLgMRR;
}
if ( compareXY(p, y) == 0 ) {
if ( N == lw ) { // L is empty
/*
|o[-----------------------][---------o|
N j M
x -> R
*/
A[M] = x;
if ( compareXY(AM, p) <= 0 ) {
A[N] = AM;
j++;
goto FinishR;
}
/*
if ( compareXY(p, AM) == 0 ) {
A[N] = AM;
j++;
goto FinishR;
}
*/
// compareXY(p, AM) < 0
A[N] = A[j]; A[j] = AM;
goto FinishR;
}
lw--;
goto LLgMRR;
}
// y -> L
if ( i < lw ) {
A[lw] = A[j]; A[j] = x;
x = y;
lw--; j--;
goto LLgMRL;
}
// left gap closed also
/*
|o---][---------------------][---------o|
N i j M
x -> R
*/
// y == A[lw] == A[i]
A[M] = x;
if ( compareXY(AM, p) < 0 ) {
A[N] = AM;
i++;
goto Finish;
}
if ( compareXY(p, AM) == 0 ) {
A[N] = y; A[i] = AM;
goto Finish;
}
// AM -> R
A[N] = y; A[i] = A[j]; A[j] = AM;
j--;
goto Finish;
LLgMRM:
/*
|o---]-----------+[---------][---------o|
N i lw j M
x -> M
*/
y = A[lw];
if ( compareXY(p, y ) < 0 ) {
A[lw] = A[j]; A[j] = y;
lw--; j--;
goto LLgMRM;
}
if ( compareXY(p, y) == 0 ) {
if ( N == lw ) { // L empty
/*
|o[-----------------------][---------o|
N j M
x -> M
*/
if ( compareXY(AM, p) <= 0 ) {
A[N] = AM;
j++;
A[M] = A[j];
A[j] = x;
j++;
goto FinishR;
}
/*
// if ( p == AM ) {
if ( compareXY(p, AM) == 0 ) {
A[N] = AM;
j++;
A[M] = A[j];
A[j] = x;
j++;
goto FinishR;
}
*/
// compareXY(p, AM) < 0
A[M] = AM; A[N] = x;
j++;
goto FinishR;
}
lw--;
goto LLgMRM;
}
// y -> L
if ( i < lw ) {
A[lw] = x;
x = y;
lw--;
goto LLgMRL;
}
// left gap closed also
/*
|o---][---------------------][---------o|
N i j M
x -> M
*/
// y == A[lw] == A[i]
if ( compareXY(p, AM) < 0 ) {
A[M] = AM;
A[N] = y; A[i] = x;
goto Finish;
}
if ( compareXY(AM, p) < 0 ) {
A[N] = AM;
j++;
A[M] = A[j]; A[j] = x;
i++;
goto Finish;
}
// AM == p
A[N] = y; A[i] = x;
j++;
A[M] = A[j]; A[j] = AM;
goto Finish;
LLgMRL:
/*
|o---]-----------+[---------][---------o|
N i lw j M
x -> L
*/
i++;
y = A[i];
if ( compareXY(y, p) < 0 ) goto LLgMRL;
if ( compareXY(p, y ) < 0 ) {
A[i] = x;
x = y;
goto LLgMRR;
}
// y -> M
if ( i <= lw ) {
A[i] = x;
x = y;
goto LLgMRM;
}
// left gap closed also y = A[i];
/*
|o---][----------------------][---------o|
N i j M
x -> L
*/
A[N] = x;
if ( compareXY(p, AM) < 0 ) {
A[M] = AM;
goto Finish;
}
if ( compareXY(p, AM) == 0 ) {
j++;
A[M] = A[j]; A[j] = AM;
goto Finish;
}
// AM -> L
j++;
A[M] = A[j]; A[j] = y; A[i] = AM;
i++;
goto Finish;
LMgRM:
/*
|o[---------------]+-------------[---o|
N up j M
x -> M
*/
y = A[up];
if ( compareXY(y, p) < 0 ) { // N == i
i++;
A[up] = A[i]; A[i] = y;
i++; up++;
goto LLMgRM;
}
if ( compareXY(p, y) == 0 ) {
if ( up == M ) {
/*
|o[----------------------------]o|
N M
x -> M
*/
if ( compareXY(AM, p) <= 0 ) {
A[N] = AM; A[M] = x;
goto Finish0;
}
/*
if ( compareXY(p, AM) == 0 ) {
A[N] = AM; A[M] = x;
goto Finish0;
}
*/
// compareXY(p, AM) < 0
A[N] = x; A[M] = AM;
goto Finish0;
}
up++;
goto LMgRM;
}
// compareXY(p, y ) < 0
if ( up < j ) {
A[up] = x;
x = y;
/*
|o[---------------]+-------------[---o|
N up j M
x -> R
*/
goto LMgRR;
}
// right gap closed y = A[up] = A[j] -> R
/*
|o[-----------------------------][---o|
N j M
x -> M
*/
if ( compareXY(AM, p) <= 0 ) {
A[N] = AM;
A[M] = y; A[j] = x;
j++;
goto FinishR;
}
/*
if ( compareXY(AM, p) < 0 ) {
A[N] = AM;
A[M] = A[j]; A[j] = x;
j++;
goto FinishR;
}
if ( compareXY(p, AM) == 0 ) {
A[N] = AM;
A[M] = A[j]; A[j] = x;
j++;
goto FinishR;
}
*/
// AM -> R
A[N] = x; A[M] = AM;
goto FinishR;
LLgMM:
/*
|o---]-----------+[------------------]o|
N i lw M
x -> M
*/
y = A[lw];
if ( compareXY(p, y ) < 0 ) {
j--;
A[lw] = A[j]; A[j] = y;
lw--;
j--;
goto LLgMRM;
}
if ( compareXY(p, y) == 0 ) {
if ( N == lw ) { // L empty also
/*
|o[----------------------------]o|
N M
x -> M
*/
if ( compareXY(AM, p) <= 0 ) {
A[N] = AM; A[M] = x;
goto Finish0;
}
/*
if ( compareXY(p, AM) == 0 ) {
A[N] = AM; A[M] = x;
goto Finish0;
}
*/
// AM -> R
A[N] = x; A[M] = AM;
goto Finish0;
}
lw--;
goto LLgMM;
}
// y -> L
if ( i < lw ) {
A[lw] = x;
x = y;
lw--;
goto LLgML;
}
// left gap closed also y = A[lw] = A[i] -> L
/*
|o---][-----------------------------]o|
N i M
x -> M
*/
if ( compareXY(AM, p) < 0 ) {
A[N] = AM; A[M] = x;
goto FinishL;
}
if ( compareXY(p, AM) == 0 ) {
A[N] = y; A[i] = AM; A[M] = x;
i--;
goto FinishL;
}
// compareXY(p, AM) < 0
A[M] = AM;
A[N] = y; A[i] = x;
i--;
goto FinishL;
LLgML:
/*
|o---]-----------+[------------------]o|
N i lw M=j
x -> L
*/
i++;
y = A[i];
if ( compareXY(y, p) < 0 ) goto LLgML;
if ( compareXY(p, y) == 0 ) {
if ( i <= lw ) {
A[i] = x;
x = y;
goto LLgMM;
}
// left gap closed y = A[i]
/*
|o--------------][------------------]o|
N i M=j
x -> L
*/
A[N] = x;
if ( compareXY(p, AM) <= 0 ) {
A[M] = AM;
i--;
goto FinishL;
}
/*
// if ( p == AM ) {
if ( compareXY(p, AM) == 0 ) {
A[M] = AM;
i--;
goto FinishL;
}
*/
// AM -> L
A[M] = y; A[i] = AM;
goto FinishL;
}
// y -> R
A[i] = x;
x = y;
goto LLgMR;
LLgMR:
/*
|o---]-----------+[------------------]o|
N i lw M=j
x -> R
*/
y = A[lw];
if ( compareXY(p, y ) < 0 ) {
j--;
A[lw] = A[j]; A[j] = y;
j--; lw--;
goto LLgMRR;
}
if ( compareXY(p, y) == 0 ) {
if ( N == lw ) { // L is empty
/*
|o[----------------------------]o|
N M=j
x -> R
*/
if ( compareXY(AM, p) <= 0 ) {
A[N] = AM;
A[M] = x;
goto Finish0;
}
/*
if ( compareXY(p, AM) == 0 ) {
A[N] = AM;
A[M] = x;
goto Finish0;
}
*/
// AM -> R
j--;// = M-1
A[N] = A[j];
if ( x <= AM ) {
A[j] = x; A[M] = AM;
} else {
A[j] = AM; A[M] = x;
}
goto Finish0;
}
lw--;
goto LLgMR;
}
// y -> L
if ( i < lw ) {
j--;
A[lw] = A[j]; A[j] = x;
x = y;
lw--; j--;
goto LLgMRL;
}
// left gap closed y = A[lw] = A[i] -> L
/*
|o---][----------------------------]o|
N i M=j
x -> R
*/
if ( compareXY(AM, p) < 0 ) {
A[N] = AM; A[M] = x;
goto FinishL;
}
if ( compareXY(p, AM) == 0 ) {
A[M] = x;
A[N] = y; A[i] = AM;
i--;
goto FinishL;
}
// AM -> R
j--;
A[N] = y; A[i] = A[j];
if ( x <= AM ) {
A[j] = x; A[M] = AM;
} else {
A[j] = AM; A[M] = x;
}
i--;
goto FinishL;
LMgRR:
/*
|o[---------------]+-------------[---o|
N up j M
x -> R
*/
j--;
y = A[j];
if ( compareXY(p, y ) < 0 ) goto LMgRR;
if ( compareXY(y, p) < 0 ) {
A[j] = x;
x = y;
/*
|o[---------------]+-------------[---o|
N up j M
x -> L
*/
goto LMgRL;
}
// y -> M
if ( up <= j ) {
A[j] = x;
x = y;
goto LMgRM;
}
// right gap closed y = A[j]
/*
|o[---------------][----------------o|
N j M
x -> R
*/
A[M] = x;
if ( compareXY(AM, p) <= 0 ) {
A[N] = AM;
j++;
goto FinishR;
}
/*
if ( compareXY(p, AM) == 0 ) {
A[N] = AM;
j++;
goto FinishR;
}
*/
// AM -> R
A[N] = A[j]; A[j] = AM;
goto FinishR;
LMgRL:
/*
|o[---------------]+-------------[---o|
N up j M
x -> L
*/
y = A[up];
if ( compareXY(y, p) < 0 ) { // create L
i++;
A[up] = A[i]; A[i] = y;
i++; up++;
goto LLMgRL;
}
if ( compareXY(p, y) == 0 ) {
if ( up == M ) { // R is empty
/*
|o[----------------------------]o|
N M
x -> L
*/
if ( compareXY(p, AM) <= 0 ) {
A[N] = x; A[M] = AM;
goto Finish0;
}
/*
if ( compareXY(p, AM) == 0 ) {
A[N] = x; A[M] = AM;
goto Finish0;
}
*/
// AM -> L
i++;
A[M] = A[i];
if ( x <= AM ) {
A[N] = x; A[i] = AM;
} else {
A[N] = AM; A[i] = x;
}
goto Finish0;
}
up++;
goto LMgRL;
}
// y -> R
if ( up < j ) {
i++;
A[up] = A[i]; A[i] = x;
x = y;
i++; up++;
goto LLMgRR;
}
// right gap closed y = A[up] = A[j]
/*
|o[-----------------------------][---o|
N j M
x -> L
*/
if ( compareXY(p, AM) < 0 ) {
A[N] = x; A[M] = AM;
goto FinishR;
}
if ( compareXY(p, AM) == 0 ) {
A[N] = x;
A[M] = A[j]; A[j] = AM;
j++;
goto FinishR;
}
// AM -> L
i++;
A[M] = A[j]; A[j] = A[i];
if ( x <= AM ) {
A[N] = x; A[i] = AM;
} else {
A[N] = AM; A[i] = x;
}
j--;
goto FinishR;
int k;
Finish0:
return;
FinishL:
(*cut)(N, i, depthLimit);
return;
FinishR:
(*cut)(j, M, depthLimit);
return;
Finish:
/* holes filled
|o---------][------][-----------------o|
N i j M
*/
/*
for (k = N; k < i; k++)
if ( p <= A[k] ) {
printf("Error L at k: %d \n", k);
exit(1);
}
for (k = i; k <= j; k++)
if ( p != A[k] ) {
printf("Error M at k: %d \n", k);
// printf("N: %d i: %d j: %d M: %d\n", N, i, j, M);
exit(1);
}
for (k = j+1; k <= M; k++)
if ( A[k] <= p ) {
printf("Error R at k: %d \n", k);
printf("N: %d i: %d j: %d M: %d\n", N, i, j, M);
exit(1);
}
*/
if ( i - N < M - j ) {
(*cut)(N, i-1, depthLimit);
(*cut)(j+1, M, depthLimit);
} else {
(*cut)(j+1, M, depthLimit);
(*cut)(N, i-1);
}
} // end of cut3duplicates
void cut2c(int N, int M, int depthLimit) {
int L;
Start:
if ( depthLimit <= 0 ) {
heapc(A, N, M);
return;
}
L = M - N;
if ( L < cut2Limit ) {
quicksort0c(N, M, depthLimit);
return;
}
depthLimit--;
// Check for duplicates
int sixth = (M - N + 1) / 6;
int e1 = N + sixth;
int e5 = M - sixth;
int e3 = (N+M) / 2; // The midpoint
int e4 = e3 + sixth;
int e2 = e3 - sixth;
// Sort these elements using a 5-element sorting network
void *ae1 = A[e1], *ae2 = A[e2], *ae3 = A[e3], *ae4 = A[e4], *ae5 = A[e5];
void *t;
// if (ae1 > ae2) { t = ae1; ae1 = ae2; ae2 = t; }
if ( compareXY(ae1, ae2) > 0 ) { t = ae1; ae1 = ae2; ae2 = t; }
if ( compareXY(ae4, ae5) > 0 ) { t = ae4; ae4 = ae5; ae5 = t; }
if ( compareXY(ae1, ae3) > 0 ) { t = ae1; ae1 = ae3; ae3 = t; }
if ( compareXY(ae2, ae3) > 0 ) { t = ae2; ae2 = ae3; ae3 = t; }
if ( compareXY(ae1, ae4) > 0 ) { t = ae1; ae1 = ae4; ae4 = t; }
if ( compareXY(ae3, ae4) > 0 ) { t = ae3; ae3 = ae4; ae4 = t; }
if ( compareXY(ae2, ae5) > 0 ) { t = ae2; ae2 = ae5; ae5 = t; }
if ( compareXY(ae2, ae3) > 0 ) { t = ae2; ae2 = ae3; ae3 = t; }
if ( compareXY(ae4, ae5) > 0 ) { t = ae4; ae4 = ae5; ae5 = t; }
A[e1] = ae1; A[e2] = ae2; A[e3] = ae3; A[e4] = ae4; A[e5] = ae5;
// Fix end points
if ( compareXY(ae1, A[N]) < 0 ) iswap(N, e1, A);
if ( compareXY(A[M], ae5) < 0 ) iswap(M, e5, A);
int duplicate = -1;
// if ( ae1, ae5 ) { duplicate = e1; } else
if ( compareXY(ae1, ae5) == 0 ) { duplicate = e1; } else
if ( compareXY(ae1, ae4) == 0 ) { duplicate = e1; } else
if ( compareXY(ae2, ae5) == 0 ) { duplicate = e2; } else
if ( compareXY(ae1, ae3) == 0 ) { duplicate = e1; } else
if ( compareXY(ae2, ae4) == 0 ) { duplicate = e2; } else
if ( compareXY(ae3, ae5) == 0 ) { duplicate = e3; } else
if ( compareXY(ae1, ae2) == 0 ) { duplicate = e1; } else
if ( compareXY(ae2, ae3) == 0 ) { duplicate = e2; } else
if ( compareXY(ae3, ae4) == 0 ) { duplicate = e3; } else
if ( compareXY(ae4, ae5) == 0 ) { duplicate = e4; };
if ( 0 <= duplicate ) {
void cut2c();
cut3duplicates(N, M, duplicate, cut2c, depthLimit);
return;
}
register void *T = ae3; // pivot
// test to play safe:
// if ( T <= A[N] || A[M] < T ) {
if ( compareXY(T, A[N]) <= 0 || compareXY(A[M], T) < 0 ) {
// give up because cannot find a good pivot
quicksort0c(N, M, depthLimit+1);
return;
}
register int I, J; // indices
register void *AI, *AJ; // array values
// initialize running indices
I= N;
J= M;
// The left segment has elements < T
// The right segment has elements >= T
Left:
I = I + 1;
AI = A[I];
// if (AI < T) goto Left;
if ( compareXY( AI, T) < 0 ) goto Left;
Right:
J = J - 1;
AJ = A[J];
// if ( T <= AJ ) goto Right;
if ( compareXY( T, AJ) <= 0 ) goto Right;
if ( I < J ) {
A[I] = AJ; A[J] = AI;
goto Left;
}
if ( (I - N) < (M - J) ) { // smallest one first
cut2c(N, J, depthLimit);
N = I;
goto Start;
}
cut2c(I, M, depthLimit);
M = J;
goto Start;
} // (* OF cut2; *) ... the brackets reminds that this was Pascal code
// Quicksort equipped with a defense against quadratic explosion;
// calling heapsort if depthlimit exhausted
void quicksort0c(int N, int M, int depthLimit) {
// printf("Enter quicksort0c N: %d M: %d %d\n", N, M, depthLimit);
while ( N < M ) {
int L = M - N;
if ( L <= 10 ) {
insertionsort(N, M);
return;
}
if ( depthLimit <= 0 ) {
heapc(A, N, M);
return;
}
depthLimit--;
// 10 < L
// grab median of 3 or 9 to get a good pivot
int pn = N;
int pm = M;
int p0 = (pn+pm)/2;
if ( 40 < L ) { // do median of 9
int d = L/8;
pn = med(A, pn, pn + d, pn + 2 * d, compareXY);
p0 = med(A, p0 - d, p0, p0 + d, compareXY);
pm = med(A, pm - 2 * d, pm - d, pm, compareXY);
/* Activation of the check for duplicates gives a slow down of
1/4% on uniform input. If you suspect that duplicates
causing quadratic deterioration are not caught higher-up by cut3
you may want to experiment with this check::::
if ( L < 100 ) { // check for duplicates
int duplicate = -1;
if ( compareXY(A[pn], A[pm]) == 0 ) { duplicate = pn; } else
if ( compareXY(A[pn], A[p0]) == 0 ) { duplicate = pn; } else
if ( compareXY(A[pm], A[p0]) == 0 ) { duplicate = pm; };
if ( 0 < duplicate ) {
void quicksort0();
cut3duplicates(N, M, duplicate, quicksort0, depthLimit);
return;
}
}
*/
}
p0 = med(A, pn, p0, pm, compareXY); // p0 is index to 'best' pivot ...
iswap(N, p0, A); // ... and is put in first position as required by quicksort0c
register void *p = A[N]; // pivot
register int i, j;
i = N;
j = M;
register void *ai; void *aj;
/* Split array A[N,M], N<M in two segments using pivot p;
construct a partition with A[N,i), A(i,M] and N <= i <= M, and
N <= k <= i -> A[k] <= p and i < k <= M -> p < A[k];
Allow the worse cases: N=i or i=M.
Recurse on A[N,i) and A(i,M) (or in the reverse order).
This code does NOT do swapping; instead it disposes
ai/aj in a hole created by setting aj/ai first.
*/
/* Start state:
|-------------------------------|
N=i j=M
N = i < j = M
N <= k < i -> A[k] <= p
N < j < k <= M -> p < A[k]
A[N] = p
N < i -> p < A[i]
*/
while ( i < j ) {
/*
|-------o---------------[--------|
N i j M
N <= i < j <= M
N <= k < i -> A[k] <= p
N < j < k <= M -> p < A[k]
A[N] <= p
N < i -> p < A[i]
p + A[N,i) + A(i,M] is a permutation of the input array
*/
aj = A[j];
while ( compareXY(p, aj) < 0 ) {
/*
|-------o---------------[--------|
N i j M
N = i < j <= M or N < i <= j <= M
N <= k < i -> A[k] <= p
N < j <= k <= M -> p < A[k]
A[N] <= p
N < i -> p < A[i}
p + A[N,i) + A(i,M] is a permutation of the input array
p < aj = A[j]
*/
j--;
aj = A[j];
}
/*
|-------o---------------[--------|
N i j M
N = i = j < M or N < i & = i-1 <= j <= M
N <= k < i -> A[k] <= p
j < k <= M -> p < A[k]
A[N] <= p
N < i -> p < A[i}
p + A[N,i) + A(i,M] is a permutation of the input array
aj = A[j] <= p
*/
if ( j <= i ) {
/*
|-------o-----------------------|
N i M
N = i = j < M or N < i & = i-1 = j < M
N <= k < i -> A[k] <= p
i < k <= M -> p < A[k]
A[N] <= p
p + A[N,i) + A(i,M] is a permutation of the input array
*/
break;
}
// i < j
A[i] = aj; // fill hole !
/*
|-------]---------------o--------|
N i j M
N <= i < j <= M
N <= k <= i -> A[k] <= p
j < k <= M -> p < A[k]
A[N] <= p
p + A[N,j) + A(j,M] is a permutation of the input array
aj = A[j] <= p
*/
i++; ai = A[i];
while ( i < j && compareXY(ai, p) <= 0 ) {
/*
|-------]---------------o--------|
N i j M
N < i < j <= M
N <= k <= i -> A[k] <= p
j < k <= M -> p < A[k]
A[N] <= p
p + A[N,j) + A(j,M] is a permutation of the input array
aj = A[j] <= p
*/
i++; ai = A[i];
}
if ( j <= i )
/*
|----------------------o--------|
N i=j M
N < i = j <= M
N <= k < i -> A[k] <= p
j < k <= M -> p < A[k]
A[N] <= p
p + A[N,j) + A(j,M] is a permutation of the input array
*/
break;
// i < j & p < ai = A[i]
A[j] = ai;
j--;
/*
|--------o--------------[--------|
N i j M
N < i <= j <= M
N <= k < i -> A[k] <= p
j < k <= M -> p < A[k]
A[N] <= p
N < i -> p < ai = A[i]
p + A[N,i) + A(i,M] is a permutation of the input array
*/
}
A[i] = p;
/*
|--------]----------------------|
N i M
N <= i <= M
N <= k <= i -> A[k] <= p
i < k <= M -> p < A[k]
A[N] <= p
A[N,i] + A(i,M] is a permutation of the input array
*/
// Recurse on the smallest one and iterate on the other one
int ia = i-1; int ib = i+1;
if ( i-N < M-i ) {
if ( N < ia ) quicksort0c(N, ia, depthLimit);
N = ib;
} else {
if ( ib < M ) quicksort0c(ib, M, depthLimit);
M = ia;
}
}
} // end of quicksort0c
void cut2S(int N, int M) {
int I, J; // indices
void *AI, *AJ; // array values
Loop:
if ( M - N <= cut2SLimit ) {
quicksort1(N, M);
return;
}
// Check for duplicates
int sixth = (M - N + 1) / 6;
int e1 = N + sixth;
int e5 = M - sixth;
int e3 = (N+M) / 2; // The midpoint
int e4 = e3 + sixth;
int e2 = e3 - sixth;
// Sort these elements using a 5-element sorting network
void *ae1 = A[e1], *ae2 = A[e2], *ae3 = A[e3], *ae4 = A[e4], *ae5 = A[e5];
void *t;
// if (ae1 > ae2) { t = ae1; ae1 = ae2; ae2 = t; }
if ( compareXY(ae1, ae2) > 0 ) { t = ae1; ae1 = ae2; ae2 = t; }
if ( compareXY(ae4, ae5) > 0 ) { t = ae4; ae4 = ae5; ae5 = t; }
if ( compareXY(ae1, ae3) > 0 ) { t = ae1; ae1 = ae3; ae3 = t; }
if ( compareXY(ae2, ae3) > 0 ) { t = ae2; ae2 = ae3; ae3 = t; }
if ( compareXY(ae1, ae4) > 0 ) { t = ae1; ae1 = ae4; ae4 = t; }
if ( compareXY(ae3, ae4) > 0 ) { t = ae3; ae3 = ae4; ae4 = t; }
if ( compareXY(ae2, ae5) > 0 ) { t = ae2; ae2 = ae5; ae5 = t; }
if ( compareXY(ae2, ae3) > 0 ) { t = ae2; ae2 = ae3; ae3 = t; }
if ( compareXY(ae4, ae5) > 0 ) { t = ae4; ae4 = ae5; ae5 = t; }
A[e1] = ae1; A[e2] = ae2; A[e3] = ae3; A[e4] = ae4; A[e5] = ae5;
// Fix end points
if ( compareXY(ae1, A[N]) < 0 ) iswap(N, e1, A);
if ( compareXY(A[M], ae5) < 0 ) iswap(M, e5, A);
int duplicate = -1;
// if ( ae1, ae5 ) { duplicate = e1; } else
if ( compareXY(ae1, ae5) == 0 ) { duplicate = e1; } else
if ( compareXY(ae1, ae4) == 0 ) { duplicate = e1; } else
if ( compareXY(ae2, ae5) == 0 ) { duplicate = e2; } else
if ( compareXY(ae1, ae3) == 0 ) { duplicate = e1; } else
if ( compareXY(ae2, ae4) == 0 ) { duplicate = e2; } else
if ( compareXY(ae3, ae5) == 0 ) { duplicate = e3; } else
if ( compareXY(ae1, ae2) == 0 ) { duplicate = e1; } else
if ( compareXY(ae2, ae3) == 0 ) { duplicate = e2; } else
if ( compareXY(ae3, ae4) == 0 ) { duplicate = e3; } else
if ( compareXY(ae4, ae5) == 0 ) { duplicate = e4; };
if ( 0 <= duplicate ) {
void cut2S();
cut3duplicates(N, M, duplicate, cut2S, 0);
return;
}
register void *T = ae3; // pivot
// initialize running indices
I= N;
J= M;
// The left segment has elements < T
// The right segment has elements >= T
// /*
Left:
I = I + 1;
AI = A[I];
// if (AI < T) goto Left;
if ( compareXY(AI, T) < 0 ) goto Left;
Right:
J = J - 1;
AJ = A[J];
// if ( T <= AJ ) goto Right;
if ( compareXY(T, AJ) <= 0 ) goto Right;
if ( I < J ) {
A[I] = AJ; A[J] = AI;
goto Left;
}
int left = I-N;
int right = M-J;
if ( left < right ) { // smallest one first
cut2S(N, J);
// cut2S(I, M);
N = I;
goto Loop;
}
cut2S(I, M);
// cut2S(N, J);
M = J;
goto Loop;
} // (* OF cut2S; *) ... the brackets remind that this was Pascal code
