CARTO/extractMeshSurfaceIntersection.m at master · peterhollender/CARTO · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
function [XSurf YSurf ZSurf CSurf] = extractMeshSurfaceIntersection(MeshObject,imData,imageApexZ,Depths)
%CARTO = CARTO_Pre2;
smoothingKernel = [1 1];
if ~exist('Depths','var')
    Depths = 0;
end

% ResIndices = CARTO.UserMapIndex.ResIndices;
% Apex = [];
% for fileIdx = 1:length(ResIndices);
%     ResIdx = ResIndices(fileIdx);
%     xd = 0;%get(ARFIHandle(i),'zData');
%     yd = -8.54;%get(ARFIHandle(i),'yData');
%     zd = 0;%get(ARFIHandle(i),'xData');
%     [apexX(fileIdx) apexY(fileIdx) apexZ(fileIdx)] = applyTransformation(xd,yd,zd,CARTO.Frames(ResIdx).XducerToWorldMatrix);
% end
% ApexX = mean(apexX);
% ApexY = mean(apexY);
% ApexZ = mean(apexZ);

[GlobalApex GlobalNormal GlobalVector FrameApex FrameNormals FrameVectors] = getCommonApex(imData.XducerToWorldMatrix,imageApexZ);

ApexX = GlobalApex(1);
ApexY = GlobalApex(2);
ApexZ = GlobalApex(3);

x = [];
y = [];
z = [];
c = [];
a = [];


frameIndices = 1:size(imData.XducerToWorldMatrix,3);
[Z X I] = ndgrid(imData.axial,imData.lat,frameIndices);
I(:) = 1:numel(I);
for frameIdx = frameIndices;
[Xt(:,:,frameIdx) Yt(:,:,frameIdx) Zt(:,:,frameIdx)] = applyTransformation(0*X(:,:,frameIdx),Z(:,:,frameIdx),X(:,:,frameIdx),imData.XducerToWorldMatrix(:,:,frameIdx));
end
Mask = ~isnan(imData.cData) & (imData.alphadata>0);
x = double(Xt(Mask));
y = double(Yt(Mask));
z = double(Zt(Mask));
c = double(imData.cData(Mask));
a = double(imData.alphadata(Mask));

    if nargout == 4
        InterpFunction = TriScatteredInterp([x y z],c);
    end

    mx = [];
    my = [];
    mz = [];
    for MeshIdx = 1:length(MeshObject)
    mx = [mx;MeshObject(MeshIdx).Vertices.X];
    my = [my;MeshObject(MeshIdx).Vertices.Y];
    mz = [mz;MeshObject(MeshIdx).Vertices.Z];
       end

    vectors = [x-ApexX y-ApexY z-ApexZ];

    vectorsRel = [dot(vectors,repmat(cross(GlobalNormal,GlobalVector),[length(x) 1]),2) dot(vectors,repmat(GlobalVector,[length(x),1]),2) dot(vectors,repmat(GlobalNormal,[length(x),1]),2)];
    [th phi r] = cart2sph(vectorsRel(:,1),vectorsRel(:,2),vectorsRel(:,3));

    mvectors = [mx-ApexX my-ApexY mz-ApexZ];
    mvectorsRel = [dot(mvectors,repmat(cross(GlobalNormal,GlobalVector),[length(mx) 1]),2) dot(mvectors,repmat(GlobalVector,[length(mx),1]),2) dot(mvectors,repmat(GlobalNormal,[length(mx),1]),2)];
    [mth mphi mr] = cart2sph(mvectorsRel(:,1),mvectorsRel(:,2),mvectorsRel(:,3));

    baseVec = [1 0 0;0 1 0;0 0 1];
    baseRel = [dot(baseVec,repmat(cross(GlobalNormal,GlobalVector),[3 1]),2) dot(baseVec,repmat(GlobalVector,[3 1]),2) dot(baseVec,repmat(GlobalNormal,[3 1]),2)];


    angleMargin = pi/16;
    meshCond = mth>=(min(th)-angleMargin) & mth<=(max(th)+angleMargin) & mphi>=(min(phi)-angleMargin) & mphi<=(max(phi)+angleMargin);
    mthMatch = mth(meshCond);
    mphiMatch = mphi(meshCond);
    mrMatch = mr(meshCond);

    angleMargin = 0;
    meshCond = mth>=(min(th)-angleMargin) & mth<=(max(th)+angleMargin) & mphi>=(min(phi)-angleMargin) & mphi<=(max(phi)+angleMargin);
    mthTightMatch = mth(meshCond);
    mphiTightMatch = mphi(meshCond);
    mrTightMatch = mr(meshCond);


    FM = TriScatteredInterp([mthMatch,mphiMatch],mrMatch,'natural');

    Ngrid = 128;
    dth = deg2rad(1);
    dphi = deg2rad(1);
    [TH PHI] = meshgrid(min(th):dth:max(th),min(phi):dphi:max(phi));
    %[TH PHI] = meshgrid(min(mthTightMatch):dth:max(mthTightMatch),min(mphiTightMatch):dphi:max(mphiTightMatch));

    trim = 0;
    TH = TH(trim+1:end-trim,trim+1:end-trim);
    PHI = PHI(trim+1:end-trim,trim+1:end-trim);
    R = FM(TH,PHI);

    for depthIdx = 1:length(Depths)
    [XRel YRel ZRel] = sph2cart(TH,PHI,R+Depths(depthIdx));
    X = XRel;
    Y = YRel;
    Z = ZRel;
    X(:) = dot([XRel(:) YRel(:) ZRel(:)],repmat(baseRel(1,:),[numel(XRel),1]),2);
    Y(:) = dot([XRel(:) YRel(:) ZRel(:)],repmat(baseRel(2,:),[numel(XRel),1]),2);
    Z(:) = dot([XRel(:) YRel(:) ZRel(:)],repmat(baseRel(3,:),[numel(XRel),1]),2);

    XSurf(:,:,depthIdx) = X+ApexX;
    YSurf(:,:,depthIdx) = Y+ApexY;
    ZSurf(:,:,depthIdx) = Z+ApexZ;


%     TriMeshHandle(depthIdx) = surf(X+ApexX,Y+ApexY,Z+ApexZ,mediannan(F(X+ApexX,Y+ApexY,Z+ApexZ),smoothingKernel));
%     set(TriMeshHandle(depthIdx),'EdgeColor','none');
%     if length(Depths)>1
%         hold on
%     end
    end
    if nargout==4
        CSurf = nan(size(XSurf));
        CSurf(:) = InterpFunction([XSurf(:) YSurf(:) ZSurf(:)]);
    end