HJBsByFDM/linearIntersection3d.m at main · janblechschmidt/HJBsByFDM · 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
125
126
127
128
129
130
131
function Xnew = linearIntersection3d(X, alpha, c)
% This function is intended to compute the points in a tetrahedron,
% where a condition alpha(x) = c is attained with equality.
% Typically, there are alpha(x) > c or alpha(x) < c.
% For tetrahedrons where both cases occur, a boundary alpha(x) = c should lie
% within the boundaries of this tetrahedron.
% If we assume a linear connection, the intersection is either a triangle or
% a quadrangle (if the case is not degenerated).

abool = (alpha <= c);
[n,m] = size(X);
Xnew = zeros(0,m);

if any(abool) ~= all(abool)
	k = 1;
	switch mod(sum(abool),2)
	case 0
		% Quadrilateral
		% Sorting is important, otherwise matlab will plot two triangles
		if allOrNothing(abool', [1, 0, 0, 1])
			i = 1; j = 2;
			l = (c - alpha(i)) / (alpha(j) - alpha(i));
			Xnew(1,:) = (1.-l) * X(i,:) + l * X(j,:);
			i = 2; j = 4;
			l = (c - alpha(i)) / (alpha(j) - alpha(i));
			Xnew(2,:) = (1.-l) * X(i,:) + l * X(j,:);
			i = 4; j = 3;
			l = (c - alpha(i)) / (alpha(j) - alpha(i));
			Xnew(3,:) = (1.-l) * X(i,:) + l * X(j,:);
			i = 3; j = 1;
			l = (c - alpha(i)) / (alpha(j) - alpha(i));
			Xnew(4,:) = (1.-l) * X(i,:) + l * X(j,:);
		else
			if allOrNothing(abool', [1, 1, 0, 0])
				i = 1; j = 3;
				l = (c - alpha(i)) / (alpha(j) - alpha(i));
				Xnew(1,:) = (1.-l) * X(i,:) + l * X(j,:);
				i = 3; j = 2;
				l = (c - alpha(i)) / (alpha(j) - alpha(i));
				Xnew(2,:) = (1.-l) * X(i,:) + l * X(j,:);
				i = 2; j = 4;
				l = (c - alpha(i)) / (alpha(j) - alpha(i));
				Xnew(3,:) = (1.-l) * X(i,:) + l * X(j,:);
				i = 4; j = 1;
				l = (c - alpha(i)) / (alpha(j) - alpha(i));
				Xnew(4,:) = (1.-l) * X(i,:) + l * X(j,:);
			else
				if allOrNothing(abool', [1, 0, 1, 0])
					i = 1; j = 2;
					l = (c - alpha(i)) / (alpha(j) - alpha(i));
					Xnew(1,:) = (1.-l) * X(i,:) + l * X(j,:);
					i = 2; j = 3;
					l = (c - alpha(i)) / (alpha(j) - alpha(i));
					Xnew(2,:) = (1.-l) * X(i,:) + l * X(j,:);
					i = 3; j = 4;
					l = (c - alpha(i)) / (alpha(j) - alpha(i));
					Xnew(3,:) = (1.-l) * X(i,:) + l * X(j,:);
					i = 4; j = 1;
					l = (c - alpha(i)) / (alpha(j) - alpha(i));
					Xnew(4,:) = (1.-l) * X(i,:) + l * X(j,:);
				else
					error('no case\n')
				end % if allOrNothing(abool', [1, 0, 1, 0])
			end % if allOrNothing(abool', [1, 1, 0, 0])
		end %if allOrNothing(abool', [1, 0, 0, 1])
	case 1
		% Triangle
		for i = 1:m
			for j = i+1:m+1
				if abool(i) ~= abool(j)
					l = (c - alpha(i)) / (alpha(j) - alpha(i));
					Xnew(k,:) = (1.-l) * X(i,:) + l * X(j,:);
					k = k + 1;
				end % if abool(i) ~= abool(j)
			end % for j = i+1:m+1
		end % for i = 1:m
	end % switch mod(sum(abool),2)
end % if any(abool) != all(abool):

	% 	if np.mod(sum(abool),2) == 1:
	% se:
	% 	# Intersection is quadrangle
	% 	Xnew = np.ndarray((dim+1,dim),dtype=np.double)
	% 	if allOrNothing(abool == np.array([True, False, False, True])):
	% 			# print('case 1')
	% 			i = 0; j = 1
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[0] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 1; j = 3
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[1] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 3; j = 2
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[2] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 2; j = 0
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[3] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			elif allOrNothing(abool == np.array([True, True, False, False])):
	% 			# print('case 2')
	% 			i = 0; j = 2
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[0] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 2; j = 1
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[1] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 1; j = 3
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[2] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 3; j = 0
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[3] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			elif allOrNothing(abool == np.array([True, False, True, False])):
	% 			# print('case 3')
	% 			i = 0; j = 1
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[0] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 1; j = 2
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[1] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 2; j = 3
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[2] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 			i = 3; j = 0
	% 			l = (c - alpha[i]) / (alpha[j] - alpha[i])
	% 			Xnew[3] = (1.-l) * Xalt[i] + l * Xalt[j]
	% 	else:
	% 			print('unknown case')
	% 			sys.exit()
	% 			return Xnew
	% 	else:
	% 			return False