5、VTK刚体变换

#include <vtkSmartPointer.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkPolyData.h>
#include <vtkMath.h>
#include <vtkCylinderSource.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkProperty.h>
#include <vtkAxesActor.h>
#include <vtkCamera.h>

#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL2);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType);

bool isValidDirection(const double vec[3]) {
    return (vec[0] != 0.0 || vec[1] != 0.0 || vec[2] != 0.0);
}

// 修正后的变换计算：从 (point1, vec1) 到 (point2, vec2)
vtkSmartPointer<vtkTransform> computeRigidTransform(double point1[3], double vec1[3],
                                                    double point2[3], double vec2[3]) {
    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();

    if (!isValidDirection(vec1) || !isValidDirection(vec2)) {
        // 无效方向时只平移
        transform->Translate(point2[0] - point1[0], point2[1] - point1[1], point2[2] - point1[2]);
        return transform;
    }

    double v1[3] = {vec1[0], vec1[1], vec1[2]};
    double v2[3] = {vec2[0], vec2[1], vec2[2]};
    vtkMath::Normalize(v1);
    vtkMath::Normalize(v2);

    double axis[3];
    double dot = vtkMath::Dot(v1, v2);
    double angleRad = acos(dot);
    double angleDeg = vtkMath::DegreesFromRadians(angleRad);
    double eps = 1e-6;

    if (fabs(angleRad) < eps) {
        // 方向相同，无需旋转
        axis[0] = axis[1] = axis[2] = 0;
        angleDeg = 0.0;
    } else if (fabs(angleRad - vtkMath::Pi()) < eps) {
        // 方向相反，找一个垂直于 v1 的轴
        if (fabs(v1[0]) < eps) {
            axis[0] = 1; axis[1] = 0; axis[2] = 0;
        } else {
            axis[0] = -v1[1]; axis[1] = v1[0]; axis[2] = 0;
            vtkMath::Normalize(axis);
        }
        angleDeg = 180.0;
    } else {
        vtkMath::Cross(v1, v2, axis);
        vtkMath::Normalize(axis);
    }

    // 正确的变换顺序（后乘规则）：
    // 1. 平移到原点  2. 旋转  3. 平移到目标点
    transform->Translate(point2[0], point2[1], point2[2]);
    transform->RotateWXYZ(angleDeg, axis[0], axis[1], axis[2]);
    transform->Translate(-point1[0], -point1[1], -point1[2]);

    return transform;
}

int main() {
    const double height = 2.0;

    // 创建一个圆柱源（默认：中心在原点，沿 Y 轴，范围 -1 到 1）
    auto cylinder = vtkSmartPointer<vtkCylinderSource>::New();
    cylinder->SetHeight(height);
    cylinder->SetRadius(0.1);
    cylinder->SetCenter(0, 0, 0);   // 中心在原点，则底端在 (0, -height/2, 0)，顶端在 (0, height/2, 0)

    // 定义三个位姿
    // 默认位姿（圆柱原始状态）：底端在 (0, -1, 0)，方向 (0,1,0)
    double defaultPoint[3] = {0, -height/2, 0};
    double defaultVec[3]  = {0, 1, 0};

    // 中间位姿 (point1, vec1)
    double point1[3] = {0, height/2, 0};   // 底端在 (0,1,0)
    double vec1[3]   = {1, 0, 0};          // 方向沿 X 轴

    // 最终位姿 (point2, vec2)
    double point2[3] = {2, 2, 2};
    double vec2[3]   = {1, 1, 1};

    // ---- 1. 默认圆柱（灰色） ----
    auto mapperDefault = vtkSmartPointer<vtkPolyDataMapper>::New();
    mapperDefault->SetInputConnection(cylinder->GetOutputPort());
    auto actorDefault = vtkSmartPointer<vtkActor>::New();
    actorDefault->SetMapper(mapperDefault);
    actorDefault->GetProperty()->SetColor(0.5, 0.5, 0.5);
    actorDefault->GetProperty()->SetOpacity(0.5);

    // ---- 2. 从默认变换到中间位姿（青色半透明） ----
    auto transformToMid = computeRigidTransform(defaultPoint, defaultVec, point1, vec1);
    auto filterMid = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    filterMid->SetInputConnection(cylinder->GetOutputPort());
    filterMid->SetTransform(transformToMid);
    filterMid->Update();
    auto mapperMid = vtkSmartPointer<vtkPolyDataMapper>::New();
    mapperMid->SetInputConnection(filterMid->GetOutputPort());
    auto actorMid = vtkSmartPointer<vtkActor>::New();
    actorMid->SetMapper(mapperMid);
    actorMid->GetProperty()->SetColor(0, 1, 1);
    actorMid->GetProperty()->SetOpacity(0.3);

    // ---- 3. 从中间位姿变换到最终位姿（蓝色实体） ----
    auto transformToFinal = computeRigidTransform(point1, vec1, point2, vec2);
    transformToFinal->Concatenate(transformToMid);

    auto filterFinal = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    filterFinal->SetInputConnection(cylinder->GetOutputPort());
    //filterFinal->SetInputConnection(filterMid->GetOutputPort());
    filterFinal->SetTransform(transformToFinal);
    filterFinal->Update();

    auto mapperFinal = vtkSmartPointer<vtkPolyDataMapper>::New();
    mapperFinal->SetInputConnection(filterFinal->GetOutputPort());

    auto actorFinal = vtkSmartPointer<vtkActor>::New();
    actorFinal->SetMapper(mapperFinal);
    actorFinal->GetProperty()->SetColor(0, 0, 1);

    // 坐标轴辅助观察
    auto axes = vtkSmartPointer<vtkAxesActor>::New();
    axes->SetTotalLength(6, 6, 6);

    auto renderer = vtkSmartPointer<vtkRenderer>::New();
    renderer->AddActor(actorDefault);
    renderer->AddActor(actorMid);
    renderer->AddActor(actorFinal);
    renderer->AddActor(axes);
    renderer->SetBackground(0.1, 0.2, 0.3);

    auto renderWindow = vtkSmartPointer<vtkRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    renderWindow->SetSize(800, 600);

    renderer->GetActiveCamera()->SetFocalPoint(0, 0, 0);
    renderer->GetActiveCamera()->SetPosition(10, -1, 30);

    auto interactor = vtkSmartPointer<vtkRenderWindowInteractor>::New();
    interactor->SetRenderWindow(renderWindow);
    renderWindow->Render();
    interactor->Start();

    return 0;
}