unit UMathInterpolation;

{$mode objfpc}{$H+}

interface

uses
  SysUtils, Matrix;

type
  TIntMode3D = (IM_DELAUNAY); // interpolation mode

  TTriangle = record
    a,b,c: Integer; // index of TriangleArray representing 3 edge points
  end;

  TTriangleArray = array of TTriangle; // list of triangles

  TCircle = record
    Center: Tvector2_double_data; // circle center
    Radius: Single; // circle radius
  end;

  TDataPoints = array of Tvector3_double_data;

  { TInterpolation3D }

  TInterpolation3D = class(TObject)
  strict private
    fInterpolationMode: TIntMode3D;
    fData: TDataPoints;
    fxmin,fxmax,fymin,fymax,fzmin,fzmax: Double;
    //  Delaunay-Triangles & Barycentric Interpolation
    fTriangles: TTriangleArray;
    function CalcTriangleArea(a,b,c: Tvector3_double_data): Double;
    function PointIsInTriangle(ax,ay: Double; iNN1,iNN2,iNN3:Integer): Boolean;
    function PointsToCircumCircle(Const a,b,c: Tvector2_double): TCircle;
    function Points2DToDelaunayTriangleList(CheckForDoublePoints: Boolean = True): TTriangleArray;
    function GetIntDataDelaunay(x,y: Double): Double;
    // misc.
    procedure SetZmin(const AValue: Double);
    procedure SetZmax(const AValue: Double);
  public
    property Data: TDataPoints read fData;
    property InterpolationMode: TIntMode3D read fInterpolationMode write fInterpolationMode default IM_DELAUNAY;
    property Triangles: TTriangleArray read fTriangles;
    property xmin: Double read fxmin write fxmin;
    property xmax: Double read fxmax write fxmax;
    property ymin: Double read fymin write fymin;
    property ymax: Double read fymax write fymax;
    property zmin: Double read fzmin write SetZmin;
    property zmax: Double read fzmax write SetZmax;
    procedure ClearData;
    procedure AddData(xMeas,yMeas,zMeas: Double);
    function GetIntData(x,y: Double): Double;
    function GetNomalizedIntData(x,y: Double): Double;
  end;


implementation

uses Dialogs,
     Math;

{ TInterpolation3D }
{******************************************************************************}
function TInterpolation3D.CalcTriangleArea(a,b,c: Tvector3_double_data): Double;
// Code by Corpsman (corpsman@corpsman.de)
var d1x,d2x,d1y,d2y: Double;
begin
  d1x := b[0] - a[0];
  d1y := b[1] - a[1];
  d2x := c[0] - a[0];
  d2y := c[1] - a[1];
  Result := 0.5 * (d1x*d2y - d2x*d1y);
  If Result = 0 then Result := 1E-15;
end;
{******************************************************************************}
function TInterpolation3D.PointIsInTriangle(ax,ay: Double; iNN1,iNN2,iNN3: Integer): Boolean;
var bx,by,cx,cy: Double;
    tmp: Double;
begin
  Result := true;

  bx := fData[iNN2,0] - fData[iNN1,0];
  by := fData[iNN2,1] - fData[iNN1,1];
  cx := ax - fData[iNN1,0];
  cy := ay - fData[iNN1,1];
  tmp := bx*cy - by*cx;

  bx := fData[iNN3,0] - fData[iNN2,0];
  by := fData[iNN3,1] - fData[iNN2,1];
  cx := ax - fData[iNN2,0];
  cy := ay - fData[iNN2,1];
  If Sign(tmp)*(bx*cy - by*cx) < 0 then begin Result := false; Exit; end;

  bx := fData[iNN1,0] - fData[iNN3,0];
  by := fData[iNN1,1] - fData[iNN3,1];
  cx := ax - fData[iNN3,0];
  cy := ay - fData[iNN3,1];
  If Sign(tmp)*(bx*cy - by*cx) < 0 then Result := false;
end;
{******************************************************************************}
function TInterpolation3D.PointsToCircumCircle(const a,b,c: Tvector2_double): TCircle;
// Calculate circumcircle for 3 points a,b,c
// Radius = negativ --> points are kolinear
var Diskr: Double;
    ax,ay,bx,by,cx,cy: Double;
begin
  ax := a.data[0];
  ay := a.data[1];
  bx := b.data[0];
  by := b.data[1];
  cx := c.data[0];
  cy := c.data[1];
  Diskr := ay*(bx-cx) + by*cx - bx*cy + ax*(-by+cy);
  If Diskr = 0 then begin
    Result.Center[0] := 0;
    Result.Center[1] := 0;
    Result.Radius := -1;
    Exit;
  end;
  Result.Radius := Sqrt(((a-b).squared_length *
                         (a-c).squared_length *
                         (b-c).squared_length)/Sqr(Diskr))/2;
  Result.Center[0] := (by*Sqr(cx) - (Sqr(bx)+Sqr(by))*cy + by*Sqr(cy) +
                       Sqr(ax)*(cy-by) + Sqr(ay)*(cy-by) +
                       ay*(Sqr(bx) + Sqr(by) - Sqr(cx) - Sqr(cy))) / (2*Diskr);
  Result.Center[1] := (Sqr(ax)*(bx-cx) + Sqr(ay)*(bx-cx) +
                       cx*(Sqr(bx)+Sqr(by)-bx*cx) - bx*Sqr(cy) +
                       ax*(Sqr(cx)-Sqr(bx)+Sqr(cy)-Sqr(by))) / (2*Diskr);
end;

// Credit to Paul Bourke (pbourke@swin.edu.au) for the original Fortran 77 Program :))
// Conversion to Visual Basic by EluZioN (EluZioN@casesladder.com)
// Conversion from VB to Delphi6 by Dr Steve Evans (steve@lociuk.com)
// Conversion from Delphi6 to FreePascal by Corpsman (corpsman@corpsman.de)
///////////////////////////////////////////////////////////////////////////////
// June 2002 Update by Dr Steve Evans (steve@lociuk.com): Heap memory allocation
// added to prevent stack overflow when MaxVertices and MaxTriangles are very large.
// Additional Updates in June 2002:
// Bug in InCircle function fixed. Radius r := Sqrt(rsqr).
// Check for duplicate points added when inserting new point.
// For speed, all points pre-sorted in x direction using quicksort algorithm and
// triangles flagged when no longer needed. The circumcircle centre and radius of
// the triangles are now stored to improve calculation time.
///////////////////////////////////////////////////////////////////////////////
// October 2012 Update by Corpsman (corpsman@corpsman.de): Added dynamical
// Arrays. Bug Fixed in calculating the outer triangle position where to small
// Added more comments in the code
///////////////////////////////////////////////////////////////////////////////
// You can use this code however you like providing the above credits remain in tact
{******************************************************************************}
function TInterpolation3D.Points2DToDelaunayTriangleList(
  CheckForDoublePoints: Boolean): TTriangleArray;
const
  BlockSize = 1000; // Allocating Memory in Blocks, keep allocating overhead small, and gives dynamic allocation
Type
  TInternal = record
    x,y: Single; // The coords of the original points
    oldindex: integer; // pointer to the original points
  end;

  TInternalArray = array of TInternal; // Container for internal storage

  //Created Triangles, vv# are the vertex pointers
  dTriangle = record
    vv0: Integer; // Index, of the 1. point in triangle, counterclockwise
    vv1: Integer; // Index, of the 2. point in triangle, counterclockwise
    vv2: Integer; // Index, of the 3. point in triangle, counterclockwise
    PreCalc: boolean; // True if xy, yc, r are defined
    xc, yc, r: Single; // Center and radius of the circumcircle
    Complete: Boolean; // If True, then all calculations of this triangle are finished (triangle will never be changed again)
  end;

var
  Vertex: TInternalArray; // copy of the input with pointer to the original index
  Triangle: array of dTriangle; // All created triangles (will be the result of the function)

  procedure Quicksort(li,re: integer); // Sort all points by x
  var h: TInternal;
      l,r: Integer;
      p: Single;
  begin
    If li < Re then begin
      p := Vertex[Trunc((li + re) / 2)].x; // read pivot
      l := li;
      r := re;
      While l < r do begin
        While Vertex[l].x < p do Inc(l);
        While Vertex[r].x > p do Dec(r);
        If l <= r then begin
          h := Vertex[l];
          Vertex[l] := Vertex[r];
          Vertex[r] := h;
          Inc(l);
          Dec(r);
        end;
      end;
      Quicksort(li,r);
      Quicksort(l,re);
    end;
  end;

  function InCircle(xp,yp: Single; // Point to insert
    out xc,yc,r: Single; j: Integer { Pointer to triangle }): Boolean;
    // Return TRUE if the point (xp,yp) lies inside the circumcircle
    // made up by triangle[j]
    // The circumcircle centre is returned in (xc,yc) and the radius r
    // NOTE: A point on the edge is inside the circumcircle
  var
    dx: Single;
    dy: Single;
    rsqr: Single;
    drsqr: Single;
    Circle: TCircle;
    tmp0,tmp1,tmp2: Tvector2_double;
  begin
    // Check if xc,yc and r have already been calculated
    If Triangle[j].PreCalc then begin
      xc := Triangle[j].xc;
      yc := Triangle[j].yc;
      r := Triangle[j].r;
      rsqr := r * r;
      dx := xp - xc;
      dy := yp - yc;
      drsqr := dx * dx + dy * dy;
    end else begin
      tmp0.init(Vertex[Triangle[j].vv0].x, Vertex[Triangle[j].vv0].y);
      tmp1.init(Vertex[Triangle[j].vv1].x, Vertex[Triangle[j].vv1].y);
      tmp2.init(Vertex[Triangle[j].vv2].x, Vertex[Triangle[j].vv2].y);
      Circle := PointsToCircumCircle(tmp0,tmp1,tmp2);
      If Circle.Radius > 0 then begin
        Triangle[j].PreCalc := true;
        Triangle[j].xc := Circle.Center[0];
        Triangle[j].yc := Circle.Center[1];
        Triangle[j].r := Circle.Radius;
        xc := Circle.Center[0];
        yc := Circle.Center[1];
        r := Circle.Radius;
        rsqr := Sqr(r);
        dx := xp - Circle.Center[0];
        dy := yp - Circle.Center[1];
        drsqr := dx*dx + dy*dy;
      end else ShowMessage('Error in function InCircle');
    end;
    Result := drsqr <= rsqr;
  end;

var
  i,j,k: Integer;
  maxx,maxy,minx,miny: Single; // to calculate die Points boundingbox
  dmax: Single; // The Max Dimension in the boundingbox
  ymid,xmid: Single; // The Center of the boundingbox
  NTri: Integer; // Counter for all triangles
  NEdge: Integer; // Counter for all edges
  NVert: integer; // Counter for all Vertices
  IsInCirc: Boolean; // True if new point lies within actual triangle and triangle needs partitioning
  xc,yc,r: Single; // Return Parameters from InCircle calculating the Actual Triangle
  Edges: array[0..1] of array of Integer; // All Edges
begin
  If High(fData) < 2 then begin // to less points
    Result := nil;
    Exit;
  end;
  If High(fData) = 2 then begin  // trivial solution
    SetLength(Result,1);
    Result[0].a := 0;
    Result[0].b := 1;
    Result[0].c := 2;
    Exit;
  end;
  Setlength(Vertex,Length(fData) + 1 + 3); // first index is unused, + 3 for supertriangle
  // Calculate bounding box
  maxx := fData[0,0];
  maxy := fData[0,1];
  minx := fData[0,0];
  miny := fData[0,1];
  NVert := Length(fData);
  For i:=0 to High(fData) do begin
    Vertex[i+1].x := fData[i,0];
    Vertex[i+1].y := fData[i,1];
    Vertex[i+1].oldindex := i;
    maxx := max(maxx,fData[i,0]);
    maxy := max(maxy, fData[i,1]);
    minx := min(minx, fData[i,0]);
    miny := min(miny, fData[0,1]);
    If CheckFordoublePoints then
      For j:=i+1 to High(fData) do
        If (Abs(fData[i,0] - fData[j,0]) < 1E-5) and
           (Abs(fData[i,1] - fData[j,1]) < 1E-5) then
          Raise Exception.Create(Format('Error, point %d and %d are the same.',[i,j]));
  end;
  // Sorting fData by x will decrease insertion time.
  QuickSort(1,NVert);
  // The Outer Triangle has to be far away, otherwise there could be some seldom
  // cases in which the convex hul is not calculated correct.
  // Unfortunately, if you choose "20" to large (e.g. 100) there come some other errors (in the circumcircle routine)
  dmax := max(maxx - minx, maxy - miny) * 20;
  xmid := (maxx + minx) / 2;
  ymid := (maxy + miny) / 2;
  Vertex[NVert+1].oldindex := -1;
  Vertex[NVert+1].x := (xmid - 2 * dmax);
  Vertex[NVert+1].y := (ymid - dmax);
  Vertex[NVert+2].oldindex := -1;
  Vertex[NVert+2].x := xmid;
  Vertex[NVert+2].y := (ymid + 2 * dmax);
  Vertex[NVert+3].oldindex := -1;
  Vertex[NVert+3].x := (xmid + 2 * dmax);
  Vertex[NVert+3].y := (ymid - dmax);
  // Allocating first blocksize
  setlength(Triangle,BlockSize);
  setlength(Edges[0],BlockSize);
  setlength(Edges[1],BlockSize);
  // inserting the supertriangle
  Triangle[1].vv0 := NVert + 1;
  Triangle[1].vv1 := NVert + 2;
  Triangle[1].vv2 := NVert + 3;
  Triangle[1].PreCalc := false;
  Triangle[1].Complete := false;
  NTri := 1;
  // Insert all fData one by one
  For i:=1 to NVert do begin
    Nedge := 0;
    // Set up the edge buffer.
    // If the point (Vertex(i).x,Vertex(i).y) lies inside the circumcircle then the
    // three edges of that triangle are added to the edge buffer.
    j := 0;
    Repeat
      j := j + 1;
      If Triangle[j].Complete <> true then begin // only check incomplete triangles
        IsInCirc := InCircle(Vertex[i].x, Vertex[i].y, // Point to be inserted
          xc,yc,r{return the circumcircle information}, j {Pointer to the triangle});
        // Include this if fData are sorted by X
        If (xc + r) < Vertex[i].x then begin
          Triangle[j].Complete := true;
        end else begin
          If IsInCirc then begin // if Triangle needs partitioning, insert edges
            // Realocate memory if necessery
            If Nedge + 3 > High(Edges[0]) then begin
              SetLength(Edges[0],High(Edges[0]) + 1 + BlockSize);
              SetLength(Edges[1],High(Edges[1]) + 1 + BlockSize);
            end;
            Edges[0,Nedge + 1] := Triangle[j].vv0;
            Edges[1,Nedge + 1] := Triangle[j].vv1;
            Edges[0,Nedge + 2] := Triangle[j].vv1;
            Edges[1,Nedge + 2] := Triangle[j].vv2;
            Edges[0,Nedge + 3] := Triangle[j].vv2;
            Edges[1,Nedge + 3] := Triangle[j].vv0;
            Nedge := Nedge + 3;
            // Duplicate the triangle, but why ??
            Triangle[j].vv0 := Triangle[NTri].vv0;
            Triangle[j].vv1 := Triangle[NTri].vv1;
            Triangle[j].vv2 := Triangle[NTri].vv2;
            Triangle[j].PreCalc := Triangle[NTri].PreCalc;
            Triangle[j].xc := Triangle[NTri].xc;
            Triangle[j].yc := Triangle[NTri].yc;
            Triangle[j].r := Triangle[NTri].r;
            Triangle[NTri].PreCalc := false;
            Triangle[j].Complete := Triangle[NTri].Complete;
            j := j - 1;
            NTri := NTri - 1;
          end;
        end;
      end;
    Until j >= NTri;

    // Tag multiple edges
    // Note: if all triangles are specified anticlockwise then all
    // interior edges are opposite pointing in direction.
    For j:=1 to Nedge - 1 do
      If not(Edges[0,j] = 0) and not(Edges[1,j] = 0) then
        For k:=j+1 to Nedge do
          If not(Edges[0,k] = 0) and not(Edges[1,k] = 0) then
            If Edges[0,j] = Edges[1,k] then
              If Edges[1,j] = Edges[0,k] then begin
                Edges[0,j] := 0;
                Edges[1,j] := 0;
                Edges[0,k] := 0;
                Edges[1,k] := 0;
              end;

    // Form new triangles for the current point
    // Skipping over any tagged edges.
    // All edges are arranged in clockwise order.
    For j:=1 to Nedge do
      If not(Edges[0,j] = 0) and not(Edges[1,j] = 0) then begin
        NTri := NTri + 1;
        // Realocate memory if necessery
        If High(Triangle) < NTri then SetLength(Triangle,High(Triangle)+1+BlockSize);
        Triangle[NTri].vv0 := Edges[0,j];
        Triangle[NTri].vv1 := Edges[1,j];
        Triangle[NTri].vv2 := i;
        Triangle[NTri].PreCalc := false;
        Triangle[NTri].Complete := false;
      end;

  end;

  // Remove triangles with supertriangle vertices
  // These are triangles which have a Vertex number greater than NVert
  i:=0;
  Repeat
    i:=i+1;
    If (Triangle[i].vv0 > NVert) or (Triangle[i].vv1 > NVert) or (Triangle[i].vv2 > NVert) then begin
      Triangle[i].vv0 := Triangle[NTri].vv0;
      Triangle[i].vv1 := Triangle[NTri].vv1;
      Triangle[i].vv2 := Triangle[NTri].vv2;
      i:=i-1;
      NTri := NTri - 1;
    end;
  Until i >= NTri;
  // Convert all results to output format, using the "unsorted" versions of the fData
  SetLength(Result,NTri);
  For i:=1 to NTri do begin
    Result[i-1].a := Vertex[Triangle[i].vv0].oldindex;
    Result[i-1].b := Vertex[Triangle[i].vv1].oldindex;
    Result[i-1].c := Vertex[Triangle[i].vv2].oldindex;
  end;
  // Free all variables
  SetLength(Vertex,0);
  SetLength(Triangle,0);
  SetLength(Edges[0],0);
  SetLength(Edges[1],0);
end;
{******************************************************************************}
function TInterpolation3D.GetIntDataDelaunay(x,y: Double): Double;
var i: Integer;
    theta,alpha,beta,gamma: Double;
    tmp: Tvector3_double;
begin
  If Length(fTriangles) > 0 then
    Result := fzmin // Default (lowest color value outside the interpolation area)
  else
    Result := 0;
  For i:=0 to High(fTriangles) do
    If PointIsInTriangle(x,y,fTriangles[i].a,fTriangles[i].b,fTriangles[i].c) then begin
      // baryzentric interpolation by Corpsman (corpsman@corpsman.de)
      tmp.init(x,y,0);
      theta := CalcTriangleArea(fData[fTriangles[i].a],fData[fTriangles[i].b],fData[fTriangles[i].c]);
      alpha := CalcTriangleArea(fData[fTriangles[i].a],tmp.data,fData[fTriangles[i].c]);
      beta := CalcTriangleArea(fData[fTriangles[i].a],fData[fTriangles[i].b],tmp.data);
      gamma := CalcTriangleArea(tmp.data,fData[fTriangles[i].b],fData[fTriangles[i].c]);
      alpha := alpha / theta;
      beta := beta / theta;
      gamma := gamma / theta;
      // calculate z
      Result := alpha * fData[fTriangles[i].b,2] +
                beta * fData[fTriangles[i].c,2] +
                gamma * fData[fTriangles[i].a,2];
      Exit;
    end;
end;
{******************************************************************************}
procedure TInterpolation3D.SetZmin(const AValue: Double);
begin
  fzmin := AValue;
end;
{******************************************************************************}
procedure TInterpolation3D.SetZmax(const AValue: Double);
begin
  fzmax := AValue;
end;
{******************************************************************************}
procedure TInterpolation3D.ClearData;
begin
  SetLength(fData,0);
  fxmin := 0;
  fxmax := 0;
  fymin := 0;
  fymax := 0;
  fzmin := 0;
  fzmax := 0;
end;
{******************************************************************************}
procedure TInterpolation3D.AddData(xMeas,yMeas,zMeas: Double);
var i: Integer;
begin
  // don't add points twice; can beeing checked later again
  For i:=0 to High(fData) do
    If (Abs(fData[i,0] - xMeas) < 1E-15) and
       (Abs(fData[i,1] - yMeas) < 1E-15) then Exit;

  SetLength(fData,Length(fData)+1);
  fData[High(fData),0] := xMeas;
  fData[High(fData),1] := yMeas;
  fData[High(fData),2] := zMeas;
  fxmin := 1E35;
  fxmax := -1E35;
  fymin := 1E35;
  fymax := -1E35;
  fzmin := 1E35;
  fzmax := -1E35;
  For i:=0 to High(fData) do begin
    If fData[i,0] < fxmin then fxmin := fData[i,0];
    If fData[i,0] > fxmax then fxmax := fData[i,0];
    If fData[i,1] < fymin then fymin := fData[i,1];
    If fData[i,1] > fymax then fymax := fData[i,1];
    If fData[i,2] < fzmin then fzmin := fData[i,2];
    If fData[i,2] > fzmax then fzmax := fData[i,2];
  end;

  Case fInterpolationMode of
    IM_DELAUNAY: begin
      SetLength(fTriangles,0);
      fTriangles := Points2DToDelaunayTriangleList(true);
    end;
  end;
end;
{******************************************************************************}
function TInterpolation3D.GetIntData(x,y: Double): Double;
begin
  Case fInterpolationMode of
    IM_DELAUNAY: Result := GetIntDataDelaunay(x,y);
    else ShowMessage('Interpolation mode is not spezified.');
  end;
end;
{******************************************************************************}
function TInterpolation3D.GetNomalizedIntData(x,y: Double): Double;
var tmp: Double;
begin
  tmp := GetIntData(x,y);
  // normalizing on inverval [-1,1]
  If fzmin = fzmax then Result := 1 else
    Result := 2 * (tmp - fzmin) / (fzmax - fzmin) - 1;
end;

END.

