API tools faq
paste
bero1985

GJK

bero1985
May 28th, 2015
339
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Delphi 56.42 KB | None | 0 0
raw download clone embed print report
  1. type PEnginePhysicsGJKStateSimplexRow=^TEnginePhysicsGJKStateSimplexRow;
  2.      TEnginePhysicsGJKStateSimplexRow=array[0..3] of TVector3;
  3.  
  4.      PEnginePhysicsGJKStateSimplices=^TEnginePhysicsGJKStateSimplices;
  5.      TEnginePhysicsGJKStateSimplices=array[0..3] of TEnginePhysicsGJKStateSimplexRow;
  6.  
  7.      PEnginePhysicsGJKStateLambdas=^TEnginePhysicsGJKStateLambdas;
  8.      TEnginePhysicsGJKStateLambdas=array[0..3] of single;
  9.  
  10.      PEnginePhysicsGJKStatePermutation=^TEnginePhysicsGJKStatePermutation;
  11.      TEnginePhysicsGJKStatePermutation=array[0..3] of longint;
  12.  
  13.      PEnginePhysicsGJKStateShapes=^TEnginePhysicsGJKStateShapes;
  14.      TEnginePhysicsGJKStateShapes=array[0..1] of TEnginePhysicsShape;
  15.  
  16.      PEnginePhysicsGJKStateTransforms=^TEnginePhysicsGJKStateTransforms;
  17.      TEnginePhysicsGJKStateTransforms=array[0..1] of PMatrix4x4;
  18.  
  19.      PEnginePhysicsGJKState=^TEnginePhysicsGJKState;
  20.      TEnginePhysicsGJKState=record
  21.       v:TVector3;
  22.       w:TVector3;
  23.       p:TVector3;
  24.       q:TVector3;
  25.       Distance:single;
  26.       Simplices:TEnginePhysicsGJKStateSimplices;
  27.       Lambda:TEnginePhysicsGJKStateLambdas;
  28.       Permutation:TEnginePhysicsGJKStatePermutation;
  29.       OldPermutation:TEnginePhysicsGJKStatePermutation;
  30.       SimplexSize:longint;
  31.       OldSimplexSize:longint;
  32.       Iterations:longint;
  33.       Failed:longbool;
  34.       Initialized:longbool;
  35.       Shapes:TEnginePhysicsGJKStateShapes;
  36.       Transforms:TEnginePhysicsGJKStateTransforms;
  37.      end;
  38.  
  39.      PEnginePhysicsFeatureGJKStateSimplexRow=^TEnginePhysicsFeatureGJKStateSimplexRow;
  40.      TEnginePhysicsFeatureGJKStateSimplexRow=array[0..3] of TVector3;
  41.  
  42.      PEnginePhysicsFeatureGJKStateSimplices=^TEnginePhysicsGJKStateSimplices;
  43.      TEnginePhysicsFeatureGJKStateSimplices=array[0..3] of TEnginePhysicsFeatureGJKStateSimplexRow;
  44.  
  45.      PEnginePhysicsFeatureGJKStateVertexIndexSimplexRow=^TEnginePhysicsFeatureGJKStateVertexIndexSimplexRow;
  46.      TEnginePhysicsFeatureGJKStateVertexIndexSimplexRow=array[0..1] of longint;
  47.  
  48.      PEnginePhysicsFeatureGJKStateVertexIndexSimplices=^TEnginePhysicsFeatureGJKStateVertexIndexSimplices;
  49.      TEnginePhysicsFeatureGJKStateVertexIndexSimplices=array[0..3] of TEnginePhysicsFeatureGJKStateVertexIndexSimplexRow;
  50.  
  51.      PEnginePhysicsFeatureGJKStateLambdas=^TEnginePhysicsFeatureGJKStateLambdas;
  52.      TEnginePhysicsFeatureGJKStateLambdas=array[0..3] of single;
  53.  
  54.      PEnginePhysicsFeatureGJKStatePermutation=^TEnginePhysicsFeatureGJKStatePermutation;
  55.      TEnginePhysicsFeatureGJKStatePermutation=array[0..3] of longint;
  56.  
  57.      PEnginePhysicsFeatureGJKStateShapes=^TEnginePhysicsFeatureGJKStateShapes;
  58.      TEnginePhysicsFeatureGJKStateShapes=array[0..1] of TEnginePhysicsShape;
  59.  
  60.      PEnginePhysicsFeatureGJKStateTransforms=^TEnginePhysicsFeatureGJKStateTransforms;
  61.      TEnginePhysicsFeatureGJKStateTransforms=array[0..1] of PMatrix4x4;
  62.  
  63.      PEnginePhysicsFeatureGJKState=^TEnginePhysicsFeatureGJKState;
  64.      TEnginePhysicsFeatureGJKState=record
  65.       v:TVector3;
  66.       w:TVector3;
  67.       p:TVector3;
  68.       q:TVector3;
  69.       Distance:single;
  70.       Simplices:TEnginePhysicsFeatureGJKStateSimplices;
  71.       VertexIndexSimplices:TEnginePhysicsFeatureGJKStateVertexIndexSimplices;
  72.       Lambda:TEnginePhysicsFeatureGJKStateLambdas;
  73.       Permutation:TEnginePhysicsFeatureGJKStatePermutation;
  74.       OldPermutation:TEnginePhysicsFeatureGJKStatePermutation;
  75.       SimplexSize:longint;
  76.       OldSimplexSize:longint;
  77.       Iterations:longint;
  78.       UseRadii:longbool;
  79.       Failed:longbool;
  80.       Initialized:longbool;
  81.       Shapes:TEnginePhysicsFeatureGJKStateShapes;
  82.       Transforms:TEnginePhysicsFeatureGJKStateTransforms;
  83.      end;
  84.  
  85. procedure ClearGJKState(var State:TEnginePhysicsGJKState);
  86. const EmptyGJKState:TEnginePhysicsGJKState=(
  87.        v:(x:1.0;y:0.0;z:0.0);
  88.        w:(x:0.0;y:0.0;z:0.0);
  89.        p:(x:0.0;y:0.0;z:0.0);
  90.        q:(x:0.0;y:0.0;z:0.0);
  91.        Distance:0.0;
  92.        Simplices:(((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)),
  93.                   ((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)),
  94.                   ((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)),
  95.                   ((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)));
  96.        Lambda:(1.0,0.0,0.0,0.0);
  97.        Permutation:(0,1,2,3);
  98.        OldPermutation:(0,1,2,3);
  99.        SimplexSize:0;
  100.        OldSimplexSize:0;
  101.        Iterations:0;
  102.        Failed:false;
  103.        Initialized:false;
  104.        Shapes:(nil,nil);
  105.        Transforms:(nil,nil);
  106.       );
  107. begin
  108.  State:=EmptyGJKState;
  109. end;
  110.  
  111. function GJKRun(var State:TEnginePhysicsGJKState):boolean;
  112.  function ReduceSimplex(var State:TEnginePhysicsGJKState):boolean;
  113.  const EPSILON=1e-6;
  114.  var t:longint;
  115.      Row0,Row1,Row2,Row3:PEnginePhysicsGJKStateSimplexRow;
  116.      y1,y2,y3,y4:PVector3;
  117.      d12_1,d12_2,d12,d13_3,d23_3,d13_1,d23_2,d23,d13,d123_1,d123_2,d123_3,d123,d14_4,d24_4,d34_4,
  118.      d14_1,d24_2,d34_3,d14,d24,d34,d124_4,d134_4,d124_2,d134_3,d234_4,d124_1,d234_3,d134_1,d234_2,
  119.      d1234_4,d1234_3,d124,d1234_2,d134,d1234_1,d234,d1234:single;
  120.  begin
  121.   result:=false;
  122.   case State.SimplexSize of
  123.    1:begin
  124.     State.Lambda[State.Permutation[0]]:=1.0;
  125.     State.SimplexSize:=1;
  126.     result:=true;
  127.     exit;
  128.    end;
  129.    2:begin
  130.     // 1 12
  131.     // 2
  132.     Row0:=@State.Simplices[State.Permutation[0]];
  133.     Row1:=@State.Simplices[State.Permutation[1]];
  134.     y1:=@Row0^[0];
  135.     y2:=@Row1^[0];
  136.  
  137.     d12_1:=Vector3Dot(Vector3Sub(y2^,y1^),y2^);
  138.     d12_2:=Vector3Dot(Vector3Sub(y1^,y2^),y1^);
  139.  
  140.     // y1 (no permutation needed)
  141.     if d12_2<=0.0 then begin
  142.      State.Lambda[State.Permutation[0]]:=1.0;
  143.      State.SimplexSize:=1;
  144.      result:=false;
  145.      exit;
  146.     end;
  147.  
  148.     //y2 (2,1)
  149.     if d12_1<=0.0 then begin
  150.      t:=State.Permutation[0];
  151.      State.Permutation[0]:=State.Permutation[1];
  152.      State.Permutation[1]:=t;
  153.      State.Lambda[State.Permutation[0]]:=1.0;
  154.      State.SimplexSize:=1;
  155.      result:=true;
  156.      exit;
  157.     end;
  158.  
  159.     d12:=d12_1+d12_2;
  160.     // terminate on affinely dependent points in the set (if d12 is zero, we can never use point y2)
  161.     if abs(d12)<=EPSILON then begin
  162.      State.SimplexSize:=1;
  163.      result:=false;
  164.      exit;
  165.     end;
  166.  
  167.     if (d12_1>0.0) and (d12_2>0.0) then begin
  168.      //y1, y2 (no permutation)
  169.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  170.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  171.      result:=true;
  172.      exit;
  173.     end else begin
  174.      State.SimplexSize:=1;
  175.      result:=false;
  176.      exit;
  177.     end;
  178.  
  179.    end;
  180.    3:begin
  181.     // 1 12 123
  182.     // 2 13
  183.     // 3 23
  184.  
  185.     Row0:=@State.Simplices[State.Permutation[0]];
  186.     Row1:=@State.Simplices[State.Permutation[1]];
  187.     Row2:=@State.Simplices[State.Permutation[2]];
  188.     y1:=@Row0^[0];
  189.     y2:=@Row1^[0];
  190.     y3:=@Row2^[0];
  191.  
  192.     //y1, (no permutation)
  193.     d13_3:=Vector3Dot(Vector3Sub(y1^,y3^),y1^);
  194.     d12_2:=Vector3Dot(Vector3Sub(y1^,y2^),y1^);
  195.     if (d12_2<=0.0) and (d13_3<=0.0) then begin
  196.      State.Lambda[State.Permutation[0]]:=1;
  197.      State.SimplexSize:=1;
  198.      result:=true;
  199.      exit;
  200.     end;
  201.  
  202.     //y2 (2,1)
  203.     d12_1:=Vector3Dot(Vector3Sub(y2^,y1^),y2^);
  204.     d23_3:=Vector3Dot(Vector3Sub(y2^,y3^),y2^);
  205.     if (d12_1<=0.0) and (d23_3<=0.0) then begin
  206.      t:=State.Permutation[0];
  207.      State.Permutation[0]:=State.Permutation[1];
  208.      State.Permutation[1]:=t;
  209.      State.Lambda[State.Permutation[0]]:=1;
  210.      State.SimplexSize:=1;
  211.      result:=true;
  212.      exit;
  213.     end;
  214.  
  215.     //y3 (3,1)
  216.     d13_1:=Vector3Dot(Vector3Sub(y3^,y1^),y3^);
  217.     d23_2:=Vector3Dot(Vector3Sub(y3^,y2^),y3^);
  218.     if (d23_2<=0.0) and (d13_1<=0.0) then begin
  219.      t:=State.Permutation[0];
  220.      State.Permutation[0]:=State.Permutation[2];
  221.      State.Permutation[2]:=t;
  222.      State.Lambda[State.Permutation[0]]:=1;
  223.      State.SimplexSize:=1;
  224.      result:=true;
  225.      exit;
  226.     end;
  227.  
  228.     d23:=d23_2+d23_3;
  229.     d13:=d13_1+d13_3;
  230.     d12:=d12_1+d12_2;
  231.  
  232.     // y2,y3 (2,1) (3,2)
  233.     d123_1:=(d23_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d23_3*Vector3Dot(Vector3Sub(y2^,y1^),y3^));
  234.     if (d123_1<=0.0) and (d23_2>0.0) and (d23_3>0.0) then begin
  235.      t:=State.Permutation[0];
  236.      State.Permutation[0]:=State.Permutation[1];
  237.      State.Permutation[1]:=t;
  238.      t:=State.Permutation[1];
  239.      State.Permutation[1]:=State.Permutation[2];
  240.      State.Permutation[2]:=t;
  241.      State.Lambda[State.Permutation[0]]:=d23_2/d23;
  242.      State.Lambda[State.Permutation[1]]:=d23_3/d23;
  243.      State.SimplexSize:=2;
  244.      result:=true;
  245.      exit;
  246.     end;
  247.  
  248.     //y1,y3 (3,2)
  249.     d123_2:=(d13_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d13_3*Vector3Dot(Vector3Sub(y1^,y2^),y3^));
  250.     if (d123_2<=0.0) and (d13_1>0.0) and (d13_3>0.0) then begin
  251.      t:=State.Permutation[1];
  252.      State.Permutation[1]:=State.Permutation[2];
  253.      State.Permutation[2]:=t;
  254.      State.Lambda[State.Permutation[0]]:=d13_1/d13;
  255.      State.Lambda[State.Permutation[1]]:=d13_3/d13;
  256.      State.SimplexSize:=2;
  257.      result:=true;
  258.      exit;
  259.     end;
  260.  
  261.     //y1,y2 (no permutation)
  262.     d123_3:=(d12_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d12_2*Vector3Dot(Vector3Sub(y1^,y3^),y2^));
  263.     if (d123_3<=0.0) and (d12_1>0.0) and (d12_2>0.0) then begin
  264.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  265.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  266.      State.SimplexSize:=2;
  267.      result:=true;
  268.      exit;
  269.     end;
  270.  
  271.     //y1, y2, y3 (no permutation)
  272.     d123:=d123_1+d123_2+d123_3;
  273.  
  274.     if (d123_1>0.0) and (d123_2>0.0) and (d123_3>0.0) then begin
  275.      State.Lambda[State.Permutation[0]]:=d123_1/d123;
  276.      State.Lambda[State.Permutation[1]]:=d123_2/d123;
  277.      State.Lambda[State.Permutation[2]]:=d123_3/d123;
  278.      State.SimplexSize:=3;
  279.      result:=true;
  280.      exit;
  281.     end else begin
  282.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  283.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  284.      State.SimplexSize:=2;
  285.      result:=false;
  286.      exit;
  287.     end;
  288.    end;
  289.    4:begin
  290.     // 1 12 123 1234
  291.     // 2 13 124
  292.     // 3 14 134
  293.     // 4 23 234
  294.     //    24
  295.     //   34
  296.  
  297.     Row0:=@State.Simplices[State.Permutation[0]];
  298.     Row1:=@State.Simplices[State.Permutation[1]];
  299.     Row2:=@State.Simplices[State.Permutation[2]];
  300.     Row3:=@State.Simplices[State.Permutation[3]];
  301.     y1:=@Row0^[0];
  302.     y2:=@Row1^[0];
  303.     y3:=@Row2^[0];
  304.     y4:=@Row3^[0];
  305.  
  306.     // y1 (no permutation)
  307.     d13_3:=Vector3Dot(Vector3Sub(y1^,y3^),y1^);
  308.     d12_2:=Vector3Dot(Vector3Sub(y1^,y2^),y1^);
  309.     d14_4:=Vector3Dot(Vector3Sub(y1^,y4^),y1^);
  310.     if (d12_2<=0.0) and (d13_3<=0.0) and (d14_4<=0.0) then begin
  311.      State.Lambda[State.Permutation[0]]:=1;
  312.      State.SimplexSize:=1;
  313.      result:=true;
  314.      exit;
  315.     end;
  316.  
  317.     // y2^ (2,1)
  318.     d12_1:=Vector3Dot(Vector3Sub(y2^,y1^),y2^);
  319.     d23_3:=Vector3Dot(Vector3Sub(y2^,y3^),y2^);
  320.     d24_4:=Vector3Dot(Vector3Sub(y2^,y4^),y2^);
  321.     if (d12_1<=0.0) and (d23_3<=0.0) and (d24_4<=0.0) then begin
  322.      t:=State.Permutation[0];
  323.      State.Permutation[0]:=State.Permutation[1];
  324.      State.Permutation[1]:=t;
  325.      State.Lambda[State.Permutation[0]]:=1;
  326.      State.SimplexSize:=1;
  327.      result:=true;
  328.      exit;
  329.     end;
  330.  
  331.     // y3^ (3,1)
  332.     d13_1:=Vector3Dot(Vector3Sub(y3^,y1^),y3^);
  333.     d23_2:=Vector3Dot(Vector3Sub(y3^,y2^),y3^);
  334.     d34_4:=Vector3Dot(Vector3Sub(y3^,y4^),y3^);
  335.     if (d23_2<=0.0) and (d13_1<=0.0) and (d34_4<=0.0) then begin
  336.      t:=State.Permutation[0];
  337.      State.Permutation[0]:=State.Permutation[2];
  338.      State.Permutation[2]:=t;
  339.      State.Lambda[State.Permutation[0]]:=1;
  340.      State.SimplexSize:=1;
  341.      result:=true;
  342.      exit;
  343.     end;
  344.  
  345.     // y4^ (4,1)
  346.     d14_1:=Vector3Dot(Vector3Sub(y4^,y1^),y4^);
  347.     d24_2:=Vector3Dot(Vector3Sub(y4^,y2^),y4^);
  348.     d34_3:=Vector3Dot(Vector3Sub(y4^,y3^),y4^);
  349.     if (d14_1<=0.0) and (d24_2<=0.0) and (d34_3<=0.0) then begin
  350.      t:=State.Permutation[0];
  351.      State.Permutation[0]:=State.Permutation[3];
  352.      State.Permutation[3]:=t;
  353.      State.Lambda[State.Permutation[0]]:=1;
  354.      State.SimplexSize:=1;
  355.      result:=true;
  356.      exit;
  357.     end;
  358.  
  359.     d12:=d12_1+d12_2;
  360.     d13:=d13_1+d13_3;
  361.     d14:=d14_1+d14_4;
  362.     d23:=d23_2+d23_3;
  363.     d24:=d24_2+d24_4;
  364.     d34:=d34_3+d34_4;
  365.  
  366.     // y1^,y2^ (no permutation)
  367.     d123_3:=(d12_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d12_2*Vector3Dot(Vector3Sub(y1^,y3^),y2^));
  368.     d124_4:=(d12_1*Vector3Dot(Vector3Sub(y1^,y4^),y1^))+(d12_2*Vector3Dot(Vector3Sub(y1^,y4^),y2^));
  369.     if (d12_1>0.0) and (d12_2>0.0) and (d123_3<=0.0) and (d124_4<=0.0) then begin
  370.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  371.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  372.      State.SimplexSize:=2;
  373.      result:=true;
  374.      exit;
  375.     end;
  376.  
  377.     // y1^, y3^ (3,2)
  378.     d123_2:=(d13_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d13_3*Vector3Dot(Vector3Sub(y1^,y2^),y3^));
  379.     d134_4:=(d13_1*Vector3Dot(Vector3Sub(y1^,y4^),y1^))+(d13_3*Vector3Dot(Vector3Sub(y1^,y4^),y3^));
  380.     if (d13_1>0.0) and (d13_3>0.0) and (d123_2<=0.0) and (d134_4<=0.0) then begin
  381.      t:=State.Permutation[1];
  382.      State.Permutation[1]:=State.Permutation[2];
  383.      State.Permutation[2]:=t;
  384.      State.Lambda[State.Permutation[0]]:=d13_1/d13;
  385.      State.Lambda[State.Permutation[1]]:=d13_3/d13;
  386.      State.SimplexSize:=2;
  387.      result:=true;
  388.      exit;
  389.     end;
  390.  
  391.     // y1^, y4^ (4,2)
  392.     d124_2:=(d14_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d14_4*Vector3Dot(Vector3Sub(y1^,y2^),y4^));
  393.     d134_3:=(d14_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d14_4*Vector3Dot(Vector3Sub(y1^,y3^),y4^));
  394.     if (d14_1>0.0) and (d14_4>0.0) and (d124_2<=0.0) and (d134_3<=0.0) then begin
  395.      t:=State.Permutation[1];
  396.      State.Permutation[1]:=State.Permutation[3];
  397.      State.Permutation[3]:=t;
  398.      State.Lambda[State.Permutation[0]]:=d14_1/d14;
  399.      State.Lambda[State.Permutation[1]]:=d14_4/d14;
  400.      State.SimplexSize:=2;
  401.      result:=true;
  402.      exit;
  403.     end;
  404.  
  405.     // y2^,y3^ (2,1) (3,2)
  406.     d123_1:=(d23_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d23_3*Vector3Dot(Vector3Sub(y2^,y1^),y3^));
  407.     d234_4:=(d23_2*Vector3Dot(Vector3Sub(y2^,y4^),y2^))+(d23_3*Vector3Dot(Vector3Sub(y2^,y4^),y3^));
  408.     if (d23_2>0.0) and (d23_3>0.0) and (d123_1<=0.0) and (d234_4<=0.0) then begin
  409.      t:=State.Permutation[0];
  410.      State.Permutation[0]:=State.Permutation[1];
  411.      State.Permutation[1]:=t;
  412.      t:=State.Permutation[1];
  413.      State.Permutation[1]:=State.Permutation[3];
  414.      State.Permutation[3]:=t;
  415.      State.Lambda[State.Permutation[0]]:=d23_2/d23;
  416.      State.Lambda[State.Permutation[1]]:=d23_3/d23;
  417.      State.SimplexSize:=2;
  418.      result:=true;
  419.      exit;
  420.     end;
  421.  
  422.     // y2^,y4^ (2,1) (4,2)
  423.     d124_1:=(d24_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d24_4*Vector3Dot(Vector3Sub(y2^,y1^),y4^));
  424.     d234_3:=(d24_2*Vector3Dot(Vector3Sub(y2^,y3^),y2^))+(d24_4*Vector3Dot(Vector3Sub(y2^,y3^),y4^));
  425.     if (d24_2>0.0) and (d24_4>0.0) and (d124_1<=0.0) and (d234_3<=0.0) then begin
  426.      t:=State.Permutation[0];
  427.      State.Permutation[0]:=State.Permutation[1];
  428.      State.Permutation[1]:=t;
  429.      t:=State.Permutation[1];
  430.      State.Permutation[1]:=State.Permutation[3];
  431.      State.Permutation[3]:=t;
  432.      State.Lambda[State.Permutation[0]]:=d24_2/d24;
  433.      State.Lambda[State.Permutation[1]]:=d24_4/d24;
  434.      State.SimplexSize:=2;
  435.      result:=true;
  436.      exit;
  437.     end;
  438.  
  439.     // y3^,y4^ (3,1) (2,4)
  440.     d134_1:=(d34_3*Vector3Dot(Vector3Sub(y3^,y1^),y3^))+(d34_4*Vector3Dot(Vector3Sub(y3^,y1^),y4^));
  441.     d234_2:=(d34_3*Vector3Dot(Vector3Sub(y3^,y2^),y3^))+(d34_4*Vector3Dot(Vector3Sub(y3^,y2^),y4^));
  442.     if (d34_3>0.0) and (d34_4>0.0) and (d134_1<=0.0) and (d234_2<=0.0) then begin
  443.      t:=State.Permutation[0];
  444.      State.Permutation[0]:=State.Permutation[2];
  445.      State.Permutation[2]:=t;
  446.      t:=State.Permutation[1];
  447.      State.Permutation[1]:=State.Permutation[3];
  448.      State.Permutation[3]:=t;
  449.      State.Lambda[State.Permutation[0]]:=d34_3/d34;
  450.      State.Lambda[State.Permutation[1]]:=d34_4/d34;
  451.      State.SimplexSize:=2;
  452.      result:=true;
  453.      exit;
  454.     end;
  455.  
  456.     // y1^,y2^,y3^ (no permutation)
  457.     d1234_4:=(d123_1*Vector3Dot(Vector3Sub(y1^,y4^),y1^))+(d123_2*Vector3Dot(Vector3Sub(y1^,y4^),y2^))+(d123_3*Vector3Dot(Vector3Sub(y1^,y4^),y3^));
  458.     if (d123_1>0.0) and (d123_2>0.0) and (d123_3>0.0) and (d1234_4<=0.0) then begin
  459.      d123:=d123_1+d123_2+d123_3;
  460.      State.Lambda[State.Permutation[0]]:=d123_1/d123;
  461.      State.Lambda[State.Permutation[1]]:=d123_2/d123;
  462.      State.Lambda[State.Permutation[2]]:=d123_3/d123;
  463.      State.SimplexSize:=3;
  464.      result:=false;
  465.      exit;
  466.     end;
  467.  
  468.     // y1^,y2^,y4^ (4,3)
  469.     d1234_3:=(d124_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d124_2*Vector3Dot(Vector3Sub(y1^,y3^),y2^))+(d124_4*Vector3Dot(Vector3Sub(y1^,y3^),y4^));
  470.     if (d124_1>0.0) and (d124_2>0.0) and (d124_4>0.0) and (d1234_3<=0.0) then begin
  471.      d124:=d124_1+d124_2+d124_4;
  472.      t:=State.Permutation[2];
  473.      State.Permutation[2]:=State.Permutation[3];
  474.      State.Permutation[3]:=t;
  475.      State.Lambda[State.Permutation[0]]:=d124_1/d124;
  476.      State.Lambda[State.Permutation[1]]:=d124_2/d124;
  477.      State.Lambda[State.Permutation[2]]:=d124_4/d124;
  478.      State.SimplexSize:=3;
  479.      result:=true;
  480.      exit;
  481.     end;
  482.  
  483.     // y1^,y3^,y4^ (3,2) (4,3)
  484.     d1234_2:=(d134_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d134_3*Vector3Dot(Vector3Sub(y1^,y2^),y3^))+(d134_4*Vector3Dot(Vector3Sub(y1^,y2^),y4^));
  485.     if (d134_1>0.0) and (d134_3>0.0) and (d134_4>0.0) and (d1234_2<=0.0) then begin
  486.      d134:=d134_1+d134_3+d134_4;
  487.      t:=State.Permutation[1];
  488.      State.Permutation[1]:=State.Permutation[2];
  489.      State.Permutation[2]:=t;
  490.      t:=State.Permutation[2];
  491.      State.Permutation[2]:=State.Permutation[3];
  492.      State.Permutation[3]:=t;
  493.      State.Lambda[State.Permutation[0]]:=d134_1/d134;
  494.      State.Lambda[State.Permutation[1]]:=d134_3/d134;
  495.      State.Lambda[State.Permutation[2]]:=d134_4/d134;
  496.      State.SimplexSize:=3;
  497.      result:=true;
  498.      exit;
  499.     end;
  500.  
  501.     // y2^,y3^,y4^ (2,1)(3,2)(4,3)
  502.     d1234_1:=(d234_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d234_3*Vector3Dot(Vector3Sub(y2^,y1^),y3^))+(d234_4*Vector3Dot(Vector3Sub(y2^,y1^),y4^));
  503.     if (d234_2>0.0) and (d234_3>0.0) and (d234_4>0.0) and (d1234_1<=0.0) then begin
  504.      d234:=d234_2+d234_3+d234_4;
  505.      t:=State.Permutation[0];
  506.      State.Permutation[0]:=State.Permutation[1];
  507.      State.Permutation[1]:=t;
  508.      t:=State.Permutation[1];
  509.      State.Permutation[1]:=State.Permutation[2];
  510.      State.Permutation[2]:=t;
  511.      t:=State.Permutation[2];
  512.      State.Permutation[2]:=State.Permutation[3];
  513.      State.Permutation[3]:=t;
  514.      State.Lambda[State.Permutation[0]]:=d234_2/d234;
  515.      State.Lambda[State.Permutation[1]]:=d234_3/d234;
  516.      State.Lambda[State.Permutation[2]]:=d234_4/d234;
  517.      State.SimplexSize:=3;
  518.      result:=true;
  519.      exit;
  520.     end;
  521.  
  522.     // y1^,y2^,y3^,y4^ (no permutation)
  523.     d1234:=d1234_1+d1234_2+d1234_3+d1234_4;
  524.  
  525.     if (d1234_1>0.0) and (d1234_2>0.0) and (d1234_3>0.0) and (d1234_4>0.0) then begin
  526.      // GJK penetrating State
  527.      // check for the accuracy, v should be the zero vector
  528.      if Vector3Length(Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(y1^,d1234_1/d1234),Vector3ScalarMul(y2^,d1234_2/d1234)),Vector3ScalarMul(y3^,d1234_3/d1234)),Vector3ScalarMul(y4^,d1234_4/d1234)))>1.0 then begin
  529.       // result is bad, terminate with last good subset {y1^,y2^,y3^}
  530.       State.SimplexSize:=3;
  531.       result:=false;
  532.      end else begin
  533.       State.Lambda[State.Permutation[0]]:=d1234_1/d1234;
  534.       State.Lambda[State.Permutation[1]]:=d1234_2/d1234;
  535.       State.Lambda[State.Permutation[2]]:=d1234_3/d1234;
  536.       State.Lambda[State.Permutation[3]]:=d1234_4/d1234;
  537.       State.SimplexSize:=4;
  538.       result:=true;
  539.      end;
  540.     end else begin
  541.      // The algorithm was unable to determine the subset. return the last best known subset:
  542.      d123:=d123_1+d123_2+d123_3;
  543.      State.Lambda[State.Permutation[0]]:=d123_1/d123;
  544.      State.Lambda[State.Permutation[1]]:=d123_2/d123;
  545.      State.Lambda[State.Permutation[2]]:=d123_3/d123;
  546.      State.SimplexSize:=3;
  547.      result:=false;
  548.     end;
  549.    end;
  550.   end;
  551.  end;
  552.  procedure GetSupport(const State:TEnginePhysicsGJKState;const Direction:TVector3;out svA,svB:TVector3);
  553.  begin
  554.   svA:=Vector3TermMatrixMul(State.Shapes[0].GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Vector3Neg(Direction),State.Transforms[0]^)),State.Transforms[0]^);
  555.   svB:=Vector3TermMatrixMul(State.Shapes[1].GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Direction,State.Transforms[1]^)),State.Transforms[1]^);
  556.  end;
  557. var Index,SupportVertexIndexA,SupportVertexIndexB:longint;
  558.     Found:boolean;
  559.     n,SupportA,SupportB:TVector3;
  560.     Row:PEnginePhysicsFeatureGJKStateSimplexRow;
  561.     VertexIndexRow:PEnginePhysicsFeatureGJKStateVertexIndexSimplexRow;
  562. begin
  563.  
  564.  if (not State.Initialized) or (Vector3LengthSquared(State.v)<GJKTolerance) then begin
  565.   if Vector3LengthSquared(State.v)<GJKTolerance then begin
  566.    State.v:=Vector3XAxis;
  567.   end;
  568.   GetSupport(State,State.v,SupportA,SupportB);
  569.   State.v:=Vector3ScalarMul(Vector3Norm(Vector3Sub(SupportA,SupportB)),65536.0);
  570.  end;
  571.  
  572.  State.Iterations:=0;
  573.  State.Failed:=false;
  574.  
  575.  if State.SimplexSize>0 then begin
  576.   for Index:=0 to State.SimplexSize-1 do begin
  577.    Row:=@State.Simplices[State.Permutation[Index]];
  578.    GetSupport(State,State.Simplices[Index,3],Row^[1],Row^[2]);
  579.    Row^[0]:=Vector3Sub(Row^[1],Row^[2]);
  580.   end;
  581.   ReduceSimplex(State);
  582.   State.v.x:=0.0;
  583.   State.v.y:=0.0;
  584.   State.v.z:=0.0;
  585.   for Index:=0 to State.SimplexSize-1 do begin
  586.    State.v:=Vector3Add(State.v,Vector3ScalarMul(State.Simplices[State.Permutation[Index],0],State.Lambda[State.Permutation[Index]]));
  587.   end;
  588.  end;
  589.  
  590.  State.OldSimplexSize:=0;
  591.  
  592.  repeat
  593.  
  594.   inc(State.Iterations);
  595.  
  596.   // store points of convex objects a and b, and A-B
  597.   n:=Vector3Norm(State.v);
  598.   GetSupport(State,Vector3Norm(State.v),SupportA,SupportB);
  599.   State.w:=Vector3Sub(SupportA,SupportB);
  600.  
  601.     // main termination condition, check for duplicate support points
  602.   Found:=false;
  603.   for Index:=0 to State.OldSimplexSize-1 do begin
  604.    Row:=@State.Simplices[State.OldPermutation[Index]];
  605.    if Vector3Compare(Row^[1],SupportA) and Vector3Compare(Row^[2],SupportB) then begin
  606.     Found:=true;
  607.     break;
  608.    end;
  609.   end;
  610.   if Found then begin
  611.    break;
  612.   end;
  613.  
  614.     // termination condition
  615.     // ||v||2 -v.w is an upper bound for ||vk-v(A-B)||2 which converges towards zero as k goes large
  616.     if (abs(Vector3LengthSquared(State.v)-abs(Vector3Dot(State.v,Vector3Norm(State.w))))<sqr(GJKTolerance)) or
  617.      (State.Iterations>GJKMaximumIterations) or (State.simplexSize>3) then begin
  618.    break;
  619.   end;
  620.  
  621.   // add w to the simplices
  622.   Row:=@State.Simplices[State.Permutation[State.SimplexSize]];
  623.   Row^[0]:=State.w;
  624.   Row^[1]:=SupportA;
  625.   Row^[2]:=SupportB;
  626.   Row^[3]:=State.v;
  627.   inc(State.SimplexSize);
  628.  
  629.   State.OldPermutation:=State.Permutation;
  630.   State.OldSimplexSize:=State.SimplexSize;
  631.  
  632.   if not ReduceSimplex(State) then begin
  633.    // latest w vector was rejected, we consequently terminate (simplex cannot chance from here on)
  634.    break;
  635.   end;
  636.  
  637.   // Calculate the vectors v and p using lambda values
  638.   case State.SimplexSize of
  639.    1:begin
  640.     State.v:=Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]);
  641.    end;
  642.    2:begin
  643.     State.v:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  644.                         Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]]));
  645.    end;
  646.    3:begin
  647.     State.v:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  648.                                    Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  649.                                    Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]]));
  650.    end;
  651.    4:begin
  652.     State.v:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  653.                                               Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  654.                                               Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]])),
  655.                                               Vector3ScalarMul(State.Simplices[State.Permutation[3],0],State.Lambda[State.Permutation[3]]));
  656.    end;
  657.    else begin
  658.     Assert(false);
  659.    end;
  660.   end;
  661.  
  662.     // Check for a penetrating state
  663.     if (Vector3LengthSquared(State.v)<sqr(GJKTolerance)) or (State.SimplexSize>3) then begin
  664.    break;
  665.   end;
  666.  
  667.  until false;
  668.  
  669.  //writeln(State.Iterations);
  670.  
  671.  // Computing d, v, p, and q, closest points of A and B
  672.  case State.SimplexSize of
  673.   1:begin
  674.    State.v:=Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]);
  675.    State.p:=Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]);
  676.    State.q:=Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]);
  677.   end;
  678.   2:begin
  679.    State.v:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  680.                        Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]]));
  681.    State.p:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]),
  682.                        Vector3ScalarMul(State.Simplices[State.Permutation[1],1],State.Lambda[State.Permutation[1]]));
  683.    State.q:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]),
  684.                        Vector3ScalarMul(State.Simplices[State.Permutation[1],2],State.Lambda[State.Permutation[1]]));
  685.   end;
  686.   3:begin
  687.    State.v:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  688.                                   Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  689.                                   Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]]));
  690.    State.p:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]),
  691.                                   Vector3ScalarMul(State.Simplices[State.Permutation[1],1],State.Lambda[State.Permutation[1]])),
  692.                                   Vector3ScalarMul(State.Simplices[State.Permutation[2],1],State.Lambda[State.Permutation[2]]));
  693.    State.q:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]),
  694.                                   Vector3ScalarMul(State.Simplices[State.Permutation[1],2],State.Lambda[State.Permutation[1]])),
  695.                                   Vector3ScalarMul(State.Simplices[State.Permutation[2],2],State.Lambda[State.Permutation[2]]));
  696.   end;
  697.   4:begin
  698.    State.v:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  699.                                              Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  700.                                              Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]])),
  701.                                              Vector3ScalarMul(State.Simplices[State.Permutation[3],0],State.Lambda[State.Permutation[3]]));
  702.    State.p:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]),
  703.                                              Vector3ScalarMul(State.Simplices[State.Permutation[1],1],State.Lambda[State.Permutation[1]])),
  704.                                              Vector3ScalarMul(State.Simplices[State.Permutation[2],1],State.Lambda[State.Permutation[2]])),
  705.                                              Vector3ScalarMul(State.Simplices[State.Permutation[3],1],State.Lambda[State.Permutation[3]]));
  706.    State.q:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]),
  707.                                              Vector3ScalarMul(State.Simplices[State.Permutation[1],2],State.Lambda[State.Permutation[1]])),
  708.                                              Vector3ScalarMul(State.Simplices[State.Permutation[2],2],State.Lambda[State.Permutation[2]])),
  709.                                              Vector3ScalarMul(State.Simplices[State.Permutation[3],2],State.Lambda[State.Permutation[3]]));
  710.   end;
  711.   else begin
  712.    State.Failed:=true;
  713.   end;
  714.  end;
  715.  State.Distance:=Vector3NormalizeEx(State.v);
  716.  
  717.  // check for intersection
  718.  if (Vector3Dist(State.p,State.q)<GJKTolerance) or ((State.SimplexSize<1) or (State.SimplexSize>3)) then begin
  719.   State.Failed:=true;
  720.  end;
  721.  
  722.  State.Initialized:=true;
  723.  
  724.  if State.Failed then begin
  725.   State.Initialized:=false;
  726.   result:=false;
  727.  end else begin
  728.   State.Initialized:=true;
  729.   result:=true;
  730.  end;
  731.  
  732. end;
  733.  
  734. procedure ClearFeatureGJKState(var State:TEnginePhysicsFeatureGJKState);
  735. const EmptyFeatureGJKState:TEnginePhysicsFeatureGJKState=(
  736.        v:(x:1.0;y:0.0;z:0.0);
  737.        w:(x:0.0;y:0.0;z:0.0);
  738.        p:(x:0.0;y:0.0;z:0.0);
  739.        q:(x:0.0;y:0.0;z:0.0);
  740.        Distance:0.0;
  741.        Simplices:(((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)),
  742.                   ((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)),
  743.                   ((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)),
  744.                   ((x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0),(x:0.0;y:0.0;z:0.0)));
  745.        VertexIndexSimplices:((-1,-2),
  746.                              (-3,-4),
  747.                              (-5,-6),
  748.                              (-7,-8));
  749.        Lambda:(1.0,0.0,0.0,0.0);
  750.        Permutation:(0,1,2,3);
  751.        OldPermutation:(0,1,2,3);
  752.        SimplexSize:0;
  753.        OldSimplexSize:0;
  754.        Iterations:0;
  755.        UseRadii:false;
  756.        Failed:false;
  757.        Initialized:false;
  758.        Shapes:(nil,nil);
  759.        Transforms:(nil,nil);
  760.       );
  761. begin
  762.  State:=EmptyFeatureGJKState;
  763. end;
  764.  
  765. function FeatureGJKRun(var State:TEnginePhysicsFeatureGJKState;const ClosestDistance:PSingle=nil;const ClosestPointA:PVector3=nil;const ClosestPointB:PVector3=nil):boolean;
  766.  function ReduceSimplex(var State:TEnginePhysicsFeatureGJKState):boolean;
  767.  const EPSILON=1e-6;
  768.  var t:longint;
  769.      Row0,Row1,Row2,Row3:PEnginePhysicsFeatureGJKStateSimplexRow;
  770.      y1,y2,y3,y4:PVector3;
  771.      d12_1,d12_2,d12,d13_3,d23_3,d13_1,d23_2,d23,d13,d123_1,d123_2,d123_3,d123,d14_4,d24_4,d34_4,
  772.      d14_1,d24_2,d34_3,d14,d24,d34,d124_4,d134_4,d124_2,d134_3,d234_4,d124_1,d234_3,d134_1,d234_2,
  773.      d1234_4,d1234_3,d124,d1234_2,d134,d1234_1,d234,d1234:single;
  774.  begin
  775.   result:=false;
  776.   case State.SimplexSize of
  777.    1:begin
  778.     State.Lambda[State.Permutation[0]]:=1.0;
  779.     State.SimplexSize:=1;
  780.     result:=true;
  781.     exit;
  782.    end;
  783.    2:begin
  784.     // 1 12
  785.     // 2
  786.     Row0:=@State.Simplices[State.Permutation[0]];
  787.     Row1:=@State.Simplices[State.Permutation[1]];
  788.     y1:=@Row0^[0];
  789.     y2:=@Row1^[0];
  790.  
  791.     d12_1:=Vector3Dot(Vector3Sub(y2^,y1^),y2^);
  792.     d12_2:=Vector3Dot(Vector3Sub(y1^,y2^),y1^);
  793.  
  794.     // y1 (no permutation needed)
  795.     if d12_2<=0.0 then begin
  796.      State.Lambda[State.Permutation[0]]:=1.0;
  797.      State.SimplexSize:=1;
  798.      result:=false;
  799.      exit;
  800.     end;
  801.  
  802.     //y2 (2,1)
  803.     if d12_1<=0.0 then begin
  804.      t:=State.Permutation[0];
  805.      State.Permutation[0]:=State.Permutation[1];
  806.      State.Permutation[1]:=t;
  807.      State.Lambda[State.Permutation[0]]:=1.0;
  808.      State.SimplexSize:=1;
  809.      result:=true;
  810.      exit;
  811.     end;
  812.  
  813.     d12:=d12_1+d12_2;
  814.     // terminate on affinely dependent points in the set (if d12 is zero, we can never use point y2)
  815.     if abs(d12)<=EPSILON then begin
  816.      State.SimplexSize:=1;
  817.      result:=false;
  818.      exit;
  819.     end;
  820.  
  821.     if (d12_1>0.0) and (d12_2>0.0) then begin
  822.      //y1, y2 (no permutation)
  823.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  824.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  825.      result:=true;
  826.      exit;
  827.     end else begin
  828.      State.SimplexSize:=1;
  829.      result:=false;
  830.      exit;
  831.     end;
  832.  
  833.    end;
  834.    3:begin
  835.     // 1 12 123
  836.     // 2 13
  837.     // 3 23
  838.  
  839.     Row0:=@State.Simplices[State.Permutation[0]];
  840.     Row1:=@State.Simplices[State.Permutation[1]];
  841.     Row2:=@State.Simplices[State.Permutation[2]];
  842.     y1:=@Row0^[0];
  843.     y2:=@Row1^[0];
  844.     y3:=@Row2^[0];
  845.  
  846.     //y1, (no permutation)
  847.     d13_3:=Vector3Dot(Vector3Sub(y1^,y3^),y1^);
  848.     d12_2:=Vector3Dot(Vector3Sub(y1^,y2^),y1^);
  849.     if (d12_2<=0.0) and (d13_3<=0.0) then begin
  850.      State.Lambda[State.Permutation[0]]:=1;
  851.      State.SimplexSize:=1;
  852.      result:=true;
  853.      exit;
  854.     end;
  855.  
  856.     //y2 (2,1)
  857.     d12_1:=Vector3Dot(Vector3Sub(y2^,y1^),y2^);
  858.     d23_3:=Vector3Dot(Vector3Sub(y2^,y3^),y2^);
  859.     if (d12_1<=0.0) and (d23_3<=0.0) then begin
  860.      t:=State.Permutation[0];
  861.      State.Permutation[0]:=State.Permutation[1];
  862.      State.Permutation[1]:=t;
  863.      State.Lambda[State.Permutation[0]]:=1;
  864.      State.SimplexSize:=1;
  865.      result:=true;
  866.      exit;
  867.     end;
  868.  
  869.     //y3 (3,1)
  870.     d13_1:=Vector3Dot(Vector3Sub(y3^,y1^),y3^);
  871.     d23_2:=Vector3Dot(Vector3Sub(y3^,y2^),y3^);
  872.     if (d23_2<=0.0) and (d13_1<=0.0) then begin
  873.      t:=State.Permutation[0];
  874.      State.Permutation[0]:=State.Permutation[2];
  875.      State.Permutation[2]:=t;
  876.      State.Lambda[State.Permutation[0]]:=1;
  877.      State.SimplexSize:=1;
  878.      result:=true;
  879.      exit;
  880.     end;
  881.  
  882.     d23:=d23_2+d23_3;
  883.     d13:=d13_1+d13_3;
  884.     d12:=d12_1+d12_2;
  885.  
  886.     // y2,y3 (2,1) (3,2)
  887.     d123_1:=(d23_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d23_3*Vector3Dot(Vector3Sub(y2^,y1^),y3^));
  888.     if (d123_1<=0.0) and (d23_2>0.0) and (d23_3>0.0) then begin
  889.      t:=State.Permutation[0];
  890.      State.Permutation[0]:=State.Permutation[1];
  891.      State.Permutation[1]:=t;
  892.      t:=State.Permutation[1];
  893.      State.Permutation[1]:=State.Permutation[2];
  894.      State.Permutation[2]:=t;
  895.      State.Lambda[State.Permutation[0]]:=d23_2/d23;
  896.      State.Lambda[State.Permutation[1]]:=d23_3/d23;
  897.      State.SimplexSize:=2;
  898.      result:=true;
  899.      exit;
  900.     end;
  901.  
  902.     //y1,y3 (3,2)
  903.     d123_2:=(d13_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d13_3*Vector3Dot(Vector3Sub(y1^,y2^),y3^));
  904.     if (d123_2<=0.0) and (d13_1>0.0) and (d13_3>0.0) then begin
  905.      t:=State.Permutation[1];
  906.      State.Permutation[1]:=State.Permutation[2];
  907.      State.Permutation[2]:=t;
  908.      State.Lambda[State.Permutation[0]]:=d13_1/d13;
  909.      State.Lambda[State.Permutation[1]]:=d13_3/d13;
  910.      State.SimplexSize:=2;
  911.      result:=true;
  912.      exit;
  913.     end;
  914.  
  915.     //y1,y2 (no permutation)
  916.     d123_3:=(d12_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d12_2*Vector3Dot(Vector3Sub(y1^,y3^),y2^));
  917.     if (d123_3<=0.0) and (d12_1>0.0) and (d12_2>0.0) then begin
  918.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  919.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  920.      State.SimplexSize:=2;
  921.      result:=true;
  922.      exit;
  923.     end;
  924.  
  925.     //y1, y2, y3 (no permutation)
  926.     d123:=d123_1+d123_2+d123_3;
  927.  
  928.     if (d123_1>0.0) and (d123_2>0.0) and (d123_3>0.0) then begin
  929.      State.Lambda[State.Permutation[0]]:=d123_1/d123;
  930.      State.Lambda[State.Permutation[1]]:=d123_2/d123;
  931.      State.Lambda[State.Permutation[2]]:=d123_3/d123;
  932.      State.SimplexSize:=3;
  933.      result:=true;
  934.      exit;
  935.     end else begin
  936.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  937.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  938.      State.SimplexSize:=2;
  939.      result:=false;
  940.      exit;
  941.     end;
  942.    end;
  943.    4:begin
  944.     // 1 12 123 1234
  945.     // 2 13 124
  946.     // 3 14 134
  947.     // 4 23 234
  948.     //    24
  949.     //   34
  950.  
  951.     Row0:=@State.Simplices[State.Permutation[0]];
  952.     Row1:=@State.Simplices[State.Permutation[1]];
  953.     Row2:=@State.Simplices[State.Permutation[2]];
  954.     Row3:=@State.Simplices[State.Permutation[3]];
  955.     y1:=@Row0^[0];
  956.     y2:=@Row1^[0];
  957.     y3:=@Row2^[0];
  958.     y4:=@Row3^[0];
  959.  
  960.     // y1 (no permutation)
  961.     d13_3:=Vector3Dot(Vector3Sub(y1^,y3^),y1^);
  962.     d12_2:=Vector3Dot(Vector3Sub(y1^,y2^),y1^);
  963.     d14_4:=Vector3Dot(Vector3Sub(y1^,y4^),y1^);
  964.     if (d12_2<=0.0) and (d13_3<=0.0) and (d14_4<=0.0) then begin
  965.      State.Lambda[State.Permutation[0]]:=1;
  966.      State.SimplexSize:=1;
  967.      result:=true;
  968.      exit;
  969.     end;
  970.  
  971.     // y2^ (2,1)
  972.     d12_1:=Vector3Dot(Vector3Sub(y2^,y1^),y2^);
  973.     d23_3:=Vector3Dot(Vector3Sub(y2^,y3^),y2^);
  974.     d24_4:=Vector3Dot(Vector3Sub(y2^,y4^),y2^);
  975.     if (d12_1<=0.0) and (d23_3<=0.0) and (d24_4<=0.0) then begin
  976.      t:=State.Permutation[0];
  977.      State.Permutation[0]:=State.Permutation[1];
  978.      State.Permutation[1]:=t;
  979.      State.Lambda[State.Permutation[0]]:=1;
  980.      State.SimplexSize:=1;
  981.      result:=true;
  982.      exit;
  983.     end;
  984.  
  985.     // y3^ (3,1)
  986.     d13_1:=Vector3Dot(Vector3Sub(y3^,y1^),y3^);
  987.     d23_2:=Vector3Dot(Vector3Sub(y3^,y2^),y3^);
  988.     d34_4:=Vector3Dot(Vector3Sub(y3^,y4^),y3^);
  989.     if (d23_2<=0.0) and (d13_1<=0.0) and (d34_4<=0.0) then begin
  990.      t:=State.Permutation[0];
  991.      State.Permutation[0]:=State.Permutation[2];
  992.      State.Permutation[2]:=t;
  993.      State.Lambda[State.Permutation[0]]:=1;
  994.      State.SimplexSize:=1;
  995.      result:=true;
  996.      exit;
  997.     end;
  998.  
  999.     // y4^ (4,1)
  1000.     d14_1:=Vector3Dot(Vector3Sub(y4^,y1^),y4^);
  1001.     d24_2:=Vector3Dot(Vector3Sub(y4^,y2^),y4^);
  1002.     d34_3:=Vector3Dot(Vector3Sub(y4^,y3^),y4^);
  1003.     if (d14_1<=0.0) and (d24_2<=0.0) and (d34_3<=0.0) then begin
  1004.      t:=State.Permutation[0];
  1005.      State.Permutation[0]:=State.Permutation[3];
  1006.      State.Permutation[3]:=t;
  1007.      State.Lambda[State.Permutation[0]]:=1;
  1008.      State.SimplexSize:=1;
  1009.      result:=true;
  1010.      exit;
  1011.     end;
  1012.  
  1013.     d12:=d12_1+d12_2;
  1014.     d13:=d13_1+d13_3;
  1015.     d14:=d14_1+d14_4;
  1016.     d23:=d23_2+d23_3;
  1017.     d24:=d24_2+d24_4;
  1018.     d34:=d34_3+d34_4;
  1019.  
  1020.     // y1^,y2^ (no permutation)
  1021.     d123_3:=(d12_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d12_2*Vector3Dot(Vector3Sub(y1^,y3^),y2^));
  1022.     d124_4:=(d12_1*Vector3Dot(Vector3Sub(y1^,y4^),y1^))+(d12_2*Vector3Dot(Vector3Sub(y1^,y4^),y2^));
  1023.     if (d12_1>0.0) and (d12_2>0.0) and (d123_3<=0.0) and (d124_4<=0.0) then begin
  1024.      State.Lambda[State.Permutation[0]]:=d12_1/d12;
  1025.      State.Lambda[State.Permutation[1]]:=d12_2/d12;
  1026.      State.SimplexSize:=2;
  1027.      result:=true;
  1028.      exit;
  1029.     end;
  1030.  
  1031.     // y1^, y3^ (3,2)
  1032.     d123_2:=(d13_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d13_3*Vector3Dot(Vector3Sub(y1^,y2^),y3^));
  1033.     d134_4:=(d13_1*Vector3Dot(Vector3Sub(y1^,y4^),y1^))+(d13_3*Vector3Dot(Vector3Sub(y1^,y4^),y3^));
  1034.     if (d13_1>0.0) and (d13_3>0.0) and (d123_2<=0.0) and (d134_4<=0.0) then begin
  1035.      t:=State.Permutation[1];
  1036.      State.Permutation[1]:=State.Permutation[2];
  1037.      State.Permutation[2]:=t;
  1038.      State.Lambda[State.Permutation[0]]:=d13_1/d13;
  1039.      State.Lambda[State.Permutation[1]]:=d13_3/d13;
  1040.      State.SimplexSize:=2;
  1041.      result:=true;
  1042.      exit;
  1043.     end;
  1044.  
  1045.     // y1^, y4^ (4,2)
  1046.     d124_2:=(d14_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d14_4*Vector3Dot(Vector3Sub(y1^,y2^),y4^));
  1047.     d134_3:=(d14_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d14_4*Vector3Dot(Vector3Sub(y1^,y3^),y4^));
  1048.     if (d14_1>0.0) and (d14_4>0.0) and (d124_2<=0.0) and (d134_3<=0.0) then begin
  1049.      t:=State.Permutation[1];
  1050.      State.Permutation[1]:=State.Permutation[3];
  1051.      State.Permutation[3]:=t;
  1052.      State.Lambda[State.Permutation[0]]:=d14_1/d14;
  1053.      State.Lambda[State.Permutation[1]]:=d14_4/d14;
  1054.      State.SimplexSize:=2;
  1055.      result:=true;
  1056.      exit;
  1057.     end;
  1058.  
  1059.     // y2^,y3^ (2,1) (3,2)
  1060.     d123_1:=(d23_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d23_3*Vector3Dot(Vector3Sub(y2^,y1^),y3^));
  1061.     d234_4:=(d23_2*Vector3Dot(Vector3Sub(y2^,y4^),y2^))+(d23_3*Vector3Dot(Vector3Sub(y2^,y4^),y3^));
  1062.     if (d23_2>0.0) and (d23_3>0.0) and (d123_1<=0.0) and (d234_4<=0.0) then begin
  1063.      t:=State.Permutation[0];
  1064.      State.Permutation[0]:=State.Permutation[1];
  1065.      State.Permutation[1]:=t;
  1066.      t:=State.Permutation[1];
  1067.      State.Permutation[1]:=State.Permutation[3];
  1068.      State.Permutation[3]:=t;
  1069.      State.Lambda[State.Permutation[0]]:=d23_2/d23;
  1070.      State.Lambda[State.Permutation[1]]:=d23_3/d23;
  1071.      State.SimplexSize:=2;
  1072.      result:=true;
  1073.      exit;
  1074.     end;
  1075.  
  1076.     // y2^,y4^ (2,1) (4,2)
  1077.     d124_1:=(d24_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d24_4*Vector3Dot(Vector3Sub(y2^,y1^),y4^));
  1078.     d234_3:=(d24_2*Vector3Dot(Vector3Sub(y2^,y3^),y2^))+(d24_4*Vector3Dot(Vector3Sub(y2^,y3^),y4^));
  1079.     if (d24_2>0.0) and (d24_4>0.0) and (d124_1<=0.0) and (d234_3<=0.0) then begin
  1080.      t:=State.Permutation[0];
  1081.      State.Permutation[0]:=State.Permutation[1];
  1082.      State.Permutation[1]:=t;
  1083.      t:=State.Permutation[1];
  1084.      State.Permutation[1]:=State.Permutation[3];
  1085.      State.Permutation[3]:=t;
  1086.      State.Lambda[State.Permutation[0]]:=d24_2/d24;
  1087.      State.Lambda[State.Permutation[1]]:=d24_4/d24;
  1088.      State.SimplexSize:=2;
  1089.      result:=true;
  1090.      exit;
  1091.     end;
  1092.  
  1093.     // y3^,y4^ (3,1) (2,4)
  1094.     d134_1:=(d34_3*Vector3Dot(Vector3Sub(y3^,y1^),y3^))+(d34_4*Vector3Dot(Vector3Sub(y3^,y1^),y4^));
  1095.     d234_2:=(d34_3*Vector3Dot(Vector3Sub(y3^,y2^),y3^))+(d34_4*Vector3Dot(Vector3Sub(y3^,y2^),y4^));
  1096.     if (d34_3>0.0) and (d34_4>0.0) and (d134_1<=0.0) and (d234_2<=0.0) then begin
  1097.      t:=State.Permutation[0];
  1098.      State.Permutation[0]:=State.Permutation[2];
  1099.      State.Permutation[2]:=t;
  1100.      t:=State.Permutation[1];
  1101.      State.Permutation[1]:=State.Permutation[3];
  1102.      State.Permutation[3]:=t;
  1103.      State.Lambda[State.Permutation[0]]:=d34_3/d34;
  1104.      State.Lambda[State.Permutation[1]]:=d34_4/d34;
  1105.      State.SimplexSize:=2;
  1106.      result:=true;
  1107.      exit;
  1108.     end;
  1109.  
  1110.     // y1^,y2^,y3^ (no permutation)
  1111.     d1234_4:=(d123_1*Vector3Dot(Vector3Sub(y1^,y4^),y1^))+(d123_2*Vector3Dot(Vector3Sub(y1^,y4^),y2^))+(d123_3*Vector3Dot(Vector3Sub(y1^,y4^),y3^));
  1112.     if (d123_1>0.0) and (d123_2>0.0) and (d123_3>0.0) and (d1234_4<=0.0) then begin
  1113.      d123:=d123_1+d123_2+d123_3;
  1114.      State.Lambda[State.Permutation[0]]:=d123_1/d123;
  1115.      State.Lambda[State.Permutation[1]]:=d123_2/d123;
  1116.      State.Lambda[State.Permutation[2]]:=d123_3/d123;
  1117.      State.SimplexSize:=3;
  1118.      result:=false;
  1119.      exit;
  1120.     end;
  1121.  
  1122.     // y1^,y2^,y4^ (4,3)
  1123.     d1234_3:=(d124_1*Vector3Dot(Vector3Sub(y1^,y3^),y1^))+(d124_2*Vector3Dot(Vector3Sub(y1^,y3^),y2^))+(d124_4*Vector3Dot(Vector3Sub(y1^,y3^),y4^));
  1124.     if (d124_1>0.0) and (d124_2>0.0) and (d124_4>0.0) and (d1234_3<=0.0) then begin
  1125.      d124:=d124_1+d124_2+d124_4;
  1126.      t:=State.Permutation[2];
  1127.      State.Permutation[2]:=State.Permutation[3];
  1128.      State.Permutation[3]:=t;
  1129.      State.Lambda[State.Permutation[0]]:=d124_1/d124;
  1130.      State.Lambda[State.Permutation[1]]:=d124_2/d124;
  1131.      State.Lambda[State.Permutation[2]]:=d124_4/d124;
  1132.      State.SimplexSize:=3;
  1133.      result:=true;
  1134.      exit;
  1135.     end;
  1136.  
  1137.     // y1^,y3^,y4^ (3,2) (4,3)
  1138.     d1234_2:=(d134_1*Vector3Dot(Vector3Sub(y1^,y2^),y1^))+(d134_3*Vector3Dot(Vector3Sub(y1^,y2^),y3^))+(d134_4*Vector3Dot(Vector3Sub(y1^,y2^),y4^));
  1139.     if (d134_1>0.0) and (d134_3>0.0) and (d134_4>0.0) and (d1234_2<=0.0) then begin
  1140.      d134:=d134_1+d134_3+d134_4;
  1141.      t:=State.Permutation[1];
  1142.      State.Permutation[1]:=State.Permutation[2];
  1143.      State.Permutation[2]:=t;
  1144.      t:=State.Permutation[2];
  1145.      State.Permutation[2]:=State.Permutation[3];
  1146.      State.Permutation[3]:=t;
  1147.      State.Lambda[State.Permutation[0]]:=d134_1/d134;
  1148.      State.Lambda[State.Permutation[1]]:=d134_3/d134;
  1149.      State.Lambda[State.Permutation[2]]:=d134_4/d134;
  1150.      State.SimplexSize:=3;
  1151.      result:=true;
  1152.      exit;
  1153.     end;
  1154.  
  1155.     // y2^,y3^,y4^ (2,1)(3,2)(4,3)
  1156.     d1234_1:=(d234_2*Vector3Dot(Vector3Sub(y2^,y1^),y2^))+(d234_3*Vector3Dot(Vector3Sub(y2^,y1^),y3^))+(d234_4*Vector3Dot(Vector3Sub(y2^,y1^),y4^));
  1157.     if (d234_2>0.0) and (d234_3>0.0) and (d234_4>0.0) and (d1234_1<=0.0) then begin
  1158.      d234:=d234_2+d234_3+d234_4;
  1159.      t:=State.Permutation[0];
  1160.      State.Permutation[0]:=State.Permutation[1];
  1161.      State.Permutation[1]:=t;
  1162.      t:=State.Permutation[1];
  1163.      State.Permutation[1]:=State.Permutation[2];
  1164.      State.Permutation[2]:=t;
  1165.      t:=State.Permutation[2];
  1166.      State.Permutation[2]:=State.Permutation[3];
  1167.      State.Permutation[3]:=t;
  1168.      State.Lambda[State.Permutation[0]]:=d234_2/d234;
  1169.      State.Lambda[State.Permutation[1]]:=d234_3/d234;
  1170.      State.Lambda[State.Permutation[2]]:=d234_4/d234;
  1171.      State.SimplexSize:=3;
  1172.      result:=true;
  1173.      exit;
  1174.     end;
  1175.  
  1176.     // y1^,y2^,y3^,y4^ (no permutation)
  1177.     d1234:=d1234_1+d1234_2+d1234_3+d1234_4;
  1178.  
  1179.     if (d1234_1>0.0) and (d1234_2>0.0) and (d1234_3>0.0) and (d1234_4>0.0) then begin
  1180.      // FeatureGJK penetrating State
  1181.      // check for the accuracy, v should be the zero vector
  1182.      if Vector3Length(Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(y1^,d1234_1/d1234),Vector3ScalarMul(y2^,d1234_2/d1234)),Vector3ScalarMul(y3^,d1234_3/d1234)),Vector3ScalarMul(y4^,d1234_4/d1234)))>1.0 then begin
  1183.       // result is bad, terminate with last good subset {y1^,y2^,y3^}
  1184.       State.SimplexSize:=3;
  1185.       result:=false;
  1186.      end else begin
  1187.       State.Lambda[State.Permutation[0]]:=d1234_1/d1234;
  1188.       State.Lambda[State.Permutation[1]]:=d1234_2/d1234;
  1189.       State.Lambda[State.Permutation[2]]:=d1234_3/d1234;
  1190.       State.Lambda[State.Permutation[3]]:=d1234_4/d1234;
  1191.       State.SimplexSize:=4;
  1192.       result:=true;
  1193.      end;
  1194.     end else begin
  1195.      // The algorithm was unable to determine the subset. return the last best known subset:
  1196.      d123:=d123_1+d123_2+d123_3;
  1197.      State.Lambda[State.Permutation[0]]:=d123_1/d123;
  1198.      State.Lambda[State.Permutation[1]]:=d123_2/d123;
  1199.      State.Lambda[State.Permutation[2]]:=d123_3/d123;
  1200.      State.SimplexSize:=3;
  1201.      result:=false;
  1202.     end;
  1203.    end;
  1204.   end;
  1205.  end;
  1206.  procedure GetSupport(const State:TEnginePhysicsFeatureGJKState;const Direction:TVector3;out svA,svB:TVector3;out siA,siB:longint);
  1207.  begin
  1208.   siA:=State.Shapes[0].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Vector3Neg(Direction),State.Transforms[0]^));
  1209.   siB:=State.Shapes[1].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Direction,State.Transforms[1]^));
  1210.   svA:=Vector3TermMatrixMul(State.Shapes[0].GetLocalFeatureSupportVertex(siA),State.Transforms[0]^);
  1211.   svB:=Vector3TermMatrixMul(State.Shapes[1].GetLocalFeatureSupportVertex(siB),State.Transforms[1]^);
  1212.  end;
  1213. var Index,SupportVertexIndexA,SupportVertexIndexB:longint;
  1214.     Found:boolean;
  1215.     n,SupportVertexVectorA,SupportVertexVectorB:TVector3;
  1216.     Row:PEnginePhysicsFeatureGJKStateSimplexRow;
  1217.     VertexIndexRow:PEnginePhysicsFeatureGJKStateVertexIndexSimplexRow;
  1218. begin
  1219.  
  1220.  if (not State.Initialized) or (Vector3LengthSquared(State.v)<GJKTolerance) then begin
  1221.   if Vector3LengthSquared(State.v)<GJKTolerance then begin
  1222.    State.v:=Vector3XAxis;
  1223.   end;
  1224.   GetSupport(State,State.v,SupportVertexVectorA,SupportVertexVectorB,SupportVertexIndexA,SupportVertexIndexB);
  1225.   State.v:=Vector3ScalarMul(Vector3Norm(Vector3Sub(SupportVertexVectorA,SupportVertexVectorB)),65536.0);
  1226.  end;
  1227.  
  1228.  State.Iterations:=0;
  1229.  State.Failed:=false;
  1230.  
  1231.  if State.SimplexSize>0 then begin
  1232.   for Index:=0 to State.SimplexSize-1 do begin
  1233.    Row:=@State.Simplices[State.Permutation[Index]];
  1234.    VertexIndexRow:=@State.VertexIndexSimplices[State.Permutation[Index]];
  1235.    GetSupport(State,State.Simplices[Index,3],Row^[1],Row^[2],VertexIndexRow^[0],VertexIndexRow^[1]);
  1236.    Row^[0]:=Vector3Sub(Row^[1],Row^[2]);
  1237.   end;
  1238.   ReduceSimplex(State);
  1239.   State.v.x:=0.0;
  1240.   State.v.y:=0.0;
  1241.   State.v.z:=0.0;
  1242.   for Index:=0 to State.SimplexSize-1 do begin
  1243.    State.v:=Vector3Add(State.v,Vector3ScalarMul(State.Simplices[State.Permutation[Index],0],State.Lambda[State.Permutation[Index]]));
  1244.   end;
  1245.  end;
  1246.  
  1247.  State.OldSimplexSize:=0;
  1248.  
  1249.  repeat
  1250.  
  1251.   inc(State.Iterations);
  1252.  
  1253.   // store points of convex objects a and b, and A-B
  1254.   GetSupport(State,Vector3Norm(State.v),SupportVertexVectorA,SupportVertexVectorB,SupportVertexIndexA,SupportVertexIndexB);
  1255.   State.w:=Vector3Sub(SupportVertexVectorA,SupportVertexVectorB);
  1256.  
  1257.     // main termination condition, check for duplicate support points
  1258.   Found:=false;
  1259.   for Index:=0 to State.OldSimplexSize-1 do begin
  1260.    VertexIndexRow:=@State.VertexIndexSimplices[State.Permutation[Index]];
  1261.    if (VertexIndexRow^[0]=SupportVertexIndexA) and (VertexIndexRow^[1]=SupportVertexIndexB) then begin
  1262.     Found:=true;
  1263.     break;
  1264.    end;
  1265.   end;
  1266.   if Found then begin
  1267.    break;
  1268.   end;
  1269.  
  1270.     // termination condition
  1271.     // ||v||2 -v.w is an upper bound for ||vk-v(A-B)||2 which converges towards zero as k goes large
  1272.     if (abs(Vector3LengthSquared(State.v)-abs(Vector3Dot(State.v,Vector3Norm(State.w))))<sqr(GJKTolerance)) or
  1273.      (State.Iterations>GJKMaximumIterations) or (State.SimplexSize>3) then begin
  1274.    break;
  1275.   end;
  1276.  
  1277.   // add w to the simplices
  1278.   Row:=@State.Simplices[State.Permutation[State.SimplexSize]];
  1279.   Row^[0]:=State.w;
  1280.   Row^[1]:=SupportVertexVectorA;
  1281.   Row^[2]:=SupportVertexVectorB;
  1282.   Row^[3]:=State.v;
  1283.   VertexIndexRow:=@State.VertexIndexSimplices[State.Permutation[State.SimplexSize]];
  1284.   VertexIndexRow^[0]:=SupportVertexIndexA;
  1285.   VertexIndexRow^[1]:=SupportVertexIndexB;
  1286.   inc(State.SimplexSize);
  1287.  
  1288.   State.OldPermutation:=State.Permutation;
  1289.   State.OldSimplexSize:=State.SimplexSize;
  1290.  
  1291.   if not ReduceSimplex(State) then begin
  1292.    // latest w vector was rejected, we consequently terminate (simplex cannot chance from here on)
  1293.    State.Failed:=true;
  1294.    break;
  1295.   end;
  1296.  
  1297.   // Calculate the vectors v and p using lambda values
  1298.   case State.SimplexSize of
  1299.    1:begin
  1300.     State.v:=Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]);
  1301.    end;
  1302.    2:begin
  1303.     State.v:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  1304.                         Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]]));
  1305.    end;
  1306.    3:begin
  1307.     State.v:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  1308.                                    Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  1309.                                    Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]]));
  1310.    end;
  1311.    4:begin
  1312.     State.v:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  1313.                                               Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  1314.                                               Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]])),
  1315.                                               Vector3ScalarMul(State.Simplices[State.Permutation[3],0],State.Lambda[State.Permutation[3]]));
  1316.    end;
  1317.    else begin
  1318.     Assert(false);
  1319.    end;
  1320.   end;
  1321.  
  1322.     // Check for a penetrating state
  1323.     if (Vector3LengthSquared(State.v)<sqr(GJKTolerance)) or (State.SimplexSize>3) then begin
  1324.    break;
  1325.   end;
  1326.  
  1327.  until false;
  1328.  
  1329.  // Computing d, v, p, and q, closest points of A and B
  1330.  case State.SimplexSize of
  1331.   1:begin
  1332.    State.v:=Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]);
  1333.    State.p:=Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]);
  1334.    State.q:=Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]);
  1335.   end;
  1336.   2:begin
  1337.    State.v:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  1338.                        Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]]));
  1339.    State.p:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]),
  1340.                        Vector3ScalarMul(State.Simplices[State.Permutation[1],1],State.Lambda[State.Permutation[1]]));
  1341.    State.q:=Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]),
  1342.                        Vector3ScalarMul(State.Simplices[State.Permutation[1],2],State.Lambda[State.Permutation[1]]));
  1343.   end;
  1344.   3:begin
  1345.    State.v:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  1346.                                   Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  1347.                                   Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]]));
  1348.    State.p:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]),
  1349.                                   Vector3ScalarMul(State.Simplices[State.Permutation[1],1],State.Lambda[State.Permutation[1]])),
  1350.                                   Vector3ScalarMul(State.Simplices[State.Permutation[2],1],State.Lambda[State.Permutation[2]]));
  1351.    State.q:=Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]),
  1352.                                   Vector3ScalarMul(State.Simplices[State.Permutation[1],2],State.Lambda[State.Permutation[1]])),
  1353.                                   Vector3ScalarMul(State.Simplices[State.Permutation[2],2],State.Lambda[State.Permutation[2]]));
  1354.   end;
  1355.   4:begin
  1356.    State.v:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],0],State.Lambda[State.Permutation[0]]),
  1357.                                              Vector3ScalarMul(State.Simplices[State.Permutation[1],0],State.Lambda[State.Permutation[1]])),
  1358.                                              Vector3ScalarMul(State.Simplices[State.Permutation[2],0],State.Lambda[State.Permutation[2]])),
  1359.                                              Vector3ScalarMul(State.Simplices[State.Permutation[3],0],State.Lambda[State.Permutation[3]]));
  1360.    State.p:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],1],State.Lambda[State.Permutation[0]]),
  1361.                                              Vector3ScalarMul(State.Simplices[State.Permutation[1],1],State.Lambda[State.Permutation[1]])),
  1362.                                              Vector3ScalarMul(State.Simplices[State.Permutation[2],1],State.Lambda[State.Permutation[2]])),
  1363.                                              Vector3ScalarMul(State.Simplices[State.Permutation[3],1],State.Lambda[State.Permutation[3]]));
  1364.    State.q:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(State.Simplices[State.Permutation[0],2],State.Lambda[State.Permutation[0]]),
  1365.                                              Vector3ScalarMul(State.Simplices[State.Permutation[1],2],State.Lambda[State.Permutation[1]])),
  1366.                                              Vector3ScalarMul(State.Simplices[State.Permutation[2],2],State.Lambda[State.Permutation[2]])),
  1367.                                              Vector3ScalarMul(State.Simplices[State.Permutation[3],2],State.Lambda[State.Permutation[3]]));
  1368.   end;
  1369.   else begin
  1370.    State.Failed:=true;
  1371.   end;
  1372.  end;
  1373.  State.Distance:=Vector3NormalizeEx(State.v);
  1374.  
  1375.  // check for intersection
  1376.  if (Vector3Dist(State.p,State.q)<GJKTolerance) or ((State.SimplexSize<1) or (State.SimplexSize>3)) then begin
  1377.   State.Failed:=true;
  1378.  end;
  1379.  
  1380.  if State.UseRadii then begin
  1381.   if (State.Distance>(State.Shapes[0].FeatureRadius+State.Shapes[1].FeatureRadius)) and (State.Distance>EPSILON) then begin
  1382.    if assigned(ClosestDistance) then begin
  1383.     ClosestDistance^:=State.Distance-(State.Shapes[0].FeatureRadius+State.Shapes[1].FeatureRadius);
  1384.    end;
  1385.    if assigned(ClosestPointA) then begin
  1386.     ClosestPointA^:=Vector3Add(State.p,Vector3ScalarMul(State.v,State.Shapes[0].FeatureRadius));
  1387.    end;
  1388.    if assigned(ClosestPointB) then begin
  1389.     ClosestPointB^:=Vector3Sub(State.q,Vector3ScalarMul(State.v,State.Shapes[1].FeatureRadius));
  1390.    end;
  1391.   end else begin
  1392.    if assigned(ClosestDistance) then begin
  1393.     ClosestDistance^:=0.0;
  1394.    end;
  1395.    if assigned(ClosestPointA) then begin
  1396.     ClosestPointA^:=Vector3Avg(State.p,State.q);
  1397.    end;
  1398.    if assigned(ClosestPointB) then begin
  1399.     ClosestPointB^:=Vector3Avg(State.p,State.q);
  1400.    end;
  1401.   end;
  1402.  end else begin
  1403.   if assigned(ClosestDistance) then begin
  1404.    ClosestDistance^:=State.Distance;
  1405.   end;
  1406.   if assigned(ClosestPointA) then begin
  1407.    ClosestPointA^:=State.p;
  1408.   end;
  1409.   if assigned(ClosestPointB) then begin
  1410.    ClosestPointB^:=State.q;
  1411.   end;
  1412.  end;
  1413.  
  1414.  if State.Failed then begin
  1415.   State.Initialized:=false;
  1416.   result:=false;
  1417.  end else begin
  1418.   State.Initialized:=true;
  1419.   result:=true;
  1420.  end;
  1421.  
  1422. end;
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!