kinematics: add simple AABB tree for collision checking

softwareComponents/kinematics/CMakeLists.txt

@@ -1,7 +1,13 @@
 cmake_minimum_required(VERSION 3.6)
 project(rofi)
 
-
 add_library(kinematics fReconfig.cpp)
 target_include_directories(kinematics INTERFACE .)
 target_link_libraries(kinematics PRIVATE legacy-configuration atoms)
+
+add_library(aabb include/aabb.hpp)
+target_link_libraries(aabb configuration)
+
+add_executable(test-aabb test/aabb.cpp)
+target_include_directories(test-aabb PRIVATE include)
+target_link_libraries(test-aabb PRIVATE Catch2WithMain aabb)

softwareComponents/kinematics/include/aabb.hpp

@@ -0,0 +1,227 @@
+#pragma once
+
+#include <optional>
+
+#include "shapes.hpp"
+
+
+namespace rofi::geometry {
+
+using namespace rofi::configuration::matrices;
+
+template< typename T >
+concept geometric = requires ( const Box& box, const T& shape ){
+    collide( box, shape );
+    shape.bounding_box();
+};
+
+// Leaves are expected to be spheres or polygons
+template< geometric LeafShape >
+struct AABB_Node {
+
+    Box bound;
+
+    // Node is either a leaf with a shape or has some children
+    std::optional< LeafShape > shape;
+    std::array< std::unique_ptr< AABB_Node >, 2 > children;
+    AABB_Node* parent = nullptr;
+
+    explicit AABB_Node( Box bound ) : bound( bound ) {}
+
+    // resize so that both children fit inside the bounding box
+    void fit(){
+        if( !children[ 0 ] && !children[ 1 ] ){
+            return;
+        }
+        for( auto idx : { 0, 1 } ){
+            if( !children[ idx ] ){
+                bound = children[ 1 - idx ]->bound;
+                return;
+            }
+        }
+
+        // new corners
+        bound = bounding_box( children[ 0 ]->bound, children[ 1 ]->bound );
+    }
+
+    size_t objects() const {
+        if( shape ){
+            return 1;
+        }
+        return ( children[ 0 ] ? children[ 0 ]->objects() : 0 ) + ( children[ 1 ] ? children[ 1 ]->objects() : 0 );
+    }
+
+    size_t depth() const {
+        return 1 + std::max( children[ 0 ] ? children[ 0 ]->depth() : 0, children[ 1 ] ? children[ 1 ]->depth() : 0 );
+    }
+
+    bool left_child(){
+        return parent->children[ 0 ] && this == parent->children[ 0 ].get();
+    }
+
+    void print( const std::string& prefix = "" ){
+        if( children[ 1 ] ){
+            children[ 1 ]->print( prefix + "  " );
+        }
+        if( shape ){
+            std::cout << prefix << "o\n";
+        } else {
+            std::cout << prefix << "B\n";
+        }
+        if( children[ 0 ] ){
+            children[ 0 ]->print( prefix + "  " );
+        }
+    }
+};
+
+template< typename LeafShape >
+class AABB {
+
+    std::unique_ptr< AABB_Node< LeafShape > > _root = nullptr;
+
+  public:
+
+    AABB(){};
+
+    AABB_Node< LeafShape >* insert( const LeafShape& shape ){
+        if( !_root ){
+            _root = std::make_unique< AABB_Node< LeafShape > >( shape.bounding_box() );
+            _root->children[ 0 ] = std::make_unique< AABB_Node< LeafShape > >( shape.bounding_box() );
+            _root->children[ 0 ]->shape = shape;
+            _root->children[ 0 ]->parent = _root.get();
+            return _root->children[ 0 ].get();
+        }
+
+        auto [ child, parent ] = find_relatives( shape );
+
+        AABB_Node< LeafShape >* new_leaf = nullptr;
+
+        if( !child ){
+            new_leaf = append( shape, parent );
+        } else {
+            if( collide( child->shape.value(), shape ) ){
+                return nullptr;
+            }
+            new_leaf = make_sibling( shape, child, parent );
+        }
+
+        // resize all boxes on the way to root
+        while( parent ){
+            parent->fit();
+            parent = parent->parent;
+        }
+
+        return new_leaf;
+    }
+
+    size_t objects() const {
+        if( !_root ){
+            return 0;
+        }
+        return _root->objects();
+    }
+
+    size_t depth() const {
+        if( !_root ){
+            return 0;
+        }
+        return _root->depth();
+    }
+
+    AABB_Node< LeafShape >* root(){
+        return _root.get();
+    }
+
+    bool erase( const LeafShape& shape ){
+        auto* node = find_parent( shape );
+        if( !node || !node->shape || node->shape.value() != shape ){
+            return false;
+        }
+        if( node->parent ){
+            if( node->left_child() ){
+                node->parent->children[ 0 ] = nullptr;
+            } else {
+                node->parent->children[ 1 ] = nullptr;
+            }
+        } else {
+            _root = nullptr;
+        }
+        return true;
+    }
+
+
+  private:
+
+    AABB_Node< LeafShape >* append( const LeafShape& shape, AABB_Node< LeafShape >* parent ){
+        auto new_leaf = std::make_unique< AABB_Node< LeafShape > >( shape.bounding_box() );
+        new_leaf->shape = shape;
+        for( auto idx : { 0, 1 } ){
+            if( !parent->children[ idx ] ){
+                new_leaf->parent = parent;
+                parent->children[ idx ] = std::move( new_leaf );
+                return parent->children[ idx ].get();
+            }
+        }
+        return nullptr;
+    }
+
+    AABB_Node< LeafShape >* make_sibling( const LeafShape& shape, AABB_Node< LeafShape >* sibling, AABB_Node< LeafShape >* parent ){
+        auto new_leaf = std::make_unique< AABB_Node< LeafShape > >( shape.bounding_box() );
+        new_leaf->shape = shape;
+        auto new_node = std::make_unique< AABB_Node< LeafShape > >( shape.bounding_box() );
+        new_node->parent = parent;
+        new_leaf->parent = new_node.get();
+
+        for( auto idx : { 0, 1 } ){
+            if( parent->children[ idx ] && parent->children[ idx ].get() == sibling ){
+                parent->children[ idx ]->parent = new_node.get();
+                new_node->children[ idx ] = std::move( parent->children[ idx ] );
+                new_node->children[ 1 - idx ] = std::move( new_leaf );
+                new_node->fit();
+                parent->children[ idx ] = std::move( new_node );
+                return parent->children[ idx ]->children[ 1 - idx ].get();
+            }
+        }
+        return nullptr;
+    }
+
+    AABB_Node< LeafShape >* find_parent( const LeafShape& shape ){
+        AABB_Node< LeafShape >* current = _root.get();
+        AABB_Node< LeafShape >* last = nullptr;
+        while( current != last ){
+            last = current;
+            for( auto idx : { 0, 1 } ){
+                if( current->children[ idx ] && collide( current->children[ idx ]->bound, shape ) ){
+                    current = current->children[ idx ].get();
+                }
+            }
+        }
+        return current;
+    }
+
+    // return pair ( sibling, parent )
+    std::pair< AABB_Node< LeafShape >*, AABB_Node< LeafShape >* > find_relatives( const LeafShape& shape ){
+        AABB_Node< LeafShape >* current = _root.get();
+        assert( current );
+
+        while( !current->shape ){
+            for( auto idx : { 0, 1 } ){
+                if( !current->children[ idx ] ){
+                    return { nullptr, current };
+                }
+            }
+            // standard heuristic for finding a sibling:
+            //  enter the child that will lead to smaller resizing of bounding boxes
+            std::array< double, 2 > costs;
+            for( auto idx : { 0, 1 } ){
+                costs[ idx ] = bounding_box( shape, current->children[ idx ]->bound ).volume()
+                    + current->children[ 1 - idx ]->bound.volume();
+            }
+            current = costs[ 0 ] < costs[ 1 ] ? current->children[ 0 ].get() : current->children[ 1 ].get();
+        }
+        return { current, current->parent };
+    }
+
+};
+
+} // namespace rofi::geometry

softwareComponents/kinematics/include/shapes.hpp

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <iostream>
+#include <variant>
+#include <vector>
+
+#include <configuration/Matrix.h>
+
+namespace rofi::geometry {
+
+using namespace rofi::configuration::matrices;
+
+struct Box {
+    Vector center;
+    Vector dimensions;
+    Box( Vector center, double x = 1.0, double y = 1.0, double z = 1.0 ) :
+        center( center ), dimensions( { x, y, z, 0 } ) {}
+
+    Box bounding_box() const {
+        return *this;
+    }
+
+    double max( size_t axis ) const {
+        return center[ axis ] + dimensions[ axis ] / 2;
+    }
+    double min( size_t axis ) const {
+        return center[ axis ] - dimensions[ axis ] / 2;
+    }
+
+    double volume() const {
+        return dimensions[ 0 ] + dimensions[ 1 ] + dimensions[ 2 ];
+    }
+
+    friend bool operator==( const Box& a, const Box& b ){
+        return equals( a.center, b.center ) && equals( a.dimensions, b.dimensions );
+    }
+    friend bool operator!=( const Box&, const Box& ) = default;
+
+    friend std::ostream& operator<<( std::ostream& os, const Box& b ){
+        os << "Center: " << b.center[ 0 ] << " " << b.center[ 1 ] << " " << b.center[ 2 ]
+           << "\nDimensions: " << b.dimensions[ 0 ] << " " << b.dimensions[ 1 ] << " " << b.dimensions[ 2 ] << '\n';
+        return os;
+    }
+};
+
+struct Sphere {
+    Vector center;
+    double radius = 0.499;
+
+    Sphere( Vector center, double radius = 0.499 ) : center( center ), radius( radius ) {}
+
+    Box bounding_box() const {
+        return Box( center, radius * 2, radius * 2, radius * 2 );
+    }
+
+    double max( size_t axis ) const {
+        return center[ axis ] + radius;
+    }
+    double min( size_t axis ) const {
+        return center[ axis ] - radius;
+    }
+
+    friend bool operator==( const Sphere& a, const Sphere& b ){
+        return equals( a.center, b.center ) && std::fabs( a.radius - b.radius ) < 0.001;
+    }
+    friend bool operator!=( const Sphere&, const Sphere& ) = default;
+};
+
+template< typename T, typename S >
+Box bounding_box( const T& a, const S& b ){
+    Box bound( Vector{ 0, 0, 0, 1 } );
+    Vector min = { 0, 0, 0, 1 };
+    Vector max = { 0, 0, 0, 1 };
+
+    for( size_t axis : { 0, 1, 2 } ){
+        min[ axis ] = std::min( a.min( axis ),
+                                b.min( axis ) );
+        max[ axis ] = std::max( a.max( axis ),
+                                b.max( axis ) );
+    }
+    bound.center = ( min + max ) / 2;
+    bound.dimensions = max - min;
+    return bound;
+}
+
+inline double magnitude( const Vector& vector ){
+    return std::sqrt( std::pow( vector[ 0 ], 2 ) +
+                      std::pow( vector[ 1 ], 2 ) +
+                      std::pow( vector[ 2 ], 2 ) );
+}
+
+inline bool collide( const Sphere& ball, const Vector& point ){
+    double dist = magnitude( ball.center - point );
+    return dist < ball.radius;
+}
+
+inline bool collide( const Sphere& ball, const Sphere& ball2 ){
+    double dist = magnitude( ball.center - ball2.center );
+    return dist < ball.radius + ball2.radius;
+}
+
+// point is inside of box when distance on all of the 3 axes is smaller or equal to its size
+inline bool collide( const Box& box, const Vector& point ){
+    for( size_t i = 0; i < 3; i++ ){
+        if( std::fabs( box.center[ i ] - point[ i ] ) >= box.dimensions[ i ] / 2.0 ){
+            return false;
+        }
+    }
+    return true;
+}
+
+// box collides with ball when a box widened by the ball radius colides with the center
+inline bool collide( const Box& box, const Sphere& ball ){
+    Box widened = box;
+    for( size_t i = 0; i < 3; i++ ){
+        widened.dimensions[ i ] += ball.radius * 2;
+    }
+    return collide( widened, ball.center );
+}
+
+// boxes collide when the distance of centers is smaller than their combined size on any axis
+inline bool collide( const Box& box, const Box& box2 ){
+    for( size_t i = 0; i < 3; i++ ){
+        if( std::fabs( box.center[ i ] - box2.center[ i ] )
+            < box.dimensions[ i ] / 2 + box2.dimensions[ i ] / 2 )
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+} // namespace rofi::geometry