基于KD树、包围盒与RayCast（射线投射）实现物体拾取的示例代码框架

以下是一个基于KD树、包围盒与RayCast（射线投射）实现物体拾取的示例代码框架及相关解释。这个示例假设是在一个三维空间场景下进行操作，主要目的是通过从指定视点发出射线，利用KD树对场景中的物体包围盒进行组织和快速搜索，来判断射线与哪个物体相交，从而实现物体的拾取。

#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>

// 定义三维点结构体
struct Point3D {
    double x;
    double y;
    double z;

    Point3D(double _x = 0, double _y = 0, double _z = 0) : x(_x), y(_y), z(_z) {}
};

// 定义包围盒结构体
struct BoundingBox {
    Point3D min;
    Point3D max;

    BoundingBox(const Point3D& _min = Point3D(), const Point3D& _max = Point3D()) : min(_min), max(_max) {}

    // 判断点是否在包围盒内
    bool contains(const Point3D& p) const {
        return p.x >= min.x && p.x <= max.x && p.y >= min.y && p.y <= max.y && p.z >= min.z && p.z <= max.z;
    }

    // 判断射线是否与包围盒相交
    bool intersectsRay(const Point3D& rayOrigin, const Point3D& rayDirection) const {
        double tmin, tmax, tymin, tymax, tzmin, tzmax;

        tmin = (min.x - rayOrigin.x) / rayDirection.x;
        tmax = (max.x - rayOrigin.x) / rayDirection.x;
        if (tmin > tmax) std::swap(tmin, tmax);

        tymin = (min.y - rayOrigin.y) / rayDirection.y;
        tymax = (max.y - rayOrigin.y) / rayDirection.y;
        if (tymin > tymax) std::swap(tymin, tymax);

        if ((tmin > tymax) || (tymin > tmax)) return false;

        if (tymin > tmin) tmin = tymin;
        if (tymax < tmax) tmax = tymax;

        tzmin = (min.z - rayOrigin.z) / rayDirection.z;
        tzmax = (max.z - rayOrigin.z) / rayDirection.z;
        if (tzmin > tzmax) std::swap(tzmin, tzmax);

        if ((tmin > tzmax) || (tzmin > tmax)) return false;

        return true;
    }
};

// KD树节点结构体
struct KDNode {
    BoundingBox boundingBox;
    KDNode* left;
    KDNode* right;
    std::vector<int> objectIndices; // 存储该节点所对应包围盒包含的物体索引

    KDNode(const BoundingBox& bb) : boundingBox(bb), left(nullptr), right(nullptr) {}
};

// 计算两点之间的欧几里得距离（三维空间）
double distance(const Point3D& p1, const Point3D& p2) {
    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
}

// 构建KD树，以包围盒为基础进行划分
KDNode* buildKDTree(const std::vector<BoundingBox>& boundingBoxes, const std::vector<int>& objectIndices, int depth) {
    if (boundingBoxes.empty()) {
        return nullptr;
    }

    int k = 3; // 三维空间
    int axis = depth % k;

    // 根据当前划分维度对包围盒进行排序
    std::vector<BoundingBox> sortedBoundingBoxes = boundingBoxes;
    if (axis == 0) {
        std::sort(sortedBoundingBoxes.begin(), sortedBoundingBoxes.end(), [](const BoundingBox& bb1, const BoundingBox& bb2) {
            return bb1.min.x < bb2.min.x;
        });
    } else if (axis == 1) {
        std::sort(sortedBoundingBoxes.begin(), sortedBoundingBoxes.end(), [](const BoundingBox& bb1, const BoundingBox& bb2) {
            return bb1.min.y < bb2.min.y;
        });
    } else {
        std::sort(sortedBoundingBoxes.begin(), sortedBoundingBoxes.end(), [](const BoundingBox& bb1, const BoundingBox& bb2) {
            return bb1.min.z < bb2.min.z;
        });
    }

    int medianIndex = sortedBoundingBoxes.size() / 2;
    KDNode* node = new KDNode(sortedBoundingBoxes[medianIndex]);

    std::vector<BoundingBox> leftBoundingBoxes(sortedBoundingBoxes.begin(), sortedBoundingBoxes.begin() + medianIndex);
    std::vector<BoundingBox> rightBoundingBoxes(sortedBoundingBoxes.begin() + medianIndex + 1, sortedBoundingBoxes.end());

    std::vector<int> leftObjectIndices;
    std::vector<int> rightObjectIndices;

    for (int i : objectIndices) {
        if (leftBoundingBoxes.back().contains(boundingBoxes[i])) {
            leftObjectIndices.push_back(i);
        } else if (rightBoundingBoxes.front().contains(boundingBoxes[i])) {
            rightObjectIndices.push_back(i);
        }
    }

    node->left = buildKDTree(leftBoundingBoxes, leftObjectIndices, depth + 1);
    node->right = buildKDTree(rightBoundingBoxes, rightObjectIndices, depth + 1);

    node->objectIndices = objectIndices;

    return node;
}

// 执行射线投射，通过KD树进行加速搜索以找到相交的物体
int rayCastPickup(KDNode* root, const Point3D& rayOrigin, const Point3D& rayDirection) {
    if (root == nullptr) {
        return -1; // -1表示未找到相交物体
    }

    int k = 3;
    int axis = depth % k;

    KDNode* nextBranch = nullptr;
    KDNode* otherBranch = nullptr;

    if ((axis == 0 && rayOrigin.x < root->boundingBox.min.x) || (axis == 1 && rayOrigin.y < root->boundingBox.min.y) ||
        (axis == 2 && rayOrigin.z < root->boundingBox.min.z)) {
        nextBranch = root->left;
        otherBranch = root->right;
    } else {
        nextBranch = root->right;
        otherBranch = root->left;
    }

    int candidateIndex = rayCastPickup(nextBranch, rayOrigin, rayDirection);

    if (candidateIndex!= -1) {
        return candidateIndex;
    }

    if (root->boundingBox.intersectsRay(rayOrigin, rayDirection)) {
        // 遍历该节点所包含的物体索引，进一步检查物体与射线是否真正相交
        for (int i : root->objectIndices) {
            // 这里假设可以通过物体索引获取到物体的详细信息，如包围盒等，并进行更精确的相交检测
            // 如果检测到相交，则返回该物体索引
            // 此处省略具体的物体相交检测代码，可根据实际情况补充
            if (true) {
                return i;
            }
        }
    }

    if (root->boundingBox.intersectsRay(rayOrigin, rayDirection)) {
        candidateIndex = rayCastPickup(otherBranch, rayOrigin, rayDirection);
        if (candidateIndex!= -1) {
            return candidateIndex;
        }
    }

    return -1;
}

int main() {
    // 示例数据，实际应用中这些数据需要根据具体场景生成
    std::vector<BoundingBox> boundingBoxes;
    std::vector<int> objectIndices;

    // 假设这里生成了一些包围盒和对应的物体索引数据
    boundingBoxes.push_back(BoundingBox(Point3D(1, 1, 1), Point3D(3, 3, 3)));
    objectIndices.push_back(0);
    boundingBoxes.push_back(BoundingBox(Point3D(4, 4, 4), Point3D(6, 6, 6)));
    objectIndices.push_back(1);

    KDNode* root = buildKDTree(boundingBoxes, objectIndices, 0);

    Point3D rayOrigin(0, 0, 0);
    Point3D rayDirection(1, 1, 1);

    int pickedObjectIndex = rayCastPickup(root, rayOrigin, rayDirection);

    if (pickedObjectIndex!= -1) {
        std::cout << "拾取到物体，索引为: " << pickedObjectIndex << std::endl;
    } else {
        std::cout << "未拾取到物体" << std::endl;
    }

    return 0;
}

以下是对上述代码的详细解释：

1. 数据结构定义

• Point3D：用于表示三维空间中的点，包含x、y、z三个坐标分量。
• BoundingBox：定义包围盒结构体，包含最小点（min）和最大点（max）两个成员，用于界定一个三维空间区域。同时提供了判断点是否在包围盒内以及射线是否与包围盒相交的函数。
• KDNode：KD树的节点结构体，包含一个包围盒（boundingBox）用于划分空间，左右子节点指针（left、right）以及一个向量（objectIndices）用于存储该节点所对应包围盒包含的物体索引。

2. 构建KD树

• buildKDTree函数以包围盒为基础构建KD树。首先根据当前划分维度（x、y、z轴循环）对包围盒进行排序，然后选取中位数对应的包围盒作为当前节点的包围盒。接着将物体索引根据其对应的包围盒所属的左右子树进行划分，分别构建左右子树。

3. 射线投射与拾取

• rayCastPickup函数执行射线投射以实现物体拾取。从KD树的根节点开始，根据射线的起始点（rayOrigin）与当前节点包围盒的关系确定下一步搜索的子树分支（nextBranch），并先在该分支中进行搜索。如果在该分支中未找到相交物体，则检查当前节点的包围盒是否与射线相交。如果相交，则遍历该节点所包含的物体索引，进一步检查物体与射线是否真正相交（此处省略了具体的物体相交检测代码，实际应用中需要根据物体的具体情况进行详细检测）。如果在当前节点及其子树中都未找到相交物体，则再检查另一个分支（otherBranch）。

4. 主函数

• 在main函数中，首先生成了一些示例的包围盒和对应的物体索引数据，然后构建KD树。接着定义了射线的起始点和方向，通过调用rayCastPickup函数执行射线投射并获取拾取到的物体索引，最后根据结果输出相应的信息。

请注意，上述代码只是一个简化的示例框架，在实际应用中，需要根据具体场景对以下方面进行完善：

• 物体相交检测：在rayCastPickup函数中，当检查节点所包含的物体与射线是否真正相交时，需要根据物体的具体情况（如物体的几何形状、表面特性等）进行详细的检测，这里只是简单假设可以通过物体索引获取到物体的详细信息并进行检测。
• 数据生成：在main函数中，示例数据只是简单给出了几个包围盒和物体索引，实际应用中需要根据具体场景准确生成这些数据，例如从三维模型文件中读取物体信息并计算其包围盒和对应索引等。
• 内存管理：代码中动态分配了KD树节点等内存，在实际应用中需要注意内存的释放，避免内存泄漏等问题。