ViewDesignEngine/src/brep/assembly_constraints.cpp

#include "vde/brep/assembly_constraints.h"
#include "vde/core/transform.h"
#include "vde/core/aabb.h"
#include <cmath>
#include <algorithm>

namespace vde::brep {
namespace {

/// Compute the world-space AABB of a node by transforming its model bounds
/// through the node's local_transform.
core::AABB3D node_world_bounds(const AssemblyNode* node) {
    // The local_transform positions the node in its parent's space.
    // For root's direct children, this IS the world transform.
    const auto& T = node->local_transform;

    if (node->model.has_value()) {
        core::AABB3D local_bb = node->model.value().bounds();
        core::Point3D corners[8] = {
            local_bb.min(),
            core::Point3D(local_bb.max().x(), local_bb.min().y(), local_bb.min().z()),
            core::Point3D(local_bb.max().x(), local_bb.max().y(), local_bb.min().z()),
            core::Point3D(local_bb.min().x(), local_bb.max().y(), local_bb.min().z()),
            core::Point3D(local_bb.min().x(), local_bb.min().y(), local_bb.max().z()),
            core::Point3D(local_bb.max().x(), local_bb.min().y(), local_bb.max().z()),
            local_bb.max(),
            core::Point3D(local_bb.min().x(), local_bb.max().y(), local_bb.max().z()),
        };
        core::AABB3D world_bb;
        for (const auto& c : corners) {
            world_bb.expand(T * c);
        }
        return world_bb;
    }

    // For subassembly nodes without their own model, compute union of children
    // node_world_bounds recursively already transforms each child by its
    // own local_transform, so children's bounds are in this node's space.
    core::AABB3D world_bb;
    for (const auto& child : node->children) {
        core::AABB3D child_bb = node_world_bounds(child.get());
        world_bb.expand(child_bb);
    }
    // Now transform through this node's own local_transform to world
    core::AABB3D result;
    core::Point3D corners[8] = {
        world_bb.min(),
        core::Point3D(world_bb.max().x(), world_bb.min().y(), world_bb.min().z()),
        core::Point3D(world_bb.max().x(), world_bb.max().y(), world_bb.min().z()),
        core::Point3D(world_bb.min().x(), world_bb.max().y(), world_bb.min().z()),
        core::Point3D(world_bb.min().x(), world_bb.min().y(), world_bb.max().z()),
        core::Point3D(world_bb.max().x(), world_bb.min().y(), world_bb.max().z()),
        world_bb.max(),
        core::Point3D(world_bb.min().x(), world_bb.max().y(), world_bb.max().z()),
    };
    for (const auto& c : corners) {
        result.expand(T * c);
    }
    return result;
}

} // anonymous namespace

bool apply_constraint(
    Assembly& /*assembly*/,
    AssemblyNode* node_a,
    AssemblyNode* node_b,
    ConstraintType type,
    double value)
{
    if (!node_a || !node_b) return false;

    // Compute world-space bounding boxes for both nodes
    core::AABB3D bb_a = node_world_bounds(node_a);
    core::AABB3D bb_b = node_world_bounds(node_b);

    core::Point3D center_a = bb_a.center();
    core::Point3D center_b = bb_b.center();

    core::Vector3D offset;

    switch (type) {
        case ConstraintType::Coincident: {
            // Align node_b so its bottom face touches node_a's top face.
            double dz = bb_a.max().z() - bb_b.min().z();
            offset = core::Vector3D(center_a.x() - center_b.x(),
                                    center_a.y() - center_b.y(),
                                    dz);
            // Pre-multiply: apply in world space, then convert to local
            node_b->local_transform = core::translate(offset) * node_b->local_transform;
            return true;
        }

        case ConstraintType::Concentric: {
            // Align centers in X and Y (keep Z where it is).
            offset = core::Vector3D(center_a.x() - center_b.x(),
                                    center_a.y() - center_b.y(),
                                    0.0);
            node_b->local_transform = core::translate(offset) * node_b->local_transform;
            return true;
        }

        case ConstraintType::Tangent: {
            // Offset node_b so it just touches node_a (same as coincident).
            double dz = bb_a.max().z() - bb_b.min().z();
            offset = core::Vector3D(center_a.x() - center_b.x(),
                                    center_a.y() - center_b.y(),
                                    dz);
            node_b->local_transform = core::translate(offset) * node_b->local_transform;
            return true;
        }

        case ConstraintType::Parallel: {
            // No geometric change for axis-aligned boxes.
            return true;
        }

        case ConstraintType::Perpendicular: {
            // Rotate node_b 90° around X so vertical becomes horizontal.
            // Pre-multiply: rotate in world space.
            node_b->local_transform = core::rotate_x(M_PI / 2.0) * node_b->local_transform;
            return true;
        }

        case ConstraintType::Distance: {
            // Offset node_b by a fixed distance from node_a.
            double dz = bb_a.max().z() - bb_b.min().z() + value;
            offset = core::Vector3D(center_a.x() - center_b.x(),
                                    center_a.y() - center_b.y(),
                                    dz);
            node_b->local_transform = core::translate(offset) * node_b->local_transform;
            return true;
        }

        case ConstraintType::Angle: {
            // Rotate node_b by the specified angle around X axis.
            node_b->local_transform = core::rotate_x(value) * node_b->local_transform;
            return true;
        }
    }

    return false;
}

} // namespace vde::brep