Put Item In Right Status By Pose Transform

#include <iostream>
#include <vtkSmartPointer.h>
#include <vtkSphereSource.h>
#include <vtkActor.h>
#include <vtkConeSource.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkPolyDataMapper.h>
#include <vtkLineSource.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkAxesActor.h>
#include <vtkProperty.h>
#include <vtkXMLPolyDataReader.h>
#include <vtkCharArray.h>
#include <vtkPointData.h>
#include <vtkConeSource.h>
#include <vtkColorTransferFunction.h>

using namespace std;

#define vtkSPtr vtkSmartPointer
#define vtkSPtrNew(Var, Type) vtkSPtr<Type> Var = vtkSPtr<Type>::New();

int main()
{
    vtkSPtrNew( coneSource, vtkConeSource );
    coneSource->Update();

    vtkSPtrNew( mapper, vtkPolyDataMapper );
    mapper->SetInputData( coneSource->GetOutput() );

    vtkSPtrNew( coneActor, vtkActor );
    coneActor->SetMapper( mapper );

    vtkSPtrNew( axesActor, vtkAxesActor );

    vtkSPtrNew( renderer, vtkRenderer );
    renderer->AddActor( axesActor );
    renderer->AddActor( coneActor );
    renderer->SetBackground( 0, 0, 0 );

    vtkSPtrNew( renderWindow, vtkRenderWindow );
    renderWindow->AddRenderer( renderer );

    vtkSPtrNew( renderWindowInteractor, vtkRenderWindowInteractor );
    renderWindowInteractor->SetRenderWindow( renderWindow );

    renderer->ResetCamera();
    renderWindow->Render();
    renderWindowInteractor->Start();
    return 0;
}

    // =============== move and rotate axes =================
    vtkSPtrNew( trans, vtkTransform );
    trans->Translate( 2, 2, 0 );
    trans->RotateZ( 45 );
    trans->Update();

    axesActor->SetUserTransform( trans );

    PointStruct xDir( 1, 0, 0 );
    trans->InternalTransformVector( xDir.point, xDir.point );
    PointStruct yDir( 0, 1, 0 );
    trans->InternalTransformVector( yDir.point, yDir.point );
    cout << "xDir " << xDir;
    cout << "yDir " << yDir;
    // =============== finished: move and rotate axes =================

    // =============== Use Pose Transform to put cone =================
    vtkSPtr<vtkTransform> poseTrans = CreatePoseTransfrom( PointStruct(0, 0, 0), PointStruct(1, 0, 0), PointStruct(0, 1, 0),
                                                           PointStruct(2, 2, 0), xDir, yDir );
    coneActor->SetUserTransform( poseTrans );
    // =============== finished: Use Pose Transform to put cone =================

vtkSPtr<vtkTransform> CreateLocalTrans(PointStruct origin, PointStruct xDir, PointStruct yDir)
{
    vtkSPtrNew( resultTrans, vtkTransform );
    PointStruct zDir = xDir ^ yDir;
    zDir.Unit();

    double elements1[16] = { xDir[0], xDir[1], xDir[2], 0,
                            yDir[0], yDir[1], yDir[2], 0,
                            zDir[0], zDir[1], zDir[2], 0,
                            0, 0, 0, 1 };

    resultTrans->Concatenate(elements1);     //rotation

    double elements2[16] = { 1, 0, 0, -origin[0],
                             0, 1, 0, -origin[1],
                             0, 0, 1, -origin[2],
                             0, 0, 0, 1 };

    resultTrans->Concatenate(elements2);    //translation
    resultTrans->Update();

    return resultTrans;
}


vtkSPtr<vtkTransform> CreatePoseTransfrom(PointStruct fromPos, PointStruct fromXDir, PointStruct fromYDir, PointStruct toPos, PointStruct toXDir, PointStruct toYDir)
{
    fromXDir.Unit();
    fromYDir.Unit();
    toXDir.Unit();
    toYDir.Unit();

    vtkSmartPointer<vtkTransform> localCoordSystem = CreateLocalTrans(fromPos, fromXDir, fromYDir);
    vtkSmartPointer<vtkTransform> worldCoordSystem = CreateLocalTrans(fromPos, fromXDir, fromYDir);
    worldCoordSystem->Inverse();
    worldCoordSystem->Update();

    PointStruct LocalToPosInFromCoor, LocalToXDirInFromCoor, LocalToYDirInFromCoor;
    localCoordSystem->InternalTransformPoint(toPos.point, LocalToPosInFromCoor.point);
    localCoordSystem->InternalTransformVector(toXDir.point, LocalToXDirInFromCoor.point);
    localCoordSystem->InternalTransformVector(toYDir.point, LocalToYDirInFromCoor.point);
    vtkSmartPointer<vtkTransform> toToPoseXFMInFromCoor = CreateLocalTrans(LocalToPosInFromCoor, LocalToXDirInFromCoor, LocalToYDirInFromCoor);
    toToPoseXFMInFromCoor->Inverse();
    toToPoseXFMInFromCoor->Update();

    //construct pose transform
    vtkSmartPointer<vtkTransform> ResultTrans = vtkSmartPointer<vtkTransform>::New();
    ResultTrans->Concatenate(worldCoordSystem->GetMatrix());
    ResultTrans->Concatenate(toToPoseXFMInFromCoor->GetMatrix());
    ResultTrans->Concatenate(localCoordSystem->GetMatrix());
    ResultTrans->Update();

    return ResultTrans;
}