#include "FITKAssInstanceTR.h"
#include "FITK_Kernel/FITKCore/FITKVec3DAlg.h"
 

namespace Interface
{
    void FITKAssemblyInstanceTR::setTransform(const double x, const double y, const double z)
    {
        //平移矢量
        _trans[0] = x;
        _trans[1] = y;
        _trans[2] = z;
    }

    void FITKAssemblyInstanceTR::setRotateAxis(double* p1, double* p2)
    {
        //旋转轴第一点与第二点
        for (int i = 0; i < 3; ++i )
        {
            _rotate1[i] = p1[i];
            _rotate2[i] = p2[i];
        }
    }

    void FITKAssemblyInstanceTR::setAngle(double a)
    {
        //旋转角度
        _angle = a;
    }

    void FITKAssemblyInstanceTR::getTransfom(double* transform) const
    {
        for (int i = 0; i < 3; ++i)
        {
            transform[i] = _trans[i];
        }
    }

    void FITKAssemblyInstanceTR::getRotateAxis(double* p1, double* p2) const
    {
        //旋转轴第一点与第二点
        for (int i = 0; i < 3; ++i)
        {
            p1[i] = _rotate1[i];
            p2[i] = _rotate2[i];
        }
    }

    double FITKAssemblyInstanceTR::getAngle() const
    {
        return _angle;
    }

    bool FITKAssemblyInstanceTR::getPointCoor(int pointID, double* coor, int modelIndex /*= 0*/)
    {
        FITKAbstractModel* model = this->getModel();
        if (model == nullptr) return false;
        //部件坐标
        double pt[3] = { 0,0,0 };
        bool ok = model->getPointCoor(pointID, pt, modelIndex);
        if (!ok) return false;
        //空间变换
        //计算轴线
        double axis[3] = { 0,0,0 };
        for (int i = 0; i < 3; ++i) axis[i] = _rotate2[i] - _rotate1[i];
        //旋转
        Core::FITKPoint FKp(pt);
        Core::FITKVec3D FKAxis(axis);
        Core::FITKPoint res = Core::RotateAroundAxis(FKp, FKAxis, _angle*FITK_PI /180.0);
        res.getCoor(pt);
        //平移
        for (int i = 0; i < 3; ++i)  
            coor[i] = pt[i] + _trans[i];

        return true;
    }


}