Annotation of OpenXM_contrib/PHC/Ada/Math_Lib/Supports/floating_linear_inequality_solvers.adb, Revision 1.1.1.1
1.1 maekawa 1: package body Floating_Linear_Inequality_Solvers is
2:
3: -- AUXILIARIES :
4:
5: function Is_In ( v : Standard_Integer_Vectors.Vector;
6: i : integer ) return boolean is
7:
8: -- DESCRIPTION :
9: -- Returns true if the number i appears in one of the elements of v.
10:
11: begin
12: for j in v'range loop
13: if v(j) = i
14: then return true;
15: end if;
16: end loop;
17: return false;
18: end Is_In;
19:
20: function Is_All_In ( v : Standard_Integer_Vectors.Vector; i : integer )
21: return boolean is
22:
23: -- DESCRIPTION :
24: -- Returns true if v(k) = i, for k in v'range, false otherwise.
25:
26: begin
27: for k in v'range loop
28: if v(k) /= i
29: then return false;
30: end if;
31: end loop;
32: return true;
33: end Is_All_In;
34:
35: procedure Copy ( m1 : in Matrix; m2 : out Matrix ) is
36:
37: -- REQUIRED : ranges must be equal.
38:
39: -- DESCRIPTION :
40: -- Copies the first matrix m1 onto the second matrix m2.
41: -- The statement `m2 := m1' does not produce the same result when
42: -- compiled with the VADS compiler on IBM RS/6000.
43:
44: begin
45: for i in m1'range(1) loop
46: for j in m1'range(2) loop
47: m2(i,j) := m1(i,j);
48: end loop;
49: end loop;
50: end Copy;
51:
52: function Inner_Product ( m : Matrix; i,j : integer ) return double_float is
53:
54: -- DESCRIPTION :
55: -- Returns the inner product of the normals to the ith and jth hyperplane.
56:
57: res : double_float := 0.0;
58:
59: begin
60: for k in m'first(1)..m'last(1)-1 loop
61: res := res + m(k,i)*m(k,j);
62: end loop;
63: return res;
64: end Inner_Product;
65:
66: -- AUXILIARIES FOR RECURSION ON THE DIMENSION :
67:
68: function Pivot ( m : Matrix; i : integer ) return integer is
69:
70: -- DESCRIPTION :
71: -- Returns the index of the coefficient with largest absolute value
72: -- of the ith inequality.
73:
74: res : integer := m'first(1);
75: max : double_float := abs(m(res,i));
76: tmpabs : double_float;
77:
78: begin
79: for j in m'first(1)+1..m'last(1)-1 loop
80: tmpabs := abs(m(j,i));
81: if tmpabs > max
82: then max := tmpabs; res := j;
83: end if;
84: end loop;
85: return res;
86: end Pivot;
87:
88: procedure Eliminate ( m1 : in Matrix; i,j,piv : in integer;
89: tol : in double_float; m2 : out Matrix ) is
90:
91: -- DESCRIPTION :
92: -- Performs the elimination on the jth column of the original matrix m1
93: -- of inequalities to the reduced matrix m2.
94:
95: -- REQUIRED : m(piv,i) /= 0 and dimensions of m2 must match.
96:
97: fac : double_float;
98:
99: begin
100: if abs(m1(piv,j)) < tol
101: then for k in m1'range(1) loop
102: if k < piv
103: then m2(k,j) := m1(k,j);
104: elsif k > piv
105: then m2(k-1,j) := m1(k,j);
106: end if;
107: end loop;
108: else fac := m1(piv,j)/m1(piv,i);
109: for k in m1'range(1) loop
110: if k < piv
111: then m2(k,j) := m1(k,j) - fac*m1(k,i);
112: elsif k > piv
113: then m2(k-1,j) := m1(k,j) - fac*m1(k,i);
114: end if;
115: end loop;
116: end if;
117: end Eliminate;
118:
119: function Eliminate ( m : Matrix; mlast2,i,piv : integer;
120: tol : in double_float ) return Matrix is
121:
122: -- DESCRIPTION :
123: -- Eliminates one unknown from the system of inequalities.
124:
125: -- REQUIRED : m(piv,i) /= 0 and m'last(1) > 2.
126:
127: res : Matrix(m'first(1)..m'last(1)-1,m'first(2)..mlast2);
128:
129: begin
130: for j in res'range(2) loop
131: Eliminate(m,i,j,piv,tol,res);
132: end loop;
133: return res;
134: end Eliminate;
135:
136: function Eliminate ( m : Matrix; mlast2,i : integer; tol : double_float )
137: return Matrix is
138:
139: -- DESCRIPTION :
140: -- Computes the pivot for the ith inequality and eliminates one unknown.
141: -- The matrix has as columns m'first(2)..mlast2.
142:
143: -- REQUIRED : Inner_Product(m,i,i) > 0 and m'last(1) > 2.
144:
145: piv : constant integer := Pivot(m,i);
146:
147: begin
148: return Eliminate(m,mlast2,i,piv,tol);
149: end Eliminate;
150:
151: function Back_Substitute ( m : Matrix; i,piv : integer; x : Vector )
152: return Vector is
153:
154: -- DESCRIPTION :
155: -- Returns the back substitution of the vector x which lies on the ith
156: -- hyperplane of original inequality system before elimination.
157:
158: -- REQUIRED : abs(m(piv,i)) > tolerance.
159:
160: res : Vector(x'first..x'last+1);
161:
162: begin
163: res(res'first..piv-1) := x(x'first..piv-1);
164: res(piv+1..res'last) := x(piv..x'last);
165: res(piv) := m(m'last(1),i);
166: for k in m'first(1)..m'last(1)-1 loop
167: if k < piv
168: then res(piv) := res(piv) - m(k,i)*x(k);
169: elsif k > piv
170: then res(piv) := res(piv) - m(k,i)*x(k-1);
171: end if;
172: end loop;
173: res(piv) := res(piv)/m(piv,i);
174: return res;
175: end Back_Substitute;
176:
177: -- SELECTORS :
178:
179: function Evaluate ( m : Matrix; i : integer; x : Vector )
180: return double_float is
181:
182: -- DESCRIPTION :
183: -- Evaluates the vector x in the ith inequality.
184:
185: res : double_float := 0.0;
186:
187: begin
188: for j in x'range loop
189: res := res + m(j,i)*x(j);
190: end loop;
191: return res;
192: end Evaluate;
193:
194: function Satisfies ( m : Matrix; i : integer; x : Vector; tol : double_float )
195: return boolean is
196: begin
197: return (Evaluate(m,i,x) >= m(m'last(1),i) - tol);
198: end Satisfies;
199:
200: function Satisfies ( m : Matrix; x : Vector; tol : double_float )
201: return boolean is
202: begin
203: for i in m'range(2) loop
204: if not Satisfies(m,i,x,tol)
205: then return false;
206: end if;
207: end loop;
208: return true;
209: end Satisfies;
210:
211: function Satisfies ( m : Matrix; fst,lst : integer;
212: x : Vector; tol : double_float ) return boolean is
213: begin
214: for i in fst..lst loop
215: if not Satisfies(m,i,x,tol)
216: then return false;
217: end if;
218: end loop;
219: return true;
220: end Satisfies;
221:
222: function First_Violated ( m : Matrix; x : Vector; tol : double_float )
223: return integer is
224: begin
225: for i in m'range(2) loop
226: if not Satisfies(m,i,x,tol)
227: then return i;
228: end if;
229: end loop;
230: return (m'last(2)+1);
231: end First_Violated;
232:
233: function First_Violated ( m : Matrix; fst,lst : integer;
234: x : Vector; tol : double_float ) return integer is
235: begin
236: for i in fst..lst loop
237: if not Satisfies(m,i,x,tol)
238: then return i;
239: end if;
240: end loop;
241: return (lst+1);
242: end First_Violated;
243:
244: -- INCONSISTENCY CHECKS :
245:
246: function Inconsistent ( m : Matrix; i : integer; tol : double_float )
247: return boolean is
248: begin
249: for j in m'first(1)..m'last(1)-1 loop
250: if abs(m(j,i)) > tol
251: then return false;
252: end if;
253: end loop;
254: return (m(m'last(1),i) > tol);
255: end Inconsistent;
256:
257: function Inconsistent ( m : Matrix; cols : Standard_Integer_Vectors.Vector;
258: x : Vector; tol : double_float ) return boolean is
259:
260: -- ALGORITHM : checks whether the inequality 0 >= c, with c > tol,
261: -- can be derived from the combination of the inequalities.
262:
263: sum : double_float;
264:
265: begin
266: for i in x'range loop -- x should be a positive combination
267: if x(i) < 0.0
268: then return false;
269: end if;
270: end loop;
271: for i in m'first(1)..m'last(1)-1 loop -- check zero left hand side
272: sum := 0.0;
273: for j in x'range loop
274: sum := sum + x(j)*m(i,cols(j));
275: end loop;
276: if abs(sum) > tol
277: then return false;
278: end if;
279: end loop;
280: sum := 0.0; -- right hand side must be positive
281: for j in x'range loop
282: sum := sum + x(j)*m(m'last(1),cols(j));
283: end loop;
284: return (sum > tol);
285: end Inconsistent;
286:
287: function Inconsistent ( m : Matrix; i : integer;
288: cols : Standard_Integer_Vectors.Vector; x : Vector;
289: tol : double_float ) return boolean is
290:
291: allcols : Standard_Integer_Vectors.Vector(cols'first..cols'last+1);
292: allcoef : Vector(x'first..x'last+1);
293:
294: begin
295: allcols(cols'range) := cols; allcoef(x'range) := x;
296: allcols(allcols'last) := i;
297: if Is_In(cols,i)
298: then allcoef(allcoef'last) := 0.0;
299: else allcoef(allcoef'last) := 1.0;
300: end if;
301: return Inconsistent(m,allcols,allcoef,tol);
302: end Inconsistent;
303:
304: function Inconsistent2D ( m : Matrix; cols : Standard_Integer_Vectors.Vector;
305: tol : double_float ) return Vector is
306:
307: -- REQUIRED : the normals in the corresponding inequality are opposite.
308:
309: -- DESCRIPTION :
310: -- Returns the coefficients in the inconsistency proof.
311:
312: res : Vector(cols'range) := (cols'range => 1.0);
313: f1,f2 : double_float;
314:
315: begin
316: for i in m'first(1)..m'last(1)-1 loop
317: f1 := abs(m(i,cols(1)));
318: if f1 > tol
319: then f2 := abs(m(i,cols(2)));
320: if f1 > f2
321: then res(2) := 1.0; res(1) := f2/f1;
322: else res(1) := 1.0; res(2) := f1/f2;
323: end if;
324: end if;
325: exit when (f1 > tol);
326: end loop;
327: return res;
328: end Inconsistent2D;
329:
330: function Inconsistent2D ( m : Matrix; i : integer;
331: cols : Standard_Integer_Vectors.Vector;
332: tol : double_float ) return Vector is
333:
334: -- REQUIRED : the inequalities of the columns define a nonsingular system.
335:
336: -- DESCRIPTION :
337: -- Returns the coefficients in the inconsistency proof.
338:
339: res : Vector(cols'range) := (cols'range => 1.0);
340: detm12 : constant double_float
341: := m(1,cols(1))*m(2,cols(2)) - m(2,cols(1))*m(1,cols(2));
342:
343: begin
344: if abs(detm12) > tol
345: then res(1) := (m(2,i)*m(1,cols(2)) - m(1,i)*m(2,cols(2)))/detm12;
346: res(2) := (m(1,i)*m(2,cols(1)) - m(2,i)*m(1,cols(1)))/detm12;
347: end if;
348: return res;
349: end Inconsistent2D;
350:
351: function InconsistentnD ( m : Matrix; i,piv : integer; x : Vector;
352: cols : Standard_Integer_Vectors.Vector;
353: k : integer; tol : double_float ) return Vector is
354:
355: -- DESCRIPTION :
356: -- Computes the coefficients of the inconsistency proof, based on the
357: -- inconsistency proof of the eliminated problem.
358:
359: res : Vector(x'first..x'last+1);
360: fac : double_float;
361:
362: procedure Compute_Factor is
363: begin
364: for j in x'range loop
365: fac := fac + x(j)*m(piv,cols(j));
366: end loop;
367: fac := -fac/m(piv,i);
368: if abs(fac) > tol
369: then for j in x'range loop
370: res(j) := x(j)/fac;
371: end loop;
372: else res(x'range) := x;
373: end if;
374: end Compute_Factor;
375:
376: begin
377: if Is_In(cols,k)
378: then res := (res'range => 0.0);
379: if Is_All_In(cols,k)
380: then res(res'first) := -m(piv,i)/m(piv,k);
381: else fac := 0.0;
382: Compute_Factor;
383: end if;
384: else fac := m(piv,k);
385: Compute_Factor;
386: if abs(fac) > tol
387: then res(res'last) := 1.0/fac;
388: else res(res'last) := 1.0;
389: end if;
390: end if;
391: return res;
392: end InconsistentnD;
393:
394: -- CONSTRUCTORS :
395:
396: procedure Scale ( m : in out Matrix; i : in integer;
397: tol : in double_float ) is
398:
399: ip : double_float := abs(m(Pivot(m,i),i)); --Inner_Product(m,i,i);
400:
401: begin
402: if ip > tol
403: then --ip := SQRT(ip);
404: for j in m'range(1) loop
405: m(j,i) := m(j,i)/ip;
406: end loop;
407: end if;
408: end Scale;
409:
410: procedure Scale ( m : in out Matrix; tol : in double_float ) is
411: begin
412: for i in m'range(2) loop
413: Scale(m,i,tol);
414: end loop;
415: end Scale;
416:
417: procedure Center ( m : in out Matrix; x : in Vector ) is
418: begin
419: for i in m'range(2) loop
420: m(m'last(1),i) := m(m'last(1),i) - Evaluate(m,i,x);
421: end loop;
422: -- put_line("The centered inequality system : "); put(m,3,3,3);
423: end Center;
424:
425: function Center ( m : Matrix; x : Vector ) return Matrix is
426:
427: mx : Matrix(m'range(1),m'range(2)); -- := m; problems on RS/6000 ??
428:
429: begin
430: Copy(m,mx);
431: Center(mx,x);
432: return mx;
433: end Center;
434:
435: procedure Intersect2D ( m : in Matrix; i,j : in integer;
436: tol : in double_float;
437: x : out Vector; sing : out boolean ) is
438:
439: detmij : constant double_float := m(1,i)*m(2,j) - m(2,i)*m(1,j);
440:
441: begin
442: if abs(detmij) <= tol
443: then sing := true;
444: else sing := false;
445: x(1) := (m(3,i)*m(2,j) - m(2,i)*m(3,j))/detmij;
446: x(2) := (m(1,i)*m(3,j) - m(3,i)*m(1,j))/detmij;
447: end if;
448: -- put(" i : "); put(i,1); put(" j : "); put(j,1);
449: -- put(" detmij : "); put(detmij,3,3,3); new_line;
450: end Intersect2D;
451:
452: -- SOLVE BY INTERSECTION :
453:
454: procedure Solve_Intersect2D
455: ( m : in Matrix; i : in integer; tol : in double_float;
456: x : in out Vector; fail : out boolean;
457: cols : out Standard_Integer_Vectors.Vector ) is
458:
459: -- DESCRIPTION :
460: -- Enumerates all intersection points of the ith hyperplane with all the
461: -- other previous ones. The enumeration stops when either the current
462: -- intersection point satisfies the system of inequalities or when the
463: -- inequalities for the inconsistency proof are detected.
464:
465: -- REQUIRED : m'last(1) = 3 and the inequalities are centered.
466:
467: firstviol,j : integer;
468: ffail,sing : boolean := true;
469: columns : Standard_Integer_Vectors.Vector(x'range) := (x'range => 0);
470:
471: begin
472: j := m'first(2);
473: loop
474: Intersect2D(m,j,i,tol,x,sing);
475: if not sing
476: then firstviol := First_Violated(m,m'first(2),i-1,x,tol);
477: if firstviol < j
478: then columns(1) := firstviol; columns(2) := j; sing := true;
479: x := Inconsistent2D(m,i,columns,tol);
480: end if;
481: else if Inner_Product(m,i,j) < -tol
482: then sing := false; x := (x'range => 0.0);
483: for k in m'first(1)..m'last(1)-1 loop
484: if abs(m(k,i)) > tol
485: then x(k) := m(3,i)/m(k,i);
486: if m(k,i) > 0.0
487: then sing := ((x(k) - m(3,j)/m(k,j)) > tol);
488: else sing := ((m(3,j)/m(k,j) - x(k)) > tol);
489: end if;
490: end if;
491: exit when (abs(m(k,i)) > tol);
492: end loop;
493: if sing
494: then columns(1) := j; columns(2) := i;
495: x := Inconsistent2D(m,columns,tol);
496: end if;
497: else sing := false;
498: end if;
499: if sing
500: then firstviol := j;
501: else firstviol := j+1;
502: end if;
503: end if;
504: ffail := (firstviol < i);
505: exit when not ffail or sing;
506: j := firstviol;
507: end loop;
508: fail := ffail;
509: cols := columns;
510: end Solve_Intersect2D;
511:
512: procedure Solve_IntersectnD
513: ( m : in Matrix; i : in integer; tol : in double_float;
514: x : in out Vector; fail : out boolean;
515: cols : out Standard_Integer_Vectors.Vector ) is
516:
517: -- DESCRIPTION :
518: -- Eliminates one unknown by intersecting with the ith hyperplane.
519:
520: -- REQUIRED : m'last(1) > 3 and the inequalities are centered.
521:
522: m2 : Matrix(m'first(1)..m'last(1)-1,m'first(2)..i-1);
523: piv : constant integer := Pivot(m,i);
524: x2 : Vector(x'first..x'last-1);
525: cols2 : Standard_Integer_Vectors.Vector(x2'range);
526: k2 : integer;
527:
528: begin
529: if abs(m(piv,i)) > tol
530: then m2 := Eliminate(m,i-1,i,piv,tol);
531: x2 := (x2'range => 0.0);
532: Solve(m2,tol,x2,k2,cols2);
533: if k2 >= i
534: then fail := false;
535: x := Back_Substitute(m,i,piv,x2);
536: else fail := true;
537: cols(cols2'range) := cols2;
538: cols(cols2'last+1) := k2;
539: x := InconsistentnD(m,i,piv,x2,cols2,k2,tol);
540: end if;
541: end if;
542: end Solve_IntersectnD;
543:
544: procedure Solve_Intersect
545: ( m : in Matrix; i : in integer; tol : in double_float;
546: x : in out Vector; fail : out boolean;
547: cols : out Standard_Integer_Vectors.Vector ) is
548:
549: -- DESCRIPTION :
550: -- Intersects the inequalities with the ith hyperplane.
551:
552: -- REQUIRED : the inequalities are centered,
553: -- the ith inequality is first one that is violated.
554:
555: begin
556: if m'last(1) = 3
557: then Solve_Intersect2D(m,i,tol,x,fail,cols);
558: elsif m'last(1) > 3
559: then Solve_IntersectnD(m,i,tol,x,fail,cols);
560: end if;
561: end Solve_Intersect;
562:
563: procedure Solve ( m : in Matrix; i : in integer; tol : in double_float;
564: x : in out Vector; fail : out boolean;
565: cols : out Standard_Integer_Vectors.Vector ) is
566:
567: inx,xx : Vector(x'range) := (x'range => 0.0);
568: wrk : Matrix(m'range(1),m'range(2));
569: sing : boolean := true;
570: fac : double_float;
571:
572: begin
573: if abs(Inner_Product(m,i,i)) < tol
574: then if m(m'last(1),i) > tol
575: then fail := true; cols := (x'range => i);
576: else fail := false;
577: end if;
578: else if i = m'first(2)
579: then x := (x'range => 0.0);
580: for j in x'range loop
581: if abs(m(j,i)) > tol
582: then sing := false;
583: x(j) := m(m'last(1),i)/m(j,i);
584: end if;
585: exit when not sing;
586: end loop;
587: fail := sing;
588: else Copy(m,wrk);
589: if x /= inx
590: then Center(wrk,x);
591: end if;
592: fac := wrk(wrk'last(1),i)/Inner_Product(wrk,i,i);
593: for j in inx'range loop
594: inx(j) := fac*wrk(j,i);
595: end loop;
596: if First_Violated(wrk,inx,tol) > i
597: then xx := x + inx;
598: if Satisfies(m,xx,tol)
599: then sing := false;
600: end if;
601: end if;
602: if not sing
603: then fail := false; x := xx;
604: else Solve_Intersect(wrk,i,tol,inx,sing,cols);
605: fail := sing;
606: if not sing
607: then Add(x,inx);
608: else x := inx;
609: end if;
610: end if;
611: end if;
612: end if;
613: end Solve;
614:
615: procedure Solve ( m : in Matrix; tol : in double_float; x : in out Vector;
616: k : out integer;
617: cols : out Standard_Integer_Vectors.Vector ) is
618:
619: indviol : integer := First_Violated(m,x,tol);
620: fail : boolean := false;
621:
622: begin
623: while indviol <= m'last(2) loop
624: Solve(m,indviol,tol,x,fail,cols);
625: exit when fail;
626: indviol := First_Violated(m,indviol+1,m'last(2),x,tol);
627: end loop;
628: if fail
629: then k := indviol;
630: else k := m'last(2) + 1;
631: end if;
632: end Solve;
633:
634: procedure Iterated_Solve ( m : in Matrix; tol : in double_float;
635: x : in out Vector; k : out integer;
636: cols : out Standard_Integer_Vectors.Vector ) is
637:
638: indviol : integer := First_Violated(m,x,tol);
639: fail : boolean := false;
640: continue : boolean := true;
641:
642: begin
643: while indviol <= m'last(2) loop
644: Report(x,indviol-1,continue);
645: exit when not continue;
646: Solve(m,indviol,tol,x,fail,cols);
647: exit when fail;
648: indviol := First_Violated(m,indviol+1,m'last(2),x,tol);
649: end loop;
650: if fail
651: then k := indviol;
652: else k := m'last(2) + 1;
653: end if;
654: end Iterated_Solve;
655:
656: end Floating_Linear_Inequality_Solvers;
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>