1 files changed, 79 insertions, 0 deletions
diff --git a/grampsmath.pas b/grampsmath.pas
new file mode 100644
index 0000000..6739273
--- /dev/null
+++ b/grampsmath.pas
@@ -0,0 +1,79 @@
+unit grampsmath;
+
+{$mode objfpc}{$H+}
+
+interface
+
+uses
+  Classes, SysUtils, grampstypen, math;
+
+function KreuzproduktZ(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended; inline;
+function QAbstand(P: tExtendedArray; A1,A2: tPerson): extended; inline;
+function Skalarprodukt(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended; inline;
+function rootDet(mat: array of tExtendedarray): extended;
+
+implementation
+
+function KreuzproduktZ(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended;
+begin
+  result:=            //   ( (A1-A2) x (B1-B2) ).z =
+      (P[A1.p1]-P[A2.p1])*(P[B1.p2]-P[B2.p2])      //   (A1.x - A2.x) * (B1.y - B2.y)
+    - (P[B1.p1]-P[B2.p1])*(P[A1.p2]-P[A2.p2]);     // - (B1.x - B2.x) * (A1.y - A2.y)
+(*  writeln((P[A1.p1]-P[A2.p1])*(P[B1.p2]-P[B2.p2]) - (P[B1.p1]-P[B2.p1])*(P[A1.p2]-P[A2.p2]));
+  writeln('(',P[A1.p1],' - ',P[A2.p1],') * (',P[B1.p2],' - ',P[B2.p2],') - (',P[B1.p1],' - ',P[B2.p1],') * (',P[A1.p2],' - ',P[A2.p2],')');
+  writeln('(',A1.p1,' - ',A2.p1,') * (',B1.p2,' - ',B2.p2,') - (',B1.p1,' - ',B2.p1,') * (',A1.p2,' - ',A2.p2,')'); *)
+end;
+
+function QAbstand(P: tExtendedArray; A1,A2: tPerson): extended;
+begin
+ result:=
+   sqr(P[A1.p1]-P[A2.p1]) +
+   sqr(P[A1.p2]-P[A2.p2]);
+end;
+
+function Skalarprodukt(P: tExtendedArray; A1,A2,B1,B2: tPerson): extended;
+begin
+ result:=
+   (P[A1.p1]-P[A2.p1])*(P[B1.p1]-P[B2.p1]) +
+   (P[A1.p2]-P[A2.p2])*(P[B1.p2]-P[B2.p2]);
+end;
+
+function rootDet(mat: array of tExtendedarray): extended;
+var
+  dim,i,j,k: longint;
+  fak,tmp:   extended;
+begin
+  dim:=length(mat);
+  result:=1;
+
+  for i:=0 to dim-1 do begin          // die zu eliminierende Spalte
+    k:=-1;
+    tmp:=0;
+    for j:=i to dim-1 do
+      if abs(mat[j,i])>tmp then begin
+        k:=j;
+        tmp:=abs(mat[j,i]);
+      end;
+
+    if tmp<1e-40 then begin           // nur noch 0en in Spalte i
+      result:=0;
+      break;
+    end;
+
+    if k<>i then                      // Zeile k und i tauschen
+      for j:=i to dim-1 do begin
+        tmp:=mat[i,j];
+        mat[i,j]:=mat[k,j];
+        mat[k,j]:=tmp;
+      end;
+    result:=result*power(abs(mat[i,i]),1/dim);
+    for j:=i+1 to dim-1 do begin
+      fak:=-mat[j,i]/mat[i,i];
+      for k:=i to dim-1 do
+        mat[j,k]:=mat[j,k]+fak*mat[i,k];
+    end;
+  end;
+end;
+
+end.
+