priq.c

两幅图像匹配算法

/*
 *
 *	$Header: /usr/u/wjr/src/ADT/RCS/priq.c,v 1.10 1994/09/01 23:12:20 wjr Exp $
 *
 * Copyright (c) 1990, 1991, 1992, 1993 Cornell University.  All Rights
 * Reserved.
 *
 * Copyright (c) 1991, 1992 Xerox Corporation.  All Rights Reserved.
 *
 * Use, reproduction and preparation of derivative works of this software is
 * permitted.  Any copy of this software or of any derivative work must
 * include both the above copyright notices of Cornell University and Xerox
 * Corporation and this paragraph.  This software is made available AS IS, and
 * XEROX CORPORATION DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
 * WITHOUT LIMITATION THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE, AND NOTWITHSTANDING ANY OTHER PROVISION CONTAINED
 * HEREIN, ANY LIABILITY FOR DAMAGES RESULTING FROM THE SOFTWARE OR ITS USE IS
 * EXPRESSLY DISCLAIMED, WHETHER ARISING IN CONTRACT, TORT (INCLUDING
 * NEGLIGENCE) OR STRICT LIABILITY, EVEN IF XEROX CORPORATION IS ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGES.
 */
/*
 * Xerox may use this code without restriction.
 */

static char *rcsid = "@(#)$Header: /usr/u/wjr/src/ADT/RCS/priq.c,v 1.10 1994/09/01 23:12:20 wjr Exp $";

/*
 *	priq.c - Priority Queues
 *
 * This module handles generic priority queues.
 *
 * Exports:
 *	type PriQ
 *
 *	constant PriQ NULLPRIQ
 *	constant ListNode NULLLISTNODE
 *
 *	PriQ pqNew(boolean (*compareGT)(void *a, void *b)) - create a new,
 *		empty priority queue
 *
 *	void pqFree(PriQ p) - free priority queue p
 *
 *	PriQNode pqFirst(PriQ p) - return the first entry in p
 *
 *	PriQNode pqAdd(PriQ p, void *userdata) - add a node containing
 *		userdata to p
 *
 *	void pqRem(PriQNode pqn) - remove pqn from the queue containing it
 *
 *	void *pqGet(PriQNode pqn) - return pqn's userdata
 *
 *	unsigned int pqLen(PriQ p) - return the number of nodes in p
 *
 * An object of type PriQ is used as a handle on a priority queue.
 *	Each element of the queue contains a void *
 *	field, which is used for whatever data the user wishes to attach
 *	to that node.
 *
 * A PriQNode refers to an entry in a priority queue.
 *
 * pqNew creates an empty queue. If this is not possible, it returns NULLPRIQ.
 *	The function which it takes is used as the comparison function to
 *	sort the queue. It should be declared as
 *	boolean compareGT(void *a, void *b)
 *	and it returns FALSE if "a" <= "b" and TRUE if "a" > "b".
 *	It does not compare the pointers themselves (priq could do that
 *	itself), but rather compares the user data values associated with
 *	those pointers in whatever way the user desires (which should be
 *	a total order). It will be called as required to ensure that
 *	pqFirst returns the smallest valued entry.
 *
 * pqFree frees the storage associated with a priority queue.
 *	Note that it does not free the userdata; the user
 *	must do this before freeing the queue structure.
 *
 * pqFirst returns the first (smallest) entry of the given queue.
 *	It does not guarantee any order among equal elements.
 *	If the queue is empty, it returns NULLPRIQNODE.
 *
 * pqAdd adds an entry to the given priority queue. It returns the new
 *	PriQNode. If it could not allocate needed storage, it returns
 *	NULLPRIQNODE.
 *
 * pqRem removes the given PriQNode from its PriQ. The PriQNode is then
 *	invalid.
 *
 * pqGet returns the userdata value of some PriQNode.
 *
 * pqLen returns the number of nodes in a given PriQ (i.e. the number of
 *	successful pqAdds minus the number of pqRems).
 *
 * Changing the values pointed to by the userdata pointers may result
 *	in pqFirst not returning the smallest current value. To change these,
 *	perform a pqRem and a pqAdd.
 *
 * pqAdd, pqRem and other operations involving PriQNodes may be arbitrarily
 *	interleaved. A PriQNode is valid from the moment pqAdd first returns
 *	it until it is passed into pqRem, or its containing PriQ is passed
 *	to pqFree.
 *
 */

#include "misc.h"
#include "priq.h"
#include "chunk.h"

#define	NULLPRIQINTNODE	((PriQIntNode)NULL)

static void swap(PriQIntNode pqin1, PriQIntNode pqin2);

/*
 * This module uses the Chunk service to allocate and free various things
 * efficiently.
 */
static PriQNode malloc_pqn(void);
static void free_pqn(PriQNode pqn);
static PriQIntNode malloc_pqin(void);
static void free_pqin(PriQIntNode pqin);

static Chunk pqnChunk = NULLCHUNK;
static Chunk pqinChunk = NULLCHUNK;

PriQ
pqNew(boolean (*compareGT)(void *a, void *b))
{
    PriQ newPriQ;

    assert(compareGT != (boolean (*)(void *a, void *b))NULL);
    if ((newPriQ = (PriQ)malloc(sizeof(* newPriQ))) == NULL) {
	return(NULLPRIQ);
	}
    newPriQ->root = newPriQ->last = NULLPRIQINTNODE;
    newPriQ->compareGT = compareGT;
    newPriQ->count = 0;
	   
    return(newPriQ);
    }

void
pqFree(PriQ p)
{
    PriQIntNode pqin, nextpqin;

    assert(p != NULLPRIQ);
    for (pqin = p->root; pqin != NULLPRIQINTNODE; pqin = nextpqin) {
	nextpqin = pqin->next;
	free_pqn(pqin->owner);
	free_pqin(pqin);
	}
    free((void *)p);
    }

PriQNode
pqFirst(PriQ p)
{
    assert(p != NULLPRIQ);
    if (p->root == NULLPRIQINTNODE) {
	return(NULLPRIQNODE);
	}
    else {
	return(p->root->owner);
	}
    }

void
pqRem(PriQNode pqn)
{
    PriQIntNode pqlast;
    PriQIntNode smallchild;
    PriQIntNode pqin;
    PriQ p;

    assert(pqn != NULLPRIQNODE);
    assert(pqn->val != NULLPRIQINTNODE);
    assert(pqn->val->owner == pqn);
    p = pqn->owner;
    assert(p != NULLPRIQ);
    assert(p->root != NULLPRIQINTNODE);
    assert(p->count > 0);

    /* Was that the last thing in the queue? */
    if (p->root == p->last) {
	assert(pqn->val == p->root);
	assert(p->count == 1);
	free_pqin(p->root);
	free_pqn(pqn);
	p->root = p->last = NULLPRIQINTNODE;

	p->count--;
	return;
	}

    if (pqn->val == p->last) {
	/*
	 * They're about to remove the last element. Don't need
	 * to reheapify - just patch the data structures
	 */
	pqlast = p->last;
	assert(pqlast->parent != NULLPRIQINTNODE);
	if (pqlast->parent->right == pqlast) {
	    pqlast->parent->right = NULLPRIQINTNODE;
	    }
	else {
	    assert(pqlast->parent->left == pqlast);
	    pqlast->parent->left = NULLPRIQINTNODE;
	    }
	assert(pqlast->prev != NULLPRIQINTNODE);
	pqlast->prev->next = NULLPRIQINTNODE;
	p->last = pqlast->prev;
	free_pqin(pqlast);
	free_pqn(pqn);

	p->count--;
	return;
	}

    /* Now, move the last element up to the to-be deleted element */
    pqlast = p->last;
    pqn->val->userdata = pqlast->userdata;
    assert(pqlast->owner != NULLPRIQNODE);
    assert(pqlast->owner->val == pqlast);
    pqlast->owner->val = pqn->val;
    pqn->val->owner = pqlast->owner;

    /* and delete the last element structure */
    assert(pqlast->parent != NULLPRIQINTNODE);	/*
						 * if it was the root,
						 * it was handled above
						 */
    /* which child was it? */
    if (pqlast->parent->right == pqlast) {
	pqlast->parent->right = NULLPRIQINTNODE;
	}
    else {
	assert(pqlast->parent->left == pqlast);
	pqlast->parent->left = NULLPRIQINTNODE;
	}

    /* if it was the root as well, it was handled above */
    assert(pqlast->prev != NULLPRIQINTNODE);
    pqlast->prev->next = NULLPRIQINTNODE;
    p->last = pqlast->prev;
    free_pqin(pqlast);

    /* Now reheapify. */
    /*
     * We have just taken some random value and stuck it into the middle
     * of the heap. It might have to move up or down
     */
    pqin = pqn->val;
    if ((pqin->parent == NULLPRIQINTNODE) ||
	((*(p->compareGT))(pqin->userdata, pqin->parent->userdata))) {
	/* Reheapify down, if at all */
	while (TRUE) {
	    if (pqin->left == NULLPRIQINTNODE) {
		/* Hit the bottom */
		break;
		}

	    /* Find which child has the smaller value */
	    if ((pqin->right == NULLPRIQINTNODE) ||
		((*(p->compareGT))(pqin->right->userdata,
				   pqin->left->userdata))) {
		smallchild = pqin->left;
		}
	    else {
		smallchild = pqin->right;
		}

	    /* Swap if necessary */
	    if ((*(p->compareGT))(pqin->userdata, smallchild->userdata)) {
		/* Swap the two */
		swap(pqin, smallchild);
		pqin = smallchild;
		}
	    else {
		/* Not smaller - heapification has been done */
		break;
		}
	    }
	}
    else {
	/* Need to reheapify upwards */
	while (TRUE) {
	    if ((pqin->parent != NULLPRIQINTNODE) &&
		((*(p->compareGT))(pqin->parent->userdata, pqin->userdata))) {
		/* Swap it and its parent */
		swap(pqin, pqin->parent);
		pqin = pqin->parent;
		}
	    else {
		break;
		}
	    }
	}

    /* and get rid of the PriQNode */
    free_pqn(pqn);

    /* and adjust the count */
    p->count--;
    }

PriQNode
pqAdd(PriQ p, void *userdata)
{
    PriQIntNode pqin;
    PriQNode newpqn;
    PriQIntNode newpqin;

    assert(p != NULLPRIQ);
    if ((newpqn = (PriQNode)malloc_pqn()) == NULLPRIQNODE) {
	return(NULLPRIQNODE);
	}

    if ((newpqin = (PriQIntNode)malloc_pqin()) == NULLPRIQINTNODE) {
	free_pqn(newpqn);
	return(NULLPRIQNODE);
	}

    newpqin->left = newpqin->right = newpqin->next = NULLPRIQINTNODE;
    newpqin->owner = newpqn;
    newpqin->userdata = userdata;
    newpqn->val = newpqin;
    newpqn->owner = p;

    if (p->root == NULLPRIQINTNODE) {
	/* The first thing in the queue */
	assert(p->count == 0);

	p->root = p->last = newpqin;
	newpqin->prev = NULLPRIQINTNODE;
	newpqin->parent = NULLPRIQINTNODE;
	p->count++;
	return(newpqn);
	}

    assert(p->count > 0);

    newpqin->prev = p->last;
    if (p->last->parent == NULLPRIQINTNODE) {
	/* The last element is also the root */
	assert(p->last == p->root);
	p->last->left = newpqin;
	newpqin->parent = p->last;
	}
    else if (p->last->parent->right == NULLPRIQINTNODE) {
	/* The last entry's parent has an empty slot - use it */
	p->last->parent->right = newpqin;
	newpqin->parent = p->last->parent;
	}
    else {
	assert(p->last->parent->next != NULLPRIQINTNODE);
	assert(p->last->parent->next->left == NULLPRIQINTNODE);
	p->last->parent->next->left = newpqin;
	newpqin->parent = p->last->parent->next;
	}
    p->last->next = newpqin;
    p->last = newpqin;

    /* Now reheapify */
    for (pqin = p->last; ; pqin = pqin->parent) {
	if ((pqin->parent != NULLPRIQINTNODE) &&
	    ((*(p->compareGT))(pqin->parent->userdata, pqin->userdata))) {
	    /* Swap it and its parent */
	    swap(pqin, pqin->parent);
	    }
	else {
	    break;
	    }
	}

    /* Successfully added */
    p->count++;
    return(newpqn);
    }

void *
pqGet(PriQNode pqn)
{
    assert(pqn != NULLPRIQNODE);
    assert(pqn->val != NULLPRIQINTNODE);
    return(pqn->val->userdata);
    }

unsigned int
pqLen(PriQ p)
{
    assert(p != NULLPRIQ);
    return(p->count);
    }

static void
swap(PriQIntNode pqin1, PriQIntNode pqin2)
{
    void *temp;
    PriQNode tempqn;

    assert(pqin1 != NULLPRIQINTNODE);
    assert(pqin2 != NULLPRIQINTNODE);

    /* Swap the values */
    temp = pqin1->userdata;
    pqin1->userdata = pqin2->userdata;
    pqin2->userdata = temp;

    /* Now, swap the owner's pointers to them */
    assert(pqin1->owner != NULLPRIQNODE);
    assert(pqin2->owner != NULLPRIQNODE);
    assert(pqin1->owner->val == pqin1);
    assert(pqin2->owner->val == pqin2);

    tempqn = pqin1->owner;
    pqin1->owner = pqin2->owner;
    pqin2->owner = tempqn;
    pqin1->owner->val = pqin1;
    pqin2->owner->val = pqin2;
    }

/*
 * This routine gets a chunk of memory suitable for use as a PriQNode
 */
static PriQNode
malloc_pqn(void)
{
    PriQNode pqn;

    /* Allocate the Chunk if it hasn't been already */
    if (pqnChunk == NULLCHUNK) {
	pqnChunk = chNew(sizeof(*pqn));
	if (pqnChunk == NULLCHUNK) {
	    return(NULLPRIQNODE);
	    }
	}

    pqn = (PriQNode)chAlloc(pqnChunk);

    return(pqn);
    }

/*
 * This routine gets rid of a PriQNode, previously allocated through
 * malloc_pqn.
 */
static void
free_pqn(PriQNode pqn)
{
    assert(pqn != NULLPRIQNODE);
    assert(pqnChunk != NULLCHUNK);

    chFree(pqnChunk, (void *)pqn);

    return;
    }

/*
 * This routine gets a chunk of memory suitable for use as a PriQIntNode
 */
static PriQIntNode
malloc_pqin(void)
{
    PriQIntNode pqin;

    /* Allocate the Chunk if it hasn't been already */
    if (pqinChunk == NULLCHUNK) {
	pqinChunk = chNew(sizeof(*pqin));
	if (pqinChunk == NULLCHUNK) {
	    return(NULLPRIQINTNODE);
	    }
	}

    pqin = (PriQIntNode)chAlloc(pqinChunk);

    return(pqin);
    }

/*
 * This routine gets rid of a PriQIntNode, previously allocated through
 * malloc_pqin.
 */
static void
free_pqin(PriQIntNode pqin)
{
    assert(pqin != NULLPRIQINTNODE);
    assert(pqinChunk != NULLCHUNK);

    chFree(pqinChunk, (void *)pqin);

    return;
    }