A simple, generic point cloud class example implemented with C++

pointcloud.cpp

/*
 * This file is licensed under the MIT License (https://opensource.org/licenses/MIT)
 * Developed by Tomi Paananen.
 *  https://github.com/tompaana/
 *  http://tomipaananen.azurewebsites.net/?p=2543
 */

#include "pointcloud.h"

#include <limits>
#include <stdexcept>
#include <stdio.h>

template class PointCloud<short>;
template class PointCloud<unsigned short>;
template class PointCloud<int>;
template class PointCloud<unsigned int>;

/**
 * Constructor.
 *
 * @param pointHasValue If true, the points in the cloud will have a value associated with them.
 * @param chunkSizeAsPointCount Defines the size of chunks to be allocated when the point count
 * increases.
 */
template <typename T>
PointCloud<T>::PointCloud(bool pointHasValue, unsigned int chunkSizeAsPointCount)
    : mData(NULL),
      mCurrentPointCount(0),
      mPointsHaveValues(pointHasValue),
      mChunkSizeAsPointCount(chunkSizeAsPointCount),
      mPointLengthAsElementCount(DimensionCount + (pointHasValue ? 1 : 0))
{
    mCurrentAllocatedDataSizeAsElementCount = mPointLengthAsElementCount * mChunkSizeAsPointCount;
    mData = (T*)malloc(mCurrentAllocatedDataSizeAsElementCount * sizeof(T));
    memset(mData, Undefined, mCurrentAllocatedDataSizeAsElementCount * sizeof(T));

    T min = std::numeric_limits<T>::min();
    T max = std::numeric_limits<T>::max();

    for (short i = 0; i < DimensionCount; ++i)
    {
        mMin[i] = max;
        mMax[i] = min;
    }
}

/**
 * Destructor.
 */
template <typename T>
PointCloud<T>::~PointCloud()
{
    delete[] mData;
}

/**
 * @returns The data of this point cloud as an array.
 */
template <typename T>
const T* PointCloud<T>::data() const
{
    return mData;
}

/**
 * @returns The current point count.
 */
template <typename T>
unsigned long PointCloud<T>::pointCount() const
{
    return mCurrentPointCount;
}

/**
 * @param x The X coordinate of the point.
 * @param y The Y coordinate of the point.
 * @param z The Z coordinate of the point.
 * @returns True, if a point with the given coordinates exists in the point cloud. False otherwise.
 */
template <typename T>
bool PointCloud<T>::hasPoint(T x, T y, T z) const
{
    return (accessorToPoint(x, y, z) != NULL);
}

/**
 * @param x The X coordinate of the point.
 * @param y The Y coordinate of the point.
 * @param z The Z coordinate of the point.
 * @returns The value of the point in the given coordinates. Will return 'Undefined' if no point is
 * found and will throw an exception if the points in this point cloud do not have a value
 * associated with them.
 */
template <typename T>
T PointCloud<T>::pointValue(T x, T y, T z) const
{
    if (!mPointsHaveValues)
    {
        throw std::exception("The points in this point cloud have no values");
    }

    T* point = accessorToPoint(x, y, z);

    if (point != NULL)
    {
        return point[mPointLengthAsElementCount - 1];
    }

    return Undefined;
}

/**
 * Adds a point to the point cloud with the given coordinates (and value if supported).
 * Note that there cannot be two points with the exact same coordinates.
 *
 * @param x The X coordinate of the point.
 * @param y The Y coordinate of the point.
 * @param z The Z coordinate of the point.
 * @param value The value of the point (given that this point cloud has values associated with the points).
 * @returns True, if the point was added. False otherwise.
 */
template <typename T>
bool PointCloud<T>::addPoint(T x, T y, T z, T value)
{
    if (hasPoint(x, y, z))
    {
        return false;
    }

    unsigned long index = mCurrentPointCount * mPointLengthAsElementCount;
    bool canAdd = (index + 1 < mCurrentAllocatedDataSizeAsElementCount);

    if (!canAdd && !increaseDataByChunkSize())
    {
        return false;
    }

    mData[index] = x;
    mData[index + 1] = y;
    mData[index + 2] = z;

    if (mPointsHaveValues)
    {
        mData[index + 3] = value;
    }

    T point[DimensionCount] = { x, y, z };

    for (short i = 0; i < DimensionCount; ++i)
    {
        if (mMin[i] > point[i])
        {
            mMin[i] = point[i];
        }

        if (mMax[i] < point[i])
        {
            mMax[i] = point[i];
        }
    }

    mCurrentPointCount++;
    return true;
}

/**
 * Sets the value of the point in the given coordinates.
 * If this point cloud has no value associated with the points, an exception will be thrown.
 *
 * @param x The X coordinate of the point.
 * @param y The Y coordinate of the point.
 * @param z The Z coordinate of the point.
 * @param value The value of the point.
 * @param addPointIfDoesNotExist If true, the point will be added if one didn't exist before.
 */
template <typename T>
bool PointCloud<T>::setPointValue(T x, T y, T z, T value, bool addPointIfDoesNotExist)
{
    bool wasPointValueSet = false;

    if (!mPointsHaveValues)
    {
        throw std::exception("The points in this point cloud have no values");
    }

    T* pointStart = accessorToPoint(x, y, z);

    if (pointStart != NULL)
    {
        pointStart[mPointLengthAsElementCount - 1] = value;
	wasPointValueSet = true;
    }
    else if (addPointIfDoesNotExist)
    {
        wasPointValueSet = addPoint(x, y, z, value);
    }

    return wasPointValueSet;
}

/**
 * Adds the given value to the current value of the point in the given coordinates.
 * If this point cloud has no value associated with the points, an exception will be thrown.
 *
 * @param x The X coordinate of the point.
 * @param y The Y coordinate of the point.
 * @param z The Z coordinate of the point.
 * @param valueToAdd The value to add to the current value of the point.
 */
template <typename T>
bool PointCloud<T>::addToPointValue(T x, T y, T z, T valueToAdd)
{
    if (!mPointsHaveValues)
    {
        throw std::exception("The points in this point cloud have no values");
    }

    T* pointStart = accessorToPoint(x, y, z);

    if (pointStart != NULL)
    {
        pointStart[mPointLengthAsElementCount - 1] += valueToAdd;
        return true;
    }

    return false;
}

/**
 * @returns True, if the point cloud has reached its maximum allocation. False otherwise.
 */
template <typename T>
bool PointCloud<T>::maxSizeReached() const
{
    return ((mCurrentAllocatedDataSizeAsElementCount + mChunkSizeAsPointCount * mPointLengthAsElementCount) * sizeof(T)
                > MaxDataSizeInBytes);
}

/**
 * @returns The length of a point as element count (e.g. if a point consists of three coordinates
 * and has one value associated with it, this method will return 4).
 */
template <typename T>
unsigned short PointCloud<T>::pointLengthAsElementCount() const
{
    return mPointLengthAsElementCount;
}

/**
 * @returns The size allocated for this point cloud as element count.
 * For example, if the length of a point is 4 (see pointLengthAsElementCount()) and there are
 * chunks allocated to house 1000 points, the return value of this method will be 4000.
 * Note that the allocated size can (and in many cases is) larger than the size of the defined points.
 */
template <typename T>
unsigned long PointCloud<T>::allocatedDataSizeAsElementCount() const
{
    return mCurrentAllocatedDataSizeAsElementCount;
}

/**
 * @returns The minimum boundaries (of all dimensions) of the points in the point cloud.
 */
template <typename T>
const T* PointCloud<T>::min() const
{
    return mMin;
}

/**
 * @returns The maximum boundaries (of all dimensions) of the points in the point cloud.
 */
template <typename T>
const T* PointCloud<T>::max() const
{
    return mMax;
}

/**
 * Allocates a new chunk for the point cloud unless the maximum size is reached.
 *
 * @returns True, if a new chunk was allocated. False otherwise.
 */
template <typename T>
bool PointCloud<T>::increaseDataByChunkSize()
{
    if (maxSizeReached())
    {
        return false;
    }

    unsigned int chunkSizeAsElementCount = mPointLengthAsElementCount * mChunkSizeAsPointCount;
    unsigned long newallocatedDataSizeAsElementCount =
        mCurrentAllocatedDataSizeAsElementCount + chunkSizeAsElementCount;
    T* newData = (T*)malloc(newallocatedDataSizeAsElementCount * sizeof(T));
    memcpy(newData, mData, mCurrentAllocatedDataSizeAsElementCount * sizeof(T));
    T* newChunkStart = newData + mCurrentAllocatedDataSizeAsElementCount;
    memset(newChunkStart, Undefined, chunkSizeAsElementCount * sizeof(T));

    delete[] mData;
    mData = newData;
    mCurrentAllocatedDataSizeAsElementCount = newallocatedDataSizeAsElementCount;
    return true;
}

/**
 * @param x The X coordinate of the point.
 * @param y The Y coordinate of the point.
 * @param z The Z coordinate of the point.
 * @returns The accessor (a pointer to the beginning) of the point in the given coordinates.
 * NULL, if no point found.
 */
template <typename T>
inline T* PointCloud<T>::accessorToPoint(T x, T y, T z) const
{
    const long maxIndex = mCurrentPointCount * mPointLengthAsElementCount;

    for (long i = 0; i < maxIndex; i += mPointLengthAsElementCount)
    {
        if (mData[i] == x && mData[i + 1] == y && mData[i + 2] == z)
        {
            return &mData[i];
        }
    }

    return NULL;
}

pointcloud.h

/*
* This file is licensed under the MIT License (https://opensource.org/licenses/MIT)
* Developed by Tomi Paananen.
*  https://github.com/tompaana/
*  http://tomipaananen.azurewebsites.net/?p=2543
*/

#ifndef POINTCLOUD_H
#define POINTCLOUD_H

const unsigned short DimensionCount = 3;
const unsigned long MaxDataSizeInBytes = 104857600;
const unsigned int DefaultChunkSizeAsPointCount = 1000;
const int Undefined = 0;

template <typename T>
class PointCloud
{
public:
    PointCloud(bool pointsHaveValues, unsigned int chunkSizeAsPointCount = DefaultChunkSizeAsPointCount);
    virtual ~PointCloud();

public:
    const T* data() const;
    unsigned long pointCount() const;
    bool hasPoint(T x, T y, T z) const;
    T pointValue(T x, T y, T z) const;
    bool addPoint(T x, T y, T z, T value = Undefined);
    bool setPointValue(T x, T y, T z, T value, bool addPointIfDoesNotExist = true);
    bool addToPointValue(T x, T y, T z, T valueToAdd);
    bool maxSizeReached() const;
    unsigned short pointLengthAsElementCount() const;
    unsigned long allocatedDataSizeAsElementCount() const;
    const T* min() const;
    const T* max() const;

private:
    bool increaseDataByChunkSize();
    T* accessorToPoint(T x, T y, T z) const;

public:

private:
    T* mData;
    unsigned long mCurrentPointCount;
    unsigned long mCurrentAllocatedDataSizeAsElementCount; // == mCurrentPointCount * mPointLengthAsElementCount
    unsigned int mChunkSizeAsPointCount;
    unsigned short mPointLengthAsElementCount; // E.g. three dimensions (x, y, z) plus a value -> 4
    T mMin[DimensionCount];
    T mMax[DimensionCount];
    bool mPointsHaveValues;
};

#endif // POINTCLOUD_H