PointCloudTpl.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#pragma once
 
// Local
#include "BoundingBox.h"
#include "GenericIndexedCloudPersist.h"
#include "ScalarField.h"
 
// STL
#include <vector>
 
namespace cloudViewer {
 
template <class T>
class PointCloudTpl : public T {
    static_assert(std::is_base_of<GenericIndexedCloudPersist, T>::value,
                  "T must be a descendant of GenericIndexedCloudPersist");
 
public:
    PointCloudTpl()
        : T(),
          m_currentPointIndex(0),
          m_currentInScalarFieldIndex(-1),
          m_currentOutScalarFieldIndex(-1) {}
 
    virtual ~PointCloudTpl() { deleteAllScalarFields(); }
 
    inline unsigned size() const override {
        return static_cast<unsigned>(m_points.size());
    }
 
    void forEach(GenericCloud::genericPointAction action) override {
        // there's no point of calling forEach if there's no activated scalar
        // field!
        ScalarField* currentOutScalarFieldArray = getCurrentOutScalarField();
        if (!currentOutScalarFieldArray) {
            assert(false);
            return;
        }
 
        unsigned n = size();
        for (unsigned i = 0; i < n; ++i) {
            action(m_points[i], (*currentOutScalarFieldArray)[i]);
        }
    }
 
    void getBoundingBox(CCVector3& bbMin, CCVector3& bbMax) override {
        if (!m_bbox.isValid()) {
            m_bbox.clear();
            for (const CCVector3& P : m_points) {
                m_bbox.add(P);
            }
        }
 
        bbMin = m_bbox.minCorner();
        bbMax = m_bbox.maxCorner();
    }
 
    void placeIteratorAtBeginning() override { m_currentPointIndex = 0; }
 
    const CCVector3* getNextPoint() override {
        return (m_currentPointIndex < m_points.size()
                        ? point(m_currentPointIndex++)
                        : 0);
    }
 
    bool enableScalarField() override {
        ScalarField* currentInScalarField = getCurrentInScalarField();
 
        if (!currentInScalarField) {
            // if we get there, it means that either the caller has forgot to
            // create (and assign) a scalar field to the cloud, or that we are
            // in a compatibility mode with old/basic behaviour: a unique SF for
            // everything (input/output)
 
            // we look for any already existing "default" scalar field
            m_currentInScalarFieldIndex = getScalarFieldIndexByName("Default");
            if (m_currentInScalarFieldIndex < 0) {
                // if not, we create it
                m_currentInScalarFieldIndex = addScalarField("Default");
                if (m_currentInScalarFieldIndex < 0)  // Something went wrong
                {
                    return false;
                }
            }
 
            currentInScalarField = getCurrentInScalarField();
            assert(currentInScalarField);
        }
 
        // if there's no output scalar field either, we set this new scalar
        // field as output also
        if (!getCurrentOutScalarField()) {
            m_currentOutScalarFieldIndex = m_currentInScalarFieldIndex;
        }
 
        return currentInScalarField->resizeSafe(m_points.capacity());
    }
 
    bool isScalarFieldEnabled() const override {
        ScalarField* currentInScalarFieldArray = getCurrentInScalarField();
        if (!currentInScalarFieldArray) {
            return false;
        }
 
        std::size_t sfValuesCount = currentInScalarFieldArray->size();
        return (sfValuesCount != 0 && sfValuesCount >= m_points.size());
    }
 
    void setPointScalarValue(unsigned pointIndex, ScalarType value) override {
        assert(m_currentInScalarFieldIndex >= 0 &&
               m_currentInScalarFieldIndex <
                       static_cast<int>(m_scalarFields.size()));
        // slow version
        // ScalarField* currentInScalarFieldArray = getCurrentInScalarField();
        // if (currentInScalarFieldArray)
        //  currentInScalarFieldArray->setValue(pointIndex,value);
 
        // fast version
        m_scalarFields[m_currentInScalarFieldIndex]->setValue(pointIndex,
                                                              value);
    }
 
    ScalarType getPointScalarValue(unsigned pointIndex) const override {
        assert(m_currentOutScalarFieldIndex >= 0 &&
               m_currentOutScalarFieldIndex <
                       static_cast<int>(m_scalarFields.size()));
 
        return m_scalarFields[m_currentOutScalarFieldIndex]->getValue(
                pointIndex);
    }
 
    inline const CCVector3* getPoint(unsigned index) const override {
        return point(index);
    }
    inline CCVector3* getPointPtr(size_t index) {
        return point(static_cast<unsigned>(index));
    }
    inline std::vector<CCVector3>& getPoints() { return m_points; }
    inline const std::vector<CCVector3>& getPoints() const { return m_points; }
    inline void getPoint(unsigned index, CCVector3& P) const override {
        P = *point(index);
    }
    inline void getPoint(unsigned index, double P[3]) const override {
        P[0] = static_cast<double>(point(index)->x);
        P[1] = static_cast<double>(point(index)->y);
        P[2] = static_cast<double>(point(index)->z);
    }
 
    inline const CCVector3* getPointPersistentPtr(unsigned index) override {
        return point(index);
    }
    inline const CCVector3* getPointPersistentPtr(unsigned index) const {
        return point(index);
    }
 
 
    virtual bool resize(unsigned newCount) {
        std::size_t oldCount = m_points.size();
 
        // we try to enlarge the 3D points array
        try {
            m_points.resize(newCount);
        } catch (const std::bad_alloc&) {
            return false;
        }
 
        // then the scalar fields
        for (std::size_t i = 0; i < m_scalarFields.size(); ++i) {
            if (!m_scalarFields[i]->resizeSafe(newCount)) {
                // if something fails, we restore the previous size for already
                // processed SFs!
                for (std::size_t j = 0; j < i; ++j) {
                    m_scalarFields[j]->resize(oldCount);
                    m_scalarFields[j]->computeMinAndMax();
                }
                // we can assume that newCount > oldNumberOfPoints, so it should
                // always be ok
                m_points.resize(oldCount);
                return false;
            }
            m_scalarFields[i]->computeMinAndMax();
        }
 
        return true;
    }
 
 
    virtual bool reserve(unsigned newCapacity) {
        // we try to enlarge the 3D points array
        try {
            m_points.reserve(newCapacity);
        } catch (const std::bad_alloc&) {
            return false;
        }
 
        // then the scalar fields
        for (std::size_t i = 0; i < m_scalarFields.size(); ++i) {
            if (!m_scalarFields[i]->reserveSafe(newCapacity)) return false;
        }
 
        // double check
        return (m_points.capacity() >= newCapacity);
    }
 
 
    void reset() {
        m_points.resize(0);
        deleteAllScalarFields();
        placeIteratorAtBeginning();
        invalidateBoundingBox();
    }
 
 
    void addPoint(const CCVector3& P) {
        // NaN coordinates check
        if (P.x != P.x || P.y != P.y || P.z != P.z) {
            // replace NaN point by (0, 0, 0)
            CCVector3 fakeP(0, 0, 0);
            m_points.push_back(fakeP);
        } else {
            m_points.push_back(P);
        }
 
        m_bbox.setValidity(false);
    }
 
    void setPoint(size_t index, const CCVector3& P) {
        if (index < size()) {
            // NaN coordinates check
            if (P.x != P.x || P.y != P.y || P.z != P.z) {
                // replace NaN point by (0, 0, 0)
                CCVector3 fakeP(0, 0, 0);
                m_points[index] = fakeP;
            } else {
                m_points[index] = P;
            }
        }
    }
 
    void setEigenPoint(size_t index, const Eigen::Vector3d& point) {
        if (index < size()) {
            bool is_nan = (std::isnan(point(0)) || std::isnan(point(1)) ||
                           std::isnan(point(2)));
            bool is_infinite = (std::isinf(point(0)) || std::isinf(point(1)) ||
                                std::isinf(point(2)));
 
            // NaN coordinates check
            if (!is_nan && !is_infinite) {
                m_points[index] = point;
            } else {
                // replace NaN or infinite point by (0, 0, 0)
                CCVector3 fakeP(0, 0, 0);
                m_points[index] = fakeP;
            }
        }
    }
 
    void addEigenPoint(const Eigen::Vector3d& point) { addPoint(point); }
 
    void addPoint(double x, double y, double z) {
        addPoint(CCVector3(static_cast<PointCoordinateType>(x),
                           static_cast<PointCoordinateType>(y),
                           static_cast<PointCoordinateType>(z)));
    }
 
    void addPoint(double xyz[3]) { addPoint(xyz[0], xyz[1], xyz[2]); }
 
    void addPoints(const std::vector<CCVector3>& points) {
        for (auto& P : points) {
            addPoint(P);
        }
    }
 
    void addPoints(const std::vector<Eigen::Vector3d>& points) {
        if (m_points.size() == 0) {
            m_points.reserve(points.size());
        }
 
        if (points.size() == m_points.size()) {
            setEigenPoints(points);
        } else {
            m_points.clear();
            m_points.reserve(points.size());
            addPoints(CCVector3::fromArrayContainer(points));
        }
    }
 
    inline Eigen::Vector3d getEigenPoint(size_t index) const {
        return CCVector3d::fromArray(*point(static_cast<unsigned int>(index)));
    }
 
    inline std::vector<Eigen::Vector3d> getEigenPoints() const {
        return CCVector3::fromArrayContainer(m_points);
    }
    inline void setEigenPoints(const std::vector<Eigen::Vector3d>& points) {
        assert(points.size() == m_points.size());
        for (size_t i = 0; i < points.size(); ++i) {
            setEigenPoint(i, points[i]);
        }
    }
 
 
    virtual void invalidateBoundingBox() { m_bbox.setValidity(false); }
 
    /*** scalar fields management ***/
 
 
    inline unsigned getNumberOfScalarFields() const {
        return static_cast<unsigned>(m_scalarFields.size());
    }
 
 
    ScalarField* getScalarField(int index) const {
        return (index >= 0 && index < static_cast<int>(m_scalarFields.size())
                        ? m_scalarFields[index]
                        : 0);
    }
 
 
    const char* getScalarFieldName(int index) const {
        return (index >= 0 && index < static_cast<int>(m_scalarFields.size())
                        ? m_scalarFields[index]->getName()
                        : 0);
    }
 
 
    int getScalarFieldIndexByName(const char* name) const {
        std::size_t sfCount = m_scalarFields.size();
        for (std::size_t i = 0; i < sfCount; ++i) {
            // we don't accept two SF with the same name!
            if (strcmp(m_scalarFields[i]->getName(), name) == 0)
                return static_cast<int>(i);
        }
 
        return -1;
    }
 
 
    inline ScalarField* getCurrentInScalarField() const {
        return getScalarField(m_currentInScalarFieldIndex);
    }
 
 
    inline ScalarField* getCurrentOutScalarField() const {
        return getScalarField(m_currentOutScalarFieldIndex);
    }
 
 
    inline void setCurrentInScalarField(int index) {
        m_currentInScalarFieldIndex = index;
    }
 
    inline int getCurrentInScalarFieldIndex() {
        return m_currentInScalarFieldIndex;
    }
 
 
    inline void setCurrentOutScalarField(int index) {
        m_currentOutScalarFieldIndex = index;
    }
 
    inline int getCurrentOutScalarFieldIndex() {
        return m_currentOutScalarFieldIndex;
    }
 
 
    inline void setCurrentScalarField(int index) {
        setCurrentInScalarField(index);
        setCurrentOutScalarField(index);
    }
 
 
    virtual int addScalarField(const char* uniqueName) {
        // we don't accept two SF with the same name!
        if (getScalarFieldIndexByName(uniqueName) >= 0) {
            return -1;
        }
 
        // create requested scalar field
        ScalarField* sf = new ScalarField(uniqueName);
        if (!sf || (size() && !sf->resizeSafe(size()))) {
            // Not enough memory!
            if (sf) sf->release();
            return -1;
        }
 
        try {
            // we don't want 'm_scalarFields' to grow by 50% each time! (default
            // behavior of std::vector::push_back)
            m_scalarFields.resize(m_scalarFields.size() + 1, sf);
        } catch (const std::bad_alloc&)  // out of memory
        {
            sf->release();
            return -1;
        }
 
        return static_cast<int>(m_scalarFields.size()) - 1;
    }
 
 
    bool renameScalarField(int index, const char* newName) {
        if (getScalarFieldIndexByName(newName) < 0) {
            ScalarField* sf = getScalarField(index);
            if (sf) {
                sf->setName(newName);
                return true;
            }
        }
        return false;
    }
 
 
    virtual void deleteScalarField(int index) {
        int sfCount = static_cast<int>(m_scalarFields.size());
        if (index < 0 || index >= sfCount) return;
 
        // we update SF roles if they point to the deleted scalar field
        if (index == m_currentInScalarFieldIndex)
            m_currentInScalarFieldIndex = -1;
        if (index == m_currentOutScalarFieldIndex)
            m_currentOutScalarFieldIndex = -1;
 
        // if the deleted SF is not the last one, we swap it with the last
        // element
        int lastIndex = sfCount - 1;  // lastIndex>=0
        if (index < lastIndex)        // i.e.lastIndex>0
        {
            std::swap(m_scalarFields[index], m_scalarFields[lastIndex]);
            // don't forget to update SF roles also if they point to the last
            // element
            if (lastIndex == m_currentInScalarFieldIndex)
                m_currentInScalarFieldIndex = index;
            if (lastIndex == m_currentOutScalarFieldIndex)
                m_currentOutScalarFieldIndex = index;
        }
 
        // we can always delete the last element (and the vector stays
        // consistent)
        m_scalarFields.back()->release();
        m_scalarFields.pop_back();
    }
 
    virtual void deleteAllScalarFields() {
        m_currentInScalarFieldIndex = m_currentOutScalarFieldIndex = -1;
 
        while (!m_scalarFields.empty()) {
            m_scalarFields.back()->release();
            m_scalarFields.pop_back();
        }
    }
 
    inline unsigned capacity() const {
        return static_cast<unsigned>(m_points.capacity());
    }
 
protected:
    virtual void swapPoints(unsigned firstIndex, unsigned secondIndex) {
        if (firstIndex == secondIndex || firstIndex >= m_points.size() ||
            secondIndex >= m_points.size()) {
            return;
        }
 
        std::swap(m_points[firstIndex], m_points[secondIndex]);
 
        for (std::size_t i = 0; i < m_scalarFields.size(); ++i) {
            m_scalarFields[i]->swap(firstIndex, secondIndex);
        }
    }
 
 
    inline CCVector3* point(unsigned index) {
        assert(index < size());
        return &(m_points[index]);
    }
 
 
    inline const CCVector3* point(unsigned index) const {
        assert(index < size());
        return &(m_points[index]);
    }
 
    std::vector<CCVector3> m_points;
 
    BoundingBox m_bbox;
 
    unsigned m_currentPointIndex;
 
    std::vector<ScalarField*> m_scalarFields;
 
    int m_currentInScalarFieldIndex;
 
    int m_currentOutScalarFieldIndex;
};
 
}  // namespace cloudViewer