allocator.h

Go to the documentation of this file.

/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
 * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/

#ifndef RTABMAP_FLANN_ALLOCATOR_H_
#define RTABMAP_FLANN_ALLOCATOR_H_

#include <stdlib.h>
#include <stdio.h>


namespace rtflann
{

template <typename T>
T* allocate(size_t count = 1)
{
    T* mem = (T*) ::malloc(sizeof(T)*count);
    return mem;
}



const size_t     WORDSIZE=16;
const  size_t     BLOCKSIZE=8192;

class PooledAllocator
{
    /* We maintain memory alignment to word boundaries by requiring that all
        allocations be in multiples of the machine wordsize.  */
    /* Size of machine word in bytes.  Must be power of 2. */
    /* Minimum number of bytes requested at a time from the system.  Must be multiple of WORDSIZE. */


    int     remaining;  /* Number of bytes left in current block of storage. */
    void*   base;     /* Pointer to base of current block of storage. */
    void*   loc;      /* Current location in block to next allocate memory. */
    int     blocksize;


public:
    int     usedMemory;
    int     wastedMemory;

    PooledAllocator(int blocksize = BLOCKSIZE)
    {
        this->blocksize = blocksize;
        remaining = 0;
        base = NULL;

        usedMemory = 0;
        wastedMemory = 0;
    }

    ~PooledAllocator()
    {
        free();
    }
    
    void free()
    {
        void* prev;
        while (base != NULL) {
            prev = *((void**) base); /* Get pointer to prev block. */
            ::free(base);
            base = prev;
        }
        base = NULL;
        remaining = 0;
        usedMemory = 0;
        wastedMemory = 0;
    }

    void* allocateMemory(int size)
    {
        int blocksize;

        /* Round size up to a multiple of wordsize.  The following expression
            only works for WORDSIZE that is a power of 2, by masking last bits of
            incremented size to zero.
         */
        size = (size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);

        /* Check whether a new block must be allocated.  Note that the first word
            of a block is reserved for a pointer to the previous block.
         */
        if (size > remaining) {

            wastedMemory += remaining;

            /* Allocate new storage. */
            blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
                        size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;

            // use the standard C malloc to allocate memory
            void* m = ::malloc(blocksize);
            if (!m) {
                fprintf(stderr,"Failed to allocate memory.\n");
                return NULL;
            }

            /* Fill first word of new block with pointer to previous block. */
            ((void**) m)[0] = base;
            base = m;

            int shift = 0;
            //int shift = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);

            remaining = blocksize - sizeof(void*) - shift;
            loc = ((char*)m + sizeof(void*) + shift);
        }
        void* rloc = loc;
        loc = (char*)loc + size;
        remaining -= size;

        usedMemory += size;

        return rloc;
    }

    template <typename T>
    T* allocate(size_t count = 1)
    {
        T* mem = (T*) this->allocateMemory((int)(sizeof(T)*count));
        return mem;
    }

};

}

inline void* operator new (std::size_t size, rtflann::PooledAllocator& allocator)
{
    return allocator.allocateMemory(size) ;
}

#endif //FLANN_ALLOCATOR_H_