(****************************************************************************** 
 *                            KRAFT PHYSICS ENGINE                            *
 ******************************************************************************
 *                        Version 2017-09-19-14-07-0000                       *
 ******************************************************************************
 *                                zlib license                                *
 *============================================================================*
 *                                                                            *
 * Copyright (c) 2015-2017, Benjamin Rosseaux (benjamin@rosseaux.de)          *
 *                                                                            *
 * This software is provided 'as-is', without any express or implied          *
 * warranty. In no event will the authors be held liable for any damages      *
 * arising from the use of this software.                                     *
 *                                                                            *
 * Permission is granted to anyone to use this software for any purpose,      *
 * including commercial applications, and to alter it and redistribute it     *
 * freely, subject to the following restrictions:                             *
 *                                                                            *
 * 1. The origin of this software must not be misrepresented; you must not    *
 *    claim that you wrote the original software. If you use this software    *
 *    in a product, an acknowledgement in the product documentation would be  *
 *    appreciated but is not required.                                        *
 * 2. Altered source versions must be plainly marked as such, and must not be *
 *    misrepresented as being the original software.                          *
 * 3. This notice may not be removed or altered from any source distribution. *
 *                                                                            *
 ******************************************************************************
 *                  General guidelines for code contributors                  *
 *============================================================================*
 *                                                                            *
 * 1. Make sure you are legally allowed to make a contribution under the zlib *
 *    license.                                                                *
 * 2. The zlib license header goes at the top of each source file, with       *
 *    appropriate copyright notice.                                           *
 * 3. After a pull request, check the status of your pull request on          *
      http://github.com/BeRo1985/kraft                                        *
 * 4. Write code, which is compatible with Delphi 7-XE7 and FreePascal >= 2.6 *
 *    so don't use generics/templates, operator overloading and another newer *
 *    syntax features than Delphi 7 has support for that.                     *
 * 5. Don't use Delphi VCL, FreePascal FCL or Lazarus LCL libraries/units.    *
 * 6. No use of third-party libraries/units as possible, but if needed, make  *
 *    it out-ifdef-able                                                       *
 * 7. Try to use const when possible.                                         *
 * 8. Make sure to comment out writeln, used while debugging                  *
 * 9. Use TKraftScalar instead of float/double so that Kraft can be compiled  *
 *    as double/single precision.                                             *
 * 10. Make sure the code compiles on 32-bit and 64-bit platforms in single   *
 *     and double precision.                                                  *
 *                                                                            *
 ******************************************************************************)
unit kraft;
{$ifdef fpc}
 {$mode delphi}
 {$warnings off}
 {$hints off}
 {$define caninline}
 {$ifdef cpui386}
  {$define cpu386}
 {$endif}
 {$ifdef cpuamd64}
  {$define cpux86_64}
  {$define cpux64}
 {$else}
  {$ifdef cpux86_64}
   {$define cpuamd64}
   {$define cpux64}
  {$endif}
 {$endif}
 {$ifdef cpu386}
  {$define cpu386}
  {$asmmode intel}
  {$define canx86simd}
 {$endif}
 {$ifdef FPC_LITTLE_ENDIAN}
  {$define LITTLE_ENDIAN}
 {$else}
  {$ifdef FPC_BIG_ENDIAN}
   {$define BIG_ENDIAN}
  {$endif}
 {$endif}
{$else}
 {$define LITTLE_ENDIAN}
 {$ifndef cpu64}
  {$define cpu32}
 {$endif}
 {$safedivide off}
 {$optimization on}
 {$undef caninline}
 {$undef canx86simd}
 {$ifdef conditionalexpressions}
  {$if CompilerVersion>=24.0}
   {$legacyifend on}
  {$ifend} 
 {$endif}
 {$ifdef ver180}
  {$define caninline}
  {$ifdef cpu386}
   {$define canx86simd}
  {$endif}
  {$finitefloat off}
 {$endif}
{$endif}
{$ifdef win32}
 {$define windows}
{$endif}
{$ifdef win64}
 {$define windows}
{$endif}
{$extendedsyntax on}
{$writeableconst on}
{$varstringchecks on}
{$typedaddress off}
{$overflowchecks off}
{$rangechecks off}
{$ifndef fpc}
{$realcompatibility off}
{$endif}
{$openstrings on}
{$longstrings on}
{$booleval off}
{$typeinfo on}

{-$define UseMoreCollisionGroups}

{$define UseTriangleMeshFullPerturbation}

{-$define DebugDraw}

{-$define memdebug}

{$ifdef UseDouble}
 {$define NonSIMD}
{$endif}

{-$define NonSIMD}

{$ifdef NonSIMD}
 {$undef CPU386ASMForSinglePrecision}
 {$undef SIMD}
{$else}
 {$ifdef cpu386}
  {$if not (defined(Darwin) or defined(CompileForWithPIC))}
   {$define CPU386ASMForSinglePrecision}
  {$ifend}	
 {$endif}
 {$undef SIMD}
 {$ifdef CPU386ASMForSinglePrecision}
  {$define SIMD}
 {$endif}
{$endif}

interface

uses {$ifdef windows}
      Windows,
      MMSystem,
     {$else}
      {$ifdef unix}
       BaseUnix,
       Unix,
       UnixType,
       {$ifdef linux}
        linux,
       {$endif}
      {$else}
       SDL,
      {$endif}
     {$endif}
     {$ifdef DebugDraw}
      {$ifdef fpc}
       GL,
       GLext,
      {$else}
       OpenGL,
      {$endif}
     {$endif}
     SysUtils,
     Classes,
     SyncObjs,
{$ifdef KraftPasMP}
     PasMP,
{$endif}
     Math;

const EPSILON={$ifdef UseDouble}1e-14{$else}1e-5{$endif}; // actually {$ifdef UseDouble}1e-16{$else}1e-7{$endif}; but we are conservative here

      MAX_SCALAR={$ifdef UseDouble}1.7e+308{$else}3.4e+28{$endif};

      DEG2RAD=pi/180.0;
      RAD2DEG=180.0/PI;

      MAX_CONTACTS=4;             // After largest-area contact reduction

      MAX_TEMPORARY_CONTACTS=256; // Before largest-area contact reduction

      MAX_THREADS=32;

      MaxSATSupportVertices=64;
      MaxSATContacts=64;

      MPRTolerance=1e-3;

      MPRMaximumIterations=128;

      MPRSweepCastMaximumIterations=32;

      GJKTolerance=1e-4;

      GJKMaximumIterations=128;

      TimeOfImpactTolerance=1e-3;

      TimeOfImpactMaximumIterations=64;

      TimeOfImpactSphericalExpansionRadius=1e-5;

      PhysicsFPUPrecisionMode:TFPUPrecisionMode={$ifdef cpu386}pmExtended{$else}{$ifdef cpux64}pmExtended{$else}pmDouble{$endif}{$endif};

      PhysicsFPUExceptionMask:TFPUExceptionMask=[exInvalidOp,exDenormalized,exZeroDivide,exOverflow,exUnderflow,exPrecision];

      KRAFT_QUICKHULL_FACE_MARK_VISIBLE=1;
      KRAFT_QUICKHULL_FACE_MARK_NON_CONVEX=2;
      KRAFT_QUICKHULL_FACE_MARK_NON_DELETED=3;

      KRAFT_QUICKHULL_FACE_FLAG_CLOCKWISE=1 shl 0;
      KRAFT_QUICKHULL_FACE_FLAG_INDEXED_FROM_ONE=1 shl 1;
      KRAFT_QUICKHULL_FACE_FLAG_INDEXED_FROM_ZERO=1 shl 2;
      KRAFT_QUICKHULL_FACE_FLAG_POINT_RELATIVE=1 shl 3;

      KRAFT_QUICKHULL_AUTOMATIC_TOLERANCE=-1;

      KRAFT_QUICKHULL_MERGE_TYPE_NONCONVEX_WRT_LARGER_FACE=1;
      KRAFT_QUICKHULL_MERGE_TYPE_NONCONVEX=2;

      KRAFT_QUICKHULL_HASHBITS=8;
      KRAFT_QUICKHULL_HASHSIZE=1 shl KRAFT_QUICKHULL_HASHBITS;
      KRAFT_QUICKHULL_HASHMASK=KRAFT_QUICKHULL_HASHSIZE-1;

type PKraftForceMode=^TKraftForceMode;
     TKraftForceMode=(kfmForce,        // The unit of the force parameter is applied to the rigidbody as mass*distance/time^2.
                      kfmAcceleration, // The unit of the force parameter is applied to the rigidbody as distance/time^2.
                      kfmImpulse,      // The unit of the force parameter is applied to the rigidbody as mass*distance/time.
                      kfmVelocity);    // The unit of the force parameter is applied to the rigidbody as distance/time.

     PKraftContinuousMode=^TKraftContinuousMode;
     TKraftContinuousMode=(kcmNone,                     // No continuous collision detection and response
                           kcmSpeculativeContacts,      // Fake continuous collision detection and response with speculative contacts
                           kcmMotionClamping,           // Continuous collision detection and response with motion clamping
                           kcmTimeOfImpactSubSteps);    // Continuous collision detection and response with time of impact sub stepping
                                                        // (needs kpcmNonLinearGaussSeidel at ContactPositionCorrectionMode, otherwise it
                                                        // fallbacks to kcmMotionClamping, when kpcmBaumgarte is used at
                                                        // ContactPositionCorrectionMode)

     PKraftTimeOfImpactAlgorithm=^TKraftTimeOfImpactAlgorithm;
     TKraftTimeOfImpactAlgorithm=(ktoiaConservativeAdvancement,       // Time of impact detection with conservative advancement
                                  ktoiaBilateralAdvancement);         // Time of impact detection with bilateral advancement

     PKraftPositionCorrectionMode=^TKraftPositionCorrectionMode;
     TKraftPositionCorrectionMode=(kpcmBaumgarte,                     // Faster but it can be inaccurate in some situations
                                   kpcmNonLinearGaussSeidel);         // Slower but it's more precise (default)

     PKraftContactFlag=^TKraftContactFlag;
     TKraftContactFlag=(kcfEnabled,       // To disable contacts by users and for internal usage for processing continuous collection detection
                        kcfColliding,     // Set when contact collides during a step
                        kcfWasColliding,  // Set when two objects stop colliding
                        kcfInIsland,      // For internal marking during island forming
                        kcfFiltered,      // For internal filering
                        kcfTimeOfImpact); // For internal marking during time of impact stuff

     PKraftContactFlags=^TKraftContactFlags;
     TKraftContactFlags=set of TKraftContactFlag;

     PKraftConstraintFlag=^TKraftConstraintFlag;
     TKraftConstraintFlag=(kcfCollideConnected,
                           kcfBreakable,
                           kcfActive,
                           kcfVisited,
                           kcfBreaked,
                           kcfFreshBreaked);

     PKraftConstraintFlags=^TKraftConstraintFlags;

     TKraftConstraintFlags=set of TKraftConstraintFlag;

     PKraftConstraintLimitBehavior=^TKraftConstraintLimitBehavior;
     TKraftConstraintLimitBehavior=(kclbLimitDistance,
                                    kclbLimitMaximumDistance,
                                    kclbLimitMinimumDistance);

     PKraftShapeType=^TKraftShapeType;
     TKraftShapeType=(kstUnknown=0,
                      kstSphere,
                      kstCapsule,
                      kstConvexHull,
                      kstBox,          // Internally derived from convex hull
                      kstPlane,        // Internally derived from convex hull
                      kstTriangle,     // Internally derived from convex hull and only for internal usage only at mesh shapes
                      kstMesh);        // Static only

     PKraftShapeFlag=^TKraftShapeFlag;
     TKraftShapeFlag=(ksfCollision,
                      ksfMass,
                      ksfSensor);

     PKraftShapeFlags=^TKraftShapeFlags;
     TKraftShapeFlags=set of TKraftShapeFlag;

     PKraftRigidBodyType=^TKraftRigidBodyType;
     TKraftRigidBodyType=(krbtUnknown,
                          krbtStatic,
                          krbtDynamic,
                          krbtKinematic);

     PKraftRigidBodyFlag=^TKraftRigidBodyFlag;
     TKraftRigidBodyFlag=(krbfHasOwnGravity,
                          krbfContinuous,
                          krbfContinuousAgainstDynamics,
                          krbfAllowSleep,
                          krbfAwake,
                          krbfActive,
                          krbfLockTranslationAxisX,
                          krbfLockTranslationAxisY,
                          krbfLockTranslationAxisZ,
                          krbfLockRotationAxisX,
                          krbfLockRotationAxisY,
                          krbfLockRotationAxisZ,
                          krbfSensor,
                          krbfIslandVisited,
                          krbfIslandStatic);

     PKraftRigidBodyFlags=^TKraftRigidBodyFlags;
     TKraftRigidBodyFlags=set of TKraftRigidBodyFlag;

     PKraftRigidBodyCollisionGroup=^TKraftRigidBodyCollisionGroup;
     TKraftRigidBodyCollisionGroup={$ifdef UseMoreCollisionGroups}0..255{$else}0..31{$endif};

     PKraftRigidBodyCollisionGroups=^TKraftRigidBodyCollisionGroup;
     TKraftRigidBodyCollisionGroups=set of TKraftRigidBodyCollisionGroup;

     EKraftShapeTypeOnlyForStaticRigidBody=class(Exception);

     EKraftCorruptMeshData=class(Exception);

     EKraftDegeneratedConvexHull=class(Exception);

     TKraft=class;

     TKraftContactManager=class;

     TKraftIsland=class;

     TKraftShape=class;

     TKraftRigidBody=class;

     TKraftHighResolutionTimer=class
      private
       fFrequency:int64;
       fFrequencyShift:longint;
       fFrameInterval:int64;
       fMillisecondInterval:int64;
       fTwoMillisecondsInterval:int64;
       fFourMillisecondsInterval:int64;
       fQuarterSecondInterval:int64;
       fHourInterval:int64;
      public
       constructor Create(FrameRate:longint=60);
       destructor Destroy; override;
       procedure SetFrameRate(FrameRate:longint);
       function GetTime:int64;
       function GetEventTime:int64;
       procedure Sleep(Delay:int64);
       function ToFixedPointSeconds(Time:int64):int64;
       function ToFixedPointFrames(Time:int64):int64;
       function ToFloatSeconds(Time:int64):double;
       function FromFloatSeconds(Time:double):int64;
       function ToMilliseconds(Time:int64):int64;
       function FromMilliseconds(Time:int64):int64;
       function ToMicroseconds(Time:int64):int64;
       function FromMicroseconds(Time:int64):int64;
       function ToNanoseconds(Time:int64):int64;
       function FromNanoseconds(Time:int64):int64;
       property SecondInterval:int64 read fFrequency;
       property Frequency:int64 read fFrequency;
       property FrequencyShift:longint read fFrequencyShift;
       property FrameInterval:int64 read fFrameInterval;
       property MillisecondInterval:int64 read fMillisecondInterval;
       property TwoMillisecondsInterval:int64 read fTwoMillisecondsInterval;
       property FourMillisecondsInterval:int64 read fFourMillisecondsInterval;
       property QuarterSecondInterval:int64 read fQuarterSecondInterval;
       property HourInterval:int64 read fHourInterval;
     end;

     PKraftScalar=^TKraftScalar;
     TKraftScalar={$ifdef UseDouble}double{$else}single{$endif};

     PKraftColor=^TKraftColor;
     TKraftColor=record
      r,g,b,a:TKraftScalar;
     end;

     PKraftAngles=^TKraftAngles;
     TKraftAngles=record
      Pitch,Yaw,Roll:TKraftScalar;
     end;

     PKraftVector2=^TKraftVector2;
     TKraftVector2=record
      x,y:TKraftScalar;
     end;

     PKraftRawVector3=^TKraftRawVector3;
     TKraftRawVector3=record
      case byte of
       0:(x,y,z:TKraftScalar);
       1:(xyz:array[0..2] of TKraftScalar);
     end;

     PKraftVector3=^TKraftVector3;
     TKraftVector3=record
      case byte of
       0:(x,y,z{$ifdef SIMD},w{$endif}:TKraftScalar);
       1:(Pitch,Yaw,Roll:single);
       2:(xyz:array[0..2] of TKraftScalar);
       3:(PitchYawRoll:array[0..2] of single);
       4:(RawVector:TKraftRawVector3);
{$ifdef SIMD}
       5:(xyzw:array[0..3] of TKraftScalar);
{$endif}
     end;

     PKraftVector4=^TKraftVector4;
     TKraftVector4=record
      case byte of
       0:(x,y,z,w:TKraftScalar);
       1:(xyz:array[0..2] of TKraftScalar);
       2:(xyzw:array[0..3] of TKraftScalar);
     end;

     TKraftVector3Array=array of TKraftVector3;

     PKraftVector3s=^TKraftVector3s;
     TKraftVector3s=array[0..$ff] of TKraftVector3;

     PPKraftVector3s=^TPKraftVector3s;
     TPKraftVector3s=array[0..$ff] of PKraftVector3;

     PKraftPlane=^TKraftPlane;
     TKraftPlane=record
      Normal:TKraftVector3;
      Distance:TKraftScalar;
     end;

     PKraftQuaternion=^TKraftQuaternion;
     TKraftQuaternion=record
      x,y,z,w:TKraftScalar;
     end;

     PKraftMatrix2x2=^TKraftMatrix2x2;
     TKraftMatrix2x2=array[0..1,0..1] of TKraftScalar;

     PKraftMatrix3x3=^TKraftMatrix3x3;
     TKraftMatrix3x3=array[0..2,0..{$ifdef SIMD}3{$else}2{$endif}] of TKraftScalar;

     PKraftMatrix4x4=^TKraftMatrix4x4;
     TKraftMatrix4x4=array[0..3,0..3] of TKraftScalar;

     PKraftAABB=^TKraftAABB;
     TKraftAABB=record
      case boolean of
       false:(
        Min,Max:TKraftVector3;
       );
       true:(
        MinMax:array[0..1] of TKraftVector3;
       );
     end;

     PKraftAABBs=^TKraftAABBs;
     TKraftAABBs=array[0..65535] of TKraftAABB;

     PKraftSphere=^TKraftSphere;
     TKraftSphere=record
      Center:TKraftVector3;
      Radius:TKraftScalar;
     end;

     PKraftSpheres=^TKraftSpheres;
     TKraftSpheres=array[0..65535] of TKraftSphere;

     PKraftSegment=^TKraftSegment;
     TKraftSegment=record
      Points:array[0..1] of TKraftVector3;
     end;

     PKraftRelativeSegment=^TKraftRelativeSegment;
     TKraftRelativeSegment=record
      Origin:TKraftVector3;
      Delta:TKraftVector3;
     end;

     PKraftTriangle=^TKraftTriangle;
     TKraftTriangle=record
      Points:array[0..2] of TKraftVector3;
      Normal:TKraftVector3;
     end;

     PKraftTimeStep=^TKraftTimeStep;
     TKraftTimeStep=record
      DeltaTime:TKraftScalar;
      InverseDeltaTime:TKraftScalar;
      DeltaTimeRatio:TKraftScalar;
      WarmStarting:boolean;
     end;

     // This class does exist as workaround for FreePascal, which doesn't support TKraftVector3 as published property (but Delphi does it)
     TKraftVector3Property=class(TPersistent)
      private
       fVector:PKraftVector3;
       function GetX:TKraftScalar;
       function GetY:TKraftScalar;
       function GetZ:TKraftScalar;
       function GetVector:TKraftVector3;
       procedure SetX(const NewValue:TKraftScalar);
       procedure SetY(const NewValue:TKraftScalar);
       procedure SetZ(const NewValue:TKraftScalar);
       procedure SetVector(const NewVector:TKraftVector3);
      public
       constructor Create(AVector:PKraftVector3);
       destructor Destroy; override;
       property Vector:TKraftVector3 read GetVector write SetVector;
      published
       property x:TKraftScalar read GetX write SetX;
       property y:TKraftScalar read GetY write SetY;
       property z:TKraftScalar read GetZ write SetZ;
     end;

     PKraftDynamicAABBTreeNode=^TKraftDynamicAABBTreeNode;
     TKraftDynamicAABBTreeNode=record
      AABB:TKraftAABB;
      UserData:pointer;
      Children:array[0..1] of longint;
      Height:longint;
      case boolean of
       false:(
        Parent:longint;
       );
       true:(
        Next:longint;
       );
     end;

     PKraftDynamicAABBTreeNodes=^TKraftDynamicAABBTreeNodes;
     TKraftDynamicAABBTreeNodes=array[0..0] of TKraftDynamicAABBTreeNode;

     PKraftDynamicAABBTreeLongintArray=^TKraftDynamicAABBTreeLongintArray;
     TKraftDynamicAABBTreeLongintArray=array[0..65535] of longint;

     TKraftDynamicAABBTree=class
      private
       fRoot:longint;
       fNodes:PKraftDynamicAABBTreeNodes;
       fNodeCount:longint;
       fNodeCapacity:longint;
       fFreeList:longint;
       fPath:longword;
       fInsertionCount:longint;
       fStack:PKraftDynamicAABBTreeLongintArray;
       fStackCapacity:longint;
      public
       constructor Create;
       destructor Destroy; override;
       function AllocateNode:longint;
       procedure FreeNode(NodeID:longint);
       function Balance(NodeAID:longint):longint;
       procedure InsertLeaf(Leaf:longint);
       procedure RemoveLeaf(Leaf:longint);
       function CreateProxy(const AABB:TKraftAABB;UserData:pointer):longint;
       procedure DestroyProxy(NodeID:longint);
       function MoveProxy(NodeID:longint;const AABB:TKraftAABB;const Displacement,BoundsExpansion:TKraftVector3):boolean;
       procedure Rebalance(Iterations:longint);
       procedure Rebuild;
       function ComputeHeight:longint;
       function GetHeight:longint;
       function GetAreaRatio:TKraftScalar;
       function GetMaxBalance:longint;
       function ValidateStructure:boolean;
       function ValidateMetrics:boolean;
       function Validate:boolean;
       function GetIntersectionProxy(const AABB:TKraftAABB):pointer;
       property Root:longint read fRoot;
       property Nodes:PKraftDynamicAABBTreeNodes read fNodes;
       property NodeCount:longint read fNodeCount;
       property NodeCapacity:longint read fNodeCapacity;
       property FreeList:longint read fFreeList;
       property Path:longword read fPath;
       property InsertionCount:longint read fInsertionCount;
     end;

     PKraftSweep=^TKraftSweep;
     TKraftSweep=record
      LocalCenter:TKraftVector3; // Center of mass in local space
      c0,c:TKraftVector3;        // Center of mass in world space
      q0,q:TKraftQuaternion;     // Rotation/Orientation
      Alpha0:TKraftScalar;       // Fraction of timestep from [0, 1]; c0, and q0 are at Alpha0
     end;

     PKraftMassData=^TKraftMassData;
     TKraftMassData=record
      Inertia:TKraftMatrix3x3;
      Center:TKraftVector3;
      Mass:TKraftScalar;
      Volume:TKraftScalar;
     end;

     // 64-bit contact feature ID
     PKraftContactFeatureID=^TKraftContactFeatureID;
     TKraftContactFeatureID=record
      case longint of
       0:(
        // when the last bit isn't set => continuous edge numbering at convex hulls for face<->face contacts
        // when the last bit is set    => continuous edge numbering at convex hulls for edge<->edge contacts
        ElementA:longword;
        ElementB:longword;
       );
       1:(
        // -1 => feature matching per nearest point search (like for example Bullet does it also)
        Key:int64;
       );
     end;

     PKraftClipVertex=^TKraftClipVertex;
     TKraftClipVertex=record
      Position:TKraftVector3;
      FeatureID:TKraftContactFeatureID;
     end;

     TKraftClipVertices=array of TKraftClipVertex;

     TKraftClipVertexList=class
      private
       fVertices:TKraftClipVertices;
       fCapacity:longint;
       fCount:longint;
       fColor:TKraftColor;
      public
       constructor Create;
       destructor Destroy; override;
       procedure Clear;
       procedure Add(const v:TKraftVector3;const fp:TKraftContactFeatureID); overload;
       procedure Add(const v:TKraftClipVertex); overload;
       property Vertices:TKraftClipVertices read fVertices write fVertices;
       property Capacity:longint read fCapacity write fCapacity;
       property Count:longint read fCount write fCount;
       property Color:TKraftColor read fColor write fColor;
     end;

     PKraftRaycastData=^TKraftRaycastData;
     TKraftRaycastData=record
      Origin:TKraftVector3;
      Direction:TKraftVector3;
      MaxTime:TKraftScalar;
      TimeOfImpact:TKraftScalar;
      Point:TKraftVector3;
      Normal:TKraftVector3;
     end;

     EKraftQuickHull=class(Exception);

     PKraftQuickHullIntegerArray=^TKraftQuickHullIntegerArray;
     TKraftQuickHullIntegerArray=array[0..(2147483647 div sizeof(longint))-1] of longint;

     TKraftQuickHullIntegerList=class
      private
       fList:PKraftQuickHullIntegerArray;
       fCountItems:longint;
       fAllocated:longint;
       fIsSorted:boolean;
       function GetItem(Index:longint):longint;
       procedure SetItem(Index:longint;Value:longint);
       function GetItemPointer(Index:longint):pointer;
      public
       constructor Create;
       destructor Destroy; override;
       procedure Clear;
       function Add(Item:longint):longint;
       procedure AddSorted(Item:longint);
       procedure Insert(Index:longint;Item:longint);
       procedure Delete(Index:longint);
       function Remove(Item:longint):longint;
       function Find(Item:longint):longint;
       function IndexOf(Item:longint):longint;
       procedure Exchange(Index1,Index2:longint);
       procedure SetCapacity(NewCapacity:longint);
       procedure SetCount(NewCount:longint);
       procedure Sort;
       property Count:longint read fCountItems;
       property Capacity:longint read fAllocated write SetCapacity;
       property Item[Index:longint]:longint read GetItem write SetItem; default;
       property Items[Index:longint]:longint read GetItem write SetItem;
       property PItems[Index:longint]:pointer read GetItemPointer;
     end;

     TKraftQuickHull=class;

     PKraftQuickHullVector3D=^TKraftQuickHullVector3D;
     TKraftQuickHullVector3D=object
      public
       x:double;
       y:double;
       z:double;
       function Init(const ax:double=0.0;const ay:double=0.0;const az:double=0.0):TKraftQuickHullVector3D;
       procedure SetValue(i:longint;const v:double);
       function GetValue(i:longint):double;
       procedure Add(const v0,v1:TKraftQuickHullVector3D); overload;
       procedure Add(const v:TKraftQuickHullVector3D); overload;
       procedure Sub(const v0,v1:TKraftQuickHullVector3D); overload;
       procedure Sub(const v:TKraftQuickHullVector3D); overload;
       procedure Scale(const v:TKraftQuickHullVector3D;const s:double); overload;
       procedure Scale(const s:double); overload;
       function Length:double;
       function LengthSquared:double;
       function Distance(const v:TKraftQuickHullVector3D):double;
       function DistanceSquared(const v:TKraftQuickHullVector3D):double;
       procedure Normalize;
       function Dot(const v:TKraftQuickHullVector3D):double;
       procedure CrossProduct(const v0,v1:TKraftQuickHullVector3D);
       procedure SetRandom(Lower,Upper:double);
       function Equals(const v:TKraftQuickHullVector3D):boolean;
     end;

     TKraftQuickHullVector3DArray=array of TKraftQuickHullVector3D;

     TKraftQuickHullOutputFace=array of longint;

     TKraftQuickHullOutputFaces=array of TKraftQuickHullOutputFace;

     TKraftQuickHullHalfEdge=class;

     TKraftQuickHullFace=class;

     TKraftQuickHullVertex=class
      private
       fInstance:TKraftQuickHull;
       fPoint:TKraftQuickHullVector3D;
       fIndex:longint;
       fPrevious:TKraftQuickHullVertex;
       fNext:TKraftQuickHullVertex;
       fFace:TKraftQuickHullFace;
       fHashNext:TKraftQuickHullVertex;
      public
       constructor Create(const AInstance:TKraftQuickHull);
       destructor Destroy; override;
       property Instance:TKraftQuickHull read fInstance;
       property Point:TKraftQuickHullVector3D read fPoint write fPoint;
       property Index:longint read fIndex write fIndex;
       property Previous:TKraftQuickHullVertex read fPrevious write fPrevious;
       property Next:TKraftQuickHullVertex read fNext write fNext;
       property Face:TKraftQuickHullFace read fFace write fFace;
       property HashNext:TKraftQuickHullVertex read fHashNext write fHashNext;
     end;

     PKraftQuickHullThreeVertices=^TKraftQuickHullThreeVertices;
     TKraftQuickHullThreeVertices=array[0..2] of TKraftQuickHullVertex;

     PKraftQuickHullVertexList=^TKraftQuickHullVertexList;
     TKraftQuickHullVertexList=object
      public
       Head:TKraftQuickHullVertex;
       Tail:TKraftQuickHullVertex;
       procedure Clear;
       procedure Add(vtx:TKraftQuickHullVertex);
       procedure AddAll(vtx:TKraftQuickHullVertex);
       procedure Delete(vtx:TKraftQuickHullVertex); overload;
       procedure Delete(vtx1,vtx2:TKraftQuickHullVertex); overload;
       procedure InsertBefore(vtx,Next:TKraftQuickHullVertex);
       function First:TKraftQuickHullVertex;
       function IsEmpty:boolean;
     end;

     PKraftQuickHullFaceList=^TKraftQuickHullFaceList;
     TKraftQuickHullFaceList=object
      public
       Head:TKraftQuickHullFace;
       Tail:TKraftQuickHullFace;
       procedure Clear;
       procedure Add(vtx:TKraftQuickHullFace);
       function First:TKraftQuickHullFace;
       function IsEmpty:boolean;
     end;

     TKraftQuickHullFace=class
      private
       fInstance:TKraftQuickHull;
       fNext:TKraftQuickHullFace;
       fhe0:TKraftQuickHullHalfEdge;
       fNormal:TKraftQuickHullVector3D;
       fArea:double;
       fCentroid:TKraftQuickHullVector3D;
       fPlaneOffset:double;
       fIndex:longint;
       fCountVertices:longint;
       fMark:longint;
       fOutside:TKraftQuickHullVertex;
      public
       constructor Create(const AInstance:TKraftQuickHull);
       constructor CreatePolygon(const AInstance:TKraftQuickHull;const AVertices:array of TKraftQuickHullVertex;const AIndices:array of longint);
       constructor CreateTriangle(const AInstance:TKraftQuickHull;const v0,v1,v2:TKraftQuickHullVertex;const AMinArea:double=0.0);
       destructor Destroy; override;
       procedure ComputeCentroid(var ACentroid:TKraftQuickHullVector3D);
       procedure ComputeNormal(var ANormal:TKraftQuickHullVector3D); overload;
       procedure ComputeNormal(var ANormal:TKraftQuickHullVector3D;const AMinArea:double); overload;
       procedure ComputeNormalAndCentroid; overload;
       procedure ComputeNormalAndCentroid(const AMinArea:double); overload;
       function GetEdge(i:longint):TKraftQuickHullHalfEdge;
       function GetFirstEdge:TKraftQuickHullHalfEdge;
       function FindEdge(const vt,vh:TKraftQuickHullVertex):TKraftQuickHullHalfEdge;
       function DistanceToPlane(const p:TKraftQuickHullVector3D):double;
       function ConnectHalfEdges(const hedgePrev,hedge:TKraftQuickHullHalfEdge):TKraftQuickHullFace;
       procedure CheckConsistency;
       function MergeAdjacentFace(const hedgeAdj:TKraftQuickHullHalfEdge;const Discarded:TList):longint;
       function AreaSquared(const hedge0,hedge1:TKraftQuickHullHalfEdge):double;
       procedure Triangulate(var NewFaces:TKraftQuickHullFaceList;const MinArea:double);
       property Instance:TKraftQuickHull read fInstance;
       property Next:TKraftQuickHullFace read fNext write fNext;
       property he0:TKraftQuickHullHalfEdge read fhe0 write fhe0;
       property Normal:TKraftQuickHullVector3D read fNormal write fNormal;
       property Area:double read fArea write fArea;
       property Centroid:TKraftQuickHullVector3D read fCentroid write fCentroid;
       property PlaneOffset:double read fPlaneOffset write fPlaneOffset;
       property Index:longint read fIndex write fIndex;
       property CountVertices:longint read fCountVertices write fCountVertices;
       property Mark:longint read fMark write fMark;
       property Outside:TKraftQuickHullVertex read fOutside write fOutside;
     end;

     TKraftQuickHullHalfEdge=class
      private
       fInstance:TKraftQuickHull;
       fVertex:TKraftQuickHullVertex;
       fFace:TKraftQuickHullFace;
       fNext:TKraftQuickHullHalfEdge;
       fPrevious:TKraftQuickHullHalfEdge;
       fOpposite:TKraftQuickHullHalfEdge;
      public
       constructor Create(const AInstance:TKraftQuickHull;const v:TKraftQuickHullVertex=nil;const f:TKraftQuickHullFace=nil);
       destructor Destroy; override;
       procedure SetOpposite(const Edge:TKraftQuickHullHalfEdge);
       function Head:TKraftQuickHullVertex;
       function Tail:TKraftQuickHullVertex;
       function OppositeFace:TKraftQuickHullFace;
       function Length:double;
       function LengthSquared:double;
       property Instance:TKraftQuickHull read fInstance;
       property Vertex:TKraftQuickHullVertex read fVertex write fVertex;
       property Face:TKraftQuickHullFace read fFace write fFace;
       property Next:TKraftQuickHullHalfEdge read fNext write fNext;
       property Previous:TKraftQuickHullHalfEdge read fPrevious write fPrevious;
       property Opposite:TKraftQuickHullHalfEdge read fOpposite write fOpposite;
     end;

     PKraftQuickHullVertexHashTable=^TKraftQuickHullVertexHashTable;
     TKraftQuickHullVertexHashTable=array[0..KRAFT_QUICKHULL_HASHSIZE-1] of TKraftQuickHullVertex;

     TKraftQuickHull=class
      private
       fGarbageCollectedClassInstances:TList;
       fFindIndex:longint;
       fPointBuffer:TList;
       fVertexHashTable:TKraftQuickHullVertexHashTable;
       fVertexPointIndices:TKraftQuickHullIntegerList;
       fDiscardedFaces:TList;
       fMinVertices:TKraftQuickHullThreeVertices;
       fMaxVertices:TKraftQuickHullThreeVertices;
       fFaces:TList;
       fHorizon:TList;
       fNewFaces:TKraftQuickHullFaceList;
       fUnclaimed:TKraftQuickHullVertexList;
       fClaimed:TKraftQuickHullVertexList;
       fCountVertices:longint;
       fCountFaces:longint;
       fCountPoints:longint;
       fExplicitTolerance:double;
       fTolerance:double;
       fCharLength:double;
      public
       constructor Create;
       destructor Destroy; override;
       procedure Reset;
       procedure AddPoint(const x,y,z:double);
       procedure AddPointToFace(const AVertex:TKraftQuickHullVertex;const AFace:TKraftQuickHullFace);
       procedure RemovePointFromFace(const AVertex:TKraftQuickHullVertex;const AFace:TKraftQuickHullFace);
       function RemoveAllPointsFromFace(const AFace:TKraftQuickHullFace):TKraftQuickHullVertex;
       function FindHalfEdge(const Tail,Head:TKraftQuickHullVertex):TKraftQuickHullHalfEdge;
       procedure Triangulate;
       procedure ComputeMinAndMax;
       procedure CreateInitialSimplex;
       function NextPointToAdd:TKraftQuickHullVertex;
       procedure DeleteFacePoints(Face,AbsorbingFace:TKraftQuickHullFace);
       procedure CalculateHorizon(const EyePoint:TKraftQuickHullVector3D;Edge0:TKraftQuickHullHalfEdge;const Face:TKraftQuickHullFace;const Horizon:TList);
       function AddAdjoiningFace(const EyeVertex:TKraftQuickHullVertex;const he:TKraftQuickHullHalfEdge):TKraftQuickHullHalfEdge;
       procedure AddNewFaces(const NewFaces:TKraftQuickHullFaceList;const EyeVertex:TKraftQuickHullVertex;const Horizon:TList);
       function OppFaceDistance(he:TKraftQuickHullHalfEdge):double;
       function DoAdjacentMerge(const Face:TKraftQuickHullFace;const MergeType:longint):boolean;
       procedure ResolveUnclaimedPoints(NewFaces:TKraftQuickHullFaceList);
       procedure AddPointToHull(const EyeVertex:TKraftQuickHullVertex);
       procedure MarkFaceVertices(const Face:TKraftQuickHullFace;const Mark:longint);
       procedure ReindexFacesAndVertices;
       procedure Build(const MaximumVertices:longint=-1);
       procedure GetFaceIndices(out OutputFace:TKraftQuickHullOutputFace;const Face:TKraftQuickHullFace;const Flags:longint);
       procedure GetVertices(out OutputVertices:TKraftQuickHullVector3DArray);
       procedure GetFaces(out OutputFaces:TKraftQuickHullOutputFaces);
       property GarbageCollectedClassInstances:TList read fGarbageCollectedClassInstances write fGarbageCollectedClassInstances;
       property FindIndex:longint read fFindIndex write fFindIndex;
       property PointBuffer:TList read fPointBuffer write fPointBuffer;
       property VertexHashTable:TKraftQuickHullVertexHashTable read fVertexHashTable write fVertexHashTable;
       property VertexPointIndices:TKraftQuickHullIntegerList read fVertexPointIndices;
       property DiscardedFaces:TList read fDiscardedFaces;
       property MinVertices:TKraftQuickHullThreeVertices read fMinVertices write fMinVertices;
       property MaxVertices:TKraftQuickHullThreeVertices read fMaxVertices write fMaxVertices;
       property Faces:TList read fFaces;
       property Horizon:TList read fHorizon;
       property NewFaces:TKraftQuickHullFaceList read fNewFaces write fNewFaces;
       property Unclaimed:TKraftQuickHullVertexList read fUnclaimed write fUnclaimed;
       property Claimed:TKraftQuickHullVertexList read fClaimed write fClaimed;
       property CountVertices:longint read fCountVertices write fCountVertices;
       property CountFaces:longint read fCountFaces write fCountFaces;
       property CountPoints:longint read fCountPoints write fCountPoints;
       property ExplicitTolerance:double read fExplicitTolerance write fExplicitTolerance;
       property Tolerance:double read fTolerance write fTolerance;
       property CharLength:double read fCharLength write fCharLength;
     end;

     PKraftConvexHullVertex=^TKraftConvexHullVertex;
     TKraftConvexHullVertex=record
      Position:TKraftVector3;
      CountAdjacencies:longint;
      Adjacencies:array of longint;
     end;

     PPKraftConvexHullVertices=^TPKraftConvexHullVertices;
     TPKraftConvexHullVertices=array[0..65535] of TKraftConvexHullVertex;

     TKraftConvexHullVertices=array of TKraftConvexHullVertex;

     PKraftConvexHullFace=^TKraftConvexHullFace;
     TKraftConvexHullFace=record
      Plane:TKraftPlane;
      Vertices:array of longint;
      CountVertices:longint;
      EdgeVertexOffset:longint;
     end;

     TKraftConvexHullFaces=array of TKraftConvexHullFace;

     PKraftConvexHullEdge=^TKraftConvexHullEdge;
     TKraftConvexHullEdge=record
      Vertices:array[0..1] of longint;
      Faces:array[0..1] of longint;
     end;

     TKraftConvexHullEdges=array of TKraftConvexHullEdge;

     TKraftConvexHull=class(TPersistent)
      private              

       fPhysics:TKraft;

       fPrevious:TKraftConvexHull;
       fNext:TKraftConvexHull;

       fVertices:TKraftConvexHullVertices;
       fCountVertices:longint;

       fFaces:TKraftConvexHullFaces;
       fCountFaces:longint;

       fEdges:TKraftConvexHullEdges;
       fCountEdges:longint;

       fSphere:TKraftSphere;

       fAABB:TKraftAABB;

       fAngularMotionDisc:TKraftScalar;

       fMassData:TKraftMassData;

       fCentroid:TKraftVector3;

       procedure CalculateMassData;

      public

       constructor Create(const APhysics:TKraft);
       destructor Destroy; override;

       procedure LoadFromStream(const AStream:TStream);
       procedure SaveToStream(const AStream:TStream);

       function AddVertex(const AVertex:TKraftVector3):longint;

       procedure Load(const AVertices:PKraftVector3;const ACountVertices:longint);

       procedure Scale(const WithFactor:TKraftScalar); overload;
       procedure Scale(const WithVector:TKraftVector3); overload;

       procedure Transform(const WithMatrix:TKraftMatrix3x3); overload;
       procedure Transform(const WithMatrix:TKraftMatrix4x4); overload;

       procedure Build(const AMaximumCountConvexHullPoints:longint=-1;const AUserDefinedTolerance:double=-1.0);

       procedure Update;

       procedure Finish;

       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar;

       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;

       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;

       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;

       property Physics:TKraft read fPhysics;

       property Previous:TKraftConvexHull read fPrevious;
       property Next:TKraftConvexHull read fNext;

       property Vertices:TKraftConvexHullVertices read fVertices;
       property CountVertices:longint read fCountVertices;

       property Faces:TKraftConvexHullFaces read fFaces;
       property CountFaces:longint read fCountFaces;

       property Edges:TKraftConvexHullEdges read fEdges;
       property CountEdges:longint read fCountEdges;

       property Sphere:TKraftSphere read fSphere;

       property AABB:TKraftAABB read fAABB;

       property AngularMotionDisc:TKraftScalar read fAngularMotionDisc;

       property MassData:TKraftMassData read fMassData;

       property Centroid:TKraftVector3 read fCentroid;

     end;

     PKraftMeshTriangle=^TKraftMeshTriangle;
     TKraftMeshTriangle=record
      Next:longint;
      Vertices:array[0..2] of longint;
      Normals:array[0..2] of longint;
      Plane:TKraftPlane;
      AABB:TKraftAABB;
     end;

     TKraftMeshTriangles=array of TKraftMeshTriangle;

     PKraftMeshNode=^TKraftMeshNode;
     TKraftMeshNode=record
      Children:array[0..1] of longint;
      TriangleIndex:longint;
      AABB:TKraftAABB;
     end;

     TKraftMeshNodes=array of TKraftMeshNode;

     PKraftMeshSkipListNode=^TKraftMeshSkipListNode;
     TKraftMeshSkipListNode=record
      SkipToNodeIndex:longint;
      TriangleIndex:longint;
      AABB:TKraftAABB;
     end;

     TKraftMeshSkipListNodes=array of TKraftMeshSkipListNode;

     TKraftMesh=class(TPersistent)
      private

       fPhysics:TKraft;

       fPrevious:TKraftMesh;
       fNext:TKraftMesh;

       fVertices:TKraftVector3Array;
       fCountVertices:longint;

       fNormals:TKraftVector3Array;
       fCountNormals:longint;

       fTriangles:TKraftMeshTriangles;
       fCountTriangles:longint;

       fNodes:TKraftMeshNodes;
       fCountNodes:longint;

       fSkipListNodes:TKraftMeshSkipListNodes;
       fCountSkipListNodes:longint;

       fAABB:TKraftAABB;

       fDoubleSided:boolean;

       procedure CalculateNormals;

      public

       constructor Create(const APhysics:TKraft);
       destructor Destroy; override;

       function AddVertex(const AVertex:TKraftVector3):longint;

       function AddNormal(const ANormal:TKraftVector3):longint;

       function AddTriangle(const AVertexIndex0,AVertexIndex1,AVertexIndex2:longint;const ANormalIndex0:longint=-1;const ANormalIndex1:longint=-1;ANormalIndex2:longint=-1):longint;

       procedure Load(const AVertices:PKraftVector3;const ACountVertices:longint;const ANormals:PKraftVector3;const ACountNormals:longint;const AVertexIndices,ANormalIndices:pointer;const ACountIndices:longint); overload;
       procedure Load(const ASourceData:pointer;const ASourceSize:longint); overload;

       procedure Scale(const WithFactor:TKraftScalar); overload;
       procedure Scale(const WithVector:TKraftVector3); overload;

       procedure Transform(const WithMatrix:TKraftMatrix3x3); overload;
       procedure Transform(const WithMatrix:TKraftMatrix4x4); overload;

       procedure Finish;

       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar;

       property Physics:TKraft read fPhysics;

       property Previous:TKraftMesh read fPrevious;
       property Next:TKraftMesh read fNext;

       property Vertices:TKraftVector3Array read fVertices;
       property CountVertices:longint read fCountVertices;

       property Normals:TKraftVector3Array read fNormals;
       property CountNormals:longint read fCountNormals;

       property Triangles:TKraftMeshTriangles read fTriangles;
       property CountTriangles:longint read fCountTriangles;

       property Nodes:TKraftMeshNodes read fNodes;
       property CountNodes:longint read fCountNodes;

       property SkipListNodes:TKraftMeshSkipListNodes read fSkipListNodes;
       property CountSkipListNodes:longint read fCountSkipListNodes;

       property AABB:TKraftAABB read fAABB;

      published

       property DoubleSided:boolean read fDoubleSided write fDoubleSided;

     end;

     PKraftContactPair=^TKraftContactPair;

     TKraftShapeOnContactBeginHook=procedure(const ContactPair:PKraftContactPair;const WithShape:TKraftShape) of object;
     TKraftShapeOnContactEndHook=procedure(const ContactPair:PKraftContactPair;const WithShape:TKraftShape) of object;
     TKraftShapeOnContactStayHook=procedure(const ContactPair:PKraftContactPair;const WithShape:TKraftShape) of object;

     TKraftShape=class(TPersistent)
      private

       fPhysics:TKraft;

       fRigidBody:TKraftRigidBody;

       fShapeType:TKraftShapeType;

       fShapePrevious:TKraftShape;
       fShapeNext:TKraftShape;

       fFlags:TKraftShapeFlags;

       fFriction:TKraftScalar;

       fRestitution:TKraftScalar;

       fDensity:TKraftScalar;

       fUserData:pointer;

       fStaticAABBTreeProxy:longint;
       fSleepingAABBTreeProxy:longint;
       fDynamicAABBTreeProxy:longint;
       fKinematicAABBTreeProxy:longint;

       fShapeAABB:TKraftAABB;

       fShapeSphere:TKraftSphere;

       fWorldAABB:TKraftAABB;

       fLocalTransform:TKraftMatrix4x4;

       fLocalCenterOfMass:TKraftVector3;

       fLocalCentroid:TKraftVector3;

       fWorldTransform:TKraftMatrix4x4;

       fLastWorldTransform:TKraftMatrix4x4;

       fInterpolatedWorldTransform:TKraftMatrix4x4;

       fMassData:TKraftMassData;

       fAngularMotionDisc:TKraftScalar;

       fFeatureRadius:TKraftScalar;

       fContinuousMinimumRadiusScaleFactor:TKraftScalar;

       fOnContactBegin:TKraftShapeOnContactBeginHook;
       fOnContactEnd:TKraftShapeOnContactEndHook;
       fOnContactStay:TKraftShapeOnContactStayHook;

{$ifdef DebugDraw}
       fDrawDisplayList:glUint;
{$endif}

       fIsMesh:boolean;

       function GetProxyFatWorldAABB:PKraftAABB;

      public

       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody);
       destructor Destroy; override;

       procedure UpdateShapeAABB; virtual;

       procedure CalculateMassData; virtual;

       procedure SynchronizeTransform; virtual;

       procedure SynchronizeProxies; virtual;

       procedure Finish; virtual;

       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; virtual;

       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; virtual;

       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; virtual;

       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; virtual;

       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; virtual;

       function TestPoint(const p:TKraftVector3):boolean; virtual;

       function RayCast(var RayCastData:TKraftRaycastData):boolean; virtual;

       procedure StoreWorldTransform; virtual;

       procedure InterpolateWorldTransform(const Alpha:TKraftScalar); virtual;

{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); virtual;
{$endif}

       property Physics:TKraft read fPhysics;

       property RigidBody:TKraftRigidBody read fRigidBody;

       property ShapeType:TKraftShapeType read fShapeType;

       property ShapePrevious:TKraftShape read fShapePrevious;
       property ShapeNext:TKraftShape read fShapeNext;

       property UserData:pointer read fUserData write fUserData;

       property StaticAABBTreeProxy:longint read fStaticAABBTreeProxy write fStaticAABBTreeProxy;
       property SleepingAABBTreeProxy:longint read fSleepingAABBTreeProxy write fSleepingAABBTreeProxy;
       property DynamicAABBTreeProxy:longint read fDynamicAABBTreeProxy write fDynamicAABBTreeProxy;
       property KinematicAABBTreeProxy:longint read fKinematicAABBTreeProxy write fKinematicAABBTreeProxy;

       property ShapeAABB:TKraftAABB read fShapeAABB;

       property ShapeSphere:TKraftSphere read fShapeSphere;

       property WorldAABB:TKraftAABB read fWorldAABB;

       property ProxyFatWorldAABB:PKraftAABB read GetProxyFatWorldAABB;

       property LocalTransform:TKraftMatrix4x4 read fLocalTransform write fLocalTransform;

       property LocalCenterOfMass:TKraftVector3 read fLocalCenterOfMass write fLocalCenterOfMass;

       property LocalCentroid:TKraftVector3 read fLocalCentroid write fLocalCentroid;

       property WorldTransform:TKraftMatrix4x4 read fWorldTransform write fWorldTransform;

       property LastWorldTransform:TKraftMatrix4x4 read fLastWorldTransform write fLastWorldTransform;

       property InterpolatedWorldTransform:TKraftMatrix4x4 read fInterpolatedWorldTransform write fInterpolatedWorldTransform;

       property MassData:TKraftMassData read fMassData write fMassData;

       property AngularMotionDisc:TKraftScalar read fAngularMotionDisc write fAngularMotionDisc;

       property FeatureRadius:TKraftScalar read fFeatureRadius write fFeatureRadius;

       property ContinuousMinimumRadiusScaleFactor:TKraftScalar read fContinuousMinimumRadiusScaleFactor write fContinuousMinimumRadiusScaleFactor;

{$ifdef DebugDraw}
       property DrawDisplayList:glUint read fDrawDisplayList write fDrawDisplayList;
{$endif}

       property IsMesh:boolean read fIsMesh write fIsMesh;

      published

       property Flags:TKraftShapeFlags read fFlags write fFlags;

       property Friction:TKraftScalar read fFriction write fFriction;

       property Restitution:TKraftScalar read fRestitution write fRestitution;

       property Density:TKraftScalar read fDensity write fDensity;

       property OnContactBegin:TKraftShapeOnContactBeginHook read fOnContactBegin write fOnContactBegin;
       property OnContactEnd:TKraftShapeOnContactEndHook read fOnContactEnd write fOnContactEnd;
       property OnContactStay:TKraftShapeOnContactStayHook read fOnContactStay write fOnContactStay;

     end;

     TKraftShapes=array of TKraftShape;

     TKraftShapeSphere=class(TKraftShape)
      private
       fRadius:TKraftScalar;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const ARadius:TKraftScalar); reintroduce;
       destructor Destroy; override;
       procedure UpdateShapeAABB; override;
       procedure CalculateMassData; override;
       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; override;
       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; override;
       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; override;
       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; override;
       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; override;
       function TestPoint(const p:TKraftVector3):boolean; override;
       function RayCast(var RayCastData:TKraftRaycastData):boolean; override;
{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); override;
{$endif}
       property Radius:TKraftScalar read fRadius;
     end;

     TKraftShapeCapsule=class(TKraftShape)
      private
       fRadius:TKraftScalar;
       fHeight:TKraftScalar;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const ARadius,AHeight:TKraftScalar); reintroduce;
       destructor Destroy; override;
       procedure UpdateShapeAABB; override;
       procedure CalculateMassData; override;
       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; override;
       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; override;
       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; override;
       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; override;
       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; override;
       function TestPoint(const p:TKraftVector3):boolean; override;
       function RayCast(var RayCastData:TKraftRaycastData):boolean; override;
{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); override;
{$endif}
       property Radius:TKraftScalar read fRadius;
       property Height:TKraftScalar read fHeight;
     end;

     TKraftShapeConvexHull=class(TKraftShape)
      private
       fConvexHull:TKraftConvexHull;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AConvexHull:TKraftConvexHull); reintroduce; overload;
       destructor Destroy; override;
       procedure UpdateShapeAABB; override;
       procedure CalculateMassData; override;
       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; override;
       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; override;
       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; override;
       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; override;
       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; override;
       function TestPoint(const p:TKraftVector3):boolean; override;
       function RayCast(var RayCastData:TKraftRaycastData):boolean; override;
{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); override;
{$endif}
       property ConvexHull:TKraftConvexHull read fConvexHull;
     end;

     TKraftShapeBox=class(TKraftShapeConvexHull)
      private
       fShapeConvexHull:TKraftConvexHull;
       fExtents:TKraftVector3;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AExtents:TKraftVector3); reintroduce;
       destructor Destroy; override;
       procedure UpdateShapeAABB; override;
       procedure CalculateMassData; override;
       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; override;
       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; override;
       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; override;
       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; override;
       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; override;
       function TestPoint(const p:TKraftVector3):boolean; override;
       function RayCast(var RayCastData:TKraftRaycastData):boolean; override;
{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); override;
{$endif}
       property Extents:TKraftVector3 read fExtents;
     end;

     TKraftShapePlane=class(TKraftShapeConvexHull)
      private
       fShapeConvexHull:TKraftConvexHull;
       fPlaneVertices:array[0..3] of TKraftVector3;
       fPlaneCenter:TKraftVector3;
       fPlane:TKraftPlane;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const APlane:TKraftPlane); reintroduce;
       destructor Destroy; override;
       procedure UpdateShapeAABB; override;
       procedure CalculateMassData; override;
       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; override;
       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; override;
       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; override;
       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; override;
       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; override;
       function TestPoint(const p:TKraftVector3):boolean; override;
       function RayCast(var RayCastData:TKraftRaycastData):boolean; override;
{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); override;
{$endif}
       property Plane:TKraftPlane read fPlane;
     end;

     TKraftShapeMesh=class(TKraftShape)
      private
       fMesh:TKraftMesh;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AMesh:TKraftMesh); reintroduce;
       destructor Destroy; override;
       procedure UpdateShapeAABB; override;
       procedure CalculateMassData; override;
       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; override;
       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; override;
       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; override;
       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; override;
       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; override;
       function TestPoint(const p:TKraftVector3):boolean; override;
       function RayCast(var RayCastData:TKraftRaycastData):boolean; override;
{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); override;
{$endif}
       property Mesh:TKraftMesh read fMesh;
     end;

     PKraftGJKStateShapes=^TKraftGJKStateShapes;
     TKraftGJKStateShapes=array[0..1] of TKraftShape;

     PKraftGJKStateTransforms=^TKraftGJKStateTransforms;
     TKraftGJKStateTransforms=array[0..1] of PKraftMatrix4x4;

     PKraftGJKSimplexVertex=^TKraftGJKSimplexVertex;
     TKraftGJKSimplexVertex=record
      sA:TKraftVector3;
      sB:TKraftVector3;
      w:TKraftVector3;
      a:TKraftScalar;
      iA:longint;
      iB:longint;
      Dummy:longint;
     end;

     PKraftGJKSimplexVertices=^TKraftGJKSimplexVertices;
     TKraftGJKSimplexVertices=array[0..3] of TKraftGJKSimplexVertex;

     PPKraftGJKSimplexVertices=^PKraftGJKSimplexVertices;
     TPKraftGJKSimplexVertices=array[0..3] of PKraftGJKSimplexVertex;

     PKraftGJKCachedSimplexVertex=^TKraftGJKCachedSimplexVertex;
     TKraftGJKCachedSimplexVertex=record
      a:TKraftScalar;
      iA:longint;
      iB:longint;
      Dummy:longint;
     end;

     PKraftGJKCachedSimplexVertices=^TKraftGJKCachedSimplexVertices;
     TKraftGJKCachedSimplexVertices=array[0..3] of TKraftGJKCachedSimplexVertex;

     PKraftGJKCachedSimplex=^TKraftGJKCachedSimplex;
     TKraftGJKCachedSimplex=record
      Vertices:TKraftGJKCachedSimplexVertices;
      Count:longint;
      Metric:TKraftScalar;
     end;

     PKraftGJKSimplex=^TKraftGJKSimplex;
     TKraftGJKSimplex=record
      VerticesData:TKraftGJKSimplexVertices;
      Vertices:TPKraftGJKSimplexVertices;
      Divisor:TKraftScalar;
      Count:longint;
     end;

     PKraftGJKClosestPoints=^TKraftGJKClosestPoints;
     TKraftGJKClosestPoints=array[0..1] of TKraftVector3;

     PKraftGJK=^TKraftGJK;
     TKraftGJK=object
      public
       Distance:TKraftScalar;
       Iterations:longint;
       UseRadii:boolean;
       Failed:boolean;
       Normal:TKraftVector3;
       ClosestPoints:TKraftGJKClosestPoints;
       Simplex:TKraftGJKSimplex;
       CachedSimplex:PKraftGJKCachedSimplex;
       Shapes:TKraftGJKStateShapes;
       Transforms:TKraftGJKStateTransforms;
       function Run:boolean;
     end;

     PKraftContact=^TKraftContact;
     TKraftContact=record
      LocalPoints:array[0..1] of TKraftVector3;
      Penetration:TKraftScalar; // Only needed for contact reduction
      NormalImpulse:TKraftScalar;
      TangentImpulse:array[0..1] of TKraftScalar;
      Bias:TKraftScalar;
      NormalMass:TKraftScalar;
      TangentMass:array[0..1] of TKraftScalar;
      FeatureID:TKraftContactFeatureID;
      WarmStartState:longword;
     end;

     PKraftContacts=^TKraftContacts;
     TKraftContacts=array[0..65536] of TKraftContact;

     PKraftContactFaceQuery=^TKraftContactFaceQuery;
     TKraftContactFaceQuery=record
      Index:longint;
      Separation:TKraftScalar;
     end;

     PKraftContactEdgeQuery=^TKraftContactEdgeQuery;
     TKraftContactEdgeQuery=record
      IndexA:longint;
      IndexB:longint;
      Separation:TKraftScalar;
      Normal:TKraftVector3;
     end;

     PKraftSolverContact=^TKraftSolverContact;
     TKraftSolverContact=record
      Separation:TKraftScalar;
      Point:TKraftVector3;
     end;

     PKraftSolverContactManifold=^TKraftSolverContactManifold;
     TKraftSolverContactManifold=record
      CountContacts:longint;
      Contacts:array[0..MAX_CONTACTS-1] of TKraftSolverContact;
      Points:array[0..1] of TKraftVector3;
      Normal:TKraftVector3;
     end;

     PKraftContactManifoldType=^TKraftContactManifoldType;
     TKraftContactManifoldType=(kcmtUnknown,
                                kcmtImplicit,
                                kcmtFaceA,
                                kcmtFaceB,
                                kcmtEdges,
                                kcmtImplicitEdge,
                                kcmtImplicitNormal,
                                kcmtPersistentImplicit,
                                kcmtSpeculative);

     PKraftContactManifold=^TKraftContactManifold;
     TKraftContactManifold=record
      ContactManifoldType:TKraftContactManifoldType;
      HaveData:boolean;
      Persistent:boolean;
      CountContacts:longint;
      LostSpeculativeBounce:single;
      LocalRadius:array[0..1] of TKraftScalar;
      LocalNormal:TKraftVector3;
      TangentVectors:array[0..1] of TKraftVector3;
      Contacts:array[0..MAX_CONTACTS-1] of TKraftContact;
      RelativeTransform:TKraftMatrix4x4;
      FaceQueryAB:TKraftContactFaceQuery;
      FaceQueryBA:TKraftContactFaceQuery;
      EdgeQuery:TKraftContactEdgeQuery;
      GJKCachedSimplex:TKraftGJKCachedSimplex;
     end;

     PKraftContactPairEdge=^TKraftContactPairEdge;
     TKraftContactPairEdge=record
      Previous:PKraftContactPairEdge;
      Next:PKraftContactPairEdge;
      OtherRigidBody:TKraftRigidBody;
      ContactPair:PKraftContactPair;
     end;

     TKraftMeshContactPair=class;

     PKraftContactPairContactManifoldMode=^TKraftContactPairContactManifoldMode;
     TKraftContactPairContactManifoldMode=(kcpcmmVelocitySolver,kcpcmmPositionSolver,kcpcmmBaumgarte,kcpcmmTemporalCoherence);

     TKraftContactPair=object
      public
       Previous:PKraftContactPair;
       Next:PKraftContactPair;
       HashBucket:longint;
       HashPrevious:PKraftContactPair;
       HashNext:PKraftContactPair;
       Island:TKraftIsland;
       Shapes:array[0..1] of TKraftShape;
       ElementIndex:longint;
       MeshContactPair:TKraftMeshContactPair;
       RigidBodies:array[0..1] of TKraftRigidBody;
       Edges:array[0..1] of TKraftContactPairEdge;
       Friction:TKraftScalar;
       Restitution:TKraftScalar;
       Manifold:TKraftContactManifold;
       Flags:TKraftContactFlags;
       TimeOfImpactCount:longint;
       TimeOfImpact:TKraftScalar;
       procedure GetSolverContactManifold(out SolverContactManifold:TKraftSolverContactManifold;const WorldTransformA,WorldTransformB:TKraftMatrix4x4;const ContactManifoldMode:TKraftContactPairContactManifoldMode);
       procedure DetectCollisions(const ContactManager:TKraftContactManager;const TriangleShape:TKraftShape=nil;const ThreadIndex:longint=0;const SpeculativeContacts:boolean=true;const DeltaTime:double=0.0);
     end;

     TPKraftContactPairs=array of PKraftContactPair;

     TKraftContactManagerOnContactBeginHook=procedure(const ContactPair:PKraftContactPair) of object;
     TKraftContactManagerOnContactEndHook=procedure(const ContactPair:PKraftContactPair) of object;
     TKraftContactManagerOnContactStayHook=procedure(const ContactPair:PKraftContactPair) of object;

     PKraftContactIndices=^TKraftContactIndices;
     TKraftContactIndices=array[0..MAX_CONTACTS-1] of longint;

     TKraftMeshContactPair=class
      private

       fContactManager:TKraftContactManager;

       fPrevious:TKraftMeshContactPair;
       fNext:TKraftMeshContactPair;

       fHashBucket:longint;
       fHashPrevious:TKraftMeshContactPair;
       fHashNext:TKraftMeshContactPair;

       fIsOnFreeList:boolean;

       fFlags:TKraftContactFlags;

       fShapeConvex:TKraftShape;
       fShapeMesh:TKraftShape;

       fRigidBodyConvex:TKraftRigidBody;
       fRigidBodyMesh:TKraftRigidBody;

       fConvexAABBInMeshLocalSpace:TKraftAABB;

      public

       constructor Create(const AContactManager:TKraftContactManager);
       destructor Destroy; override;
       procedure AddToHashTable; {$ifdef caninline}inline;{$endif}
       procedure RemoveFromHashTable; {$ifdef caninline}inline;{$endif}
       procedure MoveToFreeList;
       procedure MoveFromFreeList;
       procedure Query;
       procedure Update;
       
     end;

     PKraftMeshContactPairHashTableBucket=^TKraftMeshContactPairHashTableBucket;
     TKraftMeshContactPairHashTableBucket=record
      First:TKraftMeshContactPair;
      Last:TKraftMeshContactPair;
     end;

     TKraftMeshContactPairHashTable=array[0..(1 shl 16)-1] of TKraftMeshContactPairHashTableBucket;

     PKraftContactPairHashTableBucket=^TKraftContactPairHashTableBucket;
     TKraftContactPairHashTableBucket=record
      First:PKraftContactPair;
      Last:PKraftContactPair;
     end;

     TKraftContactPairHashTable=array[0..(1 shl 16)-1] of TKraftContactPairHashTableBucket;

     TKraftContactManagerOnCanCollide=function(const AShapeA,AShapeB:TKraftShape):boolean of object;

     PKraftContactManagerMeshTriangleContactQueueItem=^TKraftContactManagerMeshTriangleContactQueueItem;
     TKraftContactManagerMeshTriangleContactQueueItem=record
      MeshContactPair:TKraftMeshContactPair;
      TriangleIndex:longint;
     end;

     TKraftContactManagerMeshTriangleContactQueueItems=array of TKraftContactManagerMeshTriangleContactQueueItem;

     TKraftContactManager=class
      private

       fPhysics:TKraft;

       fContactPairFirst:PKraftContactPair;
       fContactPairLast:PKraftContactPair;

       fFreeContactPairs:PKraftContactPair;

       fCountContactPairs:longint;

       fMeshContactPairFirst:TKraftMeshContactPair;
       fMeshContactPairLast:TKraftMeshContactPair;

       fMeshContactPairFirstFree:TKraftMeshContactPair;
       fMeshContactPairLastFree:TKraftMeshContactPair;

       fCountMeshContactPairs:longint;

       fOnContactBegin:TKraftContactManagerOnContactBeginHook;
       fOnContactEnd:TKraftContactManagerOnContactEndHook;
       fOnContactStay:TKraftContactManagerOnContactStayHook;

       fOnCanCollide:TKraftContactManagerOnCanCollide;

       fClipVertexLists:array[0..MAX_THREADS-1,0..1] of TKraftClipVertexList;

{$ifdef DebugDraw}
       fDebugClipVertexLists:array[0..255] of TKraftClipVertexList;
       fCountDebugClipVertexLists:longint;
{$endif}

       fTemporaryContacts:array[0..MAX_THREADS-1,0..MAX_TEMPORARY_CONTACTS-1] of TKraftContact;
       fCountTemporaryContacts:array[0..MAX_THREADS-1] of longint;

       fActiveContactPairs:array of PKraftContactPair;
       fCountActiveContactPairs:longint;
       fCountRemainActiveContactPairsToDo:longint;

       fConvexConvexContactPairHashTable:TKraftContactPairHashTable;

       fConvexMeshTriangleContactPairHashTable:TKraftContactPairHashTable;

       fMeshContactPairHashTable:TKraftMeshContactPairHashTable;

      public

       constructor Create(const APhysics:TKraft);
       destructor Destroy; override;

       function HasDuplicateContact(const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AShapeA,AShapeB:TKraftShape;const AElementIndex:longint=-1):boolean;

       procedure AddConvexContact(const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AShapeA,AShapeB:TKraftShape;const AElementIndex:longint=-1;const AMeshContactPair:TKraftMeshContactPair=nil);

       procedure AddMeshContact(const ARigidBodyConvex,ARigidBodyMesh:TKraftRigidBody;const AShapeConvex,AShapeMesh:TKraftShape);

       procedure AddContact(const AShapeA,AShapeB:TKraftShape);

       procedure RemoveContact(AContactPair:PKraftContactPair);

       procedure RemoveMeshContact(AMeshContactPair:TKraftMeshContactPair);

       procedure RemoveContactsFromRigidBody(ARigidBody:TKraftRigidBody);

       procedure DoBroadPhase;

       procedure DoMidPhase;

       procedure ProcessContactPair(const ContactPair:PKraftContactPair;const ThreadIndex:longint=0);

{$ifdef KraftPasMP}
       procedure ProcessContactPairParallelForFunction(const Job:PPasMPJob;const ThreadIndex:longint;const Data:pointer;const FromIndex,ToIndex:TPasMPNativeInt);
{$else}
       procedure ProcessContactPairJob(const JobIndex,ThreadIndex:longint);
{$endif}

       procedure DoNarrowPhase;

{$ifdef DebugDraw}
       procedure DebugDraw(const CameraMatrix:TKraftMatrix4x4);
{$endif}

       function ReduceContacts(const AInputContacts:PKraftContacts;const ACountInputContacts:longint;const AOutputContacts:PKraftContacts):longint;

       function GetMaximizedAreaReducedContactIndices(const AInputContactPositions:PPKraftVector3s;const ACountInputContactPositions:longint;var AOutputContactIndices:TKraftContactIndices):longint;

       property ContactPairFirst:PKraftContactPair read fContactPairFirst write fContactPairFirst;
       property ContactPairLast:PKraftContactPair read fContactPairLast write fContactPairLast;

       property FreeContactPairs:PKraftContactPair read fFreeContactPairs write fFreeContactPairs;

       property Physics:TKraft read fPhysics;

       property CountContactPairs:longint read fCountContactPairs;

       property MeshContactPairFirst:TKraftMeshContactPair read fMeshContactPairFirst;
       property MeshContactPairLast:TKraftMeshContactPair read fMeshContactPairLast;

       property MeshContactPairFirstFree:TKraftMeshContactPair read fMeshContactPairFirstFree;
       property MeshContactPairLastFree:TKraftMeshContactPair read fMeshContactPairLastFree;

       property CountMeshContactPairs:longint read fCountMeshContactPairs;

      published

       property OnContactBegin:TKraftContactManagerOnContactBeginHook read fOnContactBegin write fOnContactBegin;
       property OnContactEnd:TKraftContactManagerOnContactEndHook read fOnContactEnd write fOnContactEnd;
       property OnContactStay:TKraftContactManagerOnContactStayHook read fOnContactStay write fOnContactStay;

       property OnCanCollide:TKraftContactManagerOnCanCollide read fOnCanCollide write fOnCanCollide;

     end;

     PKraftBroadPhaseContactPair=^TKraftBroadPhaseContactPair;
     TKraftBroadPhaseContactPair=array[0..1] of TKraftShape;

     TKraftBroadPhaseContactPairs=array of TKraftBroadPhaseContactPair;

     TKraftBroadPhase=class
      private

       fPhysics:TKraft;

       fStack:array[0..MAX_THREADS-1] of PKraftDynamicAABBTreeLongintArray;
       fStackCapacity:array[0..MAX_THREADS-1] of longint;

       fContactPairs:array[0..MAX_THREADS-1] of TKraftBroadPhaseContactPairs;
       fCountContactPairs:array[0..MAX_THREADS-1] of longint;

       fStaticMoveBuffer:array of longint;
       fStaticMoveBufferSize:longint;

       fDynamicMoveBuffer:array of longint;
       fDynamicMoveBufferSize:longint;

       fKinematicMoveBuffer:array of longint;
       fKinematicMoveBufferSize:longint;

       fAllMoveBufferSize:longint;

       procedure AddPair(const ThreadIndex:longint;ShapeA,ShapeB:TKraftShape); {$ifdef caninline}inline;{$endif}

       procedure QueryShapeWithTree(const ThreadIndex:longint;const Shape:TKraftShape;const AABBTree:TKraftDynamicAABBTree);

{$ifdef KraftPasMP}
       procedure ProcessMoveBufferItemParallelForFunction(const Job:PPasMPJob;const ThreadIndex:longint;const Data:pointer;const FromIndex,ToIndex:TPasMPNativeInt);
{$else}
       procedure ProcessMoveBufferItem(const JobIndex,ThreadIndex:longint);
{$endif}


      public

       constructor Create(const APhysics:TKraft);
       destructor Destroy; override;

       procedure UpdatePairs; {$ifdef caninline}inline;{$endif}

       procedure StaticBufferMove(ProxyID:longint); {$ifdef caninline}inline;{$endif}
       procedure DynamicBufferMove(ProxyID:longint); {$ifdef caninline}inline;{$endif}
       procedure KinematicBufferMove(ProxyID:longint); {$ifdef caninline}inline;{$endif}

     end;

     TKraftRigidBodyOnDamping=procedure(const RigidBody:TKraftRigidBody;const TimeStep:TKraftTimeStep) of object;

     TKraftRigidBodyOnStep=procedure(const RigidBody:TKraftRigidBody;const TimeStep:TKraftTimeStep) of object;

     TKraftConstraint=class;

     PKraftConstraintEdge=^TKraftConstraintEdge;
     TKraftConstraintEdge=record
      Previous:PKraftConstraintEdge;
      Next:PKraftConstraintEdge;
      Constraint:TKraftConstraint;
      OtherRigidBody:TKraftRigidBody;
     end;

     TKraftRigidBodyIslandIndices=array of longint;

     TKraftRigidBody=class(TPersistent)
      private

       fPhysics:TKraft;

       fIsland:TKraftIsland;

       fIslandIndices:TKraftRigidBodyIslandIndices;

       fID:uint64;

       fRigidBodyType:TKraftRigidBodyType;

       fRigidBodyPrevious:TKraftRigidBody;
       fRigidBodyNext:TKraftRigidBody;

       fStaticRigidBodyIsOnList:boolean;
       fStaticRigidBodyPrevious:TKraftRigidBody;
       fStaticRigidBodyNext:TKraftRigidBody;

       fDynamicRigidBodyIsOnList:boolean;
       fDynamicRigidBodyPrevious:TKraftRigidBody;
       fDynamicRigidBodyNext:TKraftRigidBody;

       fKinematicRigidBodyIsOnList:boolean;
       fKinematicRigidBodyPrevious:TKraftRigidBody;
       fKinematicRigidBodyNext:TKraftRigidBody;

       fShapeFirst:TKraftShape;
       fShapeLast:TKraftShape;

       fShapeCount:longint;

       fFlags:TKraftRigidBodyFlags;

       fWorldDisplacement:TKraftVector3;

       fWorldTransform:TKraftMatrix4x4;

       fSweep:TKraftSweep;

       fGravity:TKraftVector3;

       fGravityProperty:TKraftVector3Property;

       fLinearFactor:TKraftVector3;

       fUserData:pointer;

       fTimeOfImpact:TKraftScalar;

       fNextOnIslandBuildStack:TKraftRigidBody;
       fNextStaticRigidBody:TKraftRigidBody;

       fBodyInertiaTensor:TKraftMatrix3x3;
       fBodyInverseInertiaTensor:TKraftMatrix3x3;

       fWorldInertiaTensor:TKraftMatrix3x3;
       fWorldInverseInertiaTensor:TKraftMatrix3x3;

       fForcedMass:TKraftScalar;

       fMass:TKraftScalar;
       fInverseMass:TKraftScalar;

       fLinearVelocity:TKraftVector3;
       fAngularVelocity:TKraftVector3;

       fMaximalLinearVelocity:TKraftScalar;
       fMaximalAngularVelocity:TKraftScalar;

       fLinearVelocityDamp:TKraftScalar;
       fAngularVelocityDamp:TKraftScalar;
       fAdditionalDamping:boolean;
       fAdditionalDamp:TKraftScalar;
       fLinearVelocityAdditionalDamp:TKraftScalar;
       fAngularVelocityAdditionalDamp:TKraftScalar;
       fLinearVelocityAdditionalDampThresholdSqr:TKraftScalar;
       fAngularVelocityAdditionalDampThresholdSqr:TKraftScalar;

       fForce:TKraftVector3;
       fTorque:TKraftVector3;

       fSleepTime:TKraftScalar;

       fGravityScale:TKraftScalar;

       fEnableGyroscopicForce:boolean;

       fMaximalGyroscopicForce:TKraftScalar;

       fCollisionGroups:TKraftRigidBodyCollisionGroups;

       fCollideWithCollisionGroups:TKraftRigidBodyCollisionGroups;

       fCountConstraints:longint;

       fConstraintEdgeFirst:PKraftConstraintEdge;
       fConstraintEdgeLast:PKraftConstraintEdge;

       fContactPairEdgeFirst:PKraftContactPairEdge;
       fContactPairEdgeLast:PKraftContactPairEdge;

       fOnDamping:TKraftRigidBodyOnDamping;

       fOnPreStep:TKraftRigidBodyOnStep;
       fOnPostStep:TKraftRigidBodyOnStep;

       function GetAngularMomentum:TKraftVector3;
       procedure SetAngularMomentum(const NewAngularMomentum:TKraftVector3);

      public

       constructor Create(const APhysics:TKraft);
       destructor Destroy; override;

       function SetRigidBodyType(ARigidBodyType:TKraftRigidBodyType):TKraftRigidBody;

       function IsStatic:boolean;
       function IsDynamic:boolean;
       function IsKinematic:boolean;

       procedure SynchronizeTransform;

       procedure SynchronizeTransformIncludingShapes;

       procedure StoreWorldTransform; virtual;

       procedure InterpolateWorldTransform(const Alpha:TKraftScalar); virtual;

       procedure Advance(Alpha:TKraftScalar);

       procedure UpdateWorldInertiaTensor;

       procedure Finish;

       procedure SynchronizeProxies;

       procedure Refilter;

       function CanCollideWith(OtherRigidBody:TKraftRigidBody):boolean;

       procedure SetToAwake;

       procedure SetToSleep;

       procedure SetWorldTransformation(const AWorldTransformation:TKraftMatrix4x4);

       procedure SetWorldPosition(const AWorldPosition:TKraftVector3);

       procedure SetOrientation(const AOrientation:TKraftMatrix3x3); overload;
       procedure SetOrientation(const x,y,z:TKraftScalar); overload;
       procedure AddOrientation(const x,y,z:TKraftScalar);

       procedure LimitVelocities;

       procedure ApplyImpulseAtPosition(const Point,Impulse:TKraftVector3);
       procedure ApplyImpulseAtRelativePosition(const RelativePosition,Impulse:TKraftVector3);

       procedure SetForceAtPosition(const AForce,APosition:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddForceAtPosition(const AForce,APosition:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetWorldForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddWorldForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetBodyForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddBodyForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetWorldTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddWorldTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetBodyTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddBodyTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetWorldAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddWorldAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetBodyAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddBodyAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetWorldAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddWorldAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       procedure SetBodyAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
       procedure AddBodyAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);

       property Physics:TKraft read fPhysics;

       property Island:TKraftIsland read fIsland;

       property IslandIndices:TKraftRigidBodyIslandIndices read fIslandIndices;

       property ID:uint64 read fID;

       property RigidBodyPrevious:TKraftRigidBody read fRigidBodyPrevious;
       property RigidBodyNext:TKraftRigidBody read fRigidBodyNext;

       property StaticRigidBodyIsOnList:boolean read fStaticRigidBodyIsOnList;
       property StaticRigidBodyPrevious:TKraftRigidBody read fStaticRigidBodyPrevious;
       property StaticRigidBodyNext:TKraftRigidBody read fStaticRigidBodyNext;

       property DynamicRigidBodyIsOnList:boolean read fDynamicRigidBodyIsOnList write fDynamicRigidBodyIsOnList;
       property DynamicRigidBodyPrevious:TKraftRigidBody read fDynamicRigidBodyPrevious write fDynamicRigidBodyPrevious;
       property DynamicRigidBodyNext:TKraftRigidBody read fDynamicRigidBodyNext write fDynamicRigidBodyNext;

       property KinematicRigidBodyIsOnList:boolean read fKinematicRigidBodyIsOnList write fKinematicRigidBodyIsOnList;
       property KinematicRigidBodyPrevious:TKraftRigidBody read fKinematicRigidBodyPrevious write fKinematicRigidBodyPrevious;
       property KinematicRigidBodyNext:TKraftRigidBody read fKinematicRigidBodyNext write fKinematicRigidBodyNext;

       property ShapeFirst:TKraftShape read fShapeFirst write fShapeFirst;
       property ShapeLast:TKraftShape read fShapeLast write fShapeLast;

       property ShapeCount:longint read fShapeCount write fShapeCount;

       property WorldTransform:TKraftMatrix4x4 read fWorldTransform write fWorldTransform;

       property UserData:pointer read fUserData write fUserData;

       property BodyInertiaTensor:TKraftMatrix3x3 read fBodyInertiaTensor write fBodyInertiaTensor;
       property BodyInverseInertiaTensor:TKraftMatrix3x3 read fBodyInverseInertiaTensor write fBodyInverseInertiaTensor;

       property WorldInertiaTensor:TKraftMatrix3x3 read fWorldInertiaTensor write fWorldInertiaTensor;
       property WorldInverseInertiaTensor:TKraftMatrix3x3 read fWorldInverseInertiaTensor write fWorldInverseInertiaTensor;

       property ConstraintEdgeFirst:PKraftConstraintEdge read fConstraintEdgeFirst write fConstraintEdgeFirst;
       property ConstraintEdgeLast:PKraftConstraintEdge read fConstraintEdgeLast write fConstraintEdgeLast;

       property CountConstraints:longint read fCountConstraints;

       property ContactPairEdgeFirst:PKraftContactPairEdge read fContactPairEdgeFirst write fContactPairEdgeFirst;
       property ContactPairEdgeLast:PKraftContactPairEdge read fContactPairEdgeLast write fContactPairEdgeLast;

       property WorldDisplacement:TKraftVector3 read fWorldDisplacement;

       property Sweep:TKraftSweep read fSweep write fSweep;

       property SleepTime:TKraftScalar read fSleepTime;

       property Mass:TKraftScalar read fMass;
       property InverseMass:TKraftScalar read fInverseMass;

       property LinearVelocity:TKraftVector3 read fLinearVelocity write fLinearVelocity;
       property AngularVelocity:TKraftVector3 read fAngularVelocity write fAngularVelocity;

       property AngularMomentum:TKraftVector3 read GetAngularMomentum write SetAngularMomentum;

       property Force:TKraftVector3 read fForce write fForce;
       property Torque:TKraftVector3 read fTorque write fTorque;

       property NextOnIslandBuildStack:TKraftRigidBody read fNextOnIslandBuildStack;
       property NextStaticRigidBody:TKraftRigidBody read fNextStaticRigidBody;

      published

       property RigidBodyType:TKraftRigidBodyType read fRigidBodyType write fRigidBodyType;

       property Flags:TKraftRigidBodyFlags read fFlags write fFlags;

       property Gravity:TKraftVector3Property read fGravityProperty write fGravityProperty;

       property TimeOfImpact:TKraftScalar read fTimeOfImpact write fTimeOfImpact;

       property ForcedMass:TKraftScalar read fForcedMass write fForcedMass;

       property MaximalLinearVelocity:TKraftScalar read fMaximalLinearVelocity write fMaximalLinearVelocity;
       property MaximalAngularVelocity:TKraftScalar read fMaximalAngularVelocity write fMaximalAngularVelocity;

       property LinearVelocityDamp:TKraftScalar read fLinearVelocityDamp write fLinearVelocityDamp;
       property AngularVelocityDamp:TKraftScalar read fAngularVelocityDamp write fAngularVelocityDamp;
       property AdditionalDamping:boolean read fAdditionalDamping write fAdditionalDamping;
       property AdditionalDamp:TKraftScalar read fAdditionalDamp write fAdditionalDamp;
       property LinearVelocityAdditionalDamp:TKraftScalar read fLinearVelocityAdditionalDamp write fLinearVelocityAdditionalDamp;
       property AngularVelocityAdditionalDamp:TKraftScalar read fAngularVelocityAdditionalDamp write fAngularVelocityAdditionalDamp;
       property LinearVelocityAdditionalDampThresholdSqr:TKraftScalar read fLinearVelocityAdditionalDampThresholdSqr write fLinearVelocityAdditionalDampThresholdSqr;
       property AngularVelocityAdditionalDampThresholdSqr:TKraftScalar read fAngularVelocityAdditionalDampThresholdSqr write fAngularVelocityAdditionalDampThresholdSqr;

       property GravityScale:TKraftScalar read fGravityScale write fGravityScale;

       property EnableGyroscopicForce:boolean read fEnableGyroscopicForce write fEnableGyroscopicForce;

       property MaximalGyroscopicForce:TKraftScalar read fMaximalGyroscopicForce write fMaximalGyroscopicForce;

       property CollisionGroups:TKraftRigidBodyCollisionGroups read fCollisionGroups write fCollisionGroups;

       property CollideWithCollisionGroups:TKraftRigidBodyCollisionGroups read fCollideWithCollisionGroups write fCollideWithCollisionGroups;

       property OnDamping:TKraftRigidBodyOnDamping read fOnDamping write fOnDamping;

       property OnPreStep:TKraftRigidBodyOnStep read fOnPreStep write fOnPreStep;
       property OnPostStep:TKraftRigidBodyOnStep read fOnPostStep write fOnPostStep;

     end;

     TKraftRigidBodies=array of TKraftRigidBody;

     PKraftSolverVelocity=^TKraftSolverVelocity;
     TKraftSolverVelocity=record
      LinearVelocity:TKraftVector3;
      AngularVelocity:TKraftVector3;
     end;

     TKraftSolverVelocities=array of TKraftSolverVelocity;

     PKraftSolverPosition=^TKraftSolverPosition;
     TKraftSolverPosition=record
      Position:TKraftVector3;
      Orientation:TKraftQuaternion;
     end;

     TKraftSolverPositions=array of TKraftSolverPosition;

     TKraftSolverLinearFactors=array of TKraftVector3;

     TKraftConstraintEdges=array[0..1] of TKraftConstraintEdge;

     TKraftConstraintRigidBodies=array[0..1] of TKraftRigidBody;

     TKraftConstraintLimitState=(kclsInactiveLimit,kclsAtLowerLimit,kclsAtUpperLimit,kclsEqualLimits);

     TKraftConstraintOnBreak=procedure(APhysics:TKraft;AConstraint:TKraftConstraint) of object;

     TKraftConstraint=class(TPersistent)
      private

       fPhysics:TKraft;

       fPrevious:TKraftConstraint;
       fNext:TKraftConstraint;

       fUserData:pointer;

       fFlags:TKraftConstraintFlags;

       fConstraintEdges:TKraftConstraintEdges;

       fRigidBodies:TKraftConstraintRigidBodies;

       fBreakThresholdForce:TKraftScalar;

       fBreakThresholdTorque:TKraftScalar;

       fOnBreak:TKraftConstraintOnBreak;

       fParent:TKraftConstraint;

       fChildren:array of TKraftConstraint;
       fCountChildren:longint;

      public
      
       constructor Create(const APhysics:TKraft);
       destructor Destroy; override;

       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); virtual;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); virtual;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; virtual;

       function GetAnchorA:TKraftVector3; virtual;
       function GetAnchorB:TKraftVector3; virtual;

       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; virtual;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; virtual;

       property Physics:TKraft read fPhysics;

       property Previous:TKraftConstraint read fPrevious;
       property Next:TKraftConstraint read fNext;

       property UserData:pointer read fUserData write fUserData;

       property ConstraintEdges:TKraftConstraintEdges read fConstraintEdges write fConstraintEdges;

       property RigidBodies:TKraftConstraintRigidBodies read fRigidBodies write fRigidBodies;

      published

       property Flags:TKraftConstraintFlags read fFlags write fFlags;

       property BreakThresholdForce:TKraftScalar read fBreakThresholdForce write fBreakThresholdForce;

       property BreakThresholdTorque:TKraftScalar read fBreakThresholdTorque write fBreakThresholdTorque;

       property OnBreak:TKraftConstraintOnBreak read fOnBreak write fOnBreak;

     end;

     TKraftConstraints=array of TKraftConstraint;

     TKraftConstraintJoint=class(TKraftConstraint);

     // Constrains a body to a specified world position which can change over time.
     TKraftConstraintJointGrab=class(TKraftConstraintJoint)
      private
       fIslandIndex:longint;
       fInverseMass:TKraftScalar;
       fSolverVelocity:PKraftSolverVelocity;
       fSolverPosition:PKraftSolverPosition;
       fSolverLinearFactor:PKraftVector3;
       fWorldInverseInertiaTensor:TKraftMatrix3x3;
       fRelativePosition:TKraftVector3;
       fLocalCenter:TKraftVector3;
       fLocalAnchor:TKraftVector3;
       fmC:TKraftVector3;
       fFrequencyHz:TKraftScalar;
       fDampingRatio:TKraftScalar;
       fAccumulatedImpulse:TKraftVector3;
       fBeta:TKraftScalar;
       fGamma:TKraftScalar;
       fMass:TKraftScalar;
       fEffectiveMass:TKraftMatrix3x3;
       fWorldPoint:TKraftVector3;
       fMaximalForce:TKraftScalar;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AWorldPoint:TKraftVector3;const AFrequencyHz:TKraftScalar=5.0;const ADampingRatio:TKraftScalar=0.7;const AMaximalForce:TKraftScalar=MAX_SCALAR;const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchor:TKraftVector3; virtual;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetWorldPoint:TKraftVector3; virtual;
       function GetMaximalForce:TKraftScalar; virtual;
       procedure SetWorldPoint(AWorldPoint:TKraftVector3); virtual;
       procedure SetMaximalForce(AMaximalForce:TKraftScalar); virtual;
     end;

     // Keeps body at some fixed distance to a world plane.
     TKraftConstraintJointWorldPlaneDistance=class(TKraftConstraintJoint)
      private
       fIslandIndex:longint;
       fInverseMass:TKraftScalar;
       fSolverVelocity:PKraftSolverVelocity;
       fSolverPosition:PKraftSolverPosition;
       fSolverLinearFactor:PKraftVector3;
       fWorldInverseInertiaTensor:TKraftMatrix3x3;
       fRelativePosition:TKraftVector3;
       fLocalCenter:TKraftVector3;
       fLocalAnchor:TKraftVector3;
       fmU:TKraftVector3;
       fWorldPoint:TKraftVector3;
       fWorldPlane:TKraftPlane;
       fWorldDistance:TKraftScalar;
       fFrequencyHz:TKraftScalar;
       fDampingRatio:TKraftScalar;
       fInverseInertiaTensorRatio:TKraftScalar;
       fAccumulatedImpulse:TKraftScalar;
       fGamma:TKraftScalar;
       fBias:TKraftScalar;
       fMass:TKraftScalar;
       fLimitBehavior:TKraftConstraintLimitBehavior;
       fDoubleSidedWorldPlane:boolean;
       fSoftConstraint:boolean;
       fSkip:boolean;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const ALocalAnchorPoint:TKraftVector3;const AWorldPlane:TKraftPlane;const ADoubleSidedWorldPlane:boolean=true;const AWorldDistance:single=1.0;const ALimitBehavior:TKraftConstraintLimitBehavior=kclbLimitDistance;const AFrequencyHz:TKraftScalar=0.0;const ADampingRatio:TKraftScalar=0.0;const AInverseInertiaTensorRatio:TKraftScalar=1.0;const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchor:TKraftVector3; virtual;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetWorldPoint:TKraftVector3; virtual;
       function GetWorldPlane:TKraftPlane; virtual;
       procedure SetWorldPlane(const AWorldPlane:TKraftPlane); virtual;
       function GetWorldDistance:TKraftScalar; virtual;
       procedure SetWorldDistance(const AWorldDistance:TKraftScalar); virtual;
     end;

     // Keeps bodies at some fixed distance from each other.
     TKraftConstraintJointDistance=class(TKraftConstraintJoint)
      private
       fIslandIndices:array[0..1] of longint;
       fInverseMasses:array[0..1] of TKraftScalar;
       fSolverVelocities:array[0..1] of PKraftSolverVelocity;
       fSolverPositions:array[0..1] of PKraftSolverPosition;
       fSolverLinearFactors:array[0..1] of PKraftVector3;
       fWorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
       fRelativePositions:array[0..1] of TKraftVector3;
       fLocalCenters:array[0..1] of TKraftVector3;
       fLocalAnchors:array[0..1] of TKraftVector3;
       fmU:TKraftVector3;
       fAnchorDistanceLength:TKraftScalar;
       fFrequencyHz:TKraftScalar;
       fDampingRatio:TKraftScalar;
       fAccumulatedImpulse:TKraftScalar;
       fGamma:TKraftScalar;
       fBias:TKraftScalar;
       fMass:TKraftScalar;
      public
       constructor Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const ALocalAnchorPointA,ALocalAnchorPointB:TKraftVector3;const AFrequencyHz:TKraftScalar=0.0;const ADampingRatio:TKraftScalar=0.0;const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchorA:TKraftVector3; override;
       function GetAnchorB:TKraftVector3; override;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
     end;

     // Restricts the maximum distance between two points.
     TKraftConstraintJointRope=class(TKraftConstraintJoint)
      private
       fIslandIndices:array[0..1] of longint;
       fInverseMasses:array[0..1] of TKraftScalar;
       fSolverVelocities:array[0..1] of PKraftSolverVelocity;
       fSolverPositions:array[0..1] of PKraftSolverPosition;
       fSolverLinearFactors:array[0..1] of PKraftVector3;
       fWorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
       fRelativePositions:array[0..1] of TKraftVector3;
       fLocalCenters:array[0..1] of TKraftVector3;
       fLocalAnchors:array[0..1] of TKraftVector3;
       fMaximalLength:TKraftScalar;
       fAccumulatedImpulse:TKraftScalar;
       fmU:TKraftVector3;
       fCurrentLength:TKraftScalar;
       fMass:TKraftScalar;
       fBias:TKraftScalar;
       fLimitState:TKraftConstraintLimitState;
      public
       constructor Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const ALocalAnchorPointA,ALocalAnchorPointB:TKraftVector3;const AMaximalLength:TKraftScalar=1.0;const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchorA:TKraftVector3; override;
       function GetAnchorB:TKraftVector3; override;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       property LimitState:TKraftConstraintLimitState read fLimitState;
     end;

     // Connects two bodies to ground and to each other. As one body goes up, the other goes down.
     TKraftConstraintJointPulley=class(TKraftConstraintJoint)
      private
       fIslandIndices:array[0..1] of longint;
       fInverseMasses:array[0..1] of TKraftScalar;
       fSolverVelocities:array[0..1] of PKraftSolverVelocity;
       fSolverPositions:array[0..1] of PKraftSolverPosition;
       fSolverLinearFactors:array[0..1] of PKraftVector3;
       fWorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
       fRelativePositions:array[0..1] of TKraftVector3;
       fLocalCenters:array[0..1] of TKraftVector3;
       fGroundAnchors:array[0..1] of TKraftVector3;
       fLocalAnchors:array[0..1] of TKraftVector3;
       fmU:array[0..1] of TKraftVector3;
       fLengths:array[0..1] of TKraftScalar;
       fAccumulatedImpulse:TKraftScalar;
       fConstant:TKraftScalar;
       fMass:TKraftScalar;
       fRatio:TKraftScalar;
       fBias:TKraftScalar;
      public
       constructor Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AWorldGroundAnchorA,AWorldGroundAnchorB,AWorldAnchorPointA,AWorldAnchorPointB:TKraftVector3;const ARatio:TKraftScalar=1.0;const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchorA:TKraftVector3; override;
       function GetAnchorB:TKraftVector3; override;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetCurrentLengthA:TKraftScalar;
       function GetCurrentLengthB:TKraftScalar;
     end;

     // Allows arbitrary rotation between two bodies. This joint has three degrees of freedom.
     TKraftConstraintJointBallSocket=class(TKraftConstraintJoint)
      private
       fIslandIndices:array[0..1] of longint;
       fInverseMasses:array[0..1] of TKraftScalar;
       fSolverVelocities:array[0..1] of PKraftSolverVelocity;
       fSolverPositions:array[0..1] of PKraftSolverPosition;
       fSolverLinearFactors:array[0..1] of PKraftVector3;
       fWorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
       fRelativePositions:array[0..1] of TKraftVector3;
       fLocalCenters:array[0..1] of TKraftVector3;
       fLocalAnchors:array[0..1] of TKraftVector3;
       fAccumulatedImpulse:TKraftVector3;
       fBiasVector:TKraftVector3;
       fInverseMassMatrix:TKraftMatrix3x3;
      public
       constructor Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AWorldAnchorPoint:TKraftVector3;const ACollideConnected:boolean=false); reintroduce; overload;
       constructor Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const ALocalAnchorPointA,ALocalAnchorPointB:TKraftVector3;const ACollideConnected:boolean=false); reintroduce; overload;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchorA:TKraftVector3; override;
       function GetAnchorB:TKraftVector3; override;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
     end;

     // Forbids any translation or rotation between two bodies.
     TKraftConstraintJointFixed=class(TKraftConstraintJoint)
      private
       fIslandIndices:array[0..1] of longint;
       fInverseMasses:array[0..1] of TKraftScalar;
       fSolverVelocities:array[0..1] of PKraftSolverVelocity;
       fSolverPositions:array[0..1] of PKraftSolverPosition;
       fSolverLinearFactors:array[0..1] of PKraftVector3;
       fWorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
       fRelativePositions:array[0..1] of TKraftVector3;
       fLocalCenters:array[0..1] of TKraftVector3;
       fLocalAnchors:array[0..1] of TKraftVector3;
       fAccumulatedImpulseTranslation:TKraftVector3;
       fAccumulatedImpulseRotation:TKraftVector3;
       fBiasTranslation:TKraftVector3;
       fBiasRotation:TKraftVector3;
       fInverseMassMatrixTranslation:TKraftMatrix3x3;
       fInverseMassMatrixRotation:TKraftMatrix3x3;
       fInverseInitialOrientationDifference:TKraftQuaternion;
      public
       constructor Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AWorldAnchorPoint:TKraftVector3;const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchorA:TKraftVector3; override;
       function GetAnchorB:TKraftVector3; override;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
     end;

     // Allows arbitrary rotation between two bodies around a TKraftScalar axis. This joint has one degree of freedom.
     TKraftConstraintJointHinge=class(TKraftConstraintJoint)
      private
       fIslandIndices:array[0..1] of longint;
       fInverseMasses:array[0..1] of TKraftScalar;
       fSolverVelocities:array[0..1] of PKraftSolverVelocity;
       fSolverPositions:array[0..1] of PKraftSolverPosition;
       fSolverLinearFactors:array[0..1] of PKraftVector3;
       fWorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
       fRelativePositions:array[0..1] of TKraftVector3;
       fLocalCenters:array[0..1] of TKraftVector3;
       fLocalAnchors:array[0..1] of TKraftVector3;
       fLocalAxes:array[0..1] of TKraftVector3;
       fAccumulatedImpulseLowerLimit:TKraftScalar;
       fAccumulatedImpulseUpperLimit:TKraftScalar;
       fAccumulatedImpulseMotor:TKraftScalar;
       fAccumulatedImpulseTranslation:TKraftVector3;
       fAccumulatedImpulseRotation:TKraftVector2;
       fB2CrossA1:TKraftVector3;
       fC2CrossA1:TKraftVector3;
       fA1:TKraftVector3;
       fBiasTranslation:TKraftVector3;
       fBiasRotation:TKraftVector2;
       fInverseMassMatrixTranslation:TKraftMatrix3x3;
       fInverseMassMatrixRotation:TKraftMatrix2x2;
       fInverseMassMatrixLimitMotor:TKraftScalar;
       fInverseInitialOrientationDifference:TKraftQuaternion;
       fLimitState:boolean;
       fMotorState:boolean;
       fLowerLimit:TKraftScalar;
       fUpperLimit:TKraftScalar;
       fBiasLowerLimit:TKraftScalar;
       fBiasUpperLimit:TKraftScalar;
       fIsLowerLimitViolated:boolean;
       fIsUpperLimitViolated:boolean;
       fMotorSpeed:TKraftScalar;
       fMaximalMotorTorque:TKraftScalar;
       function ComputeCurrentHingeAngle(const OrientationA,OrientationB:TKraftQuaternion):TKraftScalar;
      public
       constructor Create(const APhysics:TKraft;
                          const ARigidBodyA,ARigidBodyB:TKraftRigidBody;
                          const AWorldAnchorPoint:TKraftVector3;
                          const AWorldRotationAxis:TKraftVector3;
                          const ALimitEnabled:boolean=false;
                          const AMotorEnabled:boolean=false;
                          const AMinimumAngleLimit:TKraftScalar=-1.0;
                          const AMaximumAngleLimit:TKraftScalar=1.0;
                          const AMotorSpeed:TKraftScalar=0.0;
                          const AMaximalMotorTorque:TKraftScalar=0.0;
                          const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchorA:TKraftVector3; override;
       function GetAnchorB:TKraftVector3; override;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetWorldRotationAxis:TKraftVector3; virtual;
       procedure SetWorldRotationAxis(AWorldRotationAxis:TKraftVector3); virtual;
       function IsLimitEnabled:boolean; virtual;
       function IsMotorEnabled:boolean; virtual;
       function GetMinimumAngleLimit:TKraftScalar; virtual;
       function GetMaximumAngleLimit:TKraftScalar; virtual;
       function GetMotorSpeed:TKraftScalar; virtual;
       function GetMaximalMotorTorque:TKraftScalar; virtual;
       function GetMotorTorque(const DeltaTime:TKraftScalar):TKraftScalar; virtual;
       procedure ResetLimits; virtual;
       procedure EnableLimit(const ALimitEnabled:boolean); virtual;
       procedure EnableMotor(const AMotorEnabled:boolean); virtual;
       procedure SetMinimumAngleLimit(const AMinimumAngleLimit:TKraftScalar); virtual;
       procedure SetMaximumAngleLimit(const AMaximumAngleLimit:TKraftScalar); virtual;
       procedure SetMotorSpeed(const AMotorSpeed:TKraftScalar); virtual;
       procedure SetMaximalMotorTorque(const AMaximalMotorTorque:TKraftScalar); virtual;
     end;

     // Allows relative translation of the bodies along a TKraftScalar direction and no rotation
     TKraftConstraintJointSlider=class(TKraftConstraintJoint)
      private
       fIslandIndices:array[0..1] of longint;
       fInverseMasses:array[0..1] of TKraftScalar;
       fSolverVelocities:array[0..1] of PKraftSolverVelocity;
       fSolverPositions:array[0..1] of PKraftSolverPosition;
       fSolverLinearFactors:array[0..1] of PKraftVector3;
       fWorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
       fRelativePositions:array[0..1] of TKraftVector3;
       fLocalCenters:array[0..1] of TKraftVector3;
       fLocalAnchors:array[0..1] of TKraftVector3;
       fAccumulatedImpulseLowerLimit:TKraftScalar;
       fAccumulatedImpulseUpperLimit:TKraftScalar;
       fAccumulatedImpulseMotor:TKraftScalar;
       fAccumulatedImpulseTranslation:TKraftVector2;
       fAccumulatedImpulseRotation:TKraftVector3;
       fSliderAxisBodyA:TKraftVector3;
       fSliderAxisWorld:TKraftVector3;
       fN1:TKraftVector3;
       fN2:TKraftVector3;
       fR2CrossN1:TKraftVector3;
       fR2CrossN2:TKraftVector3;
       fR2CrossSliderAxis:TKraftVector3;
       fR1PlusUCrossN1:TKraftVector3;
       fR1PlusUCrossN2:TKraftVector3;
       fR1PlusUCrossSliderAxis:TKraftVector3;
       fBiasTranslation:TKraftVector2;
       fBiasRotation:TKraftVector3;
       fInverseMassMatrixTranslationConstraint:TKraftMatrix2x2;
       fInverseMassMatrixRotationConstraint:TKraftMatrix3x3;
       fInverseMassMatrixLimit:TKraftScalar;
       fInverseMassMatrixMotor:TKraftScalar;
       fInverseInitialOrientationDifference:TKraftQuaternion;
       fLimitState:boolean;
       fMotorState:boolean;
       fLowerLimit:TKraftScalar;
       fUpperLimit:TKraftScalar;
       fBiasLowerLimit:TKraftScalar;
       fBiasUpperLimit:TKraftScalar;
       fIsLowerLimitViolated:boolean;
       fIsUpperLimitViolated:boolean;
       fMotorSpeed:TKraftScalar;
       fMaximalMotorForce:TKraftScalar;
      public
       constructor Create(const APhysics:TKraft;
                          const ARigidBodyA,ARigidBodyB:TKraftRigidBody;
                          const AWorldAnchorPoint:TKraftVector3;
                          const AWorldSliderAxis:TKraftVector3;
                          const ALimitEnabled:boolean=false;
                          const AMotorEnabled:boolean=false;
                          const AMinimumTranslationLimit:TKraftScalar=-1.0;
                          const AMaximumTranslationLimit:TKraftScalar=1.0;
                          const AMotorSpeed:TKraftScalar=0.0;
                          const AMaximalMotorForce:TKraftScalar=0.0;
                          const ACollideConnected:boolean=false); reintroduce;
       destructor Destroy; override;
       procedure InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       procedure SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep); override;
       function SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean; override;
       function GetAnchorA:TKraftVector3; override;
       function GetAnchorB:TKraftVector3; override;
       function GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3; override;
       function IsLimitEnabled:boolean; virtual;
       function IsMotorEnabled:boolean; virtual;
       function GetMinimumTranslationLimit:TKraftScalar; virtual;
       function GetMaximumTranslationLimit:TKraftScalar; virtual;
       function GetMotorSpeed:TKraftScalar; virtual;
       function GetMaximalMotorForce:TKraftScalar; virtual;
       function GetMotorForce(const DeltaTime:TKraftScalar):TKraftScalar; virtual;
       function GetTranslation:TKraftScalar; virtual;
       procedure ResetLimits; virtual;
       procedure EnableLimit(const ALimitEnabled:boolean); virtual;
       procedure EnableMotor(const AMotorEnabled:boolean); virtual;
       procedure SetMinimumTranslationLimit(const AMinimumTranslationLimit:TKraftScalar); virtual;
       procedure SetMaximumTranslationLimit(const AMaximumTranslationLimit:TKraftScalar); virtual;
       procedure SetMotorSpeed(const AMotorSpeed:TKraftScalar); virtual;
       procedure SetMaximalMotorForce(const AMaximalMotorForce:TKraftScalar); virtual;
     end;

     PKraftSolverVelocityStateContactPoint=^TKraftSolverVelocityStateContactPoint;
     TKraftSolverVelocityStateContactPoint=record
      RelativePositions:array[0..1] of TKraftVector3; // Vectors from center of mass to contact position
	    Penetration:TKraftScalar; // Depth of penetration from collision
	    NormalImpulse:TKraftScalar; // Accumulated normal impulse
	    TangentImpulse:array[0..1] of TKraftScalar; // Accumulated friction impulse
      BaumgarteBias:TKraftScalar; // Baumgarte bias
	    Bias:TKraftScalar; // Restitution + baumgarte
      NormalMass:TKraftScalar; // Normal constraint mass
      TangentMass:array[0..1] of TKraftScalar; // Tangent constraint mass
     end;

     PKraftSolverVelocityState=^TKraftSolverVelocityState;
     TKraftSolverVelocityState=record
      Points:array[0..MAX_CONTACTS-1] of TKraftSolverVelocityStateContactPoint;
      Normal:TKraftVector3;
      Centers:array[0..1] of TKraftVector3;
      LostSpeculativeBounce:single;
      SpeculativeVelocity:single;      
      WorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
      NormalMass:TKraftScalar;
      TangentMass:array[0..1] of TKraftScalar;
      InverseMasses:array[0..1] of TKraftScalar;
      Restitution:TKraftScalar;
      Friction:TKraftScalar;
      Indices:array[0..1] of longint;
      CountPoints:longint;
     end;

     TKraftSolverVelocityStates=array of TKraftSolverVelocityState;

     PKraftSolverPositionState=^TKraftSolverPositionState;
     TKraftSolverPositionState=record
      LocalPoints:array[0..MAX_CONTACTS-1] of TKraftVector3;
      LocalNormal:TKraftVector3;
      LocalCenters:array[0..1] of TKraftVector3;
      WorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
      InverseMasses:array[0..1] of TKraftScalar;
      Indices:array[0..1] of longint;
      CountPoints:longint;
     end;

     TKraftSolverPositionStates=array of TKraftSolverPositionState;

     PKraftSolverSpeculativeContactState=^TKraftSolverSpeculativeContactState;
     TKraftSolverSpeculativeContactState=record
      Points:array[0..1] of TKraftVector3;
      Normal:TKraftVector3;
      Centers:array[0..1] of TKraftVector3;
      LocalCenters:array[0..1] of TKraftVector3;
      RelativePositions:array[0..1] of TKraftVector3;
      WorldInverseInertiaTensors:array[0..1] of TKraftMatrix3x3;
      Separation:single;
      RestitutionBias:single;
      LostSpeculativeBounce:single;
      SpeculativeVelocity:single;
      NormalImpulse:single;
      TangentImpulse:array[0..1] of single;
      NormalMass:single;
      TangentMass:array[0..1] of single;
      InverseMasses:array[0..1] of single;
      Restitution:single;
      Friction:single;
      Indices:array[0..1] of longint;
     end;

     TKraftSolverSpeculativeContactStates=array of TKraftSolverSpeculativeContactState;

     TKraftSolver=class
      private

       fPhysics:TKraft;

       fIsland:TKraftIsland;

       fVelocities:TKraftSolverVelocities;
       fCountVelocities:longint;

       fPositions:TKraftSolverPositions;
       fCountPositions:longint;

       fLinearFactors:TKraftSolverLinearFactors;
       fCountLinearFactors:longint;

       fVelocityStates:TKraftSolverVelocityStates;
       fCountVelocityStates:longint;

       fPositionStates:TKraftSolverPositionStates;
       fCountPositionStates:longint;

       fSpeculativeContactStates:TKraftSolverSpeculativeContactStates;
       fCountSpeculativeContactStates:longint;

       fCountContacts:longint;

       fCountSpeculativeContacts:longint;

       fDeltaTime:TKraftScalar;

       fDeltaTimeRatio:TKraftScalar;

       fEnableFriction:boolean;

       fPositionCorrectionMode:TKraftPositionCorrectionMode;

      public

       constructor Create(const APhysics:TKraft;const AIsland:TKraftIsland);
       destructor Destroy; override;

       procedure Store;

       procedure Initialize(const TimeStep:TKraftTimeStep);

       procedure InitializeConstraints;

       procedure WarmStart;

       procedure SolveVelocityConstraints;

       function SolvePositionConstraints:boolean;

       function SolveTimeOfImpactConstraints(IndexA,IndexB:longint):boolean;

       procedure SolveSpeculativeContactConstraints;

       procedure StoreImpulses;

       property Physics:TKraft read fPhysics;

       property Island:TKraftIsland read fIsland;

       property Velocities:TKraftSolverVelocities read fVelocities;
       property CountVelocities:longint read fCountVelocities;

       property Positions:TKraftSolverPositions read fPositions;
       property CountPositions:longint read fCountPositions;

       property LinearFactors:TKraftSolverLinearFactors read fLinearFactors;
       property CountLinearFactors:longint read fCountLinearFactors;

       property VelocityStates:TKraftSolverVelocityStates read fVelocityStates;
       property CountVelocityStates:longint read fCountVelocityStates;

       property PositionStates:TKraftSolverPositionStates read fPositionStates;
       property CountPositionStates:longint read fCountPositionStates;

       property SpeculativeContactStates:TKraftSolverSpeculativeContactStates read fSpeculativeContactStates;
       property CountSpeculativeContactStates:longint read fCountSpeculativeContactStates;

       property CountContacts:longint read fCountContacts;

       property CountSpeculativeContacts:longint read fCountSpeculativeContacts;

       property DeltaTime:TKraftScalar read fDeltaTime;

       property DeltaTimeRatio:TKraftScalar read fDeltaTimeRatio;

       property EnableFriction:boolean read fEnableFriction;

       property PositionCorrectionMode:TKraftPositionCorrectionMode read fPositionCorrectionMode;

     end;

     TKraftIsland=class
      private

       fPhysics:TKraft;

       fIslandIndex:longint;

       fRigidBodies:TKraftRigidBodies;
       fCountRigidBodies:longint;

       fConstraints:TKraftConstraints;
       fCountConstraints:longint;

       fContactPairs:TPKraftContactPairs;
       fCountContactPairs:longint;

       fStaticContactPairs:TPKraftContactPairs;
       fCountStaticContactPairs:longint;

       fSpeculativeContactPairs:TPKraftContactPairs;
       fCountSpeculativeContactPairs:longint;

       fSolver:TKraftSolver;

      public

       constructor Create(const APhysics:TKraft;const AIndex:longint);
       destructor Destroy; override;
       procedure Clear;
       function AddRigidBody(RigidBody:TKraftRigidBody):longint;
       procedure AddConstraint(Constraint:TKraftConstraint);
       procedure AddContactPair(ContactPair:PKraftContactPair);
       procedure MergeContactPairs;
       procedure Solve(const TimeStep:TKraftTimeStep);
       procedure SolveTimeOfImpact(const TimeStep:TKraftTimeStep;const IndexA,IndexB:longint);

       property Physics:TKraft read fPhysics;

       property IslandIndex:longint read fIslandIndex;

       property RigidBodies:TKraftRigidBodies read fRigidBodies;
       property CountRigidBodies:longint read fCountRigidBodies;

       property Constraints:TKraftConstraints read fConstraints;
       property CountConstraints:longint read fCountConstraints;

       property ContactPairs:TPKraftContactPairs read fContactPairs;
       property CountContactPairs:longint read fCountContactPairs;

       property StaticContactPairs:TPKraftContactPairs read fStaticContactPairs;
       property CountStaticContactPairs:longint read fCountStaticContactPairs;

       property SpeculativeContactPairs:TPKraftContactPairs read fSpeculativeContactPairs;
       property CountSpeculativeContactPairs:longint read fCountSpeculativeContactPairs;

       property Solver:TKraftSolver read fSolver;

     end;

     TKraftIslands=array of TKraftIsland;

{$ifndef KraftPasMP}
     TKraftJobManager=class;

     TKraftJobManagerOnProcessJob=procedure(const JobIndex,ThreadIndex:longint) of object;

     TKraftJobThread=class(TThread)
      private
       fPhysics:TKraft;
       fJobManager:TKraftJobManager;
       fThreadNumber:longint;
       fEvent:TEvent;
       fDoneEvent:TEvent;
      protected
       procedure Execute; override;
      public
       constructor Create(const APhysics:TKraft;const AJobManager:TKraftJobManager;const AThreadNumber:longint);
       destructor Destroy; override;
       property Physics:TKraft read fPhysics;
       property JobManager:TKraftJobManager read fJobManager;
       property ThreadNumber:longint read fThreadNumber;
       property Event:TEvent read fEvent;
       property DoneEvent:TEvent read fDoneEvent;
     end;

     TKraftJobThreads=array of TKraftJobThread;

     TKraftJobManager=class
      private
       fPhysics:TKraft;
       fThreads:TKraftJobThreads;
       fCountThreads:longint;
       fCountAliveThreads:longint;
       fThreadsTerminated:boolean;
       fOnProcessJob:TKraftJobManagerOnProcessJob;
       fCountRemainJobs:longint;
       fGranularity:longint;
      public
       constructor Create(const APhysics:TKraft);
       destructor Destroy; override;
       procedure WakeUp;
       procedure WaitFor;
       procedure ProcessJobs;
       property Physics:TKraft read fPhysics;
       property Threads:TKraftJobThreads read fThreads;
       property CountThreads:longint read fCountThreads;
       property CountAliveThreads:longint read fCountAliveThreads write fCountAliveThreads;
       property ThreadsTerminated:boolean read fThreadsTerminated write fThreadsTerminated;
       property OnProcessJob:TKraftJobManagerOnProcessJob read fOnProcessJob write fOnProcessJob;
       property CountRemainJobs:longint read fCountRemainJobs write fCountRemainJobs;
       property Granularity:longint read fGranularity write fGranularity;
     end;
{$endif}

     TKraft=class(TPersistent)
      private

{$ifdef KraftPasMP}
       fPasMP:TPasMP;
{$endif}

       fSingleThreaded:boolean;

       fHighResolutionTimer:TKraftHighResolutionTimer;

       fBroadPhaseTime:int64;
       fMidPhaseTime:int64;
       fNarrowPhaseTime:int64;
       fSolverTime:int64;
       fContinuousTime:int64;
       fTotalTime:int64;

       fNewShapes:boolean;

       fConvexHullFirst:TKraftConvexHull;
       fConvexHullLast:TKraftConvexHull;

       fMeshFirst:TKraftMesh;
       fMeshLast:TKraftMesh;

       fConstraintFirst:TKraftConstraint;
       fConstraintLast:TKraftConstraint;

       fCountRigidBodies:longint;
       fRigidBodyIDCounter:uint64;

       fRigidBodyFirst:TKraftRigidBody;
       fRigidBodyLast:TKraftRigidBody;

       fStaticRigidBodyCount:longint;

       fStaticRigidBodyFirst:TKraftRigidBody;
       fStaticRigidBodyLast:TKraftRigidBody;

       fDynamicRigidBodyCount:longint;

       fDynamicRigidBodyFirst:TKraftRigidBody;
       fDynamicRigidBodyLast:TKraftRigidBody;

       fKinematicRigidBodyCount:longint;

       fKinematicRigidBodyFirst:TKraftRigidBody;
       fKinematicRigidBodyLast:TKraftRigidBody;

       fStaticAABBTree:TKraftDynamicAABBTree;
       fSleepingAABBTree:TKraftDynamicAABBTree;
       fDynamicAABBTree:TKraftDynamicAABBTree;
       fKinematicAABBTree:TKraftDynamicAABBTree;

       fIslands:TKraftIslands;
       fCountIslands:longint;

       fBroadPhase:TKraftBroadPhase;

       fContactManager:TKraftContactManager;

       fWorldFrequency:TKraftScalar;

       fWorldDeltaTime:TKraftScalar;

       fWorldInverseDeltaTime:TKraftScalar;

       fLastInverseDeltaTime:TKraftScalar;

       fAllowSleep:boolean;

       fAllowedPenetration:TKraftScalar;

       fGravity:TKraftVector3;

       fGravityProperty:TKraftVector3Property;

       fMaximalLinearVelocity:TKraftScalar;
       fLinearVelocityThreshold:TKraftScalar;

       fMaximalAngularVelocity:TKraftScalar;
       fAngularVelocityThreshold:TKraftScalar;

       fSleepTimeThreshold:TKraftScalar;

       fVelocityThreshold:TKraftScalar;

       fContactBaumgarte:TKraftScalar;

       fConstraintBaumgarte:TKraftScalar;

       fTimeOfImpactBaumgarte:TKraftScalar;

       fPenetrationSlop:TKraftScalar;

       fLinearSlop:TKraftScalar;

       fAngularSlop:TKraftScalar;

       fMaximalLinearCorrection:TKraftScalar;

       fMaximalAngularCorrection:TKraftScalar;

       fWarmStarting:boolean;

       fContinuousMode:TKraftContinuousMode;

       fContinuousAgainstDynamics:boolean;

       fTimeOfImpactAlgorithm:TKraftTimeOfImpactAlgorithm;

       fMaximalSubSteps:longint;

       fContactPositionCorrectionMode:TKraftPositionCorrectionMode;

       fConstraintPositionCorrectionMode:TKraftPositionCorrectionMode;

       fVelocityIterations:longint;

       fPositionIterations:longint;

       fSpeculativeIterations:longint;

       fTimeOfImpactIterations:longint;

       fPerturbationIterations:longint;

       fPersistentContactManifold:boolean;

       fAlwaysPerturbating:boolean;

       fEnableFriction:boolean;

       fLinearVelocityRK4Integration:boolean;

       fAngularVelocityRK4Integration:boolean;

       fContactBreakingThreshold:TKraftScalar;

       fCountThreads:longint;

{$ifndef KraftPasMP}
       fJobManager:TKraftJobManager;
{$endif}

       fIsSolving:boolean;
       fTriangleShapes:TKraftShapes;

       fJobTimeStep:TKraftTimeStep;

       procedure Integrate(var Position:TKraftVector3;var Orientation:TKraftQuaternion;const LinearVelocity,AngularVelocity:TKraftVector3;const DeltaTime:TKraftScalar);

       procedure BuildIslands;
{$ifdef KraftPasMP}
       procedure ProcessSolveIslandParallelForFunction(const Job:PPasMPJob;const ThreadIndex:longint;const Data:pointer;const FromIndex,ToIndex:TPasMPNativeInt);
{$else}
       procedure ProcessSolveIslandJob(const JobIndex,ThreadIndex:longint);
{$endif}
       procedure SolveIslands(const TimeStep:TKraftTimeStep);

       function GetConservativeAdvancementTimeOfImpact(const ShapeA:TKraftShape;const SweepA:TKraftSweep;const ShapeB:TKraftShape;const ShapeBTriangleIndex:longint;const SweepB:TKraftSweep;const TimeStep:TKraftTimeStep;const ThreadIndex:longint;var Beta:TKraftScalar):boolean;

       function GetBilateralAdvancementTimeOfImpact(const ShapeA:TKraftShape;const SweepA:TKraftSweep;const ShapeB:TKraftShape;const ShapeBTriangleIndex:longint;const SweepB:TKraftSweep;const TimeStep:TKraftTimeStep;const ThreadIndex:longint;var Beta:TKraftScalar):boolean;

       function GetTimeOfImpact(const ShapeA:TKraftShape;const SweepA:TKraftSweep;const ShapeB:TKraftShape;const ShapeBTriangleIndex:longint;const SweepB:TKraftSweep;const TimeStep:TKraftTimeStep;const ThreadIndex:longint;var Beta:TKraftScalar):boolean;

       procedure Solve(const TimeStep:TKraftTimeStep);

       procedure SolveContinuousTimeOfImpactSubSteps(const TimeStep:TKraftTimeStep);

       procedure SolveContinuousMotionClamping(const TimeStep:TKraftTimeStep);

      protected

       property IsSolving:boolean read fIsSolving;
       property TriangleShapes:TKraftShapes read fTriangleShapes;
       property JobTimeStep:TKraftTimeStep read fJobTimeStep;

      public

{$ifdef KraftPasMP}
       constructor Create(const APasMP:TPasMP);
{$else}
       constructor Create(const ACountThreads:longint=-1);
{$endif}
       destructor Destroy; override;

       procedure SetFrequency(const AFrequency:TKraftScalar);

       procedure StoreWorldTransforms;

       procedure InterpolateWorldTransforms(const Alpha:TKraftScalar);

       procedure Step(const ADeltaTime:TKraftScalar=0);

       function TestPoint(const Point:TKraftVector3):TKraftShape;

       function RayCast(const Origin,Direction:TKraftVector3;const MaxTime:TKraftScalar;var Shape:TKraftShape;var Time:TKraftScalar;var Point,Normal:TKraftVector3;const CollisionGroups:TKraftRigidBodyCollisionGroups=[low(TKraftRigidBodyCollisionGroup)..high(TKraftRigidBodyCollisionGroup)]):boolean;

       function PushSphere(var Center:TKraftVector3;const Radius:TKraftScalar;const CollisionGroups:TKraftRigidBodyCollisionGroups=[low(TKraftRigidBodyCollisionGroup)..high(TKraftRigidBodyCollisionGroup)];const TryIterations:longint=4):boolean;

       function GetDistance(const ShapeA,ShapeB:TKraftShape):TKraftScalar;

       property HighResolutionTimer:TKraftHighResolutionTimer read fHighResolutionTimer;

       property BroadPhaseTime:int64 read fBroadPhaseTime;
       property MidPhaseTime:int64 read fMidPhaseTime;
       property NarrowPhaseTime:int64 read fNarrowPhaseTime;
       property SolverTime:int64 read fSolverTime;
       property ContinuousTime:int64 read fContinuousTime;
       property TotalTime:int64 read fTotalTime;

       property NewShapes:boolean read fNewShapes;

       property ConvexHullFirst:TKraftConvexHull read fConvexHullFirst;
       property ConvexHullLast:TKraftConvexHull read fConvexHullLast;

       property MeshFirst:TKraftMesh read fMeshFirst;
       property MeshLast:TKraftMesh read fMeshLast;

       property ConstraintFirst:TKraftConstraint read fConstraintFirst;
       property ConstraintLast:TKraftConstraint read fConstraintLast;

       property CountRigidBodies:longint read fCountRigidBodies;
       property RigidBodyIDCounter:uint64 read fRigidBodyIDCounter;

       property RigidBodyFirst:TKraftRigidBody read fRigidBodyFirst;
       property RigidBodyLast:TKraftRigidBody read fRigidBodyLast;

       property StaticRigidBodyCount:longint read fStaticRigidBodyCount;

       property StaticRigidBodyFirst:TKraftRigidBody read fStaticRigidBodyFirst;
       property StaticRigidBodyLast:TKraftRigidBody read fStaticRigidBodyLast;

       property DynamicRigidBodyCount:longint read fDynamicRigidBodyCount;

       property DynamicRigidBodyFirst:TKraftRigidBody read fDynamicRigidBodyFirst;
       property DynamicRigidBodyLast:TKraftRigidBody read fDynamicRigidBodyLast;

       property KinematicRigidBodyCount:longint read fKinematicRigidBodyCount;

       property KinematicRigidBodyFirst:TKraftRigidBody read fKinematicRigidBodyFirst;
       property KinematicRigidBodyLast:TKraftRigidBody read fKinematicRigidBodyLast;

       property StaticAABBTree:TKraftDynamicAABBTree read fStaticAABBTree;
       property SleepingAABBTree:TKraftDynamicAABBTree read fSleepingAABBTree;
       property DynamicAABBTree:TKraftDynamicAABBTree read fDynamicAABBTree;
       property KinematicAABBTree:TKraftDynamicAABBTree read fKinematicAABBTree;

       property Islands:TKraftIslands read fIslands;
       property CountIslands:longint read fCountIslands;

       property BroadPhase:TKraftBroadPhase read fBroadPhase;

       property ContactManager:TKraftContactManager read fContactManager;

       property WorldDeltaTime:TKraftScalar read fWorldDeltaTime;

       property WorldInverseDeltaTime:TKraftScalar read fWorldInverseDeltaTime;

       property LastInverseDeltaTime:TKraftScalar read fLastInverseDeltaTime;

       property CountThreads:longint read fCountThreads;

{$ifndef KraftPasMP}
       property JobManager:TKraftJobManager read fJobManager;
{$endif}

      published

       property SingleThreaded:boolean read fSingleThreaded write fSingleThreaded;

       property WorldFrequency:TKraftScalar read fWorldFrequency write SetFrequency;

       property AllowSleep:boolean read fAllowSleep write fAllowSleep;

       property AllowedPenetration:TKraftScalar read fAllowedPenetration write fAllowedPenetration;

       property Gravity:TKraftVector3Property read fGravityProperty;

       property MaximalLinearVelocity:TKraftScalar read fMaximalLinearVelocity write fMaximalLinearVelocity;
       property LinearVelocityThreshold:TKraftScalar read fLinearVelocityThreshold write fLinearVelocityThreshold;

       property MaximalAngularVelocity:TKraftScalar read fMaximalAngularVelocity write fMaximalAngularVelocity;
       property AngularVelocityThreshold:TKraftScalar read fAngularVelocityThreshold write fAngularVelocityThreshold;

       property SleepTimeThreshold:TKraftScalar read fSleepTimeThreshold write fSleepTimeThreshold;

       property VelocityThreshold:TKraftScalar read fVelocityThreshold write fVelocityThreshold;

       property ContactBaumgarte:TKraftScalar read fContactBaumgarte write fContactBaumgarte;

       property ConstraintBaumgarte:TKraftScalar read fConstraintBaumgarte write fConstraintBaumgarte;

       property TimeOfImpactBaumgarte:TKraftScalar read fTimeOfImpactBaumgarte write fTimeOfImpactBaumgarte;

       property PenetrationSlop:TKraftScalar read fPenetrationSlop write fPenetrationSlop;

       property LinearSlop:TKraftScalar read fLinearSlop write fLinearSlop;

       property AngularSlop:TKraftScalar read fAngularSlop write fAngularSlop;

       property MaximalLinearCorrection:TKraftScalar read fMaximalLinearCorrection write fMaximalLinearCorrection;

       property MaximalAngularCorrection:TKraftScalar read fMaximalAngularCorrection write fMaximalAngularCorrection;

       property WarmStarting:boolean read fWarmStarting write fWarmStarting;

       property ContinuousMode:TKraftContinuousMode read fContinuousMode write fContinuousMode;

       property ContinuousAgainstDynamics:boolean read fContinuousAgainstDynamics write fContinuousAgainstDynamics;

       property TimeOfImpactAlgorithm:TKraftTimeOfImpactAlgorithm read fTimeOfImpactAlgorithm write fTimeOfImpactAlgorithm;

       property MaximalSubSteps:longint read fMaximalSubSteps write fMaximalSubSteps;

       property ContactPositionCorrectionMode:TKraftPositionCorrectionMode read fContactPositionCorrectionMode write fContactPositionCorrectionMode;

       property ConstraintPositionCorrectionMode:TKraftPositionCorrectionMode read fConstraintPositionCorrectionMode write fConstraintPositionCorrectionMode;

       property VelocityIterations:longint read fVelocityIterations write fVelocityIterations;

       property PositionIterations:longint read fPositionIterations write fPositionIterations;

       property SpeculativeIterations:longint read fSpeculativeIterations write fSpeculativeIterations;

       property TimeOfImpactIterations:longint read fTimeOfImpactIterations write fTimeOfImpactIterations;

       property PerturbationIterations:longint read fPerturbationIterations write fPerturbationIterations;

       property PersistentContactManifold:boolean read fPersistentContactManifold write fPersistentContactManifold;

       property AlwaysPerturbating:boolean read fAlwaysPerturbating write fAlwaysPerturbating;

       property EnableFriction:boolean read fEnableFriction write fEnableFriction;

       property LinearVelocityRK4Integration:boolean read fLinearVelocityRK4Integration write fLinearVelocityRK4Integration;

       property AngularVelocityRK4Integration:boolean read fAngularVelocityRK4Integration write fAngularVelocityRK4Integration;

       property ContactBreakingThreshold:TKraftScalar read fContactBreakingThreshold write fContactBreakingThreshold;

     end;

const Vector2Origin:TKraftVector2=(x:0.0;y:0.0);
      Vector2XAxis:TKraftVector2=(x:1.0;y:0.0);
      Vector2YAxis:TKraftVector2=(x:0.0;y:1.0);
      Vector2ZAxis:TKraftVector2=(x:0.0;y:0.0);

{$ifdef SIMD}
      Vector3Origin:TKraftVector3=(x:0.0;y:0.0;z:0.0;w:0.0);
      Vector3XAxis:TKraftVector3=(x:1.0;y:0.0;z:0.0;w:0.0);
      Vector3YAxis:TKraftVector3=(x:0.0;y:1.0;z:0.0;w:0.0);
      Vector3ZAxis:TKraftVector3=(x:0.0;y:0.0;z:1.0;w:0.0);
      Vector3All:TKraftVector3=(x:1.0;y:1.0;z:1.0;w:0.0);
{$else}
      Vector3Origin:TKraftVector3=(x:0.0;y:0.0;z:0.0);
      Vector3XAxis:TKraftVector3=(x:1.0;y:0.0;z:0.0);
      Vector3YAxis:TKraftVector3=(x:0.0;y:1.0;z:0.0);
      Vector3ZAxis:TKraftVector3=(x:0.0;y:0.0;z:1.0);
      Vector3All:TKraftVector3=(x:1.0;y:1.0;z:1.0);
{$endif}

      Vector4Origin:TKraftVector4=(x:0.0;y:0.0;z:0.0;w:1.0);
      Vector4XAxis:TKraftVector4=(x:1.0;y:0.0;z:0.0;w:1.0);
      Vector4YAxis:TKraftVector4=(x:0.0;y:1.0;z:0.0;w:1.0);
      Vector4ZAxis:TKraftVector4=(x:0.0;y:0.0;z:1.0;w:1.0);

      Matrix2x2Identity:TKraftMatrix2x2=((1.0,0.0),(0.0,1.0));
      Matrix2x2Null:TKraftMatrix2x2=((0.0,0.0),(0.0,0.0));

{$ifdef SIMD}
      Matrix3x3Identity:TKraftMatrix3x3=((1.0,0.0,0.0,0.0),(0.0,1.0,0.0,0.0),(0.0,0.0,1.0,0.0));
      Matrix3x3Null:TKraftMatrix3x3=((0.0,0.0,0.0,0.0),(0.0,0.0,0.0,0.0),(0.0,0.0,0.0,0.0));
{$else}
      Matrix3x3Identity:TKraftMatrix3x3=((1.0,0.0,0.0),(0.0,1.0,0.0),(0.0,0.0,1.0));
      Matrix3x3Null:TKraftMatrix3x3=((0.0,0.0,0.0),(0.0,0.0,0.0),(0.0,0.0,0.0));
{$endif}

      Matrix4x4Identity:TKraftMatrix4x4=((1.0,0.0,0,0.0),(0.0,1.0,0.0,0.0),(0.0,0.0,1.0,0.0),(0.0,0.0,0,1.0));
      Matrix4x4RightToLeftHanded:TKraftMatrix4x4=((1.0,0.0,0,0.0),(0.0,1.0,0.0,0.0),(0.0,0.0,-1.0,0.0),(0.0,0.0,0,1.0));
      Matrix4x4Flip:TKraftMatrix4x4=((0.0,0.0,-1.0,0.0),(-1.0,0.0,0,0.0),(0.0,1.0,0.0,0.0),(0.0,0.0,0,1.0));
      Matrix4x4InverseFlip:TKraftMatrix4x4=((0.0,-1.0,0.0,0.0),(0.0,0.0,1.0,0.0),(-1.0,0.0,0,0.0),(0.0,0.0,0,1.0));
      Matrix4x4FlipYZ:TKraftMatrix4x4=((1.0,0.0,0,0.0),(0.0,0.0,1.0,0.0),(0.0,-1.0,0.0,0.0),(0.0,0.0,0,1.0));
      Matrix4x4InverseFlipYZ:TKraftMatrix4x4=((1.0,0.0,0,0.0),(0.0,0.0,-1.0,0.0),(0.0,1.0,0.0,0.0),(0.0,0.0,0,1.0));
      Matrix4x4Null:TKraftMatrix4x4=((0.0,0.0,0,0.0),(0.0,0.0,0,0.0),(0.0,0.0,0,0.0),(0.0,0.0,0,0.0));
      Matrix4x4NormalizedSpace:TKraftMatrix4x4=((2.0,0.0,0,0.0),(0.0,2.0,0.0,0.0),(0.0,0.0,2.0,0.0),(-1.0,-1.0,-1.0,1.0));

      QuaternionIdentity:TKraftQuaternion=(x:0.0;y:0.0;z:0.0;w:1.0);

function Vector2(x,y:TKraftScalar):TKraftVector2; {$ifdef caninline}inline;{$endif}
function Vector3(x,y,z:TKraftScalar):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3(const v:TKraftVector4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Matrix3x3(const m:TKraftMatrix4x4):TKraftMatrix3x3; overload; {$ifdef caninline}inline;{$endif}
function Plane(Normal:TKraftVector3;Distance:TKraftScalar):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}
function Quaternion(w,x,y,z:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}

function Vector2Compare(const v1,v2:TKraftVector2):boolean; {$ifdef caninline}inline;{$endif}
function Vector2CompareEx(const v1,v2:TKraftVector2;const Threshold:TKraftScalar=EPSILON):boolean; {$ifdef caninline}inline;{$endif}
function Vector2Add(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
function Vector2Sub(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
function Vector2Avg(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
function Vector2ScalarMul(const v:TKraftVector2;s:TKraftScalar):TKraftVector2; {$ifdef caninline}inline;{$endif}
function Vector2Dot(const v1,v2:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector2Neg(const v:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
procedure Vector2Scale(var v:TKraftVector2;s:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
procedure Vector2Scale(var v:TKraftVector2;sx,sy:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
function Vector2Mul(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
function Vector2Length(const v:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector2Dist(const v1,v2:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector2LengthSquared(const v:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector2Angle(const v1,v2,v3:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
procedure Vector2Normalize(var v:TKraftVector2); {$ifdef caninline}inline;{$endif}
function Vector2Norm(const v:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
procedure Vector2Rotate(var v:TKraftVector2;a:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
procedure Vector2Rotate(var v:TKraftVector2;const Center:TKraftVector2;a:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
procedure Vector2MatrixMul(var v:TKraftVector2;const m:TKraftMatrix2x2); {$ifdef caninline}inline;{$endif}
function Vector2TermMatrixMul(const v:TKraftVector2;const m:TKraftMatrix2x2):TKraftVector2; {$ifdef caninline}inline;{$endif}
function Vector2Lerp(const v1,v2:TKraftVector2;w:TKraftScalar):TKraftVector2; {$ifdef caninline}inline;{$endif}

{$ifdef SIMD}
function Vector3Flip(const v:TKraftVector3):TKraftVector3;
function Vector3Abs(const v:TKraftVector3):TKraftVector3;
function Vector3Compare(const v1,v2:TKraftVector3):boolean;
function Vector3CompareEx(const v1,v2:TKraftVector3;const Threshold:TKraftScalar=EPSILON):boolean;
procedure Vector3DirectAdd(var v1:TKraftVector3;const v2:TKraftVector3);
procedure Vector3DirectSub(var v1:TKraftVector3;const v2:TKraftVector3);
function Vector3Add(const v1,v2:TKraftVector3):TKraftVector3;
function Vector3Sub(const v1,v2:TKraftVector3):TKraftVector3;
function Vector3Avg(const v1,v2:TKraftVector3):TKraftVector3; overload;
function Vector3Avg(const v1,v2,v3:TKraftVector3):TKraftVector3; overload;
function Vector3Avg(const va:PKraftVector3s;Count:longint):TKraftVector3; overload;
function Vector3ScalarMul(const v:TKraftVector3;const s:TKraftScalar):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3Dot(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3Cos(const v1,v2:TKraftVector3):TKraftScalar;
function Vector3GetOneUnitOrthogonalVector(const v:TKraftVector3):TKraftVector3;
function Vector3Cross(const v1,v2:TKraftVector3):TKraftVector3;
function Vector3Neg(const v:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
procedure Vector3Scale(var v:TKraftVector3;const sx,sy,sz:TKraftScalar); overload; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
procedure Vector3Scale(var v:TKraftVector3;const s:TKraftScalar); overload; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3Mul(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3Length(const v:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3Dist(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3LengthSquared(const v:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3DistSquared(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3Angle(const v1,v2,v3:TKraftVector3):TKraftScalar;
function Vector3LengthNormalize(var v:TKraftVector3):TKraftScalar;
procedure Vector3Normalize(var v:TKraftVector3);
procedure Vector3NormalizeEx(var v:TKraftVector3);
function Vector3SafeNorm(const v:TKraftVector3):TKraftVector3;
function Vector3Norm(const v:TKraftVector3):TKraftVector3;
function Vector3NormEx(const v:TKraftVector3):TKraftVector3; 
procedure Vector3RotateX(var v:TKraftVector3;a:TKraftScalar);
procedure Vector3RotateY(var v:TKraftVector3;a:TKraftScalar);
procedure Vector3RotateZ(var v:TKraftVector3;a:TKraftScalar);
procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix3x3); overload;
procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix4x4); overload; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
procedure Vector3MatrixMulBasis(var v:TKraftVector3;const m:TKraftMatrix4x4); overload;
procedure Vector3MatrixMulInverted(var v:TKraftVector3;const m:TKraftMatrix4x4); overload;
function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload;
function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3TermMatrixMulInverse(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload;
function Vector3TermMatrixMulInverted(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload;
function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
function Vector3TermMatrixMulTransposedBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
function Vector3TermMatrixMulBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
function Vector3TermMatrixMulHomogen(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3;
function Vector3Lerp(const v1,v2:TKraftVector3;w:TKraftScalar):TKraftVector3;
function Vector3Perpendicular(v:TKraftVector3):TKraftVector3;
function Vector3TermQuaternionRotate(const v:TKraftVector3;const q:TKraftQuaternion):TKraftVector3;
function Vector3ProjectToBounds(const v:TKraftVector3;const MinVector,MaxVector:TKraftVector3):TKraftScalar;
{$else}
function Vector3Flip(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Abs(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Compare(const v1,v2:TKraftVector3):boolean; {$ifdef caninline}inline;{$endif}
function Vector3CompareEx(const v1,v2:TKraftVector3;const Threshold:TKraftScalar=EPSILON):boolean; {$ifdef caninline}inline;{$endif}
function Vector3DirectAdd(var v1:TKraftVector3;const v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3DirectSub(var v1:TKraftVector3;const v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Add(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Sub(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Avg(const v1,v2:TKraftVector3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3Avg(const v1,v2,v3:TKraftVector3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3Avg(const va:PKraftVector3s;Count:longint):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3ScalarMul(const v:TKraftVector3;const s:TKraftScalar):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Dot(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector3Cos(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector3GetOneUnitOrthogonalVector(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Cross(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Neg(const v:TKraftVector3):TKraftVector3;  {$ifdef caninline}inline;{$endif}
procedure Vector3Scale(var v:TKraftVector3;const sx,sy,sz:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
procedure Vector3Scale(var v:TKraftVector3;const s:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
function Vector3Mul(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Length(const v:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector3Dist(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector3LengthSquared(const v:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector3DistSquared(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector3Angle(const v1,v2,v3:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Vector3LengthNormalize(var v:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
procedure Vector3Normalize(var v:TKraftVector3); {$ifdef caninline}inline;{$endif}
procedure Vector3NormalizeEx(var v:TKraftVector3); {$ifdef caninline}inline;{$endif}
function Vector3SafeNorm(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Norm(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3NormEx(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
procedure Vector3RotateX(var v:TKraftVector3;a:TKraftScalar); {$ifdef caninline}inline;{$endif}
procedure Vector3RotateY(var v:TKraftVector3;a:TKraftScalar); {$ifdef caninline}inline;{$endif}
procedure Vector3RotateZ(var v:TKraftVector3;a:TKraftScalar); {$ifdef caninline}inline;{$endif}
procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix3x3); overload; {$ifdef caninline}inline;{$endif}
procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix4x4); overload; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
procedure Vector3MatrixMulBasis(var v:TKraftVector3;const m:TKraftMatrix4x4); overload; {$ifdef caninline}inline;{$endif}
procedure Vector3MatrixMulInverted(var v:TKraftVector3;const m:TKraftMatrix4x4); overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef CPU386ASMForSinglePrecision}assembler;{$endif}
function Vector3TermMatrixMulInverse(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMulInverted(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMulTransposedBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMulBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
function Vector3TermMatrixMulHomogen(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Lerp(const v1,v2:TKraftVector3;w:TKraftScalar):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3Perpendicular(v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3TermQuaternionRotate(const v:TKraftVector3;const q:TKraftQuaternion):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Vector3ProjectToBounds(const v:TKraftVector3;const MinVector,MaxVector:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
{$endif}

function Vector4Compare(const v1,v2:TKraftVector4):boolean;
function Vector4CompareEx(const v1,v2:TKraftVector4;const Threshold:TKraftScalar=EPSILON):boolean;
function Vector4Add(const v1,v2:TKraftVector4):TKraftVector4;
function Vector4Sub(const v1,v2:TKraftVector4):TKraftVector4;
function Vector4ScalarMul(const v:TKraftVector4;s:TKraftScalar):TKraftVector4;
function Vector4Dot(const v1,v2:TKraftVector4):TKraftScalar;
function Vector4Cross(const v1,v2:TKraftVector4):TKraftVector4;
function Vector4Neg(const v:TKraftVector4):TKraftVector4;
procedure Vector4Scale(var v:TKraftVector4;sx,sy,sz:TKraftScalar); overload;
procedure Vector4Scale(var v:TKraftVector4;s:TKraftScalar); overload;
function Vector4Mul(const v1,v2:TKraftVector4):TKraftVector4;
function Vector4Length(const v:TKraftVector4):TKraftScalar;
function Vector4Dist(const v1,v2:TKraftVector4):TKraftScalar;
function Vector4LengthSquared(const v:TKraftVector4):TKraftScalar;
function Vector4DistSquared(const v1,v2:TKraftVector4):TKraftScalar;
function Vector4Angle(const v1,v2,v3:TKraftVector4):TKraftScalar;
procedure Vector4Normalize(var v:TKraftVector4);
function Vector4Norm(const v:TKraftVector4):TKraftVector4;
procedure Vector4RotateX(var v:TKraftVector4;a:TKraftScalar);
procedure Vector4RotateY(var v:TKraftVector4;a:TKraftScalar);
procedure Vector4RotateZ(var v:TKraftVector4;a:TKraftScalar);
procedure Vector4MatrixMul(var v:TKraftVector4;const m:TKraftMatrix4x4); {$ifdef CPU386ASMForSinglePrecision}register;{$endif}
function Vector4TermMatrixMul(const v:TKraftVector4;const m:TKraftMatrix4x4):TKraftVector4; {$ifdef CPU386ASMForSinglePrecision}register;{$endif}
function Vector4TermMatrixMulHomogen(const v:TKraftVector4;const m:TKraftMatrix4x4):TKraftVector4;
procedure Vector4Rotate(var v:TKraftVector4;const Axis:TKraftVector4;a:TKraftScalar);
function Vector4Lerp(const v1,v2:TKraftVector4;w:TKraftScalar):TKraftVector4;

function Matrix2x2Inverse(var mr:TKraftMatrix2x2;const ma:TKraftMatrix2x2):boolean;
function Matrix2x2TermInverse(const m:TKraftMatrix2x2):TKraftMatrix2x2;

function Matrix3x3RotateX(Angle:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
function Matrix3x3RotateY(Angle:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
function Matrix3x3RotateZ(Angle:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
function Matrix3x3Rotate(Angle:TKraftScalar;Axis:TKraftVector3):TKraftMatrix3x3; overload;
function Matrix3x3Scale(sx,sy,sz:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
procedure Matrix3x3Add(var m1:TKraftMatrix3x3;const m2:TKraftMatrix3x3); {$ifdef caninline}inline;{$endif}
procedure Matrix3x3Sub(var m1:TKraftMatrix3x3;const m2:TKraftMatrix3x3); {$ifdef caninline}inline;{$endif}
procedure Matrix3x3Mul(var m1:TKraftMatrix3x3;const m2:TKraftMatrix3x3);
function Matrix3x3TermAdd(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
function Matrix3x3TermSub(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
function Matrix3x3TermMul(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3;
function Matrix3x3TermMulTranspose(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3;
procedure Matrix3x3ScalarMul(var m:TKraftMatrix3x3;s:TKraftScalar); {$ifdef caninline}inline;{$endif}
function Matrix3x3TermScalarMul(const m:TKraftMatrix3x3;s:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
procedure Matrix3x3Transpose(var m:TKraftMatrix3x3); {$ifdef caninline}inline;{$endif}
function Matrix3x3TermTranspose(const m:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
function Matrix3x3Determinant(const m:TKraftMatrix3x3):TKraftScalar; {$ifdef caninline}inline;{$endif}
function Matrix3x3EulerAngles(const m:TKraftMatrix3x3):TKraftVector3;
procedure Matrix3x3SetColumn(var m:TKraftMatrix3x3;const c:longint;const v:TKraftVector3); {$ifdef caninline}inline;{$endif}
function Matrix3x3GetColumn(const m:TKraftMatrix3x3;const c:longint):TKraftVector3; {$ifdef caninline}inline;{$endif}
procedure Matrix3x3SetRow(var m:TKraftMatrix3x3;const r:longint;const v:TKraftVector3); {$ifdef caninline}inline;{$endif}
function Matrix3x3GetRow(const m:TKraftMatrix3x3;const r:longint):TKraftVector3; {$ifdef caninline}inline;{$endif}
function Matrix3x3Compare(const m1,m2:TKraftMatrix3x3):boolean;
function Matrix3x3Inverse(var mr:TKraftMatrix3x3;const ma:TKraftMatrix3x3):boolean;
function Matrix3x3TermInverse(const m:TKraftMatrix3x3):TKraftMatrix3x3;
procedure Matrix3x3OrthoNormalize(var m:TKraftMatrix3x3);
function Matrix3x3Slerp(const a,b:TKraftMatrix3x3;x:TKraftScalar):TKraftMatrix3x3;
function Matrix3x3FromToRotation(const FromDirection,ToDirection:TKraftVector3):TKraftMatrix3x3;
function Matrix3x3Construct(const Forwards,Up:TKraftVector3):TKraftMatrix3x3;
function Matrix3x3OuterProduct(const u,v:TKraftVector3):TKraftMatrix3x3;

function Matrix4x4Set(m:TKraftMatrix3x3):TKraftMatrix4x4;
function Matrix4x4Rotation(m:TKraftMatrix4x4):TKraftMatrix4x4;
function Matrix4x4RotateX(Angle:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4RotateY(Angle:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4RotateZ(Angle:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4Rotate(Angle:TKraftScalar;Axis:TKraftVector3):TKraftMatrix4x4; overload;
function Matrix4x4Translate(x,y,z:TKraftScalar):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
function Matrix4x4Translate(const v:TKraftVector3):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
function Matrix4x4Translate(const v:TKraftVector4):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
procedure Matrix4x4Translate(var m:TKraftMatrix4x4;const v:TKraftVector3); overload; {$ifdef caninline}inline;{$endif}
procedure Matrix4x4Translate(var m:TKraftMatrix4x4;const v:TKraftVector4); overload; {$ifdef caninline}inline;{$endif}
function Matrix4x4Scale(sx,sy,sz:TKraftScalar):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
function Matrix4x4Scale(const s:TKraftVector3):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
procedure Matrix4x4Add(var m1:TKraftMatrix4x4;const m2:TKraftMatrix4x4); {$ifdef caninline}inline;{$endif}
procedure Matrix4x4Sub(var m1:TKraftMatrix4x4;const m2:TKraftMatrix4x4); {$ifdef caninline}inline;{$endif}
procedure Matrix4x4Mul(var m1:TKraftMatrix4x4;const m2:TKraftMatrix4x4); overload; {$ifdef CPU386ASMForSinglePrecision}register;{$endif}
procedure Matrix4x4Mul(var mr:TKraftMatrix4x4;const m1,m2:TKraftMatrix4x4); overload; {$ifdef CPU386ASMForSinglePrecision}register;{$endif}
function Matrix4x4TermAdd(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
function Matrix4x4TermSub(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
function Matrix4x4TermMul(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef CPU386ASMForSinglePrecision}register;{$endif}
function Matrix4x4TermMulInverted(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
function Matrix4x4TermMulSimpleInverted(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
function Matrix4x4TermMulTranspose(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4;
function Matrix4x4Lerp(const a,b:TKraftMatrix4x4;x:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4Slerp(const a,b:TKraftMatrix4x4;x:TKraftScalar):TKraftMatrix4x4;
procedure Matrix4x4ScalarMul(var m:TKraftMatrix4x4;s:TKraftScalar); {$ifdef caninline}inline;{$endif}
procedure Matrix4x4Transpose(var m:TKraftMatrix4x4);
function Matrix4x4TermTranspose(const m:TKraftMatrix4x4):TKraftMatrix4x4;
function Matrix4x4Determinant(const m:TKraftMatrix4x4):TKraftScalar;
procedure Matrix4x4SetColumn(var m:TKraftMatrix4x4;const c:longint;const v:TKraftVector4); {$ifdef caninline}inline;{$endif}
function Matrix4x4GetColumn(const m:TKraftMatrix4x4;const c:longint):TKraftVector4; {$ifdef caninline}inline;{$endif}
procedure Matrix4x4SetRow(var m:TKraftMatrix4x4;const r:longint;const v:TKraftVector4); {$ifdef caninline}inline;{$endif}
function Matrix4x4GetRow(const m:TKraftMatrix4x4;const r:longint):TKraftVector4; {$ifdef caninline}inline;{$endif}
function Matrix4x4Compare(const m1,m2:TKraftMatrix4x4):boolean;
function Matrix4x4LengthSquared(const m:TKraftMatrix4x4):TKraftScalar;
function Matrix4x4Length(const m:TKraftMatrix4x4):TKraftScalar;
function Matrix4x4DifferenceSquared(const m1,m2:TKraftMatrix4x4):TKraftScalar;
function Matrix4x4Difference(const m1,m2:TKraftMatrix4x4):TKraftScalar;
procedure Matrix4x4Reflect(var mr:TKraftMatrix4x4;Plane:TKraftPlane);
function Matrix4x4TermReflect(Plane:TKraftPlane):TKraftMatrix4x4;
function Matrix4x4SimpleInverse(var mr:TKraftMatrix4x4;const ma:TKraftMatrix4x4):boolean; {$ifdef caninline}inline;{$endif}
function Matrix4x4TermSimpleInverse(const ma:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
function Matrix4x4Inverse(var mr:TKraftMatrix4x4;const ma:TKraftMatrix4x4):boolean;
function Matrix4x4TermInverse(const ma:TKraftMatrix4x4):TKraftMatrix4x4;
function Matrix4x4InverseOld(var mr:TKraftMatrix4x4;const ma:TKraftMatrix4x4):boolean;
function Matrix4x4TermInverseOld(const ma:TKraftMatrix4x4):TKraftMatrix4x4;
function Matrix4x4GetSubMatrix3x3(const m:TKraftMatrix4x4;i,j:longint):TKraftMatrix3x3;
function Matrix4x4Frustum(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4Ortho(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4OrthoLH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4OrthoRH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4OrthoOffCenterLH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4OrthoOffCenterRH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4Perspective(fovy,Aspect,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
function Matrix4x4LookAt(const Eye,Center,Up:TKraftVector3):TKraftMatrix4x4;
function Matrix4x4Fill(const Eye,RightVector,UpVector,ForwardVector:TKraftVector3):TKraftMatrix4x4;
function Matrix4x4ConstructX(const xAxis:TKraftVector3):TKraftMatrix4x4;
function Matrix4x4ConstructY(const yAxis:TKraftVector3):TKraftMatrix4x4;
function Matrix4x4ConstructZ(const zAxis:TKraftVector3):TKraftMatrix4x4;
function Matrix4x4ProjectionMatrixClip(const ProjectionMatrix:TKraftMatrix4x4;const ClipPlane:TKraftPlane):TKraftMatrix4x4;

procedure PlaneNormalize(var Plane:TKraftPlane); {$ifdef caninline}inline;{$endif}
function PlaneMatrixMul(const Plane:TKraftPlane;const Matrix:TKraftMatrix4x4):TKraftPlane;
function PlaneTransform(const Plane:TKraftPlane;const Matrix:TKraftMatrix4x4):TKraftPlane; overload;
function PlaneTransform(const Plane:TKraftPlane;const Matrix,NormalMatrix:TKraftMatrix4x4):TKraftPlane; overload;
function PlaneFastTransform(const Plane:TKraftPlane;const Matrix:TKraftMatrix4x4):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}
function PlaneVectorDistance(const Plane:TKraftPlane;const Point:TKraftVector3):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
function PlaneVectorDistance(const Plane:TKraftPlane;const Point:TKraftVector4):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
function PlaneFromPoints(const p1,p2,p3:TKraftVector3):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}
function PlaneFromPoints(const p1,p2,p3:TKraftVector4):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}

function QuaternionNormal(const AQuaternion:TKraftQuaternion):TKraftScalar;
function QuaternionLengthSquared(const AQuaternion:TKraftQuaternion):TKraftScalar;
procedure QuaternionNormalize(var AQuaternion:TKraftQuaternion);
function QuaternionTermNormalize(const AQuaternion:TKraftQuaternion):TKraftQuaternion;
function QuaternionNeg(const AQuaternion:TKraftQuaternion):TKraftQuaternion;
function QuaternionConjugate(const AQuaternion:TKraftQuaternion):TKraftQuaternion;
function QuaternionInverse(const AQuaternion:TKraftQuaternion):TKraftQuaternion;
function QuaternionAdd(const q1,q2:TKraftQuaternion):TKraftQuaternion;
function QuaternionSub(const q1,q2:TKraftQuaternion):TKraftQuaternion;
function QuaternionScalarMul(const q:TKraftQuaternion;const s:TKraftScalar):TKraftQuaternion;
function QuaternionMul(const q1,q2:TKraftQuaternion):TKraftQuaternion;
function QuaternionRotateAroundAxis(const q1,q2:TKraftQuaternion):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
function QuaternionFromAxisAngle(const Axis:TKraftVector3;Angle:TKraftScalar):TKraftQuaternion; overload; {$ifdef caninline}inline;{$endif}
function QuaternionFromSpherical(const Latitude,Longitude:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
procedure QuaternionToSpherical(const q:TKraftQuaternion;var Latitude,Longitude:TKraftScalar);
function QuaternionFromAngles(const Pitch,Yaw,Roll:TKraftScalar):TKraftQuaternion; overload; {$ifdef caninline}inline;{$endif}
function QuaternionFromAngles(const Angles:TKraftAngles):TKraftQuaternion; overload; {$ifdef caninline}inline;{$endif}
function QuaternionFromMatrix3x3(const AMatrix:TKraftMatrix3x3):TKraftQuaternion;
function QuaternionToMatrix3x3(AQuaternion:TKraftQuaternion):TKraftMatrix3x3;
function QuaternionFromTangentSpaceMatrix3x3(AMatrix:TKraftMatrix3x3):TKraftQuaternion;
function QuaternionToTangentSpaceMatrix3x3(AQuaternion:TKraftQuaternion):TKraftMatrix3x3;
function QuaternionFromMatrix4x4(const AMatrix:TKraftMatrix4x4):TKraftQuaternion;
function QuaternionToMatrix4x4(AQuaternion:TKraftQuaternion):TKraftMatrix4x4;
function QuaternionToEuler(const AQuaternion:TKraftQuaternion):TKraftVector3; {$ifdef caninline}inline;{$endif}
procedure QuaternionToAxisAngle(AQuaternion:TKraftQuaternion;var Axis:TKraftVector3;var Angle:TKraftScalar); {$ifdef caninline}inline;{$endif}
function QuaternionGenerator(AQuaternion:TKraftQuaternion):TKraftVector3; {$ifdef caninline}inline;{$endif}
function QuaternionLerp(const q1,q2:TKraftQuaternion;const t:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
function QuaternionNlerp(const q1,q2:TKraftQuaternion;const t:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
function QuaternionSlerp(const q1,q2:TKraftQuaternion;const t:TKraftScalar):TKraftQuaternion;
function QuaternionIntegrate(const q:TKraftQuaternion;const Omega:TKraftVector3;const DeltaTime:TKraftScalar):TKraftQuaternion;
function QuaternionSpin(const q:TKraftQuaternion;const Omega:TKraftVector3;const DeltaTime:TKraftScalar):TKraftQuaternion; overload;
procedure QuaternionDirectSpin(var q:TKraftQuaternion;const Omega:TKraftVector3;const DeltaTime:TKraftScalar); overload;
function QuaternionFromToRotation(const FromDirection,ToDirection:TKraftVector3):TKraftQuaternion; {$ifdef caninline}inline;{$endif}

function InertiaTensorTransform(const Inertia,Transform:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
function InertiaTensorParallelAxisTheorem(const Center:TKraftVector3;const Mass:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}

implementation

const daabbtNULLNODE=-1;

      AABB_EXTENSION=0.1;

      AABB_MULTIPLIER=2.0;

      AABB_MAX_EXPANSION=128.0;

      AABBExtensionVector:TKraftVector3=(x:AABB_EXTENSION;y:AABB_EXTENSION;z:AABB_EXTENSION);

      pi2=pi*2.0;

{$ifdef cpu386}
      {%H-}MMXExt:boolean=false;
      {%H-}SSEExt:boolean=false;
      {%H-}SSE2Ext:boolean=false;
      {%H-}SSE3Ext:boolean=false;
{$endif}

{$ifdef fpc}
 {$undef OldDelphi}
{$else}
 {$ifdef conditionalexpressions}
  {$if CompilerVersion>=23.0}
   {$undef OldDelphi}
type qword=uint64;
     ptruint=NativeUInt;
     ptrint=NativeInt;
  {$else}
   {$define OldDelphi}
  {$ifend}
 {$else}
  {$define OldDelphi}
 {$endif}
{$endif}
{$ifdef OldDelphi}
type qword=int64;
{$ifdef cpu64}
     ptruint=qword;
     ptrint=int64;
{$else}
     ptruint=longword;
     ptrint=longint;
{$endif}
{$endif}

type TUInt128=packed record
{$ifdef BIG_ENDIAN}
      case byte of
       0:(
        Hi,Lo:qword;
       );
       1:(
        Q3,Q2,Q1,Q0:longword;
       );
{$else}
      case byte of
       0:(
        Lo,Hi:qword;
       );
       1:(
        Q0,Q1,Q2,Q3:longword;
       );
{$endif}
     end;

function AddWithCarry(const a,b:longword;var Carry:longword):longword; {$ifdef caninline}inline;{$endif}
var r:qword;
begin
 r:=qword(a)+qword(b)+qword(Carry);
 Carry:=(r shr 32) and 1;
 result:=r and $ffffffff;
end;

function MultiplyWithCarry(const a,b:longword;var Carry:longword):longword; {$ifdef caninline}inline;{$endif}
var r:qword;
begin
 r:=(qword(a)*qword(b))+qword(Carry);
 Carry:=r shr 32;
 result:=r and $ffffffff;
end;

function DivideWithRemainder(const a,b:longword;var Remainder:longword):longword; {$ifdef caninline}inline;{$endif}
var r:qword;
begin
 r:=(qword(Remainder) shl 32) or a;
 Remainder:=r mod b;
 result:=r div b;
end;

procedure UInt64ToUInt128(var Dest:TUInt128;const x:qword); {$ifdef caninline}inline;{$endif}
begin
 Dest.Hi:=0;
 Dest.Lo:=x;
end;

procedure UInt128Add(var Dest:TUInt128;const x,y:TUInt128); {$ifdef caninline}inline;{$endif}
var a,b,c,d:qword;
begin
 a:=x.Hi shr 32;
 b:=x.Hi and $ffffffff;
 c:=x.Lo shr 32;
 d:=x.Lo and $ffffffff;
 inc(d,y.Lo and $ffffffff);
 inc(c,(y.Lo shr 32)+(d shr 32));
 inc(b,(y.Hi and $ffffffff)+(c shr 32));
 inc(a,(y.Hi shr 32)+(b shr 32));
 Dest.Hi:=((a and $ffffffff) shl 32) or (b and $ffffffff);
 Dest.Lo:=((c and $ffffffff) shl 32) or (d and $ffffffff);
end;

procedure UInt128Mul(var Dest:TUInt128;const x,y:TUInt128); {$ifdef caninline}inline;{$endif}
var c,xw,yw,dw:array[0..15] of longword;
    i,j,k:longint;
    v:longword;
begin
 for i:=0 to 15 do begin
  c[i]:=0;
 end;
 xw[7]:=(x.Lo shr 0) and $ffff;
 xw[6]:=(x.Lo shr 16) and $ffff;
 xw[5]:=(x.Lo shr 32) and $ffff;
 xw[4]:=(x.Lo shr 48) and $ffff;
 xw[3]:=(x.Hi shr 0) and $ffff;
 xw[2]:=(x.Hi shr 16) and $ffff;
 xw[1]:=(x.Hi shr 32) and $ffff;
 xw[0]:=(x.Hi shr 48) and $ffff;
 yw[7]:=(y.Lo shr 0) and $ffff;
 yw[6]:=(y.Lo shr 16) and $ffff;
 yw[5]:=(y.Lo shr 32) and $ffff;
 yw[4]:=(y.Lo shr 48) and $ffff;
 yw[3]:=(y.Hi shr 0) and $ffff;
 yw[2]:=(y.Hi shr 16) and $ffff;
 yw[1]:=(y.Hi shr 32) and $ffff;
 yw[0]:=(y.Hi shr 48) and $ffff;
 for i:=0 to 7 do begin
  for j:=0 to 7 do begin
   v:=xw[i]*yw[j];
   k:=i+j;
   inc(c[k],v shr 16);
   inc(c[k+1],v and $ffff);
  end;
 end;
 for i:=15 downto 1 do begin
  inc(c[i-1],c[i] shr 16);
  c[i]:=c[i] and $ffff;
 end;
 for i:=0 to 7 do begin
  dw[i]:=c[8+i];
 end;
 Dest.Hi:=(qword(dw[0] and $ffff) shl 48) or (qword(dw[1] and $ffff) shl 32) or (qword(dw[2] and $ffff) shl 16) or (qword(dw[3] and $ffff) shl 0);
 Dest.Lo:=(qword(dw[4] and $ffff) shl 48) or (qword(dw[5] and $ffff) shl 32) or (qword(dw[6] and $ffff) shl 16) or (qword(dw[7] and $ffff) shl 0);
end;

procedure UInt128Div64(var Dest:TUInt128;const Dividend:TUInt128;Divisor:qword); {$ifdef caninline}inline;{$endif}
var Quotient:TUInt128;
    Remainder:qword;
    Bit:longint;
begin
 Quotient:=Dividend;
 Remainder:=0;
 for Bit:=1 to 128 do begin
  Remainder:=(Remainder shl 1) or (ord((Quotient.Hi and $8000000000000000)<>0) and 1);
  Quotient.Hi:=(Quotient.Hi shl 1) or (Quotient.Lo shr 63);
  Quotient.Lo:=Quotient.Lo shl 1;
  if (longword(Remainder shr 32)>longword(Divisor shr 32)) or
     ((longword(Remainder shr 32)=longword(Divisor shr 32)) and (longword(Remainder and $ffffffff)>=longword(Divisor and $ffffffff))) then begin
   dec(Remainder,Divisor);
   Quotient.Lo:=Quotient.Lo or 1;
  end;
 end;
 Dest:=Quotient;
end;

procedure UInt128Mul64(var Dest:TUInt128;u,v:qword); {$ifdef caninline}inline;{$endif}
var u0,u1,v0,v1,k,t,w0,w1,w2:qword;
begin
 u1:=u shr 32;
 u0:=u and qword($ffffffff);
 v1:=v shr 32;
 v0:=v and qword($ffffffff);
 t:=u0*v0;
 w0:=t and qword($ffffffff);
 k:=t shr 32;
 t:=(u1*v0)+k;
 w1:=t and qword($ffffffff);
 w2:=t shr 32;
 t:=(u0*v1)+w1;
 k:=t shr 32;
 Dest.Lo:=(t shl 32)+w0;
 Dest.Hi:=((u1*v1)+w2)+k;
end;

function RoundUpToPowerOfTwo(x:longword):longword; {$ifdef caninline}inline;{$endif}
begin
 dec(x);
 x:=x or (x shr 1);
 x:=x or (x shr 2);
 x:=x or (x shr 4);
 x:=x or (x shr 8);
 x:=x or (x shr 16);
 result:=x+1;
end;

function SIMDGetFlags:longword; {$ifdef cpu386}assembler;
asm
 stmxcsr dword ptr result
end;
{$else}
begin
 result:=0;
end;
{$endif}

procedure SIMDSetFlags(const Flags:longword); {$ifdef cpu386}register; assembler;
var Temp:longword;
asm
 mov dword ptr Temp,eax
 ldmxcsr dword ptr Temp
end;
{$else}
begin
end;
{$endif}

procedure SIMDSetOurFlags;
{$ifdef cpu386}
// Flush to Zero=Bit 15
// Underflow exception mask=Bit 11
// Denormals are zeros=Bit 6
// Denormal exception mask=Bit 8
// $8840(ftz+uem+daz+dem) and $8940(ftz+uem+daz)
const DenormalsAreZero=1 shl 6;
      InvalidOperationExceptionMask=1 shl 7;
      DenormalExceptionMask=1 shl 8;
      DivodeByZeroExceptionMask=1 shl 9;
      OverflowExceptionMask=1 shl 10;
      UnderflowExceptionMask=1 shl 11;
      PrecisionMask=1 shl 12;
      FlushToZero=1 shl 15;
      SIMDFlags=InvalidOperationExceptionMask or DenormalExceptionMask or DivodeByZeroExceptionMask or OverflowExceptionMask or UnderflowExceptionMask or PrecisionMask or FlushToZero or DenormalsAreZero;
      RoundToNearest=longword(longword($ffffffff) and not ((1 shl 13) or (1 shl 14)));
var SIMDCtrl:longword;
begin
 if SSEExt then begin
  asm
   push eax
   stmxcsr dword ptr SIMDCtrl
   mov eax,dword ptr SIMDCtrl
   or eax,SIMDFlags
   and eax,RoundToNearest
   mov dword ptr SIMDCtrl,eax
   ldmxcsr dword ptr SIMDCtrl
   pop eax
  end;
 end;
end;
{$else}
begin
end;
{$endif}

procedure CheckCPU;
{$ifdef cpu386}
var Features,FeaturesExt:longword;
{$endif}
begin
{$ifdef cpu386}
 Features:=0;
 FeaturesExt:=0;
 asm
  pushad

  // Check for CPUID opcode
  pushfd
  pop eax
  mov edx,eax
  xor eax,$200000
  push eax
  popfd
  pushfd
  pop eax
  xor eax,edx
  jz @NoCPUID
   // Get cpu features per CPUID opcode
   mov eax,1
   cpuid
   mov dword ptr FeaturesExt,ecx
   mov dword ptr Features,edx
  @NoCPUID:
  popad
 end;
 MMXExt:=(Features and $00800000)<>0;
 SSEExt:=(Features and $02000000)<>0;
 SSE2Ext:=(Features and $04000000)<>0;
 SSE3Ext:=(FeaturesExt and $00000001)<>0;
{$else}
{MMXExt:=false;
 SSEExt:=false;
 SSE2Ext:=false;
 SSE3Ext:=false;{}
{$endif}
end;

function Vector2(x,y:TKraftScalar):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=x;
 result.y:=y;
end;

function Vector3(x,y,z:TKraftScalar):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
begin
 result.x:=x;
 result.y:=y;
 result.z:=z;
end;

function Vector3(const v:TKraftVector4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v.x;
 result.y:=v.y;
 result.z:=v.z;
end;

function Matrix3x3(const m:TKraftMatrix4x4):TKraftMatrix3x3; overload; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=m[0,0];
 result[0,1]:=m[0,1];
 result[0,2]:=m[0,2];
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=m[1,0];
 result[1,1]:=m[1,1];
 result[1,2]:=m[1,2];
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=m[2,0];
 result[2,1]:=m[2,1];
 result[2,2]:=m[2,2];
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function Plane(Normal:TKraftVector3;Distance:TKraftScalar):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}
begin
 result.Normal:=Normal;
 result.Distance:=Distance;
end;

function Quaternion(w,x,y,z:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
begin
 result.w:=w;
 result.x:=x;
 result.y:=y;
 result.z:=z;
end;

function Vector2Compare(const v1,v2:TKraftVector2):boolean; {$ifdef caninline}inline;{$endif}
begin
 result:=(abs(v1.x-v2.x)<EPSILON) and (abs(v1.y-v2.y)<EPSILON);
end;

function Vector2CompareEx(const v1,v2:TKraftVector2;const Threshold:TKraftScalar=EPSILON):boolean; {$ifdef caninline}inline;{$endif}
begin
 result:=(abs(v1.x-v2.x)<Threshold) and (abs(v1.y-v2.y)<Threshold);
end;

function Vector2Add(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v1.x+v2.x;
 result.y:=v1.y+v2.y;
end;

function Vector2Sub(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v1.x-v2.x;
 result.y:=v1.y-v2.y;
end;

function Vector2Avg(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(v1.x+v2.x)*0.5;
 result.y:=(v1.y+v2.y)*0.5;
end;

function Vector2ScalarMul(const v:TKraftVector2;s:TKraftScalar):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v.x*s;
 result.y:=v.y*s;
end;

function Vector2Dot(const v1,v2:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=(v1.x*v2.x)+(v1.y*v2.y);
end;

function Vector2Neg(const v:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=-v.x;
 result.y:=-v.y;
end;

procedure Vector2Scale(var v:TKraftVector2;sx,sy:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
begin
 v.x:=v.x*sx;
 v.y:=v.y*sy;
end;

procedure Vector2Scale(var v:TKraftVector2;s:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
begin
 v.x:=v.x*s;
 v.y:=v.y*s;
end;

function Vector2Mul(const v1,v2:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v1.x*v2.x;
 result.y:=v1.y*v2.y;
end;

function Vector2Length(const v:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=sqrt(sqr(v.x)+sqr(v.y));
end;

function Vector2Dist(const v1,v2:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=Vector2Length(Vector2Sub(v2,v1));
end;

function Vector2LengthSquared(const v:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=sqr(v.x)+sqr(v.y);
end;

function Vector2Angle(const v1,v2,v3:TKraftVector2):TKraftScalar; {$ifdef caninline}inline;{$endif}
var A1,A2:TKraftVector2;
    L1,L2:TKraftScalar;
begin
 A1:=Vector2Sub(v1,v2);
 A2:=Vector2Sub(v3,v2);
 L1:=Vector2Length(A1);
 L2:=Vector2Length(A2);
 if (L1=0) or (L2=0) then begin
  result:=0;
 end else begin
  result:=ArcCos(Vector2Dot(A1,A2)/(L1*L2));
 end;
end;

procedure Vector2Normalize(var v:TKraftVector2); {$ifdef caninline}inline;{$endif}
var L:TKraftScalar;
begin
 L:=Vector2Length(v);
 if L<>0.0 then begin
  Vector2Scale(v,1/L);
 end else begin
  v:=Vector2Origin;
 end;
end;

function Vector2Norm(const v:TKraftVector2):TKraftVector2; {$ifdef caninline}inline;{$endif}
var L:TKraftScalar;
begin
 L:=Vector2Length(v);
 if L<>0.0 then begin
  result:=Vector2ScalarMul(v,1/L);
 end else begin
  result:=Vector2Origin;
 end;
end;

procedure Vector2Rotate(var v:TKraftVector2;a:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
var r:TKraftVector2;
begin
 r.x:=(v.x*cos(a))-(v.y*sin(a));
 r.y:=(v.y*cos(a))+(v.x*sin(a));
 v:=r;
end;

procedure Vector2Rotate(var v:TKraftVector2;const Center:TKraftVector2;a:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
var V0,r:TKraftVector2;
begin
 V0:=Vector2Sub(v,Center);
 r.x:=(V0.x*cos(a))-(V0.y*sin(a));
 r.y:=(V0.y*cos(a))+(V0.x*sin(a));
 v:=Vector2Add(r,Center);
end;

procedure Vector2MatrixMul(var v:TKraftVector2;const m:TKraftMatrix2x2); {$ifdef caninline}inline;{$endif}
var t:TKraftVector2;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y);
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y);
 v:=t;
end;

function Vector2TermMatrixMul(const v:TKraftVector2;const m:TKraftMatrix2x2):TKraftVector2; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y);
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y);
end;

function Vector2Lerp(const v1,v2:TKraftVector2;w:TKraftScalar):TKraftVector2; {$ifdef caninline}inline;{$endif}
var iw:TKraftScalar;
begin
 if w<0.0 then begin
  result:=v1;
 end else if w>1.0 then begin
  result:=v2;
 end else begin
  iw:=1.0-w;
  result.x:=(iw*v1.x)+(w*v2.x);
  result.y:=(iw*v1.y)+(w*v2.y);
 end;
end;

{$ifdef SIMD}      
function Vector3Flip(const v:TKraftVector3):TKraftVector3;
begin
 result.x:=v.x;
 result.y:=v.z;
 result.z:=-v.y;
end;

const Vector3Mask:array[0..3] of longword=($ffffffff,$ffffffff,$ffffffff,$00000000);

function Vector3Abs(const v:TKraftVector3):TKraftVector3;
begin
 result.x:=abs(v.x);
 result.y:=abs(v.y);
 result.z:=abs(v.z);
end;

function Vector3Compare(const v1,v2:TKraftVector3):boolean;
begin
 result:=(abs(v1.x-v2.x)<EPSILON) and (abs(v1.y-v2.y)<EPSILON) and (abs(v1.z-v2.z)<EPSILON);
end;

function Vector3CompareEx(const v1,v2:TKraftVector3;const Threshold:TKraftScalar=EPSILON):boolean;
begin
 result:=(abs(v1.x-v2.x)<Threshold) and (abs(v1.y-v2.y)<Threshold) and (abs(v1.z-v2.z)<Threshold);
end;

procedure Vector3DirectAdd(var v1:TKraftVector3;const v2:TKraftVector3); {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 addps xmm0,xmm1
 movups dqword ptr [v1],xmm0
end;
{$else}
begin
 v1.x:=v1.x+v2.x;
 v1.y:=v1.y+v2.y;
 v1.z:=v1.z+v2.z;
end;
{$endif}

procedure Vector3DirectSub(var v1:TKraftVector3;const v2:TKraftVector3); {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 subps xmm0,xmm1
 movups dqword ptr [v1],xmm0
end;
{$else}
begin
 v1.x:=v1.x-v2.x;
 v1.y:=v1.y-v2.y;
 v1.z:=v1.z-v2.z;
end;
{$endif}

function Vector3Add(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 addps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=v1.x+v2.x;
 result.y:=v1.y+v2.y;
 result.z:=v1.z+v2.z;
end;
{$endif}

function Vector3Sub(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 subps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=v1.x-v2.x;
 result.y:=v1.y-v2.y;
 result.z:=v1.z-v2.z;
end;
{$endif}

function Vector3Avg(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
const Half:TKraftVector3=(x:0.5;y:0.5;z:0.5;w:0.0);
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 movups xmm2,dqword ptr [Half]
 addps xmm0,xmm1
 mulps xmm0,xmm2
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=(v1.x+v2.x)*0.5;
 result.y:=(v1.y+v2.y)*0.5;
 result.z:=(v1.z+v2.z)*0.5;
end;
{$endif}

function Vector3Avg(const v1,v2,v3:TKraftVector3):TKraftVector3;
begin
 result.x:=(v1.x+v2.x+v3.x)/3.0;
 result.y:=(v1.y+v2.y+v3.y)/3.0;
 result.z:=(v1.z+v2.z+v3.z)/3.0;
end;

function Vector3Avg(const va:PKraftVector3s;Count:longint):TKraftVector3;
var i:longint;
begin
 result.x:=0.0;
 result.y:=0.0;
 result.z:=0.0;
 if Count>0 then begin
  for i:=0 to Count-1 do begin
   result.x:=result.x+va^[i].x;
   result.y:=result.y+va^[i].y;
   result.z:=result.z+va^[i].z;
  end;
  result.x:=result.x/Count;
  result.y:=result.y/Count;
  result.z:=result.z/Count;
 end;
end;

function Vector3ScalarMul(const v:TKraftVector3;const s:TKraftScalar):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 movss xmm1,dword ptr [s]
 shufps xmm1,xmm1,$00
 mulps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=v.x*s;
 result.y:=v.y*s;
 result.z:=v.z*s;
end;
{$endif}

function Vector3Dot(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 mulps xmm0,xmm1         // xmm0 = ?, z1*z2, y1*y2, x1*x2
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z1*z2
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z1*z2 + x1*x2
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y1*y2
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z1*z2 + y1*y2 + x1*x2
 movss dword ptr [result],xmm1
end;
{$else}
begin
 result:=(v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z);
end;
{$endif}

function Vector3Cos(const v1,v2:TKraftVector3):TKraftScalar;
var d:extended;
begin
 d:=sqrt(Vector3LengthSquared(v1)*Vector3LengthSquared(v2));
 if d<>0.0 then begin
  result:=((v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z))/d; //result:=Vector3Dot(v1,v2)/d;
 end else begin
  result:=0.9;
 end
end;

function Vector3GetOneUnitOrthogonalVector(const v:TKraftVector3):TKraftVector3;
var MinimumAxis:longint;
    l:TKraftScalar;
begin
 if abs(v.x)<abs(v.y) then begin
  if abs(v.x)<abs(v.z) then begin
   MinimumAxis:=0;
  end else begin
   MinimumAxis:=2;
  end;
 end else begin
  if abs(v.y)<abs(v.z) then begin
   MinimumAxis:=1;
  end else begin
   MinimumAxis:=2;
  end;
 end;
 case MinimumAxis of
  0:begin
   l:=sqrt(sqr(v.y)+sqr(v.z));
   result.x:=0.0;
   result.y:=-(v.z/l);
   result.z:=v.y/l;
  end;
  1:begin
   l:=sqrt(sqr(v.x)+sqr(v.z));
   result.x:=-(v.z/l);
   result.y:=0.0;
   result.z:=v.x/l;
  end;
  else begin
   l:=sqrt(sqr(v.x)+sqr(v.y));
   result.x:=-(v.y/l);
   result.y:=v.x/l;
   result.z:=0.0;
  end;
 end;
 result.w:=0.0;
end;

function Vector3Cross(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
{$ifdef SSEVector3CrossOtherVariant}
 movups xmm0,dqword ptr [v1]
 movups xmm2,dqword ptr [v2]
 movaps xmm1,xmm0
 movaps xmm3,xmm2
 shufps xmm0,xmm0,$c9
 shufps xmm1,xmm1,$d2
 shufps xmm2,xmm2,$d2
 shufps xmm3,xmm3,$c9
 mulps xmm0,xmm2
 mulps xmm1,xmm3
 subps xmm0,xmm1
 movups dqword ptr [result],xmm0
{$else}
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 movaps xmm2,xmm0
 movaps xmm3,xmm1
 shufps xmm0,xmm0,$12
 shufps xmm1,xmm1,$09
 shufps xmm2,xmm2,$09
 shufps xmm3,xmm3,$12
 mulps xmm0,xmm1
 mulps xmm2,xmm3
 subps xmm2,xmm0
 movups dqword ptr [result],xmm2
{$endif}
end;
{$else}
begin
 result.x:=(v1.y*v2.z)-(v1.z*v2.y);
 result.y:=(v1.z*v2.x)-(v1.x*v2.z);
 result.z:=(v1.x*v2.y)-(v1.y*v2.x);
end;
{$endif}

function Vector3Neg(const v:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 xorps xmm0,xmm0
 movups xmm1,dqword ptr [v]
 subps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=-v.x;
 result.y:=-v.y;
 result.z:=-v.z;
end;
{$endif}

procedure Vector3Scale(var v:TKraftVector3;const sx,sy,sz:TKraftScalar); overload; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movss xmm0,dword ptr [v+0]
 movss xmm1,dword ptr [v+4]
 movss xmm2,dword ptr [v+8]
 mulss xmm0,dword ptr [sx]
 mulss xmm1,dword ptr [sy]
 mulss xmm2,dword ptr [sz]
 movss dword ptr [v+0],xmm0
 movss dword ptr [v+4],xmm1
 movss dword ptr [v+8],xmm2
end;
{$else}
begin
 v.x:=v.x*sx;
 v.y:=v.y*sy;
 v.z:=v.z*sz;
end;
{$endif}

procedure Vector3Scale(var v:TKraftVector3;const s:TKraftScalar); overload; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 movss xmm1,dword ptr [s]
 shufps xmm1,xmm1,$00
 mulps xmm0,xmm1
 movups dqword ptr [v],xmm0
end;
{$else}
begin
 v.x:=v.x*s;
 v.y:=v.y*s;
 v.z:=v.z*s;
end;
{$endif}

function Vector3Mul(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 mulps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=v1.x*v2.x;
 result.y:=v1.y*v2.y;
 result.z:=v1.z*v2.z;
end;
{$endif}

function Vector3Length(const v:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 sqrtss xmm0,xmm1
 movss dword ptr [result],xmm0
end;
{$else}
begin
 result:=sqrt(sqr(v.x)+sqr(v.y)+sqr(v.z));
end;
{$endif}

function Vector3Dist(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 subps xmm0,xmm1
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 sqrtss xmm0,xmm1
 movss dword ptr [result],xmm0
end;
{$else}
begin
 result:=sqrt(sqr(v2.x-v1.x)+sqr(v2.y-v1.y)+sqr(v2.z-v1.z));
end;
{$endif}

function Vector3LengthSquared(const v:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 movss dword ptr [result],xmm1
end;
{$else}
begin
 result:=sqr(v.x)+sqr(v.y)+sqr(v.z);
end;
{$endif}

function Vector3DistSquared(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v1]
 movups xmm1,dqword ptr [v2]
 subps xmm0,xmm1
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 movss dword ptr [result],xmm1
end;
{$else}
begin
 result:=sqr(v2.x-v1.x)+sqr(v2.y-v1.y)+sqr(v2.z-v1.z);
end;
{$endif}

function Vector3Angle(const v1,v2,v3:TKraftVector3):TKraftScalar;
var A1,A2:TKraftVector3;
    L1,L2:TKraftScalar;
begin
 A1:=Vector3Sub(v1,v2);
 A2:=Vector3Sub(v3,v2);
 L1:=Vector3Length(A1);
 L2:=Vector3Length(A2);
 if (L1=0) or (L2=0) then begin
  result:=0;
 end else begin
  result:=ArcCos(Vector3Dot(A1,A2)/(L1*L2));
 end;
end;

function Vector3LengthNormalize(var v:TKraftVector3):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 movaps xmm1,xmm0
 subps xmm1,xmm0
 cmpps xmm1,xmm0,7
 andps xmm0,xmm1
 movaps xmm2,xmm0
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 sqrtss xmm0,xmm1
 movss dword ptr [result],xmm0
 shufps xmm0,xmm0,$00
 divps xmm2,xmm0
 movaps xmm1,xmm2
 subps xmm1,xmm2
 cmpps xmm1,xmm2,7
 andps xmm2,xmm1
 movups dqword ptr [v],xmm2
end;
{$else}
var l:TKraftScalar;
begin
 result:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if result>0.0 then begin
  result:=sqrt(result);
  l:=1.0/result;
  v.x:=v.x*l;
  v.y:=v.y*l;
  v.z:=v.z*l;
 end else begin
  result:=0.0;
  v.x:=0.0;
  v.y:=0.0;
  v.z:=0.0;
 end;
end;
{$endif}

procedure Vector3Normalize(var v:TKraftVector3); {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 movaps xmm2,xmm0
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 rsqrtss xmm0,xmm1
 shufps xmm0,xmm0,$00
 mulps xmm2,xmm0
 movaps xmm1,xmm2
 subps xmm1,xmm2
 cmpps xmm1,xmm2,7
 andps xmm2,xmm1
 movups dqword ptr [v],xmm2
end;
{$else}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=1.0/sqrt(l);
  v.x:=v.x*l;
  v.y:=v.y*l;
  v.z:=v.z*l;
 end else begin
  v.x:=0.0;
  v.y:=0.0;
  v.z:=0.0;
 end;
end;
{$endif}

procedure Vector3NormalizeEx(var v:TKraftVector3); {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 movaps xmm2,xmm0
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 sqrtss xmm0,xmm1
 shufps xmm0,xmm0,$00
 divps xmm2,xmm0
 movaps xmm1,xmm2
 subps xmm1,xmm2
 cmpps xmm1,xmm2,7
 andps xmm2,xmm1
 movups dqword ptr [v],xmm2
end;
{$else}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=sqrt(l);
  v.x:=v.x/l;
  v.y:=v.y/l;
  v.z:=v.z/l;
 end else begin
  v.x:=0.0;
  v.y:=0.0;
  v.z:=0.0;
 end;
end;
{$endif}

function Vector3SafeNorm(const v:TKraftVector3):TKraftVector3;
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=1.0/sqrt(l);
  result.x:=v.x*l;
  result.y:=v.y*l;
  result.z:=v.z*l;
 end else begin
  result.x:=1.0;
  result.y:=0.0;
  result.z:=0.0;
 end;
end;

function Vector3Norm(const v:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 movaps xmm2,xmm0
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 rsqrtss xmm0,xmm1
 shufps xmm0,xmm0,$00
 mulps xmm2,xmm0
 movaps xmm1,xmm2
 subps xmm1,xmm2
 cmpps xmm1,xmm2,7
 andps xmm2,xmm1
 movups dqword ptr [result],xmm2
end;
{$else}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=1.0/sqrt(l);
  result.x:=v.x*l;
  result.y:=v.y*l;
  result.z:=v.z*l;
 end else begin
  result.x:=0.0;
  result.y:=0.0;
  result.z:=0.0;
 end;
end;
{$endif}

function Vector3NormEx(const v:TKraftVector3):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [v]
 movaps xmm2,xmm0
 mulps xmm0,xmm0         // xmm0 = ?, z*z, y*y, x*x
 movhlps xmm1,xmm0       // xmm1 = ?, ?, ?, z*z
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + x*x
 shufps xmm0,xmm0,$55    // xmm0 = ?, ?, ?, y*y
 addss xmm1,xmm0         // xmm1 = ?, ?, ?, z*z + y*y + x*x
 sqrtss xmm0,xmm1
 shufps xmm0,xmm0,$00
 divps xmm2,xmm0
 movaps xmm1,xmm2
 subps xmm1,xmm2
 cmpps xmm1,xmm2,7
 andps xmm2,xmm1
 movups dqword ptr [result],xmm2
end;
{$else}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=sqrt(l);
  result.x:=v.x/l;
  result.y:=v.y/l;
  result.z:=v.z/l;
 end else begin
  result.x:=0.0;
  result.y:=0.0;
  result.z:=0.0;
 end;
end;
{$endif}

procedure Vector3RotateX(var v:TKraftVector3;a:TKraftScalar);
var t:TKraftVector3;
begin
 t.x:=v.x;
 t.y:=(v.y*cos(a))-(v.z*sin(a));
 t.z:=(v.y*sin(a))+(v.z*cos(a));
 v:=t;
end;

procedure Vector3RotateY(var v:TKraftVector3;a:TKraftScalar);
var t:TKraftVector3;
begin
 t.x:=(v.x*cos(a))+(v.z*sin(a));
 t.y:=v.y;
 t.z:=(v.z*cos(a))-(v.x*sin(a));
 v:=t;
end;

procedure Vector3RotateZ(var v:TKraftVector3;a:TKraftScalar);
var t:TKraftVector3;
begin
 t.x:=(v.x*cos(a))-(v.y*sin(a));
 t.y:=(v.x*sin(a))+(v.y*cos(a));
 t.z:=v.z;
 v:=t;
end;

procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix3x3); overload;
var t:TKraftVector3;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 t.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
 v:=t;
end;

procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix4x4); overload; {$ifdef CPU386ASMForSinglePrecision}assembler;
const cOne:array[0..3] of TKraftScalar=(0.0,0.0,0.0,1.0);
asm
 movups xmm0,dqword ptr [v]     // d c b a
 movups xmm1,dqword ptr [Vector3Mask]
 movups xmm2,dqword ptr [cOne]
 andps xmm0,xmm1
 addps xmm0,xmm2
 movaps xmm1,xmm0               // d c b a
 movaps xmm2,xmm0               // d c b a
 movaps xmm3,xmm0               // d c b a
 shufps xmm0,xmm0,$00           // a a a a 00000000b
 shufps xmm1,xmm1,$55           // b b b b 01010101b
 shufps xmm2,xmm2,$aa           // c c c c 10101010b
 shufps xmm3,xmm3,$ff           // d d d d 11111111b
 movups xmm4,dqword ptr [m+0]
 movups xmm5,dqword ptr [m+16]
 movups xmm6,dqword ptr [m+32]
 movups xmm7,dqword ptr [m+48]
 mulps xmm0,xmm4
 mulps xmm1,xmm5
 mulps xmm2,xmm6
 mulps xmm3,xmm7
 addps xmm0,xmm1
 addps xmm2,xmm3
 addps xmm0,xmm2
 movups dqword ptr [v],xmm0
end;
{$else}
var t:TKraftVector3;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+m[3,0];
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+m[3,1];
 t.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+m[3,2];
 v:=t;
end;
{$endif}

procedure Vector3MatrixMulBasis(var v:TKraftVector3;const m:TKraftMatrix4x4); overload;
var t:TKraftVector3;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 t.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
 v:=t;
end;

procedure Vector3MatrixMulInverted(var v:TKraftVector3;const m:TKraftMatrix4x4); overload;
var p,t:TKraftVector3;
begin
 p.x:=v.x-m[3,0];
 p.y:=v.y-m[3,1];
 p.z:=v.z-m[3,2];
 t.x:=(m[0,0]*p.x)+(m[0,1]*p.y)+(m[0,2]*p.z);
 t.y:=(m[1,0]*p.x)+(m[1,1]*p.y)+(m[1,2]*p.z);
 t.z:=(m[2,0]*p.x)+(m[2,1]*p.y)+(m[2,2]*p.z);
 v:=t;
end;

(*
function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef CPU386ASMForSinglePrecision}assembler;
const Mask:array[0..3] of longword=($ffffffff,$ffffffff,$ffffffff,$00000000);
asm
 movups xmm6,dqword ptr [Mask]
 movups xmm0,dqword ptr [v]     // d c b a
 movaps xmm1,xmm0               // d c b a
 movaps xmm2,xmm0               // d c b a
 shufps xmm0,xmm0,$00           // a a a a 00000000b
 shufps xmm1,xmm1,$55           // b b b b 01010101b
 shufps xmm2,xmm2,$aa           // c c c c 10101010b
 movups xmm3,dqword ptr [m+0]
 movups xmm4,dqword ptr [m+12]
 andps xmm3,xmm6
 andps xmm4,xmm6
 movss xmm5,dword ptr [m+24]
 movss xmm6,dword ptr [m+28]
 movlhps xmm5,xmm6
 movss xmm6,dword ptr [m+32]
 shufps xmm5,xmm6,$88
 mulps xmm0,xmm3
 mulps xmm1,xmm4
 mulps xmm2,xmm5
 addps xmm0,xmm1
 addps xmm0,xmm2
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
end;
{$endif}
(**)

function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef CPU386ASMForSinglePrecision}assembler;
const cOne:array[0..3] of TKraftScalar=(0.0,0.0,0.0,1.0);
asm
 movups xmm0,dqword ptr [v]     // d c b a
 movaps xmm1,xmm0               // d c b a
 movaps xmm2,xmm0               // d c b a
 shufps xmm0,xmm0,$00           // a a a a 00000000b
 shufps xmm1,xmm1,$55           // b b b b 01010101b
 shufps xmm2,xmm2,$aa           // c c c c 10101010b
 movups xmm3,dqword ptr [m+0]
 movups xmm4,dqword ptr [m+16]
 movups xmm5,dqword ptr [m+32]
 mulps xmm0,xmm3
 mulps xmm1,xmm4
 mulps xmm2,xmm5
 addps xmm0,xmm1
 addps xmm0,xmm2
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
end;
{$endif}

function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef CPU386ASMForSinglePrecision}assembler;
const cOne:array[0..3] of TKraftScalar=(0.0,0.0,0.0,1.0);
asm
 movups xmm0,dqword ptr [v]     // d c b a
 movups xmm1,dqword ptr [Vector3Mask]
 movups xmm2,dqword ptr [cOne]
 andps xmm0,xmm1
 addps xmm0,xmm2
 movaps xmm1,xmm0               // d c b a
 movaps xmm2,xmm0               // d c b a
 movaps xmm3,xmm0               // d c b a
 shufps xmm0,xmm0,$00           // a a a a 00000000b
 shufps xmm1,xmm1,$55           // b b b b 01010101b
 shufps xmm2,xmm2,$aa           // c c c c 10101010b
 shufps xmm3,xmm3,$ff           // d d d d 11111111b
 movups xmm4,dqword ptr [m+0]
 movups xmm5,dqword ptr [m+16]
 movups xmm6,dqword ptr [m+32]
 movups xmm7,dqword ptr [m+48]
 mulps xmm0,xmm4
 mulps xmm1,xmm5
 mulps xmm2,xmm6
 mulps xmm3,xmm7
 addps xmm0,xmm1
 addps xmm2,xmm3
 addps xmm0,xmm2
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+m[3,0];
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+m[3,1];
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+m[3,2];
end;
{$endif}

function Vector3TermMatrixMulInverse(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload;
var Determinant:TKraftScalar;
begin
 Determinant:=((m[0,0]*((m[1,1]*m[2,2])-(m[2,1]*m[1,2])))-
               (m[0,1]*((m[1,0]*m[2,2])-(m[2,0]*m[1,2]))))+
               (m[0,2]*((m[1,0]*m[2,1])-(m[2,0]*m[1,1])));
 if Determinant<>0.0 then begin
  Determinant:=1.0/Determinant;
 end;
 result.x:=((v.x*((m[1,1]*m[2,2])-(m[1,2]*m[2,1])))+(v.y*((m[1,2]*m[2,0])-(m[1,0]*m[2,2])))+(v.z*((m[1,0]*m[2,1])-(m[1,1]*m[2,0]))))*Determinant;
 result.y:=((m[0,0]*((v.y*m[2,2])-(v.z*m[2,1])))+(m[0,1]*((v.z*m[2,0])-(v.x*m[2,2])))+(m[0,2]*((v.x*m[2,1])-(v.y*m[2,0]))))*Determinant;
 result.z:=((m[0,0]*((m[1,1]*v.z)-(m[1,2]*v.y)))+(m[0,1]*((m[1,2]*v.x)-(m[1,0]*v.z)))+(m[0,2]*((m[1,0]*v.y)-(m[1,1]*v.x))))*Determinant;
end;

function Vector3TermMatrixMulInverted(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
var p:TKraftVector3;
begin
 p.x:=v.x-m[3,0];
 p.y:=v.y-m[3,1];
 p.z:=v.z-m[3,2];
 result.x:=(m[0,0]*p.x)+(m[0,1]*p.y)+(m[0,2]*p.z);
 result.y:=(m[1,0]*p.x)+(m[1,1]*p.y)+(m[1,2]*p.z);
 result.z:=(m[2,0]*p.x)+(m[2,1]*p.y)+(m[2,2]*p.z);
end;

function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload;
begin
 result.x:=(m[0,0]*v.x)+(m[0,1]*v.y)+(m[0,2]*v.z);
 result.y:=(m[1,0]*v.x)+(m[1,1]*v.y)+(m[1,2]*v.z);
 result.z:=(m[2,0]*v.x)+(m[2,1]*v.y)+(m[2,2]*v.z);
end;

function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
begin
 result.x:=(m[0,0]*v.x)+(m[0,1]*v.y)+(m[0,2]*v.z)+m[0,3];
 result.y:=(m[1,0]*v.x)+(m[1,1]*v.y)+(m[1,2]*v.z)+m[1,3];
 result.z:=(m[2,0]*v.x)+(m[2,1]*v.y)+(m[2,2]*v.z)+m[2,3];
end;

function Vector3TermMatrixMulTransposedBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
begin
 result.x:=(m[0,0]*v.x)+(m[0,1]*v.y)+(m[0,2]*v.z);
 result.y:=(m[1,0]*v.x)+(m[1,1]*v.y)+(m[1,2]*v.z);
 result.z:=(m[2,0]*v.x)+(m[2,1]*v.y)+(m[2,2]*v.z);
end;

function Vector3TermMatrixMulHomogen(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3;
var result_w:TKraftScalar;
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+m[3,0];
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+m[3,1];
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+m[3,2];
 result_w:=(m[0,3]*v.x)+(m[1,3]*v.y)+(m[2,3]*v.z)+m[3,3];
 result.x:=result.x/result_w;
 result.y:=result.y/result_w;
 result.z:=result.z/result_w;
end;

function Vector3TermMatrixMulBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef CPU386ASMForSinglePrecision}assembler;
const Mask:array[0..3] of longword=($ffffffff,$ffffffff,$ffffffff,$00000000);
asm
 movups xmm0,dqword ptr [v]     // d c b a
 movaps xmm1,xmm0               // d c b a
 movaps xmm2,xmm0               // d c b a
 shufps xmm0,xmm0,$00           // a a a a 00000000b
 shufps xmm1,xmm1,$55           // b b b b 01010101b
 shufps xmm2,xmm2,$aa           // c c c c 10101010b
 movups xmm3,dqword ptr [m+0]
 movups xmm4,dqword ptr [m+16]
 movups xmm5,dqword ptr [m+32]
 movups xmm6,dqword ptr [Mask]
 andps xmm3,xmm6
 andps xmm4,xmm6
 andps xmm5,xmm6
 mulps xmm0,xmm3
 mulps xmm1,xmm4
 mulps xmm2,xmm5
 addps xmm0,xmm1
 addps xmm0,xmm2
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
end;
{$endif}

function Vector3Lerp(const v1,v2:TKraftVector3;w:TKraftScalar):TKraftVector3;
var iw:TKraftScalar;
begin
 if w<0.0 then begin
  result:=v1;
 end else if w>1.0 then begin
  result:=v2;
 end else begin
  iw:=1.0-w;
  result.x:=(iw*v1.x)+(w*v2.x);
  result.y:=(iw*v1.y)+(w*v2.y);
  result.z:=(iw*v1.z)+(w*v2.z);
 end;
end;

function Vector3Perpendicular(v:TKraftVector3):TKraftVector3;
var p:TKraftVector3;
begin
 Vector3NormalizeEx(v);
 p.x:=abs(v.x);
 p.y:=abs(v.y);
 p.z:=abs(v.z);
 if (p.x<=p.y) and (p.x<=p.z) then begin
  p:=Vector3XAxis;
 end else if (p.y<=p.x) and (p.y<=p.z) then begin
  p:=Vector3YAxis;
 end else begin
  p:=Vector3ZAxis;
 end;
 result:=Vector3NormEx(Vector3Sub(p,Vector3ScalarMul(v,Vector3Dot(v,p))));
end;

function Vector3TermQuaternionRotate(const v:TKraftVector3;const q:TKraftQuaternion):TKraftVector3; {$ifdef CPU386ASMForSinglePrecision}assembler;
const Mask:array[0..3] of longword=($ffffffff,$ffffffff,$ffffffff,$00000000);
var t,qv:TKraftVector3;
asm

 movups xmm4,dqword ptr [q] // xmm4 = q.xyzw

 movups xmm5,dqword ptr [v] // xmm5 = v.xyz?

 movaps xmm6,xmm4
 shufps xmm6,xmm6,$ff // xmm6 = q.wwww

 movups xmm7,dqword ptr [Mask] // xmm7 = Mask

 andps xmm4,xmm7 // xmm4 = q.xyz0

 andps xmm5,xmm7 // xmm5 = v.xyz0

 // t:=Vector3ScalarMul(Vector3Cross(qv,v),2.0);
 movaps xmm0,xmm4 // xmm4 = qv
 movaps xmm1,xmm5 // xmm5 = v
 movaps xmm2,xmm4 // xmm4 = qv
 movaps xmm3,xmm5 // xmm5 = v
 shufps xmm0,xmm0,$12
 shufps xmm1,xmm1,$09
 shufps xmm2,xmm2,$09
 shufps xmm3,xmm3,$12
 mulps xmm0,xmm1
 mulps xmm2,xmm3
 subps xmm2,xmm0
 addps xmm2,xmm2

 // xmm6 = Vector3Add(v,Vector3ScalarMul(t,q.w))
 mulps xmm6,xmm2 // xmm6 = q.wwww, xmm2 = t
 addps xmm6,xmm5 // xmm5 = v

 // Vector3Cross(qv,t)
 movaps xmm1,xmm4 // xmm4 = qv
 movaps xmm3,xmm2 // xmm2 = t
 shufps xmm4,xmm4,$12
 shufps xmm2,xmm2,$09
 shufps xmm1,xmm1,$09
 shufps xmm3,xmm3,$12
 mulps xmm4,xmm2
 mulps xmm1,xmm3
 subps xmm1,xmm4

 // result:=Vector3Add(Vector3Add(v,Vector3ScalarMul(t,q.w)),Vector3Cross(qv,t));
 addps xmm1,xmm6

 movups dqword ptr [result],xmm1

end;
{$else}
var t,qv:TKraftVector3;
begin
 // t = 2 * cross(q.xyz, v)
 // v' = v + q.w * t + cross(q.xyz, t)
 qv.x:=q.x;
 qv.y:=q.y;
 qv.z:=q.z;
 qv.w:=0.0;
 t:=Vector3ScalarMul(Vector3Cross(qv,v),2.0);
 result:=Vector3Add(Vector3Add(v,Vector3ScalarMul(t,q.w)),Vector3Cross(qv,t));
end;
{$endif}

function Vector3ProjectToBounds(const v:TKraftVector3;const MinVector,MaxVector:TKraftVector3):TKraftScalar;
begin
 if v.x<0.0 then begin
  result:=v.x*MaxVector.x;
 end else begin
  result:=v.x*MinVector.x;
 end;
 if v.y<0.0 then begin
  result:=result+(v.y*MaxVector.y);
 end else begin
  result:=result+(v.y*MinVector.y);
 end;
 if v.z<0.0 then begin
  result:=result+(v.z*MaxVector.z);
 end else begin
  result:=result+(v.z*MinVector.z);
 end;
end;
{$else}
function Vector3Flip(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v.x;
 result.y:=v.z;
 result.z:=-v.y;
end;

function Vector3Abs(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=abs(v.x);
 result.y:=abs(v.y);
 result.z:=abs(v.z);
end;

function Vector3Compare(const v1,v2:TKraftVector3):boolean; {$ifdef caninline}inline;{$endif}
begin
 result:=(abs(v1.x-v2.x)<EPSILON) and (abs(v1.y-v2.y)<EPSILON) and (abs(v1.z-v2.z)<EPSILON);
end;

function Vector3CompareEx(const v1,v2:TKraftVector3;const Threshold:TKraftScalar=EPSILON):boolean; {$ifdef caninline}inline;{$endif}
begin
 result:=(abs(v1.x-v2.x)<Threshold) and (abs(v1.y-v2.y)<Threshold) and (abs(v1.z-v2.z)<Threshold);
end;

function Vector3DirectAdd(var v1:TKraftVector3;const v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 v1.x:=v1.x+v2.x;
 v1.y:=v1.y+v2.y;
 v1.z:=v1.z+v2.z;
end;

function Vector3DirectSub(var v1:TKraftVector3;const v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 v1.x:=v1.x-v2.x;
 v1.y:=v1.y-v2.y;
 v1.z:=v1.z-v2.z;
end;

function Vector3Add(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v1.x+v2.x;
 result.y:=v1.y+v2.y;
 result.z:=v1.z+v2.z;
end;

function Vector3Sub(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v1.x-v2.x;
 result.y:=v1.y-v2.y;
 result.z:=v1.z-v2.z;
end;

function Vector3Avg(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(v1.x+v2.x)*0.5;
 result.y:=(v1.y+v2.y)*0.5;
 result.z:=(v1.z+v2.z)*0.5;
end;

function Vector3Avg(const v1,v2,v3:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(v1.x+v2.x+v3.x)/3.0;
 result.y:=(v1.y+v2.y+v3.y)/3.0;
 result.z:=(v1.z+v2.z+v3.z)/3.0;
end;

function Vector3Avg(const va:PKraftVector3s;Count:longint):TKraftVector3; {$ifdef caninline}inline;{$endif}
var i:longint;
begin
 result.x:=0.0;
 result.y:=0.0;
 result.z:=0.0;
 if Count>0 then begin
  for i:=0 to Count-1 do begin
   result.x:=result.x+va^[i].x;
   result.y:=result.y+va^[i].y;
   result.z:=result.z+va^[i].z;
  end;
  result.x:=result.x/Count;
  result.y:=result.y/Count;
  result.z:=result.z/Count;
 end;
end;

function Vector3ScalarMul(const v:TKraftVector3;const s:TKraftScalar):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v.x*s;
 result.y:=v.y*s;
 result.z:=v.z*s;
end;

function Vector3Dot(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=(v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z);
end;

function Vector3Cos(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
var d:extended;
begin
 d:=SQRT(Vector3LengthSquared(v1)*Vector3LengthSquared(v2));
 if d<>0.0 then begin
  result:=((v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z))/d; //result:=Vector3Dot(v1,v2)/d;
 end else begin
  result:=0.0;
 end
end;

function Vector3GetOneUnitOrthogonalVector(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
var MinimumAxis:longint;
    l:TKraftScalar;
begin
 if abs(v.x)<abs(v.y) then begin
  if abs(v.x)<abs(v.z) then begin
   MinimumAxis:=0;
  end else begin
   MinimumAxis:=2;
  end;
 end else begin
  if abs(v.y)<abs(v.z) then begin
   MinimumAxis:=1;
  end else begin
   MinimumAxis:=2;
  end;
 end;
 case MinimumAxis of
  0:begin
   l:=sqrt(sqr(v.y)+sqr(v.z));
   result.x:=0.0;
   result.y:=-(v.z/l);
   result.z:=v.y/l;
  end;
  1:begin
   l:=sqrt(sqr(v.x)+sqr(v.z));
   result.x:=-(v.z/l);
   result.y:=0.0;
   result.z:=v.x/l;
  end;
  else begin
   l:=sqrt(sqr(v.x)+sqr(v.y));
   result.x:=-(v.y/l);
   result.y:=v.x/l;
   result.z:=0.0;
  end;
 end;
end;

function Vector3Cross(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(v1.y*v2.z)-(v1.z*v2.y);
 result.y:=(v1.z*v2.x)-(v1.x*v2.z);
 result.z:=(v1.x*v2.y)-(v1.y*v2.x);
end;

function Vector3Neg(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=-v.x;
 result.y:=-v.y;
 result.z:=-v.z;
end;

procedure Vector3Scale(var v:TKraftVector3;const sx,sy,sz:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
begin
 v.x:=v.x*sx;
 v.y:=v.y*sy;
 v.z:=v.z*sz;
end;

procedure Vector3Scale(var v:TKraftVector3;const s:TKraftScalar); overload; {$ifdef caninline}inline;{$endif}
begin
 v.x:=v.x*s;
 v.y:=v.y*s;
 v.z:=v.z*s;
end;

function Vector3Mul(const v1,v2:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=v1.x*v2.x;
 result.y:=v1.y*v2.y;
 result.z:=v1.z*v2.z;
end;

function Vector3Length(const v:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=sqrt(sqr(v.x)+sqr(v.y)+sqr(v.z));
end;

function Vector3Dist(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=sqrt(sqr(v2.x-v1.x)+sqr(v2.y-v1.y)+sqr(v2.z-v1.z));
end;

function Vector3LengthSquared(const v:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=sqr(v.x)+sqr(v.y)+sqr(v.z);
end;

function Vector3DistSquared(const v1,v2:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=sqr(v2.x-v1.x)+sqr(v2.y-v1.y)+sqr(v2.z-v1.z);
end;

function Vector3Angle(const v1,v2,v3:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
var A1,A2:TKraftVector3;
    L1,L2:TKraftScalar;
begin
 A1:=Vector3Sub(v1,v2);
 A2:=Vector3Sub(v3,v2);
 L1:=Vector3Length(A1);
 L2:=Vector3Length(A2);
 if (L1=0) or (L2=0) then begin
  result:=0;
 end else begin
  result:=ArcCos(Vector3Dot(A1,A2)/(L1*L2));
 end;
end;

function Vector3LengthNormalize(var v:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
var l:TKraftScalar;
begin
 result:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if result>0.0 then begin
  result:=sqrt(result);
  l:=1.0/result;
  v.x:=v.x*l;
  v.y:=v.y*l;
  v.z:=v.z*l;
 end else begin
  result:=0.0;
  v.x:=0.0;
  v.y:=0.0;
  v.z:=0.0;
 end;
end;

procedure Vector3Normalize(var v:TKraftVector3); {$ifdef caninline}inline;{$endif}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=1.0/sqrt(l);
  v.x:=v.x*l;
  v.y:=v.y*l;
  v.z:=v.z*l;
 end else begin
  v.x:=0.0;
  v.y:=0.0;
  v.z:=0.0;
 end;
end;

procedure Vector3NormalizeEx(var v:TKraftVector3); {$ifdef caninline}inline;{$endif}
var l:single;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=sqrt(l);
  v.x:=v.x/l;
  v.y:=v.y/l;
  v.z:=v.z/l;
 end else begin
  v.x:=0.0;
  v.y:=0.0;
  v.z:=0.0;
 end;
end;

function Vector3SafeNorm(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=1.0/sqrt(l);
  result.x:=v.x*l;
  result.y:=v.y*l;
  result.z:=v.z*l;
 end else begin
  result.x:=1.0;
  result.y:=0.0;
  result.z:=0.0;
 end;
end;

function Vector3Norm(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=1.0/sqrt(l);
  result.x:=v.x*l;
  result.y:=v.y*l;
  result.z:=v.z*l;
 end else begin
  result.x:=0.0;
  result.y:=0.0;
  result.z:=0.0;
 end;
end;

function Vector3NormEx(const v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
var l:TKraftScalar;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>0.0 then begin
  l:=sqrt(l);
  result.x:=v.x/l;
  result.y:=v.y/l;
  result.z:=v.z/l;
 end else begin
  result.x:=0.0;
  result.y:=0.0;
  result.z:=0.0;
 end;
end;

procedure Vector3RotateX(var v:TKraftVector3;a:TKraftScalar); {$ifdef caninline}inline;{$endif}
var t:TKraftVector3;
begin
 t.x:=v.x;
 t.y:=(v.y*cos(a))-(v.z*sin(a));
 t.z:=(v.y*sin(a))+(v.z*cos(a));
 v:=t;
end;

procedure Vector3RotateY(var v:TKraftVector3;a:TKraftScalar); {$ifdef caninline}inline;{$endif}
var t:TKraftVector3;
begin
 t.x:=(v.x*cos(a))+(v.z*sin(a));
 t.y:=v.y;
 t.z:=(v.z*cos(a))-(v.x*sin(a));
 v:=t;
end;

procedure Vector3RotateZ(var v:TKraftVector3;a:TKraftScalar); {$ifdef caninline}inline;{$endif}
var t:TKraftVector3;
begin
 t.x:=(v.x*cos(a))-(v.y*sin(a));
 t.y:=(v.x*sin(a))+(v.y*cos(a));
 t.z:=v.z;
 v:=t;
end;

procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix3x3); overload; {$ifdef caninline}inline;{$endif}
var t:TKraftVector3;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 t.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
 v:=t;
end;

procedure Vector3MatrixMul(var v:TKraftVector3;const m:TKraftMatrix4x4); overload;
var t:TKraftVector3;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+m[3,0];
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+m[3,1];
 t.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+m[3,2];
 v:=t;
end;

procedure Vector3MatrixMulBasis(var v:TKraftVector3;const m:TKraftMatrix4x4); overload; {$ifdef caninline}inline;{$endif}
var t:TKraftVector3;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 t.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
 v:=t;
end;

procedure Vector3MatrixMulInverted(var v:TKraftVector3;const m:TKraftMatrix4x4); overload; {$ifdef caninline}inline;{$endif}
var p,t:TKraftVector3;
begin
 p.x:=v.x-m[3,0];
 p.y:=v.y-m[3,1];
 p.z:=v.z-m[3,2];
 t.x:=(m[0,0]*p.x)+(m[0,1]*p.y)+(m[0,2]*p.z);
 t.y:=(m[1,0]*p.x)+(m[1,1]*p.y)+(m[1,2]*p.z);
 t.z:=(m[2,0]*p.x)+(m[2,1]*p.y)+(m[2,2]*p.z);
 v:=t;
end;

function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
end;

function Vector3TermMatrixMulInverse(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
var Determinant:TKraftScalar;
begin
 Determinant:=((m[0,0]*((m[1,1]*m[2,2])-(m[2,1]*m[1,2])))-
               (m[0,1]*((m[1,0]*m[2,2])-(m[2,0]*m[1,2]))))+
               (m[0,2]*((m[1,0]*m[2,1])-(m[2,0]*m[1,1])));
 if Determinant<>0.0 then begin
  Determinant:=1.0/Determinant;
 end;
 result.x:=((v.x*((m[1,1]*m[2,2])-(m[1,2]*m[2,1])))+(v.y*((m[1,2]*m[2,0])-(m[1,0]*m[2,2])))+(v.z*((m[1,0]*m[2,1])-(m[1,1]*m[2,0]))))*Determinant;
 result.y:=((m[0,0]*((v.y*m[2,2])-(v.z*m[2,1])))+(m[0,1]*((v.z*m[2,0])-(v.x*m[2,2])))+(m[0,2]*((v.x*m[2,1])-(v.y*m[2,0]))))*Determinant;
 result.z:=((m[0,0]*((m[1,1]*v.z)-(m[1,2]*v.y)))+(m[0,1]*((m[1,2]*v.x)-(m[1,0]*v.z)))+(m[0,2]*((m[1,0]*v.y)-(m[1,1]*v.x))))*Determinant;
end;

function Vector3TermMatrixMulInverted(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
var p:TKraftVector3;
begin
 p.x:=v.x-m[3,0];
 p.y:=v.y-m[3,1];
 p.z:=v.z-m[3,2];
 result.x:=(m[0,0]*p.x)+(m[0,1]*p.y)+(m[0,2]*p.z);
 result.y:=(m[1,0]*p.x)+(m[1,1]*p.y)+(m[1,2]*p.z);
 result.z:=(m[2,0]*p.x)+(m[2,1]*p.y)+(m[2,2]*p.z);
end;

function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix3x3):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(m[0,0]*v.x)+(m[0,1]*v.y)+(m[0,2]*v.z);
 result.y:=(m[1,0]*v.x)+(m[1,1]*v.y)+(m[1,2]*v.z);
 result.z:=(m[2,0]*v.x)+(m[2,1]*v.y)+(m[2,2]*v.z);
end;

function Vector3TermMatrixMulTransposed(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(m[0,0]*v.x)+(m[0,1]*v.y)+(m[0,2]*v.z)+m[0,3];
 result.y:=(m[1,0]*v.x)+(m[1,1]*v.y)+(m[1,2]*v.z)+m[1,3];
 result.z:=(m[2,0]*v.x)+(m[2,1]*v.y)+(m[2,2]*v.z)+m[2,3];
end;

function Vector3TermMatrixMulTransposedBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(m[0,0]*v.x)+(m[0,1]*v.y)+(m[0,2]*v.z);
 result.y:=(m[1,0]*v.x)+(m[1,1]*v.y)+(m[1,2]*v.z);
 result.z:=(m[2,0]*v.x)+(m[2,1]*v.y)+(m[2,2]*v.z);
end;

function Vector3TermMatrixMulHomogen(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; {$ifdef caninline}inline;{$endif}
var result_w:TKraftScalar;
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+m[3,0];
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+m[3,1];
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+m[3,2];
 result_w:=(m[0,3]*v.x)+(m[1,3]*v.y)+(m[2,3]*v.z)+m[3,3];
 result.x:=result.x/result_w;
 result.y:=result.y/result_w;
 result.z:=result.z/result_w;
end;

function Vector3TermMatrixMulBasis(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z);
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z);
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z);
end;

function Vector3TermMatrixMul(const v:TKraftVector3;const m:TKraftMatrix4x4):TKraftVector3; overload;
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+m[3,0];
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+m[3,1];
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+m[3,2];
end;

function Vector3Lerp(const v1,v2:TKraftVector3;w:TKraftScalar):TKraftVector3; {$ifdef caninline}inline;{$endif}
var iw:TKraftScalar;
begin
 if w<0.0 then begin
  result:=v1;
 end else if w>1.0 then begin
  result:=v2;
 end else begin
  iw:=1.0-w;
  result.x:=(iw*v1.x)+(w*v2.x);
  result.y:=(iw*v1.y)+(w*v2.y);
  result.z:=(iw*v1.z)+(w*v2.z);
 end;
end;

function Vector3Perpendicular(v:TKraftVector3):TKraftVector3; {$ifdef caninline}inline;{$endif}
var p:TKraftVector3;
begin
 Vector3NormalizeEx(v);
 p.x:=abs(v.x);
 p.y:=abs(v.y);
 p.z:=abs(v.z);
 if (p.x<=p.y) and (p.x<=p.z) then begin
  p:=Vector3XAxis;
 end else if (p.y<=p.x) and (p.y<=p.z) then begin
  p:=Vector3YAxis;
 end else begin
  p:=Vector3ZAxis;
 end;
 result:=Vector3NormEx(Vector3Sub(p,Vector3ScalarMul(v,Vector3Dot(v,p))));
end;

function Vector3TermQuaternionRotate(const v:TKraftVector3;const q:TKraftQuaternion):TKraftVector3; {$ifdef caninline}inline;{$endif}
var t,qv:TKraftVector3;
begin
 // t = 2 * cross(q.xyz, v)
 // v' = v + q.w * t + cross(q.xyz, t)
 qv:=PKraftVector3(pointer(@q))^;
 t:=Vector3ScalarMul(Vector3Cross(qv,v),2.0);
 result:=Vector3Add(Vector3Add(v,Vector3ScalarMul(t,q.w)),Vector3Cross(qv,t));
end;
{var vn:TKraftVector3;
vq,rq:TKraftQuaternion;
begin
 vq.x:=vn.x;
 vq.y:=vn.y;
 vq.z:=vn.z;
 vq.w:=0.0;
 rq:=QuaternionMul(q,QuaternionMul(vq,QuaternionConjugate(q)));
 result.x:=rq.x;
 result.y:=rq.y;
 result.z:=rq.z;
end;{}

function Vector3ProjectToBounds(const v:TKraftVector3;const MinVector,MaxVector:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 if v.x<0.0 then begin
  result:=v.x*MaxVector.x;
 end else begin
  result:=v.x*MinVector.x;
 end;
 if v.y<0.0 then begin
  result:=result+(v.y*MaxVector.y);
 end else begin
  result:=result+(v.y*MinVector.y);
 end;
 if v.z<0.0 then begin
  result:=result+(v.z*MaxVector.z);
 end else begin
  result:=result+(v.z*MinVector.z);
 end;
end;
{$endif}

function Vector4Compare(const v1,v2:TKraftVector4):boolean;
begin
 result:=(abs(v1.x-v2.x)<EPSILON) and (abs(v1.y-v2.y)<EPSILON) and (abs(v1.z-v2.z)<EPSILON) and (abs(v1.w-v2.w)<EPSILON);
end;

function Vector4CompareEx(const v1,v2:TKraftVector4;const Threshold:TKraftScalar=EPSILON):boolean;
begin
 result:=(abs(v1.x-v2.x)<Threshold) and (abs(v1.y-v2.y)<Threshold) and (abs(v1.z-v2.z)<Threshold) and (abs(v1.w-v2.w)<Threshold);
end;

function Vector4Add(const v1,v2:TKraftVector4):TKraftVector4;
begin
 result.x:=v1.x+v2.x;
 result.y:=v1.y+v2.y;
 result.z:=v1.z+v2.z;
 result.w:=v1.w+v2.w;
end;

function Vector4Sub(const v1,v2:TKraftVector4):TKraftVector4;
begin
 result.x:=v1.x-v2.x;
 result.y:=v1.y-v2.y;
 result.z:=v1.z-v2.z;
 result.w:=v1.w-v2.w;
end;

function Vector4ScalarMul(const v:TKraftVector4;s:TKraftScalar):TKraftVector4;
begin
 result.x:=v.x*s;
 result.y:=v.y*s;
 result.z:=v.z*s;
 result.w:=v.w*s;
end;

function Vector4Dot(const v1,v2:TKraftVector4):TKraftScalar;
begin
 result:=(v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z)+(v1.w*v2.w);
end;

function Vector4Cross(const v1,v2:TKraftVector4):TKraftVector4;
begin
 result.x:=(v1.y*v2.z)-(v2.y*v1.z);
 result.y:=(v2.x*v1.z)-(v1.x*v2.z);
 result.z:=(v1.x*v2.y)-(v2.x*v1.y);
 result.w:=1;
end;

function Vector4Neg(const v:TKraftVector4):TKraftVector4;
begin
 result.x:=-v.x;
 result.y:=-v.y;
 result.z:=-v.z;
 result.w:=1;
end;

procedure Vector4Scale(var v:TKraftVector4;sx,sy,sz:TKraftScalar); overload;
begin
 v.x:=v.x*sx;
 v.y:=v.y*sy;
 v.z:=v.z*sz;
end;

procedure Vector4Scale(var v:TKraftVector4;s:TKraftScalar); overload;
begin
 v.x:=v.x*s;
 v.y:=v.y*s;
 v.z:=v.z*s;
end;

function Vector4Mul(const v1,v2:TKraftVector4):TKraftVector4;
begin
 result.x:=v1.x*v2.x;
 result.y:=v1.y*v2.y;
 result.z:=v1.z*v2.z;
 result.w:=1;
end;

function Vector4Length(const v:TKraftVector4):TKraftScalar;
begin
 result:=SQRT((v.x*v.x)+(v.y*v.y)+(v.z*v.z));
end;

function Vector4Dist(const v1,v2:TKraftVector4):TKraftScalar;
begin
 result:=Vector4Length(Vector4Sub(v2,v1));
end;

function Vector4LengthSquared(const v:TKraftVector4):TKraftScalar;
begin
 result:=sqr(v.x)+sqr(v.y)+sqr(v.z)+sqr(v.w);
end;

function Vector4DistSquared(const v1,v2:TKraftVector4):TKraftScalar;
begin
 result:=Vector4LengthSquared(Vector4Sub(v2,v1));
end;

function Vector4Angle(const v1,v2,v3:TKraftVector4):TKraftScalar;
var A1,A2:TKraftVector4;
    L1,L2:TKraftScalar;
begin
 A1:=Vector4Sub(v1,v2);
 A2:=Vector4Sub(v3,v2);
 L1:=Vector4Length(A1);
 L2:=Vector4Length(A2);
 if (L1=0) or (L2=0) then begin
  result:=0;
 end else begin
  result:=ArcCos(Vector4Dot(A1,A2)/(L1*L2));
 end;
end;

procedure Vector4Normalize(var v:TKraftVector4);
var L:TKraftScalar;
begin
 L:=Vector4Length(v);
 if L<>0.0 then begin
  Vector4Scale(v,1/L);
 end else begin
  v:=Vector4Origin;
 end;
end;

function Vector4Norm(const v:TKraftVector4):TKraftVector4;
var L:TKraftScalar;
begin
 L:=Vector4Length(v);
 if L<>0.0 then begin
  result:=Vector4ScalarMul(v,1/L);
 end else begin
  result:=Vector4Origin;
 end;
end;

procedure Vector4RotateX(var v:TKraftVector4;a:TKraftScalar);
var t:TKraftVector4;
begin
 t.x:=v.x;
 t.y:=(v.y*cos(a))+(v.z*-sin(a));
 t.z:=(v.y*sin(a))+(v.z*cos(a));
 t.w:=1;
 v:=t;
end;

procedure Vector4RotateY(var v:TKraftVector4;a:TKraftScalar);
var t:TKraftVector4;
begin
 t.x:=(v.x*cos(a))+(v.z*sin(a));
 t.y:=v.y;
 t.z:=(v.x*-sin(a))+(v.z*cos(a));
 t.w:=1;
 v:=t;
end;

procedure Vector4RotateZ(var v:TKraftVector4;a:TKraftScalar);
var t:TKraftVector4;
begin
 t.x:=(v.x*cos(a))+(v.y*-sin(a));
 t.y:=(v.x*sin(a))+(v.y*cos(a));
 t.z:=v.z;
 t.w:=1;
 v:=t;
end;

procedure Vector4MatrixMul(var v:TKraftVector4;const m:TKraftMatrix4x4); {$ifdef CPU386ASMForSinglePrecision}register;
asm
{mov eax,v
 mov edx,m}
 movups xmm0,dqword ptr [v]     // d c b a
 movaps xmm1,xmm0               // d c b a
 movaps xmm2,xmm0               // d c b a
 movaps xmm3,xmm0               // d c b a
 shufps xmm0,xmm0,$00           // a a a a 00000000b
 shufps xmm1,xmm1,$55           // b b b b 01010101b
 shufps xmm2,xmm2,$aa           // c c c c 10101010b
 shufps xmm3,xmm3,$ff           // d d d d 11111111b
 movups xmm4,dqword ptr [m+0]
 movups xmm5,dqword ptr [m+16]
 movups xmm6,dqword ptr [m+32]
 movups xmm7,dqword ptr [m+48]
 mulps xmm0,xmm4
 mulps xmm1,xmm5
 mulps xmm2,xmm6
 mulps xmm3,xmm7
 addps xmm0,xmm1
 addps xmm2,xmm3
 addps xmm0,xmm2
 movups dqword ptr [v],xmm0
end;
{$else}
var t:TKraftVector4;
begin
 t.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+(m[3,0]*v.w);
 t.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+(m[3,1]*v.w);
 t.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+(m[3,2]*v.w);
 t.w:=(m[0,3]*v.x)+(m[1,3]*v.y)+(m[2,3]*v.z)+(m[3,3]*v.w);
 v:=t;
end;
{$endif}

function Vector4TermMatrixMul(const v:TKraftVector4;const m:TKraftMatrix4x4):TKraftVector4; {$ifdef CPU386ASMForSinglePrecision}register;
asm
{mov eax,v
 mov edx,m
 mov ecx,result}
 movups xmm0,[eax]              // d c b a
 movaps xmm1,xmm0               // d c b a
 movaps xmm2,xmm0               // d c b a
 movaps xmm3,xmm0               // d c b a
 shufps xmm0,xmm0,$00           // a a a a 00000000b
 shufps xmm1,xmm1,$55           // b b b b 01010101b
 shufps xmm2,xmm2,$aa           // c c c c 10101010b
 shufps xmm3,xmm3,$ff           // d d d d 11111111b
 movups xmm4,[edx+0]
 movups xmm5,[edx+16]
 movups xmm6,[edx+32]
 movups xmm7,[edx+48]
 mulps xmm0,xmm4
 mulps xmm1,xmm5
 mulps xmm2,xmm6
 mulps xmm3,xmm7
 addps xmm0,xmm1
 addps xmm2,xmm3
 addps xmm0,xmm2
 movups [ecx],xmm0
end;
{$else}
begin
 result.x:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+(m[3,0]*v.w);
 result.y:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+(m[3,1]*v.w);
 result.z:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+(m[3,2]*v.w);
 result.w:=(m[0,3]*v.x)+(m[1,3]*v.y)+(m[2,3]*v.z)+(m[3,3]*v.w);
end;
{$endif}

function Vector4TermMatrixMulHomogen(const v:TKraftVector4;const m:TKraftMatrix4x4):TKraftVector4;
begin
 result.w:=(m[0,3]*v.x)+(m[1,3]*v.y)+(m[2,3]*v.z)+(m[3,3]*v.w);
 result.x:=((m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+(m[3,0]*v.w))/result.w;
 result.y:=((m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+(m[3,1]*v.w))/result.w;
 result.z:=((m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+(m[3,2]*v.w))/result.w;
 result.w:=1.0;
end;

procedure Vector4Rotate(var v:TKraftVector4;const Axis:TKraftVector4;a:TKraftScalar);
var t:TKraftVector3;
begin
 t.x:=Axis.x;
 t.y:=Axis.y;
 t.z:=Axis.z;
 Vector4MatrixMul(v,Matrix4x4Rotate(a,t));
end;

function Vector4Lerp(const v1,v2:TKraftVector4;w:TKraftScalar):TKraftVector4;
var iw:TKraftScalar;
begin
 if w<0.0 then begin
  result:=v1;
 end else if w>1.0 then begin
  result:=v2;
 end else begin
  iw:=1.0-w;
  result.x:=(iw*v1.x)+(w*v2.x);
  result.y:=(iw*v1.y)+(w*v2.y);
  result.z:=(iw*v1.z)+(w*v2.z);
  result.w:=(iw*v1.w)+(w*v2.w);
 end;
end;

function Matrix2x2Inverse(var mr:TKraftMatrix2x2;const ma:TKraftMatrix2x2):boolean;
var Determinant:TKraftScalar;
begin
 Determinant:=(ma[0,0]*ma[1,1])-(ma[0,1]*ma[1,0]);
 if abs(Determinant)<EPSILON then begin
  mr:=Matrix2x2Identity;
  result:=false;
 end else begin
  Determinant:=1.0/Determinant;
  mr[0,0]:=ma[1,1]*Determinant;
  mr[0,1]:=-(ma[0,1]*Determinant);
  mr[1,0]:=-(ma[1,0]*Determinant);
  mr[1,1]:=ma[0,0]*Determinant;
  result:=true;
 end;
end;

function Matrix2x2TermInverse(const m:TKraftMatrix2x2):TKraftMatrix2x2;
var Determinant:TKraftScalar;
begin
 Determinant:=(m[0,0]*m[1,1])-(m[0,1]*m[1,0]);
 if abs(Determinant)<EPSILON then begin
  result:=Matrix2x2Identity;
 end else begin
  Determinant:=1.0/Determinant;
  result[0,0]:=m[1,1]*Determinant;
  result[0,1]:=-(m[0,1]*Determinant);
  result[1,0]:=-(m[1,0]*Determinant);
  result[1,1]:=m[0,0]*Determinant;
 end;
end;

function Matrix3x3RotateX(Angle:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix3x3Identity;
 result[1,1]:=cos(Angle);
 result[2,2]:=result[1,1];
 result[1,2]:=sin(Angle);
 result[2,1]:=-result[1,2];
end;

function Matrix3x3RotateY(Angle:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix3x3Identity;
 result[0,0]:=cos(Angle);
 result[2,2]:=result[0,0];
 result[0,2]:=-sin(Angle);
 result[2,0]:=-result[0,2];
end;

function Matrix3x3RotateZ(Angle:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix3x3Identity;
 result[0,0]:=cos(Angle);
 result[1,1]:=result[0,0];
 result[0,1]:=sin(Angle);
 result[1,0]:=-result[0,1];
end;

function Matrix3x3Rotate(Angle:TKraftScalar;Axis:TKraftVector3):TKraftMatrix3x3; overload;
var m:TKraftMatrix3x3;
    CosinusAngle,SinusAngle:TKraftScalar;
begin
 m:=Matrix3x3Identity;
 CosinusAngle:=cos(Angle);
 SinusAngle:=sin(Angle);
 m[0,0]:=CosinusAngle+((1.0-CosinusAngle)*sqr(Axis.x));
 m[1,0]:=((1.0-CosinusAngle)*Axis.x*Axis.y)-(Axis.z*SinusAngle);
 m[2,0]:=((1.0-CosinusAngle)*Axis.x*Axis.z)+(Axis.y*SinusAngle);
 m[0,1]:=((1.0-CosinusAngle)*Axis.x*Axis.z)+(Axis.z*SinusAngle);
 m[1,1]:=CosinusAngle+((1.0-CosinusAngle)*sqr(Axis.y));
 m[2,1]:=((1.0-CosinusAngle)*Axis.y*Axis.z)-(Axis.x*SinusAngle);
 m[0,2]:=((1.0-CosinusAngle)*Axis.x*Axis.z)-(Axis.y*SinusAngle);
 m[1,2]:=((1.0-CosinusAngle)*Axis.y*Axis.z)+(Axis.x*SinusAngle);
 m[2,2]:=CosinusAngle+((1.0-CosinusAngle)*sqr(Axis.z));
{$ifdef SIMD}
 m[0,3]:=0.0;
 m[1,3]:=0.0;
 m[2,3]:=0.0;
{$endif}
 result:=m;
end;

function Matrix3x3Scale(sx,sy,sz:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix3x3Identity;
 result[0,0]:=sx;
 result[1,1]:=sy;
 result[2,2]:=sz;
end;

procedure Matrix3x3Add(var m1:TKraftMatrix3x3;const m2:TKraftMatrix3x3); {$ifdef caninline}inline;{$endif}
begin
 m1[0,0]:=m1[0,0]+m2[0,0];
 m1[0,1]:=m1[0,1]+m2[0,1];
 m1[0,2]:=m1[0,2]+m2[0,2];
{$ifdef SIMD}
 m1[0,3]:=0.0;
{$endif}
 m1[1,0]:=m1[1,0]+m2[1,0];
 m1[1,1]:=m1[1,1]+m2[1,1];
 m1[1,2]:=m1[1,2]+m2[1,2];
{$ifdef SIMD}
 m1[1,3]:=0.0;
{$endif}
 m1[2,0]:=m1[2,0]+m2[2,0];
 m1[2,1]:=m1[2,1]+m2[2,1];
 m1[2,2]:=m1[2,2]+m2[2,2];
{$ifdef SIMD}
 m1[2,3]:=0.0;
{$endif}
end;

procedure Matrix3x3Sub(var m1:TKraftMatrix3x3;const m2:TKraftMatrix3x3); {$ifdef caninline}inline;{$endif}
begin
 m1[0,0]:=m1[0,0]-m2[0,0];
 m1[0,1]:=m1[0,1]-m2[0,1];
 m1[0,2]:=m1[0,2]-m2[0,2];
{$ifdef SIMD}
 m1[0,3]:=0.0;
{$endif}
 m1[1,0]:=m1[1,0]-m2[1,0];
 m1[1,1]:=m1[1,1]-m2[1,1];
 m1[1,2]:=m1[1,2]-m2[1,2];
{$ifdef SIMD}
 m1[1,3]:=0.0;
{$endif}
 m1[2,0]:=m1[2,0]-m2[2,0];
 m1[2,1]:=m1[2,1]-m2[2,1];
 m1[2,2]:=m1[2,2]-m2[2,2];
{$ifdef SIMD}
 m1[2,3]:=0.0;
{$endif}
end;

procedure Matrix3x3Mul(var m1:TKraftMatrix3x3;const m2:TKraftMatrix3x3);
var t:TKraftMatrix3x3;
begin
 t[0,0]:=(m1[0,0]*m2[0,0])+(m1[0,1]*m2[1,0])+(m1[0,2]*m2[2,0]);
 t[0,1]:=(m1[0,0]*m2[0,1])+(m1[0,1]*m2[1,1])+(m1[0,2]*m2[2,1]);
 t[0,2]:=(m1[0,0]*m2[0,2])+(m1[0,1]*m2[1,2])+(m1[0,2]*m2[2,2]);
{$ifdef SIMD}
 t[0,3]:=0.0;
{$endif}
 t[1,0]:=(m1[1,0]*m2[0,0])+(m1[1,1]*m2[1,0])+(m1[1,2]*m2[2,0]);
 t[1,1]:=(m1[1,0]*m2[0,1])+(m1[1,1]*m2[1,1])+(m1[1,2]*m2[2,1]);
 t[1,2]:=(m1[1,0]*m2[0,2])+(m1[1,1]*m2[1,2])+(m1[1,2]*m2[2,2]);
{$ifdef SIMD}
 t[1,3]:=0.0;
{$endif}
 t[2,0]:=(m1[2,0]*m2[0,0])+(m1[2,1]*m2[1,0])+(m1[2,2]*m2[2,0]);
 t[2,1]:=(m1[2,0]*m2[0,1])+(m1[2,1]*m2[1,1])+(m1[2,2]*m2[2,1]);
 t[2,2]:=(m1[2,0]*m2[0,2])+(m1[2,1]*m2[1,2])+(m1[2,2]*m2[2,2]);
{$ifdef SIMD}
 t[2,3]:=0.0;
{$endif}
 m1:=t;
end;
          
function Matrix3x3TermAdd(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=m1[0,0]+m2[0,0];
 result[0,1]:=m1[0,1]+m2[0,1];
 result[0,2]:=m1[0,2]+m2[0,2];
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=m1[1,0]+m2[1,0];
 result[1,1]:=m1[1,1]+m2[1,1];
 result[1,2]:=m1[1,2]+m2[1,2];
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=m1[2,0]+m2[2,0];
 result[2,1]:=m1[2,1]+m2[2,1];
 result[2,2]:=m1[2,2]+m2[2,2];
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function Matrix3x3TermSub(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=m1[0,0]-m2[0,0];
 result[0,1]:=m1[0,1]-m2[0,1];
 result[0,2]:=m1[0,2]-m2[0,2];
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=m1[1,0]-m2[1,0];
 result[1,1]:=m1[1,1]-m2[1,1];
 result[1,2]:=m1[1,2]-m2[1,2];
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=m1[2,0]-m2[2,0];
 result[2,1]:=m1[2,1]-m2[2,1];
 result[2,2]:=m1[2,2]-m2[2,2];
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function Matrix3x3TermMul(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3;
begin
 result[0,0]:=(m1[0,0]*m2[0,0])+(m1[0,1]*m2[1,0])+(m1[0,2]*m2[2,0]);
 result[0,1]:=(m1[0,0]*m2[0,1])+(m1[0,1]*m2[1,1])+(m1[0,2]*m2[2,1]);
 result[0,2]:=(m1[0,0]*m2[0,2])+(m1[0,1]*m2[1,2])+(m1[0,2]*m2[2,2]);
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=(m1[1,0]*m2[0,0])+(m1[1,1]*m2[1,0])+(m1[1,2]*m2[2,0]);
 result[1,1]:=(m1[1,0]*m2[0,1])+(m1[1,1]*m2[1,1])+(m1[1,2]*m2[2,1]);
 result[1,2]:=(m1[1,0]*m2[0,2])+(m1[1,1]*m2[1,2])+(m1[1,2]*m2[2,2]);
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=(m1[2,0]*m2[0,0])+(m1[2,1]*m2[1,0])+(m1[2,2]*m2[2,0]);
 result[2,1]:=(m1[2,0]*m2[0,1])+(m1[2,1]*m2[1,1])+(m1[2,2]*m2[2,1]);
 result[2,2]:=(m1[2,0]*m2[0,2])+(m1[2,1]*m2[1,2])+(m1[2,2]*m2[2,2]);
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function Matrix3x3TermMulTranspose(const m1,m2:TKraftMatrix3x3):TKraftMatrix3x3;
begin
 result[0,0]:=(m1[0,0]*m2[0,0])+(m1[0,1]*m2[0,1])+(m1[0,2]*m2[0,2]);
 result[0,1]:=(m1[0,0]*m2[1,0])+(m1[0,1]*m2[1,1])+(m1[0,2]*m2[1,2]);
 result[0,2]:=(m1[0,0]*m2[2,0])+(m1[0,1]*m2[2,1])+(m1[0,2]*m2[2,2]);
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=(m1[1,0]*m2[0,0])+(m1[1,1]*m2[0,1])+(m1[1,2]*m2[0,2]);
 result[1,1]:=(m1[1,0]*m2[1,0])+(m1[1,1]*m2[1,1])+(m1[1,2]*m2[1,2]);
 result[1,2]:=(m1[1,0]*m2[2,0])+(m1[1,1]*m2[2,1])+(m1[1,2]*m2[2,2]);
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=(m1[2,0]*m2[0,0])+(m1[2,1]*m2[0,1])+(m1[2,2]*m2[0,2]);
 result[2,1]:=(m1[2,0]*m2[1,0])+(m1[2,1]*m2[1,1])+(m1[2,2]*m2[1,2]);
 result[2,2]:=(m1[2,0]*m2[2,0])+(m1[2,1]*m2[2,1])+(m1[2,2]*m2[2,2]);
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

procedure Matrix3x3ScalarMul(var m:TKraftMatrix3x3;s:TKraftScalar); {$ifdef caninline}inline;{$endif}
begin
 m[0,0]:=m[0,0]*s;
 m[0,1]:=m[0,1]*s;
 m[0,2]:=m[0,2]*s;
{$ifdef SIMD}
 m[0,3]:=0.0;
{$endif}
 m[1,0]:=m[1,0]*s;
 m[1,1]:=m[1,1]*s;
 m[1,2]:=m[1,2]*s;
{$ifdef SIMD}
 m[1,3]:=0.0;
{$endif}
 m[2,0]:=m[2,0]*s;
 m[2,1]:=m[2,1]*s;
 m[2,2]:=m[2,2]*s;
{$ifdef SIMD}
 m[2,3]:=0.0;
{$endif}
end;

function Matrix3x3TermScalarMul(const m:TKraftMatrix3x3;s:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=m[0,0]*s;
 result[0,1]:=m[0,1]*s;
 result[0,2]:=m[0,2]*s;
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=m[1,0]*s;
 result[1,1]:=m[1,1]*s;
 result[1,2]:=m[1,2]*s;
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=m[2,0]*s;
 result[2,1]:=m[2,1]*s;
 result[2,2]:=m[2,2]*s;
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

procedure Matrix3x3Transpose(var m:TKraftMatrix3x3); {$ifdef caninline}inline;{$endif}
var mt:TKraftMatrix3x3;
begin
 mt[0,0]:=m[0,0];
 mt[1,0]:=m[0,1];
 mt[2,0]:=m[0,2];
 mt[0,1]:=m[1,0];
 mt[1,1]:=m[1,1];
 mt[2,1]:=m[1,2];
 mt[0,2]:=m[2,0];
 mt[1,2]:=m[2,1];
 mt[2,2]:=m[2,2];
{$ifdef SIMD}
 mt[0,3]:=0.0;
 mt[1,3]:=0.0;
 mt[2,3]:=0.0;
{$endif}
 m:=mt;
end;

function Matrix3x3TermTranspose(const m:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=m[0,0];
 result[1,0]:=m[0,1];
 result[2,0]:=m[0,2];
 result[0,1]:=m[1,0];
 result[1,1]:=m[1,1];
 result[2,1]:=m[1,2];
 result[0,2]:=m[2,0];
 result[1,2]:=m[2,1];
 result[2,2]:=m[2,2];
{$ifdef SIMD}
 result[0,3]:=0.0;
 result[1,3]:=0.0;
 result[2,3]:=0.0;
{$endif}
end;

function Matrix3x3Determinant(const m:TKraftMatrix3x3):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
 result:=(m[0,0]*((m[1,1]*m[2,2])-(m[2,1]*m[1,2])))-
         (m[0,1]*((m[1,0]*m[2,2])-(m[2,0]*m[1,2])))+
         (m[0,2]*((m[1,0]*m[2,1])-(m[2,0]*m[1,1])));
end;

function Matrix3x3EulerAngles(const m:TKraftMatrix3x3):TKraftVector3;
var v0,v1:TKraftVector3;
begin
 if abs((-1.0)-m[0,2])<EPSILON then begin
  result.x:=0.0;
  result.y:=pi*0.5;
  result.z:=ArcTan2(m[1,0],m[2,0]);
 end else if abs(1.0-m[0,2])<EPSILON then begin
  result.x:=0.0;
  result.y:=-(pi*0.5);
  result.z:=ArcTan2(-m[1,0],-m[2,0]);
 end else begin
  v0.x:=-ArcSin(m[0,2]);
  v1.x:=pi-v0.x;
  v0.y:=ArcTan2(m[1,2]/cos(v0.x),m[2,2]/cos(v0.x));
  v1.y:=ArcTan2(m[1,2]/cos(v1.x),m[2,2]/cos(v1.x));
  v0.z:=ArcTan2(m[0,1]/cos(v0.x),m[0,0]/cos(v0.x));
  v1.z:=ArcTan2(m[0,1]/cos(v1.x),m[0,0]/cos(v1.x));
  if Vector3LengthSquared(v0)<Vector3LengthSquared(v1) then begin
   result:=v0;
  end else begin
   result:=v1;
  end;
 end;
end;

procedure Matrix3x3SetColumn(var m:TKraftMatrix3x3;const c:longint;const v:TKraftVector3); {$ifdef caninline}inline;{$endif}
begin
 m[c,0]:=v.x;
 m[c,1]:=v.y;
 m[c,2]:=v.z;
end;

function Matrix3x3GetColumn(const m:TKraftMatrix3x3;const c:longint):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=m[c,0];
 result.y:=m[c,1];
 result.z:=m[c,2];
end;

procedure Matrix3x3SetRow(var m:TKraftMatrix3x3;const r:longint;const v:TKraftVector3); {$ifdef caninline}inline;{$endif}
begin
 m[0,r]:=v.x;
 m[1,r]:=v.y;
 m[2,r]:=v.z;
end;

function Matrix3x3GetRow(const m:TKraftMatrix3x3;const r:longint):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=m[0,r];
 result.y:=m[1,r];
 result.z:=m[2,r];
end;

function Matrix3x3Compare(const m1,m2:TKraftMatrix3x3):boolean;
var r,c:longint;
begin
 result:=true;
 for r:=0 to 2 do begin
  for c:=0 to 2 do begin
   if abs(m1[r,c]-m2[r,c])>EPSILON then begin
    result:=false;
    exit;
   end;
  end;
 end;
end;

function Matrix3x3Inverse(var mr:TKraftMatrix3x3;const ma:TKraftMatrix3x3):boolean;
var Determinant:TKraftScalar;
begin
 Determinant:=((ma[0,0]*((ma[1,1]*ma[2,2])-(ma[2,1]*ma[1,2])))-
               (ma[0,1]*((ma[1,0]*ma[2,2])-(ma[2,0]*ma[1,2]))))+
               (ma[0,2]*((ma[1,0]*ma[2,1])-(ma[2,0]*ma[1,1])));
 if abs(Determinant)<EPSILON then begin
  mr:=Matrix3x3Identity;
  result:=false;
 end else begin
  Determinant:=1.0/Determinant;
  mr[0,0]:=((ma[1,1]*ma[2,2])-(ma[2,1]*ma[1,2]))*Determinant;
  mr[0,1]:=((ma[0,2]*ma[2,1])-(ma[0,1]*ma[2,2]))*Determinant;
  mr[0,2]:=((ma[0,1]*ma[1,2])-(ma[0,2]*ma[1,1]))*Determinant;
{$ifdef SIMD}
  mr[0,3]:=0.0;
{$endif}
  mr[1,0]:=((ma[1,2]*ma[2,0])-(ma[1,0]*ma[2,2]))*Determinant;
  mr[1,1]:=((ma[0,0]*ma[2,2])-(ma[0,2]*ma[2,0]))*Determinant;
  mr[1,2]:=((ma[1,0]*ma[0,2])-(ma[0,0]*ma[1,2]))*Determinant;
{$ifdef SIMD}
  mr[1,3]:=0.0;
{$endif}
  mr[2,0]:=((ma[1,0]*ma[2,1])-(ma[2,0]*ma[1,1]))*Determinant;
  mr[2,1]:=((ma[2,0]*ma[0,1])-(ma[0,0]*ma[2,1]))*Determinant;
  mr[2,2]:=((ma[0,0]*ma[1,1])-(ma[1,0]*ma[0,1]))*Determinant;
{$ifdef SIMD}
  mr[2,3]:=0.0;
{$endif}
  result:=true;
 end;
end;

function Matrix3x3TermInverse(const m:TKraftMatrix3x3):TKraftMatrix3x3;
var Determinant:TKraftScalar;
begin
 Determinant:=((m[0,0]*((m[1,1]*m[2,2])-(m[2,1]*m[1,2])))-
               (m[0,1]*((m[1,0]*m[2,2])-(m[2,0]*m[1,2]))))+
               (m[0,2]*((m[1,0]*m[2,1])-(m[2,0]*m[1,1])));
 if abs(Determinant)<EPSILON then begin
  result:=Matrix3x3Identity;
 end else begin
  Determinant:=1.0/Determinant;
  result[0,0]:=((m[1,1]*m[2,2])-(m[2,1]*m[1,2]))*Determinant;
  result[0,1]:=((m[0,2]*m[2,1])-(m[0,1]*m[2,2]))*Determinant;
  result[0,2]:=((m[0,1]*m[1,2])-(m[0,2]*m[1,1]))*Determinant;
{$ifdef SIMD}
  result[0,3]:=0.0;
{$endif}
  result[1,0]:=((m[1,2]*m[2,0])-(m[1,0]*m[2,2]))*Determinant;
  result[1,1]:=((m[0,0]*m[2,2])-(m[0,2]*m[2,0]))*Determinant;
  result[1,2]:=((m[1,0]*m[0,2])-(m[0,0]*m[1,2]))*Determinant;
{$ifdef SIMD}
  result[1,3]:=0.0;
{$endif}
  result[2,0]:=((m[1,0]*m[2,1])-(m[2,0]*m[1,1]))*Determinant;
  result[2,1]:=((m[2,0]*m[0,1])-(m[0,0]*m[2,1]))*Determinant;
  result[2,2]:=((m[0,0]*m[1,1])-(m[1,0]*m[0,1]))*Determinant;
{$ifdef SIMD}
  result[2,3]:=0.0;
{$endif}
 end;
end;

procedure Matrix3x3OrthoNormalize(var m:TKraftMatrix3x3);
var x,y,z:TKraftVector3;
begin
 x.x:=m[0,0];
 x.y:=m[0,1];
 x.z:=m[0,2];
 Vector3NormalizeEx(x);
 y.x:=m[1,0];
 y.y:=m[1,1];
 y.z:=m[1,2];
 z:=Vector3NormEx(Vector3Cross(x,y));
 y:=Vector3NormEx(Vector3Cross(z,x));
 m[0,0]:=x.x;
 m[0,1]:=x.y;
 m[0,2]:=x.z;
{$ifdef SIMD}
 m[0,3]:=0.0;
{$endif}
 m[1,0]:=y.x;
 m[1,1]:=y.y;
 m[1,2]:=y.z;
{$ifdef SIMD}
 m[1,3]:=0.0;
{$endif}
 m[2,0]:=z.x;
 m[2,1]:=z.y;
 m[2,2]:=z.z;
{$ifdef SIMD}
 m[2,3]:=0.0;
{$endif}
end;

function Matrix3x3Slerp(const a,b:TKraftMatrix3x3;x:TKraftScalar):TKraftMatrix3x3;
//var ix:TKraftScalar;
begin
 if x<=0.0 then begin
  result:=a;
 end else if x>=1.0 then begin
  result:=b;
 end else begin
  result:=QuaternionToMatrix3x3(QuaternionSlerp(QuaternionFromMatrix3x3(a),QuaternionFromMatrix3x3(b),x));
 end;
end;

function Matrix3x3FromToRotation(const FromDirection,ToDirection:TKraftVector3):TKraftMatrix3x3;
var e,h,hvx,hvz,hvxy,hvxz,hvyz:TKraftScalar;
    x,u,v,c:TKraftVector3;
begin
 e:=(FromDirection.x*ToDirection.x)+(FromDirection.y*ToDirection.y)+(FromDirection.z*ToDirection.z);
 if abs(e)>(1.0-EPSILON) then begin
  x.x:=abs(FromDirection.x);
  x.y:=abs(FromDirection.y);
  x.z:=abs(FromDirection.z);
  if x.x<x.y then begin
   if x.x<x.z then begin
    x.x:=1.0;
    x.y:=0.0;
    x.z:=0.0;
   end else begin
    x.x:=0.0;
    x.y:=0.0;
    x.z:=1.0;
   end;
  end else begin
   if x.y<x.z then begin
    x.x:=0.0;
    x.y:=1.0;
    x.z:=0.0;
   end else begin
    x.x:=0.0;
    x.y:=0.0;
    x.z:=1.0;
   end;
  end;
  u.x:=x.x-FromDirection.x;
  u.y:=x.y-FromDirection.y;
  u.z:=x.z-FromDirection.z;
  v.x:=x.x-ToDirection.x;
  v.y:=x.y-ToDirection.y;
  v.z:=x.z-ToDirection.z;
  c.x:=2.0/(sqr(u.x)+sqr(u.y)+sqr(u.z));
  c.y:=2.0/(sqr(v.x)+sqr(v.y)+sqr(v.z));
  c.z:=c.x*c.y*((u.x*v.x)+(u.y*v.y)+(u.z*v.z));
  result[0,0]:=1.0+((c.z*(v.x*u.x))-((c.y*(v.x*v.x))+(c.x*(u.x*u.x))));
  result[0,1]:=(c.z*(v.x*u.y))-((c.y*(v.x*v.y))+(c.x*(u.x*u.y)));
  result[0,2]:=(c.z*(v.x*u.z))-((c.y*(v.x*v.z))+(c.x*(u.x*u.z)));
{$ifdef SIMD}
  result[0,3]:=0.0;
{$endif}
  result[1,0]:=(c.z*(v.y*u.x))-((c.y*(v.y*v.x))+(c.x*(u.y*u.x)));
  result[1,1]:=1.0+((c.z*(v.y*u.y))-((c.y*(v.y*v.y))+(c.x*(u.y*u.y))));
  result[1,2]:=(c.z*(v.y*u.z))-((c.y*(v.y*v.z))+(c.x*(u.y*u.z)));
{$ifdef SIMD}
  result[1,3]:=0.0;
{$endif}
  result[2,0]:=(c.z*(v.z*u.x))-((c.y*(v.z*v.x))+(c.x*(u.z*u.x)));
  result[2,1]:=(c.z*(v.z*u.y))-((c.y*(v.z*v.y))+(c.x*(u.z*u.y)));
  result[2,2]:=1.0+((c.z*(v.z*u.z))-((c.y*(v.z*v.z))+(c.x*(u.z*u.z))));
{$ifdef SIMD}
  result[2,3]:=0.0;
{$endif}
 end else begin
  v:=Vector3Cross(FromDirection,ToDirection);
  h:=1.0/(1.0+e);
  hvx:=h*v.x;
  hvz:=h*v.z;
  hvxy:=hvx*v.y;
  hvxz:=hvx*v.z;
  hvyz:=hvz*v.y;
  result[0,0]:=e+(hvx*v.x);
  result[0,1]:=hvxy-v.z;
  result[0,2]:=hvxz+v.y;
{$ifdef SIMD}
  result[0,3]:=0.0;
{$endif}
  result[1,0]:=hvxy+v.z;
  result[1,1]:=e+(h*sqr(v.y));
  result[1,2]:=hvyz-v.x;
{$ifdef SIMD}
  result[1,3]:=0.0;
{$endif}
  result[2,0]:=hvxz-v.y;
  result[2,1]:=hvyz+v.x;
  result[2,2]:=e+(hvz*v.z);
{$ifdef SIMD}
  result[2,3]:=0.0;
{$endif}
 end;
end;

function Matrix3x3Construct(const Forwards,Up:TKraftVector3):TKraftMatrix3x3;
var RightVector,UpVector,ForwardVector:TKraftVector3;
begin
 ForwardVector:=Vector3NormEx(Vector3Neg(Forwards));
 RightVector:=Vector3NormEx(Vector3Cross(Up,ForwardVector));
 UpVector:=Vector3NormEx(Vector3Cross(ForwardVector,RightVector));
 result[0,0]:=RightVector.x;
 result[0,1]:=RightVector.y;
 result[0,2]:=RightVector.z;
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=UpVector.x;
 result[1,1]:=UpVector.y;
 result[1,2]:=UpVector.z;
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=ForwardVector.x;
 result[2,1]:=ForwardVector.y;
 result[2,2]:=ForwardVector.z;
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function Matrix3x3OuterProduct(const u,v:TKraftVector3):TKraftMatrix3x3;
begin
 result[0,0]:=u.x*v.x;
 result[0,1]:=u.x*v.y;
 result[0,2]:=u.x*v.z;
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=u.y*v.x;
 result[1,1]:=u.y*v.y;
 result[1,2]:=u.y*v.z;
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=u.z*v.x;
 result[2,1]:=u.z*v.y;
 result[2,2]:=u.z*v.z;
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function Matrix4x4Set(m:TKraftMatrix3x3):TKraftMatrix4x4;
begin
 result[0,0]:=m[0,0];
 result[0,1]:=m[0,1];
 result[0,2]:=m[0,2];
 result[0,3]:=0;
 result[1,0]:=m[1,0];
 result[1,1]:=m[1,1];
 result[1,2]:=m[1,2];
 result[1,3]:=0;
 result[2,0]:=m[2,0];
 result[2,1]:=m[2,1];
 result[2,2]:=m[2,2];
 result[2,3]:=0;
 result[3,0]:=0;
 result[3,1]:=0;
 result[3,2]:=0;
 result[3,3]:=1;
end;

function Matrix4x4Rotation(m:TKraftMatrix4x4):TKraftMatrix4x4;
begin
 result[0,0]:=m[0,0];
 result[0,1]:=m[0,1];
 result[0,2]:=m[0,2];
 result[0,3]:=0;
 result[1,0]:=m[1,0];
 result[1,1]:=m[1,1];
 result[1,2]:=m[1,2];
 result[1,3]:=0;
 result[2,0]:=m[2,0];
 result[2,1]:=m[2,1];
 result[2,2]:=m[2,2];
 result[2,3]:=0;
 result[3,0]:=0;
 result[3,1]:=0;
 result[3,2]:=0;
 result[3,3]:=1;
end;

function Matrix4x4RotateX(Angle:TKraftScalar):TKraftMatrix4x4;
begin
 result:=Matrix4x4Identity;
 result[1,1]:=cos(Angle);
 result[2,2]:=result[1,1];
 result[1,2]:=sin(Angle);
 result[2,1]:=-result[1,2];
end;

function Matrix4x4RotateY(Angle:TKraftScalar):TKraftMatrix4x4;
begin
 result:=Matrix4x4Identity;
 result[0,0]:=cos(Angle);
 result[2,2]:=result[0,0];
 result[0,2]:=-sin(Angle);
 result[2,0]:=-result[0,2];
end;

function Matrix4x4RotateZ(Angle:TKraftScalar):TKraftMatrix4x4;
begin
 result:=Matrix4x4Identity;
 result[0,0]:=cos(Angle);
 result[1,1]:=result[0,0];
 result[0,1]:=sin(Angle);
 result[1,0]:=-result[0,1];
end;

function Matrix4x4Rotate(Angle:TKraftScalar;Axis:TKraftVector3):TKraftMatrix4x4; overload;
var m:TKraftMatrix4x4;
    CosinusAngle,SinusAngle:TKraftScalar;
begin
 m:=Matrix4x4Identity;
 CosinusAngle:=cos(Angle);
 SinusAngle:=sin(Angle);    
 m[0,0]:=CosinusAngle+((1-CosinusAngle)*Axis.x*Axis.x);
 m[1,0]:=((1-CosinusAngle)*Axis.x*Axis.y)-(Axis.z*SinusAngle);
 m[2,0]:=((1-CosinusAngle)*Axis.x*Axis.z)+(Axis.y*SinusAngle);
 m[0,1]:=((1-CosinusAngle)*Axis.x*Axis.z)+(Axis.z*SinusAngle);
 m[1,1]:=CosinusAngle+((1-CosinusAngle)*Axis.y*Axis.y);
 m[2,1]:=((1-CosinusAngle)*Axis.y*Axis.z)-(Axis.x*SinusAngle);
 m[0,2]:=((1-CosinusAngle)*Axis.x*Axis.z)-(Axis.y*SinusAngle);
 m[1,2]:=((1-CosinusAngle)*Axis.y*Axis.z)+(Axis.x*SinusAngle);
 m[2,2]:=CosinusAngle+((1-CosinusAngle)*Axis.z*Axis.z);
 result:=m;
end;

function Matrix4x4Translate(x,y,z:TKraftScalar):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix4x4Identity;
 result[3,0]:=x;
 result[3,1]:=y;
 result[3,2]:=z;
end;

function Matrix4x4Translate(const v:TKraftVector3):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix4x4Identity;
 result[3,0]:=v.x;
 result[3,1]:=v.y;
 result[3,2]:=v.z;
end;

function Matrix4x4Translate(const v:TKraftVector4):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix4x4Identity;
 result[3,0]:=v.x;
 result[3,1]:=v.y;
 result[3,2]:=v.z;
end;

procedure Matrix4x4Translate(var m:TKraftMatrix4x4;const v:TKraftVector3); overload; {$ifdef caninline}inline;{$endif}
begin
 m[3,0]:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+m[3,0];
 m[3,1]:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+m[3,1];
 m[3,2]:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+m[3,2];
 m[3,3]:=(m[0,3]*v.x)+(m[1,3]*v.y)+(m[2,3]*v.z)+m[3,3];
end;

procedure Matrix4x4Translate(var m:TKraftMatrix4x4;const v:TKraftVector4); overload; {$ifdef caninline}inline;{$endif}
begin
 m[3,0]:=(m[0,0]*v.x)+(m[1,0]*v.y)+(m[2,0]*v.z)+(m[3,0]*v.w);
 m[3,1]:=(m[0,1]*v.x)+(m[1,1]*v.y)+(m[2,1]*v.z)+(m[3,1]*v.w);
 m[3,2]:=(m[0,2]*v.x)+(m[1,2]*v.y)+(m[2,2]*v.z)+(m[3,2]*v.w);
 m[3,3]:=(m[0,3]*v.x)+(m[1,3]*v.y)+(m[2,3]*v.z)+(m[3,3]*v.w);
end;

function Matrix4x4Scale(sx,sy,sz:TKraftScalar):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix4x4Identity;
 result[0,0]:=sx;
 result[1,1]:=sy;
 result[2,2]:=sz;
end;

function Matrix4x4Scale(const s:TKraftVector3):TKraftMatrix4x4; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix4x4Identity;
 result[0,0]:=s.x;
 result[1,1]:=s.y;
 result[2,2]:=s.z;
end;

procedure Matrix4x4Add(var m1:TKraftMatrix4x4;const m2:TKraftMatrix4x4); {$ifdef caninline}inline;{$endif}
begin
 m1[0,0]:=m1[0,0]+m2[0,0];
 m1[0,1]:=m1[0,1]+m2[0,1];
 m1[0,2]:=m1[0,2]+m2[0,2];
 m1[0,3]:=m1[0,3]+m2[0,3];
 m1[1,0]:=m1[1,0]+m2[1,0];
 m1[1,1]:=m1[1,1]+m2[1,1];
 m1[1,2]:=m1[1,2]+m2[1,2];
 m1[1,3]:=m1[1,3]+m2[1,3];
 m1[2,0]:=m1[2,0]+m2[2,0];
 m1[2,1]:=m1[2,1]+m2[2,1];
 m1[2,2]:=m1[2,2]+m2[2,2];
 m1[2,3]:=m1[2,3]+m2[2,3];
 m1[3,0]:=m1[3,0]+m2[3,0];
 m1[3,1]:=m1[3,1]+m2[3,1];
 m1[3,2]:=m1[3,2]+m2[3,2];
 m1[3,3]:=m1[3,3]+m2[3,3];
end;

procedure Matrix4x4Sub(var m1:TKraftMatrix4x4;const m2:TKraftMatrix4x4); {$ifdef caninline}inline;{$endif}
begin
 m1[0,0]:=m1[0,0]-m2[0,0];
 m1[0,1]:=m1[0,1]-m2[0,1];
 m1[0,2]:=m1[0,2]-m2[0,2];
 m1[0,3]:=m1[0,3]-m2[0,3];
 m1[1,0]:=m1[1,0]-m2[1,0];
 m1[1,1]:=m1[1,1]-m2[1,1];
 m1[1,2]:=m1[1,2]-m2[1,2];
 m1[1,3]:=m1[1,3]-m2[1,3];
 m1[2,0]:=m1[2,0]-m2[2,0];
 m1[2,1]:=m1[2,1]-m2[2,1];
 m1[2,2]:=m1[2,2]-m2[2,2];
 m1[2,3]:=m1[2,3]-m2[2,3];
 m1[3,0]:=m1[3,0]-m2[3,0];
 m1[3,1]:=m1[3,1]-m2[3,1];
 m1[3,2]:=m1[3,2]-m2[3,2];
 m1[3,3]:=m1[3,3]-m2[3,3];
end;

procedure Matrix4x4Mul(var m1:TKraftMatrix4x4;const m2:TKraftMatrix4x4); overload; {$ifdef CPU386ASMForSinglePrecision}register;
asm
 movups xmm0,dqword ptr [m2+0]
 movups xmm1,dqword ptr [m2+16]
 movups xmm2,dqword ptr [m2+32]
 movups xmm3,dqword ptr [m2+48]

 movups xmm7,dqword ptr [m1+0]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [m1+0],xmm4

 movups xmm7,dqword ptr [m1+16]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [m1+16],xmm4

 movups xmm7,dqword ptr [m1+32]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [m1+32],xmm4

 movups xmm7,dqword ptr [m1+48]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [m1+48],xmm4

end;
{$else}
var t:TKraftMatrix4x4;
begin
 t[0,0]:=(m1[0,0]*m2[0,0])+(m1[0,1]*m2[1,0])+(m1[0,2]*m2[2,0])+(m1[0,3]*m2[3,0]);
 t[0,1]:=(m1[0,0]*m2[0,1])+(m1[0,1]*m2[1,1])+(m1[0,2]*m2[2,1])+(m1[0,3]*m2[3,1]);
 t[0,2]:=(m1[0,0]*m2[0,2])+(m1[0,1]*m2[1,2])+(m1[0,2]*m2[2,2])+(m1[0,3]*m2[3,2]);
 t[0,3]:=(m1[0,0]*m2[0,3])+(m1[0,1]*m2[1,3])+(m1[0,2]*m2[2,3])+(m1[0,3]*m2[3,3]);
 t[1,0]:=(m1[1,0]*m2[0,0])+(m1[1,1]*m2[1,0])+(m1[1,2]*m2[2,0])+(m1[1,3]*m2[3,0]);
 t[1,1]:=(m1[1,0]*m2[0,1])+(m1[1,1]*m2[1,1])+(m1[1,2]*m2[2,1])+(m1[1,3]*m2[3,1]);
 t[1,2]:=(m1[1,0]*m2[0,2])+(m1[1,1]*m2[1,2])+(m1[1,2]*m2[2,2])+(m1[1,3]*m2[3,2]);
 t[1,3]:=(m1[1,0]*m2[0,3])+(m1[1,1]*m2[1,3])+(m1[1,2]*m2[2,3])+(m1[1,3]*m2[3,3]);
 t[2,0]:=(m1[2,0]*m2[0,0])+(m1[2,1]*m2[1,0])+(m1[2,2]*m2[2,0])+(m1[2,3]*m2[3,0]);
 t[2,1]:=(m1[2,0]*m2[0,1])+(m1[2,1]*m2[1,1])+(m1[2,2]*m2[2,1])+(m1[2,3]*m2[3,1]);
 t[2,2]:=(m1[2,0]*m2[0,2])+(m1[2,1]*m2[1,2])+(m1[2,2]*m2[2,2])+(m1[2,3]*m2[3,2]);
 t[2,3]:=(m1[2,0]*m2[0,3])+(m1[2,1]*m2[1,3])+(m1[2,2]*m2[2,3])+(m1[2,3]*m2[3,3]);
 t[3,0]:=(m1[3,0]*m2[0,0])+(m1[3,1]*m2[1,0])+(m1[3,2]*m2[2,0])+(m1[3,3]*m2[3,0]);
 t[3,1]:=(m1[3,0]*m2[0,1])+(m1[3,1]*m2[1,1])+(m1[3,2]*m2[2,1])+(m1[3,3]*m2[3,1]);
 t[3,2]:=(m1[3,0]*m2[0,2])+(m1[3,1]*m2[1,2])+(m1[3,2]*m2[2,2])+(m1[3,3]*m2[3,2]);
 t[3,3]:=(m1[3,0]*m2[0,3])+(m1[3,1]*m2[1,3])+(m1[3,2]*m2[2,3])+(m1[3,3]*m2[3,3]);
 m1:=t;
end;
{$endif}

procedure Matrix4x4Mul(var mr:TKraftMatrix4x4;const m1,m2:TKraftMatrix4x4); overload; {$ifdef CPU386ASMForSinglePrecision}register;
asm

 movups xmm0,dqword ptr [m2+0]
 movups xmm1,dqword ptr [m2+16]
 movups xmm2,dqword ptr [m2+32]
 movups xmm3,dqword ptr [m2+48]

 movups xmm7,dqword ptr [m1+0]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [mr+0],xmm4

 movups xmm7,dqword ptr [m1+16]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [mr+16],xmm4

 movups xmm7,dqword ptr [m1+32]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [mr+32],xmm4

 movups xmm7,dqword ptr [m1+48]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [mr+48],xmm4

end;
{$else}
begin
 mr[0,0]:=(m1[0,0]*m2[0,0])+(m1[0,1]*m2[1,0])+(m1[0,2]*m2[2,0])+(m1[0,3]*m2[3,0]);
 mr[0,1]:=(m1[0,0]*m2[0,1])+(m1[0,1]*m2[1,1])+(m1[0,2]*m2[2,1])+(m1[0,3]*m2[3,1]);
 mr[0,2]:=(m1[0,0]*m2[0,2])+(m1[0,1]*m2[1,2])+(m1[0,2]*m2[2,2])+(m1[0,3]*m2[3,2]);
 mr[0,3]:=(m1[0,0]*m2[0,3])+(m1[0,1]*m2[1,3])+(m1[0,2]*m2[2,3])+(m1[0,3]*m2[3,3]);
 mr[1,0]:=(m1[1,0]*m2[0,0])+(m1[1,1]*m2[1,0])+(m1[1,2]*m2[2,0])+(m1[1,3]*m2[3,0]);
 mr[1,1]:=(m1[1,0]*m2[0,1])+(m1[1,1]*m2[1,1])+(m1[1,2]*m2[2,1])+(m1[1,3]*m2[3,1]);
 mr[1,2]:=(m1[1,0]*m2[0,2])+(m1[1,1]*m2[1,2])+(m1[1,2]*m2[2,2])+(m1[1,3]*m2[3,2]);
 mr[1,3]:=(m1[1,0]*m2[0,3])+(m1[1,1]*m2[1,3])+(m1[1,2]*m2[2,3])+(m1[1,3]*m2[3,3]);
 mr[2,0]:=(m1[2,0]*m2[0,0])+(m1[2,1]*m2[1,0])+(m1[2,2]*m2[2,0])+(m1[2,3]*m2[3,0]);
 mr[2,1]:=(m1[2,0]*m2[0,1])+(m1[2,1]*m2[1,1])+(m1[2,2]*m2[2,1])+(m1[2,3]*m2[3,1]);
 mr[2,2]:=(m1[2,0]*m2[0,2])+(m1[2,1]*m2[1,2])+(m1[2,2]*m2[2,2])+(m1[2,3]*m2[3,2]);
 mr[2,3]:=(m1[2,0]*m2[0,3])+(m1[2,1]*m2[1,3])+(m1[2,2]*m2[2,3])+(m1[2,3]*m2[3,3]);
 mr[3,0]:=(m1[3,0]*m2[0,0])+(m1[3,1]*m2[1,0])+(m1[3,2]*m2[2,0])+(m1[3,3]*m2[3,0]);
 mr[3,1]:=(m1[3,0]*m2[0,1])+(m1[3,1]*m2[1,1])+(m1[3,2]*m2[2,1])+(m1[3,3]*m2[3,1]);
 mr[3,2]:=(m1[3,0]*m2[0,2])+(m1[3,1]*m2[1,2])+(m1[3,2]*m2[2,2])+(m1[3,3]*m2[3,2]);
 mr[3,3]:=(m1[3,0]*m2[0,3])+(m1[3,1]*m2[1,3])+(m1[3,2]*m2[2,3])+(m1[3,3]*m2[3,3]);
end;
{$endif}

function Matrix4x4TermAdd(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=m1[0,0]+m2[0,0];
 result[0,1]:=m1[0,1]+m2[0,1];
 result[0,2]:=m1[0,2]+m2[0,2];
 result[0,3]:=m1[0,3]+m2[0,3];
 result[1,0]:=m1[1,0]+m2[1,0];
 result[1,1]:=m1[1,1]+m2[1,1];
 result[1,2]:=m1[1,2]+m2[1,2];
 result[1,3]:=m1[1,3]+m2[1,3];
 result[2,0]:=m1[2,0]+m2[2,0];
 result[2,1]:=m1[2,1]+m2[2,1];
 result[2,2]:=m1[2,2]+m2[2,2];
 result[2,3]:=m1[2,3]+m2[2,3];
 result[3,0]:=m1[3,0]+m2[3,0];
 result[3,1]:=m1[3,1]+m2[3,1];
 result[3,2]:=m1[3,2]+m2[3,2];
 result[3,3]:=m1[3,3]+m2[3,3];
end;

function Matrix4x4TermSub(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=m1[0,0]-m2[0,0];
 result[0,1]:=m1[0,1]-m2[0,1];
 result[0,2]:=m1[0,2]-m2[0,2];
 result[0,3]:=m1[0,3]-m2[0,3];
 result[1,0]:=m1[1,0]-m2[1,0];
 result[1,1]:=m1[1,1]-m2[1,1];
 result[1,2]:=m1[1,2]-m2[1,2];
 result[1,3]:=m1[1,3]-m2[1,3];
 result[2,0]:=m1[2,0]-m2[2,0];
 result[2,1]:=m1[2,1]-m2[2,1];
 result[2,2]:=m1[2,2]-m2[2,2];
 result[2,3]:=m1[2,3]-m2[2,3];
 result[3,0]:=m1[3,0]-m2[3,0];
 result[3,1]:=m1[3,1]-m2[3,1];
 result[3,2]:=m1[3,2]-m2[3,2];
 result[3,3]:=m1[3,3]-m2[3,3];
end;

function Matrix4x4TermMul(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef CPU386ASMForSinglePrecision}register;
asm

 movups xmm0,dqword ptr [m2+0]
 movups xmm1,dqword ptr [m2+16]
 movups xmm2,dqword ptr [m2+32]
 movups xmm3,dqword ptr [m2+48]

 movups xmm7,dqword ptr [m1+0]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [result+0],xmm4

 movups xmm7,dqword ptr [m1+16]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [result+16],xmm4

 movups xmm7,dqword ptr [m1+32]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [result+32],xmm4

 movups xmm7,dqword ptr [m1+48]
 pshufd xmm4,xmm7,$00
 pshufd xmm5,xmm7,$55
 pshufd xmm6,xmm7,$aa
 pshufd xmm7,xmm7,$ff
 mulps xmm4,xmm0
 mulps xmm5,xmm1
 mulps xmm6,xmm2
 mulps xmm7,xmm3
 addps xmm4,xmm5
 addps xmm6,xmm7
 addps xmm4,xmm6
 movups dqword ptr [result+48],xmm4

end;
{$else}
begin
 result[0,0]:=(m1[0,0]*m2[0,0])+(m1[0,1]*m2[1,0])+(m1[0,2]*m2[2,0])+(m1[0,3]*m2[3,0]);
 result[0,1]:=(m1[0,0]*m2[0,1])+(m1[0,1]*m2[1,1])+(m1[0,2]*m2[2,1])+(m1[0,3]*m2[3,1]);
 result[0,2]:=(m1[0,0]*m2[0,2])+(m1[0,1]*m2[1,2])+(m1[0,2]*m2[2,2])+(m1[0,3]*m2[3,2]);
 result[0,3]:=(m1[0,0]*m2[0,3])+(m1[0,1]*m2[1,3])+(m1[0,2]*m2[2,3])+(m1[0,3]*m2[3,3]);
 result[1,0]:=(m1[1,0]*m2[0,0])+(m1[1,1]*m2[1,0])+(m1[1,2]*m2[2,0])+(m1[1,3]*m2[3,0]);
 result[1,1]:=(m1[1,0]*m2[0,1])+(m1[1,1]*m2[1,1])+(m1[1,2]*m2[2,1])+(m1[1,3]*m2[3,1]);
 result[1,2]:=(m1[1,0]*m2[0,2])+(m1[1,1]*m2[1,2])+(m1[1,2]*m2[2,2])+(m1[1,3]*m2[3,2]);
 result[1,3]:=(m1[1,0]*m2[0,3])+(m1[1,1]*m2[1,3])+(m1[1,2]*m2[2,3])+(m1[1,3]*m2[3,3]);
 result[2,0]:=(m1[2,0]*m2[0,0])+(m1[2,1]*m2[1,0])+(m1[2,2]*m2[2,0])+(m1[2,3]*m2[3,0]);
 result[2,1]:=(m1[2,0]*m2[0,1])+(m1[2,1]*m2[1,1])+(m1[2,2]*m2[2,1])+(m1[2,3]*m2[3,1]);
 result[2,2]:=(m1[2,0]*m2[0,2])+(m1[2,1]*m2[1,2])+(m1[2,2]*m2[2,2])+(m1[2,3]*m2[3,2]);
 result[2,3]:=(m1[2,0]*m2[0,3])+(m1[2,1]*m2[1,3])+(m1[2,2]*m2[2,3])+(m1[2,3]*m2[3,3]);
 result[3,0]:=(m1[3,0]*m2[0,0])+(m1[3,1]*m2[1,0])+(m1[3,2]*m2[2,0])+(m1[3,3]*m2[3,0]);
 result[3,1]:=(m1[3,0]*m2[0,1])+(m1[3,1]*m2[1,1])+(m1[3,2]*m2[2,1])+(m1[3,3]*m2[3,1]);
 result[3,2]:=(m1[3,0]*m2[0,2])+(m1[3,1]*m2[1,2])+(m1[3,2]*m2[2,2])+(m1[3,3]*m2[3,2]);
 result[3,3]:=(m1[3,0]*m2[0,3])+(m1[3,1]*m2[1,3])+(m1[3,2]*m2[2,3])+(m1[3,3]*m2[3,3]);
end;
{$endif}

function Matrix4x4TermMulInverted(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix4x4TermMul(m1,Matrix4x4TermInverse(m2));
end;

function Matrix4x4TermMulSimpleInverted(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix4x4TermMul(m1,Matrix4x4TermSimpleInverse(m2));
end;

function Matrix4x4TermMulTranspose(const m1,m2:TKraftMatrix4x4):TKraftMatrix4x4;
begin
 result[0,0]:=(m1[0,0]*m2[0,0])+(m1[0,1]*m2[1,0])+(m1[0,2]*m2[2,0])+(m1[0,3]*m2[3,0]);
 result[1,0]:=(m1[0,0]*m2[0,1])+(m1[0,1]*m2[1,1])+(m1[0,2]*m2[2,1])+(m1[0,3]*m2[3,1]);
 result[2,0]:=(m1[0,0]*m2[0,2])+(m1[0,1]*m2[1,2])+(m1[0,2]*m2[2,2])+(m1[0,3]*m2[3,2]);
 result[3,0]:=(m1[0,0]*m2[0,3])+(m1[0,1]*m2[1,3])+(m1[0,2]*m2[2,3])+(m1[0,3]*m2[3,3]);
 result[0,1]:=(m1[1,0]*m2[0,0])+(m1[1,1]*m2[1,0])+(m1[1,2]*m2[2,0])+(m1[1,3]*m2[3,0]);
 result[1,1]:=(m1[1,0]*m2[0,1])+(m1[1,1]*m2[1,1])+(m1[1,2]*m2[2,1])+(m1[1,3]*m2[3,1]);
 result[2,1]:=(m1[1,0]*m2[0,2])+(m1[1,1]*m2[1,2])+(m1[1,2]*m2[2,2])+(m1[1,3]*m2[3,2]);
 result[3,1]:=(m1[1,0]*m2[0,3])+(m1[1,1]*m2[1,3])+(m1[1,2]*m2[2,3])+(m1[1,3]*m2[3,3]);
 result[0,2]:=(m1[2,0]*m2[0,0])+(m1[2,1]*m2[1,0])+(m1[2,2]*m2[2,0])+(m1[2,3]*m2[3,0]);
 result[1,2]:=(m1[2,0]*m2[0,1])+(m1[2,1]*m2[1,1])+(m1[2,2]*m2[2,1])+(m1[2,3]*m2[3,1]);
 result[2,2]:=(m1[2,0]*m2[0,2])+(m1[2,1]*m2[1,2])+(m1[2,2]*m2[2,2])+(m1[2,3]*m2[3,2]);
 result[3,2]:=(m1[2,0]*m2[0,3])+(m1[2,1]*m2[1,3])+(m1[2,2]*m2[2,3])+(m1[2,3]*m2[3,3]);
 result[0,3]:=(m1[3,0]*m2[0,0])+(m1[3,1]*m2[1,0])+(m1[3,2]*m2[2,0])+(m1[3,3]*m2[3,0]);
 result[1,3]:=(m1[3,0]*m2[0,1])+(m1[3,1]*m2[1,1])+(m1[3,2]*m2[2,1])+(m1[3,3]*m2[3,1]);
 result[2,3]:=(m1[3,0]*m2[0,2])+(m1[3,1]*m2[1,2])+(m1[3,2]*m2[2,2])+(m1[3,3]*m2[3,2]);
 result[3,3]:=(m1[3,0]*m2[0,3])+(m1[3,1]*m2[1,3])+(m1[3,2]*m2[2,3])+(m1[3,3]*m2[3,3]);
end;

function Matrix4x4Lerp(const a,b:TKraftMatrix4x4;x:TKraftScalar):TKraftMatrix4x4;
var ix:TKraftScalar;
begin
 if x<=0.0 then begin
  result:=a;
 end else if x>=1.0 then begin
  result:=b;
 end else begin
  ix:=1.0-x;
  result[0,0]:=(a[0,0]*ix)+(b[0,0]*x);
  result[0,1]:=(a[0,1]*ix)+(b[0,1]*x);
  result[0,2]:=(a[0,2]*ix)+(b[0,2]*x);
  result[0,3]:=(a[0,3]*ix)+(b[0,3]*x);
  result[1,0]:=(a[1,0]*ix)+(b[1,0]*x);
  result[1,1]:=(a[1,1]*ix)+(b[1,1]*x);
  result[1,2]:=(a[1,2]*ix)+(b[1,2]*x);
  result[1,3]:=(a[1,3]*ix)+(b[1,3]*x);
  result[2,0]:=(a[2,0]*ix)+(b[2,0]*x);
  result[2,1]:=(a[2,1]*ix)+(b[2,1]*x);
  result[2,2]:=(a[2,2]*ix)+(b[2,2]*x);
  result[2,3]:=(a[2,3]*ix)+(b[2,3]*x);
  result[3,0]:=(a[3,0]*ix)+(b[3,0]*x);
  result[3,1]:=(a[3,1]*ix)+(b[3,1]*x);
  result[3,2]:=(a[3,2]*ix)+(b[3,2]*x);
  result[3,3]:=(a[3,3]*ix)+(b[3,3]*x);
 end;
end;

function Matrix4x4Slerp(const a,b:TKraftMatrix4x4;x:TKraftScalar):TKraftMatrix4x4;
var ix:TKraftScalar;
    m:TKraftMatrix3x3;
begin
 if x<=0.0 then begin
  result:=a;
 end else if x>=1.0 then begin
  result:=b;
 end else begin
  m:=QuaternionToMatrix3x3(QuaternionSlerp(QuaternionFromMatrix4x4(a),QuaternionFromMatrix4x4(b),x));
  ix:=1.0-x;
  result[0,0]:=m[0,0];
  result[0,1]:=m[0,1];
  result[0,2]:=m[0,2];
  result[0,3]:=(a[0,3]*ix)+(b[0,3]*x);
  result[1,0]:=m[1,0];
  result[1,1]:=m[1,1];
  result[1,2]:=m[1,2];
  result[1,3]:=(a[1,3]*ix)+(b[1,3]*x);
  result[2,0]:=m[2,0];
  result[2,1]:=m[2,1];
  result[2,2]:=m[2,2];
  result[2,3]:=(a[2,3]*ix)+(b[2,3]*x);
  result[3,0]:=(a[3,0]*ix)+(b[3,0]*x);
  result[3,1]:=(a[3,1]*ix)+(b[3,1]*x);
  result[3,2]:=(a[3,2]*ix)+(b[3,2]*x);
  result[3,3]:=(a[3,3]*ix)+(b[3,3]*x);
 end;
end;

procedure Matrix4x4ScalarMul(var m:TKraftMatrix4x4;s:TKraftScalar); {$ifdef caninline}inline;{$endif}
begin
 m[0,0]:=m[0,0]*s;
 m[0,1]:=m[0,1]*s;
 m[0,2]:=m[0,2]*s;
 m[0,3]:=m[0,3]*s;
 m[1,0]:=m[1,0]*s;
 m[1,1]:=m[1,1]*s;
 m[1,2]:=m[1,2]*s;
 m[1,3]:=m[1,3]*s;
 m[2,0]:=m[2,0]*s;
 m[2,1]:=m[2,1]*s;
 m[2,2]:=m[2,2]*s;
 m[2,3]:=m[2,3]*s;
 m[3,0]:=m[3,0]*s;
 m[3,1]:=m[3,1]*s;
 m[3,2]:=m[3,2]*s;
 m[3,3]:=m[3,3]*s;
end;

procedure Matrix4x4Transpose(var m:TKraftMatrix4x4);
{$ifdef CPU386ASMForSinglePrecision}
asm
 movups xmm0,dqword ptr [m+0]
 movups xmm4,dqword ptr [m+16]
 movups xmm2,dqword ptr [m+32]
 movups xmm5,dqword ptr [m+48]
 movaps xmm1,xmm0
 movaps xmm3,xmm2
 unpcklps xmm0,xmm4
 unpckhps xmm1,xmm4
 unpcklps xmm2,xmm5
 unpckhps xmm3,xmm5
 movaps xmm4,xmm0
 movaps xmm6,xmm1
 shufps xmm0,xmm2,$44 // 01000100b
 shufps xmm4,xmm2,$ee // 11101110b
 shufps xmm1,xmm3,$44 // 01000100b
 shufps xmm6,xmm3,$ee // 11101110b
 movups dqword ptr [m+0],xmm0
 movups dqword ptr [m+16],xmm4
 movups dqword ptr [m+32],xmm1
 movups dqword ptr [m+48],xmm6
end;
{$else}
var mt:TKraftMatrix4x4;
begin
 mt[0,0]:=m[0,0];
 mt[0,1]:=m[1,0];
 mt[0,2]:=m[2,0];
 mt[0,3]:=m[3,0];
 mt[1,0]:=m[0,1];
 mt[1,1]:=m[1,1];
 mt[1,2]:=m[2,1];
 mt[1,3]:=m[3,1];
 mt[2,0]:=m[0,2];
 mt[2,1]:=m[1,2];
 mt[2,2]:=m[2,2];
 mt[2,3]:=m[3,2];
 mt[3,0]:=m[0,3];
 mt[3,1]:=m[1,3];
 mt[3,2]:=m[2,3];
 mt[3,3]:=m[3,3];
 m:=mt;
end;
{$endif}

function Matrix4x4TermTranspose(const m:TKraftMatrix4x4):TKraftMatrix4x4;
{$ifdef CPU386ASMForSinglePrecision}
asm
 movups xmm0,dqword ptr [m+0]
 movups xmm4,dqword ptr [m+16]
 movups xmm2,dqword ptr [m+32]
 movups xmm5,dqword ptr [m+48]
 movaps xmm1,xmm0
 movaps xmm3,xmm2
 unpcklps xmm0,xmm4
 unpckhps xmm1,xmm4
 unpcklps xmm2,xmm5
 unpckhps xmm3,xmm5
 movaps xmm4,xmm0
 movaps xmm6,xmm1
 shufps xmm0,xmm2,$44 // 01000100b
 shufps xmm4,xmm2,$ee // 11101110b
 shufps xmm1,xmm3,$44 // 01000100b
 shufps xmm6,xmm3,$ee // 11101110b
 movups dqword ptr [result+0],xmm0
 movups dqword ptr [result+16],xmm4
 movups dqword ptr [result+32],xmm1
 movups dqword ptr [result+48],xmm6
end;
{$else}
begin
 result[0,0]:=m[0,0];
 result[0,1]:=m[1,0];
 result[0,2]:=m[2,0];
 result[0,3]:=m[3,0];
 result[1,0]:=m[0,1];
 result[1,1]:=m[1,1];
 result[1,2]:=m[2,1];
 result[1,3]:=m[3,1];
 result[2,0]:=m[0,2];
 result[2,1]:=m[1,2];
 result[2,2]:=m[2,2];
 result[2,3]:=m[3,2];
 result[3,0]:=m[0,3];
 result[3,1]:=m[1,3];
 result[3,2]:=m[2,3];
 result[3,3]:=m[3,3];
end;
{$endif}

function Matrix4x4Determinant(const m:TKraftMatrix4x4):TKraftScalar;
{$ifdef CPU386ASMForSinglePrecision}
asm
 movups xmm0,dqword ptr [m+32]
 movups xmm1,dqword ptr [m+48]
 movups xmm2,dqword ptr [m+16]
 movaps xmm3,xmm0
 movaps xmm4,xmm0
 movaps xmm6,xmm1
 movaps xmm7,xmm2
 shufps xmm0,xmm0,$1b // 00011011b
 shufps xmm1,xmm1,$b1 // 10110001b
 shufps xmm2,xmm2,$4e // 01001110b
 shufps xmm7,xmm7,$39 // 00111001b
 mulps xmm0,xmm1
 shufps xmm3,xmm3,$7d // 01111101b
 shufps xmm6,xmm6,$0a // 00001010b
 movaps xmm5,xmm0
 shufps xmm0,xmm0,$4e // 01001110b
 shufps xmm4,xmm4,$0a // 00001010b
 shufps xmm1,xmm1,$28 // 00101000b
 subps xmm5,xmm0
 mulps xmm3,xmm6
 mulps xmm4,xmm1
 mulps xmm5,xmm2
 shufps xmm2,xmm2,$39 // 00111001b
 subps xmm3,xmm4
 movaps xmm0,xmm3
 shufps xmm0,xmm0,$39 // 00111001b
 mulps xmm3,xmm2
 mulps xmm0,xmm7
 addps xmm5,xmm3
 subps xmm5,xmm0
 movups xmm6,dqword ptr [m+0]
 mulps xmm5,xmm6
 movhlps xmm7,xmm5
 addps xmm5,xmm7
 movaps xmm6,xmm5
 shufps xmm6,xmm6,$01
 addss xmm5,xmm6
 movss dword ptr [result],xmm5
end;
{$else}
var inv:array[0..15] of TKraftScalar;
begin
 inv[0]:=(((m[1,1]*m[2,2]*m[3,3])-(m[1,1]*m[2,3]*m[3,2]))-(m[2,1]*m[1,2]*m[3,3])+(m[2,1]*m[1,3]*m[3,2])+(m[3,1]*m[1,2]*m[2,3]))-(m[3,1]*m[1,3]*m[2,2]);
 inv[4]:=((((-(m[1,0]*m[2,2]*m[3,3]))+(m[1,0]*m[2,3]*m[3,2])+(m[2,0]*m[1,2]*m[3,3]))-(m[2,0]*m[1,3]*m[3,2]))-(m[3,0]*m[1,2]*m[2,3]))+(m[3,0]*m[1,3]*m[2,2]);
 inv[8]:=((((m[1,0]*m[2,1]*m[3,3])-(m[1,0]*m[2,3]*m[3,1]))-(m[2,0]*m[1,1]*m[3,3]))+(m[2,0]*m[1,3]*m[3,1])+(m[3,0]*m[1,1]*m[2,3]))-(m[3,0]*m[1,3]*m[2,1]);
 inv[12]:=((((-(m[1,0]*m[2,1]*m[3,2]))+(m[1,0]*m[2,2]*m[3,1])+(m[2,0]*m[1,1]*m[3,2]))-(m[2,0]*m[1,2]*m[3,1]))-(m[3,0]*m[1,1]*m[2,2]))+(m[3,0]*m[1,2]*m[2,1]);
 inv[1]:=((((-(m[0,1]*m[2,2]*m[3,3]))+(m[0,1]*m[2,3]*m[3,2])+(m[2,1]*m[0,2]*m[3,3]))-(m[2,1]*m[0,3]*m[3,2]))-(m[3,1]*m[0,2]*m[2,3]))+(m[3,1]*m[0,3]*m[2,2]);
 inv[5]:=(((m[0,0]*m[2,2]*m[3,3])-(m[0,0]*m[2,3]*m[3,2]))-(m[2,0]*m[0,2]*m[3,3])+(m[2,0]*m[0,3]*m[3,2])+(m[3,0]*m[0,2]*m[2,3]))-(m[3,0]*m[0,3]*m[2,2]);
 inv[9]:=((((-(m[0,0]*m[2,1]*m[3,3]))+(m[0,0]*m[2,3]*m[3,1])+(m[2,0]*m[0,1]*m[3,3]))-(m[2,0]*m[0,3]*m[3,1]))-(m[3,0]*m[0,1]*m[2,3]))+(m[3,0]*m[0,3]*m[2,1]);
 inv[13]:=((((m[0,0]*m[2,1]*m[3,2])-(m[0,0]*m[2,2]*m[3,1]))-(m[2,0]*m[0,1]*m[3,2]))+(m[2,0]*m[0,2]*m[3,1])+(m[3,0]*m[0,1]*m[2,2]))-(m[3,0]*m[0,2]*m[2,1]);
 inv[2]:=((((m[0,1]*m[1,2]*m[3,3])-(m[0,1]*m[1,3]*m[3,2]))-(m[1,1]*m[0,2]*m[3,3]))+(m[1,1]*m[0,3]*m[3,2])+(m[3,1]*m[0,2]*m[1,3]))-(m[3,1]*m[0,3]*m[1,2]);
 inv[6]:=((((-(m[0,0]*m[1,2]*m[3,3]))+(m[0,0]*m[1,3]*m[3,2])+(m[1,0]*m[0,2]*m[3,3]))-(m[1,0]*m[0,3]*m[3,2]))-(m[3,0]*m[0,2]*m[1,3]))+(m[3,0]*m[0,3]*m[1,2]);
 inv[10]:=((((m[0,0]*m[1,1]*m[3,3])-(m[0,0]*m[1,3]*m[3,1]))-(m[1,0]*m[0,1]*m[3,3]))+(m[1,0]*m[0,3]*m[3,1])+(m[3,0]*m[0,1]*m[1,3]))-(m[3,0]*m[0,3]*m[1,1]);
 inv[14]:=((((-(m[0,0]*m[1,1]*m[3,2]))+(m[0,0]*m[1,2]*m[3,1])+(m[1,0]*m[0,1]*m[3,2]))-(m[1,0]*m[0,2]*m[3,1]))-(m[3,0]*m[0,1]*m[1,2]))+(m[3,0]*m[0,2]*m[1,1]);
 inv[3]:=((((-(m[0,1]*m[1,2]*m[2,3]))+(m[0,1]*m[1,3]*m[2,2])+(m[1,1]*m[0,2]*m[2,3]))-(m[1,1]*m[0,3]*m[2,2]))-(m[2,1]*m[0,2]*m[1,3]))+(m[2,1]*m[0,3]*m[1,2]);
 inv[7]:=((((m[0,0]*m[1,2]*m[2,3])-(m[0,0]*m[1,3]*m[2,2]))-(m[1,0]*m[0,2]*m[2,3]))+(m[1,0]*m[0,3]*m[2,2])+(m[2,0]*m[0,2]*m[1,3]))-(m[2,0]*m[0,3]*m[1,2]);
 inv[11]:=((((-(m[0,0]*m[1,1]*m[2,3]))+(m[0,0]*m[1,3]*m[2,1])+(m[1,0]*m[0,1]*m[2,3]))-(m[1,0]*m[0,3]*m[2,1]))-(m[2,0]*m[0,1]*m[1,3]))+(m[2,0]*m[0,3]*m[1,1]);
 inv[15]:=((((m[0,0]*m[1,1]*m[2,2])-(m[0,0]*m[1,2]*m[2,1]))-(m[1,0]*m[0,1]*m[2,2]))+(m[1,0]*m[0,2]*m[2,1])+(m[2,0]*m[0,1]*m[1,2]))-(m[2,0]*m[0,2]*m[1,1]);
 result:=(m[0,0]*inv[0])+(m[0,1]*inv[4])+(m[0,2]*inv[8])+(m[0,3]*inv[12]);
end;
{$endif}

procedure Matrix4x4SetColumn(var m:TKraftMatrix4x4;const c:longint;const v:TKraftVector4); {$ifdef caninline}inline;{$endif}
begin
 m[c,0]:=v.x;
 m[c,1]:=v.y;
 m[c,2]:=v.z;
 m[c,3]:=v.w;
end;

function Matrix4x4GetColumn(const m:TKraftMatrix4x4;const c:longint):TKraftVector4; {$ifdef caninline}inline;{$endif}
begin
 result.x:=m[c,0];
 result.y:=m[c,1];
 result.z:=m[c,2];
 result.w:=m[c,3];
end;

procedure Matrix4x4SetRow(var m:TKraftMatrix4x4;const r:longint;const v:TKraftVector4); {$ifdef caninline}inline;{$endif}
begin
 m[0,r]:=v.x;
 m[1,r]:=v.y;
 m[2,r]:=v.z;
 m[3,r]:=v.w;
end;

function Matrix4x4GetRow(const m:TKraftMatrix4x4;const r:longint):TKraftVector4; {$ifdef caninline}inline;{$endif}
begin
 result.x:=m[0,r];
 result.y:=m[1,r];
 result.z:=m[2,r];
 result.w:=m[3,r];
end;

function Matrix4x4Compare(const m1,m2:TKraftMatrix4x4):boolean;
var r,c:longint;
begin
 result:=true;
 for r:=0 to 3 do begin
  for c:=0 to 3 do begin
   if abs(m1[r,c]-m2[r,c])>EPSILON then begin
    result:=false;
    exit;
   end;
  end;
 end;
end;
                      
function Matrix4x4LengthSquared(const m:TKraftMatrix4x4):TKraftScalar;
begin
 result:=Vector4LengthSquared(PKraftVector4(pointer(@m[0,0]))^)+
         Vector4LengthSquared(PKraftVector4(pointer(@m[1,0]))^)+
         Vector4LengthSquared(PKraftVector4(pointer(@m[2,0]))^)+
         Vector4LengthSquared(PKraftVector4(pointer(@m[3,0]))^);
end;

function Matrix4x4Length(const m:TKraftMatrix4x4):TKraftScalar;
begin
 result:=Matrix4x4LengthSquared(m);
 if result>0.0 then begin
  result:=sqrt(result);
 end else begin
  result:=0.0;
 end;
end;

function Matrix4x4DifferenceSquared(const m1,m2:TKraftMatrix4x4):TKraftScalar;
begin
 result:=Matrix4x4LengthSquared(Matrix4x4TermSub(m2,m1));
end;

function Matrix4x4Difference(const m1,m2:TKraftMatrix4x4):TKraftScalar;
begin
 result:=Matrix4x4Length(Matrix4x4TermSub(m2,m1));
end;

procedure Matrix4x4Reflect(var mr:TKraftMatrix4x4;Plane:TKraftPlane);
begin
 PlaneNormalize(Plane);
 mr[0,0]:=1.0-(2.0*(Plane.Normal.x*Plane.Normal.x));
 mr[0,1]:=-(2.0*(Plane.Normal.x*Plane.Normal.y));
 mr[0,2]:=-(2.0*(Plane.Normal.x*Plane.Normal.z));
 mr[0,3]:=0.0;
 mr[1,0]:=-(2.0*(Plane.Normal.x*Plane.Normal.y));
 mr[1,1]:=1.0-(2.0*(Plane.Normal.y*Plane.Normal.y));
 mr[1,2]:=-(2.0*(Plane.Normal.y*Plane.Normal.z));
 mr[1,3]:=0.0;
 mr[2,0]:=-(2.0*(Plane.Normal.z*Plane.Normal.x));
 mr[2,1]:=-(2.0*(Plane.Normal.z*Plane.Normal.y));
 mr[2,2]:=1.0-(2.0*(Plane.Normal.z*Plane.Normal.z));
 mr[2,3]:=0.0;
 mr[3,0]:=-(2.0*(Plane.Distance*Plane.Normal.x));
 mr[3,1]:=-(2.0*(Plane.Distance*Plane.Normal.y));
 mr[3,2]:=-(2.0*(Plane.Distance*Plane.Normal.z));
 mr[3,3]:=1.0;
end;

function Matrix4x4TermReflect(Plane:TKraftPlane):TKraftMatrix4x4;
begin
 PlaneNormalize(Plane);
 result[0,0]:=1.0-(2.0*(Plane.Normal.x*Plane.Normal.x));
 result[0,1]:=-(2.0*(Plane.Normal.x*Plane.Normal.y));
 result[0,2]:=-(2.0*(Plane.Normal.x*Plane.Normal.z));
 result[0,3]:=0.0;
 result[1,0]:=-(2.0*(Plane.Normal.x*Plane.Normal.y));
 result[1,1]:=1.0-(2.0*(Plane.Normal.y*Plane.Normal.y));
 result[1,2]:=-(2.0*(Plane.Normal.y*Plane.Normal.z));
 result[1,3]:=0.0;
 result[2,0]:=-(2.0*(Plane.Normal.z*Plane.Normal.x));
 result[2,1]:=-(2.0*(Plane.Normal.z*Plane.Normal.y));
 result[2,2]:=1.0-(2.0*(Plane.Normal.z*Plane.Normal.z));
 result[2,3]:=0.0;
 result[3,0]:=-(2.0*(Plane.Distance*Plane.Normal.x));
 result[3,1]:=-(2.0*(Plane.Distance*Plane.Normal.y));
 result[3,2]:=-(2.0*(Plane.Distance*Plane.Normal.z));
 result[3,3]:=1.0;
end;

function Matrix4x4SimpleInverse(var mr:TKraftMatrix4x4;const ma:TKraftMatrix4x4):boolean; {$ifdef caninline}inline;{$endif}
begin
 mr[0,0]:=ma[0,0];
 mr[0,1]:=ma[1,0];
 mr[0,2]:=ma[2,0];
 mr[0,3]:=ma[0,3];
 mr[1,0]:=ma[0,1];
 mr[1,1]:=ma[1,1];
 mr[1,2]:=ma[2,1];
 mr[1,3]:=ma[1,3];
 mr[2,0]:=ma[0,2];
 mr[2,1]:=ma[1,2];
 mr[2,2]:=ma[2,2];
 mr[2,3]:=ma[2,3];
 mr[3,0]:=-Vector3Dot(PKraftVector3(pointer(@ma[3,0]))^,Vector3(ma[0,0],ma[0,1],ma[0,2]));
 mr[3,1]:=-Vector3Dot(PKraftVector3(pointer(@ma[3,0]))^,Vector3(ma[1,0],ma[1,1],ma[1,2]));
 mr[3,2]:=-Vector3Dot(PKraftVector3(pointer(@ma[3,0]))^,Vector3(ma[2,0],ma[2,1],ma[2,2]));
 mr[3,3]:=ma[3,3];
 result:=true;
end;

function Matrix4x4TermSimpleInverse(const ma:TKraftMatrix4x4):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=ma[0,0];
 result[0,1]:=ma[1,0];
 result[0,2]:=ma[2,0];
 result[0,3]:=ma[0,3];
 result[1,0]:=ma[0,1];
 result[1,1]:=ma[1,1];
 result[1,2]:=ma[2,1];
 result[1,3]:=ma[1,3];
 result[2,0]:=ma[0,2];
 result[2,1]:=ma[1,2];
 result[2,2]:=ma[2,2];
 result[2,3]:=ma[2,3];
 result[3,0]:=-Vector3Dot(PKraftVector3(pointer(@ma[3,0]))^,Vector3(ma[0,0],ma[0,1],ma[0,2]));
 result[3,1]:=-Vector3Dot(PKraftVector3(pointer(@ma[3,0]))^,Vector3(ma[1,0],ma[1,1],ma[1,2]));
 result[3,2]:=-Vector3Dot(PKraftVector3(pointer(@ma[3,0]))^,Vector3(ma[2,0],ma[2,1],ma[2,2]));
 result[3,3]:=ma[3,3];
end;

function Matrix4x4Inverse(var mr:TKraftMatrix4x4;const ma:TKraftMatrix4x4):boolean;
{$ifdef CPU386ASMForSinglePrecision}
asm
 mov ecx,esp
 and esp,$fffffff0
 sub esp,$b0
 movlps xmm2,qword ptr [ma+8]
 movlps xmm4,qword ptr [ma+40]
 movhps xmm2,qword ptr [ma+24]
 movhps xmm4,qword ptr [ma+56]
 movlps xmm3,qword ptr [ma+32]
 movlps xmm1,qword ptr [ma]
 movhps xmm3,qword ptr [ma+48]
 movhps xmm1,qword ptr [ma+16]
 movaps xmm5,xmm2
 shufps xmm5,xmm4,$88
 shufps xmm4,xmm2,$dd
 movaps xmm2,xmm4
 mulps xmm2,xmm5
 shufps xmm2,xmm2,$b1
 movaps xmm6,xmm2
 shufps xmm6,xmm6,$4e
 movaps xmm7,xmm3
 shufps xmm3,xmm1,$dd
 shufps xmm1,xmm7,$88
 movaps xmm7,xmm3
 mulps xmm3,xmm6
 mulps xmm6,xmm1
 movaps xmm0,xmm6
 movaps xmm6,xmm7
 mulps xmm7,xmm2
 mulps xmm2,xmm1
 subps xmm3,xmm7
 movaps xmm7,xmm6
 mulps xmm7,xmm5
 shufps xmm5,xmm5,$4e
 shufps xmm7,xmm7,$b1
 movaps dqword ptr [esp+16],xmm2
 movaps xmm2,xmm4
 mulps xmm2,xmm7
 addps xmm2,xmm3
 movaps xmm3,xmm7
 shufps xmm7,xmm7,$4e
 mulps xmm3,xmm1
 movaps dqword ptr [esp+32],xmm3
 movaps xmm3,xmm4
 mulps xmm3,xmm7
 mulps xmm7,xmm1
 subps xmm2,xmm3
 movaps xmm3,xmm6
 shufps xmm3,xmm3,$4e
 mulps xmm3,xmm4
 shufps xmm3,xmm3,$b1
 movaps dqword ptr [esp+48],xmm7
 movaps xmm7,xmm5
 mulps xmm5,xmm3
 addps xmm5,xmm2
 movaps xmm2,xmm3
 shufps xmm3,xmm3,$4e
 mulps xmm2,xmm1
 movaps dqword ptr [esp+64],xmm4
 movaps xmm4,xmm7
 mulps xmm7,xmm3
 mulps xmm3,xmm1
 subps xmm5,xmm7
 subps xmm3,xmm2
 movaps xmm2,xmm1
 mulps xmm1,xmm5
 shufps xmm3,xmm3,$4e
 movaps xmm7,xmm1
 shufps xmm1,xmm1,$4e
 movaps dqword ptr [esp],xmm5
 addps xmm1,xmm7
 movaps xmm5,xmm1
 shufps xmm1,xmm1,$b1
 addss xmm1,xmm5
 movaps xmm5,xmm6
 mulps xmm5,xmm2
 shufps xmm5,xmm5,$b1
 movaps xmm7,xmm5
 shufps xmm5,xmm5,$4e
 movaps dqword ptr [esp+80],xmm4
 movaps xmm4,dqword ptr [esp+64]
 movaps dqword ptr [esp+64],xmm6
 movaps xmm6,xmm4
 mulps xmm6,xmm7
 addps xmm6,xmm3
 movaps xmm3,xmm4
 mulps xmm3,xmm5
 subps xmm3,xmm6
 movaps xmm6,xmm4
 mulps xmm6,xmm2
 shufps xmm6,xmm6,$b1
 movaps dqword ptr [esp+112],xmm5
 movaps xmm5,dqword ptr [esp+64]
 movaps dqword ptr [esp+128],xmm7
 movaps xmm7,xmm6
 mulps xmm7,xmm5
 addps xmm7,xmm3
 movaps xmm3,xmm6
 shufps xmm3,xmm3,$4e
 movaps dqword ptr [esp+144],xmm4
 movaps xmm4,xmm5
 mulps xmm5,xmm3
 movaps dqword ptr [esp+160],xmm4
 movaps xmm4,xmm6
 movaps xmm6,xmm7
 subps xmm6,xmm5
 movaps xmm5,xmm0
 movaps xmm7,dqword ptr [esp+16]
 subps xmm5,xmm7
 shufps xmm5,xmm5,$4e
 movaps xmm7,dqword ptr [esp+80]
 mulps xmm4,xmm7
 mulps xmm3,xmm7
 subps xmm5,xmm4
 mulps xmm2,xmm7
 addps xmm3,xmm5
 shufps xmm2,xmm2,$b1
 movaps xmm4,xmm2
 shufps xmm4,xmm4,$4e
 movaps xmm5,dqword ptr [esp+144]
 movaps xmm0,xmm6
 movaps xmm6,xmm5
 mulps xmm5,xmm2
 mulps xmm6,xmm4
 addps xmm5,xmm3
 movaps xmm3,xmm4
 movaps xmm4,xmm5
 subps xmm4,xmm6
 movaps xmm5,dqword ptr [esp+48]
 movaps xmm6,dqword ptr [esp+32]
 subps xmm5,xmm6
 shufps xmm5,xmm5,$4e
 movaps xmm6,[esp+128]
 mulps xmm6,xmm7
 subps xmm6,xmm5
 movaps xmm5,dqword ptr [esp+112]
 mulps xmm7,xmm5
 subps xmm6,xmm7
 movaps xmm5,dqword ptr [esp+160]
 mulps xmm2,xmm5
 mulps xmm5,xmm3
 subps xmm6,xmm2
 movaps xmm2,xmm5
 addps xmm2,xmm6
 movaps xmm6,xmm0
 movaps xmm0,xmm1
 movaps xmm1,dqword ptr [esp]
 movaps xmm3,xmm0
 rcpss xmm5,xmm0
 mulss xmm0,xmm5
 mulss xmm0,xmm5
 addss xmm5,xmm5
 subss xmm5,xmm0
 movaps xmm0,xmm5
 addss xmm5,xmm5
 mulss xmm0,xmm0
 mulss xmm3,xmm0
 subss xmm5,xmm3
 shufps xmm5,xmm5,$00
 mulps xmm1,xmm5
 mulps xmm4,xmm5
 mulps xmm6,xmm5
 mulps xmm5,xmm2
 movups dqword ptr [mr+0],xmm1
 movups dqword ptr [mr+16],xmm4
 movups dqword ptr [mr+32],xmm6
 movups dqword ptr [mr+48],xmm5
 mov esp,ecx
 mov eax,1
end;
{$else}
var inv:array[0..15] of TKraftScalar;
    det:TKraftScalar;
begin
 inv[0]:=(((ma[1,1]*ma[2,2]*ma[3,3])-(ma[1,1]*ma[2,3]*ma[3,2]))-(ma[2,1]*ma[1,2]*ma[3,3])+(ma[2,1]*ma[1,3]*ma[3,2])+(ma[3,1]*ma[1,2]*ma[2,3]))-(ma[3,1]*ma[1,3]*ma[2,2]);
 inv[4]:=((((-(ma[1,0]*ma[2,2]*ma[3,3]))+(ma[1,0]*ma[2,3]*ma[3,2])+(ma[2,0]*ma[1,2]*ma[3,3]))-(ma[2,0]*ma[1,3]*ma[3,2]))-(ma[3,0]*ma[1,2]*ma[2,3]))+(ma[3,0]*ma[1,3]*ma[2,2]);
 inv[8]:=((((ma[1,0]*ma[2,1]*ma[3,3])-(ma[1,0]*ma[2,3]*ma[3,1]))-(ma[2,0]*ma[1,1]*ma[3,3]))+(ma[2,0]*ma[1,3]*ma[3,1])+(ma[3,0]*ma[1,1]*ma[2,3]))-(ma[3,0]*ma[1,3]*ma[2,1]);
 inv[12]:=((((-(ma[1,0]*ma[2,1]*ma[3,2]))+(ma[1,0]*ma[2,2]*ma[3,1])+(ma[2,0]*ma[1,1]*ma[3,2]))-(ma[2,0]*ma[1,2]*ma[3,1]))-(ma[3,0]*ma[1,1]*ma[2,2]))+(ma[3,0]*ma[1,2]*ma[2,1]);
 inv[1]:=((((-(ma[0,1]*ma[2,2]*ma[3,3]))+(ma[0,1]*ma[2,3]*ma[3,2])+(ma[2,1]*ma[0,2]*ma[3,3]))-(ma[2,1]*ma[0,3]*ma[3,2]))-(ma[3,1]*ma[0,2]*ma[2,3]))+(ma[3,1]*ma[0,3]*ma[2,2]);
 inv[5]:=(((ma[0,0]*ma[2,2]*ma[3,3])-(ma[0,0]*ma[2,3]*ma[3,2]))-(ma[2,0]*ma[0,2]*ma[3,3])+(ma[2,0]*ma[0,3]*ma[3,2])+(ma[3,0]*ma[0,2]*ma[2,3]))-(ma[3,0]*ma[0,3]*ma[2,2]);
 inv[9]:=((((-(ma[0,0]*ma[2,1]*ma[3,3]))+(ma[0,0]*ma[2,3]*ma[3,1])+(ma[2,0]*ma[0,1]*ma[3,3]))-(ma[2,0]*ma[0,3]*ma[3,1]))-(ma[3,0]*ma[0,1]*ma[2,3]))+(ma[3,0]*ma[0,3]*ma[2,1]);
 inv[13]:=((((ma[0,0]*ma[2,1]*ma[3,2])-(ma[0,0]*ma[2,2]*ma[3,1]))-(ma[2,0]*ma[0,1]*ma[3,2]))+(ma[2,0]*ma[0,2]*ma[3,1])+(ma[3,0]*ma[0,1]*ma[2,2]))-(ma[3,0]*ma[0,2]*ma[2,1]);
 inv[2]:=((((ma[0,1]*ma[1,2]*ma[3,3])-(ma[0,1]*ma[1,3]*ma[3,2]))-(ma[1,1]*ma[0,2]*ma[3,3]))+(ma[1,1]*ma[0,3]*ma[3,2])+(ma[3,1]*ma[0,2]*ma[1,3]))-(ma[3,1]*ma[0,3]*ma[1,2]);
 inv[6]:=((((-(ma[0,0]*ma[1,2]*ma[3,3]))+(ma[0,0]*ma[1,3]*ma[3,2])+(ma[1,0]*ma[0,2]*ma[3,3]))-(ma[1,0]*ma[0,3]*ma[3,2]))-(ma[3,0]*ma[0,2]*ma[1,3]))+(ma[3,0]*ma[0,3]*ma[1,2]);
 inv[10]:=((((ma[0,0]*ma[1,1]*ma[3,3])-(ma[0,0]*ma[1,3]*ma[3,1]))-(ma[1,0]*ma[0,1]*ma[3,3]))+(ma[1,0]*ma[0,3]*ma[3,1])+(ma[3,0]*ma[0,1]*ma[1,3]))-(ma[3,0]*ma[0,3]*ma[1,1]);
 inv[14]:=((((-(ma[0,0]*ma[1,1]*ma[3,2]))+(ma[0,0]*ma[1,2]*ma[3,1])+(ma[1,0]*ma[0,1]*ma[3,2]))-(ma[1,0]*ma[0,2]*ma[3,1]))-(ma[3,0]*ma[0,1]*ma[1,2]))+(ma[3,0]*ma[0,2]*ma[1,1]);
 inv[3]:=((((-(ma[0,1]*ma[1,2]*ma[2,3]))+(ma[0,1]*ma[1,3]*ma[2,2])+(ma[1,1]*ma[0,2]*ma[2,3]))-(ma[1,1]*ma[0,3]*ma[2,2]))-(ma[2,1]*ma[0,2]*ma[1,3]))+(ma[2,1]*ma[0,3]*ma[1,2]);
 inv[7]:=((((ma[0,0]*ma[1,2]*ma[2,3])-(ma[0,0]*ma[1,3]*ma[2,2]))-(ma[1,0]*ma[0,2]*ma[2,3]))+(ma[1,0]*ma[0,3]*ma[2,2])+(ma[2,0]*ma[0,2]*ma[1,3]))-(ma[2,0]*ma[0,3]*ma[1,2]);
 inv[11]:=((((-(ma[0,0]*ma[1,1]*ma[2,3]))+(ma[0,0]*ma[1,3]*ma[2,1])+(ma[1,0]*ma[0,1]*ma[2,3]))-(ma[1,0]*ma[0,3]*ma[2,1]))-(ma[2,0]*ma[0,1]*ma[1,3]))+(ma[2,0]*ma[0,3]*ma[1,1]);
 inv[15]:=((((ma[0,0]*ma[1,1]*ma[2,2])-(ma[0,0]*ma[1,2]*ma[2,1]))-(ma[1,0]*ma[0,1]*ma[2,2]))+(ma[1,0]*ma[0,2]*ma[2,1])+(ma[2,0]*ma[0,1]*ma[1,2]))-(ma[2,0]*ma[0,2]*ma[1,1]);
 det:=(ma[0,0]*inv[0])+(ma[0,1]*inv[4])+(ma[0,2]*inv[8])+(ma[0,3]*inv[12]);
 if det<>0.0 then begin
  det:=1.0/det;
  mr[0,0]:=inv[0]*det;
  mr[0,1]:=inv[1]*det;
  mr[0,2]:=inv[2]*det;
  mr[0,3]:=inv[3]*det;
  mr[1,0]:=inv[4]*det;
  mr[1,1]:=inv[5]*det;
  mr[1,2]:=inv[6]*det;
  mr[1,3]:=inv[7]*det;
  mr[2,0]:=inv[8]*det;
  mr[2,1]:=inv[9]*det;
  mr[2,2]:=inv[10]*det;
  mr[2,3]:=inv[11]*det;
  mr[3,0]:=inv[12]*det;
  mr[3,1]:=inv[13]*det;
  mr[3,2]:=inv[14]*det;
  mr[3,3]:=inv[15]*det;
  result:=true;
 end else begin
  result:=false;
 end;
end;
{$endif}

function Matrix4x4TermInverse(const ma:TKraftMatrix4x4):TKraftMatrix4x4;
{$ifdef CPU386ASMForSinglePrecision}
asm
 mov ecx,esp
 and esp,$fffffff0
 sub esp,$b0
 movlps xmm2,qword ptr [ma+8]
 movlps xmm4,qword ptr [ma+40]
 movhps xmm2,qword ptr [ma+24]
 movhps xmm4,qword ptr [ma+56]
 movlps xmm3,qword ptr [ma+32]
 movlps xmm1,qword ptr [ma]
 movhps xmm3,qword ptr [ma+48]
 movhps xmm1,qword ptr [ma+16]
 movaps xmm5,xmm2
 shufps xmm5,xmm4,$88
 shufps xmm4,xmm2,$dd
 movaps xmm2,xmm4
 mulps xmm2,xmm5
 shufps xmm2,xmm2,$b1
 movaps xmm6,xmm2
 shufps xmm6,xmm6,$4e
 movaps xmm7,xmm3
 shufps xmm3,xmm1,$dd
 shufps xmm1,xmm7,$88
 movaps xmm7,xmm3
 mulps xmm3,xmm6
 mulps xmm6,xmm1
 movaps xmm0,xmm6
 movaps xmm6,xmm7
 mulps xmm7,xmm2
 mulps xmm2,xmm1
 subps xmm3,xmm7
 movaps xmm7,xmm6
 mulps xmm7,xmm5
 shufps xmm5,xmm5,$4e
 shufps xmm7,xmm7,$b1
 movaps dqword ptr [esp+16],xmm2
 movaps xmm2,xmm4
 mulps xmm2,xmm7
 addps xmm2,xmm3
 movaps xmm3,xmm7
 shufps xmm7,xmm7,$4e
 mulps xmm3,xmm1
 movaps dqword ptr [esp+32],xmm3
 movaps xmm3,xmm4
 mulps xmm3,xmm7
 mulps xmm7,xmm1
 subps xmm2,xmm3
 movaps xmm3,xmm6
 shufps xmm3,xmm3,$4e
 mulps xmm3,xmm4
 shufps xmm3,xmm3,$b1
 movaps dqword ptr [esp+48],xmm7
 movaps xmm7,xmm5
 mulps xmm5,xmm3
 addps xmm5,xmm2
 movaps xmm2,xmm3
 shufps xmm3,xmm3,$4e
 mulps xmm2,xmm1
 movaps dqword ptr [esp+64],xmm4
 movaps xmm4,xmm7
 mulps xmm7,xmm3
 mulps xmm3,xmm1
 subps xmm5,xmm7
 subps xmm3,xmm2
 movaps xmm2,xmm1
 mulps xmm1,xmm5
 shufps xmm3,xmm3,$4e
 movaps xmm7,xmm1
 shufps xmm1,xmm1,$4e
 movaps dqword ptr [esp],xmm5
 addps xmm1,xmm7
 movaps xmm5,xmm1
 shufps xmm1,xmm1,$b1
 addss xmm1,xmm5
 movaps xmm5,xmm6
 mulps xmm5,xmm2
 shufps xmm5,xmm5,$b1
 movaps xmm7,xmm5
 shufps xmm5,xmm5,$4e
 movaps dqword ptr [esp+80],xmm4
 movaps xmm4,dqword ptr [esp+64]
 movaps dqword ptr [esp+64],xmm6
 movaps xmm6,xmm4
 mulps xmm6,xmm7
 addps xmm6,xmm3
 movaps xmm3,xmm4
 mulps xmm3,xmm5
 subps xmm3,xmm6
 movaps xmm6,xmm4
 mulps xmm6,xmm2
 shufps xmm6,xmm6,$b1
 movaps dqword ptr [esp+112],xmm5
 movaps xmm5,dqword ptr [esp+64]
 movaps dqword ptr [esp+128],xmm7
 movaps xmm7,xmm6
 mulps xmm7,xmm5
 addps xmm7,xmm3
 movaps xmm3,xmm6
 shufps xmm3,xmm3,$4e
 movaps dqword ptr [esp+144],xmm4
 movaps xmm4,xmm5
 mulps xmm5,xmm3
 movaps dqword ptr [esp+160],xmm4
 movaps xmm4,xmm6
 movaps xmm6,xmm7
 subps xmm6,xmm5
 movaps xmm5,xmm0
 movaps xmm7,dqword ptr [esp+16]
 subps xmm5,xmm7
 shufps xmm5,xmm5,$4e
 movaps xmm7,dqword ptr [esp+80]
 mulps xmm4,xmm7
 mulps xmm3,xmm7
 subps xmm5,xmm4
 mulps xmm2,xmm7
 addps xmm3,xmm5
 shufps xmm2,xmm2,$b1
 movaps xmm4,xmm2
 shufps xmm4,xmm4,$4e
 movaps xmm5,dqword ptr [esp+144]
 movaps xmm0,xmm6
 movaps xmm6,xmm5
 mulps xmm5,xmm2
 mulps xmm6,xmm4
 addps xmm5,xmm3
 movaps xmm3,xmm4
 movaps xmm4,xmm5
 subps xmm4,xmm6
 movaps xmm5,dqword ptr [esp+48]
 movaps xmm6,dqword ptr [esp+32]
 subps xmm5,xmm6
 shufps xmm5,xmm5,$4e
 movaps xmm6,[esp+128]
 mulps xmm6,xmm7
 subps xmm6,xmm5
 movaps xmm5,dqword ptr [esp+112]
 mulps xmm7,xmm5
 subps xmm6,xmm7
 movaps xmm5,dqword ptr [esp+160]
 mulps xmm2,xmm5
 mulps xmm5,xmm3
 subps xmm6,xmm2
 movaps xmm2,xmm5
 addps xmm2,xmm6
 movaps xmm6,xmm0
 movaps xmm0,xmm1
 movaps xmm1,dqword ptr [esp]
 movaps xmm3,xmm0
 rcpss xmm5,xmm0
 mulss xmm0,xmm5
 mulss xmm0,xmm5
 addss xmm5,xmm5
 subss xmm5,xmm0
 movaps xmm0,xmm5
 addss xmm5,xmm5
 mulss xmm0,xmm0
 mulss xmm3,xmm0
 subss xmm5,xmm3
 shufps xmm5,xmm5,$00
 mulps xmm1,xmm5
 mulps xmm4,xmm5
 mulps xmm6,xmm5
 mulps xmm5,xmm2
 movups dqword ptr [result+0],xmm1
 movups dqword ptr [result+16],xmm4
 movups dqword ptr [result+32],xmm6
 movups dqword ptr [result+48],xmm5
 mov esp,ecx
end;
{$else}
var inv:array[0..15] of TKraftScalar;
    det:TKraftScalar;
begin
 inv[0]:=(((ma[1,1]*ma[2,2]*ma[3,3])-(ma[1,1]*ma[2,3]*ma[3,2]))-(ma[2,1]*ma[1,2]*ma[3,3])+(ma[2,1]*ma[1,3]*ma[3,2])+(ma[3,1]*ma[1,2]*ma[2,3]))-(ma[3,1]*ma[1,3]*ma[2,2]);
 inv[4]:=((((-(ma[1,0]*ma[2,2]*ma[3,3]))+(ma[1,0]*ma[2,3]*ma[3,2])+(ma[2,0]*ma[1,2]*ma[3,3]))-(ma[2,0]*ma[1,3]*ma[3,2]))-(ma[3,0]*ma[1,2]*ma[2,3]))+(ma[3,0]*ma[1,3]*ma[2,2]);
 inv[8]:=((((ma[1,0]*ma[2,1]*ma[3,3])-(ma[1,0]*ma[2,3]*ma[3,1]))-(ma[2,0]*ma[1,1]*ma[3,3]))+(ma[2,0]*ma[1,3]*ma[3,1])+(ma[3,0]*ma[1,1]*ma[2,3]))-(ma[3,0]*ma[1,3]*ma[2,1]);
 inv[12]:=((((-(ma[1,0]*ma[2,1]*ma[3,2]))+(ma[1,0]*ma[2,2]*ma[3,1])+(ma[2,0]*ma[1,1]*ma[3,2]))-(ma[2,0]*ma[1,2]*ma[3,1]))-(ma[3,0]*ma[1,1]*ma[2,2]))+(ma[3,0]*ma[1,2]*ma[2,1]);
 inv[1]:=((((-(ma[0,1]*ma[2,2]*ma[3,3]))+(ma[0,1]*ma[2,3]*ma[3,2])+(ma[2,1]*ma[0,2]*ma[3,3]))-(ma[2,1]*ma[0,3]*ma[3,2]))-(ma[3,1]*ma[0,2]*ma[2,3]))+(ma[3,1]*ma[0,3]*ma[2,2]);
 inv[5]:=(((ma[0,0]*ma[2,2]*ma[3,3])-(ma[0,0]*ma[2,3]*ma[3,2]))-(ma[2,0]*ma[0,2]*ma[3,3])+(ma[2,0]*ma[0,3]*ma[3,2])+(ma[3,0]*ma[0,2]*ma[2,3]))-(ma[3,0]*ma[0,3]*ma[2,2]);
 inv[9]:=((((-(ma[0,0]*ma[2,1]*ma[3,3]))+(ma[0,0]*ma[2,3]*ma[3,1])+(ma[2,0]*ma[0,1]*ma[3,3]))-(ma[2,0]*ma[0,3]*ma[3,1]))-(ma[3,0]*ma[0,1]*ma[2,3]))+(ma[3,0]*ma[0,3]*ma[2,1]);
 inv[13]:=((((ma[0,0]*ma[2,1]*ma[3,2])-(ma[0,0]*ma[2,2]*ma[3,1]))-(ma[2,0]*ma[0,1]*ma[3,2]))+(ma[2,0]*ma[0,2]*ma[3,1])+(ma[3,0]*ma[0,1]*ma[2,2]))-(ma[3,0]*ma[0,2]*ma[2,1]);
 inv[2]:=((((ma[0,1]*ma[1,2]*ma[3,3])-(ma[0,1]*ma[1,3]*ma[3,2]))-(ma[1,1]*ma[0,2]*ma[3,3]))+(ma[1,1]*ma[0,3]*ma[3,2])+(ma[3,1]*ma[0,2]*ma[1,3]))-(ma[3,1]*ma[0,3]*ma[1,2]);
 inv[6]:=((((-(ma[0,0]*ma[1,2]*ma[3,3]))+(ma[0,0]*ma[1,3]*ma[3,2])+(ma[1,0]*ma[0,2]*ma[3,3]))-(ma[1,0]*ma[0,3]*ma[3,2]))-(ma[3,0]*ma[0,2]*ma[1,3]))+(ma[3,0]*ma[0,3]*ma[1,2]);
 inv[10]:=((((ma[0,0]*ma[1,1]*ma[3,3])-(ma[0,0]*ma[1,3]*ma[3,1]))-(ma[1,0]*ma[0,1]*ma[3,3]))+(ma[1,0]*ma[0,3]*ma[3,1])+(ma[3,0]*ma[0,1]*ma[1,3]))-(ma[3,0]*ma[0,3]*ma[1,1]);
 inv[14]:=((((-(ma[0,0]*ma[1,1]*ma[3,2]))+(ma[0,0]*ma[1,2]*ma[3,1])+(ma[1,0]*ma[0,1]*ma[3,2]))-(ma[1,0]*ma[0,2]*ma[3,1]))-(ma[3,0]*ma[0,1]*ma[1,2]))+(ma[3,0]*ma[0,2]*ma[1,1]);
 inv[3]:=((((-(ma[0,1]*ma[1,2]*ma[2,3]))+(ma[0,1]*ma[1,3]*ma[2,2])+(ma[1,1]*ma[0,2]*ma[2,3]))-(ma[1,1]*ma[0,3]*ma[2,2]))-(ma[2,1]*ma[0,2]*ma[1,3]))+(ma[2,1]*ma[0,3]*ma[1,2]);
 inv[7]:=((((ma[0,0]*ma[1,2]*ma[2,3])-(ma[0,0]*ma[1,3]*ma[2,2]))-(ma[1,0]*ma[0,2]*ma[2,3]))+(ma[1,0]*ma[0,3]*ma[2,2])+(ma[2,0]*ma[0,2]*ma[1,3]))-(ma[2,0]*ma[0,3]*ma[1,2]);
 inv[11]:=((((-(ma[0,0]*ma[1,1]*ma[2,3]))+(ma[0,0]*ma[1,3]*ma[2,1])+(ma[1,0]*ma[0,1]*ma[2,3]))-(ma[1,0]*ma[0,3]*ma[2,1]))-(ma[2,0]*ma[0,1]*ma[1,3]))+(ma[2,0]*ma[0,3]*ma[1,1]);
 inv[15]:=((((ma[0,0]*ma[1,1]*ma[2,2])-(ma[0,0]*ma[1,2]*ma[2,1]))-(ma[1,0]*ma[0,1]*ma[2,2]))+(ma[1,0]*ma[0,2]*ma[2,1])+(ma[2,0]*ma[0,1]*ma[1,2]))-(ma[2,0]*ma[0,2]*ma[1,1]);
 det:=(ma[0,0]*inv[0])+(ma[0,1]*inv[4])+(ma[0,2]*inv[8])+(ma[0,3]*inv[12]);
 if det<>0.0 then begin
  det:=1.0/det;
  result[0,0]:=inv[0]*det;
  result[0,1]:=inv[1]*det;
  result[0,2]:=inv[2]*det;
  result[0,3]:=inv[3]*det;
  result[1,0]:=inv[4]*det;
  result[1,1]:=inv[5]*det;
  result[1,2]:=inv[6]*det;
  result[1,3]:=inv[7]*det;
  result[2,0]:=inv[8]*det;
  result[2,1]:=inv[9]*det;
  result[2,2]:=inv[10]*det;
  result[2,3]:=inv[11]*det;
  result[3,0]:=inv[12]*det;
  result[3,1]:=inv[13]*det;
  result[3,2]:=inv[14]*det;
  result[3,3]:=inv[15]*det;
 end else begin
  result:=ma;
 end;
end;
{$endif}

function Matrix4x4InverseOld(var mr:TKraftMatrix4x4;const ma:TKraftMatrix4x4):boolean;
var Det,IDet:TKraftScalar;
begin
 Det:=(ma[0,0]*ma[1,1]*ma[2,2])+
      (ma[1,0]*ma[2,1]*ma[0,2])+
      (ma[2,0]*ma[0,1]*ma[1,2])-
      (ma[2,0]*ma[1,1]*ma[0,2])-
      (ma[1,0]*ma[0,1]*ma[2,2])-
      (ma[0,0]*ma[2,1]*ma[1,2]);
 if abs(Det)<EPSILON then begin
  mr:=Matrix4x4Identity;
  result:=false;
 end else begin
  IDet:=1/Det;
  mr[0,0]:=(ma[1,1]*ma[2,2]-ma[2,1]*ma[1,2])*IDet;
  mr[0,1]:=-(ma[0,1]*ma[2,2]-ma[2,1]*ma[0,2])*IDet;
  mr[0,2]:=(ma[0,1]*ma[1,2]-ma[1,1]*ma[0,2])*IDet;
  mr[0,3]:=0.0;
  mr[1,0]:=-(ma[1,0]*ma[2,2]-ma[2,0]*ma[1,2])*IDet;
  mr[1,1]:=(ma[0,0]*ma[2,2]-ma[2,0]*ma[0,2])*IDet;
  mr[1,2]:=-(ma[0,0]*ma[1,2]-ma[1,0]*ma[0,2])*IDet;
  mr[1,3]:=0.0;
  mr[2,0]:=(ma[1,0]*ma[2,1]-ma[2,0]*ma[1,1])*IDet;
  mr[2,1]:=-(ma[0,0]*ma[2,1]-ma[2,0]*ma[0,1])*IDet;
  mr[2,2]:=(ma[0,0]*ma[1,1]-ma[1,0]*ma[0,1])*IDet;
  mr[2,3]:=0.0;
  mr[3,0]:=-(ma[3,0]*mr[0,0]+ma[3,1]*mr[1,0]+ma[3,2]*mr[2,0]);
  mr[3,1]:=-(ma[3,0]*mr[0,1]+ma[3,1]*mr[1,1]+ma[3,2]*mr[2,1]);
  mr[3,2]:=-(ma[3,0]*mr[0,2]+ma[3,1]*mr[1,2]+ma[3,2]*mr[2,2]);
  mr[3,3]:=1.0;
  result:=true;
 end;
end;

function Matrix4x4TermInverseOld(const ma:TKraftMatrix4x4):TKraftMatrix4x4;
var Det,IDet:TKraftScalar;
begin
 Det:=((((ma[0,0]*ma[1,1]*ma[2,2])+
         (ma[1,0]*ma[2,1]*ma[0,2])+
         (ma[2,0]*ma[0,1]*ma[1,2]))-
        (ma[2,0]*ma[1,1]*ma[0,2]))-
       (ma[1,0]*ma[0,1]*ma[2,2]))-
      (ma[0,0]*ma[2,1]*ma[1,2]);
 if abs(Det)<EPSILON then begin
  result:=Matrix4x4Identity;
 end else begin
  IDet:=1/Det;
  result[0,0]:=(ma[1,1]*ma[2,2]-ma[2,1]*ma[1,2])*IDet;
  result[0,1]:=-(ma[0,1]*ma[2,2]-ma[2,1]*ma[0,2])*IDet;
  result[0,2]:=(ma[0,1]*ma[1,2]-ma[1,1]*ma[0,2])*IDet;
  result[0,3]:=0.0;
  result[1,0]:=-(ma[1,0]*ma[2,2]-ma[2,0]*ma[1,2])*IDet;
  result[1,1]:=(ma[0,0]*ma[2,2]-ma[2,0]*ma[0,2])*IDet;
  result[1,2]:=-(ma[0,0]*ma[1,2]-ma[1,0]*ma[0,2])*IDet;
  result[1,3]:=0.0;
  result[2,0]:=(ma[1,0]*ma[2,1]-ma[2,0]*ma[1,1])*IDet;
  result[2,1]:=-(ma[0,0]*ma[2,1]-ma[2,0]*ma[0,1])*IDet;
  result[2,2]:=(ma[0,0]*ma[1,1]-ma[1,0]*ma[0,1])*IDet;
  result[2,3]:=0.0;
  result[3,0]:=-(ma[3,0]*result[0,0]+ma[3,1]*result[1,0]+ma[3,2]*result[2,0]);
  result[3,1]:=-(ma[3,0]*result[0,1]+ma[3,1]*result[1,1]+ma[3,2]*result[2,1]);
  result[3,2]:=-(ma[3,0]*result[0,2]+ma[3,1]*result[1,2]+ma[3,2]*result[2,2]);
  result[3,3]:=1.0;
 end;
end;

function Matrix4x4GetSubMatrix3x3(const m:TKraftMatrix4x4;i,j:longint):TKraftMatrix3x3;
var di,dj,si,sj:longint;
begin
 for di:=0 to 2 do begin
  for dj:=0 to 2 do begin
   if di>=i then begin
    si:=di+1;
   end else begin
    si:=di;
   end;
   if dj>=j then begin
    sj:=dj+1;
   end else begin
    sj:=dj;
   end;
   result[di,dj]:=m[si,sj];
  end;
 end;
{$ifdef SIMD}
 result[0,3]:=0.0;
 result[1,3]:=0.0;
 result[2,3]:=0.0;
{$endif}
end;

function Matrix4x4Frustum(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
var rml,tmb,fmn:TKraftScalar;
begin
 rml:=Right-Left;
 tmb:=Top-Bottom;
 fmn:=zFar-zNear;
 result[0,0]:=(zNear*2.0)/rml;
 result[0,1]:=0.0;
 result[0,2]:=0.0;
 result[0,3]:=0.0;
 result[1,0]:=0.0;
 result[1,1]:=(zNear*2.0)/tmb;
 result[1,2]:=0.0;
 result[1,3]:=0.0;
 result[2,0]:=(Right+Left)/rml;
 result[2,1]:=(Top+Bottom)/tmb;
 result[2,2]:=(-(zFar+zNear))/fmn;
 result[2,3]:=-1.0;
 result[3,0]:=0.0;
 result[3,1]:=0.0;
 result[3,2]:=(-((zFar*zNear)*2.0))/fmn;
 result[3,3]:=0.0;
end;

function Matrix4x4Ortho(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
var rml,tmb,fmn:TKraftScalar;
begin
 rml:=Right-Left;
 tmb:=Top-Bottom;
 fmn:=zFar-zNear;
 result[0,0]:=2.0/rml;
 result[0,1]:=0.0;
 result[0,2]:=0.0;
 result[0,3]:=0.0;
 result[1,0]:=0.0;
 result[1,1]:=2.0/tmb;
 result[1,2]:=0.0;
 result[1,3]:=0.0;
 result[2,0]:=0.0;
 result[2,1]:=0.0;
 result[2,2]:=(-2.0)/fmn;
 result[2,3]:=0.0;
 result[3,0]:=(-(Right+Left))/rml;
 result[3,1]:=(-(Top+Bottom))/tmb;
 result[3,2]:=(-(zFar+zNear))/fmn;
 result[3,3]:=1.0;
end;

function Matrix4x4OrthoLH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
var rml,tmb,fmn:TKraftScalar;
begin
 rml:=Right-Left;
 tmb:=Top-Bottom;
 fmn:=zFar-zNear;
 result[0,0]:=2.0/rml;
 result[0,1]:=0.0;
 result[0,2]:=0.0;
 result[0,3]:=0.0;
 result[1,0]:=0.0;
 result[1,1]:=2.0/tmb;
 result[1,2]:=0.0;
 result[1,3]:=0.0;
 result[2,0]:=0.0;
 result[2,1]:=0.0;
 result[2,2]:=1.0/fmn;
 result[2,3]:=0.0;
 result[3,0]:=0;
 result[3,1]:=0;
 result[3,2]:=(-zNear)/fmn;
 result[3,3]:=1.0;
end;

function Matrix4x4OrthoRH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
var rml,tmb,fmn:TKraftScalar;
begin
 rml:=Right-Left;
 tmb:=Top-Bottom;
 fmn:=zFar-zNear;
 result[0,0]:=2.0/rml;
 result[0,1]:=0.0;
 result[0,2]:=0.0;
 result[0,3]:=0.0;
 result[1,0]:=0.0;
 result[1,1]:=2.0/tmb;
 result[1,2]:=0.0;
 result[1,3]:=0.0;
 result[2,0]:=0.0;
 result[2,1]:=0.0;
 result[2,2]:=1.0/fmn;
 result[2,3]:=0.0;
 result[3,0]:=0;
 result[3,1]:=0;
 result[3,2]:=zNear/fmn;
 result[3,3]:=1.0;
end;

function Matrix4x4OrthoOffCenterLH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
var rml,tmb,fmn:TKraftScalar;
begin
 rml:=Right-Left;
 tmb:=Top-Bottom;
 fmn:=zFar-zNear;
 result[0,0]:=2.0/rml;
 result[0,1]:=0.0;
 result[0,2]:=0.0;
 result[0,3]:=0.0;
 result[1,0]:=0.0;
 result[1,1]:=2.0/tmb;
 result[1,2]:=0.0;
 result[1,3]:=0.0;
 result[2,0]:=0.0;
 result[2,1]:=0.0;
 result[2,2]:=1.0/fmn;
 result[2,3]:=0.0;
 result[3,0]:=(Right+Left)/rml;
 result[3,1]:=(Top+Bottom)/tmb;
 result[3,2]:=zNear/fmn;
 result[3,3]:=1.0;
end;            

function Matrix4x4OrthoOffCenterRH(Left,Right,Bottom,Top,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
var rml,tmb,fmn:TKraftScalar;
begin
 rml:=Right-Left;
 tmb:=Top-Bottom;
 fmn:=zFar-zNear;
 result[0,0]:=2.0/rml;
 result[0,1]:=0.0;
 result[0,2]:=0.0;
 result[0,3]:=0.0;
 result[1,0]:=0.0;
 result[1,1]:=2.0/tmb;
 result[1,2]:=0.0;
 result[1,3]:=0.0;
 result[2,0]:=0.0;
 result[2,1]:=0.0;
 result[2,2]:=(-2.0)/fmn;
 result[2,3]:=0.0;
 result[3,0]:=(-(Right+Left))/rml;
 result[3,1]:=(-(Top+Bottom))/tmb;
 result[3,2]:=(-(zFar+zNear))/fmn;
 result[3,3]:=1.0;
end;

function Matrix4x4Perspective(fovy,Aspect,zNear,zFar:TKraftScalar):TKraftMatrix4x4;
(*)var Top,Right:TKraftScalar;
begin
 Top:=zNear*tan(fovy*pi/360.0);
 Right:=Top*Aspect;
 result:=Matrix4x4Frustum(-Right,Right,-Top,Top,zNear,zFar);
end;{(**)var Sine,Cotangent,ZDelta,Radians:TKraftScalar;
begin
 Radians:=(fovy*0.5)*DEG2RAD;
 ZDelta:=zFar-zNear;
 Sine:=sin(Radians);
 if not ((ZDelta=0) or (Sine=0) or (aspect=0)) then begin
  Cotangent:=cos(Radians)/Sine;
  result:=Matrix4x4Identity;
  result[0][0]:=Cotangent/aspect;
  result[1][1]:=Cotangent;
  result[2][2]:=(-(zFar+zNear))/ZDelta;
  result[2][3]:=-1-0;
  result[3][2]:=(-(2.0*zNear*zFar))/ZDelta;
  result[3][3]:=0.0;
 end;
end;{}

function Matrix4x4LookAt(const Eye,Center,Up:TKraftVector3):TKraftMatrix4x4;
var RightVector,UpVector,ForwardVector:TKraftVector3;
begin
 ForwardVector:=Vector3NormEx(Vector3Sub(Eye,Center));
 RightVector:=Vector3NormEx(Vector3Cross(Up,ForwardVector));
 UpVector:=Vector3NormEx(Vector3Cross(ForwardVector,RightVector));
 result[0,0]:=RightVector.x;
 result[1,0]:=RightVector.y;
 result[2,0]:=RightVector.z;
 result[3,0]:=-((RightVector.x*Eye.x)+(RightVector.y*Eye.y)+(RightVector.z*Eye.z));
 result[0,1]:=UpVector.x;
 result[1,1]:=UpVector.y;
 result[2,1]:=UpVector.z;
 result[3,1]:=-((UpVector.x*Eye.x)+(UpVector.y*Eye.y)+(UpVector.z*Eye.z));
 result[0,2]:=ForwardVector.x;
 result[1,2]:=ForwardVector.y;
 result[2,2]:=ForwardVector.z;
 result[3,2]:=-((ForwardVector.x*Eye.x)+(ForwardVector.y*Eye.y)+(ForwardVector.z*Eye.z));
 result[0,3]:=0.0;
 result[1,3]:=0.0;
 result[2,3]:=0.0;
 result[3,3]:=1.0;
end;

function Matrix4x4Fill(const Eye,RightVector,UpVector,ForwardVector:TKraftVector3):TKraftMatrix4x4;
begin
 result[0,0]:=RightVector.x;
 result[1,0]:=RightVector.y;
 result[2,0]:=RightVector.z;
 result[3,0]:=-((RightVector.x*Eye.x)+(RightVector.y*Eye.y)+(RightVector.z*Eye.z));
 result[0,1]:=UpVector.x;
 result[1,1]:=UpVector.y;
 result[2,1]:=UpVector.z;
 result[3,1]:=-((UpVector.x*Eye.x)+(UpVector.y*Eye.y)+(UpVector.z*Eye.z));
 result[0,2]:=ForwardVector.x;
 result[1,2]:=ForwardVector.y;
 result[2,2]:=ForwardVector.z;
 result[3,2]:=-((ForwardVector.x*Eye.x)+(ForwardVector.y*Eye.y)+(ForwardVector.z*Eye.z));
 result[0,3]:=0.0;
 result[1,3]:=0.0;
 result[2,3]:=0.0;
 result[3,3]:=1.0;
end;

function Matrix4x4ConstructX(const xAxis:TKraftVector3):TKraftMatrix4x4;
var a,b,c:TKraftVector3;
begin
 a:=Vector3NormEx(xAxis);
 result[0,0]:=a.x;
 result[0,1]:=a.y;
 result[0,2]:=a.z;
 result[0,3]:=0.0;
//b:=Vector3NormEx(Vector3Cross(Vector3(0,0,1),a));
 b:=Vector3NormEx(Vector3Perpendicular(a));
 result[1,0]:=b.x;
 result[1,1]:=b.y;
 result[1,2]:=b.z;
 result[1,3]:=0.0;
 c:=Vector3NormEx(Vector3Cross(b,a));
 result[2,0]:=c.x;
 result[2,1]:=c.y;
 result[2,2]:=c.z;
 result[2,3]:=0.0;
 result[3,0]:=0.0;
 result[3,1]:=0.0;
 result[3,2]:=0.0;
 result[3,3]:=1.0;
end;{}

function Matrix4x4ConstructY(const yAxis:TKraftVector3):TKraftMatrix4x4;
var a,b,c:TKraftVector3;
begin
 a:=Vector3NormEx(yAxis);
 result[1,0]:=a.x;
 result[1,1]:=a.y;
 result[1,2]:=a.z;
 result[1,3]:=0.0;
 b:=Vector3NormEx(Vector3Perpendicular(a));
 result[0,0]:=b.x;
 result[0,1]:=b.y;
 result[0,2]:=b.z;
 result[0,3]:=0.0;
 c:=Vector3Cross(b,a);
 result[2,0]:=c.x;
 result[2,1]:=c.y;
 result[2,2]:=c.z;
 result[2,3]:=0.0;
 result[3,0]:=0.0;
 result[3,1]:=0.0;
 result[3,2]:=0.0;
 result[3,3]:=1.0;
end;

function Matrix4x4ConstructZ(const zAxis:TKraftVector3):TKraftMatrix4x4;
var a,b,c:TKraftVector3;
begin
 a:=Vector3NormEx(zAxis);
 result[2,0]:=a.x;
 result[2,1]:=a.y;
 result[2,2]:=a.z;
 result[2,3]:=0.0;
 b:=Vector3NormEx(Vector3Perpendicular(a));
//b:=Vector3Sub(Vector3(0,1,0),Vector3ScalarMul(a,a.y));
 result[1,0]:=b.x;
 result[1,1]:=b.y;
 result[1,2]:=b.z;
 result[1,3]:=0.0;
 c:=Vector3Cross(b,a);
 result[0,0]:=c.x;
 result[0,1]:=c.y;
 result[0,2]:=c.z;
 result[0,3]:=0.0;
 result[3,0]:=0.0;
 result[3,1]:=0.0;
 result[3,2]:=0.0;
 result[3,3]:=1.0;
end;

function Matrix4x4ProjectionMatrixClip(const ProjectionMatrix:TKraftMatrix4x4;const ClipPlane:TKraftPlane):TKraftMatrix4x4;
var q,c:TKraftVector4;
begin
 result:=ProjectionMatrix;
 q.x:=(Sign(ClipPlane.Normal.x)+result[2,0])/result[0,0];
 q.y:=(Sign(ClipPlane.Normal.y)+result[2,1])/result[1,1];
 q.z:=-1.0;
 q.w:=(1.0+result[2,2])/result[3,2];
 c.x:=ClipPlane.Normal.x;
 c.y:=ClipPlane.Normal.y;
 c.z:=ClipPlane.Normal.z;
 c.w:=ClipPlane.Distance;
 c:=Vector4ScalarMul(c,2.0/Vector4Dot(c,q));
 result[0,2]:=c.x;
 result[1,2]:=c.y;
 result[2,2]:=c.z+1.0;
 result[3,2]:=c.w;
end;

function PlaneMatrixMul(const Plane:TKraftPlane;const Matrix:TKraftMatrix4x4):TKraftPlane;
begin
 result.Normal.x:=(Matrix[0,0]*Plane.Normal.x)+(Matrix[1,0]*Plane.Normal.y)+(Matrix[2,0]*Plane.Normal.z)+(Matrix[3,0]*Plane.Distance);
 result.Normal.y:=(Matrix[0,1]*Plane.Normal.x)+(Matrix[1,1]*Plane.Normal.y)+(Matrix[2,1]*Plane.Normal.z)+(Matrix[3,1]*Plane.Distance);
 result.Normal.z:=(Matrix[0,2]*Plane.Normal.x)+(Matrix[1,2]*Plane.Normal.y)+(Matrix[2,2]*Plane.Normal.z)+(Matrix[3,2]*Plane.Distance);
 result.Distance:=(Matrix[0,3]*Plane.Normal.x)+(Matrix[1,3]*Plane.Normal.y)+(Matrix[2,3]*Plane.Normal.z)+(Matrix[3,3]*Plane.Distance);
end;

function PlaneTransform(const Plane:TKraftPlane;const Matrix:TKraftMatrix4x4):TKraftPlane; overload;
begin
 result.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(Plane.Normal,Matrix4x4TermTranspose(Matrix4x4TermInverse(Matrix))));
 result.Distance:=-Vector3Dot(result.Normal,Vector3TermMatrixMul(Vector3ScalarMul(Plane.Normal,-Plane.Distance),Matrix));
end;

function PlaneTransform(const Plane:TKraftPlane;const Matrix,NormalMatrix:TKraftMatrix4x4):TKraftPlane; overload;
begin
 result.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(Plane.Normal,NormalMatrix));
 result.Distance:=-Vector3Dot(result.Normal,Vector3TermMatrixMul(Vector3ScalarMul(Plane.Normal,-Plane.Distance),Matrix));
end;

function PlaneFastTransform(const Plane:TKraftPlane;const Matrix:TKraftMatrix4x4):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}
begin
 result.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(Plane.Normal,Matrix));
 result.Distance:=-Vector3Dot(result.Normal,Vector3TermMatrixMul(Vector3ScalarMul(Plane.Normal,-Plane.Distance),Matrix));
end;

procedure PlaneNormalize(var Plane:TKraftPlane); {$ifdef caninline}inline;{$endif}
var l:TKraftScalar;
begin
 l:=sqr(Plane.Normal.x)+sqr(Plane.Normal.y)+sqr(Plane.Normal.z);
 if l>0.0 then begin
  l:=sqrt(l);
  Plane.Normal.x:=Plane.Normal.x/l;
  Plane.Normal.y:=Plane.Normal.y/l;
  Plane.Normal.z:=Plane.Normal.z/l;
  Plane.Distance:=Plane.Distance/l;
 end else begin
  Plane.Normal.x:=0.0;
  Plane.Normal.y:=0.0;
  Plane.Normal.z:=0.0;
  Plane.Distance:=0.0;
 end;
end;

function PlaneVectorDistance(const Plane:TKraftPlane;const Point:TKraftVector3):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=(Plane.Normal.x*Point.x)+(Plane.Normal.y*Point.y)+(Plane.Normal.z*Point.z)+Plane.Distance;
end;

function PlaneVectorDistance(const Plane:TKraftPlane;const Point:TKraftVector4):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=(Plane.Normal.x*Point.x)+(Plane.Normal.y*Point.y)+(Plane.Normal.z*Point.z)+(Plane.Distance*Point.w);
end;

function PlaneFromPoints(const p1,p2,p3:TKraftVector3):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}
var n:TKraftVector3;
begin
 n:=Vector3NormEx(Vector3Cross(Vector3Sub(p2,p1),Vector3Sub(p3,p1)));
 result.Normal.x:=n.x;
 result.Normal.y:=n.y;
 result.Normal.z:=n.z;
 result.Distance:=-((result.Normal.x*p1.x)+(result.Normal.y*p1.y)+(result.Normal.z*p1.z));
end;

function PlaneFromPoints(const p1,p2,p3:TKraftVector4):TKraftPlane; overload; {$ifdef caninline}inline;{$endif}
var n:TKraftVector4;
begin
 n:=Vector4Norm(Vector4Cross(Vector4Sub(p2,p1),Vector4Sub(p3,p1)));
 result.Normal.x:=n.x;
 result.Normal.y:=n.y;
 result.Normal.z:=n.z;
 result.Distance:=-((result.Normal.x*p1.x)+(result.Normal.y*p1.y)+(result.Normal.z*p1.z));
end;

function QuaternionNormal(const AQuaternion:TKraftQuaternion):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [AQuaternion]
 mulps xmm0,xmm0
 movhlps xmm1,xmm0
 addps xmm0,xmm1
 pshufd xmm1,xmm0,$01
 addss xmm0,xmm1
 sqrtss xmm0,xmm0
 movss dword ptr [result],xmm0
end;
{$else}
begin
 result:=sqrt(sqr(AQuaternion.x)+sqr(AQuaternion.y)+sqr(AQuaternion.z)+sqr(AQuaternion.w));
end;
{$endif}
                            
function QuaternionLengthSquared(const AQuaternion:TKraftQuaternion):TKraftScalar; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [AQuaternion]
 mulps xmm0,xmm0
 movhlps xmm1,xmm0
 addps xmm0,xmm1
 pshufd xmm1,xmm0,$01
 addss xmm0,xmm1
 movss dword ptr [result],xmm0
end;
{$else}
begin
 result:=sqr(AQuaternion.x)+sqr(AQuaternion.y)+sqr(AQuaternion.z)+sqr(AQuaternion.w);
end;
{$endif}

procedure QuaternionNormalize(var AQuaternion:TKraftQuaternion); {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
{movups xmm2,dqword ptr [AQuaternion]
 movaps xmm0,xmm2
 mulps xmm0,xmm0
 movhlps xmm1,xmm0
 addps xmm0,xmm1
 pshufd xmm1,xmm0,$01
 addss xmm0,xmm1
 movss xmm3,xmm0
 xorps xmm1,xmm1
 cmpps xmm3,xmm1,4
 rsqrtss xmm0,xmm0
 andps xmm0,xmm3
 shufps xmm0,xmm0,$00
 mulps xmm2,xmm0
 movups dqword ptr [AQuaternion],xmm2}
 movups xmm2,dqword ptr [AQuaternion]
 movaps xmm0,xmm2
 mulps xmm0,xmm0
 movhlps xmm1,xmm0
 addps xmm0,xmm1
 pshufd xmm1,xmm0,$01
 addss xmm0,xmm1
 sqrtss xmm0,xmm0
 shufps xmm0,xmm0,$00
 divps xmm2,xmm0
 subps xmm1,xmm2
 cmpps xmm1,xmm0,7
 andps xmm2,xmm1
 movups dqword ptr [AQuaternion],xmm2
end;
{$else}
var Normal:TKraftScalar;
begin
 Normal:=sqrt(sqr(AQuaternion.x)+sqr(AQuaternion.y)+sqr(AQuaternion.z)+sqr(AQuaternion.w));
 if Normal>0.0 then begin
  Normal:=1.0/Normal;
 end;
 AQuaternion.x:=AQuaternion.x*Normal;
 AQuaternion.y:=AQuaternion.y*Normal;
 AQuaternion.z:=AQuaternion.z*Normal;
 AQuaternion.w:=AQuaternion.w*Normal;
end;
{$endif}

function QuaternionTermNormalize(const AQuaternion:TKraftQuaternion):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm2,dqword ptr [AQuaternion]
 movaps xmm0,xmm2
 mulps xmm0,xmm0
 movhlps xmm1,xmm0
 addps xmm0,xmm1
 pshufd xmm1,xmm0,$01
 addss xmm0,xmm1
 sqrtss xmm0,xmm0
 shufps xmm0,xmm0,$00
 divps xmm2,xmm0
 subps xmm1,xmm2
 cmpps xmm1,xmm0,7
 andps xmm2,xmm1
 movups dqword ptr [result],xmm2
end;
{$else}
var Normal:TKraftScalar;
begin
 Normal:=sqrt(sqr(AQuaternion.x)+sqr(AQuaternion.y)+sqr(AQuaternion.z)+sqr(AQuaternion.w));
 if Normal>0.0 then begin
  Normal:=1.0/Normal;
 end;
 result.x:=AQuaternion.x*Normal;
 result.y:=AQuaternion.y*Normal;
 result.z:=AQuaternion.z*Normal;
 result.w:=AQuaternion.w*Normal;
end;
{$endif}

function QuaternionNeg(const AQuaternion:TKraftQuaternion):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm1,dqword ptr [AQuaternion]
 xorps xmm0,xmm0
 subps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=-AQuaternion.x;
 result.y:=-AQuaternion.y;
 result.z:=-AQuaternion.z;
 result.w:=-AQuaternion.w;
end;
{$endif}

function QuaternionConjugate(const AQuaternion:TKraftQuaternion):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
const XORMask:array[0..3] of longword=($80000000,$80000000,$80000000,$00000000);
asm
 movups xmm0,dqword ptr [AQuaternion]
 movups xmm1,dqword ptr [XORMask]
 xorps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=-AQuaternion.x;
 result.y:=-AQuaternion.y;
 result.z:=-AQuaternion.z;
 result.w:=AQuaternion.w;
end;
{$endif}

function QuaternionInverse(const AQuaternion:TKraftQuaternion):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
const XORMask:array[0..3] of longword=($80000000,$80000000,$80000000,$00000000);
asm
 movups xmm2,dqword ptr [AQuaternion]
 movups xmm3,dqword ptr [XORMask]
 movaps xmm0,xmm2
 mulps xmm0,xmm0
 movhlps xmm1,xmm0
 addps xmm0,xmm1
 pshufd xmm1,xmm0,$01
 addss xmm0,xmm1
 sqrtss xmm0,xmm0
 shufps xmm0,xmm0,$00
 divps xmm2,xmm0
 xorps xmm2,xmm3
 movups dqword ptr [result],xmm2
end;
{$else}
var Normal:TKraftScalar;
begin
 Normal:=sqrt(sqr(AQuaternion.x)+sqr(AQuaternion.y)+sqr(AQuaternion.z)+sqr(AQuaternion.w));
 if Normal>0.0 then begin
  Normal:=1.0/Normal;
 end;
 result.x:=-(AQuaternion.x*Normal);
 result.y:=-(AQuaternion.y*Normal);
 result.z:=-(AQuaternion.z*Normal);
 result.w:=(AQuaternion.w*Normal);
end;
{$endif}

function QuaternionAdd(const q1,q2:TKraftQuaternion):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [q1]
 movups xmm1,dqword ptr [q2]
 addps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=q1.x+q2.x;
 result.y:=q1.y+q2.y;
 result.z:=q1.z+q2.z;
 result.w:=q1.w+q2.w;
end;
{$endif}

function QuaternionSub(const q1,q2:TKraftQuaternion):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm
 movups xmm0,dqword ptr [q1]
 movups xmm1,dqword ptr [q2]
 subps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=q1.x-q2.x;
 result.y:=q1.y-q2.y;
 result.z:=q1.z-q2.z;
 result.w:=q1.w-q2.w;
end;
{$endif}

function QuaternionScalarMul(const q:TKraftQuaternion;const s:TKraftScalar):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
asm                    
 movups xmm0,dqword ptr [q]
 movss xmm1,dword ptr [s]
 shufps xmm1,xmm1,$00
 mulps xmm0,xmm1
 movups dqword ptr [result],xmm0
end;
{$else}
begin
 result.x:=q.x*s;
 result.y:=q.y*s;
 result.z:=q.z*s;
 result.w:=q.w*s;
end;
{$endif}

function QuaternionMul(const q1,q2:TKraftQuaternion):TKraftQuaternion; {$ifdef CPU386ASMForSinglePrecision}assembler;
const XORMaskW:array[0..3] of longword=($00000000,$00000000,$00000000,$80000000);
asm
 movups xmm4,dqword ptr [q1]
 movaps xmm0,xmm4
 shufps xmm0,xmm4,$49
 movups xmm2,dqword ptr [q2]
 movaps xmm3,xmm2
 movaps xmm1,xmm2
 shufps xmm3,xmm2,$52 // 001010010b
 mulps xmm3,xmm0
 movaps xmm0,xmm4
 shufps xmm0,xmm4,$24 // 000100100b
 shufps xmm1,xmm2,$3f // 000111111b
 movups xmm5,dqword ptr [XORMaskW]
 mulps xmm1,xmm0
 movaps xmm0,xmm4
 shufps xmm0,xmm4,$92 // 001001001b
 shufps xmm4,xmm4,$ff // 011111111b
 mulps xmm4,xmm2
 addps xmm3,xmm1
 movaps xmm1,xmm2
 shufps xmm1,xmm2,$89 // 010001001b
 mulps xmm1,xmm0
 xorps xmm3,xmm5
 subps xmm4,xmm1
 addps xmm3,xmm4
 movups dqword ptr [result],xmm3
end;
{$else}
begin
 result.x:=((q1.w*q2.x)+(q1.x*q2.w)+(q1.y*q2.z))-(q1.z*q2.y);
 result.y:=((q1.w*q2.y)+(q1.y*q2.w)+(q1.z*q2.x))-(q1.x*q2.z);
 result.z:=((q1.w*q2.z)+(q1.z*q2.w)+(q1.x*q2.y))-(q1.y*q2.x);
 result.w:=(q1.w*q2.w)-((q1.x*q2.x)+(q1.y*q2.y)+(q1.z*q2.z));
end;
{$endif}

function QuaternionRotateAroundAxis(const q1,q2:TKraftQuaternion):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
begin
 result.x:=((q1.x*q2.w)+(q1.z*q2.y))-(q1.y*q2.z);
 result.y:=((q1.x*q2.z)+(q1.y*q2.w))-(q1.z*q2.x);
 result.z:=((q1.y*q2.x)+(q1.z*q2.w))-(q1.x*q2.y);
 result.w:=((q1.x*q2.x)+(q1.y*q2.y))+(q1.z*q2.z);
end;

function QuaternionFromAxisAngle(const Axis:TKraftVector3;Angle:TKraftScalar):TKraftQuaternion; overload; {$ifdef caninline}inline;{$endif}
var sa2:TKraftScalar;
begin
 result.w:=cos(Angle*0.5);
 sa2:=sin(Angle*0.5);
 result.x:=Axis.x*sa2;
 result.y:=Axis.y*sa2;
 result.z:=Axis.z*sa2;
 QuaternionNormalize(result);
end;

function QuaternionFromSpherical(const Latitude,Longitude:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
begin
 result.x:=cos(Latitude)*sin(Longitude);
 result.y:=sin(Latitude);
 result.z:=cos(Latitude)*cos(Longitude);
 result.w:=0.0;
end;

procedure QuaternionToSpherical(const q:TKraftQuaternion;var Latitude,Longitude:TKraftScalar);
var y:TKraftScalar;
begin
 y:=q.y;
 if y<-1.0 then begin
  y:=-1.0;
 end else if y>1.0 then begin
  y:=1.0;
 end;
 Latitude:=ArcSin(y);
 if (sqr(q.x)+sqr(q.z))>0.00005 then begin
  Longitude:=ArcTan2(q.x,q.z);
 end else begin
  Longitude:=0.0;
 end;
end;

function QuaternionFromAngles(const Pitch,Yaw,Roll:TKraftScalar):TKraftQuaternion; overload; {$ifdef caninline}inline;{$endif}
var sp,sy,sr,cp,cy,cr:TKraftScalar;
begin
 sp:=sin(Pitch*0.5);
 sy:=sin(Yaw*0.5);
 sr:=sin(Roll*0.5);
 cp:=cos(Pitch*0.5);
 cy:=cos(Yaw*0.5);
 cr:=cos(Roll*0.5);
 result.x:=(sr*cp*cy)-(cr*sp*sy);
 result.y:=(cr*sp*cy)+(sr*cp*sy);
 result.z:=(cr*cp*sy)-(sr*sp*cy);
 result.w:=(cr*cp*cy)+(sr*sp*sy);
 QuaternionNormalize(result);
end;

function QuaternionFromAngles(const Angles:TKraftAngles):TKraftQuaternion; overload; {$ifdef caninline}inline;{$endif}
var sp,sy,sr,cp,cy,cr:TKraftScalar;
begin
 sp:=sin(Angles.Pitch*0.5);
 sy:=sin(Angles.Yaw*0.5);
 sr:=sin(Angles.Roll*0.5);
 cp:=cos(Angles.Pitch*0.5);
 cy:=cos(Angles.Yaw*0.5);
 cr:=cos(Angles.Roll*0.5);
 result.x:=(sr*cp*cy)-(cr*sp*sy);
 result.y:=(cr*sp*cy)+(sr*cp*sy);
 result.z:=(cr*cp*sy)-(sr*sp*cy);
 result.w:=(cr*cp*cy)+(sr*sp*sy);
 QuaternionNormalize(result);
end;

function QuaternionFromMatrix3x3(const AMatrix:TKraftMatrix3x3):TKraftQuaternion;
var t,s:TKraftScalar;
begin
 t:=AMatrix[0,0]+(AMatrix[1,1]+AMatrix[2,2]);
 if t>2.9999999 then begin
  result.x:=0.0;
  result.y:=0.0;
  result.z:=0.0;
  result.w:=1.0;
 end else if t>0.0000001 then begin
  s:=sqrt(1.0+t)*2.0;
  result.x:=(AMatrix[1,2]-AMatrix[2,1])/s;
  result.y:=(AMatrix[2,0]-AMatrix[0,2])/s;
  result.z:=(AMatrix[0,1]-AMatrix[1,0])/s;
  result.w:=s*0.25;
 end else if (AMatrix[0,0]>AMatrix[1,1]) and (AMatrix[0,0]>AMatrix[2,2]) then begin
  s:=sqrt(1.0+(AMatrix[0,0]-(AMatrix[1,1]+AMatrix[2,2])))*2.0;
  result.x:=s*0.25;
  result.y:=(AMatrix[1,0]+AMatrix[0,1])/s;
  result.z:=(AMatrix[2,0]+AMatrix[0,2])/s;
  result.w:=(AMatrix[1,2]-AMatrix[2,1])/s;
 end else if AMatrix[1,1]>AMatrix[2,2] then begin
  s:=sqrt(1.0+(AMatrix[1,1]-(AMatrix[0,0]+AMatrix[2,2])))*2.0;
  result.x:=(AMatrix[1,0]+AMatrix[0,1])/s;
  result.y:=s*0.25;
  result.z:=(AMatrix[2,1]+AMatrix[1,2])/s;
  result.w:=(AMatrix[2,0]-AMatrix[0,2])/s;
 end else begin
  s:=sqrt(1.0+(AMatrix[2,2]-(AMatrix[0,0]+AMatrix[1,1])))*2.0;
  result.x:=(AMatrix[2,0]+AMatrix[0,2])/s;
  result.y:=(AMatrix[2,1]+AMatrix[1,2])/s;
  result.z:=s*0.25;
  result.w:=(AMatrix[0,1]-AMatrix[1,0])/s;
 end;
 QuaternionNormalize(result);
end;
{var xx,yx,zx,xy,yy,zy,xz,yz,zz,Trace,Radicand,Scale,TempX,TempY,TempZ,TempW:TKraftScalar;
    NegativeTrace,ZgtX,ZgtY,YgtX,LargestXorY,LargestYorZ,LargestZorX:boolean;
begin
 xx:=AMatrix[0,0];
 yx:=AMatrix[0,1];
 zx:=AMatrix[0,2];
 xy:=AMatrix[1,0];
 yy:=AMatrix[1,1];
 zy:=AMatrix[1,2];
 xz:=AMatrix[2,0];
 yz:=AMatrix[2,1];
 zz:=AMatrix[2,2];
 Trace:=(xx+yy)+zz;
 NegativeTrace:=Trace<0.0;
 ZgtX:=zz>xx;
 ZgtY:=zz>yy;
 YgtX:=yy>xx;
 LargestXorY:=NegativeTrace and ((not ZgtX) or not ZgtY);
 LargestYorZ:=NegativeTrace and (YgtX or ZgtX);
 LargestZorX:=NegativeTrace and (ZgtY or not YgtX);
 if LargestXorY then begin
  zz:=-zz;
  xy:=-xy;
 end;
 if LargestYorZ then begin
  xx:=-xx;
  yz:=-yz;
 end;
 if LargestZorX then begin
  yy:=-yy;
  zx:=-zx;
 end;
 Radicand:=((xx+yy)+zz)+1.0;
 Scale:=0.5/sqrt(Radicand);
 TempX:=(zy-yz)*Scale;
 TempY:=(xz-zx)*Scale;
 TempZ:=(yx-xy)*Scale;
 TempW:=Radicand*Scale;
 if LargestXorY then begin
  result.x:=TempW;
  result.y:=TempZ;
  result.z:=TempY;
  result.w:=TempX;
 end else begin
  result.x:=TempX;
  result.y:=TempY;
  result.z:=TempZ;
  result.w:=TempW;
 end;
 if LargestYorZ then begin
  TempX:=result.x;
  TempZ:=result.z;
  result.x:=result.y;
  result.y:=TempX;
  result.z:=result.w;
  result.w:=TempZ;
 end;
end;{}

function QuaternionToMatrix3x3(AQuaternion:TKraftQuaternion):TKraftMatrix3x3;
var qx2,qy2,qz2,qxqx2,qxqy2,qxqz2,qxqw2,qyqy2,qyqz2,qyqw2,qzqz2,qzqw2:TKraftScalar;
begin
 QuaternionNormalize(AQuaternion);
 qx2:=AQuaternion.x+AQuaternion.x;
 qy2:=AQuaternion.y+AQuaternion.y;
 qz2:=AQuaternion.z+AQuaternion.z;
 qxqx2:=AQuaternion.x*qx2;
 qxqy2:=AQuaternion.x*qy2;
 qxqz2:=AQuaternion.x*qz2;
 qxqw2:=AQuaternion.w*qx2;
 qyqy2:=AQuaternion.y*qy2;
 qyqz2:=AQuaternion.y*qz2;
 qyqw2:=AQuaternion.w*qy2;
 qzqz2:=AQuaternion.z*qz2;
 qzqw2:=AQuaternion.w*qz2;
 result[0,0]:=1.0-(qyqy2+qzqz2);
 result[0,1]:=qxqy2+qzqw2;
 result[0,2]:=qxqz2-qyqw2;
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=qxqy2-qzqw2;
 result[1,1]:=1.0-(qxqx2+qzqz2);
 result[1,2]:=qyqz2+qxqw2;
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=qxqz2+qyqw2;
 result[2,1]:=qyqz2-qxqw2;
 result[2,2]:=1.0-(qxqx2+qyqy2);
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function QuaternionFromTangentSpaceMatrix3x3(AMatrix:TKraftMatrix3x3):TKraftQuaternion;
const Threshold=1.0/127.0;
var Scale,t,s,Renormalization:TKraftScalar;
begin
 if ((((((AMatrix[0,0]*AMatrix[1,1]*AMatrix[2,2])+
         (AMatrix[0,1]*AMatrix[1,2]*AMatrix[2,0])
        )+
        (AMatrix[0,2]*AMatrix[1,0]*AMatrix[2,1])
       )-
       (AMatrix[0,2]*AMatrix[1,1]*AMatrix[2,0])
      )-
      (AMatrix[0,1]*AMatrix[1,0]*AMatrix[2,2])
     )-
     (AMatrix[0,0]*AMatrix[1,2]*AMatrix[2,1])
    )<0.0 then begin
  // Reflection matrix, so flip y axis in case the tangent frame encodes a reflection
  Scale:=-1.0;
  AMatrix[2,0]:=-AMatrix[2,0];
  AMatrix[2,1]:=-AMatrix[2,1];
  AMatrix[2,2]:=-AMatrix[2,2];
 end else begin
  // Rotation matrix, so nothing is doing to do
  Scale:=1.0;
 end;
 begin
  // Convert to quaternion
  t:=AMatrix[0,0]+(AMatrix[1,1]+AMatrix[2,2]);
  if t>2.9999999 then begin
   result.x:=0.0;
   result.y:=0.0;
   result.z:=0.0;
   result.w:=1.0;
  end else if t>0.0000001 then begin
   s:=sqrt(1.0+t)*2.0;
   result.x:=(AMatrix[1,2]-AMatrix[2,1])/s;
   result.y:=(AMatrix[2,0]-AMatrix[0,2])/s;
   result.z:=(AMatrix[0,1]-AMatrix[1,0])/s;
   result.w:=s*0.25;
  end else if (AMatrix[0,0]>AMatrix[1,1]) and (AMatrix[0,0]>AMatrix[2,2]) then begin
   s:=sqrt(1.0+(AMatrix[0,0]-(AMatrix[1,1]+AMatrix[2,2])))*2.0;
   result.x:=s*0.25;
   result.y:=(AMatrix[1,0]+AMatrix[0,1])/s;
   result.z:=(AMatrix[2,0]+AMatrix[0,2])/s;
   result.w:=(AMatrix[1,2]-AMatrix[2,1])/s;
  end else if AMatrix[1,1]>AMatrix[2,2] then begin
   s:=sqrt(1.0+(AMatrix[1,1]-(AMatrix[0,0]+AMatrix[2,2])))*2.0;
   result.x:=(AMatrix[1,0]+AMatrix[0,1])/s;
   result.y:=s*0.25;
   result.z:=(AMatrix[2,1]+AMatrix[1,2])/s;
   result.w:=(AMatrix[2,0]-AMatrix[0,2])/s;
  end else begin
   s:=sqrt(1.0+(AMatrix[2,2]-(AMatrix[0,0]+AMatrix[1,1])))*2.0;
   result.x:=(AMatrix[2,0]+AMatrix[0,2])/s;
   result.y:=(AMatrix[2,1]+AMatrix[1,2])/s;
   result.z:=s*0.25;
   result.w:=(AMatrix[0,1]-AMatrix[1,0])/s;
  end;
  QuaternionNormalize(result);
 end;
 begin
  // Make sure, that we don't end up with 0 as w component
  if abs(result.w)<=Threshold then begin
   Renormalization:=sqrt(1.0-sqr(Threshold));
   result.x:=result.x*Renormalization;
   result.y:=result.y*Renormalization;
   result.z:=result.z*Renormalization;
   if result.w<0.0 then begin
    result.w:=-Threshold;
   end else begin
    result.w:=Threshold;
   end;
  end;
 end;
 if ((Scale<0.0) and (result.w>=0.0)) or ((Scale>=0.0) and (result.w<0.0)) then begin
  // Encode reflection into quaternion's w element by making sign of w negative,
  // if y axis needs to be flipped, otherwise it stays positive
  result.x:=-result.x;
  result.y:=-result.y;
  result.z:=-result.z;
  result.w:=-result.w;
 end;
end;

function QuaternionToTangentSpaceMatrix3x3(AQuaternion:TKraftQuaternion):TKraftMatrix3x3;
var qx2,qy2,qz2,qxqx2,qxqy2,qxqz2,qxqw2,qyqy2,qyqz2,qyqw2,qzqz2,qzqw2:TKraftScalar;
begin
 QuaternionNormalize(AQuaternion);
 qx2:=AQuaternion.x+AQuaternion.x;
 qy2:=AQuaternion.y+AQuaternion.y;
 qz2:=AQuaternion.z+AQuaternion.z;
 qxqx2:=AQuaternion.x*qx2;
 qxqy2:=AQuaternion.x*qy2;
 qxqz2:=AQuaternion.x*qz2;
 qxqw2:=AQuaternion.w*qx2;
 qyqy2:=AQuaternion.y*qy2;
 qyqz2:=AQuaternion.y*qz2;
 qyqw2:=AQuaternion.w*qy2;
 qzqz2:=AQuaternion.z*qz2;
 qzqw2:=AQuaternion.w*qz2;
 result[0,0]:=1.0-(qyqy2+qzqz2);
 result[0,1]:=qxqy2+qzqw2;
 result[0,2]:=qxqz2-qyqw2;
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=qxqy2-qzqw2;
 result[1,1]:=1.0-(qxqx2+qzqz2);
 result[1,2]:=qyqz2+qxqw2;
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=qxqz2+qyqw2;
 result[2,1]:=qyqz2-qxqw2;
 result[2,2]:=1.0-(qxqx2+qyqy2);
 if AQuaternion.w<0.0 then begin
  result[2,0]:=-result[2,0];
  result[2,1]:=-result[2,1];
  result[2,2]:=-result[2,2];
 end;
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function QuaternionFromMatrix4x4(const AMatrix:TKraftMatrix4x4):TKraftQuaternion;
var t,s:TKraftScalar;
begin
 t:=AMatrix[0,0]+(AMatrix[1,1]+AMatrix[2,2]);
 if t>2.9999999 then begin
  result.x:=0.0;
  result.y:=0.0;
  result.z:=0.0;
  result.w:=1.0;
 end else if t>0.0000001 then begin
  s:=sqrt(1.0+t)*2.0;
  result.x:=(AMatrix[1,2]-AMatrix[2,1])/s;
  result.y:=(AMatrix[2,0]-AMatrix[0,2])/s;
  result.z:=(AMatrix[0,1]-AMatrix[1,0])/s;
  result.w:=s*0.25;
 end else if (AMatrix[0,0]>AMatrix[1,1]) and (AMatrix[0,0]>AMatrix[2,2]) then begin
  s:=sqrt(1.0+(AMatrix[0,0]-(AMatrix[1,1]+AMatrix[2,2])))*2.0;
  result.x:=s*0.25;
  result.y:=(AMatrix[1,0]+AMatrix[0,1])/s;
  result.z:=(AMatrix[2,0]+AMatrix[0,2])/s;
  result.w:=(AMatrix[1,2]-AMatrix[2,1])/s;
 end else if AMatrix[1,1]>AMatrix[2,2] then begin
  s:=sqrt(1.0+(AMatrix[1,1]-(AMatrix[0,0]+AMatrix[2,2])))*2.0;
  result.x:=(AMatrix[1,0]+AMatrix[0,1])/s;
  result.y:=s*0.25;
  result.z:=(AMatrix[2,1]+AMatrix[1,2])/s;
  result.w:=(AMatrix[2,0]-AMatrix[0,2])/s;
 end else begin
  s:=sqrt(1.0+(AMatrix[2,2]-(AMatrix[0,0]+AMatrix[1,1])))*2.0;
  result.x:=(AMatrix[2,0]+AMatrix[0,2])/s;
  result.y:=(AMatrix[2,1]+AMatrix[1,2])/s;
  result.z:=s*0.25;
  result.w:=(AMatrix[0,1]-AMatrix[1,0])/s;
 end;
 QuaternionNormalize(result);
end;
{var xx,yx,zx,xy,yy,zy,xz,yz,zz,Trace,Radicand,Scale,TempX,TempY,TempZ,TempW:TKraftScalar;
    NegativeTrace,ZgtX,ZgtY,YgtX,LargestXorY,LargestYorZ,LargestZorX:boolean;
begin
 xx:=AMatrix[0,0];
 yx:=AMatrix[0,1];
 zx:=AMatrix[0,2];
 xy:=AMatrix[1,0];
 yy:=AMatrix[1,1];
 zy:=AMatrix[1,2];
 xz:=AMatrix[2,0];
 yz:=AMatrix[2,1];
 zz:=AMatrix[2,2];
 Trace:=(xx+yy)+zz;
 NegativeTrace:=Trace<0.0;
 ZgtX:=zz>xx;
 ZgtY:=zz>yy;
 YgtX:=yy>xx;
 LargestXorY:=NegativeTrace and ((not ZgtX) or not ZgtY);
 LargestYorZ:=NegativeTrace and (YgtX or ZgtX);
 LargestZorX:=NegativeTrace and (ZgtY or not YgtX);
 if LargestXorY then begin
  zz:=-zz;
  xy:=-xy;
 end;
 if LargestYorZ then begin
  xx:=-xx;
  yz:=-yz;
 end;
 if LargestZorX then begin
  yy:=-yy;
  zx:=-zx;
 end;
 Radicand:=((xx+yy)+zz)+1.0;
 Scale:=0.5/sqrt(Radicand);
 TempX:=(zy-yz)*Scale;
 TempY:=(xz-zx)*Scale;
 TempZ:=(yx-xy)*Scale;
 TempW:=Radicand*Scale;
 if LargestXorY then begin
  result.x:=TempW;
  result.y:=TempZ;
  result.z:=TempY;
  result.w:=TempX;
 end else begin
  result.x:=TempX;
  result.y:=TempY;
  result.z:=TempZ;
  result.w:=TempW;
 end;
 if LargestYorZ then begin
  TempX:=result.x;
  TempZ:=result.z;
  result.x:=result.y;
  result.y:=TempX;
  result.z:=result.w;
  result.w:=TempZ;
 end;
end;{}

function QuaternionToMatrix4x4(AQuaternion:TKraftQuaternion):TKraftMatrix4x4;
var qx2,qy2,qz2,qxqx2,qxqy2,qxqz2,qxqw2,qyqy2,qyqz2,qyqw2,qzqz2,qzqw2:TKraftScalar;
begin
 QuaternionNormalize(AQuaternion);
 qx2:=AQuaternion.x+AQuaternion.x;
 qy2:=AQuaternion.y+AQuaternion.y;
 qz2:=AQuaternion.z+AQuaternion.z;
 qxqx2:=AQuaternion.x*qx2;
 qxqy2:=AQuaternion.x*qy2;
 qxqz2:=AQuaternion.x*qz2;
 qxqw2:=AQuaternion.w*qx2;
 qyqy2:=AQuaternion.y*qy2;
 qyqz2:=AQuaternion.y*qz2;
 qyqw2:=AQuaternion.w*qy2;
 qzqz2:=AQuaternion.z*qz2;
 qzqw2:=AQuaternion.w*qz2;
 result[0,0]:=1.0-(qyqy2+qzqz2);
 result[0,1]:=qxqy2+qzqw2;
 result[0,2]:=qxqz2-qyqw2;
 result[0,3]:=0.0;
 result[1,0]:=qxqy2-qzqw2;
 result[1,1]:=1.0-(qxqx2+qzqz2);
 result[1,2]:=qyqz2+qxqw2;
 result[1,3]:=0.0;
 result[2,0]:=qxqz2+qyqw2;
 result[2,1]:=qyqz2-qxqw2;
 result[2,2]:=1.0-(qxqx2+qyqy2);
 result[2,3]:=0.0;
 result[3,0]:=0.0;
 result[3,1]:=0.0;
 result[3,2]:=0.0;
 result[3,3]:=1.0;
end;

function QuaternionToEuler(const AQuaternion:TKraftQuaternion):TKraftVector3; {$ifdef caninline}inline;{$endif}
begin
 result.x:=ArcTan2(2.0*((AQuaternion.x*AQuaternion.y)+(AQuaternion.z*AQuaternion.w)),1.0-(2.0*(sqr(AQuaternion.y)+sqr(AQuaternion.z))));
 result.y:=ArcSin(2.0*((AQuaternion.x*AQuaternion.z)-(AQuaternion.y*AQuaternion.w)));
 result.z:=ArcTan2(2.0*((AQuaternion.x*AQuaternion.w)+(AQuaternion.y*AQuaternion.z)),1.0-(2.0*(sqr(AQuaternion.z)+sqr(AQuaternion.w))));
end;

procedure QuaternionToAxisAngle(AQuaternion:TKraftQuaternion;var Axis:TKraftVector3;var Angle:TKraftScalar); {$ifdef caninline}inline;{$endif}
var SinAngle:TKraftScalar;
begin
 QuaternionNormalize(AQuaternion);
 SinAngle:=sqrt(1.0-sqr(AQuaternion.w));
 if abs(SinAngle)<EPSILON then begin
  SinAngle:=1.0;
 end;
 Angle:=2.0*ArcCos(AQuaternion.w);
 Axis.x:=AQuaternion.x/SinAngle;
 Axis.y:=AQuaternion.y/SinAngle;
 Axis.z:=AQuaternion.z/SinAngle;
end;

function QuaternionGenerator(AQuaternion:TKraftQuaternion):TKraftVector3; {$ifdef caninline}inline;{$endif}
var s:TKraftScalar;
begin
 s:=sqrt(1.0-sqr(AQuaternion.w));
 result.x:=AQuaternion.x;
 result.y:=AQuaternion.y;
 result.z:=AQuaternion.z;
 if s>0.0 then begin
  result:=Vector3ScalarMul(result,s);
 end;
 result:=Vector3ScalarMul(result,2.0*ArcTan2(s,AQuaternion.w));
end;

function QuaternionLerp(const q1,q2:TKraftQuaternion;const t:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
var it,sf:TKraftScalar;
begin
 if ((q1.x*q2.x)+(q1.y*q2.y)+(q1.z*q2.z)+(q1.w*q2.w))<0.0 then begin
  sf:=-1.0;
 end else begin
  sf:=1.0;
 end;
 it:=1.0-t;
 result.x:=(it*q1.x)+(t*(sf*q2.x));
 result.y:=(it*q1.y)+(t*(sf*q2.y));
 result.z:=(it*q1.z)+(t*(sf*q2.z));
 result.w:=(it*q1.w)+(t*(sf*q2.w));
end;

function QuaternionNlerp(const q1,q2:TKraftQuaternion;const t:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
var it,sf:TKraftScalar;
begin
 if ((q1.x*q2.x)+(q1.y*q2.y)+(q1.z*q2.z)+(q1.w*q2.w))<0.0 then begin
  sf:=-1.0;
 end else begin
  sf:=1.0;
 end;
 it:=1.0-t;
 result.x:=(it*q1.x)+(t*(sf*q2.x));
 result.y:=(it*q1.y)+(t*(sf*q2.y));
 result.z:=(it*q1.z)+(t*(sf*q2.z));
 result.w:=(it*q1.w)+(t*(sf*q2.w));
 QuaternionNormalize(result);
end;

function QuaternionSlerp(const q1,q2:TKraftQuaternion;const t:TKraftScalar):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
var Omega,co,so,s0,s1,s2:TKraftScalar;
begin
 co:=(q1.x*q2.x)+(q1.y*q2.y)+(q1.z*q2.z)+(q1.w*q2.w);
 if co<0.0 then begin
  co:=-co;
  s2:=-1.0;
 end else begin
  s2:=1.0;
 end;
 if (1.0-co)>EPSILON then begin
  Omega:=ArcCos(co);
  so:=sin(Omega);
  s0:=sin((1.0-t)*Omega)/so;
  s1:=sin(t*Omega)/so;
 end else begin
  s0:=1.0-t;
  s1:=t;
 end;
 result.x:=(s0*q1.x)+(s1*(s2*q2.x));
 result.y:=(s0*q1.y)+(s1*(s2*q2.y));
 result.z:=(s0*q1.z)+(s1*(s2*q2.z));
 result.w:=(s0*q1.w)+(s1*(s2*q2.w));
end;

function QuaternionIntegrate(const q:TKraftQuaternion;const Omega:TKraftVector3;const DeltaTime:TKraftScalar):TKraftQuaternion;
var ThetaLenSquared,ThetaLen,s:TKraftScalar;
    DeltaQ:TKraftQuaternion;
    Theta:TKraftVector3;
begin
 Theta:=Vector3ScalarMul(Omega,DeltaTime*0.5);
 ThetaLenSquared:=Vector3LengthSquared(Theta);
 if (sqr(ThetaLenSquared)/24.0)<EPSILON then begin
  DeltaQ.w:=1.0-(ThetaLenSquared*0.5);
  s:=1.0-(ThetaLenSquared/6.0);
 end else begin
  ThetaLen:=sqrt(ThetaLenSquared);
  DeltaQ.w:=cos(ThetaLen);
  s:=sin(ThetaLen)/ThetaLen;
 end;
 DeltaQ.x:=Theta.x*s;
 DeltaQ.y:=Theta.y*s;
 DeltaQ.z:=Theta.z*s;
 result:=QuaternionMul(DeltaQ,q);
end;

function QuaternionSpin(const q:TKraftQuaternion;const Omega:TKraftVector3;const DeltaTime:TKraftScalar):TKraftQuaternion; overload;
var wq:TKraftQuaternion;
begin
 wq.x:=Omega.x*DeltaTime;
 wq.y:=Omega.y*DeltaTime;
 wq.z:=Omega.z*DeltaTime;
 wq.w:=0.0;
 result:=QuaternionTermNormalize(QuaternionAdd(q,QuaternionScalarMul(QuaternionMul(wq,q),0.5)));
end;

procedure QuaternionDirectSpin(var q:TKraftQuaternion;const Omega:TKraftVector3;const DeltaTime:TKraftScalar); overload;
var wq,tq:TKraftQuaternion;
begin
 wq.x:=Omega.x*DeltaTime;
 wq.y:=Omega.y*DeltaTime;
 wq.z:=Omega.z*DeltaTime;
 wq.w:=0.0;
 tq:=QuaternionAdd(q,QuaternionScalarMul(QuaternionMul(wq,q),0.5));
 q:=QuaternionTermNormalize(tq);
end;

function QuaternionFromToRotation(const FromDirection,ToDirection:TKraftVector3):TKraftQuaternion; {$ifdef caninline}inline;{$endif}
var t:TKraftVector3;
begin
 t:=Vector3Cross(Vector3Norm(FromDirection),Vector3Norm(ToDirection));
 result.x:=t.x;
 result.y:=t.y;
 result.z:=t.z;
 result.w:=sqrt((sqr(FromDirection.x)+sqr(FromDirection.y)+sqr(FromDirection.z))*
                (sqr(ToDirection.x)+sqr(ToDirection.y)+sqr(ToDirection.z)))+
               ((FromDirection.x*ToDirection.x)+(FromDirection.y*ToDirection.y)+(FromDirection.z*ToDirection.z));
end;

function AABBCost(const AABB:TKraftAABB):TKraftScalar; {$ifdef caninline}inline;{$endif}
begin
// result:=(AABB.Max.x-AABB.Min.x)+(AABB.Max.y-AABB.Min.y)+(AABB.Max.z-AABB.Min.z); // Manhattan distance
 result:=(AABB.Max.x-AABB.Min.x)*(AABB.Max.y-AABB.Min.y)*(AABB.Max.z-AABB.Min.z); // Volume
end;
                    
function AABBCombine(const AABB,WithAABB:TKraftAABB):TKraftAABB; {$ifdef caninline}inline;{$endif}
begin
 result.Min.x:=Min(AABB.Min.x,WithAABB.Min.x);
 result.Min.y:=Min(AABB.Min.y,WithAABB.Min.y);
 result.Min.z:=Min(AABB.Min.z,WithAABB.Min.z);
 result.Max.x:=Max(AABB.Max.x,WithAABB.Max.x);
 result.Max.y:=Max(AABB.Max.y,WithAABB.Max.y);
 result.Max.z:=Max(AABB.Max.z,WithAABB.Max.z);
end;

function AABBCombineVector3(const AABB:TKraftAABB;v:TKraftVector3):TKraftAABB; {$ifdef caninline}inline;{$endif}
begin
 result.Min.x:=Min(AABB.Min.x,v.x);
 result.Min.y:=Min(AABB.Min.y,v.y);
 result.Min.z:=Min(AABB.Min.z,v.z);
 result.Max.x:=Max(AABB.Max.x,v.x);
 result.Max.y:=Max(AABB.Max.y,v.y);
 result.Max.z:=Max(AABB.Max.z,v.z);
end;

function AABBIntersect(const AABB,WithAABB:TKraftAABB;Threshold:TKraftScalar=EPSILON):boolean; {$ifdef caninline}inline;{$endif}
begin
 result:=(((AABB.Max.x+Threshold)>=(WithAABB.Min.x-Threshold)) and ((AABB.Min.x-Threshold)<=(WithAABB.Max.x+Threshold))) and
         (((AABB.Max.y+Threshold)>=(WithAABB.Min.y-Threshold)) and ((AABB.Min.y-Threshold)<=(WithAABB.Max.y+Threshold))) and
         (((AABB.Max.z+Threshold)>=(WithAABB.Min.z-Threshold)) and ((AABB.Min.z-Threshold)<=(WithAABB.Max.z+Threshold)));
end;

function AABBContains(const InAABB,AABB:TKraftAABB):boolean; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=((InAABB.Min.x-EPSILON)<=(AABB.Min.x+EPSILON)) and ((InAABB.Min.y-EPSILON)<=(AABB.Min.y+EPSILON)) and ((InAABB.Min.z-EPSILON)<=(AABB.Min.z+EPSILON)) and
         ((InAABB.Max.x+EPSILON)>=(AABB.Min.x+EPSILON)) and ((InAABB.Max.y+EPSILON)>=(AABB.Min.y+EPSILON)) and ((InAABB.Max.z+EPSILON)>=(AABB.Min.z+EPSILON)) and
         ((InAABB.Min.x-EPSILON)<=(AABB.Max.x-EPSILON)) and ((InAABB.Min.y-EPSILON)<=(AABB.Max.y-EPSILON)) and ((InAABB.Min.z-EPSILON)<=(AABB.Max.z-EPSILON)) and
         ((InAABB.Max.x+EPSILON)>=(AABB.Max.x-EPSILON)) and ((InAABB.Max.y+EPSILON)>=(AABB.Max.y-EPSILON)) and ((InAABB.Max.z+EPSILON)>=(AABB.Max.z-EPSILON));
end;

function AABBContains(const AABB:TKraftAABB;Vector:TKraftVector3):boolean; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=((Vector.x>=(AABB.Min.x-EPSILON)) and (Vector.x<=(AABB.Max.x+EPSILON))) and
         ((Vector.y>=(AABB.Min.y-EPSILON)) and (Vector.y<=(AABB.Max.y+EPSILON))) and
         ((Vector.z>=(AABB.Min.z-EPSILON)) and (Vector.z<=(AABB.Max.z+EPSILON)));
end;

function AABBTransform(const DstAABB:TKraftAABB;const Transform:TKraftMatrix4x4):TKraftAABB; {$ifdef caninline}inline;{$endif}
var i,j:longint;
    a,b:TKraftScalar;
begin
 result.Min:=Vector3(Transform[3,0],Transform[3,1],Transform[3,2]);
 result.Max:=result.Min;
 for i:=0 to 2 do begin
  for j:=0 to 2 do begin
   a:=Transform[j,i]*DstAABB.Min.xyz[j];
   b:=Transform[j,i]*DstAABB.Max.xyz[j];
   if a<b then begin
    result.Min.xyz[i]:=result.Min.xyz[i]+a;
    result.Max.xyz[i]:=result.Max.xyz[i]+b;
   end else begin
    result.Min.xyz[i]:=result.Min.xyz[i]+b;
    result.Max.xyz[i]:=result.Max.xyz[i]+a;
   end;
  end;
 end;
end;

function AABBRayIntersection(const AABB:TKraftAABB;const Origin,Direction:TKraftVector3;var Time:TKraftScalar):boolean; overload; {$ifdef caninline}inline;{$endif}
var InvDirection,a,b,AABBMin,AABBMax:TKraftVector3;
    TimeMin,TimeMax:TKraftScalar;
begin
 if Direction.x<>0.0 then begin
  InvDirection.x:=1.0/Direction.x;
 end else begin
  InvDirection.x:=0.0;
 end;
 if Direction.y<>0.0 then begin
  InvDirection.y:=1.0/Direction.y;
 end else begin
  InvDirection.y:=0.0;
 end;
 if Direction.z<>0.0 then begin
  InvDirection.z:=1.0/Direction.z;
 end else begin
  InvDirection.z:=0.0;
 end;
 a.x:=(AABB.Min.x-Origin.x)*InvDirection.x;
 a.y:=(AABB.Min.y-Origin.y)*InvDirection.y;
 a.z:=(AABB.Min.z-Origin.z)*InvDirection.z;
 b.x:=(AABB.Max.x-Origin.x)*InvDirection.x;
 b.y:=(AABB.Max.y-Origin.y)*InvDirection.y;
 b.z:=(AABB.Max.z-Origin.z)*InvDirection.z;
 if a.x<b.x then begin
  AABBMin.x:=a.x;
  AABBMax.x:=b.x;
 end else begin
  AABBMin.x:=b.x;
  AABBMax.x:=a.x;
 end;
 if a.y<b.y then begin
  AABBMin.y:=a.y;
  AABBMax.y:=b.y;
 end else begin
  AABBMin.y:=b.y;
  AABBMax.y:=a.y;
 end;
 if a.z<b.z then begin
  AABBMin.z:=a.z;
  AABBMax.z:=b.z;
 end else begin
  AABBMin.z:=b.z;
  AABBMax.z:=a.z;
 end;
 if AABBMin.x<AABBMin.y then begin
  if AABBMin.x<AABBMin.z then begin
   TimeMin:=AABBMin.x;
  end else begin
   TimeMin:=AABBMin.z;
  end;
 end else begin
  if AABBMin.y<AABBMin.z then begin
   TimeMin:=AABBMin.y;
  end else begin
   TimeMin:=AABBMin.z;
  end;
 end;
 if AABBMax.x>AABBMax.y then begin
  if AABBMax.x>AABBMax.z then begin
   TimeMax:=AABBMax.x;
  end else begin
   TimeMax:=AABBMax.z;
  end;
 end else begin
  if AABBMax.y>AABBMax.z then begin
   TimeMax:=AABBMax.y;
  end else begin
   TimeMax:=AABBMax.z;
  end;
 end;
 if (TimeMax<0) or (TimeMin>TimeMax) then begin
  Time:=TimeMax;
  result:=false;
 end else begin
  Time:=TimeMin;
  result:=true;
 end;
end;

function AABBRayIntersect(const AABB:TKraftAABB;const Origin,Direction:TKraftVector3):boolean; {$ifdef caninline}inline;{$endif}
var Center,BoxExtents,Diff:TKraftVector3;
begin
 Center:=Vector3ScalarMul(Vector3Add(AABB.Min,AABB.Max),0.5);
 BoxExtents:=Vector3Sub(Center,AABB.Min);
 Diff:=Vector3Sub(Origin,Center);
 result:=not ((((abs(Diff.x)>BoxExtents.x) and ((Diff.x*Direction.x)>=0)) or
               ((abs(Diff.y)>BoxExtents.y) and ((Diff.y*Direction.y)>=0)) or
               ((abs(Diff.z)>BoxExtents.z) and ((Diff.z*Direction.z)>=0))) or
              ((abs((Direction.y*Diff.z)-(Direction.z*Diff.y))>((BoxExtents.y*abs(Direction.z))+(BoxExtents.z*abs(Direction.y)))) or
               (abs((Direction.z*Diff.x)-(Direction.x*Diff.z))>((BoxExtents.x*abs(Direction.z))+(BoxExtents.z*abs(Direction.x)))) or
               (abs((Direction.x*Diff.y)-(Direction.y*Diff.x))>((BoxExtents.x*abs(Direction.y))+(BoxExtents.y*abs(Direction.x))))));
end;

function ClosestPointToAABB(const AABB:TKraftAABB;const Point:TKraftVector3;const ClosestPointOnAABB:PKraftVector3=nil):TKraftScalar; {$ifdef caninline}inline;{$endif}
var ClosestPoint:TKraftVector3;
begin
 ClosestPoint.x:=Min(Max(Point.x,AABB.Min.x),AABB.Max.x);
 ClosestPoint.y:=Min(Max(Point.y,AABB.Min.y),AABB.Max.y);
 ClosestPoint.z:=Min(Max(Point.z,AABB.Min.z),AABB.Max.z);
 if assigned(ClosestPointOnAABB) then begin
  ClosestPointOnAABB^:=ClosestPoint;
 end;
 result:=Vector3Dist(ClosestPoint,Point);
end;

function SquaredDistanceFromPointToAABB(const AABB:TKraftAABB;const Point:TKraftVector3):TKraftScalar; {$ifdef caninline}inline;{$endif}
var ClosestPoint:TKraftVector3;
begin
 ClosestPoint.x:=Min(Max(Point.x,AABB.Min.x),AABB.Max.x);
 ClosestPoint.y:=Min(Max(Point.y,AABB.Min.y),AABB.Max.y);
 ClosestPoint.z:=Min(Max(Point.z,AABB.Min.z),AABB.Max.z);
 result:=Vector3DistSquared(ClosestPoint,Point);
end;

function SphereFromAABB(const AABB:TKraftAABB):TKraftSphere; {$ifdef caninline}inline;{$endif}
begin
 result.Center:=Vector3Avg(AABB.Min,AABB.Max);
 result.Radius:=Vector3Dist(AABB.Min,AABB.Max)*0.5;
end;

function RayIntersectTriangle(const RayOrigin,RayDirection,v0,v1,v2:TKraftVector3;var Time,u,v:TKraftScalar):boolean; overload;
var e0,e1,p,t,q:TKraftVector3;
    Determinant,InverseDeterminant:TKraftScalar;
begin
 result:=false;

 e0.x:=v1.x-v0.x;
 e0.y:=v1.y-v0.y;
 e0.z:=v1.z-v0.z;
 e1.x:=v2.x-v0.x;
 e1.y:=v2.y-v0.y;
 e1.z:=v2.z-v0.z;

 p.x:=(RayDirection.y*e1.z)-(RayDirection.z*e1.y);
 p.y:=(RayDirection.z*e1.x)-(RayDirection.x*e1.z);
 p.z:=(RayDirection.x*e1.y)-(RayDirection.y*e1.x);

 Determinant:=(e0.x*p.x)+(e0.y*p.y)+(e0.z*p.z);
 if Determinant<EPSILON then begin
  exit;
 end;

 t.x:=RayOrigin.x-v0.x;
 t.y:=RayOrigin.y-v0.y;
 t.z:=RayOrigin.z-v0.z;

 u:=(t.x*p.x)+(t.y*p.y)+(t.z*p.z);
 if (u<0.0) or (u>Determinant) then begin
  exit;
 end;

 q.x:=(t.y*e0.z)-(t.z*e0.y);
 q.y:=(t.z*e0.x)-(t.x*e0.z);
 q.z:=(t.x*e0.y)-(t.y*e0.x);

 v:=(RayDirection.x*q.x)+(RayDirection.y*q.y)+(RayDirection.z*q.z);
 if (v<0.0) or ((u+v)>Determinant) then begin
  exit;
 end;

 Time:=(e1.x*q.x)+(e1.y*q.y)+(e1.z*q.z);
 if abs(Determinant)<EPSILON then begin
  Determinant:=0.01;
 end;
 InverseDeterminant:=1.0/Determinant;
 Time:=Time*InverseDeterminant;
 u:=u*InverseDeterminant;
 v:=v*InverseDeterminant;

 result:=true;
end;

function IsPointsSameSide(const p0,p1,Origin,Direction:TKraftVector3):boolean; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Vector3Dot(Vector3Cross(Direction,Vector3Sub(p0,Origin)),Vector3Cross(Direction,Vector3Sub(p1,Origin)))>=0.0;
end;

function PointInTriangle(const p0,p1,p2,Normal,p:TKraftVector3):boolean; overload; {$ifdef caninline}inline;{$endif}
var r0,r1,r2:TKraftScalar;
begin
 r0:=Vector3Dot(Vector3Cross(Vector3Sub(p1,p0),Normal),Vector3Sub(p,p0));
 r1:=Vector3Dot(Vector3Cross(Vector3Sub(p2,p1),Normal),Vector3Sub(p,p1));
 r2:=Vector3Dot(Vector3Cross(Vector3Sub(p0,p2),Normal),Vector3Sub(p,p2));
 result:=((r0>0.0) and (r1>0.0) and (r2>0.0)) or ((r0<=0.0) and (r1<=0.0) and (r2<=0.0));
end;

function PointInTriangle(const p0,p1,p2,p:TKraftVector3):boolean; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=IsPointsSameSide(p,p0,p1,Vector3Sub(p2,p1)) and
         IsPointsSameSide(p,p1,p0,Vector3Sub(p2,p0)) and
         IsPointsSameSide(p,p2,p0,Vector3Sub(p1,p0));
end;

function SquaredDistanceFromPointToTriangle(const p,a,b,c:TKraftVector3):TKraftScalar;
var ab,ac,bc,pa,pb,pc,ap,bp,cp,n:TKraftVector3;
    snom,sdenom,tnom,tdenom,unom,udenom,vc,vb,va,u,v,w:TKraftScalar;
begin

 ab.x:=b.x-a.x;
 ab.y:=b.y-a.y;
 ab.z:=b.z-a.z;

 ac.x:=c.x-a.x;
 ac.y:=c.y-a.y;
 ac.z:=c.z-a.z;

 bc.x:=c.x-b.x;
 bc.y:=c.y-b.y;
 bc.z:=c.z-b.z;

 pa.x:=p.x-a.x;
 pa.y:=p.y-a.y;
 pa.z:=p.z-a.z;

 pb.x:=p.x-b.x;
 pb.y:=p.y-b.y;
 pb.z:=p.z-b.z;

 pc.x:=p.x-c.x;
 pc.y:=p.y-c.y;
 pc.z:=p.z-c.z;

 // Determine the parametric position s for the projection of P onto AB (i.e. PPU2 = A+s*AB, where
 // s = snom/(snom+sdenom), and then parametric position t for P projected onto AC
 snom:=(ab.x*pa.x)+(ab.y*pa.y)+(ab.z*pa.z);
 sdenom:=(pb.x*(a.x-b.x))+(pb.y*(a.y-b.y))+(pb.z*(a.z-b.z));
 tnom:=(ac.x*pa.x)+(ac.y*pa.y)+(ac.z*pa.z);
 tdenom:=(pc.x*(a.x-c.x))+(pc.y*(a.y-c.y))+(pc.z*(a.z-c.z));
 if (snom<=0.0) and (tnom<=0.0) then begin
  // Vertex voronoi region hit early out
  result:=sqr(a.x-p.x)+sqr(a.y-p.y)+sqr(a.z-p.z);
  exit;
 end;

 // Parametric position u for P projected onto BC
 unom:=(bc.x*pb.x)+(bc.y*pb.y)+(bc.z*pb.z);
 udenom:=(pc.x*(b.x-c.x))+(pc.y*(b.y-c.y))+(pc.z*(b.z-c.z));
 if (sdenom<=0.0) and (unom<=0.0) then begin
  // Vertex voronoi region hit early out
  result:=sqr(b.x-p.x)+sqr(b.y-p.y)+sqr(b.z-p.z);
  exit;
 end;
 if (tdenom<=0.0) and (udenom<=0.0) then begin
  // Vertex voronoi region hit early out
  result:=sqr(c.x-p.x)+sqr(c.y-p.y)+sqr(c.z-p.z);
  exit;
 end;

 // Determine if P is outside (or on) edge AB by finding the area formed by vectors PA, PB and
 // the triangle normal. A scalar triple product is used. P is outside (or on) AB if the triple
 // scalar product [N PA PB] <= 0
 n.x:=(ab.y*ac.z)-(ab.z*ac.y);
 n.y:=(ab.z*ac.x)-(ab.x*ac.z);
 n.z:=(ab.x*ac.y)-(ab.y*ac.x);
 ap.x:=a.x-p.x;
 ap.y:=a.y-p.y;
 ap.z:=a.z-p.z;
 bp.x:=b.x-p.x;
 bp.y:=b.y-p.y;
 bp.z:=b.z-p.z;
 vc:=(n.x*((ap.y*bp.z)-(ap.z*bp.y)))+(n.y*((ap.z*bp.x)-(ap.x*bp.z)))+(n.z*((ap.x*bp.y)-(ap.y*bp.x)));

 // If P is outside of AB (signed area <= 0) and within voronoi feature region, then return
 // projection of P onto AB
 if (vc<=0.0) and (snom>=0.0) and (sdenom>=0.0) then begin
  u:=snom/(snom+sdenom);
  result:=sqr((a.x+(ab.x*u))-p.x)+sqr((a.y+(ab.y*u))-p.y)+sqr((a.z+(ab.z*u))-p.z);
  exit;
 end;

 // Repeat the same test for P onto BC
 cp.x:=c.x-p.x;
 cp.y:=c.y-p.y;
 cp.z:=c.z-p.z;
 va:=(n.x*((bp.y*cp.z)-(bp.z*cp.y)))+(n.y*((bp.z*cp.x)-(bp.x*cp.z)))+(n.z*((bp.x*cp.y)-(bp.y*cp.x)));
 if (va<=0.0) and (unom>=0.0) and (udenom>=0.0) then begin
  v:=unom/(unom+udenom);
  result:=sqr((b.x+(bc.x*v))-p.x)+sqr((b.y+(bc.y*v))-p.y)+sqr((b.z+(bc.z*v))-p.z);
  exit;
 end;

 // Repeat the same test for P onto CA
 vb:=(n.x*((cp.y*ap.z)-(cp.z*ap.y)))+(n.y*((cp.z*ap.x)-(cp.x*ap.z)))+(n.z*((cp.x*ap.y)-(cp.y*ap.x)));
 if (vb<=0.0) and (tnom>=0.0) and (tdenom>=0.0) then begin
  w:=tnom/(tnom+tdenom);
  result:=sqr((a.x+(ac.x*w))-p.x)+sqr((a.y+(ac.y*w))-p.y)+sqr((a.z+(ac.z*w))-p.z);
  exit;
 end;

 // P must project onto inside face. Find closest point using the barycentric coordinates
 w:=1.0/(va+vb+vc);
 u:=va*w;
 v:=vb*w;
 w:=(1.0-u)-v;

 result:=sqr(((a.x*u)+(b.x*v)+(c.x*w))-p.x)+sqr(((a.y*u)+(b.y*v)+(c.y*w))-p.y)+sqr(((a.z*u)+(b.z*v)+(c.z*w))-p.z);

end;

function SegmentSqrDistance(const FromVector,ToVector,p:TKraftVector3;out Nearest:TKraftVector3):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
var t,DotUV:TKraftScalar;
    Diff,v:TKraftVector3;
begin
 Diff:=Vector3Sub(p,FromVector);
 v:=Vector3Sub(ToVector,FromVector);
 t:=Vector3Dot(v,Diff);
 if t>0.0 then begin
  DotUV:=Vector3LengthSquared(v);
  if t<DotUV then begin
   t:=t/DotUV;
   Diff:=Vector3Sub(Diff,Vector3ScalarMul(v,t));
  end else begin
   t:=1;
   Diff:=Vector3Sub(Diff,v);
  end;
 end else begin
  t:=0.0;
 end;
 Nearest:=Vector3Lerp(FromVector,ToVector,t);
 result:=Vector3LengthSquared(Diff);
end;

function ClipSegmentToPlane(const Plane:TKraftPlane;var p0,p1:TKraftVector3):boolean;
var d0,d1:TKraftScalar;
    o0,o1:boolean;
begin
 d0:=PlaneVectorDistance(Plane,p0);
 d1:=PlaneVectorDistance(Plane,p1);
 o0:=d0<0.0;
 o1:=d1<0.0;
 if o0 and o1 then begin
  // Both points are below which means that the whole line segment is below => return false
  result:=false;
 end else begin
  // At least one point is above or in the plane which means that the line segment is above => return true
  if (o0<>o1) and (abs(d0-d1)>EPSILON) then begin
   if o0 then begin
    // p1 is above or in the plane which means that the line segment is above => clip l0
    p0:=Vector3Add(p0,Vector3ScalarMul(Vector3Sub(p1,p0),d0/(d0-d1)));
   end else begin
    // p0 is above or in the plane which means that the line segment is above => clip l1
    p1:=Vector3Add(p0,Vector3ScalarMul(Vector3Sub(p1,p0),d0/(d0-d1)));
   end;
  end else begin
   // Near parallel case => no clipping
  end;
  result:=true;
 end;
end;

function SegmentSegmentDistanceSq(out t0,t1:single;seg0,seg1:TKraftRelativeSegment):single;
var kDiff:TKraftVector3;
    fA00,fA01,fA11,fB0,fC,fDet,fB1,fS,fT,fSqrDist,fTmp,fInvDet:TKraftScalar;
begin
 kDiff:=Vector3Sub(seg0.Origin,seg1.Origin);
 fA00:=Vector3LengthSquared(seg0.Delta);
 fA01:=-Vector3Dot(seg0.Delta,seg1.Delta);
 fA11:=Vector3LengthSquared(seg1.Delta);
 fB0:=Vector3Dot(kDiff,seg0.Delta);
 fC:=Vector3LengthSquared(kDiff);
 fDet:=abs((fA00*fA11)-(fA01*fA01));
 if fDet>=EPSILON then begin
  // line segments are not parallel
  fB1:=-Vector3Dot(kDiff,seg1.Delta);
  fS:=(fA01*fB1)-(fA11*fB0);
  fT:=(fA01*fB0)-(fA00*fB1);
  if fS>=0.0 then begin
   if fS<=fDet then begin
    if fT>=0.0 then begin
     if fT<=fDet then begin // region 0 (interior)
      // minimum at two interior points of 3D lines
      fInvDet:=1.0/fDet;
      fS:=fS*fInvDet;
      fT:=fT*fInvDet;
      fSqrDist:=(fS*((fA00*fS)+(fA01*fT)+(2.0*fB0)))+(fT*((fA01*fS)+(fA11*fT)+(2.0*fB1)))+fC;
     end else begin // region 3 (side)
      fT:=1.0;
      fTmp:=fA01+fB0;
      if fTmp>=0.0 then begin
       fS:=0.0;
       fSqrDist:=fA11+(2.0*fB1)+fC;
      end else if (-fTmp)>=fA00 then begin
       fS:=1.0;
       fSqrDist:=fA00+fA11+fC+(2.0*(fB1+fTmp));
      end else begin
       fS:=-fTmp/fA00;
       fSqrDist:=fTmp*fS+fA11+(2.0*fB1)+fC;
      end;
     end;
    end else begin // region 7 (side)
     fT:=0.0;
     if fB0>=0.0 then begin
      fS:=0.0;
      fSqrDist:=fC;
     end else if (-fB0)>=fA00 then begin
      fS:=1.0;
      fSqrDist:=fA00+(2.0*fB0)+fC;
     end else begin
      fS:=(-fB0)/fA00;
      fSqrDist:=(fB0*fS)+fC;
     end;
    end;
   end else begin
    if fT>=0.0 then begin
     if fT<=fDet then begin // region 1 (side)
      fS:=1.0;
      fTmp:=fA01+fB1;
      if fTmp>=0.0 then begin
       fT:=0.0;
       fSqrDist:=fA00+(2.0*fB0)+fC;
      end else if (-fTmp)>=fA11 then begin
       fT:=1.0;
       fSqrDist:=fA00+fA11+fC+(2.0*(fB0+fTmp));
      end else begin
       fT:=(-fTmp)/fA11;
       fSqrDist:=(fTmp*fT)+fA00+(2.0*fB0)+fC;
      end;
     end else begin // region 2 (corner)
      fTmp:=fA01+fB0;
      if (-fTmp)<=fA00 then begin
       fT:=1.0;
       if fTmp>=0.0 then begin
        fS:=0.0;
        fSqrDist:=fA11+(2.0*fB1)+fC;
       end else begin
        fS:=(-fTmp)/fA00;
        fSqrDist:=(fTmp*fS)+fA11+(2.0*fB1)+fC;
       end;
      end else begin
       fS:=1.0;
       fTmp:=fA01+fB1;
       if fTmp>=0.0 then begin
        fT:=0.0;
        fSqrDist:=fA00+(2.0*fB0)+fC;
       end else if (-fTmp)>=fA11 then begin
        fT:=1.0;
        fSqrDist:=fA00+fA11+fC+(2.0*(fB0+fTmp));
       end else begin
        fT:=(-fTmp)/fA11;
        fSqrDist:=(fTmp*fT)+fA00+(2.0*fB0)+fC;
       end;
      end;
     end;
    end else begin // region 8 (corner)
     if (-fB0)<fA00 then begin
      fT:=0.0;
      if fB0>=0.0 then begin
       fS:=0.0;
       fSqrDist:=fC;
      end else begin
       fS:=(-fB0)/fA00;
       fSqrDist:=(fB0*fS)+fC;
      end;
     end else begin
      fS:=1.0;
      fTmp:=fA01+fB1;
      if fTmp>=0.0 then begin
       fT:=0.0;
       fSqrDist:=fA00+(2.0*fB0)+fC;
      end else if (-fTmp)>=fA11 then begin
       fT:=1.0;
       fSqrDist:=fA00+fA11+fC+(2.0*(fB0+fTmp));
      end else begin
       fT:=(-fTmp)/fA11;
       fSqrDist:=(fTmp*fT)+fA00+(2.0*fB0)+fC;
      end;
      end;
    end;
   end;
  end else begin
   if fT>=0.0 then begin
    if fT<=fDet then begin // region 5 (side)
     fS:=0.0;
     if fB1>=0.0 then begin
      fT:=0.0;
      fSqrDist:=fC;
     end else if (-fB1)>=fA11 then begin
      fT:=1.0;
      fSqrDist:=fA11+(2.0*fB1)+fC;
     end else begin
      fT:=(-fB1)/fA11;
      fSqrDist:=fB1*fT+fC;
     end
    end else begin // region 4 (corner)
     fTmp:=fA01+fB0;
     if fTmp<0.0 then begin
      fT:=1.0;
      if (-fTmp)>=fA00 then begin
       fS:=1.0;
       fSqrDist:=fA00+fA11+fC+(2.0*(fB1+fTmp));
      end else begin
       fS:=(-fTmp)/fA00;
       fSqrDist:=fTmp*fS+fA11+(2.0*fB1)+fC;
      end;
     end else begin
      fS:=0.0;
      if fB1>=0.0 then begin
       fT:=0.0;
       fSqrDist:=fC;
      end else if (-fB1)>=fA11 then begin
       fT:=1.0;
       fSqrDist:=fA11+(2.0*fB1)+fC;
      end else begin
       fT:=(-fB1)/fA11;
       fSqrDist:=(fB1*fT)+fC;
      end;
     end;
    end;
   end else begin // region 6 (corner)
    if fB0<0.0 then begin
     fT:=0.0;
     if (-fB0)>=fA00 then begin
      fS:=1.0;
      fSqrDist:=fA00+(2.0*fB0)+fC;
     end else begin
      fS:=(-fB0)/fA00;
      fSqrDist:=(fB0*fS)+fC;
     end;
    end else begin
     fS:=0.0;
     if fB1>=0.0 then begin
      fT:=0.0;
      fSqrDist:=fC;
     end else if (-fB1)>=fA11 then begin
      fT:=1.0;
      fSqrDist:=fA11+(2.0*fB1)+fC;
     end else begin
      fT:=(-fB1)/fA11;
      fSqrDist:=(fB1*fT)+fC;
     end;
    end;
   end;
  end;
 end else begin // line segments are parallel
  if fA01>0.0 then begin // direction vectors form an obtuse angle
   if fB0>=0.0 then begin
    fS:=0.0;
    fT:=0.0;
    fSqrDist:=fC;
   end else if (-fB0)<=fA00 then begin
    fS:=(-fB0)/fA00;
    fT:=0.0;
    fSqrDist:=(fB0*fS)+fC;
   end else begin
    fB1:=-Vector3Dot(kDiff,seg1.Delta);
    fS:=1.0;
    fTmp:=fA00+fB0;
    if (-fTmp)>=fA01 then begin
     fT:=1.0;
     fSqrDist:=fA00+fA11+fC+(2.0*(fA01+fB0+fB1));
    end else begin
     fT:=(-fTmp)/fA01;
     fSqrDist:=fA00+(2.0*fB0)+fC+(fT*((fA11*fT)+(2.0*(fA01+fB1))));
    end;
   end;
  end else begin // direction vectors form an acute angle
   if (-fB0)>=fA00 then begin
    fS:=1.0;
    fT:=0.0;
    fSqrDist:=fA00+(2.0*fB0)+fC;
   end else if fB0<=0.0 then begin
    fS:=(-fB0)/fA00;
    fT:=0.0;
    fSqrDist:=(fB0*fS)+fC;
   end else begin
    fB1:=-Vector3Dot(kDiff,seg1.Delta);
    fS:=0.0;
    if fB0>=(-fA01) then begin
     fT:=1.0;
     fSqrDist:=fA11+(2.0*fB1)+fC;
    end else begin
     fT:=(-fB0)/fA01;
     fSqrDist:=fC+(fT*((2.0)*fB1)+(fA11*fT));
    end;
   end;
  end;
 end;
 t0:=fS;
 t1:=fT;
 result:=abs(fSqrDist);
end;

procedure SIMDSegment(out Segment:TKraftSegment;const p0,p1:TKraftVector3); overload;
begin
 Segment.Points[0]:=p0;
 Segment.Points[1]:=p1;
end;

function SIMDSegment(const p0,p1:TKraftVector3):TKraftSegment; overload;
begin
 result.Points[0]:=p0;
 result.Points[1]:=p1;
end;

function SIMDSegmentSquaredDistanceTo(const Segment:TKraftSegment;const p:TKraftVector3):TKraftScalar;
var pq,pp:TKraftVector3;
    e,f:TKraftScalar;
begin
 pq:=Vector3Sub(Segment.Points[1],Segment.Points[0]);
 pp:=Vector3Sub(p,Segment.Points[0]);
 e:=Vector3Dot(pp,pq);
 if e<=0.0 then begin
  result:=Vector3LengthSquared(pp);
 end else begin
  f:=Vector3LengthSquared(pq);
  if e<f then begin
   result:=Vector3LengthSquared(pp)-(sqr(e)/f);
  end else begin
   result:=Vector3LengthSquared(Vector3Sub(p,Segment.Points[1]));
  end;
 end;
end;

procedure SIMDSegmentClosestPointTo(const Segment:TKraftSegment;const p:TKraftVector3;out Time:TKraftScalar;out ClosestPoint:TKraftVector3);
var u,v:TKraftVector3;
begin
 u:=Vector3Sub(Segment.Points[1],Segment.Points[0]);
 v:=Vector3Sub(p,Segment.Points[0]);
 Time:=Vector3Dot(u,v)/Vector3LengthSquared(u);
 if Time<=0.0 then begin
  ClosestPoint:=Segment.Points[0];
 end else if Time>=1.0 then begin
  ClosestPoint:=Segment.Points[1];
 end else begin
  ClosestPoint:=Vector3Add(Vector3ScalarMul(Segment.Points[0],1.0-Time),Vector3ScalarMul(Segment.Points[1],Time));
 end;
end;

procedure SIMDSegmentTransform(out OutputSegment:TKraftSegment;const Segment:TKraftSegment;const Transform:TKraftMatrix4x4); overload;
begin
 OutputSegment.Points[0]:=Vector3TermMatrixMul(Segment.Points[0],Transform);
 OutputSegment.Points[1]:=Vector3TermMatrixMul(Segment.Points[1],Transform);
end;

function SIMDSegmentTransform(const Segment:TKraftSegment;const Transform:TKraftMatrix4x4):TKraftSegment; overload;
begin
 result.Points[0]:=Vector3TermMatrixMul(Segment.Points[0],Transform);
 result.Points[1]:=Vector3TermMatrixMul(Segment.Points[1],Transform);
end;

procedure SIMDSegmentClosestPoints(const SegmentA,SegmentB:TKraftSegment;out TimeA:TKraftScalar;out ClosestPointA:TKraftVector3;out TimeB:TKraftScalar;out ClosestPointB:TKraftVector3);
var dA,dB,r:TKraftVector3;
    a,b,c,{d,}e,f,Denominator,aA,aB,bA,bB:TKraftScalar;
begin
 dA:=Vector3Sub(SegmentA.Points[1],SegmentA.Points[0]);
 dB:=Vector3Sub(SegmentB.Points[1],SegmentB.Points[0]);
 r:=Vector3Sub(SegmentA.Points[0],SegmentB.Points[0]);
 a:=Vector3LengthSquared(dA);
 e:=Vector3LengthSquared(dB);
 f:=Vector3Dot(dB,r);
 if (a<EPSILON) and (e<EPSILON) then begin
  // segment a and b are both points
  TimeA:=0.0;
  TimeB:=0.0;
  ClosestPointA:=SegmentA.Points[0];
  ClosestPointB:=SegmentB.Points[0];
 end else begin
  if a<EPSILON then begin
   // segment a is a point
	 TimeA:=0.0;
   TimeB:=f/e;
   if TimeB<0.0 then begin
    TimeB:=0.0;
   end else if TimeB>1.0 then begin
    TimeB:=1.0;
   end;
  end else begin
   c:=Vector3Dot(dA,r);
   if e<EPSILON then begin
		// segment b is a point
    TimeA:=-(c/a);
    if TimeA<0.0 then begin
     TimeA:=0.0;
    end else if TimeA>1.0 then begin
     TimeA:=1.0;
    end;
    TimeB:=0.0;
	 end else begin
    b:=Vector3Dot(dA,dB);
    Denominator:=(a*e)-sqr(b);
		if Denominator<EPSILON then begin
     // segments are parallel
     aA:=Vector3Dot(dB,SegmentA.Points[0]);
     aB:=Vector3Dot(dB,SegmentA.Points[1]);
     bA:=Vector3Dot(dB,SegmentB.Points[0]);
     bB:=Vector3Dot(dB,SegmentB.Points[1]);
     if (aA<=bA) and (aB<=bA) then begin
			// segment A is completely "before" segment B
      if aB>aA then begin
       TimeA:=1.0;
      end else begin
       TimeA:=0.0;
      end;
      TimeB:=0.0;
     end else if (aA>=bB) and (aB>=bB) then begin
      // segment B is completely "before" segment A
      if aB>aA then begin
       TimeA:=0.0;
      end else begin
       TimeA:=1.0;
      end;
      TimeB:=1.0;
     end else begin
      // segments A and B overlap, use midpoint of shared length
			if aA>aB then begin
       f:=aA;
       aA:=aB;
       aB:=f;
      end;
      f:=(Min(aB,bB)+Max(aA,bA))*0.5;
      TimeB:=(f-bA)/e;
      ClosestPointB:=Vector3Add(SegmentB.Points[0],Vector3ScalarMul(dB,TimeB));
      SIMDSegmentClosestPointTo(SegmentA,ClosestPointB,TimeB,ClosestPointA);
      exit;
     end;
    end	else begin
     // general case
     TimeA:=((b*f)-(c*e))/Denominator;
     if TimeA<0.0 then begin
      TimeA:=0.0;
     end else if TimeA>1.0 then begin
      TimeA:=1.0;
     end;
     TimeB:=((b*TimeA)+f)/e;
     if TimeB<0.0 then begin
      TimeB:=0.0;
      TimeA:=-(c/a);
      if TimeA<0.0 then begin
       TimeA:=0.0;
      end else if TimeA>1.0 then begin
       TimeA:=1.0;
      end;
     end else if TimeB>1.0 then begin
      TimeB:=1.0;
      TimeA:=(b-c)/a;
      if TimeA<0.0 then begin
       TimeA:=0.0;
      end else if TimeA>1.0 then begin
       TimeA:=1.0;
      end;
     end;
    end;
   end;
  end;
  ClosestPointA:=Vector3Add(SegmentA.Points[0],Vector3ScalarMul(dA,TimeA));
  ClosestPointB:=Vector3Add(SegmentB.Points[0],Vector3ScalarMul(dB,TimeB));
 end;
end;

function SIMDSegmentIntersect(const SegmentA,SegmentB:TKraftSegment;out TimeA,TimeB:TKraftScalar;out IntersectionPoint:TKraftVector3):boolean;
var PointA:TKraftVector3;
begin
 SIMDSegmentClosestPoints(SegmentA,SegmentB,TimeA,PointA,TimeB,IntersectionPoint);
 result:=Vector3DistSquared(PointA,IntersectionPoint)<EPSILON;
end;

function SIMDTriangleContains(const Triangle:TKraftTriangle;const p:TKraftVector3):boolean;
var vA,vB,vC:TKraftVector3;
    dAB,dAC,dBC:TKraftScalar;
begin
 vA:=Vector3Sub(Triangle.Points[0],p);
 vB:=Vector3Sub(Triangle.Points[1],p);
 vC:=Vector3Sub(Triangle.Points[2],p);
 dAB:=Vector3Dot(vA,vB);
 dAC:=Vector3Dot(vA,vC);
 dBC:=Vector3Dot(vB,vC);
 if ((dBC*dAC)-(Vector3LengthSquared(vC)*dAB))<0.0 then begin
  result:=false;
 end else begin
  result:=((dAB*dBC)-(dAC*Vector3LengthSquared(vB)))>=0.0;
 end;
end;

function SIMDTriangleIntersect(const Triangle:TKraftTriangle;const Segment:TKraftSegment;out Time:TKraftScalar;out IntersectionPoint:TKraftVector3):boolean;
var Switched:boolean;
    d,t,v,w:TKraftScalar;
    vAB,vAC,pBA,vApA,e,n:TKraftVector3;
    s:TKraftSegment;
begin

 result:=false;

 Time:=NaN;

 IntersectionPoint:=Vector3Origin;

 Switched:=false;

 vAB:=Vector3Sub(Triangle.Points[1],Triangle.Points[0]);
 vAC:=Vector3Sub(Triangle.Points[2],Triangle.Points[0]);

 pBA:=Vector3Sub(Segment.Points[0],Segment.Points[1]);

 n:=Vector3Cross(vAB,vAC);

 d:=Vector3Dot(n,pBA);

 if abs(d)<EPSILON then begin
  exit; // segment is parallel
 end else if d<0.0 then begin
  s.Points[0]:=Segment.Points[1];
  s.Points[1]:=Segment.Points[0];
  Switched:=true;
  pBA:=Vector3Sub(s.Points[0],s.Points[1]);
  d:=-d;
 end else begin
  s:=Segment;
 end;

 vApA:=Vector3Sub(s.Points[0],Triangle.Points[0]);
 t:=Vector3Dot(n,vApA);
 e:=Vector3Cross(pBA,vApA);

 v:=Vector3Dot(vAC,e);
 if (v<0.0) or (v>d) then begin
  exit; // intersects outside triangle
 end;

 w:=-Vector3Dot(vAB,e);
 if (w<0.0) or ((v+w)>d) then begin
  exit; // intersects outside triangle
 end;

 d:=1.0/d;
 t:=t*d;
 v:=v*d;
 w:=w*d;
 Time:=t;

 IntersectionPoint:=Vector3Add(Triangle.Points[0],Vector3Add(Vector3ScalarMul(vAB,v),Vector3ScalarMul(vAC,w)));

 if Switched then begin
	Time:=1.0-Time;
 end;

 result:=(Time>=0.0) and (Time<=1.0);
end;

function SIMDTriangleClosestPointTo(const Triangle:TKraftTriangle;const Point:TKraftVector3;out ClosestPoint:TKraftVector3):boolean; overload;
var u,v,w,d1,d2,d3,d4,d5,d6,Denominator:TKraftScalar;
    vAB,vAC,vAp,vBp,vCp:TKraftVector3;
begin
 result:=false;

 vAB:=Vector3Sub(Triangle.Points[1],Triangle.Points[0]);
 vAC:=Vector3Sub(Triangle.Points[2],Triangle.Points[0]);
 vAp:=Vector3Sub(Point,Triangle.Points[0]);

 d1:=Vector3Dot(vAB,vAp);
 d2:=Vector3Dot(vAC,vAp);
 if (d1<=0.0) and (d2<=0.0) then begin
	ClosestPoint:=Triangle.Points[0]; // closest point is vertex A
	exit;
 end;

 vBp:=Vector3Sub(Point,Triangle.Points[1]);
 d3:=Vector3Dot(vAB,vBp);
 d4:=Vector3Dot(vAC,vBp);
 if (d3>=0.0) and (d4<=d3) then begin
	ClosestPoint:=Triangle.Points[1]; // closest point is vertex B
	exit;
 end;
                                  
 w:=(d1*d4)-(d3*d2);
 if (w<=0.0) and (d1>=0.0) and (d3<=0.0) then begin
 	// closest point is along edge 1-2
	ClosestPoint:=Vector3Add(Triangle.Points[0],Vector3ScalarMul(vAB,d1/(d1-d3)));
  exit;
 end;

 vCp:=Vector3Sub(Point,Triangle.Points[2]);
 d5:=Vector3Dot(vAB,vCp);
 d6:=Vector3Dot(vAC,vCp);
 if (d6>=0.0) and (d5<=d6) then begin
	ClosestPoint:=Triangle.Points[2]; // closest point is vertex C
	exit;
 end;

 v:=(d5*d2)-(d1*d6);
 if (v<=0.0) and (d2>=0.0) and (d6<=0.0) then begin
 	// closest point is along edge 1-3
	ClosestPoint:=Vector3Add(Triangle.Points[0],Vector3ScalarMul(vAC,d2/(d2-d6)));
  exit;
 end;

 u:=(d3*d6)-(d5*d4);
 if (u<=0.0) and ((d4-d3)>=0.0) and ((d5-d6)>=0.0) then begin
	// closest point is along edge 2-3
	ClosestPoint:=Vector3Add(Triangle.Points[1],Vector3ScalarMul(Vector3Sub(Triangle.Points[2],Triangle.Points[1]),(d4-d3)/((d4-d3)+(d5-d6))));
  exit;
 end;

 Denominator:=1.0/(u+v+w);

 ClosestPoint:=Vector3Add(Triangle.Points[0],Vector3Add(Vector3ScalarMul(vAB,v*Denominator),Vector3ScalarMul(vAC,w*Denominator)));

 result:=true;
end;

function SIMDTriangleClosestPointTo(const Triangle:TKraftTriangle;const Segment:TKraftSegment;out Time:TKraftScalar;out ClosestPointOnSegment,ClosestPointOnTriangle:TKraftVector3):boolean; overload;
var MinDist,dtri,d1,d2,sa,sb,dist:TKraftScalar;
    pAInside,pBInside:boolean;
    pa,pb:TKraftVector3;
    Edge:TKraftSegment;
begin

 result:=SIMDTriangleIntersect(Triangle,Segment,Time,ClosestPointOnTriangle);

 if result then begin

 	// segment intersects triangle
  ClosestPointOnSegment:=ClosestPointOnTriangle;

 end else begin

  MinDist:=MAX_SCALAR;

  ClosestPointOnSegment:=Vector3Origin;

  dtri:=Vector3Dot(Triangle.Normal,Triangle.Points[0]);

  pAInside:=SIMDTriangleContains(Triangle,Segment.Points[0]);
  pBInside:=SIMDTriangleContains(Triangle,Segment.Points[1]);

  if pAInside and pBInside then begin
   // both points inside triangle
   d1:=Vector3Dot(Triangle.Normal,Segment.Points[0])-dtri;
   d2:=Vector3Dot(Triangle.Normal,Segment.Points[1])-dtri;
   if abs(d2-d1)<EPSILON then begin
    // segment is parallel to triangle
    ClosestPointOnSegment:=Vector3Avg(Segment.Points[0],Segment.Points[1]);
    MinDist:=d1;
    Time:=0.5;
   end	else if abs(d1)<abs(d2) then begin
    ClosestPointOnSegment:=Segment.Points[0];
    MinDist:=d1;
    Time:=0.0;
   end else begin
    ClosestPointOnSegment:=Segment.Points[1];
    MinDist:=d2;
    Time:=1.0;
   end;
   ClosestPointOnTriangle:=Vector3Add(ClosestPointOnSegment,Vector3ScalarMul(Triangle.Normal,-MinDist));
   result:=true;
   exit;
  end else if pAInside then begin
   // one point is inside triangle
   ClosestPointOnSegment:=Segment.Points[0];
   Time:=0.0;
   MinDist:=Vector3Dot(Triangle.Normal,ClosestPointOnSegment)-dtri;
   ClosestPointOnTriangle:=Vector3Add(ClosestPointOnSegment,Vector3ScalarMul(Triangle.Normal,-MinDist));
   MinDist:=sqr(MinDist);
  end else if pBInside then begin
   // one point is inside triangle
   ClosestPointOnSegment:=Segment.Points[1];
   Time:=1.0;
   MinDist:=Vector3Dot(Triangle.Normal,ClosestPointOnSegment)-dtri;
   ClosestPointOnTriangle:=Vector3Add(ClosestPointOnSegment,Vector3ScalarMul(Triangle.Normal,-MinDist));
   MinDist:=sqr(MinDist);
  end;

  // test edge 1
  Edge.Points[0]:=Triangle.Points[0];
  Edge.Points[1]:=Triangle.Points[1];
  SIMDSegmentClosestPoints(Segment,Edge,sa,pa,sb,pb);
  Dist:=Vector3DistSquared(pa,pb);
  if Dist<MinDist then begin
   MinDist:=Dist;
   Time:=sa;
   ClosestPointOnSegment:=pa;
   ClosestPointOnTriangle:=pb;
  end;

  // test edge 2
  Edge.Points[0]:=Triangle.Points[1];
  Edge.Points[1]:=Triangle.Points[2];
  SIMDSegmentClosestPoints(Segment,Edge,sa,pa,sb,pb);
  Dist:=Vector3DistSquared(pa,pb);
  if Dist<MinDist then begin
   MinDist:=Dist;
   Time:=sa;
   ClosestPointOnSegment:=pa;
   ClosestPointOnTriangle:=pb;
  end;

  // test edge 3
  Edge.Points[0]:=Triangle.Points[2];
  Edge.Points[1]:=Triangle.Points[0];
  SIMDSegmentClosestPoints(Segment,Edge,sa,pa,sb,pb);
  Dist:=Vector3DistSquared(pa,pb);
  if Dist<MinDist then begin
// MinDist:=Dist;
   Time:=sa;
   ClosestPointOnSegment:=pa;
   ClosestPointOnTriangle:=pb;
  end;

 end;

end;

function InertiaTensorTransform(const Inertia,Transform:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
begin
 result:=Matrix3x3TermMulTranspose(Matrix3x3TermMul(Transform,Inertia),Transform);
end;

function InertiaTensorParallelAxisTheorem(const Center:TKraftVector3;const Mass:TKraftScalar):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
var CenterDotCenter:TKraftScalar;
begin
 CenterDotCenter:=sqr(Center.x)+sqr(Center.y)+sqr(Center.z);
 result[0,0]:=((Matrix3x3Identity[0,0]*CenterDotCenter)-(Center.x*Center.x))*Mass;
 result[0,1]:=((Matrix3x3Identity[0,1]*CenterDotCenter)-(Center.y*Center.x))*Mass;
 result[0,2]:=((Matrix3x3Identity[0,2]*CenterDotCenter)-(Center.z*Center.x))*Mass;
 result[1,0]:=((Matrix3x3Identity[1,0]*CenterDotCenter)-(Center.x*Center.y))*Mass;
 result[1,1]:=((Matrix3x3Identity[1,1]*CenterDotCenter)-(Center.y*Center.y))*Mass;
 result[1,2]:=((Matrix3x3Identity[1,2]*CenterDotCenter)-(Center.z*Center.y))*Mass;
 result[2,0]:=((Matrix3x3Identity[2,0]*CenterDotCenter)-(Center.x*Center.z))*Mass;
 result[2,1]:=((Matrix3x3Identity[2,1]*CenterDotCenter)-(Center.y*Center.z))*Mass;
 result[2,2]:=((Matrix3x3Identity[2,2]*CenterDotCenter)-(Center.z*Center.z))*Mass;
end;

constructor TKraftHighResolutionTimer.Create(FrameRate:longint=60);
begin
 inherited Create;
 fFrequencyShift:=0;
{$ifdef windows}
 if QueryPerformanceFrequency(fFrequency) then begin
  while (fFrequency and $ffffffffe0000000)<>0 do begin
   fFrequency:=fFrequency shr 1;
   inc(fFrequencyShift);
  end;
 end else begin
  fFrequency:=1000;
 end;
{$else}
{$ifdef linux}
  fFrequency:=1000000000;
{$else}
{$ifdef unix}
  fFrequency:=1000000;
{$else}
  fFrequency:=1000;
{$endif}
{$endif}
{$endif}
 fFrameInterval:=(fFrequency+((abs(FrameRate)+1) shr 1)) div abs(FrameRate);
 fMillisecondInterval:=(fFrequency+500) div 1000;
 fTwoMillisecondsInterval:=(fFrequency+250) div 500;
 fFourMillisecondsInterval:=(fFrequency+125) div 250;
 fQuarterSecondInterval:=(fFrequency+2) div 4;
 fHourInterval:=fFrequency*3600;
end;

destructor TKraftHighResolutionTimer.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftHighResolutionTimer.SetFrameRate(FrameRate:longint);
begin
 fFrameInterval:=(fFrequency+((abs(FrameRate)+1) shr 1)) div abs(FrameRate);
end;

function TKraftHighResolutionTimer.GetTime:int64;
{$ifdef linux}
var NowTimeSpec:TimeSpec;
    ia,ib:int64;
{$else}
{$ifdef unix}
var tv:timeval;
    tz:timezone;
    ia,ib:int64;
{$endif}
{$endif}
begin
{$ifdef windows}
 if not QueryPerformanceCounter(result) then begin
  result:=timeGetTime;
 end;
{$else}
{$ifdef linux}
 clock_gettime(CLOCK_MONOTONIC,@NowTimeSpec);
 ia:=int64(NowTimeSpec.tv_sec)*int64(1000000000);
 ib:=NowTimeSpec.tv_nsec;
 result:=ia+ib;
{$else}
{$ifdef unix}
  tz.tz_minuteswest:=0;
  tz.tz_dsttime:=0;
  fpgettimeofday(@tv,@tz);
  ia:=int64(tv.tv_sec)*int64(1000000);
  ib:=tv.tv_usec;
  result:=ia+ib;
{$else}
 result:=SDL_GetTicks;
{$endif}
{$endif}
{$endif}
 result:=result shr fFrequencyShift;
end;

function TKraftHighResolutionTimer.GetEventTime:int64;
begin
 result:=ToNanoseconds(GetTime);
end;

procedure TKraftHighResolutionTimer.Sleep(Delay:int64);
var EndTime,NowTime{$ifdef unix},SleepTime{$endif}:int64;
{$ifdef unix}
    req,rem:timespec;
{$endif}
begin
 if Delay>0 then begin
{$ifdef windows}
  NowTime:=GetTime;
  EndTime:=NowTime+Delay;
  while (NowTime+fTwoMillisecondsInterval)<EndTime do begin
   Sleep(1);
   NowTime:=GetTime;
  end;
  while (NowTime+fMillisecondInterval)<EndTime do begin
   Sleep(0);
   NowTime:=GetTime;
  end;
  while NowTime<EndTime do begin
   NowTime:=GetTime;
  end;
{$else}
{$ifdef linux}
  NowTime:=GetTime;
  EndTime:=NowTime+Delay;
  while true do begin
   SleepTime:=abs(EndTime-NowTime);
   if SleepTime>=fFourMillisecondsInterval then begin
    SleepTime:=(SleepTime+2) shr 2;
    if SleepTime>0 then begin
     req.tv_sec:=SleepTime div 1000000000;
     req.tv_nsec:=SleepTime mod 10000000000;
     fpNanoSleep(@req,@rem);
     NowTime:=GetTime;
     continue;
    end;
   end;
   break;
  end;
  while (NowTime+fTwoMillisecondsInterval)<EndTime do begin
   ThreadSwitch;
   NowTime:=GetTime;
  end;
  while NowTime<EndTime do begin
   NowTime:=GetTime;
  end;
{$else}
{$ifdef unix}
  NowTime:=GetTime;
  EndTime:=NowTime+Delay;
  while true do begin
   SleepTime:=abs(EndTime-NowTime);
   if SleepTime>=fFourMillisecondsInterval then begin
    SleepTime:=(SleepTime+2) shr 2;
    if SleepTime>0 then begin
     req.tv_sec:=SleepTime div 1000000;
     req.tv_nsec:=(SleepTime mod 1000000)*1000;
     fpNanoSleep(@req,@rem);
     NowTime:=GetTime;
     continue;
    end;
   end;
   break;
  end;
  while (NowTime+fTwoMillisecondsInterval)<EndTime do begin
   ThreadSwitch;
   NowTime:=GetTime;
  end;
  while NowTime<EndTime do begin
   NowTime:=GetTime;
  end;
{$else}
  NowTime:=GetTime;
  EndTime:=NowTime+Delay;
  while (NowTime+4)<EndTime then begin
   SDL_Delay(1);
   NowTime:=GetTime;
  end;
  while (NowTime+2)<EndTime do begin
   SDL_Delay(0);
   NowTime:=GetTime;
  end;
  while NowTime<EndTime do begin
   NowTime:=GetTime;
  end;
{$endif}
{$endif}
{$endif}
 end;
end;

function TKraftHighResolutionTimer.ToFixedPointSeconds(Time:int64):int64;
var a,b:TUInt128;
begin
 if fFrequency<>0 then begin
  if ((fFrequency or Time) and int64($ffffffff00000000))=0 then begin
   result:=int64(qword(qword(Time)*qword($100000000)) div qword(fFrequency));
  end else begin
   UInt128Mul64(a,Time,qword($100000000));
   UInt128Div64(b,a,fFrequency);
   result:=b.Lo;
  end;
 end else begin
  result:=0;
 end;
end;

function TKraftHighResolutionTimer.ToFixedPointFrames(Time:int64):int64;
var a,b:TUInt128;
begin
 if fFrameInterval<>0 then begin
  if ((fFrameInterval or Time) and int64($ffffffff00000000))=0 then begin
   result:=int64(qword(qword(Time)*qword($100000000)) div qword(fFrameInterval));
  end else begin
   UInt128Mul64(a,Time,qword($100000000));
   UInt128Div64(b,a,fFrameInterval);
   result:=b.Lo;
  end;
 end else begin
  result:=0;
 end;
end;

function TKraftHighResolutionTimer.ToFloatSeconds(Time:int64):double;
begin
 if fFrequency<>0 then begin
  result:=Time/fFrequency;
 end else begin
  result:=0;
 end;
end;

function TKraftHighResolutionTimer.FromFloatSeconds(Time:double):int64;
begin
 if fFrequency<>0 then begin
  result:=trunc(Time*fFrequency);
 end else begin
  result:=0;
 end;
end;

function TKraftHighResolutionTimer.ToMilliseconds(Time:int64):int64;
begin
 result:=Time;
 if fFrequency<>1000 then begin
  result:=((Time*1000)+((fFrequency+1) shr 1)) div fFrequency;
 end;
end;

function TKraftHighResolutionTimer.FromMilliseconds(Time:int64):int64;
begin
 result:=Time;
 if fFrequency<>1000 then begin
  result:=((Time*fFrequency)+500) div 1000;
 end;
end;

function TKraftHighResolutionTimer.ToMicroseconds(Time:int64):int64;
begin
 result:=Time;
 if fFrequency<>1000000 then begin
  result:=((Time*1000000)+((fFrequency+1) shr 1)) div fFrequency;
 end;
end;

function TKraftHighResolutionTimer.FromMicroseconds(Time:int64):int64;
begin
 result:=Time;
 if fFrequency<>1000000 then begin
  result:=((Time*fFrequency)+500000) div 1000000;
 end;
end;

function TKraftHighResolutionTimer.ToNanoseconds(Time:int64):int64;
begin
 result:=Time;
 if fFrequency<>1000000000 then begin
  result:=((Time*1000000000)+((fFrequency+1) shr 1)) div fFrequency;
 end;
end;

function TKraftHighResolutionTimer.FromNanoseconds(Time:int64):int64;
begin
 result:=Time;
 if fFrequency<>1000000000 then begin
  result:=((Time*fFrequency)+500000000) div 1000000000;
 end;
end;

function CalculateArea(const v0,v1,v2:TKraftVector3):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Vector3LengthSquared(Vector3Cross(Vector3Sub(v1,v0),Vector3Sub(v2,v0)));
end;

function CalculateVolume(const v0,v1,v2,v3:TKraftVector3):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
var a,b,c:TKraftVector3;
begin
 a:=Vector3Sub(v0,v3);
 b:=Vector3Sub(v1,v3);
 c:=Vector3Sub(v2,v3);
 result:=(a.x*((b.z*c.y)-(b.y*c.z)))+(a.y*((b.x*c.z)-(b.z*c.x)))+(a.z*((b.y*c.x)-(b.x*c.y)));
end;

type TKraftShapeTriangle=class(TKraftShapeConvexHull)
      private
       fShapeConvexHull:TKraftConvexHull;
       procedure UpdateData;
      public
       constructor Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AVertex0,AVertex1,AVertex2:TKraftVector3); reintroduce;
       destructor Destroy; override;
       procedure UpdateShapeAABB; override;
       procedure CalculateMassData; override;
       function GetSignedDistance(const Position:TKraftVector3):TKraftScalar; override;
       function GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3; override;
       function GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3; override;
       function GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint; override;
       function GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3; override;
       function TestPoint(const p:TKraftVector3):boolean; override;
       function RayCast(var RayCastData:TKraftRaycastData):boolean; override;
{$ifdef DebugDraw}
       procedure Draw(const CameraMatrix:TKraftMatrix4x4); override;
{$endif}
     end;

function IsSameValue(const a,b:TKraftScalar):boolean;
const FuzzFactor=1000.0;
      Resolution={$ifdef UseDouble}1e-15{$else}1e-7{$endif}*FuzzFactor;
var EpsilonTolerance:double;
begin
 EpsilonTolerance:=abs(a);
 if EpsilonTolerance>abs(b) then begin
  EpsilonTolerance:=abs(b);
 end;
 EpsilonTolerance:=EpsilonTolerance*Resolution;
 if EpsilonTolerance<Resolution then begin
  EpsilonTolerance:=Resolution;
 end;
 if a>b then begin
  result:=(a-b)<=EpsilonTolerance;
 end else begin
  result:=(b-a)<=EpsilonTolerance;
 end;
end;

{$ifdef cpu386}
{$ifndef ver130}
function InterlockedCompareExchange64Ex(Target,NewValue,Comperand:pointer):boolean; assembler; register;
asm
 push ebx
 push edi
 push esi
 mov edi,eax
 mov esi,edx
 mov edx,dword ptr [ecx+4]
 mov eax,dword ptr [ecx+0]
 mov ecx,dword ptr [esi+4]
 mov ebx,dword ptr [esi+0]
 lock cmpxchg8b [edi]
 setz al
 pop esi
 pop edi
 pop ebx
end;

function InterlockedCompareExchange64(var Target:int64;NewValue:int64;Comperand:int64):int64; assembler; register;
asm
 push ebx
 push edi
 mov edi,eax
 mov edx,dword ptr [Comperand+4]
 mov eax,dword ptr [Comperand+0]
 mov ecx,dword ptr [NewValue+4]
 mov ebx,dword ptr [NewValue+0]
 lock cmpxchg8b [edi]
 pop edi
 pop ebx
end;
{$endif}
{$endif}

{$ifndef fpc}
{$ifdef cpu386}
function InterlockedDecrement(var Target:longint):longint; assembler; register;
asm
 mov edx,$ffffffff
 xchg eax,edx
 lock xadd dword ptr [edx],eax
 dec eax
end;

function InterlockedIncrement(var Target:longint):longint; assembler; register;
asm
 mov edx,1
 xchg eax,edx
 lock xadd dword ptr [edx],eax
 inc eax
end;

function InterlockedExchange(var Target:longint;Source:longint):longint; assembler; register;
asm
 lock xchg dword ptr [eax],edx
 mov eax,edx
end;

function InterlockedExchangeAdd(var Target:longint;Source:longint):longint; assembler; register;
asm
 xchg edx,eax
 lock xadd dword ptr [edx],eax
end;

function InterlockedCompareExchange(var Target:longint;NewValue,Comperand:longint):longint; assembler; register;
asm
 xchg ecx,eax
 lock cmpxchg dword ptr [ecx],edx
end;
{$else}
function InterlockedDecrement(var Target:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=Windows.InterlockedDecrement(Target);
end;

function InterlockedIncrement(var Target:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=Windows.InterlockedIncrement(Target);
end;

function InterlockedExchange(var Target:longint;Source:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=Windows.InterlockedExchange(Target,Source);
end;

function InterlockedExchangeAdd(var Target:longint;Source:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=Windows.InterlockedExchangeAdd(Target,Source);
end;

function InterlockedCompareExchange(var Target:longint;NewValue,Comperand:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=Windows.InterlockedCompareExchange(Target,NewValue,Comperand);
end;
{$endif}
{$else}
function InterlockedDecrement(var Target:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=System.InterlockedDecrement(Target);
end;

function InterlockedIncrement(var Target:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=System.InterlockedIncrement(Target);
end;

function InterlockedExchange(var Target:longint;Source:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=System.InterlockedExchange(Target,Source);
end;

function InterlockedExchangeAdd(var Target:longint;Source:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=System.InterlockedExchangeAdd(Target,Source);
end;

function InterlockedCompareExchange(var Target:longint;NewValue,Comperand:longint):longint; {$ifdef caninline}inline;{$endif}
begin
 result:=System.InterlockedCompareExchange(Target,NewValue,Comperand);
end;
{$endif}

function HashPointer(p:pointer):longword; {$ifdef caninline}inline;{$endif}
(*{$ifdef cpu64}
var r:qword;
begin
 r:=ptruint(p);
 r:=r xor (r shr 33);
 r:=r*qword($ff51afd7ed558ccd);
 r:=r xor (r shr 33);
 r:=r*qword($c4ceb9fe1a85ec53);
 result:=longword(ptruint(r xor (r shr 33)));
end;
{$else}
begin
 result:=ptruint(p);
 result:=result xor (result shr 16);
 result:=result*$85ebca6b;
 result:=result xor (result shr 13);
 result:=result*$c2b2ae35;
 result:=result xor (result shr 16);
end;
{$endif}(**)
{$ifdef cpu64}
var r:ptruint;
begin
 r:=ptruint(p);
 r:=(not r)+(r shl 18); // r:=((r shl 18)-r-)1;
 r:=r xor (r shr 31);
 r:=r*21; // r:=(r+(r shl 2))+(r shl 4);
 r:=r xor (r shr 11);
 r:=r+(r shl 6);
 result:=longword(ptruint(r xor (r shr 22)));
end;
{$else}
begin
 result:=ptruint(p);
 result:=(not result)+(result shl 15);
 result:=result xor (result shr 15);
 inc(result,result shl 2);
 result:=(result xor (result shr 4))*2057;
 result:=result xor (result shr 16);
end;
{$endif}

function HashTwoLongWords(a,b:longword):longword; {$ifdef caninline}inline;{$endif}
var r:qword;
begin
 r:=(qword(a) shl 32) or b;
 r:=(not r)+(r shl 18); // r:=((r shl 18)-r-)1;
 r:=r xor (r shr 31);
{$ifdef cpu64}
 r:=r*21;
{$else}
 r:=(r+(r shl 2))+(r shl 4);
{$endif}
 r:=r xor (r shr 11);
 r:=r+(r shl 6);
 result:=longword(ptruint(r xor (r shr 22)));
end;

function HashTwoPointers(a,b:pointer):longword; {$ifdef caninline}inline;{$endif}
begin
 result:=HashTwoLongWords(HashPointer(a),HashPointer(b));
end;

function HashTwoPointersAndOneLongWord(a,b:pointer;c:longword):longword; {$ifdef caninline}inline;{$endif}
begin
 result:=HashTwoLongWords(HashTwoLongWords(HashPointer(a),HashPointer(b)),c);
end;

function AABBStretch(const AABB:TKraftAABB;const Displacement,BoundsExpansion:TKraftVector3):TKraftAABB; {$ifdef caninline}inline;{$endif}
var d:TKraftVector3;
begin
 d:=Vector3Add(AABBExtensionVector,BoundsExpansion);
 result.Min:=Vector3Sub(AABB.Min,d);
 result.Max:=Vector3Add(AABB.Max,d);
 d:=Vector3ScalarMul(Displacement,AABB_MULTIPLIER);
 if d.x<0.0 then begin
  result.Min.x:=result.Min.x+d.x;
 end else if d.x>0.0 then begin
  result.Max.x:=result.Max.x+d.x;
 end;
 if d.y<0.0 then begin
  result.Min.y:=result.Min.y+d.y;
 end else if d.y>0.0 then begin
  result.Max.y:=result.Max.y+d.y;
 end;
 if d.z<0.0 then begin
  result.Min.z:=result.Min.z+d.z;
 end else if d.z>0.0 then begin
  result.Max.z:=result.Max.z+d.z;
 end;
end;

function CompareFloat(const a,b:pointer):longint;
begin
 if TKraftScalar(a^)<TKraftScalar(b^) then begin
  result:=1;
 end else if TKraftScalar(a^)>TKraftScalar(b^) then begin
  result:=-1;
 end else begin
  result:=0;
 end;
end;

function SweepTransform(const Sweep:TKraftSweep;const Beta:TKraftScalar):TKraftMatrix4x4; {$ifdef caninline}inline;{$endif}
begin
 result:=QuaternionToMatrix4x4(QuaternionSlerp(Sweep.q0,Sweep.q,Beta));
 PKraftVector3(pointer(@result[3,0]))^:=Vector3Sub(Vector3Lerp(Sweep.c0,Sweep.c,Beta),Vector3TermMatrixMulBasis(Sweep.LocalCenter,result));
end;

function SweepTermAdvance(const Sweep:TKraftSweep;const Alpha:TKraftScalar):TKraftSweep; {$ifdef caninline}inline;{$endif}
var Beta:TKraftScalar;
begin
 Assert(Sweep.Alpha0<1.0);
 Beta:=(Alpha-Sweep.Alpha0)/(1.0-Sweep.Alpha0);
 result.LocalCenter:=Sweep.LocalCenter;
 result.c0:=Vector3Lerp(Sweep.c0,Sweep.c,Beta);
 result.c:=Sweep.c;
 result.q0:=QuaternionSlerp(Sweep.q0,Sweep.q,Beta);
 result.q:=Sweep.q;
 result.Alpha0:=Alpha;
end;

procedure SweepAdvance(var Sweep:TKraftSweep;const Alpha:TKraftScalar);
var Beta:TKraftScalar;
begin
 Beta:=(Alpha-Sweep.Alpha0)/(1.0-Sweep.Alpha0);
 Sweep.c0:=Vector3Lerp(Sweep.c0,Sweep.c,Beta);
 Sweep.q0:=QuaternionSlerp(Sweep.q0,Sweep.q,Beta);
 Sweep.Alpha0:=Alpha;
end;

function SweepTermNormalize(const Sweep:TKraftSweep):TKraftSweep; {$ifdef caninline}inline;{$endif}
begin
 result.LocalCenter:=Sweep.LocalCenter;
 result.c0:=Sweep.c0;
 result.c:=Sweep.c;
 result.q0:=QuaternionTermNormalize(Sweep.q0);
 result.q:=QuaternionTermNormalize(Sweep.q);
 result.Alpha0:=Sweep.Alpha0;
end;

procedure SweepNormalize(var Sweep:TKraftSweep); {$ifdef caninline}inline;{$endif}
begin
 QuaternionNormalize(Sweep.q0);
 QuaternionNormalize(Sweep.q);
end;

procedure ClipFace(const InVertices,OutVertices:TKraftClipVertexList;const Plane:TKraftPlane;const ReferenceEdgeIndexOffset,ReferenceFaceIndex:longint);
var Index,CountVertices:longint;
    PreviousClipVertexDistance,CurrentClipVertexDistance:TKraftScalar;
    PreviousClipVertex,CurrentClipVertex:PKraftClipVertex;
    FeatureID:TKraftContactFeatureID;
begin
 CountVertices:=InVertices.fCount;
 if CountVertices>=2 then begin
  PreviousClipVertex:=@InVertices.fVertices[InVertices.fCount-1];
  CurrentClipVertex:=@InVertices.fVertices[0];
  PreviousClipVertexDistance:=PlaneVectorDistance(Plane,PreviousClipVertex^.Position);
  for Index:=0 to CountVertices-1 do begin
   CurrentClipVertex:=@InVertices.fVertices[Index];
   CurrentClipVertexDistance:=PlaneVectorDistance(Plane,CurrentClipVertex^.Position);
   if PreviousClipVertexDistance<=0.0 then begin
    if CurrentClipVertexDistance<=0.0 then begin
     // Both vertices are behind the plane => keep CurrentClipVertex
     OutVertices.Add(CurrentClipVertex^);
    end else begin
     // PreviousClipVertex is behind the plane, CurrentClipVertex is in front => intersection point
     FeatureID.ElementA:=CurrentClipVertex^.FeatureID.ElementA;
     FeatureID.ElementB:=ReferenceEdgeIndexOffset+Index;
     OutVertices.Add(Vector3Lerp(PreviousClipVertex^.Position,CurrentClipVertex^.Position,PreviousClipVertexDistance/(PreviousClipVertexDistance-CurrentClipVertexDistance)),FeatureID);
    end;
   end else if CurrentClipVertexDistance<=0.0 then begin
    // CurrentClipVertex is behind the plane, PreviousClipVertex is in front => intersection point
    FeatureID.ElementA:=ReferenceEdgeIndexOffset+Index;
    FeatureID.ElementB:=CurrentClipVertex^.FeatureID.ElementA;
    OutVertices.Add(Vector3Lerp(PreviousClipVertex^.Position,CurrentClipVertex^.Position,PreviousClipVertexDistance/(PreviousClipVertexDistance-CurrentClipVertexDistance)),FeatureID);
    OutVertices.Add(CurrentClipVertex^);
   end;
   PreviousClipVertex:=CurrentClipVertex;
   PreviousClipVertexDistance:=CurrentClipVertexDistance;
  end;
 end;
end;

function GetSkewSymmetricMatrixPlus(const v:TKraftVector3):TKraftMatrix3x3; overload; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=0.0;
 result[0,1]:=-v.z;
 result[0,2]:=v.y;
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=v.z;
 result[1,1]:=0.0;
 result[1,2]:=-v.x;
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=-v.y;
 result[2,1]:=v.x;
 result[2,2]:=0.0;
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function GetSkewSymmetricMatrixMinus(const v:TKraftVector3):TKraftMatrix3x3; overload; {$ifdef caninline}inline;{$endif}
begin
 result[0,0]:=0.0;
 result[0,1]:=v.z;
 result[0,2]:=-v.y;
{$ifdef SIMD}
 result[0,3]:=0.0;
{$endif}
 result[1,0]:=-v.z;
 result[1,1]:=0.0;
 result[1,2]:=v.x;
{$ifdef SIMD}
 result[1,3]:=0.0;
{$endif}
 result[2,0]:=v.y;
 result[2,1]:=-v.x;
 result[2,2]:=0.0;
{$ifdef SIMD}
 result[2,3]:=0.0;
{$endif}
end;

function EvaluateEulerEquation(const w1,w0,T:TKraftVector3;const dt:TKraftScalar;const I:TKraftMatrix3x3):TKraftVector3; {$ifdef caninline}inline;{$endif}
var w1xI:TKraftVector3;
begin
 w1xI:=Vector3TermMatrixMul(w1,I);
 result:=Vector3Sub(Vector3Add(w1xI,Vector3ScalarMul(Vector3Cross(w1,w1xI),dt)),Vector3Add(Vector3ScalarMul(T,dt),Vector3TermMatrixMul(w0,I)));
end;

function EvaluateEulerEquationDerivation(const w1,w0:TKraftVector3;const dt:TKraftScalar;const I:TKraftMatrix3x3):TKraftMatrix3x3; {$ifdef caninline}inline;{$endif}
var w1x,Iw1x:TKraftMatrix3x3;
begin
 w1x:=GetSkewSymmetricMatrixMinus(w1);
 Iw1x:=GetSkewSymmetricMatrixMinus(Vector3TermMatrixMul(w1,I));
 result:=Matrix3x3TermAdd(I,Matrix3x3TermScalarMul(Matrix3x3TermSub(Matrix3x3TermMul(w1x,I),Iw1x),dt));
end;

constructor TKraftClipVertexList.Create;
begin
 inherited Create;
 fVertices:=nil;
 fCapacity:=0;
 fCount:=0;
 fColor.r:=1.0;
 fColor.b:=1.0;
 fColor.g:=1.0;
 fColor.a:=1.0;
end;

destructor TKraftClipVertexList.Destroy;
begin
 SetLength(fVertices,0);
end;

procedure TKraftClipVertexList.Clear;
begin
 fCount:=0;
end;

procedure TKraftClipVertexList.Add(const v:TKraftVector3;const fp:TKraftContactFeatureID);
var i:longint;
begin
 i:=fCount;
 inc(fCount);
 if fCount>fCapacity then begin
  fCapacity:=fCount*2;
  SetLength(fVertices,fCapacity);
 end;
 fVertices[i].Position:=v;
 fVertices[i].FeatureID:=fp;
end;

procedure TKraftClipVertexList.Add(const v:TKraftClipVertex);
var i:longint;
begin
 i:=fCount;
 inc(fCount);
 if fCount>fCapacity then begin
  fCapacity:=fCount*2;
  SetLength(fVertices,fCapacity);
 end;
 fVertices[i]:=v;
end;

procedure GetPlaneSpace(const n:TKraftVector3;out p,q:TKraftVector3); {$ifdef caninline}inline;{$endif}
var a,k:TKraftScalar;
begin
 if abs(n.z)>0.70710678 then begin
  a:=sqr(n.y)+sqr(n.z);
  k:=1.0/sqrt(a);
  p.x:=0.0;
  p.y:=-(n.z*k);
  p.z:=n.y*k;
  q.x:=a*k;
  q.y:=-(n.x*p.z);
  q.z:=n.x*p.y;
 end else begin
  a:=sqr(n.x)+sqr(n.y);
  k:=1.0/sqrt(a);
  p.x:=-(n.y*k);
  p.y:=n.x*k;
  p.z:=0.0;
  q.x:=-(n.z*p.y);
  q.y:=n.z*p.x;
  q.z:=a*k;
 end;
end;

procedure ComputeBasis(var a:TKraftVector3;out b,c:TKraftVector3); overload; {$ifdef caninline}inline;{$endif}
begin
 // Suppose vector a has all equal components and is a unit vector: a = (s, s, s)
 // Then 3*s*s = 1, s = sqrt(1/3) = 0.57735027. This means that at least one component of a
 // unit vector must be greater or equal to 0.57735027. Can use SIMD select operation.
 if abs(a.x)>=0.57735027 then begin
  b.x:=a.y;
  b.y:=-a.x;
  b.z:=0.0;
 end else begin
  b.x:=0.0;
  b.y:=a.z;
  b.z:=-a.y;
 end;
 Vector3NormalizeEx(a);
 Vector3NormalizeEx(b);
 c:=Vector3NormEx(Vector3Cross(a,b));
end;

procedure ComputeBasis(const Normal:TKraftVector3;out Matrix:TKraftMatrix3x3;const IndexA:longint=0;const IndexB:longint=1;const IndexC:longint=2); overload; {$ifdef caninline}inline;{$endif}
var a,b,c:TKraftVector3;
begin
 a:=Normal;
 ComputeBasis(a,b,c);
 Matrix[IndexA,0]:=a.x;
 Matrix[IndexA,1]:=a.y;
 Matrix[IndexA,2]:=a.z;
{$ifdef SIMD}
 Matrix[IndexA,3]:=0.0;
{$endif}
 Matrix[IndexB,0]:=b.x;
 Matrix[IndexB,1]:=b.y;
 Matrix[IndexB,2]:=b.z;
{$ifdef SIMD}
 Matrix[IndexB,3]:=0.0;
{$endif}
 Matrix[IndexC,0]:=c.x;
 Matrix[IndexC,1]:=c.y;
 Matrix[IndexC,2]:=c.z;
{$ifdef SIMD}
 Matrix[IndexC,3]:=0.0;
{$endif}
end;

function RayCastSphere(const RayOrigin,RayDirection,SpherePosition:TKraftVector3;const Radius,MaxTime:TKraftScalar;var HitTime:TKraftScalar):boolean; overload; {$ifdef caninline}inline;{$endif}
var Origin,Direction,m:TKraftVector3;
    b,c,d,t:TKraftScalar;
begin
 result:=false;
 Origin:=RayOrigin;
 Direction:=RayDirection;
 m:=Vector3Sub(Origin,SpherePosition);
 b:=Vector3Dot(m,Direction);
 c:=Vector3LengthSquared(m)-sqr(Radius);
 if (c<=0.0) or (b<=0.0) then begin
  d:=sqr(b)-c;
  if d>=EPSILON then begin
   t:=(-b)-sqrt(d);
   if (t>=0.0) and (t<=MaxTime) then begin
    HitTime:=t;
    result:=true;
   end;
  end;
 end;
end;

function RayCastSphere(const RayOrigin,RayDirection,SpherePosition:TKraftVector3;const Radius,MaxTime:TKraftScalar;var HitTime:TKraftScalar;var HitPosition,HitNormal:TKraftVector3):boolean; overload; {$ifdef caninline}inline;{$endif}
var Origin,Direction,m:TKraftVector3;
    b,c,d,t:TKraftScalar;
begin
 result:=false;
 Origin:=RayOrigin;
 Direction:=RayDirection;
 m:=Vector3Sub(Origin,SpherePosition);
 b:=Vector3Dot(m,Direction);
 c:=Vector3LengthSquared(m)-sqr(Radius);
 if (c<=0.0) or (b<=0.0) then begin
  d:=sqr(b)-c;
  if d>=EPSILON then begin
   t:=(-b)-sqrt(d);
   if (t>=0.0) and (t<=MaxTime) then begin
    HitTime:=t;
    HitPosition:=Vector3Add(Origin,Vector3ScalarMul(Direction,t));
    HitNormal:=Vector3NormEx(Vector3Sub(HitPosition,SpherePosition));
    result:=true;
   end;
  end;
 end;
end;

function CalculateAreaFromThreePoints(const p0,p1,p2:TKraftVector3):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Vector3LengthSquared(Vector3Cross(Vector3Sub(p1,p0),Vector3Sub(p2,p0)));
end;

function CalculateAreaFromFourPoints(const p0,p1,p2,p3:TKraftVector3):TKraftScalar; overload; {$ifdef caninline}inline;{$endif}
begin
 result:=Max(Max(Vector3LengthSquared(Vector3Cross(Vector3Sub(p0,p1),Vector3Sub(p3,p3))),
                 Vector3LengthSquared(Vector3Cross(Vector3Sub(p0,p2),Vector3Sub(p1,p3)))),
                 Vector3LengthSquared(Vector3Cross(Vector3Sub(p0,p3),Vector3Sub(p1,p2))));
end;

function BoxGetDistanceToPoint(Point:TKraftVector3;const Extents:TKraftVector3;const InverseTransformMatrix,TransformMatrix:TKraftMatrix4x4;var ClosestBoxPoint:TKraftVector3):TKraftScalar;
var Temp,Direction:TKraftVector3;
    Overlap:longint;
begin
 result:=0;
 ClosestBoxPoint:=Vector3TermMatrixMul(Point,InverseTransformMatrix);
 if ClosestBoxPoint.x<-Extents.x then begin
  result:=result+sqr(ClosestBoxPoint.x-(-Extents.x));
  ClosestBoxPoint.x:=-Extents.x;
  Overlap:=0;
 end else if ClosestBoxPoint.x>Extents.x then begin
  result:=result+sqr(ClosestBoxPoint.x-Extents.x);
  ClosestBoxPoint.x:=Extents.x;
  Overlap:=0;
 end else begin
  Overlap:=1;
 end;
 if ClosestBoxPoint.y<-Extents.y then begin
  result:=result+sqr(ClosestBoxPoint.y-(-Extents.y));
  ClosestBoxPoint.y:=-Extents.y;
 end else if ClosestBoxPoint.y>Extents.y then begin
  result:=result+sqr(ClosestBoxPoint.y-Extents.y);
  ClosestBoxPoint.y:=Extents.y;
 end else begin
  Overlap:=Overlap or 2;
 end;
 if ClosestBoxPoint.z<-Extents.z then begin
  result:=result+sqr(ClosestBoxPoint.z-(-Extents.z));
  ClosestBoxPoint.z:=-Extents.z;
 end else if ClosestBoxPoint.z>Extents.z then begin
  result:=result+sqr(ClosestBoxPoint.z-Extents.z);
  ClosestBoxPoint.z:=Extents.z;
 end else begin
  Overlap:=Overlap or 3;
 end;
 if Overlap<>7 then begin
  result:=sqrt(result);
 end else begin
  Temp:=ClosestBoxPoint;
  Direction.x:=ClosestBoxPoint.x/Extents.x;
  Direction.y:=ClosestBoxPoint.y/Extents.y;
  Direction.z:=ClosestBoxPoint.z/Extents.z;
  if (abs(Direction.x)>abs(Direction.y)) and (abs(Direction.x)>abs(Direction.z)) then begin
   if Direction.x<0.0 then begin
    ClosestBoxPoint.x:=-Extents.x;
   end else begin
    ClosestBoxPoint.x:=Extents.x;
   end;
  end else if (abs(Direction.y)>abs(Direction.x)) and (abs(Direction.y)>abs(Direction.z)) then begin
   if Direction.y<0.0 then begin
    ClosestBoxPoint.y:=-Extents.y;
   end else begin
    ClosestBoxPoint.y:=Extents.y;
   end;
  end else begin
   if Direction.z<0.0 then begin
    ClosestBoxPoint.z:=-Extents.z;
   end else begin
    ClosestBoxPoint.z:=Extents.z;
   end;
  end;
  result:=-Vector3Dist(ClosestBoxPoint,Temp);
 end;
 ClosestBoxPoint:=Vector3TermMatrixMul(ClosestBoxPoint,TransformMatrix);
end;

type TSortCompareFunction=function(const a,b:pointer):longint;

function IntLog2(x:longword):longword; {$ifdef cpu386}assembler; register;
asm
 test eax,eax
 jz @Done
 bsr eax,eax
 @Done:
end;
{$else}
begin
 x:=x or (x shr 1);
 x:=x or (x shr 2);
 x:=x or (x shr 4);
 x:=x or (x shr 8);
 x:=x or (x shr 16);
 x:=x shr 1;
 x:=x-((x shr 1) and $55555555);
 x:=((x shr 2) and $33333333)+(x and $33333333);
 x:=((x shr 4)+x) and $0f0f0f0f;
 x:=x+(x shr 8);
 x:=x+(x shr 16);
 result:=x and $3f;
end;
{$endif}

procedure MemorySwap(a,b:pointer;Size:longint);
var Temp:longint;
begin
 while Size>=SizeOf(longint) do begin
  Temp:=longword(a^);
  longword(a^):=longword(b^);
  longword(b^):=Temp;
  inc(PtrUInt(a),SizeOf(longword));
  inc(PtrUInt(b),SizeOf(longword));
  dec(Size,SizeOf(longword));
 end;
 while Size>=SizeOf(byte) do begin
  Temp:=byte(a^);
  byte(a^):=byte(b^);
  byte(b^):=Temp;
  inc(PtrUInt(a),SizeOf(byte));
  inc(PtrUInt(b),SizeOf(byte));
  dec(Size,SizeOf(byte));
 end;
end;

procedure DirectIntroSort(Items:pointer;Left,Right,ElementSize:longint;CompareFunc:TSortCompareFunction);
type PByteArray=^TByteArray;
     TByteArray=array[0..$3fffffff] of byte;
     PStackItem=^TStackItem;
     TStackItem=record
      Left,Right,Depth:longint;
     end;
var Depth,i,j,Middle,Size,Parent,Child,Pivot,iA,iB,iC:longint;
    StackItem:PStackItem;
    Stack:array[0..31] of TStackItem;
begin
 if Left<Right then begin
  StackItem:=@Stack[0];
  StackItem^.Left:=Left;
  StackItem^.Right:=Right;
  StackItem^.Depth:=IntLog2((Right-Left)+1) shl 1;
  inc(StackItem);
  while ptruint(pointer(StackItem))>ptruint(pointer(@Stack[0])) do begin
   dec(StackItem);
   Left:=StackItem^.Left;
   Right:=StackItem^.Right;
   Depth:=StackItem^.Depth;
   Size:=(Right-Left)+1;
   if Size<16 then begin
    // Insertion sort
    iA:=Left;
    iB:=iA+1;
    while iB<=Right do begin
     iC:=iB;
     while (iA>=Left) and
           (iC>=Left) and
           (CompareFunc(pointer(@PByteArray(Items)^[iA*ElementSize]),pointer(@PByteArray(Items)^[iC*ElementSize]))>0) do begin
      MemorySwap(@PByteArray(Items)^[iA*ElementSize],@PByteArray(Items)^[iC*ElementSize],ElementSize);
      dec(iA);
      dec(iC);
     end;
     iA:=iB;
     inc(iB);
    end;
   end else begin
    if (Depth=0) or (ptruint(pointer(StackItem))>=ptruint(pointer(@Stack[high(Stack)-1]))) then begin
     // Heap sort
     i:=Size div 2;
     repeat
      if i>0 then begin
       dec(i);
      end else begin
       dec(Size);
       if Size>0 then begin
        MemorySwap(@PByteArray(Items)^[(Left+Size)*ElementSize],@PByteArray(Items)^[Left*ElementSize],ElementSize);
       end else begin
        break;
       end;
      end;
      Parent:=i;
      repeat
       Child:=(Parent*2)+1;
       if Child<Size then begin
        if (Child<(Size-1)) and (CompareFunc(pointer(@PByteArray(Items)^[(Left+Child)*ElementSize]),pointer(@PByteArray(Items)^[(Left+Child+1)*ElementSize]))<0) then begin
         inc(Child);
        end;
        if CompareFunc(pointer(@PByteArray(Items)^[(Left+Parent)*ElementSize]),pointer(@PByteArray(Items)^[(Left+Child)*ElementSize]))<0 then begin
         MemorySwap(@PByteArray(Items)^[(Left+Parent)*ElementSize],@PByteArray(Items)^[(Left+Child)*ElementSize],ElementSize);
         Parent:=Child;
         continue;
        end;
       end;
       break;
      until false;
     until false;
    end else begin
     // Quick sort width median-of-three optimization
     Middle:=Left+((Right-Left) shr 1);
     if (Right-Left)>3 then begin
      if CompareFunc(pointer(@PByteArray(Items)^[Left*ElementSize]),pointer(@PByteArray(Items)^[Middle*ElementSize]))>0 then begin
       MemorySwap(@PByteArray(Items)^[Left*ElementSize],@PByteArray(Items)^[Middle*ElementSize],ElementSize);
      end;
      if CompareFunc(pointer(@PByteArray(Items)^[Left*ElementSize]),pointer(@PByteArray(Items)^[Right*ElementSize]))>0 then begin
       MemorySwap(@PByteArray(Items)^[Left*ElementSize],@PByteArray(Items)^[Right*ElementSize],ElementSize);
      end;
      if CompareFunc(pointer(@PByteArray(Items)^[Middle*ElementSize]),pointer(@PByteArray(Items)^[Right*ElementSize]))>0 then begin
       MemorySwap(@PByteArray(Items)^[Middle*ElementSize],@PByteArray(Items)^[Right*ElementSize],ElementSize);
      end;
     end;
     Pivot:=Middle;
     i:=Left;
     j:=Right;
     repeat
      while (i<Right) and (CompareFunc(pointer(@PByteArray(Items)^[i*ElementSize]),pointer(@PByteArray(Items)^[Pivot*ElementSize]))<0) do begin
       inc(i);
      end;
      while (j>=i) and (CompareFunc(pointer(@PByteArray(Items)^[j*ElementSize]),pointer(@PByteArray(Items)^[Pivot*ElementSize]))>0) do begin
       dec(j);
      end;
      if i>j then begin
       break;
      end else begin
       if i<>j then begin
        MemorySwap(@PByteArray(Items)^[i*ElementSize],@PByteArray(Items)^[j*ElementSize],ElementSize);
        if Pivot=i then begin
         Pivot:=j;
        end else if Pivot=j then begin
         Pivot:=i;
        end;
       end;
       inc(i);
       dec(j);
      end;
     until false;
     if i<Right then begin
      StackItem^.Left:=i;
      StackItem^.Right:=Right;
      StackItem^.Depth:=Depth-1;
      inc(StackItem);
     end;
     if Left<j then begin
      StackItem^.Left:=Left;
      StackItem^.Right:=j;
      StackItem^.Depth:=Depth-1;
      inc(StackItem);
     end;
    end;
   end;
  end;
 end;
end;

procedure IndirectIntroSort(Items:pointer;Left,Right:longint;CompareFunc:TSortCompareFunction);
type PPointers=^TPointers;
     TPointers=array[0..$ffff] of pointer;
     PStackItem=^TStackItem;
     TStackItem=record
      Left,Right,Depth:longint;
     end;
var Depth,i,j,Middle,Size,Parent,Child:longint;
    Pivot,Temp:pointer;
    StackItem:PStackItem;
    Stack:array[0..31] of TStackItem;
begin
 if Left<Right then begin
  StackItem:=@Stack[0];
  StackItem^.Left:=Left;
  StackItem^.Right:=Right;
  StackItem^.Depth:=IntLog2((Right-Left)+1) shl 1;
  inc(StackItem);
  while ptruint(pointer(StackItem))>ptruint(pointer(@Stack[0])) do begin
   dec(StackItem);
   Left:=StackItem^.Left;
   Right:=StackItem^.Right;
   Depth:=StackItem^.Depth;
   Size:=(Right-Left)+1;
   if Size<16 then begin
    // Insertion sort
    for i:=Left+1 to Right do begin
     Temp:=PPointers(Items)^[i];
     j:=i-1;
     if (j>=Left) and (CompareFunc(PPointers(Items)^[j],Temp)>0) then begin
      repeat
       PPointers(Items)^[j+1]:=PPointers(Items)^[j];
       dec(j);
      until not ((j>=Left) and (CompareFunc(PPointers(Items)^[j],Temp)>0));
      PPointers(Items)^[j+1]:=Temp;
     end;
    end;
   end else begin
    if (Depth=0) or (ptruint(pointer(StackItem))>=ptruint(pointer(@Stack[high(Stack)-1]))) then begin
     // Heap sort
     i:=Size div 2;
     Temp:=nil;
     repeat
      if i>Left then begin
       dec(i);
       Temp:=PPointers(Items)^[Left+i];
      end else begin
       dec(Size);
       if Size>0 then begin
        Temp:=PPointers(Items)^[Left+Size];
        PPointers(Items)^[Left+Size]:=PPointers(Items)^[Left];
       end else begin
        break;
       end;
      end;
      Parent:=i;
      Child:=(i*2)+1;
      while Child<Size do begin
       if ((Child+1)<Size) and (CompareFunc(PPointers(Items)^[Left+Child+1],PPointers(Items)^[Left+Child])>0) then begin
        inc(Child);
       end;
       if CompareFunc(PPointers(Items)^[Left+Child],Temp)>0 then begin
        PPointers(Items)^[Left+Parent]:=PPointers(Items)^[Left+Child];
        Parent:=Child;
        Child:=(Parent*2)+1;
       end else begin
        break;
       end;
      end;
      PPointers(Items)^[Left+Parent]:=Temp;
     until false;
    end else begin
     // Quick sort width median-of-three optimization
     Middle:=Left+((Right-Left) shr 1);
     if (Right-Left)>3 then begin
      if CompareFunc(PPointers(Items)^[Left],PPointers(Items)^[Middle])>0 then begin
       Temp:=PPointers(Items)^[Left];
       PPointers(Items)^[Left]:=PPointers(Items)^[Middle];
       PPointers(Items)^[Middle]:=Temp;
      end;
      if CompareFunc(PPointers(Items)^[Left],PPointers(Items)^[Right])>0 then begin
       Temp:=PPointers(Items)^[Left];
       PPointers(Items)^[Left]:=PPointers(Items)^[Right];
       PPointers(Items)^[Right]:=Temp;
      end;
      if CompareFunc(PPointers(Items)^[Middle],PPointers(Items)^[Right])>0 then begin
       Temp:=PPointers(Items)^[Middle];
       PPointers(Items)^[Middle]:=PPointers(Items)^[Right];
       PPointers(Items)^[Right]:=Temp;
      end;
     end;
     Pivot:=PPointers(Items)^[Middle];
     i:=Left;
     j:=Right;
     repeat
      while (i<Right) and (CompareFunc(PPointers(Items)^[i],Pivot)<0) do begin
       inc(i);
      end;
      while (j>=i) and (CompareFunc(PPointers(Items)^[j],Pivot)>0) do begin
       dec(j);
      end;
      if i>j then begin
       break;
      end else begin
       if i<>j then begin
        Temp:=PPointers(Items)^[i];
        PPointers(Items)^[i]:=PPointers(Items)^[j];
        PPointers(Items)^[j]:=Temp;
       end;
       inc(i);
       dec(j);
      end;
     until false;
     if i<Right then begin
      StackItem^.Left:=i;
      StackItem^.Right:=Right;
      StackItem^.Depth:=Depth-1;
      inc(StackItem);
     end;
     if Left<j then begin
      StackItem^.Left:=Left;
      StackItem^.Right:=j;
      StackItem^.Depth:=Depth-1;
      inc(StackItem);
     end;
    end;
   end;
  end;
 end;
end;

function SolveQuadraticRoots(const a,b,c:TKraftScalar;out t1,t2:TKraftScalar):boolean; {$ifdef caninline}inline;{$endif}
var d,InverseDenominator:TKraftScalar;
begin
 result:=false;
 d:=sqr(b)-(4.0*(a*c));
 if d>=0.0 then begin
  InverseDenominator:=1.0/(2.0*a);
  if abs(d)<EPSILON then begin
   t1:=(-b)*InverseDenominator;
   t2:=t1;
  end else begin
   d:=sqrt(d);
   t1:=((-b)-d)*InverseDenominator;
   t2:=((-b)+d)*InverseDenominator;
  end;
  result:=true;
 end;
end;

function SweepSphereSphere(const pa0,pa1:TKraftVector3;const ra:TKraftScalar;const pb0,pb1:TKraftVector3;const rb:TKraftScalar;out t0,t1:TKraftScalar):boolean; overload; {$ifdef caninline}inline;{$endif}
var va,vb,ab,vab:TKraftVector3;
    rab,a,b,c:TKraftScalar;
begin
 va:=Vector3Sub(pa1,pa0);
 vb:=Vector3Sub(pb1,pb0);
 ab:=Vector3Sub(pb0,pa0);
 vab:=Vector3Sub(vb,va);
 rab:=ra+rb;
 a:=Vector3LengthSquared(vab);
 c:=Vector3LengthSquared(ab)-sqr(rab);
 if (abs(a)<EPSILON) or (c<=0.0) then begin
  t0:=0.0;
  t1:=0.0;
  result:=true;
 end else begin
  b:=2.0*Vector3Dot(vab,ab);
  if SolveQuadraticRoots(a,b,c,t0,t1) then begin
   if t1>t0 then begin
    a:=t0;
    t0:=t1;
    t1:=a;
   end;
   result:=(t1>=0.0) and (t0<=1.0);
  end else begin
   result:=false;
  end;
 end;
end;

function SweepSphereSphere(const pa0,pa1:TKraftVector3;const ra:TKraftScalar;const pb0,pb1:TKraftVector3;const rb:TKraftScalar;out Time,Distance:TKraftScalar;out Normal:TKraftVector3):boolean; overload; {$ifdef caninline}inline;{$endif}
var t0,t1:TKraftScalar;
begin
 result:=SweepSphereSphere(pa0,pa1,ra,pb0,pb1,rb,t0,t1);
 if result then begin
  Time:=t0;
  Normal:=Vector3Sub(Vector3Lerp(pb0,pb1,t0),Vector3Lerp(pa0,pa1,t0));
  Distance:=Vector3LengthNormalize(Normal);
  if Distance<EPSILON then begin
   Normal:=Vector3Sub(pb0,pb1);
   if Vector3LengthNormalize(Normal)<EPSILON then begin
    Normal:=Vector3Sub(pa1,pa0);
   end;
  end;
 end;
end;

function MPRIntersection(const ShapeA,ShapeB:TKraftShape;const TransformA,TransformB:TKraftMatrix4x4):boolean;
var Phase1Iteration,Phase2Iterations:longint;
    v0,v1,v2,v3,v4,t,n:TKraftVector3;
begin
 result:=false;

 v0:=Vector3Sub(ShapeB.GetCenter(TransformB),ShapeA.GetCenter(TransformA));

 if Vector3LengthSquared(v0)<1e-5 then begin
  v0.x:=1e-5;
 end;

 n:=Vector3Neg(v0);
 v1:=Vector3Sub(Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB),
                Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(v0,TransformA)),TransformA));
 if Vector3Dot(v1,n)<=0.0 then begin
  exit;
 end;

 n:=Vector3Cross(v1,v0);
 if Vector3LengthSquared(n)<EPSILON then begin
  result:=true;
  exit;
 end;

 v2:=Vector3Sub(Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB),
                Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Vector3Neg(n),TransformA)),TransformA));
 if Vector3Dot(v2,n)<=0.0 then begin
  exit;
 end;

 n:=Vector3Cross(Vector3Sub(v1,v0),Vector3Sub(v2,v0));
 if Vector3Dot(n,v0)>0.0 then begin
  t:=v1;
  v1:=v2;
  v2:=t;
  n:=Vector3Neg(n);
 end;

 for Phase1Iteration:=1 to MPRMaximumIterations do begin

  v3:=Vector3Sub(Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB),
                 Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Vector3Neg(n),TransformA)),TransformA));
  if Vector3Dot(v3,n)<=0.0 then begin
   exit;
  end;

{ t:=Vector3Cross(v3,v0);

  if Vector3Dot(t,v1)<0.0 then begin}
  if Vector3Dot(v0,Vector3Cross(v3,v1))>0.0 then begin
   v2:=v3;
   n:=Vector3Cross(Vector3Sub(v1,v0),Vector3Sub(v3,v0));
   continue;
  end;

//if Vector3Dot(t,v2)>0.0 then begin
  if Vector3Dot(v0,Vector3Cross(v2,v3))>0.0 then begin
   v1:=v3;
   n:=Vector3Cross(Vector3Sub(v3,v0),Vector3Sub(v2,v0));
   continue;
  end;

  Phase2Iterations:=0;

  repeat
   inc(Phase2Iterations);
   n:=Vector3Cross(Vector3Sub(v2,v1),Vector3Sub(v3,v1));
   if Vector3LengthSquared(n)<EPSILON then begin
    result:=true;
    exit;
   end;
   Vector3Normalize(n);
   if (Vector3Dot(v1,n)>=0.0) and not result then begin
    result:=true;
   end;
   v4:=Vector3Sub(Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB),
                  Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Vector3Neg(n),TransformA)),TransformA));
   if (Vector3Dot(Vector3Sub(v4,v3),n)<=MPRTolerance) or
      (Vector3Dot(v4,n)<=0.0) or
      (Phase2Iterations>MPRMaximumIterations) then begin
    exit;
   end;
   t:=Vector3Cross(v4,v0);
   if Vector3Dot(t,v1)>0.0 then begin
    if Vector3Dot(t,v2)>0.0 then begin
     v1:=v4;
    end else begin
     v3:=v4;
    end;
   end else begin
    if Vector3Dot(t,v3)>0.0 then begin
     v2:=v4;
    end else begin
     v1:=v4;
    end;
   end;
  until false;
  
 end;

end;

function MPRPenetration(const ShapeA,ShapeB:TKraftShape;const TransformA,TransformB:TKraftMatrix4x4;out PositionA,PositionB,Normal:TKraftVector3;out PenetrationDepth:TKraftScalar):boolean;
var Phase1Iteration,Phase2Iterations:longint;
    b0,b1,b2,b3,Sum,Inv:TKraftScalar;
    v0,v0a,v0b,v1,v1a,v1b,v2,v2a,v2b,v3,v3a,v3b,v4,v4a,v4b,t,n:TKraftVector3;
begin
 result:=false;

 PositionA:=Vector3Origin;
 PositionB:=Vector3Origin;
 PenetrationDepth:=0.0;

 v0a:=ShapeA.GetCenter(TransformA);
 v0b:=ShapeB.GetCenter(TransformB);
 v0:=Vector3Sub(v0b,v0a);

 if Vector3LengthSquared(v0)<1e-5 then begin
  v0.x:=1e-5;
 end;

 n:=Vector3Neg(v0);
 v1a:=Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(v0,TransformA)),TransformA);
 v1b:=Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB);
 v1:=Vector3Sub(v1b,v1a);
 if Vector3Dot(v1,n)<=0.0 then begin
  Normal:=n;
  exit;
 end;

 n:=Vector3Cross(v1,v0);
 if Vector3LengthSquared(n)<EPSILON then begin
  PositionA:=v1a;
  PositionB:=v1b;
  Normal:=Vector3Norm(Vector3Sub(v1,v0));
  PenetrationDepth:=Vector3Dot(Vector3Sub(v1b,v1a),Normal);
  result:=true;
  exit;
 end;

 v2a:=Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Vector3Neg(n),TransformA)),TransformA);
 v2b:=Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB);
 v2:=Vector3Sub(v2b,v2a);
 if Vector3Dot(v2,n)<=0.0 then begin
  Normal:=n;
  exit;
 end;

 n:=Vector3Cross(Vector3Sub(v1,v0),Vector3Sub(v2,v0));
 if Vector3Dot(n,v0)>0.0 then begin
  t:=v1;
  v1:=v2;
  v2:=t;
  t:=v1a;
  v1a:=v2a;
  v2a:=t;
  t:=v1b;
  v1b:=v2b;
  v2b:=t;
  n:=Vector3Neg(n);
 end;

 for Phase1Iteration:=1 to MPRMaximumIterations do begin

  v3a:=Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Vector3Neg(n),TransformA)),TransformA);
  v3b:=Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB);
  v3:=Vector3Sub(v3b,v3a);
  if Vector3Dot(v3,n)<=0.0 then begin
   Normal:=n;
   exit;
  end;

{ t:=Vector3Cross(v3,v0);

  if Vector3Dot(t,v1)<0.0 then begin}
  if Vector3Dot(v0,Vector3Cross(v3,v1))>0.0 then begin
   v2:=v3;
   v2a:=v3a;
   v2b:=v3b;
   n:=Vector3Cross(Vector3Sub(v1,v0),Vector3Sub(v3,v0));
   continue;
  end;

//if Vector3Dot(t,v2)>0.0 then begin
  if Vector3Dot(v0,Vector3Cross(v2,v3))>0.0 then begin
   v1:=v3;
   v1a:=v3a;
   v1b:=v3b;
   n:=Vector3Cross(Vector3Sub(v3,v0),Vector3Sub(v2,v0));
   continue;
  end;

  Phase2Iterations:=0;

  repeat

   inc(Phase2Iterations);

   n:=Vector3Cross(Vector3Sub(v2,v1),Vector3Sub(v3,v1));

   if Vector3LengthSquared(n)<EPSILON then begin
    PositionA:=v1a;
    PositionB:=v1b;
    Normal:=Vector3Norm(Vector3Sub(v1,v0));
    PenetrationDepth:=Vector3Dot(Vector3Sub(v1b,v1a),Normal);
    result:=true;
    exit;
   end;

   Vector3Normalize(n);

   if (Vector3Dot(v1,n)>=0.0) and not result then begin
    result:=true;
   end;

   v4a:=Vector3TermMatrixMul(ShapeA.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Vector3Neg(n),TransformA)),TransformA);
   v4b:=Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(n,TransformB)),TransformB);
   v4:=Vector3Sub(v4b,v4a);

   PenetrationDepth:=Vector3Dot(v4,n);

   if (Vector3Dot(Vector3Sub(v4,v3),n)<=MPRTolerance) or
      (PenetrationDepth<=0.0) or
      (Phase2Iterations>MPRMaximumIterations) then begin

    if result then begin

     Normal:=n;

     b0:=Vector3Dot(Vector3Cross(v1,v2),v3);
     b1:=Vector3Dot(Vector3Cross(v3,v2),v0);
     b2:=Vector3Dot(Vector3Cross(v0,v1),v3);
     b3:=Vector3Dot(Vector3Cross(v2,v1),v0);

     Sum:=b0+b1+b2+b3;

     if Sum<=0.0 then begin

      b0:=0.0;
      b1:=Vector3Dot(Vector3Cross(v2,v3),n);
      b2:=Vector3Dot(Vector3Cross(v3,v1),n);
      b3:=Vector3Dot(Vector3Cross(v1,v2),n);

      Sum:=b1+b2+b3;

     end;

     Inv:=1.0/Sum;

     PositionA.x:=((v0a.x*b0)+(v1a.x*b1)+(v2a.x*b2)+(v3a.x*b3))*Inv;
     PositionA.y:=((v0a.y*b0)+(v1a.y*b1)+(v2a.y*b2)+(v3a.y*b3))*Inv;
     PositionA.z:=((v0a.z*b0)+(v1a.z*b1)+(v2a.z*b2)+(v3a.z*b3))*Inv;

     PositionB.x:=((v0b.x*b0)+(v1b.x*b1)+(v2b.x*b2)+(v3b.x*b3))*Inv;
     PositionB.y:=((v0b.y*b0)+(v1b.y*b1)+(v2b.y*b2)+(v3b.y*b3))*Inv;
     PositionB.z:=((v0b.z*b0)+(v1b.z*b1)+(v2b.z*b2)+(v3b.z*b3))*Inv;

    end;

    exit;

   end;

   t:=Vector3Cross(v4,v0);

   if Vector3Dot(t,v1)>0.0 then begin

    if Vector3Dot(t,v2)>0.0 then begin

     v1:=v4;
     v1a:=v4a;
     v1b:=v4b;

    end else begin

     v3:=v4;
     v3a:=v4a;
     v3b:=v4b;

    end;

   end else begin

    if Vector3Dot(t,v3)>0.0 then begin

     v2:=v4;
     v2a:=v4a;
     v2b:=v4b;

    end else begin

     v1:=v4;
     v1a:=v4a;
     v1b:=v4b;

    end;

   end;

  until false;

 end;

end;

function MPRAreSweptShapesIntersecting(const ShapeA,ShapeB:TKraftShape;const Sweep:TKraftVector3;const Transform:TKraftMatrix4x4;var HitPosition:TKraftVector3):boolean;
 function GetSweptExtremePoint(const Direction:TKraftVector3;out ExtremePointA:TKraftVector3):TKraftVector3;
 begin
  ExtremePointA:=ShapeA.GetLocalFullSupport(Direction);
  result:=Vector3Sub(ExtremePointA,Vector3TermMatrixMul(ShapeB.GetLocalFullSupport(Vector3TermMatrixMulTransposedBasis(Direction,Transform)),Transform));
  if Vector3Dot(Direction,Sweep)>0.0 then begin
   result:=Vector3Add(result,Sweep);
  end;
 end;
var Count:longint;
    LocalPoint,v0,v1,v1a,v2,v2a,v3,v3a,v4,v4a,v1v0,v2v0,v3v0,t,n:TKraftVector3;
    Dot,DotV0,BarycentricCoordinate,v0v1v2v3Volume,v1v2v3Volume,v0v2v3Volume,v0v1v3Volume,
    InverseTotalVolume,v0Weight,v1Weight,v2Weight,v3Weight,Dot2:TKraftScalar;
begin
 result:=false;

 LocalPoint.x:=Transform[3,0];
 LocalPoint.y:=Transform[3,1];
 LocalPoint.z:=Transform[3,2];

 if Vector3LengthSquared(LocalPoint)<EPSILON then begin
  HitPosition:=LocalPoint;
  result:=true;
  exit;
 end;

 v0:=Vector3Neg(LocalPoint);

 n:=LocalPoint;
 v1:=GetSweptExtremePoint(n,v1a);

 n:=Vector3Cross(v1,v0);
 if Vector3LengthSquared(n)<EPSILON then begin
  Dot:=Vector3Dot(v1,LocalPoint);
  if Dot<0.0 then begin
   result:=false;
  end else begin
   DotV0:=Vector3Dot(v0,LocalPoint);
   BarycentricCoordinate:=-(Dotv0/(Dot-DotV0));
   HitPosition:=Vector3ScalarMul(v1a,BarycentricCoordinate);
   result:=true;
  end;
  exit;
 end;

 v2:=GetSweptExtremePoint(n,v2a);

 n:=Vector3Cross(Vector3Sub(v1,v0),Vector3Sub(v2,v0));

 for Count:=1 to MPRMaximumIterations do begin
  v3:=GetSweptExtremePoint(n,v3a);
  if Vector3Dot(v0,Vector3Cross(v1,v3))<0.0 then begin
   v2:=v3;
   v2a:=v3a;
   n:=Vector3Cross(Vector3Sub(v1,v0),Vector3Sub(v3,v0));
  end else if Vector3Dot(v0,Vector3Cross(v3,v2))<0.0 then begin
   v1:=v3;
   v1a:=v3a;
   n:=Vector3Cross(Vector3Sub(v2,v0),Vector3Sub(v3,v0));
  end else begin
   break;
  end;
 end;

 Count:=0;
 while true do begin

  n:=Vector3Cross(Vector3Sub(v3,v2),Vector3Sub(v1,v2));

  Dot:=Vector3Dot(v1,n);
  if Dot>=0.0 then begin

   v1v0:=Vector3Sub(v1,v0);
   v2v0:=Vector3Sub(v2,v0);
   v3v0:=Vector3Sub(v3,v0);

   v0v1v2v3Volume:=Vector3Dot(Vector3Cross(v1v0,v2v0),v3v0);
   v1v2v3Volume:=Vector3Dot(Vector3Cross(v1,v2),v3);
   v0v2v3Volume:=Vector3Dot(Vector3Cross(LocalPoint,v2v0),v3v0);
   v0v1v3Volume:=Vector3Dot(Vector3Cross(v1v0,LocalPoint),v3v0);

   InverseTotalVolume:=1.0/v0v1v2v3Volume;
   v0Weight:=v1v2v3volume*InverseTotalVolume;
   v1Weight:=v0v2v3volume*InverseTotalVolume;
   v2Weight:=v0v1v3volume*InverseTotalVolume;
   v3Weight:=1.0-(v0Weight+v1Weight+v2Weight);
   HitPosition:=Vector3Add(Vector3Add(Vector3ScalarMul(v1a,v1Weight),Vector3ScalarMul(v2a,v2Weight)),Vector3ScalarMul(v3a,v3Weight));
   result:=true;
   exit;
  end;

  v4:=GetSweptExtremePoint(n,v4a);
  Dot2:=Vector3Dot(v4,n);
  if (Dot2<0.0) or
     ((Dot2-Dot)<MPRTolerance) or
     (Count>MPRSweepCastMaximumIterations) then begin
   HitPosition:=LocalPoint;
   exit;
  end;

  t:=Vector3Cross(v4,v0);
  if Vector3Dot(t,v1)>=0.0 then begin
   if Vector3Dot(t,v2)>=0.0 then begin
    v1:=v4;
    v1a:=v4a;
   end else begin
    v3:=v4;
    v3a:=v4a;
   end;
  end else begin
   if Vector3Dot(t,v3)>=0.0 then begin
    v2:=v4;
    v2a:=v4a;
   end else begin
    v1:=v4;
    v1a:=v4a;
   end;
  end;
  inc(Count);
 end;

end;

function MPRSweep(const ShapeA,ShapeB:TKraftShape;const SweepA,SweepB:TKraftSweep):boolean; overload;
var RayLengthSquared,SweepLength,MaximumRadius,t0,t1:TKraftScalar;
    VelocityWorld,LocalOrigin,LocalDirection,Sweep,HitPosition,VelocityA,VelocityB:TKraftVector3;
    NegativeSweepLength:boolean;
    LocalTransformBinA:TKraftMatrix4x4;
    Positions:array[0..1,0..1] of TKraftVector3;
    Transforms:array[0..1,0..1] of TKraftMatrix4x4;
begin
 result:=false;

 Transforms[0,0]:=Matrix4x4TermMul(ShapeA.fLocalTransform,SweepTransform(SweepA,0.0));
 Transforms[0,1]:=Matrix4x4TermMul(ShapeA.fLocalTransform,SweepTransform(SweepA,1.0));
 Transforms[1,0]:=Matrix4x4TermMul(ShapeB.fLocalTransform,SweepTransform(SweepB,0.0));
 Transforms[1,1]:=Matrix4x4TermMul(ShapeB.fLocalTransform,SweepTransform(SweepB,1.0));

 Positions[0,0]:=Vector3TermMatrixMul(ShapeA.fLocalCenterOfMass,Transforms[0,0]);
 Positions[0,1]:=Vector3TermMatrixMul(ShapeA.fLocalCenterOfMass,Transforms[0,1]);
 Positions[1,0]:=Vector3TermMatrixMul(ShapeB.fLocalCenterOfMass,Transforms[1,0]);
 Positions[1,1]:=Vector3TermMatrixMul(ShapeB.fLocalCenterOfMass,Transforms[1,1]);

 if not SweepSphereSphere(Positions[0,0],Positions[0,1],ShapeA.fShapeSphere.Radius,Positions[1,0],Positions[1,1],ShapeB.fShapeSphere.Radius,t0,t1) then begin
  exit;
 end;

 VelocityA:=Vector3Sub(Positions[0,1],Positions[0,0]);
 VelocityB:=Vector3Sub(Positions[1,1],Positions[1,0]);

 VelocityWorld:=Vector3Sub(VelocityB,VelocityA);

 LocalDirection:=Vector3SafeNorm(Vector3TermMatrixMulTransposedBasis(VelocityWorld,Transforms[0,0]));

 LocalTransformBinA:=Matrix4x4TermMulInverted(Transforms[1,0],Transforms[0,0]);

 MaximumRadius:=ShapeA.fShapeSphere.Radius+ShapeB.fShapeSphere.Radius;

 LocalOrigin.x:=LocalTransformBinA[3,0];
 LocalOrigin.y:=LocalTransformBinA[3,1];
 LocalOrigin.z:=LocalTransformBinA[3,2];

 // This sweep amount needs to expand the minkowski difference to fully intersect the plane defined by the sweep direction and origin.
 RayLengthSquared:=Vector3LengthSquared(LocalDirection);
 if RayLengthSquared>EPSILON then begin
  // Scale the sweep length by the margins. Divide by the length to pull the margin into terms of the length of the ray.
  SweepLength:=(Vector3Dot(LocalOrigin,LocalDirection)+MaximumRadius)/sqrt(RayLengthSquared);
 end else begin
  SweepLength:=0.0;
 end;

 // If the sweep direction is found to be negative, the ray can be thought of as pointing away from the shape, so then do not sweep backward.
 NegativeSweepLength:=SweepLength<0.0;
 if NegativeSweepLength then begin
  SweepLength:=0.0;
 end;

 Sweep:=Vector3ScalarMul(LocalDirection,SweepLength);

 result:=MPRAreSweptShapesIntersecting(ShapeA,ShapeB,Sweep,LocalTransformBinA,HitPosition);
end;

function TKraftGJK.Run:boolean;
var CountSaved,Index,iA,iB:longint;
    Initialized,Duplicate:boolean;
    SimplexVertex:PKraftGJKSimplexVertex;
    CachedSimplexVertex:PKraftGJKCachedSimplexVertex;
    Metrics,SquaredDistances:array[0..1] of TKraftScalar;
    Saved:array[0..3,0..1] of longint;
    Direction,a,b,c,d,ba,ab,cb,bc,ac,ca,db,bd,cd,dc,da,ad,baxca,daxba,bcxdc,caxda:TKraftVector3;
    uAB,vAB,uBC,vBC,uCA,vCA,uBD,vBD,uDC,vDC,uAD,vAD,uADB,vADB,wADB,uACD,vACD,wACD,uCBD,vCBD,wCBD,
    uABC,vABC,wABC,uABCD,vABCD,wABCD,xABCD,Denominator:TKraftScalar;
    TempVertex:PKraftGJKSimplexVertex;
begin

 Failed:=false;

 Initialized:=false;

 // Initialize simplex vertex permutation order
 Simplex.Vertices[0]:=@Simplex.VerticesData[0];
 Simplex.Vertices[1]:=@Simplex.VerticesData[1];
 Simplex.Vertices[2]:=@Simplex.VerticesData[2];
 Simplex.Vertices[3]:=@Simplex.VerticesData[3];

 // Try refill from cache
 if assigned(CachedSimplex) then begin
  Simplex.Count:=CachedSimplex^.Count;
  if Simplex.Count>0 then begin
   for Index:=0 to Simplex.Count-1 do begin
    CachedSimplexVertex:=@CachedSimplex^.Vertices[Index];
    SimplexVertex:=Simplex.Vertices[Index];
    SimplexVertex^.iA:=CachedSimplexVertex^.iA;
    SimplexVertex^.iB:=CachedSimplexVertex^.iB;
    SimplexVertex^.sA:=Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(SimplexVertex^.iA),Transforms[0]^);
    SimplexVertex^.sB:=Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(SimplexVertex^.iB),Transforms[1]^);
    SimplexVertex^.w:=Vector3Sub(SimplexVertex^.sB,SimplexVertex^.sA);
    SimplexVertex^.a:=CachedSimplexVertex^.a;
   end;
   Metrics[0]:=CachedSimplex^.Metric;
   case Simplex.Count of
    1:begin
     Metrics[1]:=0.0;
    end;
    2:begin
     Metrics[1]:=Vector3Dist(Simplex.Vertices[0]^.w,Simplex.Vertices[1]^.w);
    end;
    3:begin
     Metrics[1]:=CalculateArea(Simplex.Vertices[0]^.w,Simplex.Vertices[1]^.w,Simplex.Vertices[2]^.w);
    end;
    4:begin
     Metrics[1]:=CalculateVolume(Simplex.Vertices[0]^.w,Simplex.Vertices[1]^.w,Simplex.Vertices[2]^.w,Simplex.Vertices[3]^.w);
    end;
    else begin
     Assert(false);
     Metrics[1]:=0.0;
    end;
   end;
   if not ((Metrics[1]<(Metrics[0]*0.5)) or ((Metrics[0]*2.0)<Metrics[1]) or (Metrics[1]<EPSILON)) then begin
    Initialized:=true;
   end;
  end;
 end;

 // Initialize simplex if the cache was empty or its content was invalid
 if not Initialized then begin
  SimplexVertex:=Simplex.Vertices[0];
  SimplexVertex^.iA:=0;
  SimplexVertex^.iB:=0;
  SimplexVertex^.sA:=Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(0),Transforms[0]^);
  SimplexVertex^.sB:=Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(0),Transforms[1]^);
  SimplexVertex^.w:=Vector3Sub(SimplexVertex^.sB,SimplexVertex^.sA);
  SimplexVertex^.a:=1.0;
  Simplex.Count:=1;
 end;

 SquaredDistances[0]:=MAX_SCALAR;
 SquaredDistances[1]:=MAX_SCALAR;

 Iterations:=0;

 // The main loop
 repeat

  // Remember old simplex vertex indices
  CountSaved:=Simplex.Count;
  for Index:=0 to CountSaved-1 do begin
   Saved[Index,0]:=Simplex.Vertices[Index]^.iA;
   Saved[Index,1]:=Simplex.Vertices[Index]^.iB;
  end;

  // Reduce simplex
  case Simplex.Count of
   1:begin
    // Point
   end;
   2:begin
    // Line
    a:=Simplex.Vertices[0]^.w;
    b:=Simplex.Vertices[1]^.w;
    vAB:=Vector3Dot(a,Vector3Sub(a,b));
    if vAB<=0.0 then begin
     // Region A
     Simplex.Vertices[0].a:=1.0;
     Simplex.Divisor:=1.0;
     Simplex.Count:=1;
    end else begin
     uAB:=Vector3Dot(b,Vector3Sub(b,a));
     if uAB<=0.0 then begin
      // Region B
      TempVertex:=Simplex.Vertices[0];
      Simplex.Vertices[0]:=Simplex.Vertices[1];
      Simplex.Vertices[1]:=TempVertex;
      Simplex.Vertices[0]^.a:=1.0;
      Simplex.Divisor:=1.0;
      Simplex.Count:=1;
     end else begin
      if abs(uAB+vAB)<EPSILON then begin
       // Terminate on affinely dependent points in the set (if uAB+vAB is (nearly) zero, we can never use point B)
       Simplex.Vertices[0]^.a:=1.0;
       Simplex.Divisor:=1.0;
       Simplex.Count:=1;
      end else begin
       // Region AB
       Simplex.Vertices[0]^.a:=uAB;
       Simplex.Vertices[1]^.a:=vAB;
       Simplex.Divisor:=uAB+vAB;
       Simplex.Count:=2;
      end;
     end;
    end;
   end;
   3:begin
    // Triangle
    a:=Simplex.Vertices[0]^.w;
    b:=Simplex.Vertices[1]^.w;
    c:=Simplex.Vertices[2]^.w;
    vAB:=Vector3Dot(a,Vector3Sub(a,b));
    uCA:=Vector3Dot(a,Vector3Sub(a,c));
    if (vAB<=0.0) and (uCA<=0.0) then begin
     // Region A
     Simplex.Vertices[0]^.a:=1.0;
     Simplex.Divisor:=1.0;
     Simplex.Count:=1;
    end else begin
     ba:=Vector3Sub(b,a);
     uAB:=Vector3Dot(b,ba);
     vBC:=Vector3Dot(b,Vector3Sub(b,c));
     if (uAB<=0.0) and (vBC<=0.0) then begin
      // Region B
      TempVertex:=Simplex.Vertices[0];
      Simplex.Vertices[0]:=Simplex.Vertices[1];
      Simplex.Vertices[1]:=TempVertex;
      Simplex.Vertices[0]^.a:=1.0;
      Simplex.Divisor:=1.0;
      Simplex.Count:=1;
     end else begin
      uBC:=Vector3Dot(c,Vector3Sub(c,b));
      ca:=Vector3Sub(c,a);
      vCA:=Vector3Dot(c,ca);
      if (uBC<=0.0) and (vCA<=0.0) then begin
       // Region C
       TempVertex:=Simplex.Vertices[0];
       Simplex.Vertices[0]:=Simplex.Vertices[2];
       Simplex.Vertices[2]:=TempVertex;
       Simplex.Vertices[0]^.a:=1.0;
       Simplex.Divisor:=1.0;
       Simplex.Count:=1;
      end else begin
       baxca:=Vector3Cross(ba,ca);
       wABC:=Vector3Dot(Vector3Cross(a,b),baxca);
       if (uAB>0.0) and (vAB>0.0) and (wABC<=0.0) then begin
        // Region AB
        Simplex.Vertices[0]^.a:=uAB;
        Simplex.Vertices[1]^.a:=vAB;
        Simplex.Divisor:=uAB+vAB;
        Simplex.Count:=2;
       end else begin
        uABC:=Vector3Dot(Vector3Cross(b,c),baxca);
        if (uBC>0.0) and (vBC>0.0) and (uABC<=0.0) then begin
         // Region BC
         TempVertex:=Simplex.Vertices[0];
         Simplex.Vertices[0]:=Simplex.Vertices[1];
         Simplex.Vertices[1]:=Simplex.Vertices[2];
         Simplex.Vertices[2]:=TempVertex;
         Simplex.Vertices[0]^.a:=uBC;
         Simplex.Vertices[1]^.a:=vBC;
         Simplex.Divisor:=uBC+vBC;
         Simplex.Count:=2;
        end else begin
         vABC:=Vector3Dot(Vector3Cross(c,a),baxca);
         if (uCA>0.0) and (vCA>0.0) and (vABC<=0.0) then begin
          // Region CA
          TempVertex:=Simplex.Vertices[1];
          Simplex.Vertices[1]:=Simplex.Vertices[0];
          Simplex.Vertices[0]:=Simplex.Vertices[2];
          Simplex.Vertices[2]:=TempVertex;
          Simplex.Vertices[0]^.a:=uCA;
          Simplex.Vertices[1]^.a:=vCA;
          Simplex.Divisor:=uCA+vCA;
          Simplex.Count:=2;
         end else begin
          if (uABC>0.0) and (vABC>0.0) and (wABC>0.0) then begin
           // Region ABC
           Simplex.Vertices[0]^.a:=uABC;
           Simplex.Vertices[1]^.a:=vABC;
           Simplex.Vertices[2]^.a:=wABC;
           Simplex.Divisor:=uABC+vABC+wABC;
           Simplex.Count:=3;
          end else begin
           Assert(false);
          end;
         end;
        end;
       end;
      end;
     end;
    end;
   end;
   4:begin
    // Tetrahedron
    a:=Simplex.Vertices[0]^.w;
    b:=Simplex.Vertices[1]^.w;
    c:=Simplex.Vertices[2]^.w;
    d:=Simplex.Vertices[3]^.w;
    ab:=Vector3Sub(a,b);
    ac:=Vector3Sub(a,c);
    ad:=Vector3Sub(a,d);
    vAB:=Vector3Dot(a,ab);
    uCA:=Vector3Dot(a,ac);
    vAD:=Vector3Dot(a,ad);
    if (vAB<=0.0) and (uCA<=0.0) and (vAD<=0.0) then begin
     // Region A
     Simplex.Vertices[0]^.a:=1.0;
     Simplex.Divisor:=1.0;
     Simplex.Count:=1;
    end else begin
     ba:=Vector3Sub(b,a);
     bc:=Vector3Sub(b,c);
     bd:=Vector3Sub(b,d);
     uAB:=Vector3Dot(b,ba);
     vBC:=Vector3Dot(b,bc);
     vBD:=Vector3Dot(b,bd);
     if (uAB<=0.0) and (vBC<=0.0) and (vBD<=0.0) then begin
      // Region B
      TempVertex:=Simplex.Vertices[0];
      Simplex.Vertices[0]:=Simplex.Vertices[1];
      Simplex.Vertices[1]:=TempVertex;
      Simplex.Vertices[0]^.a:=1.0;
      Simplex.Divisor:=1.0;
      Simplex.Count:=1;
     end else begin
      cb:=Vector3Sub(c,b);
      ca:=Vector3Sub(c,a);
      cd:=Vector3Sub(c,d);
      uBC:=Vector3Dot(c,cb);
      vCA:=Vector3Dot(c,ca);
      uDC:=Vector3Dot(c,cd);
      if (uBC<=0.0) and (vCA<=0.0) and (uDC<=0.0) then begin
       // Region C
       TempVertex:=Simplex.Vertices[0];
       Simplex.Vertices[0]:=Simplex.Vertices[2];
       Simplex.Vertices[2]:=TempVertex;
       Simplex.Vertices[0]^.a:=1.0;
       Simplex.Divisor:=1.0;
       Simplex.Count:=1;
      end else begin
       db:=Vector3Sub(d,b);
       dc:=Vector3Sub(d,c);
       da:=Vector3Sub(d,a);
       uBD:=Vector3Dot(d,db);
       vDC:=Vector3Dot(d,dc);
       uAD:=Vector3Dot(d,da);
       if (uBD<=0.0) and (vDC<=0.0) and (uAD<=0.0) then begin
        // Region D
        TempVertex:=Simplex.Vertices[0];
        Simplex.Vertices[0]:=Simplex.Vertices[3];
        Simplex.Vertices[3]:=TempVertex;
        Simplex.Vertices[0]^.a:=1.0;
        Simplex.Divisor:=1.0;
        Simplex.Count:=1;
       end else begin
        baxca:=Vector3Cross(ba,ca);
        daxba:=Vector3Cross(da,ba);
        wABC:=Vector3Dot(Vector3Cross(a,b),baxca);
        vADB:=Vector3Dot(Vector3Cross(b,a),daxba);
        if (wABC<=0.0) and (vADB<=0.0) and (uAB>0.0) and (vAB>0.0) then begin
         // Region AB
         Simplex.Vertices[0]^.a:=uAB;
         Simplex.Vertices[1]^.a:=vAB;
         Simplex.Divisor:=uAB+vAB;
         Simplex.Count:=2;
        end else begin
         bcxdc:=Vector3Cross(bc,dc);
         uABC:=Vector3Dot(Vector3Cross(b,c),baxca);
         wCBD:=Vector3Dot(Vector3Cross(c,b),bcxdc);
         if (uABC<=0.0) and (wCBD<=0.0) and (uBC>0.0) and (vBC>0.0) then begin
          // Region BC
          TempVertex:=Simplex.Vertices[0];
          Simplex.Vertices[0]:=Simplex.Vertices[1];
          Simplex.Vertices[1]:=Simplex.Vertices[2];
          Simplex.Vertices[2]:=TempVertex;
          Simplex.Vertices[0]^.a:=uBC;
          Simplex.Vertices[1]^.a:=vBC;
          Simplex.Divisor:=uBC+vBC;
          Simplex.Count:=2;
         end else begin
          caxda:=Vector3Cross(ca,da);
          vABC:=Vector3Dot(Vector3Cross(c,a),baxca);
          wACD:=Vector3Dot(Vector3Cross(a,c),caxda);
          if (vABC<=0.0) and (wACD<=0.0) and (uCA>0.0) and (vCA>0.0) then begin
           // Region CA
           TempVertex:=Simplex.Vertices[1];
           Simplex.Vertices[1]:=Simplex.Vertices[0];
           Simplex.Vertices[0]:=Simplex.Vertices[2];
           Simplex.Vertices[2]:=TempVertex;
           Simplex.Vertices[0]^.a:=uCA;
           Simplex.Vertices[1]^.a:=vCA;
           Simplex.Divisor:=uCA+vCA;
           Simplex.Count:=2;
          end else begin
           vCBD:=Vector3Dot(Vector3Cross(d,c),bcxdc);
           uACD:=Vector3Dot(Vector3Cross(c,d),caxda);
           if (vCBD<=0.0) and (uACD<=0.0) and (uDC>0.0) and (vDC>0.0) then begin
            // Region DC
            TempVertex:=Simplex.Vertices[0];
            Simplex.Vertices[0]:=Simplex.Vertices[3];
            Simplex.Vertices[3]:=TempVertex;
            TempVertex:=Simplex.Vertices[1];
            Simplex.Vertices[1]:=Simplex.Vertices[2];
            Simplex.Vertices[2]:=TempVertex;
            Simplex.Vertices[0]^.a:=uDC;
            Simplex.Vertices[1]^.a:=vDC;
            Simplex.Divisor:=uDC+vDC;
            Simplex.Count:=2;
           end else begin
            vACD:=Vector3Dot(Vector3Cross(d,a),caxda);
            wADB:=Vector3Dot(Vector3Cross(a,d),daxba);
            if (vACD<=0.0) and (wADB<=0.0) and (uAD>0.0) and (vAD>0.0) then begin
             // Region AD
             TempVertex:=Simplex.Vertices[1];
             Simplex.Vertices[1]:=Simplex.Vertices[3];
             Simplex.Vertices[3]:=TempVertex;
             Simplex.Vertices[0]^.a:=uAD;
             Simplex.Vertices[1]^.a:=vAD;
             Simplex.Divisor:=uAD+vAD;
             Simplex.Count:=2;
            end else begin
             uADB:=Vector3Dot(Vector3Cross(d,b),daxba);
             uCBD:=Vector3Dot(Vector3Cross(b,d),bcxdc);
             if (uCBD<=0.0) and (uADB<=0.0) and (uBD>0.0) and (vBD>0.0) then begin
              // Region BD
              TempVertex:=Simplex.Vertices[0];
              Simplex.Vertices[0]:=Simplex.Vertices[1];
              Simplex.Vertices[1]:=Simplex.Vertices[3];
              Simplex.Vertices[3]:=TempVertex;
              Simplex.Vertices[0]^.a:=uBD;
              Simplex.Vertices[1]^.a:=vBD;
              Simplex.Divisor:=uBD+vBD;
              Simplex.Count:=2;
             end else begin
              Denominator:=Vector3Dot(cb,Vector3Cross(ab,db));
              if abs(Denominator)<EPSILON then begin
               Denominator:=1.0;
              end else begin
               Denominator:=1.0/Denominator;
              end;
              xABCD:=Vector3Dot(b,Vector3Cross(a,c))*Denominator;
              if (xABCD<=0.0) and (uABC>0.0) and (vABC>0.0) and (wABC>0.0) then begin
               // Region ABC
               Simplex.Vertices[0]^.a:=uABC;
               Simplex.Vertices[1]^.a:=vABC;
               Simplex.Vertices[2]^.a:=wABC;
               Simplex.Divisor:=uABC+vABC+wABC;
               Simplex.Count:=3;
              end else begin
               uABCD:=Vector3Dot(c,Vector3Cross(d,b))*Denominator;
               if (uABCD<=0.0) and (uCBD>0.0) and (vCBD>0.0) and (vCBD>0.0) then begin
                // Region CBD
                TempVertex:=Simplex.Vertices[0];
                Simplex.Vertices[0]:=Simplex.Vertices[2];
                Simplex.Vertices[2]:=Simplex.Vertices[3];
                Simplex.Vertices[3]:=TempVertex;
                Simplex.Vertices[0]^.a:=uCBD;
                Simplex.Vertices[1]^.a:=vCBD;
                Simplex.Vertices[2]^.a:=wCBD;
                Simplex.Divisor:=uCBD+vCBD+wCBD;
                Simplex.Count:=3;
               end else begin
                vABCD:=Vector3Dot(c,Vector3Cross(a,d))*Denominator;
                if (vABCD<=0.0) and (uACD>0.0) and (vACD>0.0) and (wACD>0.0) then begin
                 // Region ACD
                 TempVertex:=Simplex.Vertices[1];
                 Simplex.Vertices[1]:=Simplex.Vertices[2];
                 Simplex.Vertices[2]:=Simplex.Vertices[3];
                 Simplex.Vertices[3]:=TempVertex;
                 Simplex.Vertices[0]^.a:=uACD;
                 Simplex.Vertices[1]^.a:=vACD;
                 Simplex.Vertices[2]^.a:=wACD;
                 Simplex.Divisor:=uACD+vACD+wACD;
                 Simplex.Count:=3;
                end else begin
                 wABCD:=Vector3Dot(d,Vector3Cross(a,b))*Denominator;
                 if (wABCD<=0.0) and (uADB>0.0) and (vADB>0.0) and (wADB>0.0) then begin
                  // Region ADB
                  TempVertex:=Simplex.Vertices[2];
                  Simplex.Vertices[2]:=Simplex.Vertices[1];
                  Simplex.Vertices[1]:=Simplex.Vertices[3];
                  Simplex.Vertices[3]:=TempVertex;
                  Simplex.Vertices[0]^.a:=uADB;
                  Simplex.Vertices[1]^.a:=vADB;
                  Simplex.Vertices[2]^.a:=wADB;
                  Simplex.Divisor:=uADB+vADB+wADB;
                  Simplex.Count:=3;
                 end else begin
                  if (uABCD>0.0) and (vABCD>0.0) and (wABCD>0.0) and (xABCD>0.0) then begin
                   // Region ABCD
                   Simplex.Vertices[0]^.a:=uABCD;
                   Simplex.Vertices[1]^.a:=vABCD;
                   Simplex.Vertices[2]^.a:=wABCD;
                   Simplex.Vertices[3]^.a:=xABCD;
                   Simplex.Divisor:=uABCD+vABCD+wABCD+xABCD;
                   Simplex.Count:=4;
                   // Tetrahedron simplex contained the origin, so we can break the loop here
                   break;
                  end else begin
                   // The algorithm was unable to determine the subset, return the last best known subset
                   Simplex.Vertices[0]^.a:=uABC;
                   Simplex.Vertices[1]^.a:=vABC;
                   Simplex.Vertices[2]^.a:=wABC;
                   Simplex.Divisor:=uABC+vABC+wABC;
                   Simplex.Count:=3;
                  end;
                 end;
                end;
               end;
              end;
             end;
            end;
           end;
          end;
         end;
        end;
       end;
      end;
     end;
    end;
   end;
   else begin
    Assert(false);
   end;
  end;

  Assert(((Simplex.Vertices[0]<>Simplex.Vertices[1]) and (Simplex.Vertices[0]<>Simplex.Vertices[2]) and (Simplex.Vertices[0]<>Simplex.Vertices[3])) and ((Simplex.Vertices[1]<>Simplex.Vertices[2]) and (Simplex.Vertices[1]<>Simplex.Vertices[3])) and (Simplex.Vertices[2]<>Simplex.Vertices[3]));

  // Get closest point
  Denominator:=1.0/Simplex.Divisor;
  case Simplex.Count of
   1:begin
    a:=Simplex.Vertices[0]^.w;
   end;
   2:begin
    a:=Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.w,Simplex.Vertices[0]^.a*Denominator),
                  Vector3ScalarMul(Simplex.Vertices[1]^.w,Simplex.Vertices[1]^.a*Denominator));
   end;
   3:begin
    a:=Vector3Add(Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.w,Simplex.Vertices[0]^.a*Denominator),
                             Vector3ScalarMul(Simplex.Vertices[1]^.w,Simplex.Vertices[1]^.a*Denominator)),
                             Vector3ScalarMul(Simplex.Vertices[2]^.w,Simplex.Vertices[2]^.a*Denominator));
   end;
   4:begin
    a:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.w,Simplex.Vertices[0]^.a*Denominator),
                                        Vector3ScalarMul(Simplex.Vertices[1]^.w,Simplex.Vertices[1]^.a*Denominator)),
                                        Vector3ScalarMul(Simplex.Vertices[2]^.w,Simplex.Vertices[2]^.a*Denominator)),
                                        Vector3ScalarMul(Simplex.Vertices[3]^.w,Simplex.Vertices[3]^.a*Denominator));
   end;
   else begin
    Assert(false);
    a:=Vector3Origin;
   end;
  end;

  // Ensure progress. This prevents complex multi-step cycling between simplex evolutions (this is possible in 3D, but not in 2D).
  SquaredDistances[1]:=Vector3LengthSquared(a);
  if SquaredDistances[1]>SquaredDistances[0] then begin
   break;
  end;
  SquaredDistances[0]:=SquaredDistances[1];

  // Get next direction
  case Simplex.Count of
   1:begin
    Direction:=Vector3Neg(Simplex.Vertices[0]^.w);
   end;
   2:begin
    Direction:=Vector3Sub(Simplex.Vertices[0]^.w,Simplex.Vertices[1]^.w);
    Direction:=Vector3Cross(Vector3Cross(Direction,Vector3Neg(Simplex.Vertices[0]^.w)),Direction);
   end;
   3:begin
    Direction:=Vector3Cross(Vector3Sub(Simplex.Vertices[1]^.w,Simplex.Vertices[0]^.w),Vector3Sub(Simplex.Vertices[2]^.w,Simplex.Vertices[0]^.w));
    if Vector3Dot(Direction,Simplex.Vertices[0]^.w)>0.0 then begin
     Direction:=Vector3Neg(Direction);
    end;
   end;
   else begin
    Assert(false);
    Direction:=Vector3Origin;
   end;
  end;

  if Vector3LengthSquared(Direction)<EPSILON then begin
   // The origin is probably contained by a line segment or triangle. Thus the shapes are overlapped.
   // We can't return zero here even though there may be overlap.
   // In case the simplex is a point, segment, or triangle it is difficult to determine if the origin is
   // contained in the CSO or very close to it.
   break;
  end;

  // Get new support simplex vertex indices
  iA:=Shapes[0].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Vector3Neg(Direction),Transforms[0]^));
  iB:=Shapes[1].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Direction,Transforms[1]^));

  inc(Iterations);

  // Check for duplicate support points
  Duplicate:=false;
  for Index:=0 to CountSaved-1 do begin
   if (Saved[Index,0]=iA) and (Saved[Index,1]=iB) then begin
    Duplicate:=true;
    break;
   end;
  end;
  if Duplicate then begin
   // If yes, then break the loop
   break;
  end;

  // Store the new stuff
  SimplexVertex:=Simplex.Vertices[Simplex.Count];
  SimplexVertex^.iA:=iA;
  SimplexVertex^.iB:=iB;
  SimplexVertex^.sA:=Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(iA),Transforms[0]^);
  SimplexVertex^.sB:=Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(iB),Transforms[1]^);
  SimplexVertex^.w:=Vector3Sub(SimplexVertex^.sB,SimplexVertex^.sA);
  inc(Simplex.Count);

 until Iterations=GJKMaximumIterations;

 // Write the simplex information into the cache
 if assigned(CachedSimplex) then begin
  CachedSimplex^.Count:=Simplex.Count;
  if CachedSimplex^.Count>0 then begin
   for Index:=0 to CachedSimplex^.Count-1 do begin
    CachedSimplexVertex:=@CachedSimplex^.Vertices[Index];
    SimplexVertex:=Simplex.Vertices[Index];
    CachedSimplexVertex^.iA:=SimplexVertex^.iA;
    CachedSimplexVertex^.iB:=SimplexVertex^.iB;
    CachedSimplexVertex^.a:=SimplexVertex^.a;
   end;
   case Simplex.Count of
    1:begin
     CachedSimplex^.Metric:=0.0;
    end;
    2:begin
     CachedSimplex^.Metric:=Vector3Dist(Simplex.Vertices[0]^.w,Simplex.Vertices[1]^.w);
    end;
    3:begin
     CachedSimplex^.Metric:=CalculateArea(Simplex.Vertices[0]^.w,Simplex.Vertices[1]^.w,Simplex.Vertices[2]^.w);
    end;
    4:begin
     CachedSimplex^.Metric:=CalculateVolume(Simplex.Vertices[0]^.w,Simplex.Vertices[1]^.w,Simplex.Vertices[2]^.w,Simplex.Vertices[3]^.w);
    end;
    else begin
     Assert(false);
     CachedSimplex^.Metric:=0.0;
    end;
   end;
  end;
 end;

 // Get closest points
 Denominator:=1.0/Simplex.Divisor;
 case Simplex.Count of
  1:begin
   ClosestPoints[0]:=Simplex.Vertices[0]^.sA;
   ClosestPoints[1]:=Simplex.Vertices[0]^.sB;
  end;
  2:begin
   ClosestPoints[0]:=Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.sA,Simplex.Vertices[0]^.a*Denominator),
                                Vector3ScalarMul(Simplex.Vertices[1]^.sA,Simplex.Vertices[1]^.a*Denominator));
   ClosestPoints[1]:=Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.sB,Simplex.Vertices[0]^.a*Denominator),
                                Vector3ScalarMul(Simplex.Vertices[1]^.sB,Simplex.Vertices[1]^.a*Denominator));
  end;
  3:begin
   ClosestPoints[0]:=Vector3Add(Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.sA,Simplex.Vertices[0]^.a*Denominator),
                                           Vector3ScalarMul(Simplex.Vertices[1]^.sA,Simplex.Vertices[1]^.a*Denominator)),
                                           Vector3ScalarMul(Simplex.Vertices[2]^.sA,Simplex.Vertices[2]^.a*Denominator));
   ClosestPoints[1]:=Vector3Add(Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.sB,Simplex.Vertices[0]^.a*Denominator),
                                           Vector3ScalarMul(Simplex.Vertices[1]^.sB,Simplex.Vertices[1]^.a*Denominator)),
                                           Vector3ScalarMul(Simplex.Vertices[2]^.sB,Simplex.Vertices[2]^.a*Denominator));
  end;
  4:begin
   ClosestPoints[0]:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.sA,Simplex.Vertices[0]^.a*Denominator),
                                                      Vector3ScalarMul(Simplex.Vertices[1]^.sA,Simplex.Vertices[1]^.a*Denominator)),
                                                      Vector3ScalarMul(Simplex.Vertices[2]^.sA,Simplex.Vertices[2]^.a*Denominator)),
                                                      Vector3ScalarMul(Simplex.Vertices[3]^.sA,Simplex.Vertices[3]^.a*Denominator));
   ClosestPoints[1]:=Vector3Add(Vector3Add(Vector3Add(Vector3ScalarMul(Simplex.Vertices[0]^.sB,Simplex.Vertices[0]^.a*Denominator),
                                                      Vector3ScalarMul(Simplex.Vertices[1]^.sB,Simplex.Vertices[1]^.a*Denominator)),
                                                      Vector3ScalarMul(Simplex.Vertices[2]^.sB,Simplex.Vertices[2]^.a*Denominator)),
                                                      Vector3ScalarMul(Simplex.Vertices[3]^.sB,Simplex.Vertices[3]^.a*Denominator));
  end;
  else begin
   Assert(false);
   ClosestPoints[0]:=Vector3Origin;
   ClosestPoints[1]:=Vector3Origin;
  end;
 end;

 // Get the normal direction
 Normal:=Vector3Sub(ClosestPoints[0],ClosestPoints[1]);

 // Normalize normal direction to a normalized normal vector, and get the distance at the same time
 Distance:=Vector3LengthNormalize(Normal);

 // Apply the radius stuff, if requested and needed
 if UseRadii then begin
  if (Distance>(Shapes[0].fFeatureRadius+Shapes[1].fFeatureRadius)) and (Distance>EPSILON) then begin
   Distance:=Distance-(Shapes[0].fFeatureRadius+Shapes[1].fFeatureRadius);
   ClosestPoints[0]:=Vector3Sub(ClosestPoints[0],Vector3ScalarMul(Normal,Shapes[0].fFeatureRadius));
   ClosestPoints[1]:=Vector3Add(ClosestPoints[1],Vector3ScalarMul(Normal,Shapes[1].fFeatureRadius));
  end else begin
   Distance:=0.0;
   ClosestPoints[0]:=Vector3Avg(ClosestPoints[0],ClosestPoints[1]);
   ClosestPoints[1]:=ClosestPoints[0];
  end;
 end;

 Failed:=((Simplex.Count<1) or (Simplex.Count>3)) or (Iterations=GJKMaximumIterations);

 result:=not Failed;

end;

procedure CalculateVelocity(const cA:TKraftVector3;const qA:TKraftQuaternion;const cB:TKraftVector3;const qB:TKraftQuaternion;const DeltaTime:TKraftScalar;out LinearVelocity,AngularVelocity:TKraftVector3); {$ifdef caninline}inline;{$endif}
var InverseDeltaTime,Angle:TKraftScalar;
    Axis:TKraftVector3;
    qD,qS,qB0:TKraftQuaternion;
begin
 InverseDeltaTime:=1.0/DeltaTime;
 LinearVelocity:=Vector3ScalarMul(Vector3Sub(cB,cA),InverseDeltaTime);
 if (abs(qA.x-qB.x)<EPSILON) and (abs(qA.y-qB.y)<EPSILON) and (abs(qA.z-qB.z)<EPSILON) and (abs(qA.w-qB.w)<EPSILON) then begin
  AngularVelocity:=Vector3Origin;
 end else begin
  qD:=QuaternionSub(qA,qB);
  qS:=QuaternionAdd(qA,qB);
  if QuaternionLengthSquared(qD)<QuaternionLengthSquared(qS) then begin
   qB0:=qD;
  end else begin
   qB0:=QuaternionNeg(qD);
  end;
  qD:=QuaternionMul(qB0,QuaternionInverse(qA));
  QuaternionToAxisAngle(qD,Axis,Angle);
  AngularVelocity:=Vector3ScalarMul(Vector3SafeNorm(Axis),Angle*InverseDeltaTime);
 end;
end;

constructor TKraftVector3Property.Create(AVector:PKraftVector3);
begin
 inherited Create;
 fVector:=AVector;
end;

destructor TKraftVector3Property.Destroy;
begin
 inherited Destroy;
end;

function TKraftVector3Property.GetX:TKraftScalar;
begin
 result:=fVector^.x;
end;

function TKraftVector3Property.GetY:TKraftScalar;
begin
 result:=fVector^.y;
end;

function TKraftVector3Property.GetZ:TKraftScalar;
begin
 result:=fVector^.z;
end;

function TKraftVector3Property.GetVector:TKraftVector3;
begin
 result:=fVector^;
end;

procedure TKraftVector3Property.SetX(const NewValue:TKraftScalar);
begin
 fVector^.x:=NewValue;
end;

procedure TKraftVector3Property.SetY(const NewValue:TKraftScalar);
begin
 fVector^.y:=NewValue;
end;

procedure TKraftVector3Property.SetZ(const NewValue:TKraftScalar);
begin
 fVector^.z:=NewValue;
end;

procedure TKraftVector3Property.SetVector(const NewVector:TKraftVector3);
begin
 fVector^:=NewVector;
end;

constructor TKraftDynamicAABBTree.Create;
var i:longint;
begin
 inherited Create;
 fRoot:=daabbtNULLNODE;
 fNodeCount:=0;
 fNodeCapacity:=16;
 GetMem(fNodes,fNodeCapacity*SizeOf(TKraftDynamicAABBTreeNode));
 FillChar(fNodes^,fNodeCapacity*SizeOf(TKraftDynamicAABBTreeNode),#0);
 for i:=0 to fNodeCapacity-2 do begin
  fNodes^[i].Next:=i+1;
  fNodes^[i].Height:=-1;
 end;
 fNodes^[fNodeCapacity-1].Next:=daabbtNULLNODE;
 fNodes^[fNodeCapacity-1].Height:=-1;
 fFreeList:=0;
 fPath:=0;
 fInsertionCount:=0;
 fStackCapacity:=16;
 GetMem(fStack,fStackCapacity*SizeOf(longint));
end;

destructor TKraftDynamicAABBTree.Destroy;
begin
 FreeMem(fNodes);
 FreeMem(fStack);
 inherited Destroy;
end;

function TKraftDynamicAABBTree.AllocateNode:longint;
var Node:PKraftDynamicAABBTreeNode;
    i:longint;
begin
 if fFreeList=daabbtNULLNODE then begin
  inc(fNodeCapacity,fNodeCapacity);
  ReallocMem(fNodes,fNodeCapacity*SizeOf(TKraftDynamicAABBTreeNode));
  FillChar(fNodes^[fNodeCount],(fNodeCapacity-fNodeCount)*SizeOf(TKraftDynamicAABBTreeNode),#0);
  for i:=fNodeCount to fNodeCapacity-2 do begin
   fNodes^[i].Next:=i+1;
   fNodes^[i].Height:=-1;
  end;
  fNodes^[fNodeCapacity-1].Next:=daabbtNULLNODE;
  fNodes^[fNodeCapacity-1].Height:=-1;
  fFreeList:=fNodeCount;
 end;
 result:=fFreeList;
 fFreeList:=fNodes^[result].Next;
 Node:=@fNodes^[result];
 Node^.Parent:=daabbtNULLNODE;
 Node^.Children[0]:=daabbtNULLNODE;
 Node^.Children[1]:=daabbtNULLNODE;
 Node^.Height:=0;
 Node^.UserData:=nil;
 inc(fNodeCount);
end;

procedure TKraftDynamicAABBTree.FreeNode(NodeID:longint);
var Node:PKraftDynamicAABBTreeNode;
begin
 Node:=@fNodes^[NodeID];
 Node^.Next:=fFreeList;
 Node^.Height:=-1;
 fFreeList:=NodeID;
 dec(fNodeCount);
end;

function TKraftDynamicAABBTree.Balance(NodeAID:longint):longint;
var NodeA,NodeB,NodeC,NodeD,NodeE,NodeF,NodeG:PKraftDynamicAABBTreeNode;
    NodeBID,NodeCID,NodeDID,NodeEID,NodeFID,NodeGID,NodeBalance:longint;
begin
 NodeA:=@fNodes^[NodeAID];
 if (NodeA.Children[0]<0) or (NodeA^.Height<2) then begin
  result:=NodeAID;
 end else begin
  NodeBID:=NodeA^.Children[0];
  NodeCID:=NodeA^.Children[1];
  NodeB:=@fNodes^[NodeBID];
  NodeC:=@fNodes^[NodeCID];
  NodeBalance:=NodeC^.Height-NodeB^.Height;
  if NodeBalance>1 then begin
   NodeFID:=NodeC^.Children[0];
   NodeGID:=NodeC^.Children[1];
   NodeF:=@fNodes^[NodeFID];
   NodeG:=@fNodes^[NodeGID];
   NodeC^.Children[0]:=NodeAID;
   NodeC^.Parent:=NodeA^.Parent;
   NodeA^.Parent:=NodeCID;
   if NodeC^.Parent>=0 then begin
    if fNodes^[NodeC^.Parent].Children[0]=NodeAID then begin
     fNodes^[NodeC^.Parent].Children[0]:=NodeCID;
    end else begin
     fNodes^[NodeC^.Parent].Children[1]:=NodeCID;
    end;
   end else begin
    fRoot:=NodeCID;
   end;
   if NodeF^.Height>NodeG^.Height then begin
    NodeC^.Children[1]:=NodeFID;
    NodeA^.Children[1]:=NodeGID;
    NodeG^.Parent:=NodeAID;
    NodeA^.AABB:=AABBCombine(NodeB^.AABB,NodeG^.AABB);
    NodeC^.AABB:=AABBCombine(NodeA^.AABB,NodeF^.AABB);
    NodeA^.Height:=1+Max(NodeB^.Height,NodeG^.Height);
    NodeC^.Height:=1+Max(NodeA^.Height,NodeF^.Height);
   end else begin
    NodeC^.Children[1]:=NodeGID;
    NodeA^.Children[1]:=NodeFID;
    NodeF^.Parent:=NodeAID;
    NodeA^.AABB:=AABBCombine(NodeB^.AABB,NodeF^.AABB);
    NodeC^.AABB:=AABBCombine(NodeA^.AABB,NodeG^.AABB);
    NodeA^.Height:=1+Max(NodeB^.Height,NodeF^.Height);
    NodeC^.Height:=1+Max(NodeA^.Height,NodeG^.Height);
   end;
   result:=NodeCID;
  end else if NodeBalance<-1 then begin
   NodeDID:=NodeB^.Children[0];
   NodeEID:=NodeB^.Children[1];
   NodeD:=@fNodes^[NodeDID];
   NodeE:=@fNodes^[NodeEID];
   NodeB^.Children[0]:=NodeAID;
   NodeB^.Parent:=NodeA^.Parent;
   NodeA^.Parent:=NodeBID;
   if NodeB^.Parent>=0 then begin
    if fNodes^[NodeB^.Parent].Children[0]=NodeAID then begin
     fNodes^[NodeB^.Parent].Children[0]:=NodeBID;
    end else begin
     fNodes^[NodeB^.Parent].Children[1]:=NodeBID;
    end;
   end else begin
    fRoot:=NodeBID;
   end;
   if NodeD^.Height>NodeE^.Height then begin
    NodeB^.Children[1]:=NodeDID;
    NodeA^.Children[0]:=NodeEID;
    NodeE^.Parent:=NodeAID;
    NodeA^.AABB:=AABBCombine(NodeC^.AABB,NodeE^.AABB);
    NodeB^.AABB:=AABBCombine(NodeA^.AABB,NodeD^.AABB);
    NodeA^.Height:=1+Max(NodeC^.Height,NodeE^.Height);
    NodeB^.Height:=1+Max(NodeA^.Height,NodeD^.Height);
   end else begin
    NodeB^.Children[1]:=NodeEID;
    NodeA^.Children[0]:=NodeDID;
    NodeD^.Parent:=NodeAID;
    NodeA^.AABB:=AABBCombine(NodeC^.AABB,NodeD^.AABB);
    NodeB^.AABB:=AABBCombine(NodeA^.AABB,NodeE^.AABB);
    NodeA^.Height:=1+Max(NodeC^.Height,NodeD^.Height);
    NodeB^.Height:=1+Max(NodeA^.Height,NodeE^.Height);
   end;
   result:=NodeBID;
  end else begin
   result:=NodeAID;
  end;
 end;
end;

procedure TKraftDynamicAABBTree.InsertLeaf(Leaf:longint);
var Node:PKraftDynamicAABBTreeNode;
    LeafAABB,CombinedAABB,AABB:TKraftAABB;
    Index,Sibling,OldParent,NewParent:longint;
    Children:array[0..1] of longint;
    CombinedCost,Cost,InheritanceCost:TKraftScalar;
    Costs:array[0..1] of TKraftScalar;
begin
 inc(fInsertionCount);
 if fRoot<0 then begin
  fRoot:=Leaf;
  fNodes^[Leaf].Parent:=daabbtNULLNODE;
 end else begin
  LeafAABB:=fNodes^[Leaf].AABB;
  Index:=fRoot;
  while fNodes^[Index].Children[0]>=0 do begin
   Children[0]:=fNodes^[Index].Children[0];
   Children[1]:=fNodes^[Index].Children[1];

   CombinedAABB:=AABBCombine(fNodes^[Index].AABB,LeafAABB);
   CombinedCost:=AABBCost(CombinedAABB);
   Cost:=CombinedCost*2.0;
   InheritanceCost:=2.0*(CombinedCost-AABBCost(fNodes^[Index].AABB));

   AABB:=AABBCombine(LeafAABB,fNodes^[Children[0]].AABB);
   if fNodes^[Children[0]].Children[0]<0 then begin
    Costs[0]:=AABBCost(AABB)+InheritanceCost;
   end else begin
    Costs[0]:=(AABBCost(AABB)-AABBCost(fNodes^[Children[0]].AABB))+InheritanceCost;
   end;

   AABB:=AABBCombine(LeafAABB,fNodes^[Children[1]].AABB);
   if fNodes^[Children[1]].Children[1]<0 then begin
    Costs[1]:=AABBCost(AABB)+InheritanceCost;
   end else begin
    Costs[1]:=(AABBCost(AABB)-AABBCost(fNodes^[Children[1]].AABB))+InheritanceCost;
   end;

   if (Cost<Costs[0]) and (Cost<Costs[1]) then begin
    break;
   end else begin
    if Costs[0]<Costs[1] then begin
     Index:=Children[0];
    end else begin
     Index:=Children[1];
    end;
   end;

  end;

  Sibling:=Index;

  OldParent:=fNodes^[Sibling].Parent;
  NewParent:=AllocateNode;
  fNodes^[NewParent].Parent:=OldParent;
  fNodes^[NewParent].UserData:=nil;
  fNodes^[NewParent].AABB:=AABBCombine(LeafAABB,fNodes^[Sibling].AABB);
  fNodes^[NewParent].Height:=fNodes^[Sibling].Height+1;

  if OldParent>=0 then begin
   if fNodes^[OldParent].Children[0]=Sibling then begin
    fNodes^[OldParent].Children[0]:=NewParent;
   end else begin
    fNodes^[OldParent].Children[1]:=NewParent;
   end;
   fNodes^[NewParent].Children[0]:=Sibling;
   fNodes^[NewParent].Children[1]:=Leaf;
   fNodes^[Sibling].Parent:=NewParent;
   fNodes^[Leaf].Parent:=NewParent;
  end else begin
   fNodes^[NewParent].Children[0]:=Sibling;
   fNodes^[NewParent].Children[1]:=Leaf;
   fNodes^[Sibling].Parent:=NewParent;
   fNodes^[Leaf].Parent:=NewParent;
   fRoot:=NewParent;
  end;

  Index:=fNodes^[Leaf].Parent;
  while Index>=0 do begin
   Index:=Balance(Index);
   Node:=@fNodes^[Index];
   Node^.AABB:=AABBCombine(fNodes^[Node^.Children[0]].AABB,fNodes^[Node^.Children[1]].AABB);
   Node^.Height:=1+Max(fNodes^[Node^.Children[0]].Height,fNodes^[Node^.Children[1]].Height);
   Index:=Node^.Parent;
  end;

 end;
end;

procedure TKraftDynamicAABBTree.RemoveLeaf(Leaf:longint);
var Node:PKraftDynamicAABBTreeNode;
    Parent,GrandParent,Sibling,Index:longint;
begin
 if fRoot=Leaf then begin
  fRoot:=daabbtNULLNODE;
 end else begin
  Parent:=fNodes^[Leaf].Parent;
  GrandParent:=fNodes^[Parent].Parent;
  if fNodes^[Parent].Children[0]=Leaf then begin
   Sibling:=fNodes^[Parent].Children[1];
  end else begin
   Sibling:=fNodes^[Parent].Children[0];
  end;
  if GrandParent>=0 then begin
   if fNodes^[GrandParent].Children[0]=Parent then begin
    fNodes^[GrandParent].Children[0]:=Sibling;
   end else begin
    fNodes^[GrandParent].Children[1]:=Sibling;
   end;
   fNodes^[Sibling].Parent:=GrandParent;
   FreeNode(Parent);
   Index:=GrandParent;
   while Index>=0 do begin
    Index:=Balance(Index);
    Node:=@fNodes^[Index];
    Node^.AABB:=AABBCombine(fNodes^[Node^.Children[0]].AABB,fNodes^[Node^.Children[1]].AABB);
    Node^.Height:=1+Max(fNodes^[Node^.Children[0]].Height,fNodes^[Node^.Children[1]].Height);
    Index:=Node^.Parent;
   end;
  end else begin
   fRoot:=Sibling;
   fNodes^[Sibling].Parent:=daabbtNULLNODE;
   FreeNode(Parent);
  end;
 end;
end;

function TKraftDynamicAABBTree.CreateProxy(const AABB:TKraftAABB;UserData:pointer):longint;
var Node:PKraftDynamicAABBTreeNode;
begin
 result:=AllocateNode;
 Node:=@fNodes^[result];
 Node^.AABB.Min:=Vector3Sub(AABB.Min,AABBExtensionVector);
 Node^.AABB.Max:=Vector3Add(AABB.Max,AABBExtensionVector);
 Node^.UserData:=UserData;
 Node^.Height:=0;
 InsertLeaf(result);
end;

procedure TKraftDynamicAABBTree.DestroyProxy(NodeID:longint);
begin
 RemoveLeaf(NodeID);
 FreeNode(NodeID);
end;

function TKraftDynamicAABBTree.MoveProxy(NodeID:longint;const AABB:TKraftAABB;const Displacement,BoundsExpansion:TKraftVector3):boolean;
var Node:PKraftDynamicAABBTreeNode;
begin
 Node:=@fNodes^[NodeID];
 result:=not AABBContains(Node^.AABB,AABB);
 if result then begin
  RemoveLeaf(NodeID);
  Node^.AABB:=AABBStretch(AABB,Displacement,BoundsExpansion);
  InsertLeaf(NodeID);
 end;
end;

procedure TKraftDynamicAABBTree.Rebalance(Iterations:longint);
var Counter,Node:longint;
    Bit:longword;
//  Children:PKraftDynamicAABBTreeLongintArray;
begin
 if (fRoot>=0) and (fRoot<fNodeCount) then begin
  for Counter:=1 to Iterations do begin
   Bit:=0;
   Node:=fRoot;
   while fNodes[Node].Children[0]>=0 do begin
    Node:=fNodes[Node].Children[(fPath shr Bit) and 1];
    Bit:=(Bit+1) and 31;
   end;
   inc(fPath);
   if ((Node>=0) and (Node<fNodeCount)) and (fNodes[Node].Children[0]<0) then begin
    RemoveLeaf(Node);
    InsertLeaf(Node);
   end else begin
    break;
   end;
  end;
 end;
end;

procedure TKraftDynamicAABBTree.Rebuild;
var NewNodes:PKraftDynamicAABBTreeLongintArray;
    Children:array[0..1] of PKraftDynamicAABBTreeNode;
    Parent:PKraftDynamicAABBTreeNode;
    Count,i,j,iMin,jMin,Index1,Index2,ParentIndex:longint;
    MinCost,Cost:TKraftScalar;
    AABBi,AABBj:PKraftAABB;
    AABB:TKraftAABB;
    First:boolean;
begin
 if fNodeCount>0 then begin
  NewNodes:=nil;
  GetMem(NewNodes,fNodeCount*SizeOf(longint));
  FillChar(NewNodes^,fNodeCount*SizeOf(longint),#0);
  Count:=0;
  for i:=0 to fNodeCapacity-1 do begin
   if fNodes^[i].Height>=0 then begin
    if fNodes^[i].Children[0]<0 then begin
     fNodes^[i].Parent:=daabbtNULLNODE;
     NewNodes^[Count]:=i;
     inc(Count);
    end else begin
     FreeNode(i);
    end;
   end;
  end;
  while Count>1 do begin
   First:=true;
   MinCost:=MAX_SCALAR;
   iMin:=-1;
   jMin:=-1;
 {}/////////////////TOOPTIMIZE///////////////////
 {}for i:=0 to Count-1 do begin                //
 {} AABBi:=@fNodes^[NewNodes^[i]].AABB;        //
 {} for j:=i+1 to Count-1 do begin             //
 {}  AABBj:=@fNodes^[NewNodes^[j]].AABB;       //
 {}  AABB:=AABBCombine(AABBi^,AABBj^);         //
 {}  Cost:=AABBCost(AABB);                     //
 {}  if First or (Cost<MinCost) then begin     //
 {}   First:=false;                            //
 {}   MinCost:=Cost;                           //
 {}   iMin:=i;                                 //
 {}   jMin:=j;                                 //
 {}  end;                                      //
 {} end;                                       //
 {}end;                                        //
 {}/////////////////TOOPTIMIZE///////////////////
   Index1:=NewNodes^[iMin];
   Index2:=NewNodes^[jMin];
   Children[0]:=@fNodes^[Index1];
   Children[1]:=@fNodes^[Index2];
   ParentIndex:=AllocateNode;
   Parent:=@fNodes^[ParentIndex];
   Parent^.Children[0]:=Index1;
   Parent^.Children[1]:=Index2;
   Parent^.Height:=1+Max(Children[0]^.Height,Children[1]^.Height);
   Parent^.AABB:=AABBCombine(Children[0]^.AABB,Children[1]^.AABB);
   Parent^.Parent:=daabbtNULLNODE;
   Children[0]^.Parent:=ParentIndex;
   Children[1]^.Parent:=ParentIndex;
   NewNodes^[jMin]:=NewNodes^[Count-1];
   NewNodes^[iMin]:=ParentIndex;
   dec(Count);
  end;
  fRoot:=NewNodes^[0];
  FreeMem(NewNodes);
 end;
end;

function TKraftDynamicAABBTree.ComputeHeight:longint;
{$ifdef KraftSingleThreadedUsage}
var LocalStack:PKraftDynamicAABBTreeLongintArray;
    LocalStackPointer,NodeID,Height:longint;
    Node:PKraftDynamicAABBTreeNode;
begin
 result:=0;
 if fRoot>=0 then begin
  LocalStack:=fStack;
  LocalStack^[0]:=fRoot;
  LocalStack^[1]:=1;
  LocalStackPointer:=2;
  while LocalStackPointer>0 do begin
   dec(LocalStackPointer,2);
   NodeID:=LocalStack^[LocalStackPointer];
   Height:=LocalStack^[LocalStackPointer+1];
   if result<Height then begin
    result:=Height;
   end;
   if NodeID>=0 then begin
    Node:=@fNodes^[NodeID];
    if Node^.Children[0]>=0 then begin
     if fStackCapacity<=(LocalStackPointer+4) then begin
      fStackCapacity:=RoundUpToPowerOfTwo(LocalStackPointer+4);
      ReallocMem(fStack,fStackCapacity*SizeOf(longint));
      LocalStack:=fStack;
     end;
     LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
     LocalStack^[LocalStackPointer+1]:=Height+1;
     LocalStack^[LocalStackPointer+2]:=Node^.Children[1];
     LocalStack^[LocalStackPointer+3]:=Height+1;
     inc(LocalStackPointer,4);
    end;
   end;
  end;
 end;
end;
{$else}
var MaximalHeight:longint;
 procedure ProcessNode(const NodeID,Height:longint);
 var Node:PKraftDynamicAABBTreeNode;
 begin
  if result<Height then begin
   result:=Height;
  end;
  if NodeID>=0 then begin
   Node:=@fNodes^[NodeID];
   ProcessNode(Node^.Children[0],Height+1);
   ProcessNode(Node^.Children[1],Height+1);
  end;
 end;
begin
 MaximalHeight:=0;
 ProcessNode(fRoot,1);
 result:=MaximalHeight;
end;
{$endif}

function TKraftDynamicAABBTree.GetHeight:longint;
begin
 if fRoot>=0 then begin
  result:=fNodes[fRoot].Height;
 end else begin
  result:=0;
 end;
end;

function TKraftDynamicAABBTree.GetAreaRatio:TKraftScalar;
var NodeID:longint;
    Node:PKraftDynamicAABBTreeNode;
begin
 result:=0;
 if fRoot>=0 then begin
  for NodeID:=0 to fNodeCount-1 do begin
   Node:=@fNodes[NodeID];
   if Node^.Height>=0 then begin
    result:=result+AABBCost(Node^.AABB);
   end;
  end;
  result:=result/AABBCost(fNodes[fRoot].AABB);
 end;
end;

function TKraftDynamicAABBTree.GetMaxBalance:longint;
var NodeID,Balance:longint;
    Node:PKraftDynamicAABBTreeNode;
begin
 result:=0;
 for NodeID:=0 to fNodeCount-1 do begin
  Node:=@fNodes[NodeID];
  if (Node^.Height>1) and (Node^.Children[0]>=0) then begin
   Balance:=abs(fNodes[Node^.Children[1]].Height-fNodes[Node^.Children[0]].Height);
   if result<Balance then begin
    result:=Balance;
   end;
  end;
 end;
end;

function TKraftDynamicAABBTree.ValidateStructure:boolean;
{$ifdef KraftSingleThreadedUsage}
var LocalStack:PKraftDynamicAABBTreeLongintArray;
    LocalStackPointer,NodeID,Parent:longint;
    Node:PKraftDynamicAABBTreeNode;
begin
 result:=true;
 if fRoot>=0 then begin
  LocalStack:=fStack;
  LocalStack^[0]:=fRoot;
  LocalStack^[1]:=-1;
  LocalStackPointer:=2;
  while LocalStackPointer>0 do begin
   dec(LocalStackPointer,2);
   NodeID:=LocalStack^[LocalStackPointer];
   Parent:=LocalStack^[LocalStackPointer+1];
   if (NodeID>=0) and (NodeID<fNodeCount) then begin
    Node:=@fNodes^[NodeID];
    if Node^.Parent<>Parent then begin
     result:=false;
     break;
    end;
    if Node^.Children[0]<0 then begin
     if (Node^.Children[1]>=0) or (Node^.Height<>0) then begin
      result:=false;
      break;
     end;
    end else begin
     if fStackCapacity<=(LocalStackPointer+4) then begin
      fStackCapacity:=RoundUpToPowerOfTwo(LocalStackPointer+4);
      ReallocMem(fStack,fStackCapacity*SizeOf(longint));
      LocalStack:=fStack;
     end;
     LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
     LocalStack^[LocalStackPointer+1]:=NodeID;
     LocalStack^[LocalStackPointer+2]:=Node^.Children[1];
     LocalStack^[LocalStackPointer+3]:=NodeID;
     inc(LocalStackPointer,4);
    end;
   end else begin
    result:=false;
    break;
   end;
  end;
 end;
end;
{$else}
var OK:boolean;
 procedure ProcessNode(const NodeID,Parent:longint);
 var Node:PKraftDynamicAABBTreeNode;
 begin
  if (NodeID>=0) and (NodeID<fNodeCount) and OK then begin
   Node:=@fNodes^[NodeID];
   if Node^.Parent<>Parent then begin
    OK:=false;
   end else begin
    ProcessNode(Node^.Children[0],NodeID);
    ProcessNode(Node^.Children[1],NodeID);
   end;
  end;
 end;
begin
 OK:=true;
 ProcessNode(fRoot,-1);
 result:=OK;
end;
{$endif}

function TKraftDynamicAABBTree.ValidateMetrics:boolean;
{$ifdef KraftSingleThreadedUsage}
var LocalStack:PKraftDynamicAABBTreeLongintArray;
    LocalStackPointer,NodeID{,Height}:longint;
    Node:PKraftDynamicAABBTreeNode;
    AABB:TKraftAABB;
begin
 result:=true;
 if fRoot>=0 then begin
  LocalStack:=fStack;
  LocalStack^[0]:=fRoot;
  LocalStackPointer:=1;
  while LocalStackPointer>0 do begin
   dec(LocalStackPointer);
   NodeID:=LocalStack^[LocalStackPointer];
   if (NodeID>=0) and (NodeID<fNodeCount) then begin
    Node:=@fNodes^[NodeID];
    if Node^.Children[0]>=0 then begin
     if (((Node^.Children[0]<0) or (Node^.Children[0]>=fNodeCount)) or
         ((Node^.Children[1]<0) or (Node^.Children[1]>=fNodeCount))) or
        (Node^.Height<>(1+Max(fNodes[Node^.Children[0]].Height,fNodes[Node^.Children[1]].Height))) then begin
      result:=false;
      break;
     end;
     AABB:=AABBCombine(fNodes[Node^.Children[0]].AABB,fNodes[Node^.Children[1]].AABB);
     if not (Vector3Compare(Node^.AABB.Min,AABB.Min) and Vector3Compare(Node^.AABB.Max,AABB.Max)) then begin
      result:=false;
      break;
     end;
     if fStackCapacity<=(LocalStackPointer+2) then begin
      fStackCapacity:=RoundUpToPowerOfTwo(LocalStackPointer+2);
      ReallocMem(fStack,fStackCapacity*SizeOf(longint));
      LocalStack:=fStack;
     end;
     LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
     LocalStack^[LocalStackPointer+1]:=Node^.Children[1];
     inc(LocalStackPointer,2);
    end;
   end else begin
    result:=false;
    break;
   end;
  end;
 end;
end;
{$else}
var OK:boolean;
 procedure ProcessNode(const NodeID:longint);
 var Node:PKraftDynamicAABBTreeNode;
 begin
  if (NodeID>=0) and OK then begin
   Node:=@fNodes^[NodeID];
   if (((Node^.Children[0]<0) or (Node^.Children[0]>=fNodeCount)) or
       ((Node^.Children[1]<0) or (Node^.Children[1]>=fNodeCount))) or
      (Node^.Height<>(1+Max(fNodes[Node^.Children[0]].Height,fNodes[Node^.Children[1]].Height))) then begin
    OK:=false;
   end else begin
    ProcessNode(Node^.Children[0]);
    ProcessNode(Node^.Children[1]);
   end;
  end;
 end;
begin
 OK:=true;
 ProcessNode(fRoot);
 result:=OK;
end;
{$endif}

function TKraftDynamicAABBTree.Validate:boolean;
var NodeID,FreeCount:longint;
begin
 result:=ValidateStructure;
 if result then begin
  result:=ValidateMetrics;
  if result then begin
   result:=ComputeHeight=GetHeight;
   if result then begin
    NodeID:=fFreeList;
    FreeCount:=0;
    while NodeID>=0 do begin
     NodeID:=fNodes[NodeID].Next;
     inc(FreeCount);
    end;
    result:=(fNodeCount+FreeCount)=fNodeCapacity;
   end;
  end;
 end;
end;

function TKraftDynamicAABBTree.GetIntersectionProxy(const AABB:TKraftAABB):pointer;
{$ifdef KraftSingleThreadedUsage}
var LocalStack:PKraftDynamicAABBTreeLongintArray;
    LocalStackPointer,NodeID:longint;
    Node:PKraftDynamicAABBTreeNode;
begin
 result:=nil;
 if fRoot>=0 then begin
  LocalStack:=fStack;
  LocalStack^[0]:=fRoot;
  LocalStackPointer:=1;
  while LocalStackPointer>0 do begin
   dec(LocalStackPointer);
   NodeID:=LocalStack^[LocalStackPointer];
   if NodeID>=0 then begin
    Node:=@fNodes[NodeID];
    if AABBIntersect(Node^.AABB,AABB) then begin
     if Node^.Children[0]<0 then begin
      result:=Node^.UserData;
      exit;
     end else begin
      if fStackCapacity<=(LocalStackPointer+2) then begin
       fStackCapacity:=RoundUpToPowerOfTwo(LocalStackPointer+2);
       ReallocMem(fStack,fStackCapacity*SizeOf(longint));
       LocalStack:=fStack;
      end;
      LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
      LocalStack^[LocalStackPointer+1]:=Node^.Children[1];
      inc(LocalStackPointer,2);
     end;
    end;
   end;
  end;
 end;
end;
{$else}
var Data:pointer;
    Done:boolean;
 procedure ProcessNode(const NodeID:longint);
 var Node:PKraftDynamicAABBTreeNode;
 begin
  if (NodeID>=0) and not Done then begin
   Node:=@fNodes^[NodeID];
   if AABBIntersect(Node^.AABB,AABB) then begin
    if Node^.Children[0]<0 then begin
     Data:=Node^.UserData;
     Done:=true;
    end else begin
     ProcessNode(Node^.Children[0]);
     ProcessNode(Node^.Children[1]);
    end;
   end;
  end;
 end;
begin
 Data:=nil;
 Done:=false;
 ProcessNode(fRoot);
 result:=Data;
end;
{$endif}

type PConvexHullVector=^TConvexHullVector;
     TConvexHullVector=record
      x,y,z:double;
      HashNext:longint;
     end;

     TConvexHullVectors=array of TConvexHullVector;

     PConvexHullTriangle=^TConvexHullTriangle;
     TConvexHullTriangle=array[0..2] of longint;

     TConvexHullTriangles=array of TConvexHullTriangle;

     PConvexHullPlane=^TConvexHullPlane;
     TConvexHullPlane=record
      Normal:TConvexHullVector;
      Distance:double;
     end;

     TConvexHullPlanes=array of TConvexHullPlane;

     PConvexHullPolygon=^TConvexHullPolygon;
     TConvexHullPolygon=record
      Indices:array of longint;
      Count:longint;
      Plane:TConvexHullPlane;
     end;

     TConvexHullPolygons=record
      Items:array of TConvexHullPolygon;
      Count:longint;
     end;

function CompareConvexHullPoints(const a,b:pointer):longint;
 function IsSameValue(const a,b:double):boolean;
 const FuzzFactor=1000;
       DoubleResolution=1e-15*FuzzFactor;
 var EpsilonTolerance:double;
 begin
  EpsilonTolerance:=abs(a);
  if EpsilonTolerance>abs(b) then begin
   EpsilonTolerance:=abs(b);
  end;
  EpsilonTolerance:=EpsilonTolerance*DoubleResolution;
  if EpsilonTolerance<DoubleResolution then begin
   EpsilonTolerance:=DoubleResolution;
  end;
  if a>b then begin
   result:=(a-b)<=EpsilonTolerance;
  end else begin
   result:=(b-a)<=EpsilonTolerance;
  end;
 end;
var va,vb:PConvexHullVector;
begin
 va:=a;
 vb:=b;
 if (IsSameValue(va^.x,vb^.x) and ((IsSameValue(va^.y,vb^.y) and (va^.z>vb^.z)) or (va^.y>vb^.y))) or (va^.x>vb^.x) then begin
  result:=-1;
 end else if (IsSameValue(va^.x,vb^.x) and ((IsSameValue(va^.y,vb^.y) and (va^.z<vb^.z)) or (va^.y<vb^.y))) or (va^.x<vb^.x) then begin
  result:=1;
 end else begin
  result:=0;
 end;
end;

function ConvexHullIsSameValue(const a,b:double):boolean;
const FuzzFactor=1000;
      DoubleResolution=1e-15*FuzzFactor;
var EpsilonTolerance:double;
begin
 EpsilonTolerance:=abs(a);
 if EpsilonTolerance>abs(b) then begin
  EpsilonTolerance:=abs(b);
 end;
 EpsilonTolerance:=EpsilonTolerance*DoubleResolution;
 if EpsilonTolerance<DoubleResolution then begin
  EpsilonTolerance:=DoubleResolution;
 end;
 if a>b then begin
  result:=(a-b)<=EpsilonTolerance;
 end else begin
  result:=(b-a)<=EpsilonTolerance;
 end;
end;

function ConvexHullVectorCompare(const v1,v2:TConvexHullVector):boolean;
begin
 result:=ConvexHullIsSameValue(v1.x,v2.x) and ConvexHullIsSameValue(v1.y,v2.y) and ConvexHullIsSameValue(v1.z,v2.z);
end;

function ConvexHullVectorNeg(const v:TConvexHullVector):TConvexHullVector;
begin
 result.x:=-v.x;
 result.y:=-v.y;
 result.z:=-v.z;
end;

function ConvexHullVectorSub(const v1,v2:TConvexHullVector):TConvexHullVector;
begin
 result.x:=v1.x-v2.x;
 result.y:=v1.y-v2.y;
 result.z:=v1.z-v2.z;
end;

function ConvexHullVectorAdd(const v1,v2:TConvexHullVector):TConvexHullVector;
begin
 result.x:=v1.x+v2.x;
 result.y:=v1.y+v2.y;
 result.z:=v1.z+v2.z;
end;

function ConvexHullVectorCross(const v1,v2:TConvexHullVector):TConvexHullVector;
begin
 result.x:=(v1.y*v2.z)-(v1.z*v2.y);
 result.y:=(v1.z*v2.x)-(v1.x*v2.z);
 result.z:=(v1.x*v2.y)-(v1.y*v2.x);
end;

function ConvexHullVectorScale(const v:TConvexHullVector;const Scale:double):TConvexHullVector;
begin
 result.x:=v.x*Scale;
 result.y:=v.y*Scale;
 result.z:=v.z*Scale;
end;

function ConvexHullVectorDivide(const v:TConvexHullVector;const Divisor:double):TConvexHullVector;
begin
 result.x:=v.x/Divisor;
 result.y:=v.y/Divisor;
 result.z:=v.z/Divisor;
end;

function ConvexHullVectorLengthSquared(const v:TConvexHullVector):double;
begin
 result:=sqr(v.x)+sqr(v.y)+sqr(v.z);
end;

function ConvexHullVectorLength(const v:TConvexHullVector):double;
begin
 result:=sqrt(sqr(v.x)+sqr(v.y)+sqr(v.z));
end;

function ConvexHullVectorDot(const v1,v2:TConvexHullVector):double;
begin
 result:=(v1.x*v2.x)+(v1.y*v2.y)+(v1.z*v2.z);
end;
             
function ConvexHullVectorNormalize(const v:TConvexHullVector):TConvexHullVector;
var l:double;
begin
 l:=sqr(v.x)+sqr(v.y)+sqr(v.z);
 if l>EPSILON then begin
  l:=sqrt(l);
  result.x:=v.x/l;
  result.y:=v.y/l;
  result.z:=v.z/l;
 end else begin
  result.x:=0.0;
  result.y:=1.0;
  result.z:=0.0;
 end;
end;

function ConvexHullVectorLerp(const v1,v2:TConvexHullVector;const w:double):TConvexHullVector;
var iw:double;
begin
 if w<0.0 then begin
  result:=v1;
 end else if w>1.0 then begin
  result:=v2;
 end else begin
  iw:=1.0-w;
  result.x:=(iw*v1.x)+(w*v2.x);
  result.y:=(iw*v1.y)+(w*v2.y);
  result.z:=(iw*v1.z)+(w*v2.z);
 end;
end;

procedure ConvexHullGetPlaneSpace(const n:TConvexHullVector;var p,q:TConvexHullVector); {$ifdef caninline}inline;{$endif}
var a,k:double;
begin
 if abs(n.z)>0.70710678118 then begin
  a:=sqr(n.y)+sqr(n.z);
  k:=1.0/sqrt(a);
  p.x:=0.0;
  p.y:=-(n.z*k);
  p.z:=n.y*k;
  q.x:=a*k;
  q.y:=-(n.x*p.z);
  q.z:=n.x*p.y;
 end else begin
  a:=sqr(n.x)+sqr(n.y);
  k:=1.0/sqrt(a);
  p.x:=-(n.y*k);
  p.y:=n.x*k;
  p.z:=0.0;
  q.x:=-(n.z*p.y);
  q.y:=n.z*p.x;
  q.z:=a*k;
 end;
end;

function ConvexHullCalculateArea(const v0,v1,v2:TConvexHullVector):double;
begin
 result:=ConvexHullVectorLengthSquared(ConvexHullVectorCross(ConvexHullVectorSub(v1,v0),ConvexHullVectorSub(v2,v0)));
end;

function ConvexHullCalculateVolume(const v0,v1,v2,v3:TConvexHullVector):double;
var a,b,c:TConvexHullVector;
begin
 a:=ConvexHullVectorSub(v0,v3);
 b:=ConvexHullVectorSub(v1,v3);
 c:=ConvexHullVectorSub(v2,v3);
 result:=(a.x*((b.z*c.y)-(b.y*c.z)))+(a.y*((b.x*c.z)-(b.z*c.x)))+(a.z*((b.y*c.x)-(b.x*c.y)));
end;

procedure ConvexHullComputePolygonNewellPlane(const Vertices:TConvexHullVectors;var Polygon:TConvexHullPolygon);
var VertexIndex:longint;
    Last,Current:PConvexHullVector;
    Plane:PConvexHullPlane;
    Centroid,Normal:TConvexHullVector;
begin
 if Polygon.Count>0 then begin
  Centroid.x:=0.0;
  Centroid.y:=0.0;
  Centroid.z:=0.0;
  Normal.x:=0.0;
  Normal.y:=0.0;
  Normal.z:=0.0;
  Last:=@Vertices[Polygon.Indices[Polygon.Count-1]];
  for VertexIndex:=0 to Polygon.Count-1 do begin
   Current:=@Vertices[Polygon.Indices[VertexIndex]];
   Normal.x:=Normal.x+((Last^.y-Current^.y)*(Last^.z+Current^.z));
   Normal.y:=Normal.y+((Last^.z-Current^.z)*(Last^.x+Current^.x));
   Normal.z:=Normal.z+((Last^.x-Current^.x)*(Last^.y+Current^.y));
   Centroid.x:=Centroid.x+Current^.x;
   Centroid.y:=Centroid.y+Current^.y;
   Centroid.z:=Centroid.z+Current^.z;
   Last:=Current;
  end;
  if ConvexHullIsSameValue(Normal.x,0.0) and ConvexHullIsSameValue(Normal.y,0.0) and ConvexHullIsSameValue(Normal.z,0.0) then begin
   Normal.x:=0.0;
   Normal.y:=1.0;
   Normal.z:=0.0;
  end;
  Plane:=@Polygon.Plane;
  Plane^.Normal:=ConvexHullVectorNormalize(Normal);
  Plane^.Distance:=-ConvexHullVectorDot(Plane^.Normal,ConvexHullVectorDivide(Centroid,Polygon.Count));
 end else begin
  Plane:=@Polygon.Plane;
  Plane^.Normal.x:=0.0;
  Plane^.Normal.y:=0.0;
  Plane^.Normal.z:=0.0;
  Plane^.Distance:=0.0;
 end;
end;

function StanHullProcess(var Points:TConvexHullVectors;var OutTriangles:TConvexHullTriangles;const MaximumOutputPoints:longint=-1;const UserDefinedTolerance:double=-1.0):boolean;
const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
      DOUBLE_PREC:double=2.2204460492503131e-16;
      EPSILON=1e-10;
type PInt3=^TInt3;
     TInt3=array[0..2] of longint;
     TInt3s=array of TInt3;
     PInt4=^TInt4;
     TInt4=array[0..3] of longint;
     PTri=^TTri;
     TTri=record
      v:TInt3;
      n:TInt3;
      ID:longint;
      vmax:longint;
      Rise:double;
     end;
     TTris=array of TTri;
var CountPoints:longint;
    MinPoint,MaxPoint:TConvexHullVector;
    Tolerance:double;
 function Roll3(const a:TInt3):TInt3;
 begin
  result[0]:=a[1];
  result[1]:=a[2];
  result[2]:=a[0];
 end;
 function Equal3(const a,b:TInt3):boolean;
 begin
  result:=((a[0]=b[0]) and (a[1]=b[1]) and (a[2]=b[2]));
 end;
 function IsA(const a,b:TInt3):boolean;
 begin
  result:=Equal3(a,b) or Equal3(Roll3(a),b) or Equal3(a,Roll3(b));
 end;
 function B2B(const a,b:TInt3):boolean;
 var c:TInt3;
 begin
  c[0]:=b[2];
  c[1]:=b[1];
  c[2]:=b[0];
  result:=IsA(a,c);
 end;
 function TriNormal(const v0,v1,v2:TConvexHullVector):TConvexHullVector;
 var cp:TConvexHullVector;
     m:double;
 begin
  cp:=ConvexHullVectorCross(ConvexHullVectorSub(v1,v0),ConvexHullVectorSub(v2,v0));
  m:=ConvexHullVectorLength(cp);
  if m=0.0 then begin
   result.x:=0.0;
   result.y:=0.0;
   result.z:=1.0;
  end else begin
   result.x:=cp.x/m;
   result.y:=cp.y/m;
   result.z:=cp.z/m;
  end;
 end;
 function Above(const Vertices:TConvexHullVectors;const t:TInt3;const p:TConvexHullVector;const Epsilon:double):boolean;
 var n:TConvexHullVector;
 begin
  n:=TriNormal(Vertices[t[0]],Vertices[t[1]],Vertices[t[2]]);
  result:=ConvexHullVectorDot(n,ConvexHullVectorSub(p,Vertices[t[0]]))>Epsilon;
 end;
 function HasEdges(const t:TInt3;const a,b:longint):boolean;
 var i,i1:longint;
 begin
  for i:=0 to 2 do begin
   i1:=ModuloThree[i+1];
   if (t[i]=a) and (t[i1]=b) then begin
    result:=true;
    exit;
   end;
  end;
  result:=false;
 end;
 function HasVertex(const t:TInt3;const v:longint):boolean;
 begin
  result:=(t[0]=v) or (t[1]=v) or (t[2]=v);
 end;
 function HasSharedEdge(const a,b:TInt3):boolean;
 var i,i1:longint;
 begin
  for i:=0 to 2 do begin
   i1:=ModuloThree[i+1];
   if HasEdges(a,b[i1],b[i]) then begin
    result:=true;
    exit;
   end;
  end;
  result:=false;
 end;
 function Tri(const a,b,c,id:longint;const na:longint=-1;const nb:longint=-1;const nc:longint=-1):TTri;
 begin
  FillChar(result,SizeOf(TTri),#0);
  result.v[0]:=a;
  result.v[1]:=b;
  result.v[2]:=c;
  result.n[0]:=na;
  result.n[1]:=nb;
  result.n[2]:=nc;
  result.ID:=ID;
  result.vmax:=-1;
  result.Rise:=0.0;
 end;
 function TriDead(const Tri:TTri):boolean;
 begin
  result:=Tri.n[0]<0;
 end;
 function TriNEIB(const Tri:TTri;const va,vb:longint):plongint;
 var i,i1,i2:longint;
 begin
  for i:=0 to 2 do begin
   i1:=ModuloThree[i+1];
   i2:=ModuloThree[i+2];
   if (Tri.v[i]=va) and (Tri.v[i1]=vb) then begin
    result:=@Tri.n[i2];
    exit;
   end;
   if (Tri.v[i]=vb) and (Tri.v[i1]=va) then begin
    result:=@Tri.n[i2];
    exit;
   end;
  end;
  result:=nil;
  raise EKraftDegeneratedConvexHull.Create('Degenerated convex hull');
 end;
 procedure NNFix(var Tris:TTris;const CountTris,k:longint);
 var i,i1,i2:longint;
     nn:plongint;
 begin
  if Tris[k].ID>=0 then begin
   Assert(Tris[k].ID=k);
   for i:=0 to 2 do begin
    i1:=ModuloThree[i+1];
    i2:=ModuloThree[i+2];
    if Tris[k].n[i]>=0 then begin
     nn:=TriNEIB(Tris[Tris[k].n[i]],Tris[k].v[i2],Tris[k].v[i1]);
     nn^:=k;
    end;
   end;
  end;
 end;
 procedure SwapN(var Tris:TTris;const CountTris,a,b:longint);
 var TempTri:TTri;
     TempID:longint;
 begin
  TempTri:=Tris[a];
  Tris[a]:=Tris[b];
  Tris[b]:=TempTri;
  TempID:=Tris[a].ID;
  Tris[a].ID:=Tris[b].ID;
  Tris[b].ID:=TempID;
  NNFix(Tris,CountTris,a);
  NNFix(Tris,CountTris,b);
 end;
 procedure B2BFix(var Tris:TTris;const CountTris,s,t:longint);
 var i,i1,i2,va,vb:longint;
 begin
  for i:=0 to 2 do begin
   i1:=ModuloThree[i+1];
   i2:=ModuloThree[i+2];
   va:=Tris[s].v[i1];
	 vb:=Tris[s].v[i2];
   Assert(TriNEIB(Tris[TriNEIB(Tris[s],va,vb)^],vb,va)^=Tris[s].ID);
	 Assert(TriNEIB(Tris[TriNEIB(Tris[t],va,vb)^],vb,va)^=Tris[t].ID);
	 TriNEIB(Tris[TriNEIB(Tris[s],va,vb)^],vb,va)^:=TriNEIB(Tris[t],vb,va)^;
	 TriNEIB(Tris[TriNEIB(Tris[t],vb,va)^],va,vb)^:=TriNEIB(Tris[s],va,vb)^;
  end;
  Tris[s].n[0]:=-1;
  Tris[s].n[1]:=-1;
  Tris[s].n[2]:=-1;
  Tris[t].n[0]:=-1;
  Tris[t].n[1]:=-1;
  Tris[t].n[2]:=-1;
 end;
 procedure CheckIt(const Tris:TTris;const CountTris:longint;const t:TTri);
 var i,i1,i2,a,b:longint;
 begin
  Assert(Tris[t.ID].ID=t.ID);
  Assert(@Tris[t.ID]=@t);
  for i:=0 to 2 do begin
   i1:=ModuloThree[i+1];
   i2:=ModuloThree[i+2];
   a:=t.v[i1];
	 b:=t.v[i2];
   Assert(a<>b);
   Assert(TriNEIB(Tris[t.n[i]],b,a)^=t.ID);
  end;
 end;
 function Extrudable(const Tris:TTris;const CountTris:longint;const Epsilon:double):longint;
 var t,i:longint;
 begin
  t:=-1;
	for i:=0 to CountTris-1 do begin
   Assert(Tris[i].ID>=0);
   Assert(Tris[i].ID=i);
   Assert(not TriDead(Tris[i]));
   if (t<0) or (Tris[t].Rise<Tris[i].Rise) then begin
	  t:=i;
   end;
  end;
  if Tris[t].Rise>Epsilon then begin
   result:=t;
  end else begin
   result:=-1;
  end;
 end;
 function MaxDir(const Vertices:TConvexHullVectors;const CountVertices:longint;const Direction:TConvexHullVector):longint;
 var i:longint;
     Best,Current:double;
 begin
  result:=-1;
  Best:=-3.4e+28;
  for i:=0 to CountVertices-1 do begin
   Current:=ConvexHullVectorDot(Direction,Vertices[i]);
   if (i=0) or (Best<Current) then begin
    result:=i;
    Best:=Current;
   end;
  end;
 end;
 function FindSimplex(const Vertices:TConvexHullVectors;const CountVertices:longint):TInt4;
 const v000:TConvexHullVector=(x:0.0;y:0.0;z:0.0);
       v100:TConvexHullVector=(x:1.0;y:0.0;z:0.0);
       v010:TConvexHullVector=(x:0.0;y:1.0;z:0.0);
 var p0,p1,p2,p3,TempIndex:longint;
     Basis:array[0..2] of TConvexHullVector;
 begin
  Basis[0].x:=0.01;
  Basis[0].y:=0.02;
  Basis[0].z:=1.0;
  p0:=MaxDir(Vertices,CountVertices,Basis[0]);
  p1:=MaxDir(Vertices,CountVertices,ConvexHullVectorNeg(Basis[0]));
  Basis[0]:=ConvexHullVectorSub(Vertices[p0],Vertices[p1]);
	if (p0=p1) or ConvexHullVectorCompare(Basis[0],v000) then begin
   result[0]:=-1;
   result[1]:=-1;
   result[2]:=-1;
   result[3]:=-1;
   exit;
  end;
  Basis[1]:=ConvexHullVectorCross(v100,Basis[0]);
  Basis[2]:=ConvexHullVectorCross(v010,Basis[0]);
  if ConvexHullVectorLengthSquared(Basis[1])>ConvexHullVectorLengthSquared(Basis[2]) then begin
   Basis[1]:=ConvexHullVectorNormalize(Basis[1]);
  end else begin
   Basis[1]:=ConvexHullVectorNormalize(Basis[2]);
  end;
  p2:=MaxDir(Vertices,CountVertices,Basis[1]);
  if (p2=p0) or (p2=p1) then begin
   p2:=MaxDir(Vertices,CountVertices,ConvexHullVectorNeg(Basis[1]));
   if (p2=p0) or (p2=p1) then begin
    result[0]:=-1;
    result[1]:=-1;
    result[2]:=-1;
    result[3]:=-1;
    exit;
   end;
  end;
  Basis[1]:=ConvexHullVectorSub(Vertices[p2],Vertices[p0]);
	Basis[2]:=ConvexHullVectorCross(Basis[1],Basis[0]);
  p3:=MaxDir(Vertices,CountVertices,Basis[2]);
  if (p3=p0) or (p3=p1) or (p3=p2) then begin
   p3:=MaxDir(Vertices,CountVertices,ConvexHullVectorNeg(Basis[2]));
   if (p3=p0) or (p3=p1) or (p3=p2) then begin
    result[0]:=-1;
    result[1]:=-1;
    result[2]:=-1;
    result[3]:=-1;
    exit;
   end;
  end;
  Assert(not ((p0=p1) or (p0=p2) or (p0=p3) or (p1=p2) or (p1=p3) or (p2=p3)));
	if ConvexHullVectorDot(ConvexHullVectorSub(Vertices[p3],Vertices[p0]),
                         ConvexHullVectorCross(ConvexHullVectorSub(Vertices[p1],Vertices[p0]),
                                               ConvexHullVectorSub(Vertices[p2],Vertices[p0])))<0.0 then begin
   TempIndex:=p2;
   p2:=p3;
   p3:=TempIndex;
  end;
  result[0]:=p0;
  result[1]:=p1;
  result[2]:=p2;
  result[3]:=p3;
 end;
{$warnings off}
 function CalcHull(const Vertices:TConvexHullVectors;var CountVertices:longint;VerticesLimit:longint):TInt3s;
 var bmin,bmax,Center,n,TempVertex:TConvexHullVector;
     isextreme:array of boolean;
     CountTris,i,j,k,te,v,{NewStart,}nb:longint;
     Epsilon:double;
     {ti,}t,nt:TInt3;
     p:TInt4;
     Tris:TTris;
     tt:PTri;
     {TempTri:TTri;}
     Map,Used:array of longint;
  procedure TrisPushBack(const NewTri:TTri);
  begin
   if (CountTris+1)>=length(Tris) then begin
    SetLength(Tris,(CountTris+1)*2);
   end;
   Tris[CountTris]:=NewTri;
   inc(CountTris);
  end;
  procedure Extrude(const t0,v:longint);
  var t,n:TInt3;
      b:longint;
  begin
   t:=Tris[t0].v;
	 b:=CountTris;
   n:=Tris[t0].n;
   TrisPushBack(Tri(v,t[1],t[2],b+0,n[0],b+1,b+2));
	 TriNEIB(Tris[n[0]],t[1],t[2])^:=b+0;
   TrisPushBack(Tri(v,t[2],t[0],b+1,n[1],b+2,b+0));
	 TriNEIB(Tris[n[1]],t[2],t[0])^:=b+1;
   TrisPushBack(Tri(v,t[0],t[1],b+2,n[2],b+0,b+1));
	 TriNEIB(Tris[n[2]],t[0],t[1])^:=b+2;
	 Tris[t0].n[0]:=-1;
	 Tris[t0].n[1]:=-1;
	 Tris[t0].n[2]:=-1;
   CheckIt(Tris,CountTris,Tris[b+0]);
   CheckIt(Tris,CountTris,Tris[b+1]);
   CheckIt(Tris,CountTris,Tris[b+2]);
	 if HasVertex(Tris[n[0]].v,v) then begin
    B2BFix(Tris,CountTris,b+0,n[0]);
   end;
	 if HasVertex(Tris[n[1]].v,v) then begin
    B2BFix(Tris,CountTris,b+1,n[1]);
   end;
   if HasVertex(Tris[n[2]].v,v) then begin
    B2BFix(Tris,CountTris,b+2,n[2]);
   end;
  end;
 begin
  result:=nil;
  isextreme:=nil;
  Tris:=nil;
  CountTris:=0;
  Map:=nil;
  Used:=nil;
  if CountVertices<4 then begin
   exit;
  end;
  try
   if VerticesLimit<1 then begin
    VerticesLimit:=1000000000;
   end;
   try
    SetLength(isextreme,CountVertices);
    bmin:=Vertices[0];
    bmax:=Vertices[0];
    for j:=0 to CountVertices-1 do begin
     isextreme[j]:=false;
     bmin.x:=Min(bmin.x,Vertices[j].x);
     bmin.y:=Min(bmin.y,Vertices[j].y);
     bmin.z:=Min(bmin.z,Vertices[j].z);
     bmax.x:=Max(bmax.x,Vertices[j].x);
     bmax.y:=Max(bmax.y,Vertices[j].y);
     bmax.z:=Max(bmax.z,Vertices[j].z);
    end;
    Epsilon:=ConvexHullVectorLength(ConvexHullVectorSub(bmax,bmin))*0.001;
    p:=FindSimplex(Vertices,CountVertices);
    if p[0]<0 then begin
     SetLength(isextreme,0);
     result:=nil;
     exit;
    end;
    Center.x:=(Vertices[p[0]].x+Vertices[p[1]].x+Vertices[p[2]].x+Vertices[p[3]].x)*0.25;
    Center.y:=(Vertices[p[0]].y+Vertices[p[1]].y+Vertices[p[2]].y+Vertices[p[3]].y)*0.25;
    Center.z:=(Vertices[p[0]].z+Vertices[p[1]].z+Vertices[p[2]].z+Vertices[p[3]].z)*0.25;
    TrisPushBack(Tri(p[2],p[3],p[1],CountTris,2,3,1));
    TrisPushBack(Tri(p[3],p[2],p[0],CountTris,3,2,0));
    TrisPushBack(Tri(p[0],p[1],p[3],CountTris,0,1,3));
    TrisPushBack(Tri(p[1],p[0],p[2],CountTris,1,0,2));
    isextreme[p[0]]:=true;
    isextreme[p[1]]:=true;
    isextreme[p[2]]:=true;
    isextreme[p[3]]:=true;
    CheckIt(Tris,CountTris,Tris[0]);
    CheckIt(Tris,CountTris,Tris[1]);
    CheckIt(Tris,CountTris,Tris[2]);
    CheckIt(Tris,CountTris,Tris[3]);
    for k:=0 to CountTris-1 do begin
     tt:=@Tris[k];
     Assert(tt^.ID>=0);
     Assert(tt^.vmax<0);
     n:=TriNormal(Vertices[tt^.v[0]],Vertices[tt^.v[1]],Vertices[tt^.v[2]]);
     tt^.vmax:=Maxdir(Vertices,CountVertices,n);
     tt^.Rise:=ConvexHullVectorDot(n,ConvexHullVectorSub(Vertices[tt^.vmax],Vertices[tt^.v[0]]));
    end;
    dec(VerticesLimit,4);
    while VerticesLimit>0 do begin
     te:=Extrudable(Tris,CountTris,Epsilon);
     if te<0 then begin
      break;
     end;
     //ti:=Tris[te].v;
     v:=Tris[te].vmax;
     Assert(not isextreme[v]);  // wtf we've already done this vertex
     isextreme[v]:=true;
    {if(v=p0) or (v=p1) or (v=p2) or (v=p3) then begin
      continue; // done these already
     end;{}
     j:=CountTris;
     //NewStart:=j;
     while j>0 do begin
      dec(j);
      if TriDead(Tris[j]) then begin
       continue;
      end;
      t:=tris[j].v;
      if Above(Vertices,t,Vertices[v],0.01*Epsilon) then begin
       Extrude(j,v);
      end;
     end;
     // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
     j:=CountTris;
     while j>0 do begin
      dec(j);
      if TriDead(Tris[j]) then begin
       continue;
      end;
      if not HasVertex(Tris[j].v,v) then begin
       break;
      end;
      nt:=Tris[j].v;
      if Above(Vertices,nt,Center,0.01*Epsilon) or
         (ConvexHullVectorLength(ConvexHullVectorCross(ConvexHullVectorSub(Vertices[nt[1]],Vertices[nt[0]]),
                                                       ConvexHullVectorSub(Vertices[nt[2]],Vertices[nt[1]])))<(Epsilon*Epsilon*0.1)) then begin
       nb:=Tris[j].n[0];
       Assert(nb>=0);
       Assert(not TriDead(Tris[nb]));
       Assert(not HasVertex(Tris[nb].v,v));
       Assert(Tris[nb].ID<j);
       Extrude(nb,v);
       j:=CountTris;
      end;
     end;
     j:=CountTris;
     while j>0 do begin
      dec(j);
      if TriDead(Tris[j]) then begin
       continue;
      end;
      tt:=@Tris[j];
      if tt^.vmax>=0 then begin
       break;
      end;
      n:=TriNormal(Vertices[tt^.v[0]],Vertices[tt^.v[1]],Vertices[tt^.v[2]]);
      tt^.vmax:=MaxDir(Vertices,CountVertices,n);
      if isextreme[tt^.vmax] then begin
       tt^.vmax:=-1; // already done that vertex - algorithm needs to be able to terminate.
      end else begin
       tt^.Rise:=ConvexHullVectorDot(n,ConvexHullVectorSub(Vertices[tt^.vmax],Vertices[tt^.v[0]]));
      end;
     end;
     // compress, remove non-tris
     j:=CountTris;
     while j>0 do begin
      dec(j);
      if not TriDead(Tris[j]) then begin
       continue;
      end;
      SwapN(Tris,CountTris,j,CountTris-1);
      dec(CountTris);
     end;
     dec(VerticesLimit);
    end;
    SetLength(result,CountTris);
    for i:=0 to CountTris-1 do begin
     Assert(not TriDead(Tris[i]));
     result[i]:=Tris[i].v;
    end;
    SetLength(Tris,0);
{}  SetLength(Used,CountVertices);
    SetLength(Map,CountVertices);
    for i:=0 to CountVertices-1 do begin
     Used[i]:=0;
     Map[i]:=0;
    end;
    for i:=0 to CountTris-1 do begin
     for j:=0 to 2 do begin
      inc(Used[result[i,j]]);
     end;
    end;
    j:=0;
    for i:=0 to CountVertices-1 do begin
     if Used[i]>0 then begin
      TempVertex:=Vertices[j];
      Vertices[j]:=Vertices[i];
      Vertices[i]:=TempVertex;
      Map[i]:=j;
      inc(j);
     end else begin
      Map[i]:=-1;
     end;
    end;
    CountVertices:=j;
    for i:=0 to CountTris-1 do begin
     for j:=0 to 2 do begin
      result[i,j]:=Map[result[i,j]];
     end;
    end;{}
   finally
    SetLength(isextreme,0);
    SetLength(Tris,0);
    SetLength(Map,0);
    SetLength(Used,0);
   end;
  except
   result:=nil;
   raise;
  end;
 end;
{$warnings on}
 procedure SearchMinMax;
 var Index:longint;
     Point:PConvexHullVector;
 begin
  MinPoint:=Points[0];
  MaxPoint:=Points[0];
  for Index:=1 to CountPoints-1 do begin
   Point:=@Points[Index];
   if MinPoint.x>Point^.x then begin
    MinPoint.x:=Point^.x;
   end;
   if MinPoint.y>Point^.y then begin
    MinPoint.y:=Point^.y;
   end;
   if MinPoint.z>Point^.z then begin
    MinPoint.z:=Point^.z;
   end;
   if MaxPoint.x<Point^.x then begin
    MaxPoint.x:=Point^.x;
   end;
   if MaxPoint.y<Point^.y then begin
    MaxPoint.y:=Point^.y;
   end;
   if MaxPoint.z<Point^.z then begin
    MaxPoint.z:=Point^.z;
   end;
  end;
 end;
 procedure RemoveDuplicatePoints;
 const HashBits=8;
       HashSize=1 shl HashBits;
       HashMask=HashSize-1;
 var PointIndex,OtherPointIndex,CountNewPoints,HashItemIndex:longint;
     Hash:longword;
     NewPoints:TConvexHullVectors;
     PointNextIndices:array of longint;
     HashTable:array of longint;
 begin
  NewPoints:=nil;
  PointNextIndices:=nil;
  HashTable:=nil;
  try

   SetLength(PointNextIndices,length(Points));
   for PointIndex:=0 to length(PointNextIndices)-1 do begin
    PointNextIndices[PointIndex]:=-1;
   end;

   SetLength(HashTable,HashSize);
   for PointIndex:=0 to length(HashTable)-1 do begin
    HashTable[PointIndex]:=-1;
   end;

   SetLength(NewPoints,length(Points));
   CountNewPoints:=0;

   for PointIndex:=0 to length(Points)-1 do begin

    Hash:=((round(Points[PointIndex].x)*73856093) xor (round(Points[PointIndex].y)*19349663) xor (round(Points[PointIndex].z)*83492791)) and HashMask;

    HashItemIndex:=HashTable[Hash];
    while HashItemIndex>=0 do begin
     if ConvexHullVectorCompare(Points[PointIndex],NewPoints[HashItemIndex]) then begin
      break;
     end;
     HashItemIndex:=PointNextIndices[HashItemIndex];
    end;

    if HashItemIndex<0 then begin
     OtherPointIndex:=CountNewPoints;
     inc(CountNewPoints);
     if CountNewPoints>length(NewPoints) then begin
      SetLength(NewPoints,CountNewPoints*2);
     end;
     NewPoints[OtherPointIndex]:=Points[PointIndex];
     PointNextIndices[OtherPointIndex]:=HashTable[Hash];
     HashTable[Hash]:=OtherPointIndex;
    end;

   end;

   if length(Points)<>CountNewPoints then begin
    SetLength(Points,CountNewPoints);
    for PointIndex:=0 to CountNewPoints-1 do begin
     Points[PointIndex]:=NewPoints[PointIndex];
    end;
   end;

  finally
   SetLength(NewPoints,0);
   SetLength(PointNextIndices,0);
   SetLength(HashTable,0);
  end;
 end;
 procedure RemoveTooNearPoints;
 const HashBits=8;
       HashSize=1 shl HashBits;
       HashMask=HashSize-1;
 var PointIndex,OtherPointIndex,CountNewPoints,HashItemIndex:longint;
     Hash:longword;
     InverseTolerance:double;
     NewPoints:TConvexHullVectors;
     PointNextIndices:array of longint;
     HashTable:array of longint;
 begin
  NewPoints:=nil;
  PointNextIndices:=nil;
  HashTable:=nil;
  try

   SetLength(PointNextIndices,length(Points));
   for PointIndex:=0 to length(PointNextIndices)-1 do begin
    PointNextIndices[PointIndex]:=-1;
   end;

   SetLength(HashTable,HashSize);
   for PointIndex:=0 to length(HashTable)-1 do begin
    HashTable[PointIndex]:=-1;
   end;

   SetLength(NewPoints,length(Points));
   CountNewPoints:=0;

   InverseTolerance:=1.0/Tolerance;

   for PointIndex:=0 to length(Points)-1 do begin

    Hash:=((trunc(Points[PointIndex].x*InverseTolerance)*73856093) xor
           (trunc(Points[PointIndex].y*InverseTolerance)*19349663) xor
           (trunc(Points[PointIndex].z*InverseTolerance)*83492791)) and HashMask;

    HashItemIndex:=HashTable[Hash];
    while HashItemIndex>=0 do begin
     if ConvexHullVectorLengthSquared(ConvexHullVectorSub(Points[PointIndex],NewPoints[HashItemIndex]))<=(2.0*Tolerance) then begin
      break;
     end;
     HashItemIndex:=PointNextIndices[HashItemIndex];
    end;

    if HashItemIndex<0 then begin
     OtherPointIndex:=CountNewPoints;
     inc(CountNewPoints);
     if CountNewPoints>length(NewPoints) then begin
      SetLength(NewPoints,CountNewPoints*2);
     end;
     NewPoints[OtherPointIndex]:=Points[PointIndex];
     PointNextIndices[OtherPointIndex]:=HashTable[Hash];
     HashTable[Hash]:=OtherPointIndex;
    end;

   end;

   if length(Points)<>CountNewPoints then begin
    SetLength(Points,CountNewPoints);
    for PointIndex:=0 to CountNewPoints-1 do begin
     Points[PointIndex]:=NewPoints[PointIndex];
    end;
   end;

  finally
   SetLength(NewPoints,0);
   SetLength(PointNextIndices,0);
   SetLength(HashTable,0);
  end;
 end;
 procedure SortPoints;
 var Count,Index:longint;
     NewPoints:TConvexHullVectors;
     IndirectPoints:array of PConvexHullVector;
 begin
  Count:=length(Points);
  if Count>1 then begin
   NewPoints:=nil;
   IndirectPoints:=nil;
   try
    NewPoints:=Points;
    Points:=nil;
    SetLength(IndirectPoints,Count);
    SetLength(Points,Count);
    for Index:=0 to Count-1 do begin
     IndirectPoints[Index]:=@NewPoints[Index];
    end;
    IndirectIntroSort(@IndirectPoints[0],0,Count-1,@CompareConvexHullPoints);
    for Index:=0 to Count-1 do begin
     Points[Index]:=IndirectPoints[Index]^;
    end;
   finally
    SetLength(NewPoints,0);
    SetLength(IndirectPoints,0);
   end;
  end;
 end;
var Tris:TInt3s;
    i:longint;
begin
 result:=false;
 Tris:=nil;
 try
  CountPoints:=length(Points);
  SearchMinMax;
  if UserDefinedTolerance>0.0 then begin
   Tolerance:=UserDefinedTolerance;
  end else begin
   Tolerance:=Max(DOUBLE_PREC,(3.0*DOUBLE_PREC)*(Max(abs(MaxPoint.x),abs(MaxPoint.x))+Max(abs(MaxPoint.y),abs(MaxPoint.y))+Max(abs(MaxPoint.z),abs(MaxPoint.z))));
  end;
  RemoveDuplicatePoints;
  RemoveTooNearPoints;
  SortPoints;
  CountPoints:=length(Points);
  Tris:=CalcHull(Points,CountPoints,MaximumOutputPoints);
  if (length(Tris)>0) and (CountPoints>0) then begin
   SetLength(Points,CountPoints);
   SetLength(OutTriangles,length(Tris));
   for i:=0 to length(Tris)-1 do begin
    OutTriangles[i,0]:=Tris[i,0];
    OutTriangles[i,1]:=Tris[i,1];
    OutTriangles[i,2]:=Tris[i,2];
   end;
   result:=true;
  end;
 finally
  SetLength(Tris,0);
 end;
end;

function KraftQuickHullIsSameValue(const a,b:double):boolean;
const FuzzFactor=1000;
      DoubleResolution=1e-15*FuzzFactor;
var EpsilonTolerance:double;
begin
 EpsilonTolerance:=abs(a);
 if EpsilonTolerance>abs(b) then begin
  EpsilonTolerance:=abs(b);
 end;
 EpsilonTolerance:=EpsilonTolerance*DoubleResolution;
 if EpsilonTolerance<DoubleResolution then begin
  EpsilonTolerance:=DoubleResolution;
 end;
 if a>b then begin
  result:=(a-b)<=EpsilonTolerance;
 end else begin
  result:=(b-a)<=EpsilonTolerance;
 end;
end;

function KraftQuickHullIntLog2(x:longword):longword; {$ifdef cpu386}assembler; register;
asm
 test eax,eax
 jz @Done
 bsr eax,eax
 @Done:
end;
{$else}
begin
 x:=x or (x shr 1);
 x:=x or (x shr 2);
 x:=x or (x shr 4);
 x:=x or (x shr 8);
 x:=x or (x shr 16);
 x:=x shr 1;
 x:=x-((x shr 1) and $55555555);
 x:=((x shr 2) and $33333333)+(x and $33333333);
 x:=((x shr 4)+x) and $0f0f0f0f;
 x:=x+(x shr 8);
 x:=x+(x shr 16);
 result:=x and $3f;
end;
{$endif}

constructor TKraftQuickHullIntegerList.Create;
begin
 inherited Create;
 fCountItems:=0;
 fAllocated:=0;
 fList:=nil;
 Clear;
end;

destructor TKraftQuickHullIntegerList.Destroy;
begin
 Clear;
 inherited Destroy;
end;

procedure TKraftQuickHullIntegerList.Clear;
begin
 fCountItems:=0;
 fAllocated:=0;
 fIsSorted:=false;
 ReallocMem(fList,0);
end;

procedure TKraftQuickHullIntegerList.SetCapacity(NewCapacity:longint);
begin
 if NewCapacity>=0 then begin
  ReallocMem(fList,NewCapacity*sizeof(longint));
  fAllocated:=NewCapacity;
 end;
end;

procedure TKraftQuickHullIntegerList.SetCount(NewCount:longint);
begin
 if NewCount>=0 then begin
  if NewCount<fCountItems then begin
   fCountItems:=NewCount;
  end else if NewCount>fCountItems then begin
   if NewCount>fAllocated then begin
    SetCapacity((fCountItems+4096) and not 4095);
   end;
   if fCountItems<NewCount then begin
    FillChar(fList^[fCountItems],(NewCount-fCountItems)*SizeOf(longint),#0);
   end;
   fCountItems:=NewCount;
  end;
 end;
end;

function TKraftQuickHullIntegerList.Add(Item:longint):longint;
begin
 if fCountItems>=fAllocated then begin
  fAllocated:=(fCountItems+4096) and not 4095;
  ReallocMem(fList,fAllocated*SizeOf(longint));
 end;
 fList^[fCountItems]:=Item;
 result:=fCountItems;
 inc(fCountItems);
 fIsSorted:=false;
end;

procedure TKraftQuickHullIntegerList.AddSorted(Item:longint);
var i:longint;
begin
 if fIsSorted then begin
  if fCountItems=0 then begin
   Add(Item);
  end else begin
   i:=0;
   while (i<fCountItems) and (Item>=fList^[i]) do begin
    inc(i);
   end;
   Insert(i,Item);
  end;
  fIsSorted:=true;
 end else begin
  Add(Item);
  Sort;
 end;
end;

procedure TKraftQuickHullIntegerList.Insert(Index:longint;Item:longint);
var i:longint;
begin
 if (Index>=0) and (Index<fCountItems) then begin
  SetCount(fCountItems+1);
  for i:=fCountItems-1 downto Index do begin
   fList^[i+1]:=fList^[i];
  end;
  fList^[Index]:=Item;
 end else if Index=fCountItems then begin
  Add(Item);
 end else if Index>fCountItems then begin
  SetCount(Index);
  Add(Item);
 end;
 fIsSorted:=false;
end;

procedure TKraftQuickHullIntegerList.Delete(Index:longint);
var i,j,k:longint;
begin
 if (Index>=0) and (Index<fCountItems) then begin
  k:=fCountItems-1;
  j:=Index;
  for i:=j to k-1 do begin
   fList^[i]:=fList^[i+1];
  end;
  SetCount(k);
 end;
end;

function TKraftQuickHullIntegerList.Remove(Item:longint):longint;
var i,j,k:longint;
begin
 result:=-1;
 k:=fCountItems;
 j:=-1;
 for i:=0 to k-1 do begin
  if fList^[i]=Item then begin
   j:=i;
   break;
  end;
 end;
 if j>=0 then begin
  dec(k);
  for i:=j to k-1 do begin
   fList^[i]:=fList^[i+1];
  end;
  SetCount(k);
  result:=j;
 end;
end;

function TKraftQuickHullIntegerList.Find(Item:longint):longint;
var i,l,r:longint;
begin
 result:=-1;
 if fIsSorted then begin
  l:=0;
  r:=fCountItems-1;
  while l<=r do begin
   i:=(l+r) shr 1;
   if fList^[i]=Item then begin
    result:=i;
    break;
   end else begin
    if fList^[i]<Item then begin
     l:=i+1;
    end else begin
     r:=i-1;
    end;
   end;
  end;
 end else begin
  for i:=0 to fCountItems-1 do begin
   if fList^[i]=Item then begin
    result:=i;
    break;
   end;
  end;
 end;
end;

function TKraftQuickHullIntegerList.IndexOf(Item:longint):longint;
begin
 result:=Find(Item);
end;

procedure TKraftQuickHullIntegerList.Exchange(Index1,Index2:longint);
var TempInteger:longint;
begin
 if (Index1>=0) and (Index1<fCountItems) and (Index2>=0) and (Index2<fCountItems) then begin
  TempInteger:=fList^[Index1];
  fList^[Index1]:=fList^[Index2];
  fList^[Index2]:=TempInteger;
  fIsSorted:=false;
 end;
end;

function TKraftQuickHullIntegerList.GetItem(Index:longint):longint;
begin
 if (Index>=0) and (Index<fCountItems) then begin
  result:=fList^[Index];
 end else begin
  result:=0;
 end;
end;

procedure TKraftQuickHullIntegerList.SetItem(Index:longint;Value:longint);
begin
 if (Index>=0) and (Index<fCountItems) then begin
  fList^[Index]:=Value;
  fIsSorted:=false;
 end;
end;

function TKraftQuickHullIntegerList.GetItemPointer(Index:longint):pointer;
begin
 if (Index>=0) and (Index<fCountItems) then begin
  result:=@fList^[Index];
 end else begin
  result:=nil;
 end;
end;

procedure TKraftQuickHullIntegerList.Sort;
 procedure IntroSort(Left,Right,Depth:longint);
  procedure SiftDown(Current,MaxIndex:longint);
  var SiftLeft,SiftRight,Largest:longint;
      v:longint;
  begin
   SiftLeft:=Left+(2*(Current-Left))+1;
   SiftRight:=Left+(2*(Current-Left))+2;
   Largest:=Current;
   if (SiftLeft<=MaxIndex) and (fList^[SiftLeft]>fList^[Largest]) then begin
    Largest:=SiftLeft;
   end;
   if (SiftRight<=MaxIndex) and (fList^[SiftRight]>fList^[Largest]) then begin
    Largest:=SiftRight;
   end;
   if Largest<>Current then begin
    v:=fList^[Current];
    fList^[Current]:=fList^[Largest];
    fList^[Largest]:=v;
    SiftDown(Largest,MaxIndex);
   end;
  end;
  procedure InsertionSort;
  var i,j:longint;
      t:longint;
  begin
   i:=Left+1;
   while i<=Right do begin
    t:=fList^[i];
    j:=i-1;
    while (j>=Left) and (t<fList^[j]) do begin
     fList^[j+1]:=fList^[j];
     dec(j);
    end;
    fList^[j+1]:=t;
    inc(i);
   end;
  end;
  procedure HeapSort;
  var i:longint;
      v:longint;
  begin
   i:=((Left+Right+1) div 2)-1;
   while i>=Left do begin
    SiftDown(i,Right);
    dec(i);
   end;
   i:=Right;
   while i>=Left+1 do begin
    v:=fList^[i];
    fList^[i]:=fList^[Left];
    fList^[Left]:=v;
    SiftDown(Left,i-1);
    dec(i);
   end;
  end;
  procedure QuickSortWidthMedianOfThreeOptimization;
  var Middle,i,j:longint;
      Pivot,v:longint;
  begin
   Middle:=(Left+Right) div 2;
   if (Right-Left)>3 then begin
    if fList^[Left]>fList^[Middle] then begin
     v:=fList^[Left];
     fList^[Left]:=fList^[Middle];
     fList^[Middle]:=v;
    end;
    if fList^[Left]>fList^[Right] then begin
     v:=fList^[Left];
     fList^[Left]:=fList^[Right];
     fList^[Right]:=v;
    end;
    if fList^[Middle]>fList^[Right] then begin
     v:=fList^[Middle];
     fList^[Middle]:=fList^[Right];
     fList^[Right]:=v;
    end;
   end;
   Pivot:=fList^[Middle];
   i:=Left;
   j:=Right;
   while true do begin
    while (i<j) and (fList^[i]<Pivot) do begin
     inc(i);
    end;
    while (j>i) and (fList^[j]>Pivot) do begin
     dec(j);
    end;
    if i>j then begin
     break;
    end;
    v:=fList^[i];
    fList^[i]:=fList^[j];
    fList^[j]:=v;
    inc(i);
    dec(j);
   end;
   IntroSort(Left,j,Depth-1);
   IntroSort(i,Right,Depth-1);
  end;
 begin
  if Left<Right then begin
   if (Right-Left)<16 then begin
    InsertionSort;
   end else if Depth=0 then begin
    HeapSort;
   end else begin
    QuickSortWidthMedianOfThreeOptimization;
   end;
  end;
 end;
 procedure QuickSort(L,R:longint);
 var Pivot,vL,vR:longint;
     v:longint;
 begin
  if (R-L)<=1 then begin
   if (L<R) and (fList[R]<fList[L]) then begin
    v:=fList^[L];
    fList^[L]:=fList^[R];
    fList^[R]:=v;
   end;
  end else begin
   vL:=L;
   vR:=R;
   Pivot:=L+Random(R-L);
   while vL<vR do begin
    while (vL<Pivot) and (fList^[vL]<=fList^[Pivot]) do begin
     inc(vL);
    end;
    while (vR>Pivot) and (fList^[vR]>fList^[Pivot]) do begin
     dec(vR);
    end;
    v:=fList^[vL];
    fList^[vL]:=fList^[vR];
    fList^[vR]:=v;
    if Pivot=vL then begin
     Pivot:=vR;
    end else if Pivot=vR then begin
     Pivot:=vL;
    end;
   end;
   if (Pivot-1)>=L then begin
    QuickSort(L,Pivot-1);
   end;
   if (Pivot+1)<=R then begin
    QuickSort(Pivot+1,R);
   end;
  end;
 end;
 procedure QuickSortEx(Left,Right:longint);
 var Pivot,i,j:longint;
     v:longint;
 begin
  i:=Left;
  j:=Right;
  Pivot:=fList^[(i+j) div 2];
  while i<=j do begin
   while fList^[i]<Pivot do begin
    inc(i);
   end;
   while fList^[j]>Pivot do begin
    dec(j);
   end;
   if i<=j then begin
    v:=fList^[i];
    fList^[i]:=fList^[i];
    fList^[j]:=v;
    inc(i);
    dec(j);
   end;
  end;
  if Left<j then begin
   QuickSortEx(Left,j);
  end;
  if i<Right then begin
   QuickSortEx(i,Right);
  end;
 end;
 procedure BeRoSort;
 var i:longint;
     v:longint;
 begin
  i:=0;
  while i<(fCountItems-1) do begin
   if fList^[i]>fList^[i+1] then begin
    v:=fList^[i];
    fList^[i]:=fList^[i+1];
    fList^[i+1]:=v;
    if i>0 then begin
     dec(i);
    end else begin
     inc(i);
    end;
   end else begin
    inc(i);
   end;
  end;
 end;
begin
 if fCountItems>0 then begin
  IntroSort(0,fCountItems-1,KraftQuickHullIntLog2(fCountItems)*2);
  fIsSorted:=true;
 end;
end;

function TKraftQuickHullVector3D.Init(const ax:double=0.0;const ay:double=0.0;const az:double=0.0):TKraftQuickHullVector3D;
begin
 x:=ax;
 y:=ay;
 z:=az;
 result:=self;
end;

procedure TKraftQuickHullVector3D.SetValue(i:longint;const v:double);
begin
 case i of
  0:begin
   x:=v;
  end;
  1:begin
   y:=v;
  end;
  2:begin
   z:=v;
  end;
 end;
end;

function TKraftQuickHullVector3D.GetValue(i:longint):double;
begin
 case i of
  0:begin
   result:=x;
  end;
  1:begin
   result:=y;
  end;
  2:begin
   result:=z;
  end;
  else begin
   result:=0;
  end;
 end;
end;

procedure TKraftQuickHullVector3D.Add(const v0,v1:TKraftQuickHullVector3D);
begin
 x:=v0.x+v1.x;
 y:=v0.y+v1.y;
 z:=v0.z+v1.z;
end;

procedure TKraftQuickHullVector3D.Add(const v:TKraftQuickHullVector3D);
begin
 x:=x+v.x;
 y:=y+v.y;
 z:=z+v.z;
end;

procedure TKraftQuickHullVector3D.Sub(const v0,v1:TKraftQuickHullVector3D);
begin
 x:=v0.x-v1.x;
 y:=v0.y-v1.y;
 z:=v0.z-v1.z;
end;

procedure TKraftQuickHullVector3D.Sub(const v:TKraftQuickHullVector3D);
begin
 x:=x-v.x;
 y:=y-v.y;
 z:=z-v.z;
end;

procedure TKraftQuickHullVector3D.Scale(const v:TKraftQuickHullVector3D;const s:double);
begin
 x:=v.x*s;
 y:=v.y*s;
 z:=v.z*s;
end;

procedure TKraftQuickHullVector3D.Scale(const s:double);
begin
 x:=x*s;
 y:=y*s;
 z:=z*s;
end;

function TKraftQuickHullVector3D.Length:double;
begin
 result:=sqrt(sqr(x)+sqr(y)+sqr(z));
end;

function TKraftQuickHullVector3D.LengthSquared:double;
begin
 result:=sqr(x)+sqr(y)+sqr(z);
end;

function TKraftQuickHullVector3D.Distance(const v:TKraftQuickHullVector3D):double;
begin
 result:=sqrt(sqr(v.x-x)+sqr(v.y-y)+sqr(v.z-z));
end;

function TKraftQuickHullVector3D.DistanceSquared(const v:TKraftQuickHullVector3D):double;
begin
 result:=sqr(v.x-x)+sqr(v.y-y)+sqr(v.z-z);
end;

procedure TKraftQuickHullVector3D.Normalize;
var l:double;
begin
 l:=sqr(x)+sqr(y)+sqr(z);
 if abs(l)>1e-16 then begin
  l:=sqrt(l);
  x:=x/l;
  y:=y/l;
  z:=z/l;
 end;
end;

function TKraftQuickHullVector3D.Dot(const v:TKraftQuickHullVector3D):double;
begin
 result:=(x*v.x)+(y*v.y)+(z*v.z);
end;

procedure TKraftQuickHullVector3D.CrossProduct(const v0,v1:TKraftQuickHullVector3D);
var tx,ty,tz:double;
begin
 tx:=(v0.y*v1.z)-(v0.z*v1.y);
 ty:=(v0.z*v1.x)-(v0.x*v1.z);
 tz:=(v0.x*v1.y)-(v0.y*v1.x);
 x:=tx;
 y:=ty;
 z:=tz;
end;

procedure TKraftQuickHullVector3D.SetRandom(Lower,Upper:double);
var Range:double;
begin
 Range:=Upper-Lower;
 x:=Lower+(random*Range);
 y:=Lower+(random*Range);
 z:=Lower+(random*Range);
end;

function TKraftQuickHullVector3D.Equals(const v:TKraftQuickHullVector3D):boolean;
begin
 result:=SameValue(v.x,x) and SameValue(v.y,y) and SameValue(v.z,z);
end;

procedure TKraftQuickHullVertexList.Clear;
begin
 Head:=nil;
 Tail:=nil;
end;

procedure TKraftQuickHullVertexList.Add(vtx:TKraftQuickHullVertex);
begin
 if assigned(Head) then begin
  Tail.fNext:=vtx;
 end else begin
  Head:=vtx;
 end;
 vtx.fPrevious:=Tail;
 vtx.fNext:=nil;
 Tail:=vtx;
end;

procedure TKraftQuickHullVertexList.AddAll(vtx:TKraftQuickHullVertex);
begin
 if assigned(Head) then begin
  Tail.Next:=vtx;
 end else begin
  Head:=vtx;
 end;
 vtx.Previous:=Tail;
 while assigned(vtx.Next) do begin
  vtx:=vtx.Next;
 end;
 Tail:=vtx;
end;

procedure TKraftQuickHullVertexList.Delete(vtx:TKraftQuickHullVertex);
begin
 if assigned(vtx.Previous) then begin
  vtx.Previous.Next:=vtx.Next;
 end else if Head=vtx then begin
  Head:=vtx.Next;
 end;
 if assigned(vtx.Next) then begin
  vtx.Next.Previous:=vtx.Previous;
 end else if Tail=vtx then begin
  Tail:=vtx.Previous;
 end;
end;

procedure TKraftQuickHullVertexList.Delete(vtx1,vtx2:TKraftQuickHullVertex);
begin
 if assigned(vtx1.Previous) then begin
  vtx1.Previous.Next:=vtx2.Next;
 end else if Head=vtx1 then begin
  Head:=vtx2.Next;
 end;
 if assigned(vtx2.Next) then begin
  vtx2.Next.Previous:=vtx1.Previous;
 end else if Tail=vtx2 then begin
  Tail:=vtx1.Previous;
 end;
end;

procedure TKraftQuickHullVertexList.InsertBefore(vtx,Next:TKraftQuickHullVertex);
begin
 vtx.Previous:=Next.Previous;
 if assigned(Next.Previous) then begin
  Next.Previous.Next:=vtx;
 end else begin
  Head:=vtx;
 enD;
 vtx.Next:=Next;
 Next.Previous:=vtx;
end;

function TKraftQuickHullVertexList.First:TKraftQuickHullVertex;
begin
 result:=Head;
end;

function TKraftQuickHullVertexList.IsEmpty:boolean;
begin
 result:=not assigned(Head);
end;

procedure TKraftQuickHullFaceList.Clear;
begin
 Head:=nil;
 Tail:=nil;
end;

procedure TKraftQuickHullFaceList.Add(vtx:TKraftQuickHullFace);
begin
 if assigned(Head) then begin
  Tail.fNext:=vtx;
 end else begin
  Head:=vtx;
 end;
 vtx.fNext:=nil;
 Tail:=vtx;
end;

function TKraftQuickHullFaceList.First:TKraftQuickHullFace;
begin
 result:=Head;
end;

function TKraftQuickHullFaceList.IsEmpty:boolean;
begin
 result:=not assigned(Head);
end;

constructor TKraftQuickHullVertex.Create(const AInstance:TKraftQuickHull);
begin
 inherited Create;
 fInstance:=AInstance;
 fInstance.fGarbageCollectedClassInstances.Add(self);
end;

destructor TKraftQuickHullVertex.Destroy;
begin
 fInstance.fGarbageCollectedClassInstances.Remove(self);
 inherited Destroy;
end;

constructor TKraftQuickHullFace.Create(const AInstance:TKraftQuickHull);
begin
 inherited Create;
 fInstance:=AInstance;
 fInstance.fGarbageCollectedClassInstances.Add(self);
 fhe0:=nil;
 fNormal.Init;
 fArea:=0;
 fCentroid.Init;
 fPlaneOffset:=0;
 fIndex:=0;
 fCountVertices:=0;
 fMark:=KRAFT_QUICKHULL_FACE_MARK_VISIBLE;
 fOutside:=nil;
end;

constructor TKraftQuickHullFace.CreatePolygon(const AInstance:TKraftQuickHull;const AVertices:array of TKraftQuickHullVertex;const AIndices:array of longint);
var i:longint;
    hePrev,he:TKraftQuickHullHalfEdge;
begin
 Create(AInstance);
 hePrev:=nil;
 for i:=0 to length(AIndices)-1 do begin
  he:=TKraftQuickHullHalfEdge.Create(fInstance,AVertices[AIndices[i]],self);
  if assigned(hePrev) then begin
   he.fPrevious:=hePrev;
   hePrev.fNext:=he;
  end else begin
   fhe0:=he;
  end;
  hePrev:=he;
 end;
 fhe0.fPrevious:=hePrev;
 hePrev.fNext:=fhe0;
 ComputeNormalAndCentroid;
end;

constructor TKraftQuickHullFace.CreateTriangle(const AInstance:TKraftQuickHull;const v0,v1,v2:TKraftQuickHullVertex;const AMinArea:double=0.0);
var he1,he2:TKraftQuickHullHalfEdge;
begin
 Create(AInstance);
 fhe0:=TKraftQuickHullHalfEdge.Create(fInstance,v0,self);
 he1:=TKraftQuickHullHalfEdge.Create(fInstance,v1,self);
 he2:=TKraftQuickHullHalfEdge.Create(fInstance,v2,self);
 fhe0.fPrevious:=he2;
 fhe0.fNext:=he1;
 he1.fPrevious:=fhe0;
 he1.fNext:=he2;
 he2.fPrevious:=he1;
 he2.fNext:=fhe0;
 ComputeNormalAndCentroid(AMinArea);
end;

destructor TKraftQuickHullFace.Destroy;
begin
 fInstance.fGarbageCollectedClassInstances.Remove(self);
 inherited Destroy;
end;

procedure TKraftQuickHullFace.ComputeCentroid(var ACentroid:TKraftQuickHullVector3D);
var he:TKraftQuickHullHalfEdge;
begin
 ACentroid.x:=0.0;
 ACentroid.y:=0.0;
 ACentroid.z:=0.0;
 he:=fhe0;
 repeat
  ACentroid.Add(he.Head.Point);
  he:=he.fNext;
 until he=fhe0;
 ACentroid.Scale(1.0/fCountVertices);
end;

procedure TKraftQuickHullFace.ComputeNormal(var ANormal:TKraftQuickHullVector3D);
var he1,he2:TKraftQuickHullHalfEdge;
    p0,p2:TKraftQuickHullVector3D;
    d2x,d2y,d2z,d1x,d1y,d1z:double;
begin
 he1:=fhe0.fNext;
 he2:=he1.fNext;
 p0:=fhe0.Head.Point;
 p2:=he1.Head.Point;
 d2x:=p2.x-p0.x;
 d2y:=p2.y-p0.y;
 d2z:=p2.z-p0.z;
 ANormal.x:=0.0;
 ANormal.y:=0.0;
 ANormal.z:=0.0;
 fCountVertices:=2;
 while he2<>fhe0 do begin
  d1x:=d2x;
  d1y:=d2y;
  d1z:=d2z;
  p2:=he2.Head.Point;
  d2x:=p2.x-p0.x;
  d2y:=p2.y-p0.y;
  d2z:=p2.z-p0.z;
  ANormal.x:=ANormal.x+((d1y*d2z)-(d1z*d2y));
  ANormal.y:=ANormal.y+((d1z*d2x)-(d1x*d2z));
  ANormal.z:=ANormal.z+((d1x*d2y)-(d1y*d2x));
//he1:=he2;
  he2:=he2.fNext;
  inc(fCountVertices);
 end;
 fArea:=ANormal.Length;
 ANormal.x:=ANormal.x/fArea;
 ANormal.y:=ANormal.y/fArea;
 ANormal.z:=ANormal.z/fArea;
end;

procedure TKraftQuickHullFace.ComputeNormal(var ANormal:TKraftQuickHullVector3D;const AMinArea:double);
var hedgeMax,hedge:TKraftQuickHullHalfEdge;
    LenSquaredMax,LenSquared,ux,uy,uz,Dot,lenMax:double;
    p1,p2:TKraftQuickHullVector3D;
begin
 ComputeNormal(ANormal);
 if fArea<AMinArea then begin
  hedgeMax:=nil;
  LenSquaredMax:=0;
  hedge:=fhe0;
  repeat
   LenSquared:=hedge.LengthSquared;
   if LenSquared>LenSquaredMax then begin
    hedgeMax:=hedge;
    LenSquaredMax:=LenSquared;
   end;
   hedge:=hedge.fnext;
  until hedge=fhe0;
  p2:=HedgeMax.Head.Point;
  p1:=HedgeMax.Tail.Point;
  lenMax:=sqrt(LenSquaredMax);
  ux:=(p2.x-p1.x)/lenMax;
  uy:=(p2.y-p1.y)/lenMax;
  uz:=(p2.z-p1.z)/lenMax;
  Dot:=(ANormal.x*ux)+(ANormal.y*uy)+(ANormal.z*uz);
  ANormal.x:=ANormal.x-(Dot*ux);
  ANormal.y:=ANormal.y-(Dot*uy);
  ANormal.z:=ANormal.z-(Dot*uz);
  ANormal.Normalize();
 end;
end;

procedure TKraftQuickHullFace.ComputeNormalAndCentroid;
var he:TKraftQuickHullHalfEdge;
    CountFoundVertices:longint;
begin
 ComputeNormal(fNormal);
 ComputeCentroid(fCentroid);
 fPlaneOffset:=fNormal.Dot(fCentroid);
 CountFoundVertices:=0;
 he:=fhe0;
 repeat
  inc(CountFoundVertices);
  he:=he.fNext;
 until he=fhe0;
 Assert(CountFoundVertices=fCountVertices,'Wrong count of vertices');
end;

procedure TKraftQuickHullFace.ComputeNormalAndCentroid(const AMinArea:double);
begin
 ComputeNormal(fNormal,AMinArea);
 ComputeCentroid(fCentroid);
 fPlaneOffset:=fNormal.Dot(fCentroid);
end;

function TKraftQuickHullFace.GetEdge(i:longint):TKraftQuickHullHalfEdge;
begin
 result:=fhe0;
 while i>0 do begin
  result:=result.fNext;
  dec(i);
 end;
 while i<0 do begin
  result:=result.fPrevious;
  inc(i);
 end;
end;

function TKraftQuickHullFace.GetFirstEdge:TKraftQuickHullHalfEdge;
begin
 result:=fhe0;
end;

function TKraftQuickHullFace.FindEdge(const vt,vh:TKraftQuickHullVertex):TKraftQuickHullHalfEdge;
begin
 result:=fhe0;
 repeat
  if (result.Head=vh) and (result.Tail=vt) then begin
   exit;
  end;
  result:=result.fNext;
 until result=fhe0;
 result:=nil;
end;

function TKraftQuickHullFace.DistanceToPlane(const p:TKraftQuickHullVector3D):double;
begin
 result:=((fNormal.x*p.x)+(fNormal.y*p.y)+(fNormal.z*p.z))-fPlaneOffset;
end;

function TKraftQuickHullFace.ConnectHalfEdges(const hedgePrev,hedge:TKraftQuickHullHalfEdge):TKraftQuickHullFace;
var oppFace:TKraftQuickHullFace;
    hedgeOpp:TKraftQuickHullHalfEdge;
begin
 result:=nil;
 if hedgePrev.OppositeFace=hedge.OppositeFace then begin
  // then there is a redundant edge that we can get rid off
  oppFace:=hedge.OppositeFace;
  if hedgePrev=fhe0 then begin
   fhe0:=hedge;
  end;
  if oppFace.fCountVertices=3 then begin
   // then we can get rid of the opposite Face altogether
   hedgeOpp:=hedge.fOpposite.fPrevious.fOpposite;
   oppFace.fMark:=KRAFT_QUICKHULL_FACE_MARK_NON_DELETED;
   result:=oppFace;
  end else begin
   hedgeOpp:=hedge.fOpposite.fNext;
   if oppFace.fhe0=hedgeOpp.fPrevious then begin
    oppFace.fhe0:=hedgeOpp;
   end;
   hedgeOpp.fPrevious:=hedgeOpp.fPrevious.fPrevious;
   hedgeOpp.fPrevious.fNext:=hedgeOpp;
  end;
  hedge.fPrevious:=hedgePrev.fPrevious;
  hedge.fPrevious.fNext:=hedge;
  hedge.fOpposite:=hedgeOpp;
  hedgeOpp.fOpposite:=hedge;
  oppFace.ComputeNormalAndCentroid; // oppFace was modified, so need to recompute
 end else begin
  hedgePrev.fNext:=hedge;
  hedge.fPrevious:=hedgePrev;
 end;
end;

procedure TKraftQuickHullFace.CheckConsistency;
var hedge,hedgeOpp:TKraftQuickHullHalfEdge;
    oppFace:TKraftQuickHullFace;
    MaxD,d:double;
    CountFoundVertices:longint;
begin
 Assert(fCountVertices>2,'Degenerated Face');
 MaxD:=0.0;
 CountFoundVertices:=0;
 hedge:=fhe0;
 repeat
  hedgeOpp:=hedge.fOpposite;
  Assert(assigned(hedgeOpp),'Unreflected half edge');
  Assert(hedgeOpp.fOpposite<>hedgeOpp,'Opposite half edge has wrong opposite half edge');
  Assert((hedgeOpp.Head=hedge.Tail) and (hedge.Head=hedgeOpp.tail),'Wrong reflected half edge');
  oppFace:=hedgeOpp.fFace;
  Assert(assigned(oppFace),'Opposite half edge has no Face');
  Assert(oppFace.fMark<>KRAFT_QUICKHULL_FACE_MARK_NON_DELETED,'Opposite half edge Face isn''t on hull');
  d:=abs(DistanceToPlane(Hedge.Head.Point));
  if d>MaxD then begin
   MaxD:=d;
  end;
  inc(CountFoundVertices);
  hedge:=hedge.fNext;
 until hedge=fhe0;
 Assert(CountFoundVertices=fCountVertices,'Wrong count of vertices');
end;

function TKraftQuickHullFace.MergeAdjacentFace(const hedgeAdj:TKraftQuickHullHalfEdge;const Discarded:TList):longint;
var oppFace,DiscardedFace:TKraftQuickHullFace;
    hedgeOpp,hedgeAdjPrev,hedgeAdjNext,hedgeOppPrev,hedgeOppNext,hedge:TKraftQuickHullHalfEdge;
begin
 oppFace:=hedgeAdj.OppositeFace;
 result:=0;

 Discarded.Clear;
 
 Discarded.Add(oppFace);
 inc(result);
 oppFace.fMark:=KRAFT_QUICKHULL_FACE_MARK_NON_DELETED;

 hedgeOpp:=hedgeAdj.fOpposite;

 hedgeAdjPrev:=hedgeAdj.fPrevious;
 hedgeAdjNext:=hedgeAdj.fNext;
 hedgeOppPrev:=hedgeOpp.fPrevious;
 hedgeOppNext:=hedgeOpp.fNext;

 while hedgeAdjPrev.OppositeFace=oppFace do begin
  hedgeAdjPrev:=hedgeAdjPrev.fPrevious;
  hedgeOppNext:=hedgeOppNext.fNext;
 end;

 while hedgeAdjNext.OppositeFace=oppFace do begin
  hedgeOppPrev:=hedgeOppPrev.fPrevious;
  hedgeAdjNext:=hedgeAdjNext.fNext;
 end;

 hedge:=hedgeOppNext;       
 while hedge<>hedgeOppPrev.fNext do begin
  hedge.fFace:=self;
  hedge:=hedge.fNext;
 end;

 if hedgeAdj=fhe0 then begin
  fhe0:=hedgeAdjNext;
 end;

 // handle the half edges at the head
 DiscardedFace:=ConnectHalfEdges(hedgeOppPrev,hedgeAdjNext);
 if assigned(DiscardedFace) then begin
  Discarded.Add(DiscardedFace);
  inc(result);
 end;

 // handle the half edges at the tail
 DiscardedFace:=ConnectHalfEdges(hedgeAdjPrev,hedgeOppNext);
 if assigned(DiscardedFace) then begin
  Discarded.Add(DiscardedFace);
  inc(result);
 end;

 ComputeNormalAndCentroid;
 CheckConsistency;

end;

function TKraftQuickHullFace.AreaSquared(const hedge0,hedge1:TKraftQuickHullHalfEdge):double;
var p0,p1,p2:TKraftQuickHullVector3D;
    dx1,dy1,dz1,dx2,dy2,dz2,x,y,z:double;
begin
 p0:=hedge0.Tail.Point;
 p1:=hedge0.Head.Point;
 p2:=hedge1.Head.Point;
 dx1:=p1.x-p0.x;
 dy1:=p1.y-p0.y;
 dz1:=p1.z-p0.z;
 dx2:=p2.x-p0.x;
 dy2:=p2.y-p0.y;
 dz2:=p2.z-p0.z;
 x:=(dy1*dz2)-(dz1*dy2);
 y:=(dz1*dx2)-(dx1*dz2);
 z:=(dx1*dy2)-(dy1*dx2);
 result:=sqr(x)+sqr(y)+sqr(z);
end;

procedure TKraftQuickHullFace.Triangulate(var NewFaces:TKraftQuickHullFaceList;const MinArea:double);
var hedge,oppPrev:TKraftQuickHullHalfEdge;
    face0,Face:TKraftQuickHullFace;
    v0:TKraftQuickHullVertex;
begin
 if fCountVertices>3 then begin
  v0:=fhe0.Head;
  hedge:=fhe0.fNext;
  oppPrev:=hedge.fOpposite;
  face0:=nil;
  hedge:=hedge.fNext;
  while hedge<>fhe0.fPrevious do begin
   Face:=TKraftQuickHullFace.CreateTriangle(fInstance,v0,hedge.fPrevious.Head,hedge.Head,MinArea);
   Face.fhe0.fNext.SetOpposite(oppPrev);
   Face.fhe0.fPrevious.SetOpposite(hedge.fOpposite);
   oppPrev:=Face.fhe0;
   NewFaces.Add(Face);
   if not assigned(face0) then begin
    face0:=Face;
   end;
   hedge:=hedge.fNext;
  end;
  hedge:=TKraftQuickHullHalfEdge.Create(fInstance,fhe0.fPrevious.fPrevious.Head,self);
  hedge.SetOpposite(oppPrev);
  hedge.fPrevious:=fhe0;
  hedge.fPrevious.fNext:=hedge;
  hedge.fNext:=fhe0.fPrevious;
  hedge.fNext.fPrevious:=hedge;
  ComputeNormalAndCentroid(MinArea);
  CheckConsistency;
  Face:=face0;
  while assigned(Face) do begin
   Face.CheckConsistency;
   Face:=Face.fNext;
  end;
 end;
end;

constructor TKraftQuickHullHalfEdge.Create(const AInstance:TKraftQuickHull;const v:TKraftQuickHullVertex=nil;const f:TKraftQuickHullFace=nil);
begin
 inherited Create;
 fInstance:=AInstance;
 fInstance.fGarbageCollectedClassInstances.Add(self);
 fVertex:=v;
 fFace:=f;
 fNext:=nil;
 fPrevious:=nil;
 fOpposite:=nil;
end;

destructor TKraftQuickHullHalfEdge.Destroy;
begin
 fInstance.fGarbageCollectedClassInstances.Remove(self);
 inherited Destroy;
end;

procedure TKraftQuickHullHalfEdge.SetOpposite(const Edge:TKraftQuickHullHalfEdge);
begin
 fOpposite:=Edge;
 Edge.fOpposite:=self;
end;

function TKraftQuickHullHalfEdge.Head:TKraftQuickHullVertex;
begin
 result:=fVertex;
end;

function TKraftQuickHullHalfEdge.Tail:TKraftQuickHullVertex;
begin
 if assigned(fPrevious) then begin
  result:=fPrevious.fVertex;
 end else begin
  result:=nil;
 end;
end;

function TKraftQuickHullHalfEdge.OppositeFace:TKraftQuickHullFace;
begin
 if assigned(fOpposite) then begin
  result:=fOpposite.fFace;
 end else begin
  result:=nil;
 end;
end;

function TKraftQuickHullHalfEdge.Length:double;
begin
 if assigned(fPrevious) then begin
  result:=fVertex.Point.Distance(fPrevious.fVertex.Point);
 end else begin
  result:=-1.0;
 end;
end;

function TKraftQuickHullHalfEdge.LengthSquared:double;
begin
 if assigned(fPrevious) then begin
  result:=fVertex.Point.DistanceSquared(fPrevious.fVertex.Point);
 end else begin
  result:=-1.0;
 end;
end;

constructor TKraftQuickHull.Create;
begin
 inherited Create;
 fGarbageCollectedClassInstances:=TList.Create;
 fFindIndex:=-1;
 fPointBuffer:=TList.Create;
 fVertexPointIndices:=TKraftQuickHullIntegerList.Create;
 fDiscardedFaces:=TList.Create;
 FillChar(fMinVertices,SizeOf(TKraftQuickHullThreeVertices),#0);
 FillChar(fMaxVertices,SizeOf(TKraftQuickHullThreeVertices),#0);
 FillChar(fVertexHashTable,SizeOf(TKraftQuickHullVertexHashTable),#0);
 fFaces:=TList.Create;
 fHorizon:=TList.Create;
 fNewFaces.Clear;
 fUnclaimed.Clear;
 fClaimed.Clear;
 fCountVertices:=0;
 fCountFaces:=0;
 fCountPoints:=0;
 fExplicitTolerance:=KRAFT_QUICKHULL_AUTOMATIC_TOLERANCE;
 fTolerance:=0.0;
end;

destructor TKraftQuickHull.Destroy;
begin
 fPointBuffer.Free;
 fVertexPointIndices.Free;
 fDiscardedFaces.Free;
 fFaces.Free;
 fHorizon.Free;
 while fGarbageCollectedClassInstances.Count>0 do begin
  TObject(fGarbageCollectedClassInstances[fGarbageCollectedClassInstances.Count-1]).Free;
 end;
 fGarbageCollectedClassInstances.Free;
 inherited Destroy;
end;

procedure TKraftQuickHull.Reset;
begin
 fPointBuffer.Clear;
 fVertexPointIndices.Clear;
 fFaces.Clear;
 fClaimed.Clear;
 fCountFaces:=0;
 fCountPoints:=0;
end;

procedure TKraftQuickHull.AddPoint(const x,y,z:double);
var Vertex:TKraftQuickHullVertex;
    Index,HashBucket:longint;
begin
 HashBucket:=((round(x)*73856093) xor (round(y)*19349663) xor (round(z)*83492791)) and KRAFT_QUICKHULL_HASHMASK;
 Vertex:=fVertexHashTable[HashBucket];
 while assigned(Vertex) do begin
  if KraftQuickHullIsSameValue(Vertex.Point.x,x) and
     KraftQuickHullIsSameValue(Vertex.Point.y,y) and
     KraftQuickHullIsSameValue(Vertex.Point.z,z) then begin
   exit;
  end;
  Vertex:=Vertex.HashNext;
 end;
 Vertex:=TKraftQuickHullVertex.Create(self);
 Vertex.fPoint.x:=x;
 Vertex.fPoint.y:=y;
 Vertex.fPoint.z:=z;
 Vertex.HashNext:=fVertexHashTable[HashBucket];
 fVertexHashTable[HashBucket]:=Vertex;
 Index:=fPointBuffer.Add(Vertex);
 Vertex.Index:=Index;
 fVertexPointIndices.Add(Index);
 inc(fCountPoints);
end;

procedure TKraftQuickHull.AddPointToFace(const AVertex:TKraftQuickHullVertex;const AFace:TKraftQuickHullFace);
begin
 AVertex.Face:=AFace;
 if assigned(AFace.fOutside) then begin
  fClaimed.InsertBefore(AVertex,AFace.fOutside);
 end else begin
  fClaimed.Add(AVertex);
 end;
 AFace.fOutside:=AVertex;
end;

procedure TKraftQuickHull.RemovePointFromFace(const AVertex:TKraftQuickHullVertex;const AFace:TKraftQuickHullFace);
begin
 if AVertex=AFace.fOutside then begin
  if assigned(AVertex.Next) and (AVertex.Next.Face=AFAce) then begin
   AFace.fOutside:=AVertex.Next;
  end else begin
   AFace.fOutside:=nil;
  end;
 end;
 fClaimed.Delete(AVertex);
end;

function TKraftQuickHull.RemoveAllPointsFromFace(const AFace:TKraftQuickHullFace):TKraftQuickHullVertex;
var EndVertex:TKraftQuickHullVertex;
begin
 if assigned(AFace.fOutside) then begin
  EndVertex:=AFace.fOutside;
  while assigned(EndVertex.Next) and (EndVertex.Next.Face=AFace) do begin
   EndVertex:=EndVertex.Next;
  end;
  fClaimed.Delete(AFace.fOutside,EndVertex);
  EndVertex.Next:=nil;
  result:=AFace.fOutside;
 end else begin
  result:=nil;
 end;
end;

function TKraftQuickHull.FindHalfEdge(const Tail,Head:TKraftQuickHullVertex):TKraftQuickHullHalfEdge;
var i:longint;
    Face:TKraftQuickHullFace;
begin
 result:=nil;
 for i:=0 to fFaces.Count-1 do begin
  Face:=fFaces[i];
  result:=Face.FindEdge(Tail,Head);
  if assigned(result) then begin
   exit;
  end;
 end;
end;

procedure TKraftQuickHull.Triangulate;
const DOUBLE_PREC=2.2204460492503131e-16;
var i:longint;
    Face:TKraftQuickHullFace;
    MinArea:double;
begin
 MinArea:=1000.0*fCharLength*DOUBLE_PREC;
 fNewFaces.Clear;
 for i:=0 to fFaces.Count-1 do begin
  Face:=fFaces[i];
  if Face.fMark=KRAFT_QUICKHULL_FACE_MARK_VISIBLE then begin
   Face.Triangulate(fNewFaces,MinArea);
  end;
 end;
 Face:=fNewFaces.First;
 while assigned(Face) do begin
  fFaces.Add(Face);
  Face:=Face.fNext;
 end;
 ReindexFacesAndVertices;
end;

procedure TKraftQuickHull.ComputeMinAndMax;
const DOUBLE_PREC=2.2204460492503131e-16;
var i:longint;
    MinVector,MaxVector,Point:TKraftQuickHullVector3D;
    Vertex:TKraftQuickHullVertex;
begin
 for i:=0 to 2 do begin
  fMinVertices[i]:=fPointBuffer[0];
  fMaxVertices[i]:=fPointBuffer[0];
 end;
 Vertex:=fPointBuffer[0];
 MinVector:=Vertex.Point;
 MaxVector:=Vertex.Point;
 for i:=1 to fCountPoints-1 do begin
  Vertex:=fPointBuffer[i];
  Point:=Vertex.Point;
  if MinVector.x>Point.x then begin
   MinVector.x:=Point.x;
   fMinVertices[0]:=Vertex;
  end;
  if MaxVector.x<Point.x then begin
   MaxVector.x:=Point.x;
   fMaxVertices[0]:=Vertex;
  end;
  if MinVector.y>Point.y then begin
   MinVector.y:=Point.y;
   fMinVertices[1]:=Vertex;
  end;
  if MaxVector.y<Point.y then begin
   MaxVector.y:=Point.y;
   fMaxVertices[1]:=Vertex;
  end;
  if MinVector.z>Point.z then begin
   MinVector.z:=Point.z;
   fMinVertices[2]:=Vertex;
  end;
  if MaxVector.z<Point.z then begin
   MaxVector.z:=Point.z;
   fMaxVertices[2]:=Vertex;
  end;
 end;                                                                  
 fCharLength:=Max(Max(MaxVector.x-MinVector.x,MaxVector.y-MinVector.y),MaxVector.z-MinVector.z);
 if fExplicitTolerance<0.0 then begin
  fTolerance:=(3.0*DOUBLE_PREC)*(Max(abs(MaxVector.x),abs(MinVector.x))+Max(abs(MaxVector.y),abs(MinVector.y))+Max(abs(MaxVector.z),abs(MinVector.z)));
 end else begin
  fTolerance:=fExplicitTolerance;
 end;
end;
                    
procedure TKraftQuickHull.CreateInitialSimplex;
const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
var MaxIndex,Index,OtherIndex:longint;
    MaxValue,Diff,MaxSquared,LenSquared,MaxDistance,d0,Distance:double;
    v:TKraftQuickHullVertex;
    MaxFace:TKraftQuickHullFace;
    Vertices:array[0..3] of TKraftQuickHullVertex;
    Triangles:array[0..3] of TKraftQuickHullFace;
    u01,Diff02,Normal,CrossProduct:TKraftQuickHullVector3D;
begin

 MaxValue:=0.0;
 MaxIndex:=0;
 for Index:=0 to 2 do begin
  Diff:=fMaxVertices[Index].Point.GetValue(Index)-fMinVertices[Index].Point.GetValue(Index);
  if MaxValue<Diff then begin
   MaxValue:=Diff;
   MaxIndex:=Index;
  end;
 end;

 if MaxValue<=fTolerance then begin
  raise EKraftQuickHull.Create('Input points appear to be coincident');
 end;

 // set first two vertices to be those with the greatest one dimensional separation
 Vertices[0]:=fMaxVertices[MaxIndex];
 Vertices[1]:=fMinVertices[MaxIndex];

 // set third vertex to be the vertex farthest from the line between v0 and v1
 u01.Sub(Vertices[1].Point,Vertices[0].Point);
 u01.Normalize;
   
 MaxSquared:=0.0;
 for Index:=0 to fCountPoints-1 do begin
  Diff02.Sub(TKraftQuickHullVertex(fPointBuffer[Index]).Point,Vertices[0].Point);
  CrossProduct.CrossProduct(u01,Diff02);
  LenSquared:=CrossProduct.LengthSquared;
  if (MaxSquared<LenSquared) and (fPointBuffer[Index]<>Vertices[0]) and (fPointBuffer[Index]<>Vertices[1]) then begin
   MaxSquared:=LenSquared;
   Vertices[2]:=fPointBuffer[Index];
   Normal:=CrossProduct;
  end;
 end;
 if sqrt(MaxSquared)<=(100.0*fTolerance) then begin
  raise EKraftQuickHull.Create('Input points appear to be colinear');
 end;
 Normal.Normalize();

 MaxDistance:=0;
 d0:=Vertices[2].Point.Dot(Normal);
 for Index:=0 to fCountPoints-1 do begin
  Distance:=abs(TKraftQuickHullVertex(fPointBuffer[Index]).Point.Dot(Normal)-d0);
  if (MaxDistance<Distance) and (fPointBuffer[Index]<>Vertices[0]) and (fPointBuffer[Index]<>Vertices[1]) and (fPointBuffer[Index]<>Vertices[2]) then begin
   MaxDistance:=Distance;
   Vertices[3]:=fPointBuffer[Index];
  end;
 end;
 if sqrt(MaxDistance)<=(100.0*fTolerance) then begin
  raise EKraftQuickHull.Create('Input points appear to be coplanar');
 end;

 if (Vertices[3].Point.Dot(Normal)-d0)<0.0 then begin
  Triangles[0]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[0],Vertices[1],Vertices[2]);
  Triangles[1]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[3],Vertices[1],Vertices[0]);
  Triangles[2]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[3],Vertices[2],Vertices[1]);
  Triangles[3]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[3],Vertices[0],Vertices[2]);
  for Index:=0 to 2 do begin
   OtherIndex:=ModuloThree[Index+1];
   Triangles[Index+1].fhe0.fNext.SetOpposite(Triangles[OtherIndex+1].fhe0);
   Triangles[Index+1].fhe0.fNext.fNext.SetOpposite(Triangles[0].GetEdge(OtherIndex));
  end;
 end else begin
  Triangles[0]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[0],Vertices[2],Vertices[1]);
  Triangles[1]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[3],Vertices[0],Vertices[1]);
  Triangles[2]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[3],Vertices[1],Vertices[2]);
  Triangles[3]:=TKraftQuickHullFace.CreateTriangle(self,Vertices[3],Vertices[2],Vertices[0]);
  for Index:=0 to 2 do begin
   OtherIndex:=ModuloThree[Index+1];
   Triangles[Index+1].fhe0.SetOpposite(Triangles[OtherIndex+1].fhe0.fNext);
   Triangles[Index+1].fhe0.fNext.fNext.SetOpposite(Triangles[0].GetEdge(ModuloThree[3-Index]));
  end;
 end;

 for Index:=0 to 3 do begin
  fFaces.Add(Triangles[Index]);
 end;

 for Index:=0 to fCountPoints-1 do begin
  v:=fPointBuffer[Index];
  if (v<>Vertices[0]) and (v<>Vertices[1]) and (v<>Vertices[2]) and (v<>Vertices[3]) then begin
   MaxDistance:=fTolerance;
   MaxFace:=nil;
   for OtherIndex:=0 to 3 do begin
    Distance:=Triangles[OtherIndex].DistanceToPlane(v.Point);
    if MaxDistance<Distance then begin
     MaxDistance:=Distance;
     MaxFace:=Triangles[OtherIndex];
    end;
   end;
   if assigned(MaxFace) then begin
    AddPointToFace(v,MaxFace);
   end;
  end;
 end;

end;

function TKraftQuickHull.NextPointToAdd:TKraftQuickHullVertex;
var EyeFace:TKraftQuickHullFace;
    Vertex:TKraftQuickHullVertex;
    MaxDistance,Distance:double;
begin
 result:=nil;
 if not fClaimed.IsEmpty then begin
  EyeFace:=fClaimed.First.Face;
  MaxDistance:=0.0;
  Vertex:=EyeFace.fOutside;
  while assigned(Vertex) and (Vertex.Face=EyeFace) do begin
   Distance:=EyeFace.DistanceToPlane(Vertex.Point);
   if MaxDistance<Distance then begin
    MaxDistance:=Distance;
    result:=Vertex;
   end;
   Vertex:=Vertex.Next;
  end;
 end;
end;

procedure TKraftQuickHull.DeleteFacePoints(Face,AbsorbingFace:TKraftQuickHullFace);
var FaceVertices,NextVertex,Vertex:TKraftQuickHullVertex;
begin
 FaceVertices:=RemoveAllPointsFromFace(Face);
 if assigned(FaceVertices) then begin
  if assigned(AbsorbingFace) then begin
   NextVertex:=FaceVertices;
   Vertex:=NextVertex;
   while assigned(Vertex) do begin
    NextVertex:=Vertex.Next;
    if AbsorbingFace.DistanceToPlane(Vertex.Point)>fTolerance then begin
     AddPointToFace(Vertex,AbsorbingFace);
    end else begin
     fUnclaimed.Add(Vertex);
    end;
    Vertex:=NextVertex;
   end;
  end else begin
   fUnclaimed.AddAll(FaceVertices);
  end;
 end;
end;

procedure TKraftQuickHull.CalculateHorizon(const EyePoint:TKraftQuickHullVector3D;Edge0:TKraftQuickHullHalfEdge;const Face:TKraftQuickHullFace;const Horizon:TList);
var Edge:TKraftQuickHullHalfEdge;
    oppFace:TKraftQuickHullFace;
begin
 //OldFaces.Add(Face);
 DeleteFacePoints(Face,nil);
 Face.fMark:=KRAFT_QUICKHULL_FACE_MARK_NON_DELETED;
 if assigned(Edge0) then begin
  Edge:=Edge0.fNext;
 end else begin
  Edge0:=Face.fhe0;
  Edge:=Edge0;
 end;
 Assert(assigned(Edge.fPrevious),'Edge.Previous is nil');
 Assert(Edge.fPrevious.fNext=Edge,'Edge.Previous.Next mismatches Edge');
 repeat
  oppFace:=Edge.OppositeFace;
  if oppFace.fMark=KRAFT_QUICKHULL_FACE_MARK_VISIBLE then begin
   if oppFace.DistanceToPlane(EyePoint)>fTolerance then begin
    CalculateHorizon(EyePoint,Edge.fOpposite,oppFace,Horizon);
   end else begin
    Horizon.Add(Edge);
   end;
  end;
  Edge:=Edge.fNext;
 until Edge=Edge0;
end;

function TKraftQuickHull.AddAdjoiningFace(const EyeVertex:TKraftQuickHullVertex;const he:TKraftQuickHullHalfEdge):TKraftQuickHullHalfEdge;
var Face:TKraftQuickHullFace;
begin
 Face:=TKraftQuickHullFace.CreateTriangle(self,EyeVertex,he.Tail,he.Head);
 fFaces.Add(Face);
 Face.fhe0.fPrevious.SetOpposite(he.fOpposite);
 result:=Face.fhe0;
end;                            

procedure TKraftQuickHull.AddNewFaces(const NewFaces:TKraftQuickHullFaceList;const EyeVertex:TKraftQuickHullVertex;const Horizon:TList);
var Index:longint;
    hedgeSidePrev,hedgeSideBegin,horizonHe,hedgeSide:TKraftQuickHullHalfEdge;
begin
 NewFaces.clear;
 hedgeSidePrev:=nil;
 hedgeSideBegin:=nil;
 for Index:=0 to Horizon.Count-1 do begin
  horizonHe:=Horizon[Index];
  hedgeSide:=AddAdjoiningFace(EyeVertex,horizonHe);
  if assigned(hedgeSidePrev) then begin
   hedgeSide.fNext.SetOpposite(hedgeSidePrev);
  end else begin
   hedgeSideBegin:=hedgeSide;
  end;
  NewFaces.Add(hedgeSide.fFace);
  hedgeSidePrev:=hedgeSide;
 end;
 hedgeSideBegin.fNext.setOpposite(hedgeSidePrev);
end;

function TKraftQuickHull.OppFaceDistance(he:TKraftQuickHullHalfEdge):double;
begin
 result:=he.fFace.DistanceToPlane(he.fOpposite.fFace.fCentroid);
end;

function TKraftQuickHull.DoAdjacentMerge(const Face:TKraftQuickHullFace;const MergeType:longint):boolean;
var Index,CountDiscarded:longint;
    hedge:TKraftQuickHullHalfEdge;
    oppFace:TKraftQuickHullFace;
    Convex,Merge:boolean;
begin
 hedge:=Face.fhe0;
 Convex:=true;
 repeat
  oppFace:=hedge.OppositeFace;
  Merge:=false;          
  if MergeType=KRAFT_QUICKHULL_MERGE_TYPE_NONCONVEX then begin
   // merge faces if they are definitively non-convex
   if (OppFaceDistance(hedge)>(-fTolerance)) or (OppFaceDistance(hedge.fOpposite)>(-fTolerance)) then begin
    Merge:=true;
   end;
  end else{if MergeType=KRAFT_QUICKHULL_MERGE_TYPE_NONCONVEX_WRT_LARGER_FACE then}begin
   // merge faces if they are parallel or non-convex wrt to the larger face; otherwise, just mark the face non-convex for the second pass.
   if Face.fArea>oppFace.fArea then begin
    if OppFaceDistance(hedge)>(-fTolerance) then begin
     Merge:=true;
    end else if OppFaceDistance(hedge.fOpposite)>(-fTolerance) then begin
     Convex:=false;
    end;
   end else begin
    if OppFaceDistance(hedge.fOpposite)>(-fTolerance) then begin
     Merge:=true;
    end else if OppFaceDistance(hedge)>(-fTolerance) then begin
     Convex:=false;
    end;
   end;
  end;
  if Merge then begin
   CountDiscarded:=Face.MergeAdjacentFace(hedge,fDiscardedFaces);
   for Index:=0 to CountDiscarded-1 do begin
    DeleteFacePoints(fDiscardedFaces[Index],Face);
   end;
   result:=true;
   exit;
  end;
  hedge:=hedge.fNext;
 until hedge=Face.fhe0;
 if not Convex then begin
  Face.fMark:=KRAFT_QUICKHULL_FACE_MARK_NON_CONVEX;
 end;
 result:=false;
end;

procedure TKraftQuickHull.ResolveUnclaimedPoints(NewFaces:TKraftQuickHullFaceList);
var NextVertex,Vertex:TKraftQuickHullVertex;
    MaxFace,NewFace:TKraftQuickHullFace;
    MaxDistance,Distance:double;
begin
 NextVertex:=fUnclaimed.First;
 Vertex:=NextVertex;
 while assigned(Vertex) do begin
  NextVertex:=Vertex.Next;
  MaxDistance:=fTolerance;
  MaxFace:=nil;
  NewFace:=NewFaces.First;
  while assigned(NewFace) do begin
   if NewFace.fMark=KRAFT_QUICKHULL_FACE_MARK_VISIBLE then begin
    Distance:=NewFace.DistanceToPlane(Vertex.Point);
    if MaxDistance<Distance then begin
     MaxDistance:=Distance;
     MaxFace:=NewFace;
    end;
    if MaxDistance>(1000.0*fTolerance) then begin
     break;
    end;
   end;
   NewFace:=NewFace.fNext;
  end;
  if assigned(MaxFace) then begin
   AddPointToFace(Vertex,MaxFace);
  end;
  Vertex:=NextVertex;
 end;
end;

procedure TKraftQuickHull.AddPointToHull(const EyeVertex:TKraftQuickHullVertex);
var Face:TKraftQuickHullFace;
begin
 fHorizon.Clear;
 fUnclaimed.Clear;

 RemovePointFromFace(EyeVertex,EyeVertex.Face);
 CalculateHorizon(EyeVertex.Point,nil,EyeVertex.Face,fHorizon);
 fNewFaces.Clear;
 AddNewFaces(fNewFaces,EyeVertex,fHorizon);

 // first merge pass ... merge faces which are non-convex as determined by the larger face
 Face:=fNewFaces.First;
 while assigned(Face) do begin
  if Face.fMark=KRAFT_QUICKHULL_FACE_MARK_VISIBLE then begin
   while DoAdjacentMerge(Face,KRAFT_QUICKHULL_MERGE_TYPE_NONCONVEX_WRT_LARGER_FACE) do begin
   end;
  end;
  Face:=Face.fNext;
 end;

 // second merge pass ... merge faces which are non-convex wrt either face
 Face:=fNewFaces.First;
 while assigned(Face) do begin
  if Face.fMark=KRAFT_QUICKHULL_FACE_MARK_NON_CONVEX then begin
   Face.fMark:=KRAFT_QUICKHULL_FACE_MARK_VISIBLE;
   while DoAdjacentMerge(Face,KRAFT_QUICKHULL_MERGE_TYPE_NONCONVEX) do begin
   end;
  end;
  Face:=Face.fNext;
 end;

 ResolveUnclaimedPoints(fNewFaces);

end;

procedure TKraftQuickHull.MarkFaceVertices(const Face:TKraftQuickHullFace;const Mark:longint);
var he0,he:TKraftQuickHullHalfEdge;
begin
 he0:=Face.GetFirstEdge;
 he:=he0;
 repeat
  he.Head.Index:=Mark;
  he:=he.fNext;
 until he=he0;
end;

procedure TKraftQuickHull.ReindexFacesAndVertices;
var Index:longint;
    Face:TKraftQuickHullFace;
    Vertex:TKraftQuickHullVertex;
begin
 for Index:=0 to fCountPoints-1 do begin
  TKraftQuickHullVertex(fPointBuffer[Index]).Index:=-1;
 end;

 // remove inactive faces and mark active vertices
 fCountFaces:=0;
 Index:=0;
 while Index<fFaces.Count do begin
  Face:=fFaces[Index];
  if Face.fMark<>KRAFT_QUICKHULL_FACE_MARK_VISIBLE then begin
   fFaces.Delete(Index);
  end else begin
   MarkFaceVertices(Face,0);
   inc(fCountFaces);
   inc(Index);
  end;
 end;

 // reindex vertices
 fCountVertices:=0;
 for Index:=0 to fCountPoints-1 do begin
  Vertex:=fPointBuffer[Index];
  if Vertex.Index=0 then begin
   fVertexPointIndices[fCountVertices]:=Index;
   Vertex.Index:=fCountVertices;
   inc(fCountVertices);
  end;
 end;

end;

procedure TKraftQuickHull.Build(const MaximumVertices:longint=-1);
var EyeVertex:TKraftQuickHullVertex;
    RemainToDoVertices:longint;
begin
 ComputeMinAndMax;
 CreateInitialSimplex;
 EyeVertex:=NextPointToAdd;
 RemainToDoVertices:=MaximumVertices;
 while assigned(EyeVertex) and (RemainToDoVertices<>0) do begin
  AddPointToHull(EyeVertex);
  EyeVertex:=NextPointToAdd;
  if RemainToDoVertices>0 then begin
   dec(RemainToDoVertices);
  end;
 end;
 ReindexFacesAndVertices;
end;

procedure TKraftQuickHull.GetFaceIndices(out OutputFace:TKraftQuickHullOutputFace;const Face:TKraftQuickHullFace;const Flags:longint);
var CCW,IndexedFromOne,PointRelative:boolean;
    hedge:TKraftQuickHullHalfEdge;
    Count,Index:longint;
begin
 SetLength(OutputFace,Face.fCountVertices);
 CCW:=(Flags and KRAFT_QUICKHULL_FACE_FLAG_CLOCKWISE)=0;
 IndexedFromOne:=(Flags and KRAFT_QUICKHULL_FACE_FLAG_INDEXED_FROM_ONE)<>0;
 PointRelative:=(Flags and KRAFT_QUICKHULL_FACE_FLAG_POINT_RELATIVE)<>0;
 hedge:=Face.fhe0;
 Count:=0;
 repeat
  Index:=hedge.Head.Index;
  if PointRelative then begin
   Index:=fVertexPointIndices[Index];
  end;
  if IndexedFromOne then begin
   inc(Index);
  end;
  OutputFace[Count]:=Index;
  inc(Count);
  if CCW then begin
   hedge:=hedge.fNext;
  end else begin
   hedge:=hedge.fPrevious;
  end;
 until hedge=Face.fhe0;
end;

procedure TKraftQuickHull.GetVertices(out OutputVertices:TKraftQuickHullVector3DArray);
var Index:longint;
begin
 SetLength(OutputVertices,fCountVertices);
 for Index:=0 to fCountVertices-1 do begin
  OutputVertices[Index]:=TKraftQuickHullVertex(fPointBuffer[fVertexPointIndices[Index]]).Point;
 end;
end;

procedure TKraftQuickHull.GetFaces(out OutputFaces:TKraftQuickHullOutputFaces);
var Index:longint;
begin
 SetLength(OutputFaces,fCountFaces);
 for Index:=0 to fCountFaces-1 do begin
  GetFaceIndices(OutputFaces[Index],fFaces[Index],0);
 end;
end;

type PConvexHullGrahamScanVector=^TConvexHullGrahamScanVector;
     TConvexHullGrahamScanVector=record
      OriginalIndex:longint;
      Angle:double;
      LengthToAnchor:double;
      Vector:TConvexHullVector;
     end;

     TConvexHullGrahamScanVectors=array of TConvexHullGrahamScanVector;

function CompareConvexHullGrahamScanVector(const a,b:pointer):longint;
var va,vb:PConvexHullGrahamScanVector;
begin
 va:=a;
 vb:=b;
 if va^.Angle<vb^.Angle then begin
  result:=-1;
 end else if va^.Angle>vb^.Angle then begin
  result:=1;
 end else if va^.LengthToAnchor<vb^.LengthToAnchor then begin
  result:=-1;
 end else if va^.LengthToAnchor<vb^.LengthToAnchor then begin
  result:=1;
 end else if va^.OriginalIndex<vb^.OriginalIndex then begin
  result:=-1;
 end else if va^.OriginalIndex>vb^.OriginalIndex then begin
  result:=1;
 end else begin
  result:=0;
 end;
end;

procedure ConvexHullGrahamScan2D(var OriginalPoints:TConvexHullGrahamScanVectors;var Hull:TConvexHullGrahamScanVectors;const NormalAxis:TConvexHullVector);
var CountOriginalPoints,Index,Count:longint;
    ProjectionLeft,ProjectionRight:double;
    Axis0,Axis1,xVector,Relative:TConvexHullVector;
    TempPoint:TConvexHullGrahamScanVector;
    IsConvex:boolean;
    a,b:PConvexHullGrahamScanVector;
    PointerToOriginalPoints:array of PConvexHullGrahamScanVector;
begin
 PointerToOriginalPoints:=nil;
 try
  CountOriginalPoints:=length(OriginalPoints);
  if CountOriginalPoints<2 then begin
   Hull:=copy(OriginalPoints);
  end else begin
   ConvexHullGetPlaneSpace(NormalAxis,Axis0,Axis1);
   for Index:=1 to CountOriginalPoints-1 do begin
    ProjectionLeft:=ConvexHullVectorDot(OriginalPoints[Index].Vector,Axis0);
    ProjectionRight:=ConvexHullVectorDot(OriginalPoints[0].Vector,Axis0);
    if ProjectionLeft<ProjectionRight then begin
     TempPoint:=OriginalPoints[0];
     OriginalPoints[0]:=OriginalPoints[Index];
     OriginalPoints[Index]:=TempPoint;
    end;
   end;
   OriginalPoints[0].Angle:=-1.0e30;
   for Index:=1 to CountOriginalPoints-1 do begin
    xVector:=Axis0;
    Relative:=ConvexHullVectorSub(OriginalPoints[Index].Vector,OriginalPoints[0].Vector);
    OriginalPoints[Index].Angle:=ConvexHullVectorDot(ConvexHullVectorCross(xVector,Relative),NormalAxis)/ConvexHullVectorLength(Relative);
   end;
   for Index:=0 to CountOriginalPoints-1 do begin
    OriginalPoints[Index].LengthToAnchor:=ConvexHullVectorLengthSquared(ConvexHullVectorSub(OriginalPoints[Index].Vector,OriginalPoints[0].Vector));
   end;
   SetLength(PointerToOriginalPoints,CountOriginalPoints);
   for Index:=0 to CountOriginalPoints-1 do begin
    PointerToOriginalPoints[Index]:=@OriginalPoints[Index];
   end;
   IndirectIntroSort(@PointerToOriginalPoints[1],0,CountOriginalPoints-2,CompareConvexHullGrahamScanVector);
   Hull:=nil;
   SetLength(Hull,4);
   Count:=0;
   Index:=0;
   while Index<2 do begin
    Hull[Count]:=PointerToOriginalPoints[Index]^;
    inc(Count);
    inc(Index);
   end;
   while Index<CountOriginalPoints do begin
    IsConvex:=false;
    while (Count>1) and not IsConvex do begin
     a:=@Hull[Count-2];
     b:=@Hull[Count-1];
     IsConvex:=ConvexHullVectorDot(ConvexHullVectorCross(ConvexHullVectorSub(a^.Vector,b^.Vector),
                                                         ConvexHullVectorSub(a^.Vector,PointerToOriginalPoints[Index]^.Vector)),
                                   NormalAxis)>0.0;
     if IsConvex then begin
      if (Count+1)>length(Hull) then begin
       SetLength(Hull,(Count+1)*2);
      end;
      Hull[Count]:=PointerToOriginalPoints[Index]^;
      inc(Count);
     end else begin
      dec(Count);
     end;
    end;
    inc(Index);
   end;
   SetLength(Hull,Count);
  end;
 finally
  SetLength(PointerToOriginalPoints,0);
 end;
end;

constructor TKraftConvexHull.Create(const APhysics:TKraft);
begin

 inherited Create;

 fPhysics:=APhysics;

 fVertices:=nil;
 fCountVertices:=0;

 fFaces:=nil;
 fCountFaces:=0;

 fEdges:=nil;
 fCountEdges:=0;

 FillChar(fSphere,SizeOf(TKraftSphere),AnsiChar(#0));

 FillChar(fAABB,SizeOf(TKraftAABB),AnsiChar(#0));

 fAngularMotionDisc:=0.0;

 if assigned(fPhysics.fConvexHullLast) then begin
  fPhysics.fConvexHullLast.fNext:=self;
  fPrevious:=fPhysics.fConvexHullLast;
 end else begin
  fPhysics.fConvexHullFirst:=self;
  fPrevious:=nil;
 end;
 fPhysics.fConvexHullLast:=self;
 fNext:=nil;

end;

destructor TKraftConvexHull.Destroy;
begin

 SetLength(fVertices,0);

 SetLength(fFaces,0);

 SetLength(fEdges,0);

 if assigned(fPrevious) then begin
  fPrevious.fNext:=fNext;
 end else if fPhysics.fConvexHullFirst=self then begin
  fPhysics.fConvexHullFirst:=fNext;
 end;
 if assigned(fNext) then begin
  fNext.fPrevious:=fPrevious;
 end else if fPhysics.fConvexHullLast=self then begin
  fPhysics.fConvexHullLast:=fPrevious;
 end;
 fPrevious:=nil;
 fNext:=nil;

 inherited Destroy;

end;

procedure TKraftConvexHull.LoadFromStream(const AStream:TStream);
var i,j:longint;
begin

 AStream.ReadBuffer(i,SizeOf(longint));
 fCountVertices:=i;
 SetLength(fVertices,fCountVertices);
 for i:=0 to fCountVertices-1 do begin
  AStream.ReadBuffer(fVertices[i].Position.x,SizeOf(TKraftScalar));
  AStream.ReadBuffer(fVertices[i].Position.y,SizeOf(TKraftScalar));
  AStream.ReadBuffer(fVertices[i].Position.z,SizeOf(TKraftScalar));
  AStream.ReadBuffer(fVertices[i].CountAdjacencies,SizeOf(longint));
  SetLength(fVertices[i].Adjacencies,fVertices[i].CountAdjacencies);
  for j:=0 to fVertices[i].CountAdjacencies-1 do begin
   AStream.ReadBuffer(fVertices[i].Adjacencies[j],SizeOf(longint));
  end;
 end;

 AStream.ReadBuffer(i,SizeOf(longint));
 fCountFaces:=i;
 SetLength(fFaces,fCountFaces);
 for i:=0 to fCountFaces-1 do begin
  AStream.ReadBuffer(fFaces[i].Plane.Normal.x,SizeOf(TKraftScalar));
  AStream.ReadBuffer(fFaces[i].Plane.Normal.y,SizeOf(TKraftScalar));
  AStream.ReadBuffer(fFaces[i].Plane.Normal.z,SizeOf(TKraftScalar));
  AStream.ReadBuffer(fFaces[i].Plane.Distance,SizeOf(TKraftScalar));
  AStream.ReadBuffer(fFaces[i].CountVertices,SizeOf(longint));
  SetLength(fFaces[i].Vertices,fFaces[i].CountVertices);
  for j:=0 to fFaces[i].CountVertices-1 do begin
   AStream.ReadBuffer(fFaces[i].Vertices[j],SizeOf(longint));
  end;
  AStream.ReadBuffer(fFaces[i].EdgeVertexOffset,SizeOf(longint));
 end;

 AStream.ReadBuffer(i,SizeOf(longint));
 fCountEdges:=i;
 SetLength(fEdges,fCountEdges);
 for i:=0 to fCountEdges-1 do begin
  AStream.ReadBuffer(fEdges[i].Vertices[0],SizeOf(longint));
  AStream.ReadBuffer(fEdges[i].Vertices[1],SizeOf(longint));
  AStream.ReadBuffer(fEdges[i].Faces[0],SizeOf(longint));
  AStream.ReadBuffer(fEdges[i].Faces[1],SizeOf(longint));
 end;

 AStream.ReadBuffer(fSphere.Center.x,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fSphere.Center.y,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fSphere.Center.z,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fSphere.Radius,SizeOf(TKraftScalar));

 AStream.ReadBuffer(fAABB.Min.x,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fAABB.Min.y,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fAABB.Min.z,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fAABB.Max.x,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fAABB.Max.y,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fAABB.Max.z,SizeOf(TKraftScalar));

 AStream.ReadBuffer(fAngularMotionDisc,SizeOf(TKraftScalar));

 AStream.ReadBuffer(fMassData.Inertia,SizeOf(TKraftMatrix3x3));
 AStream.ReadBuffer(fMassData.Center,SizeOf(TKraftVector3));
 AStream.ReadBuffer(fMassData.Mass,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fMassData.Volume,SizeOf(TKraftScalar));

 AStream.ReadBuffer(fCentroid.x,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fCentroid.y,SizeOf(TKraftScalar));
 AStream.ReadBuffer(fCentroid.z,SizeOf(TKraftScalar));

end;

procedure TKraftConvexHull.SaveToStream(const AStream:TStream);
var i,j:longint;
begin

 i:=fCountVertices;
 AStream.WriteBuffer(i,SizeOf(longint));
 for i:=0 to fCountVertices-1 do begin
  AStream.WriteBuffer(fVertices[i].Position.x,SizeOf(TKraftScalar));
  AStream.WriteBuffer(fVertices[i].Position.y,SizeOf(TKraftScalar));
  AStream.WriteBuffer(fVertices[i].Position.z,SizeOf(TKraftScalar));
  AStream.WriteBuffer(fVertices[i].CountAdjacencies,SizeOf(longint));
  for j:=0 to fVertices[i].CountAdjacencies-1 do begin
   AStream.WriteBuffer(fVertices[i].Adjacencies[j],SizeOf(longint));
  end;
 end;

 i:=fCountFaces;
 AStream.WriteBuffer(i,SizeOf(longint));
 for i:=0 to fCountFaces-1 do begin
  AStream.WriteBuffer(fFaces[i].Plane.Normal.x,SizeOf(TKraftScalar));
  AStream.WriteBuffer(fFaces[i].Plane.Normal.y,SizeOf(TKraftScalar));
  AStream.WriteBuffer(fFaces[i].Plane.Normal.z,SizeOf(TKraftScalar));
  AStream.WriteBuffer(fFaces[i].Plane.Distance,SizeOf(TKraftScalar));
  AStream.WriteBuffer(fFaces[i].CountVertices,SizeOf(longint));
  for j:=0 to fFaces[i].CountVertices-1 do begin
   AStream.WriteBuffer(fFaces[i].Vertices[j],SizeOf(longint));
  end;
  AStream.WriteBuffer(fFaces[i].EdgeVertexOffset,SizeOf(longint));
 end;

 i:=fCountEdges;
 AStream.WriteBuffer(i,SizeOf(longint));
 for i:=0 to fCountEdges-1 do begin
  AStream.WriteBuffer(fEdges[i].Vertices[0],SizeOf(longint));
  AStream.WriteBuffer(fEdges[i].Vertices[1],SizeOf(longint));
  AStream.WriteBuffer(fEdges[i].Faces[0],SizeOf(longint));
  AStream.WriteBuffer(fEdges[i].Faces[1],SizeOf(longint));
 end;

 AStream.WriteBuffer(fSphere.Center.x,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fSphere.Center.y,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fSphere.Center.z,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fSphere.Radius,SizeOf(TKraftScalar));

 AStream.WriteBuffer(fAABB.Min.x,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fAABB.Min.y,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fAABB.Min.z,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fAABB.Max.x,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fAABB.Max.y,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fAABB.Max.z,SizeOf(TKraftScalar));

 AStream.WriteBuffer(fAngularMotionDisc,SizeOf(TKraftScalar));

 AStream.WriteBuffer(fMassData.Inertia,SizeOf(TKraftMatrix3x3));
 AStream.WriteBuffer(fMassData.Center,SizeOf(TKraftVector3));
 AStream.WriteBuffer(fMassData.Mass,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fMassData.Volume,SizeOf(TKraftScalar));

 AStream.WriteBuffer(fCentroid.x,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fCentroid.y,SizeOf(TKraftScalar));
 AStream.WriteBuffer(fCentroid.z,SizeOf(TKraftScalar));

end;

function TKraftConvexHull.AddVertex(const AVertex:TKraftVector3):longint;
var Vertex:PKraftConvexHullVertex;
begin
 result:=fCountVertices;
 inc(fCountVertices);
 if fCountVertices>length(fVertices) then begin
  SetLength(fVertices,fCountVertices*2);
 end;
 Vertex:=@fVertices[result];
 Vertex^.Position:=AVertex;
 Vertex^.CountAdjacencies:=0;
end;

procedure TKraftConvexHull.Load(const AVertices:PKraftVector3;const ACountVertices:longint);
var Index:longint;
    Vertex:PKraftConvexHullVertex;
begin
 fCountVertices:=ACountVertices;
 SetLength(fVertices,fCountVertices);
 for Index:=0 to fCountVertices-1 do begin
  Vertex:=@fVertices[Index];
  Vertex^.Position:=PKraftVector3s(AVertices)^[Index];
  Vertex^.CountAdjacencies:=0;
 end;
end;

procedure TKraftConvexHull.Scale(const WithFactor:TKraftScalar);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3Scale(fVertices[Index].Position,WithFactor);
 end;
end;

procedure TKraftConvexHull.Scale(const WithVector:TKraftVector3);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3Scale(fVertices[Index].Position,WithVector.x,WithVector.y,WithVector.z);
 end;
end;

procedure TKraftConvexHull.Transform(const WithMatrix:TKraftMatrix3x3);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3MatrixMul(fVertices[Index].Position,WithMatrix);
 end;
end;

procedure TKraftConvexHull.Transform(const WithMatrix:TKraftMatrix4x4);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3MatrixMul(fVertices[Index].Position,WithMatrix);
 end;
end;

procedure TKraftConvexHull.Build(const AMaximumCountConvexHullPoints:longint=-1;const AUserDefinedTolerance:double=-1.0);
const HashBits=8;
      HashSize=1 shl HashBits;
      HashMask=HashSize-1;
      ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
      DOUBLE_PREC:double=2.2204460492503131e-16;
type PTempFaceEdgeHashItem=^TTempFaceEdgeHashItem;
     TTempFaceEdgeHashItem=record
      Next:longint;
      Hash:longword;
      Edge:longint;
      Face:longint;
      FaceEdge:longint;
      FaceEdgeVertexA:longint;
      FaceEdgeVertexB:longint;
     end;
     PTempFaceEdge=^TTempFaceEdge;
     TTempFaceEdge=record
      Face:longint;
      Twin:longint;
      Vertices:array[0..1] of longint;
     end;
var PointIndex,TriangleIndex,TriangleVertexIndex,VertexIndex,OtherVertexIndex,{OtherTriangleIndex,}Index,
    OtherTriangleVertexIndex,SearchIndex,CountTempFaceEdges,FaceIndex,EdgeVertexOffset,TempFaceEdgeIndex,
    {OtherTempFaceEdgeIndex,}CountTempFaceEdgeHashItems,TempFaceEdgeHashItemIndex,{v0,v1,v2,}
    {FoundSharedVertex,}FaceVertexIndex,{OtherFaceVertexIndex,}CountPoints,HashBucket,CountTriangles:longint;
    TempFaceEdgeHash:longword;
    TempFaceEdgeHashItem:PTempFaceEdgeHashItem;
    TempPoints:TConvexHullVectors;
    TempPointHashTable:array of longint;
    TempTriangles:TConvexHullTriangles;
    Vertex:PKraftConvexHullVertex;
    Face:PKraftConvexHullFace;
    Edge:PKraftConvexHullEdge;
    //pa,pb:TKraftPlane;
    //vn,NewPlaneNormal:TKraftVector3;
    //Processed:array of boolean;
    TempFaceEdges:array of TTempFaceEdge;
    TempFaceEdge:PTempFaceEdge;
    Found:boolean;
    TempFaceEdgeHashItems:array of TTempFaceEdgeHashItem;
    TempFaceEdgeHashTable:array of longint;
    {TempInputPolygons,}TempOutputPolygons:TConvexHullPolygons;
    TempPolygon:PConvexHullPolygon;
    QuickHullInstance:TKraftQuickHull;
    QuickHullVertex:TKraftQuickHullVertex;
    QuickHullFaces:TKraftQuickHullOutputFaces;
    MinBounds,MaxBounds,a,b,c,Centroid,Normal:TConvexHullVector;
    Tolerance,CharLength,NearTolerance:double;
    Last,Current:PConvexHullVector;
    Plane:TConvexHullPlane;
    InputIsCoplanar:boolean;
    TemporaryGrahamScanVectors,HullGrahamScanVectors:TConvexHullGrahamScanVectors;
    Triangle:PConvexHullTriangle;
begin
 fFaces:=nil;
 fCountFaces:=0;
 fEdges:=nil;
 fCountEdges:=0;

 TempPoints:=nil;
 TempTriangles:=nil;
 TempFaceEdges:=nil;
 TempFaceEdgeHashItems:=nil;
 TempFaceEdgeHashTable:=nil;
 //TempInputPolygons.Items:=nil;
 TempOutputPolygons.Items:=nil;
 try

  if fCountVertices<1 then begin
   raise EKraftDegeneratedConvexHull.Create('Degenerated convex hull');
  end;

  Vertex:=@fVertices[0];
  MinBounds.x:=Vertex^.Position.x;
  MinBounds.y:=Vertex^.Position.y;
  MinBounds.z:=Vertex^.Position.z;
  MaxBounds.x:=Vertex^.Position.x;
  MaxBounds.y:=Vertex^.Position.y;
  MaxBounds.z:=Vertex^.Position.z;
  for Index:=0 to fCountVertices-1 do begin
   Vertex:=@fVertices[Index];
   if MinBounds.x>Vertex^.Position.x then begin
    MinBounds.x:=Vertex^.Position.x;
   end;
   if MinBounds.y>Vertex^.Position.y then begin
    MinBounds.y:=Vertex^.Position.y;
   end;
   if MinBounds.z>Vertex^.Position.z then begin
    MinBounds.z:=Vertex^.Position.z;
   end;
   if MaxBounds.x<Vertex^.Position.x then begin
    MaxBounds.x:=Vertex^.Position.x;
   end;
   if MaxBounds.y<Vertex^.Position.y then begin
    MaxBounds.y:=Vertex^.Position.y;
   end;
   if MaxBounds.z<Vertex^.Position.z then begin
    MaxBounds.z:=Vertex^.Position.z;
   end;
  end;

  if AUserDefinedTolerance>0.0 then begin
   Tolerance:=AUserDefinedTolerance;
  end else begin
   Tolerance:=Max(DOUBLE_PREC,(3.0*DOUBLE_PREC)*(Max(abs(MaxBounds.x),abs(MaxBounds.x))+Max(abs(MaxBounds.y),abs(MaxBounds.y))+Max(abs(MaxBounds.z),abs(MaxBounds.z))));
  end;

  CharLength:=Max(Max(MaxBounds.x-MaxBounds.x,MaxBounds.y-MaxBounds.y),MaxBounds.z-MaxBounds.z);

  NearTolerance:=Max(DOUBLE_PREC,(3.0*DOUBLE_PREC)*CharLength);

  // Remove duplicate and too near points
  TempPointHashTable:=nil;
  try
   SetLength(TempPoints,fCountVertices);
   CountPoints:=0;
   try
    SetLength(TempPointHashTable,HashSize);
    for Index:=0 to HashSize-1 do begin
     TempPointHashTable[Index]:=-1;
    end;
    for PointIndex:=0 to fCountVertices-1 do begin
     Vertex:=@fVertices[PointIndex];
     HashBucket:=((round(Vertex^.Position.x)*73856093) xor (round(Vertex^.Position.y)*19349663) xor (round(Vertex^.Position.z)*83492791)) and HashMask;
     Index:=TempPointHashTable[HashBucket];
     while Index>=0 do begin
      if (ConvexHullIsSameValue(TempPoints[Index].x,Vertex^.Position.x) and
          ConvexHullIsSameValue(TempPoints[Index].y,Vertex^.Position.y) and
          ConvexHullIsSameValue(TempPoints[Index].z,Vertex^.Position.z)) or
         (sqrt(sqr(TempPoints[Index].x-Vertex^.Position.x)+
               sqr(TempPoints[Index].y-Vertex^.Position.y)+
               sqr(TempPoints[Index].z-Vertex^.Position.z))<NearTolerance) then begin
       break;
      end;
      Index:=TempPoints[Index].HashNext;
     end;
     if Index<0 then begin
      Index:=CountPoints;
      inc(CountPoints);
      TempPoints[Index].x:=Vertex^.Position.x;
      TempPoints[Index].y:=Vertex^.Position.y;
      TempPoints[Index].z:=Vertex^.Position.z;
      TempPoints[Index].HashNext:=TempPointHashTable[HashBucket];
      TempPointHashTable[HashBucket]:=Index;
     end;
    end;
   finally
    SetLength(TempPoints,CountPoints);
   end;
  finally
   SetLength(TempPointHashTable,0);
  end;

  if CountPoints<3 then begin

   raise EKraftDegeneratedConvexHull.Create('Degenerated convex hull');

  end else if CountPoints=3 then begin

   TempOutputPolygons.Count:=2;
   SetLength(TempOutputPolygons.Items,TempOutputPolygons.Count);

   TempOutputPolygons.Items[0].Count:=3;
   SetLength(TempOutputPolygons.Items[0].Indices,TempOutputPolygons.Items[0].Count);
   TempOutputPolygons.Items[0].Indices[0]:=0;
   TempOutputPolygons.Items[0].Indices[1]:=1;
   TempOutputPolygons.Items[0].Indices[2]:=2;

   TempOutputPolygons.Items[1].Count:=3;
   SetLength(TempOutputPolygons.Items[1].Indices,TempOutputPolygons.Items[1].Count);
   TempOutputPolygons.Items[1].Indices[0]:=0;
   TempOutputPolygons.Items[1].Indices[1]:=2;
   TempOutputPolygons.Items[1].Indices[2]:=1;

  end else begin

   a:=ConvexHullVectorSub(TempPoints[0],TempPoints[3]);
   b:=ConvexHullVectorSub(TempPoints[1],TempPoints[3]);
   c:=ConvexHullVectorSub(TempPoints[2],TempPoints[3]);

   if (CountPoints=4) and (((a.x*((b.z*c.y)-(b.y*c.z)))+(a.y*((b.x*c.z)-(b.z*c.x)))+(a.z*((b.y*c.x)-(b.x*c.y))))<Tolerance) then begin

    TempOutputPolygons.Count:=2;
    SetLength(TempOutputPolygons.Items,TempOutputPolygons.Count);

    TempOutputPolygons.Items[0].Count:=4;
    SetLength(TempOutputPolygons.Items[0].Indices,TempOutputPolygons.Items[0].Count);
    TempOutputPolygons.Items[0].Indices[0]:=0;
    TempOutputPolygons.Items[0].Indices[1]:=1;
    TempOutputPolygons.Items[0].Indices[2]:=2;
    TempOutputPolygons.Items[0].Indices[3]:=3;

    TempOutputPolygons.Items[1].Count:=4;
    SetLength(TempOutputPolygons.Items[1].Indices,TempOutputPolygons.Items[1].Count);
    TempOutputPolygons.Items[1].Indices[0]:=3;
    TempOutputPolygons.Items[1].Indices[1]:=2;
    TempOutputPolygons.Items[1].Indices[2]:=1;
    TempOutputPolygons.Items[1].Indices[3]:=0;

   end else begin

    // Compute newell plane for coplanarity check
    Centroid.x:=0.0;
    Centroid.y:=0.0;
    Centroid.z:=0.0;
    Normal.x:=0.0;
    Normal.y:=0.0;
    Normal.z:=0.0;
    Last:=@TempPoints[CountPoints-1];
    for VertexIndex:=0 to CountPoints-1 do begin
     Current:=@TempPoints[VertexIndex];
     Normal.x:=Normal.x+((Last^.y-Current^.y)*(Last^.z+Current^.z));
     Normal.y:=Normal.y+((Last^.z-Current^.z)*(Last^.x+Current^.x));
     Normal.z:=Normal.z+((Last^.x-Current^.x)*(Last^.y+Current^.y));
     Centroid.x:=Centroid.x+Current^.x;
     Centroid.y:=Centroid.y+Current^.y;
     Centroid.z:=Centroid.z+Current^.z;
     Last:=Current;
    end;
    if ConvexHullIsSameValue(Normal.x,0.0) and ConvexHullIsSameValue(Normal.y,0.0) and ConvexHullIsSameValue(Normal.z,0.0) then begin
     Normal.x:=0.0;
     Normal.y:=1.0;
     Normal.z:=0.0;
    end;
    Plane.Normal:=ConvexHullVectorNormalize(Normal);
    Plane.Distance:=-ConvexHullVectorDot(Plane.Normal,ConvexHullVectorDivide(Centroid,CountPoints));

    // Now check for coplanarity
    InputIsCoplanar:=true;
    for Index:=0 to CountPoints-1 do begin
     Current:=@TempPoints[Index];
     if ((Plane.Normal.x*Current^.x)+(Plane.Normal.y*Current^.y)+(Plane.Normal.z*Current^.z)+Plane.Distance)>Tolerance then begin
      InputIsCoplanar:=false;
      break;
     end;
    end;

    if InputIsCoplanar then begin

     TemporaryGrahamScanVectors:=nil;
     HullGrahamScanVectors:=nil;
     try

      SetLength(TemporaryGrahamScanVectors,CountPoints);
      for Index:=0 to CountPoints-1 do begin
       TemporaryGrahamScanVectors[Index].OriginalIndex:=Index;
       TemporaryGrahamScanVectors[Index].Vector:=TempPoints[Index];
      end;

      ConvexHullGrahamScan2D(TemporaryGrahamScanVectors,HullGrahamScanVectors,Plane.Normal);

      if length(HullGrahamScanVectors)<3 then begin
       raise EKraftDegeneratedConvexHull.Create('Degenerated convex hull');
      end;

      CountPoints:=length(HullGrahamScanVectors);
      SetLength(TempPoints,CountPoints);
      for Index:=0 to CountPoints-1 do begin
       TempPoints[Index]:=HullGrahamScanVectors[Index].Vector;
      end;

      TempOutputPolygons.Count:=2;
      SetLength(TempOutputPolygons.Items,TempOutputPolygons.Count);

      TempOutputPolygons.Items[0].Count:=CountPoints;
      SetLength(TempOutputPolygons.Items[0].Indices,TempOutputPolygons.Items[0].Count);
      for Index:=0 to CountPoints-1 do begin
       TempOutputPolygons.Items[0].Indices[Index]:=Index;
      end;

      TempOutputPolygons.Items[1].Count:=CountPoints;
      SetLength(TempOutputPolygons.Items[1].Indices,TempOutputPolygons.Items[1].Count);
      for Index:=0 to CountPoints-1 do begin
       TempOutputPolygons.Items[1].Indices[Index]:=CountPoints-(Index+1);
      end;

     finally
      SetLength(TemporaryGrahamScanVectors,0);
      SetLength(HullGrahamScanVectors,0);
     end;

    end else begin

     if AMaximumCountConvexHullPoints>0 then begin

      // Here we're using triangular-output-based stan hull as maximum vertex count prefilter for the
      // non-triangular-output-based quick hull implementation
      StanHullProcess(TempPoints,TempTriangles,AMaximumCountConvexHullPoints,AUserDefinedTolerance);

      // Throw the temporary output triangles of the stan hull algorithm away, because we do need only the filtered remain points
      SetLength(TempTriangles,0);

     end;

     QuickHullInstance:=TKraftQuickHull.Create;
     try
      if AUserDefinedTolerance>0.0 then begin
       QuickHullInstance.fExplicitTolerance:=AUserDefinedTolerance;
      end;
      QuickHullInstance.Reset;
      for PointIndex:=0 to length(TempPoints)-1 do begin
       QuickHullInstance.AddPoint(TempPoints[PointIndex].x,TempPoints[PointIndex].y,TempPoints[PointIndex].z);
      end;
      QuickHullInstance.Build(AMaximumCountConvexHullPoints);
      SetLength(TempPoints,QuickHullInstance.fCountVertices);
      for PointIndex:=0 to QuickHullInstance.fCountVertices-1 do begin
       QuickHullVertex:=TKraftQuickHullVertex(QuickHullInstance.fPointBuffer[QuickHullInstance.fVertexPointIndices[PointIndex]]);
       TempPoints[PointIndex].x:=QuickHullVertex.Point.x;
       TempPoints[PointIndex].y:=QuickHullVertex.Point.y;
       TempPoints[PointIndex].z:=QuickHullVertex.Point.z;
      end;
      QuickHullFaces:=nil;
      try
       QuickHullInstance.GetFaces(QuickHullFaces);
       SetLength(TempOutputPolygons.Items,length(QuickHullFaces));
       TempOutputPolygons.Count:=length(QuickHullFaces);
       for FaceIndex:=0 to TempOutputPolygons.Count-1 do begin
        TempPolygon:=@TempOutputPolygons.Items[FaceIndex];
        TempPolygon^.Count:=length(QuickHullFaces[FaceIndex]);
        SetLength(TempPolygon^.Indices,TempPolygon^.Count);
        for VertexIndex:=0 to TempPolygon^.Count-1 do begin
         TempPolygon^.Indices[VertexIndex]:=QuickHullFaces[FaceIndex,VertexIndex];
        end;
       end;
      finally
       SetLength(QuickHullFaces,0);
      end;
     finally
      QuickHullInstance.Free;
     end;

    end;

   end;

  end;

  for FaceIndex:=0 to TempOutputPolygons.Count-1 do begin
   ConvexHullComputePolygonNewellPlane(TempPoints,TempOutputPolygons.Items[FaceIndex]);
  end;

  //SetLength(TempInputPolygons.Items,0);

  fCountVertices:=length(TempPoints);
  SetLength(fVertices,fCountVertices);
  for PointIndex:=0 to fCountVertices-1 do begin
   fVertices[PointIndex].Position.x:=TempPoints[PointIndex].x;
   fVertices[PointIndex].Position.y:=TempPoints[PointIndex].y;
   fVertices[PointIndex].Position.z:=TempPoints[PointIndex].z;
  end;

  // Copy face poylgons
  SetLength(fFaces,TempOutputPolygons.Count);
  fCountFaces:=TempOutputPolygons.Count;
  CountTempFaceEdges:=0;
  EdgeVertexOffset:=0;
  for FaceIndex:=0 to TempOutputPolygons.Count-1 do begin
   Face:=@fFaces[FaceIndex];
   TempPolygon:=@TempOutputPolygons.Items[FaceIndex];
   Face^.Plane.Normal.x:=TempPolygon^.Plane.Normal.x;
   Face^.Plane.Normal.y:=TempPolygon^.Plane.Normal.y;
   Face^.Plane.Normal.z:=TempPolygon^.Plane.Normal.z;
   Face^.Plane.Distance:=TempPolygon^.Plane.Distance;
   Face^.CountVertices:=TempPolygon^.Count;
   SetLength(Face^.Vertices,Face^.CountVertices);
   for VertexIndex:=0 to Face^.CountVertices-1 do begin
    Face^.Vertices[VertexIndex]:=TempPolygon^.Indices[VertexIndex];
   end;
   Face^.EdgeVertexOffset:=EdgeVertexOffset;
   inc(EdgeVertexOffset,Face^.CountVertices);
   for VertexIndex:=0 to Face^.CountVertices-1 do begin
    OtherVertexIndex:=VertexIndex+1;
    if OtherVertexIndex>=Face^.CountVertices then begin
     dec(OtherVertexIndex,Face^.CountVertices);
    end;
    TempFaceEdgeIndex:=CountTempFaceEdges;
    inc(CountTempFaceEdges);
    if CountTempFaceEdges>length(TempFaceEdges) then begin
     SetLength(TempFaceEdges,CountTempFaceEdges*2);
    end;
    TempFaceEdge:=@TempFaceEdges[TempFaceEdgeIndex];
    TempFaceEdge^.Face:=FaceIndex;
    TempFaceEdge^.Vertices[0]:=Face^.Vertices[VertexIndex];
    TempFaceEdge^.Vertices[1]:=Face^.Vertices[OtherVertexIndex];
   end;
  end;
  SetLength(TempFaceEdges,CountTempFaceEdges);

  // Find unique edges
  try
   SetLength(fEdges,CountTempFaceEdges);
   SetLength(TempFaceEdgeHashItems,CountTempFaceEdges);
   SetLength(TempFaceEdgeHashTable,HashSize);
   for TempFaceEdgeHashItemIndex:=0 to HashSize-1 do begin
    TempFaceEdgeHashTable[TempFaceEdgeHashItemIndex]:=-1;
   end;
   CountTempFaceEdgeHashItems:=0;
   for TempFaceEdgeIndex:=0 to CountTempFaceEdges-1 do begin
    TempFaceEdge:=@TempFaceEdges[TempFaceEdgeIndex];
    if TempFaceEdge^.Vertices[0]<TempFaceEdge^.Vertices[1] then begin
     TempFaceEdgeHash:=(longword(TempFaceEdge^.Vertices[0])*73856093) xor (longword(TempFaceEdge^.Vertices[1])*83492791);
    end else begin
     TempFaceEdgeHash:=(longword(TempFaceEdge^.Vertices[1])*73856093) xor (longword(TempFaceEdge^.Vertices[0])*83492791);
    end;
    TempFaceEdgeHashItemIndex:=TempFaceEdgeHashTable[TempFaceEdgeHash and HashMask];
    while TempFaceEdgeHashItemIndex>=0 do begin
     TempFaceEdgeHashItem:=@TempFaceEdgeHashItems[TempFaceEdgeHashItemIndex];
     if (TempFaceEdgeHashItem^.Hash=TempFaceEdgeHash) and
        (((TempFaceEdgeHashItem^.FaceEdgeVertexA=TempFaceEdge^.Vertices[0]) and (TempFaceEdgeHashItem^.FaceEdgeVertexB=TempFaceEdge^.Vertices[1])) or
         ((TempFaceEdgeHashItem^.FaceEdgeVertexA=TempFaceEdge^.Vertices[1]) and (TempFaceEdgeHashItem^.FaceEdgeVertexB=TempFaceEdge^.Vertices[0]))) then begin
      break;
     end else begin
      TempFaceEdgeHashItemIndex:=TempFaceEdgeHashItem^.Next;
     end;
    end;
    if TempFaceEdgeHashItemIndex<0 then begin
     if length(TempFaceEdgeHashItems)<(CountTempFaceEdgeHashItems+1) then begin
      SetLength(TempFaceEdgeHashItems,(CountTempFaceEdgeHashItems+1)*2);
     end;
     TempFaceEdgeHashItem:=@TempFaceEdgeHashItems[CountTempFaceEdgeHashItems];
     TempFaceEdgeHashItem^.Next:=TempFaceEdgeHashTable[TempFaceEdgeHash and HashMask];
     TempFaceEdgeHashTable[TempFaceEdgeHash and HashMask]:=CountTempFaceEdgeHashItems;
     TempFaceEdgeHashItem^.Hash:=TempFaceEdgeHash;
     inc(CountTempFaceEdgeHashItems);
     TempFaceEdgeHashItem^.Edge:=-1;
     TempFaceEdgeHashItem^.Face:=TempFaceEdge^.Face;
     TempFaceEdgeHashItem^.FaceEdge:=TempFaceEdgeIndex;
     TempFaceEdgeHashItem^.FaceEdgeVertexA:=TempFaceEdge^.Vertices[0];
     TempFaceEdgeHashItem^.FaceEdgeVertexB:=TempFaceEdge^.Vertices[1];
    end else begin
     TempFaceEdgeHashItem:=@TempFaceEdgeHashItems[TempFaceEdgeHashItemIndex];
     if (TempFaceEdgeHashItem^.Edge<0) and (TempFaceEdgeHashItem^.Face<>TempFaceEdge^.Face) then begin
      TempFaceEdgeHashItem^.Edge:=fCountEdges;
      Edge:=@fEdges[fCountEdges];
      inc(fCountEdges);
      Edge^.Vertices[0]:=TempFaceEdgeHashItem^.FaceEdgeVertexA;
      Edge^.Vertices[1]:=TempFaceEdgeHashItem^.FaceEdgeVertexB;
      Edge^.Faces[0]:=TempFaceEdgeHashItem^.Face;
      Edge^.Faces[1]:=TempFaceEdge^.Face;
     end else begin
      raise EKraftDegeneratedConvexHull.Create('Degenerated convex hull');
     end;
    end;
   end;
   for TempFaceEdgeHashItemIndex:=0 to CountTempFaceEdgeHashItems-1 do begin
    TempFaceEdgeHashItem:=@TempFaceEdgeHashItems[TempFaceEdgeHashItemIndex];
    if TempFaceEdgeHashItem^.Edge<0 then begin
     raise EKraftDegeneratedConvexHull.Create('Degenerated convex hull');
    end;
   end;
   SetLength(fEdges,fCountEdges);
  finally
   SetLength(TempFaceEdgeHashItems,0);
   SetLength(TempFaceEdgeHashTable,0);
  end;

  TempTriangles:=nil;
  try

   // Triangulate convex polygon faces into temporary triangles for the hill-climbing vertex adjacency computation
   CountTriangles:=0;
   for FaceIndex:=0 to fCountFaces-1 do begin
    Face:=@fFaces[FaceIndex];
    if Face^.CountVertices>2 then begin
     inc(CountTriangles,Face^.CountVertices-2);
    end;
   end;
   SetLength(TempTriangles,CountTriangles);
   CountTriangles:=0;
   for FaceIndex:=0 to fCountFaces-1 do begin
    Face:=@fFaces[FaceIndex];
    for FaceVertexIndex:=2 to Face^.CountVertices-1 do begin
     if (CountTriangles+1)>length(TempTriangles) then begin
      SetLength(TempTriangles,(CountTriangles+1)*2);
     end;
     Triangle:=@TempTriangles[CountTriangles];
     inc(CountTriangles);
     Triangle^[0]:=Face^.Vertices[0];
     Triangle^[1]:=Face^.Vertices[FaceVertexIndex-1];
     Triangle^[2]:=Face^.Vertices[FaceVertexIndex];
    end;
   end;

   // Compute vertex adjacency for hill-climbing
   for VertexIndex:=0 to fCountVertices-1 do begin
    fVertices[VertexIndex].CountAdjacencies:=0;
   end;
   for TriangleIndex:=0 to CountTriangles-1 do begin
    for TriangleVertexIndex:=0 to 2 do begin
     VertexIndex:=TempTriangles[TriangleIndex,TriangleVertexIndex];
     Vertex:=@fVertices[VertexIndex];
     for OtherTriangleVertexIndex:=1 to 2 do begin
      OtherVertexIndex:=TempTriangles[TriangleIndex,ModuloThree[TriangleVertexIndex+OtherTriangleVertexIndex]];
      Found:=false;
      for SearchIndex:=0 to Vertex^.CountAdjacencies-1 do begin
       if Vertex^.Adjacencies[SearchIndex]=OtherVertexIndex then begin
        Found:=true;
        break;
       end;
      end;
      if not Found then begin
       if (Vertex^.CountAdjacencies+1)>length(Vertex^.Adjacencies) then begin
        SetLength(Vertex^.Adjacencies,(Vertex^.CountAdjacencies+1)*2);
       end;
       Vertex^.Adjacencies[Vertex^.CountAdjacencies]:=OtherVertexIndex;
       inc(Vertex^.CountAdjacencies);
      end;
     end;
    end;
   end;

  finally
   SetLength(TempTriangles,0);
  end;

 finally
  SetLength(TempPoints,0);
  SetLength(TempTriangles,0);
  SetLength(TempFaceEdges,0);
  //SetLength(TempInputPolygons.Items,0);
  SetLength(TempOutputPolygons.Items,0);
 end;
end;

procedure TKraftConvexHull.Update;
var FaceIndex,VertexIndex:longint;
    Face:PKraftConvexHullFace;
    v0,v1,v2:PKraftVector3;
begin

 for FaceIndex:=0 to fCountFaces-1 do begin
  Face:=@fFaces[FaceIndex];
  if Face^.CountVertices>2 then begin
   v0:=@fVertices[Face^.Vertices[0]].Position;
   v1:=@fVertices[Face^.Vertices[1]].Position;
   v2:=@fVertices[Face^.Vertices[2]].Position;
   Face^.Plane.Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(v1^,v0^),Vector3Sub(v2^,v0^)));
   Face^.Plane.Distance:=-Vector3Dot(Face^.Plane.Normal,v0^);
  end;
 end;

 fSphere.Center.x:=0.0;
 fSphere.Center.y:=0.0;
 fSphere.Center.z:=0.0;
 fSphere.Radius:=0.0;
 if fCountVertices>0 then begin
  v0:=@fVertices[0].Position;
  fAABB.Min:=v0^;
  fAABB.Max:=v0^;
  fSphere.Center:=v0^;
  for VertexIndex:=1 to fCountVertices-1 do begin
   v0:=@fVertices[VertexIndex].Position;
   if fAABB.Min.x>v0^.x then begin
    fAABB.Min.x:=v0^.x;
   end;
   if fAABB.Min.y>v0^.y then begin
    fAABB.Min.y:=v0^.y;
   end;
   if fAABB.Min.z>v0^.z then begin
    fAABB.Min.z:=v0^.z;
   end;
   if fAABB.Max.x<v0^.x then begin
    fAABB.Max.x:=v0^.x;
   end;
   if fAABB.Max.y<v0^.y then begin
    fAABB.Max.y:=v0^.y;
   end;
   if fAABB.Max.z<v0^.z then begin
    fAABB.Max.z:=v0^.z;
   end;
   fSphere.Center.x:=fSphere.Center.x+v0^.x;
   fSphere.Center.y:=fSphere.Center.y+v0^.y;
   fSphere.Center.z:=fSphere.Center.z+v0^.z;
  end;
  fSphere.Center.x:=fSphere.Center.x/fCountVertices;
  fSphere.Center.y:=fSphere.Center.y/fCountVertices;
  fSphere.Center.z:=fSphere.Center.z/fCountVertices;
  for VertexIndex:=0 to fCountVertices-1 do begin
   fSphere.Radius:=Max(fSphere.Radius,Vector3Length(Vector3Sub(fSphere.Center,fVertices[VertexIndex].Position)));
  end;
 end else begin
  fAABB.Min.x:=MAX_SCALAR;
  fAABB.Min.y:=MAX_SCALAR;
  fAABB.Min.z:=MAX_SCALAR;
  fAABB.Max.x:=-MAX_SCALAR;
  fAABB.Max.y:=-MAX_SCALAR;
  fAABB.Max.z:=-MAX_SCALAR;
 end;

 fAngularMotionDisc:=Vector3Length(fSphere.Center)+fSphere.Radius;

end;

procedure TKraftConvexHull.CalculateMassData;
const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
      Density=1.0;
var FaceIndex,FaceVertexIndex,CoordinateIndex,SecondCoordinateIndex,ThirdCoordinateIndex:longint;
    Face:PKraftConvexHullFace;
    vU,vV,vW:PKraftVector3;
    CurrentVolume,CentroidX,CentroidY,CentroidZ,Volume,Denominator:double;
    Diag,OffDiag,u,v,w:array[0..2] of double;
begin

 CentroidX:=0.0;
 CentroidY:=0.0;
 CentroidZ:=0.0;

 Volume:=0.0;

 Diag[0]:=0.0;
 Diag[1]:=0.0;
 Diag[2]:=0.0;

 OffDiag[0]:=0.0;
 OffDiag[1]:=0.0;
 OffDiag[2]:=0.0;

 for FaceIndex:=0 to fCountFaces-1 do begin

  Face:=@fFaces[FaceIndex];

  vU:=@fVertices[Face^.Vertices[0]].Position;
  u[0]:=vU^.x;
  u[1]:=vU^.y;
  u[2]:=vU^.z;

  for FaceVertexIndex:=1 to Face^.CountVertices-2 do begin

   vV:=@fVertices[Face^.Vertices[FaceVertexIndex]].Position;
   v[0]:=vV^.x;
   v[1]:=vV^.y;
   v[2]:=vV^.z;

   vW:=@fVertices[Face^.Vertices[FaceVertexIndex+1]].Position;
   w[0]:=vW^.x;
   w[1]:=vW^.y;
   w[2]:=vW^.z;

   CurrentVolume:=Vector3Dot(vU^,Vector3Cross(vV^,vW^));

   Volume:=Volume+CurrentVolume;

   CentroidX:=CentroidX+((vU^.x+vV^.x+vW^.x)*CurrentVolume);
   CentroidY:=CentroidY+((vU^.y+vV^.y+vW^.y)*CurrentVolume);
   CentroidZ:=CentroidZ+((vU^.z+vV^.z+vW^.z)*CurrentVolume);

   for CoordinateIndex:=0 to 2 do begin

    SecondCoordinateIndex:=ModuloThree[CoordinateIndex+1];
    ThirdCoordinateIndex:=ModuloThree[CoordinateIndex+2];

    Diag[CoordinateIndex]:=Diag[CoordinateIndex]+(((u[CoordinateIndex]*v[CoordinateIndex])+
                                                   (v[CoordinateIndex]*w[CoordinateIndex])+
                                                   (w[CoordinateIndex]*u[CoordinateIndex])+
                                                   (u[CoordinateIndex]*u[CoordinateIndex])+
                                                   (v[CoordinateIndex]*v[CoordinateIndex])+
                                                   (w[CoordinateIndex]*w[CoordinateIndex]))*CurrentVolume);

	  OffDiag[CoordinateIndex]:=OffDiag[CoordinateIndex]+(((u[SecondCoordinateIndex]*v[ThirdCoordinateIndex])+
                                                         (v[SecondCoordinateIndex]*w[ThirdCoordinateIndex])+
                                                         (w[SecondCoordinateIndex]*u[ThirdCoordinateIndex])+
                                                         (u[SecondCoordinateIndex]*w[ThirdCoordinateIndex])+
                                                         (v[SecondCoordinateIndex]*u[ThirdCoordinateIndex])+
                                                         (w[SecondCoordinateIndex]*v[ThirdCoordinateIndex])+
                                                         (u[SecondCoordinateIndex]*u[ThirdCoordinateIndex]*2.0)+
                                                         (v[SecondCoordinateIndex]*v[ThirdCoordinateIndex]*2.0)+
                                                         (w[SecondCoordinateIndex]*w[ThirdCoordinateIndex]*2.0))*CurrentVolume);

   end;

  end;

 end;

 Denominator:=Volume*4.0;
 fCentroid.x:=CentroidX/Denominator;
 fCentroid.y:=CentroidY/Denominator;
 fCentroid.z:=CentroidZ/Denominator;

 Volume:=Volume/6.0;

 Denominator:=Volume*60.0;
 Diag[0]:=Diag[0]/Denominator;
 Diag[1]:=Diag[1]/Denominator;
 Diag[2]:=Diag[2]/Denominator;

 Denominator:=Volume*120.0;
 OffDiag[0]:=OffDiag[0]/Denominator;
 OffDiag[1]:=OffDiag[1]/Denominator;
 OffDiag[2]:=OffDiag[2]/Denominator;

 fMassData.Volume:=Volume;
 fMassData.Mass:=Volume*Density;

 if fMassData.Mass>EPSILON then begin
  fMassData.Inertia[0,0]:=(Diag[1]+Diag[2])*fMassData.Mass;
  fMassData.Inertia[0,1]:=(-OffDiag[2])*fMassData.Mass;
  fMassData.Inertia[0,2]:=(-OffDiag[1])*fMassData.Mass;
  fMassData.Inertia[1,0]:=(-OffDiag[2])*fMassData.Mass;
  fMassData.Inertia[1,1]:=(Diag[0]+Diag[2])*fMassData.Mass;
  fMassData.Inertia[1,2]:=(-OffDiag[0])*fMassData.Mass;
  fMassData.Inertia[2,0]:=(-OffDiag[1])*fMassData.Mass;
  fMassData.Inertia[2,1]:=(-OffDiag[0])*fMassData.Mass;
  fMassData.Inertia[2,2]:=(Diag[0]+Diag[1])*fMassData.Mass;
 end else begin
  fMassData.Inertia[0,0]:=0.0;
  fMassData.Inertia[0,1]:=0.0;
  fMassData.Inertia[0,2]:=0.0;
  fMassData.Inertia[1,0]:=0.0;
  fMassData.Inertia[1,1]:=0.0;
  fMassData.Inertia[1,2]:=0.0;
  fMassData.Inertia[2,0]:=0.0;
  fMassData.Inertia[2,1]:=0.0;
  fMassData.Inertia[2,2]:=0.0;
 end;

end;

procedure TKraftConvexHull.Finish;
const Steps=1024;
      //ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
var VertexIndex:longint;
    v:PKraftVector3;
begin

 SetLength(fVertices,fCountVertices);

 CalculateMassData;

 // Construct AABB and bounding sphere
 fSphere.Center.x:=0.0;
 fSphere.Center.y:=0.0;
 fSphere.Center.z:=0.0;
 fSphere.Radius:=0.0;
 if fCountVertices>0 then begin
  v:=@fVertices[0].Position;
  fAABB.Min:=v^;
  fAABB.Max:=v^;
  fSphere.Center:=v^;
  for VertexIndex:=1 to fCountVertices-1 do begin
   v:=@fVertices[VertexIndex].Position;
   if fAABB.Min.x>v^.x then begin
    fAABB.Min.x:=v^.x;
   end;
   if fAABB.Min.y>v^.y then begin
    fAABB.Min.y:=v^.y;
   end;
   if fAABB.Min.z>v^.z then begin
    fAABB.Min.z:=v^.z;
   end;
   if fAABB.Max.x<v^.x then begin
    fAABB.Max.x:=v^.x;
   end;
   if fAABB.Max.y<v^.y then begin
    fAABB.Max.y:=v^.y;
   end;
   if fAABB.Max.z<v^.z then begin
    fAABB.Max.z:=v^.z;
   end;
   fSphere.Center.x:=fSphere.Center.x+v^.x;
   fSphere.Center.y:=fSphere.Center.y+v^.y;
   fSphere.Center.z:=fSphere.Center.z+v^.z;
  end;
  fSphere.Center.x:=fSphere.Center.x/fCountVertices;
  fSphere.Center.y:=fSphere.Center.y/fCountVertices;
  fSphere.Center.z:=fSphere.Center.z/fCountVertices;
  for VertexIndex:=0 to fCountVertices-1 do begin
   fSphere.Radius:=Max(fSphere.Radius,Vector3Length(Vector3Sub(fSphere.Center,fVertices[VertexIndex].Position)));
  end;
 end else begin
  fAABB.Min.x:=MAX_SCALAR;
  fAABB.Min.y:=MAX_SCALAR;
  fAABB.Min.z:=MAX_SCALAR;
  fAABB.Max.x:=-MAX_SCALAR;
  fAABB.Max.y:=-MAX_SCALAR;
  fAABB.Max.z:=-MAX_SCALAR;
 end;

 fAngularMotionDisc:=Vector3Length(fSphere.Center)+fSphere.Radius;

end;

function TKraftConvexHull.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
var Index:longint;
    Distance:TKraftScalar;
begin
 if fCountFaces>0 then begin
  result:=PlaneVectorDistance(fFaces[0].Plane,Position);
  for Index:=1 to fCountFaces-1 do begin
   Distance:=PlaneVectorDistance(fFaces[Index].Plane,Position);
   if result<Distance then begin
    result:=Distance;
   end;
  end;
  if result>=0.0 then begin
   // Point lies on or outside convex hull
   for Index:=0 to fCountFaces-1 do begin
    Distance:=PlaneVectorDistance(fFaces[Index].Plane,Position);
    if result>Distance then begin
     result:=Distance;
    end;
   end;
  end else begin
   // Point lies inside convex hull
  end;
 end else begin
  result:=MAX_SCALAR;
 end;
end;

function TKraftConvexHull.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 if (Index>=0) and (Index<fCountVertices) then begin
  result:=fVertices[Index].Position;
 end else begin
  result:=fMassData.Center; //Vector3Origin;
 end;
end;

function TKraftConvexHull.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
var Normal:TKraftVector3;
    Index,BestVertexIndex,NewVertexIndex,CurrentVertexIndex:longint;
    BestDistance,NewDistance,CurrentDistance:TKraftScalar;
    Vertex,CurrentVertex:PKraftConvexHullVertex;
begin
 result:=-1;
 if fCountVertices>0 then begin
  Normal:=Vector3SafeNorm(Direction);
  BestVertexIndex:=0;
  BestDistance:=Vector3Dot(fVertices[BestVertexIndex].Position,Normal);
  if fCountVertices<32 then begin
   for Index:=1 to fCountVertices-1 do begin
    CurrentDistance:=Vector3Dot(fVertices[Index].Position,Normal);
    if BestDistance<CurrentDistance then begin
     BestDistance:=CurrentDistance;
     BestVertexIndex:=Index;
    end;
   end;
  end else begin
   repeat
    NewVertexIndex:=BestVertexIndex;
    NewDistance:=BestDistance;
    Vertex:=@fVertices[BestVertexIndex];
    for Index:=0 to Vertex^.CountAdjacencies-1 do begin
     CurrentVertexIndex:=Vertex^.Adjacencies[Index];
     CurrentVertex:=@fVertices[CurrentVertexIndex];
     CurrentDistance:=Vector3Dot(CurrentVertex^.Position,Normal);
     if NewDistance<CurrentDistance then begin
      NewVertexIndex:=CurrentVertexIndex;
      NewDistance:=CurrentDistance;
     end;
    end;
    if NewVertexIndex=BestVertexIndex then begin
     break;
    end;
    BestVertexIndex:=NewVertexIndex;
    BestDistance:=NewDistance;
   until false;
  end;
  result:=BestVertexIndex;
 end;
end;

function TKraftConvexHull.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
var Normal:TKraftVector3;
    Index,BestVertexIndex,NewVertexIndex,CurrentVertexIndex:longint;
    BestDistance,NewDistance,CurrentDistance:TKraftScalar;
    Vertex,CurrentVertex:PKraftConvexHullVertex;
begin
 result:=fMassData.Center;
 if fCountVertices>0 then begin
  Normal:=Vector3SafeNorm(Direction);
  BestVertexIndex:=0;
  BestDistance:=Vector3Dot(fVertices[BestVertexIndex].Position,Normal);
  if fCountVertices<32 then begin
   for Index:=1 to fCountVertices-1 do begin
    CurrentDistance:=Vector3Dot(fVertices[Index].Position,Normal);
    if BestDistance<CurrentDistance then begin
     BestDistance:=CurrentDistance;
     BestVertexIndex:=Index;
    end;
   end;
  end else begin
   repeat
    NewVertexIndex:=BestVertexIndex;
    NewDistance:=BestDistance;
    Vertex:=@fVertices[BestVertexIndex];
    for Index:=0 to Vertex^.CountAdjacencies-1 do begin
     CurrentVertexIndex:=Vertex^.Adjacencies[Index];
     CurrentVertex:=@fVertices[CurrentVertexIndex];
     CurrentDistance:=Vector3Dot(CurrentVertex^.Position,Normal);
     if NewDistance<CurrentDistance then begin
      NewVertexIndex:=CurrentVertexIndex;
      NewDistance:=CurrentDistance;
     end;
    end;
    if NewVertexIndex=BestVertexIndex then begin
     break;
    end;
    BestVertexIndex:=NewVertexIndex;
    BestDistance:=NewDistance;
   until false;
  end;
  result:=fVertices[BestVertexIndex].Position;
 end;
end;

constructor TKraftMesh.Create(const APhysics:TKraft);
begin

 inherited Create;

 fPhysics:=APhysics;

 fVertices:=nil;
 fCountVertices:=0;

 fNormals:=nil;
 fCountNormals:=0;

 fTriangles:=nil;
 fCountTriangles:=0;

 fNodes:=nil;
 fCountNodes:=0;

 fSkipListNodes:=nil;
 fCountSkipListNodes:=0;

 fDoubleSided:=true;
 
 if assigned(fPhysics.fMeshLast) then begin
  fPhysics.fMeshLast.fNext:=self;
  fPrevious:=fPhysics.fMeshLast;
 end else begin
  fPhysics.fMeshFirst:=self;
  fPrevious:=nil;
 end;
 fPhysics.fMeshLast:=self;
 fNext:=nil;

end;

destructor TKraftMesh.Destroy;
begin

 SetLength(fVertices,0);

 SetLength(fNormals,0);

 SetLength(fTriangles,0);

 SetLength(fNodes,0);

 SetLength(fSkipListNodes,0);

 if assigned(fPrevious) then begin
  fPrevious.fNext:=fNext;
 end else if fPhysics.fMeshFirst=self then begin
  fPhysics.fMeshFirst:=fNext;
 end;
 if assigned(fNext) then begin
  fNext.fPrevious:=fPrevious;
 end else if fPhysics.fMeshLast=self then begin
  fPhysics.fMeshLast:=fPrevious;
 end;
 fPrevious:=nil;
 fNext:=nil;

 inherited Destroy;

end;

function TKraftMesh.AddVertex(const AVertex:TKraftVector3):longint;
begin
 result:=fCountVertices;
 inc(fCountVertices);
 if fCountVertices>length(fVertices) then begin
  SetLength(fVertices,fCountVertices*2);
 end;
 fVertices[result]:=AVertex;
end;

function TKraftMesh.AddNormal(const ANormal:TKraftVector3):longint;
begin
 result:=fCountNormals;
 inc(fCountNormals);
 if fCountNormals>length(fNormals) then begin
  SetLength(fNormals,fCountNormals*2);
 end;
 fNormals[result]:=ANormal;
end;

function TKraftMesh.AddTriangle(const AVertexIndex0,AVertexIndex1,AVertexIndex2:longint;const ANormalIndex0:longint=-1;const ANormalIndex1:longint=-1;ANormalIndex2:longint=-1):longint;
var Triangle:PKraftMeshTriangle;
begin
 result:=fCountTriangles;
 inc(fCountTriangles);
 if fCountTriangles>length(fTriangles) then begin
  SetLength(fTriangles,fCountTriangles*2);
 end;
 Triangle:=@fTriangles[result];
 Triangle^.Vertices[0]:=AVertexIndex0;
 Triangle^.Vertices[1]:=AVertexIndex1;
 Triangle^.Vertices[2]:=AVertexIndex2;
 Triangle^.Normals[0]:=ANormalIndex0;
 Triangle^.Normals[1]:=ANormalIndex1;
 Triangle^.Normals[2]:=ANormalIndex2;
 Triangle^.Plane.Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(fVertices[Triangle^.Vertices[1]],fVertices[Triangle^.Vertices[0]]),Vector3Sub(fVertices[Triangle^.Vertices[2]],fVertices[Triangle^.Vertices[0]])));
 Triangle^.Plane.Distance:=-Vector3Dot(Triangle^.Plane.Normal,fVertices[Triangle^.Vertices[0]]);
 Triangle^.Next:=-1;
end;

procedure TKraftMesh.Load(const AVertices:PKraftVector3;const ACountVertices:longint;const ANormals:PKraftVector3;const ACountNormals:longint;const AVertexIndices,ANormalIndices:pointer;const ACountIndices:longint);
var i:longint;
    Triangle:PKraftMeshTriangle;
    v,n:plongint;
    HasNormals:boolean;
begin

 HasNormals:=assigned(ANormals) and (ACountNormals>0) and assigned(ANormalIndices);

 fVertices:=nil;
 fCountVertices:=ACountVertices;
 SetLength(fVertices,fCountVertices);
 for i:=0 to fCountVertices-1 do begin
  fVertices[i]:=PKraftVector3s(AVertices)^[i];
 end;

 fNormals:=nil;
 if HasNormals then begin
  fCountNormals:=ACountNormals;
  SetLength(fNormals,fCountNormals);
  for i:=0 to fCountNormals-1 do begin
   fNormals[i]:=PKraftVector3s(ANormals)^[i];
  end;
 end else begin
  fCountNormals:=0;
 end;

 fTriangles:=nil;
 fCountTriangles:=ACountIndices div 3;
 SetLength(fTriangles,fCountTriangles);
 v:=AVertexIndices;
 n:=ANormalIndices;
 for i:=0 to fCountTriangles-1 do begin
  Triangle:=@fTriangles[i];
  Triangle^.Vertices[0]:=v^;
  inc(v);
  Triangle^.Vertices[1]:=v^;
  inc(v);
  Triangle^.Vertices[2]:=v^;
  inc(v);
  if HasNormals then begin
   Triangle^.Normals[0]:=n^;
   inc(n);
   Triangle^.Normals[1]:=n^;
   inc(n);
   Triangle^.Normals[2]:=n^;
   inc(n);
  end else begin
   Triangle^.Normals[0]:=-1;
   Triangle^.Normals[1]:=-1;
   Triangle^.Normals[2]:=-1;
  end;
  Triangle^.Plane.Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(fVertices[Triangle^.Vertices[1]],fVertices[Triangle^.Vertices[0]]),Vector3Sub(fVertices[Triangle^.Vertices[2]],fVertices[Triangle^.Vertices[0]])));
  Triangle^.Plane.Distance:=-Vector3Dot(Triangle^.Plane.Normal,fVertices[Triangle^.Vertices[0]]);
  Triangle^.Next:=-1;
 end;

end;

procedure TKraftMesh.Load(const ASourceData:pointer;const ASourceSize:longint);
type TFileSignature=array[0..3] of ansichar;
var SrcPos:longint;
    Signature:TFileSignature;
 function Read(var Dst;DstLen:longword):longword;
 begin
  result:=ASourceSize-SrcPos;
  if result>DstLen then begin
   result:=DstLen;
  end;
  if result>0 then begin
   Move(PAnsiChar(ASourceData)[SrcPos],Dst,result);
   inc(SrcPos,result);
  end;
 end;
 function ReadByte:byte;
 begin
  Read(result,SizeOf(byte));
 end;
 function ReadWord:word;
 begin
  Read(result,SizeOf(word));
 end;
 function ReadLongWord:longword;
 begin
  Read(result,SizeOf(longword));
 end;
 function ReadFloat:single;
 begin
  Read(result,SizeOf(single));
 end;
 procedure LoadPMF;
 type TFace=record
       Indices:array[0..2] of longword;
      end;
      PFaces=^TFaces;
      TFaces=array[0..0] of TFace;
 var Counter:longint;
     Triangle:PKraftMeshTriangle;
 begin
  fCountTriangles:=ReadLongWord;
  SetLength(fTriangles,fCountTriangles);
  fCountVertices:=ReadLongWord;
  if (fCountTriangles>0) and (fCountVertices>0) then begin
   for Counter:=0 to fCountTriangles-1 do begin
    Triangle:=@fTriangles[Counter];
    Triangle^.Vertices[0]:=ReadLongWord;
    Triangle^.Vertices[1]:=ReadLongWord;
    Triangle^.Vertices[2]:=ReadLongWord;
    Triangle^.Normals[0]:=-1;
    Triangle^.Normals[1]:=-1;
    Triangle^.Normals[2]:=-1;
   end;
   SetLength(fVertices,fCountVertices);
   for Counter:=0 to fCountVertices-1 do begin
    fVertices[Counter].x:=ReadFloat;
    fVertices[Counter].y:=ReadFloat;
    fVertices[Counter].z:=ReadFloat;
   end;
   for Counter:=0 to fCountTriangles-1 do begin
    Triangle:=@fTriangles[Counter];
    Triangle^.Plane.Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(fVertices[Triangle^.Vertices[1]],fVertices[Triangle^.Vertices[0]]),Vector3Sub(fVertices[Triangle^.Vertices[2]],fVertices[Triangle^.Vertices[0]])));
    Triangle^.Plane.Distance:=-Vector3Dot(Triangle^.Plane.Normal,fVertices[Triangle^.Vertices[0]]);
    Triangle^.Next:=-1;
   end;
  end;
 end;
 function Load3DS(SrcData:pointer;SrcSize:longword):boolean;
 const CHUNK_3DS_MAIN=$4d4d;
       CHUNK_3DS_OBJMESH=$3d3d;
       CHUNK_3DS_OBJBLOCK=$4000;
       CHUNK_3DS_TRIMESH=$4100;
       CHUNK_3DS_VERTLIST=$4110;
       CHUNK_3DS_FACELIST=$4120;
       CHUNK_3DS_MAPLIST=$4140;
       CHUNK_3DS_SMOOTHLIST=$4150;
       CHUNK_3DS_MESHMATRIX=$4160;
 type PVector2=^TVector2;
      TVector2=TKraftVector2;
      PVector2Array=^TVector2Array;
      TVector2Array=array[0..0] of TVector2;
      PVector3Array=^TKraftVector3Array;
      TKraftVector3Array=array[0..0] of TKraftVector3;
      PFace3DS=^TFace3DS;
      TFace3DS=record
       Indices:array[0..2] of longword;
       Flags:longword;
       SmoothGroup:longword;
      end;
      PFaces3DS=^TFaces3DS;
      TFaces3DS=array[0..0] of TFace3DS;
      PObject3DSMesh=^TObject3DSMesh;
      TObject3DSMesh=record
       Vertices:PVector3Array;
       NumVertices:longint;
       TexCoords:PVector2Array;
       NumTexCoords:longint;
       Faces:PFaces3DS;
       NumFaces:longint;
       Matrix:TKraftMatrix4x4;
      end;
      PObject3DSMeshs=^TObject3DSMeshs;
      TObject3DSMeshs=array[0..0] of TObject3DSMesh;
      PObject3DS=^TObject3DS;
      TObject3DS=record
       Name:PAnsiChar;
       Meshs:PObject3DSMeshs;
       NumMeshs:longint;
      end;
      PObjects3DS=^TObjects3DS;
      TObjects3DS=array[0..0] of TObject3DS;
 var SrcPos:longword;
     Signature3DS:word;
     Size3DS:longword;
     Objects3DS:PObjects3DS;
     NumObjects3DS:longint;
  function Read(var Dst;DstLen:longword):longword;
  begin
   result:=SrcSize-SrcPos;
   if result>DstLen then begin
    result:=DstLen;
   end;
   if result>0 then begin
    Move(PAnsiChar(SrcData)[SrcPos],Dst,result);
    inc(SrcPos,result);
   end;
  end;
  function ReadByte:byte;
  begin
   Read(result,SizeOf(byte));
  end;
  function ReadWord:word;
  begin
   Read(result,SizeOf(word));
  end;
  function ReadLongWord:longword;
  begin
   Read(result,SizeOf(longword));
  end;
  function ReadFloat:single;
  begin
   Read(result,SizeOf(single));
  end;
  procedure ReallocateMemory(var p;Size:longint);
  begin
   if assigned(pointer(p)) then begin
    if Size=0 then begin
     FreeMem(pointer(p));
     pointer(p):=nil;
    end else begin
     ReallocMem(pointer(p),Size);
    end;
   end else if Size<>0 then begin
    GetMem(pointer(p),Size);
   end;
  end;
  function Read3DSChunks(const ParentChunk,Bytes:longword):longword; forward;
  function Skip3DSString:longword;
  var c:ansichar;
  begin
   result:=0;
   c:=#255;
   while c<>#0 do begin
    if Read(c,SizeOf(ansichar))<>SizeOf(ansichar) then begin
     break;
    end;
    inc(result);
   end;
  end;
  function Read3DSString(var p:PAnsiChar):longword;
  var c:ansichar;
      OldPos:longword;
  begin
   OldPos:=SrcPos;
   result:=0;
   c:=#255;
   while c<>#0 do begin
    if Read(c,SizeOf(ansichar))<>SizeOf(ansichar) then begin
     break;
    end;
    inc(result);
   end;
   GetMem(p,result);
   SrcPos:=OldPos;
   result:=0;
   c:=#255;
   while c<>#0 do begin
    if Read(c,SizeOf(ansichar))<>SizeOf(ansichar) then begin
     break;
    end;
    p[result]:=c;
    inc(result);
   end;
  end;
  function Read3DSChunk(const ParentChunk:longword):longword;
  var Chunk:word;
      Size,i,j:longword;
      Vertex:PKraftVector3;
      TexCoord:PVector2;
      Face:PFace3DS;
  begin
   if Read(Chunk,SizeOf(word))<>SizeOf(word) then begin
    result:=$80000000;
    exit;
   end;
   if Read(result,SizeOf(longword))<>SizeOf(longword) then begin
    result:=$80000000;
    exit;
   end;
   Size:=result-6;
   case ParentChunk of
    CHUNK_3DS_MAIN:begin
     case Chunk of
      CHUNK_3DS_OBJMESH:begin
       Read3DSChunks(Chunk,Size);
      end;
      else begin
       inc(SrcPos,Size);
      end;
     end;
    end;
    CHUNK_3DS_OBJMESH:begin
     case Chunk of
      CHUNK_3DS_OBJBLOCK:begin
       inc(NumObjects3DS);
       ReallocateMemory(Objects3DS,NumObjects3DS*SizeOf(TObject3DS));
       FillChar(Objects3DS^[NumObjects3DS-1],SizeOf(TObject3DS),#0);
       dec(Size,Read3DSString(Objects3DS^[NumObjects3DS-1].Name));
       Read3DSChunks(Chunk,Size);
      end;
      else begin
       inc(SrcPos,Size);
      end;
     end;
    end;
    CHUNK_3DS_OBJBLOCK:begin
     case Chunk of
      CHUNK_3DS_TRIMESH:begin
       inc(Objects3DS^[NumObjects3DS-1].NumMeshs);
       ReallocateMemory(Objects3DS^[NumObjects3DS-1].Meshs,Objects3DS^[NumObjects3DS-1].NumMeshs*SizeOf(TObject3DSMesh));
       FillChar(Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1],SizeOf(TObject3DSMesh),#0);
       Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Matrix:=Matrix4x4Identity;
       Read3DSChunks(Chunk,Size);
      end;
      else begin
       inc(SrcPos,Size);
      end;
     end;
    end;
    CHUNK_3DS_TRIMESH:begin
     case Chunk of
      CHUNK_3DS_VERTLIST:begin
       Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumVertices:=ReadWord;
       ReallocateMemory(Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Vertices,Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumVertices*SizeOf(TKraftVector3));
       Vertex:=@Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Vertices^[0];
       for i:=1 to Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumVertices do begin
        Vertex^.x:=ReadFloat;
        Vertex^.y:=ReadFloat;
        Vertex^.z:=ReadFloat;
        inc(Vertex);
       end;
      end;
      CHUNK_3DS_MAPLIST:begin
       Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumTexCoords:=ReadWord;
       ReallocateMemory(Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].TexCoords,Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumTexCoords*SizeOf(TVector2));
       TexCoord:=@Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].TexCoords^[0];
       for i:=1 to Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumTexCoords do begin
        TexCoord^.x:=ReadFloat;
        TexCoord^.y:=ReadFloat;
        inc(TexCoord);
       end;
      end;
      CHUNK_3DS_FACELIST:begin
       Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumFaces:=ReadWord;
       ReallocateMemory(Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Faces,Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumFaces*SizeOf(TFace3DS));
       Face:=@Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Faces^[0];
       for i:=1 to Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumFaces do begin
        Face^.Indices[0]:=ReadWord;
        Face^.Indices[1]:=ReadWord;
        Face^.Indices[2]:=ReadWord;
        Face^.Flags:=ReadWord;
        inc(Face);
       end;
       dec(Size,(Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumFaces*4)+2);
       Read3DSChunks(Chunk,Size);
      end;
      CHUNK_3DS_MESHMATRIX:begin
       Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Matrix:=Matrix4x4Identity;
       for i:=0 to 3 do begin
        for j:=0 to 2 do begin
         Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Matrix[i,j]:=ReadFloat;
        end;
       end;
      end;
      else begin
       inc(SrcPos,Size);
      end;
     end;
    end;
    CHUNK_3DS_FACELIST:begin
     case Chunk of
      CHUNK_3DS_SMOOTHLIST:begin
       Face:=@Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].Faces^[0];
       for i:=1 to Objects3DS^[NumObjects3DS-1].Meshs^[Objects3DS^[NumObjects3DS-1].NumMeshs-1].NumFaces do begin
        Face^.SmoothGroup:=ReadLongWord;
        inc(Face);
       end;
      end;
      else begin
       inc(SrcPos,Size);
      end;
     end;
    end;
    else begin
     inc(SrcPos,Size);
    end;
   end;
  end;
  function Read3DSChunks(const ParentChunk,Bytes:longword):longword;
  begin
   result:=0;
   while result<Bytes do begin
    inc(result,Read3DSChunk(ParentChunk));
   end;
  end;
  procedure Convert3DS;
  var i,j,k,h:longint;
      v:array[0..2] of TKraftVector3;
      tv:TKraftVector3;
  begin
   for i:=0 to NumObjects3DS-1 do begin
    for j:=0 to Objects3DS^[i].NumMeshs-1 do begin
     for k:=0 to Objects3DS^[i].Meshs^[j].NumFaces-1 do begin
      for h:=0 to 2 do begin
       tv:=Objects3DS^[i].Meshs^[j].Vertices^[Objects3DS^[i].Meshs^[j].Faces^[k].Indices[h]];
       //Vector3MatrixMul(tv,Objects3DS^[i].Meshs^[j].Matrix);
       v[h].x:=tv.x;
       v[h].y:=tv.z;
       v[h].z:=-tv.y;
      end;
      AddTriangle(AddVertex(v[0]),AddVertex(v[1]),AddVertex(v[2]));
     end;
    end;
   end;
  end;
  procedure Free3DS;
  var i,j:longint;
  begin
   for i:=0 to NumObjects3DS-1 do begin
    for j:=0 to Objects3DS^[i].NumMeshs-1 do begin
     ReallocateMemory(Objects3DS^[i].Meshs^[j].Vertices,0);
     ReallocateMemory(Objects3DS^[i].Meshs^[j].TexCoords,0);
     ReallocateMemory(Objects3DS^[i].Meshs^[j].Faces,0);
    end;
    ReallocateMemory(Objects3DS^[i].Meshs,0);
   end;
   ReallocateMemory(Objects3DS,0);
  end;
 begin
  result:=false;
  if SrcSize>0 then begin
   SrcPos:=0;
   if Read(Signature3DS,SizeOf(word))=SizeOf(word) then begin
    if Signature3DS=CHUNK_3DS_MAIN then begin
     if Read(Size3DS,SizeOf(longword))=SizeOf(longword) then begin
      Objects3DS:=nil;
      NumObjects3DS:=0;
      result:=Read3DSChunks(Signature3DS,Size3DS)>0;
      if assigned(Objects3DS) then begin
       if result then begin
        Convert3DS;
       end;
       Free3DS;
      end;
     end;
    end;
   end;
  end;
 end;
var OK:boolean;
begin
 OK:=false;
 if ASourceSize>SizeOf(TFileSignature) then begin
  SrcPos:=0;
  if Read(Signature,SizeOf(TFileSignature))=SizeOf(TFileSignature) then begin
   if (Signature[0]='P') and (Signature[1]='M') and (Signature[2]='F') and (Signature[3]='0') then begin
    LoadPMF;
    OK:=true;
   end;
  end;
  if not OK then begin
   if Load3DS(ASourceData,ASourceSize) then begin
    OK:=true;
   end;
  end;
 end;
 if not OK then begin
  raise EKraftCorruptMeshData.Create('Corrupt mesh data');
 end;
end;

procedure TKraftMesh.Scale(const WithFactor:TKraftScalar);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3Scale(fVertices[Index],WithFactor);
 end;
end;

procedure TKraftMesh.Scale(const WithVector:TKraftVector3);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3Scale(fVertices[Index],WithVector.x,WithVector.y,WithVector.z);
 end;
end;

procedure TKraftMesh.Transform(const WithMatrix:TKraftMatrix3x3);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3MatrixMul(fVertices[Index],WithMatrix);
 end;
end;

procedure TKraftMesh.Transform(const WithMatrix:TKraftMatrix4x4);
var Index:longint;
begin
 for Index:=0 to fCountVertices-1 do begin
  Vector3MatrixMul(fVertices[Index],WithMatrix);
 end;
end;

procedure TKraftMesh.CalculateNormals;
var TriangleIndex,NormalIndex,Counter:longint;
    NormalCounts:array of longint;
    Triangle:PKraftMeshTriangle;
begin
 NormalCounts:=nil;
 try
  if fCountTriangles>0 then begin
   fCountNormals:=fCountVertices;
   SetLength(NormalCounts,fCountNormals);
   SetLength(fNormals,fCountNormals);
   for NormalIndex:=0 to fCountNormals-1 do begin
    NormalCounts[NormalIndex]:=0;
    fNormals[NormalIndex]:=Vector3Origin;
   end;
   for TriangleIndex:=0 to fCountTriangles-1 do begin
    Triangle:=@fTriangles[TriangleIndex];
    Triangle^.Normals[0]:=Triangle^.Vertices[0];
    Triangle^.Normals[1]:=Triangle^.Vertices[1];
    Triangle^.Normals[2]:=Triangle^.Vertices[2];
    Triangle^.Plane.Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(fVertices[Triangle^.Vertices[1]],fVertices[Triangle^.Vertices[0]]),Vector3Sub(fVertices[Triangle^.Vertices[2]],fVertices[Triangle^.Vertices[0]])));
    Triangle^.Plane.Distance:=-Vector3Dot(Triangle^.Plane.Normal,fVertices[Triangle^.Vertices[0]]);
    for Counter:=0 to 2 do begin
     NormalIndex:=Triangle^.Normals[Counter];
     inc(NormalCounts[NormalIndex]);
     Vector3DirectAdd(fNormals[NormalIndex],Triangle^.Plane.Normal);
    end;
   end;
   for NormalIndex:=0 to fCountNormals-1 do begin
    Vector3Normalize(fNormals[NormalIndex]);
   end;
  end;
 finally
  SetLength(NormalCounts,0);
 end;
end;

procedure TKraftMesh.Finish;
type PAABBTreeNode=^TAABBTreeNode;
     TAABBTreeNode=record
      AABB:TKraftAABB;
      Children:array[0..1] of longint;
      TriangleIndex:longint;
     end;
     TAABBTreeNodes=array of TAABBTreeNode;
var Counter,StackPointer,CurrentNodeIndex,LeftCount,RightCount,ParentCount,AxisCounter,Axis,BestAxis,TriangleIndex,
    Balance,BestBalance,NextTriangleIndex,LeftNodeIndex,RightNodeIndex,TargetNodeIndex,Count,Root,CountNodes,Index,
    NodeID,Pass,NewStackCapacity:longint;
    Stack:array of longint;
    Points:array[0..2] of array of TKraftScalar;
    Center,Median:TKraftVector3;
    LeftAABB,RightAABB:TKraftAABB;
    Nodes:TAABBTreeNodes;
    Node:PAABBTreeNode;
    SkipListNode:PKraftMeshSkipListNode;
    Triangle:PKraftMeshTriangle;
    TriangleAABBs:array of TKraftAABB;
    CurrentAABB:PKraftAABB;
    v0,v1,v2:PKraftVector3;
begin
 if length(fVertices)<>fCountVertices then begin
  SetLength(fVertices,fCountVertices);
 end;
 if length(fTriangles)<>fCountTriangles then begin
  SetLength(fTriangles,fCountTriangles);
 end;
 for Index:=0 to fCountTriangles-1 do begin
  Triangle:=@fTriangles[Index];
  if (Triangle^.Normals[0]<0) or (Triangle^.Normals[1]<0) or (Triangle^.Normals[2]<0) then begin
   CalculateNormals;
   break;
  end;
 end;
 if fCountSkipListNodes=0 then begin
  if fCountTriangles>0 then begin
   Stack:=nil;
   Nodes:=nil;
   Points[0]:=nil;
   Points[1]:=nil;
   Points[2]:=nil;
   TriangleAABBs:=nil;
   try
    SetLength(TriangleAABBs,fCountTriangles);
    for Index:=0 to fCountTriangles-1 do begin
     Triangle:=@fTriangles[Index];
     v0:=@fVertices[Triangle^.Vertices[0]];
     v1:=@fVertices[Triangle^.Vertices[1]];
     v2:=@fVertices[Triangle^.Vertices[2]];
     CurrentAABB:=@TriangleAABBs[Index];
     CurrentAABB^.Min.x:=Min(Min(v0^.x,v1^.x),v2^.x)-EPSILON;
     CurrentAABB^.Min.y:=Min(Min(v0^.y,v1^.y),v2^.y)-EPSILON;
     CurrentAABB^.Min.z:=Min(Min(v0^.z,v1^.z),v2^.z)-EPSILON;
     CurrentAABB^.Max.x:=Max(Max(v0^.x,v1^.x),v2^.x)+EPSILON;
     CurrentAABB^.Max.y:=Max(Max(v0^.y,v1^.y),v2^.y)+EPSILON;
     CurrentAABB^.Max.z:=Max(Max(v0^.z,v1^.z),v2^.z)+EPSILON;
     Triangle^.AABB:=TriangleAABBs[Index];
    end;
    Root:=0;
    CountNodes:=1;
    SetLength(Nodes,Max(CountNodes,fCountTriangles));
    SetLength(Points[0],fCountTriangles*6);
    SetLength(Points[1],fCountTriangles*6);
    SetLength(Points[2],fCountTriangles*6);
    Nodes[0].AABB:=TriangleAABBs[0];
    for Counter:=1 to fCountTriangles-1 do begin
     Nodes[0].AABB:=AABBCombine(Nodes[0].AABB,TriangleAABBs[Counter]);
    end;
    for Counter:=0 to fCountTriangles-2 do begin
     fTriangles[Counter].Next:=Counter+1;
    end;
    fTriangles[fCountTriangles-1].Next:=-1;
    Nodes[0].TriangleIndex:=0;
    Nodes[0].Children[0]:=-1;
    Nodes[0].Children[1]:=-1;
    SetLength(Stack,16);
    Stack[0]:=0;
    StackPointer:=1;
    while StackPointer>0 do begin
     dec(StackPointer);
     CurrentNodeIndex:=Stack[StackPointer];
     if (CurrentNodeIndex>=0) and (Nodes[CurrentNodeIndex].TriangleIndex>=0) then begin
      TriangleIndex:=Nodes[CurrentNodeIndex].TriangleIndex;
      Nodes[CurrentNodeIndex].AABB:=TriangleAABBs[TriangleIndex];
      TriangleIndex:=fTriangles[TriangleIndex].Next;
      ParentCount:=1;
      while TriangleIndex>=0 do begin
       Nodes[CurrentNodeIndex].AABB:=AABBCombine(Nodes[CurrentNodeIndex].AABB,TriangleAABBs[TriangleIndex]);
       inc(ParentCount);
       TriangleIndex:=fTriangles[TriangleIndex].Next;
      end;
      if ParentCount>3 then begin
       Center:=Vector3Avg(Nodes[CurrentNodeIndex].AABB.Min,Nodes[CurrentNodeIndex].AABB.Max);
       TriangleIndex:=Nodes[CurrentNodeIndex].TriangleIndex;
       Count:=0;
       while TriangleIndex>=0 do begin
        v0:=@fVertices[fTriangles[TriangleIndex].Vertices[0]];
        v1:=@fVertices[fTriangles[TriangleIndex].Vertices[1]];
        v2:=@fVertices[fTriangles[TriangleIndex].Vertices[2]];
        Points[0,Count]:=v0^.x;
        Points[1,Count]:=v0^.y;
        Points[2,Count]:=v0^.z;
        Points[0,Count+1]:=v1^.x;
        Points[1,Count+1]:=v1^.y;
        Points[2,Count+1]:=v1^.z;
        Points[0,Count+2]:=v2^.x;
        Points[1,Count+2]:=v2^.y;
        Points[2,Count+2]:=v2^.z;
        inc(Count,3);
        TriangleIndex:=fTriangles[TriangleIndex].Next;
       end;
       if Count>1 then begin
        DirectIntroSort(@Points[0,0],0,Count-1,SizeOf(TKraftScalar),@CompareFloat);
        DirectIntroSort(@Points[0,1],0,Count-1,SizeOf(TKraftScalar),@CompareFloat);
        DirectIntroSort(@Points[0,2],0,Count-1,SizeOf(TKraftScalar),@CompareFloat);
        Median.x:=Points[0,Count shr 1];
        Median.y:=Points[1,Count shr 1];
        Median.z:=Points[2,Count shr 1];
       end else begin
        Median:=Center;
       end;
       BestAxis:=-1;
       BestBalance:=$7fffffff;
       for AxisCounter:=0 to 5 do begin
        if AxisCounter>2 then begin
         Axis:=AxisCounter-3;
        end else begin
         Axis:=AxisCounter;
        end;
        LeftCount:=0;
        RightCount:=0;
        LeftAABB:=Nodes[CurrentNodeIndex].AABB;
        RightAABB:=Nodes[CurrentNodeIndex].AABB;
        if AxisCounter>2 then begin
         LeftAABB.Max.xyz[Axis]:=Median.xyz[Axis];
         RightAABB.Min.xyz[Axis]:=Median.xyz[Axis];
        end else begin
         LeftAABB.Max.xyz[Axis]:=Center.xyz[Axis];
         RightAABB.Min.xyz[Axis]:=Center.xyz[Axis];
        end;
        TriangleIndex:=Nodes[CurrentNodeIndex].TriangleIndex;
        while TriangleIndex>=0 do begin
         if Vector3Avg(TriangleAABBs[TriangleIndex].Min,TriangleAABBs[TriangleIndex].Max).xyz[Axis]<RightAABB.Min.xyz[Axis] then begin
          inc(LeftCount);
         end else begin
          inc(RightCount);
         end;
         TriangleIndex:=fTriangles[TriangleIndex].Next;
        end;
        if (LeftCount>0) and (RightCount>0) then begin
         Balance:=abs(RightCount-LeftCount);
         if BestBalance>Balance then begin
          BestBalance:=Balance;
          BestAxis:=AxisCounter;
         end;
        end;
       end;
       if BestAxis>=0 then begin
        LeftNodeIndex:=CountNodes;
        RightNodeIndex:=CountNodes+1;
        inc(CountNodes,2);
        if CountNodes>=length(Nodes) then begin
         SetLength(Nodes,RoundUpToPowerOfTwo(CountNodes));
        end;
        LeftAABB:=Nodes[CurrentNodeIndex].AABB;
        RightAABB:=Nodes[CurrentNodeIndex].AABB;
        TriangleIndex:=Nodes[CurrentNodeIndex].TriangleIndex;
        Nodes[LeftNodeIndex].TriangleIndex:=-1;
        Nodes[RightNodeIndex].TriangleIndex:=-1;
        Nodes[CurrentNodeIndex].TriangleIndex:=-1;
        if BestAxis>2 then begin
         dec(BestAxis,3);
         LeftAABB.Max.xyz[BestAxis]:=Median.xyz[BestAxis];
         RightAABB.Min.xyz[BestAxis]:=Median.xyz[BestAxis];
        end else begin
         LeftAABB.Max.xyz[BestAxis]:=Center.xyz[BestAxis];
         RightAABB.Min.xyz[BestAxis]:=Center.xyz[BestAxis];
        end;
        Nodes[LeftNodeIndex].AABB:=LeftAABB;
        Nodes[RightNodeIndex].AABB:=RightAABB;
        while TriangleIndex>=0 do begin
         NextTriangleIndex:=fTriangles[TriangleIndex].Next;
         if Vector3Avg(TriangleAABBs[TriangleIndex].Min,TriangleAABBs[TriangleIndex].Max).xyz[BestAxis]<RightAABB.Min.xyz[BestAxis] then begin
          TargetNodeIndex:=LeftNodeIndex;
         end else begin
          TargetNodeIndex:=RightNodeIndex;
         end;
         fTriangles[TriangleIndex].Next:=Nodes[TargetNodeIndex].TriangleIndex;
         if Nodes[TargetNodeIndex].TriangleIndex<0 then begin
          Nodes[TargetNodeIndex].AABB:=TriangleAABBs[TriangleIndex];
         end else begin
          Nodes[TargetNodeIndex].AABB:=AABBCombine(Nodes[TargetNodeIndex].AABB,TriangleAABBs[TriangleIndex]);
         end;
         Nodes[TargetNodeIndex].TriangleIndex:=TriangleIndex;
         TriangleIndex:=NextTriangleIndex;
        end;
        Nodes[CurrentNodeIndex].Children[0]:=LeftNodeIndex;
        Nodes[CurrentNodeIndex].Children[1]:=RightNodeIndex;
        Nodes[LeftNodeIndex].Children[0]:=-1;
        Nodes[LeftNodeIndex].Children[1]:=-1;
        Nodes[RightNodeIndex].Children[0]:=-1;
        Nodes[RightNodeIndex].Children[1]:=-1;
        if (StackPointer+2)>=length(Stack) then begin
         SetLength(Stack,RoundUpToPowerOfTwo(StackPointer+2));
        end;
        Stack[StackPointer+0]:=RightNodeIndex;
        Stack[StackPointer+1]:=LeftNodeIndex;
        inc(StackPointer,2);
       end;
      end;
     end;
    end;
    SetLength(Nodes,CountNodes);
    begin
     fCountNodes:=CountNodes;
     fCountSkipListNodes:=0;
     if Root>=0 then begin
      begin
       // Pass 1 - Counting
       NewStackCapacity:=RoundUpToPowerOfTwo(CountNodes*4);
       if NewStackCapacity>length(Stack) then begin
        SetLength(Stack,NewStackCapacity);
       end;
       Stack[0]:=Root;
       StackPointer:=1;
       while StackPointer>0 do begin
        dec(StackPointer);
        NodeID:=Stack[StackPointer];
        if (NodeID>=0) and (NodeID<CountNodes) then begin
         Node:=@Nodes[NodeID];
         inc(fCountSkipListNodes);
         if Node^.Children[0]>=0 then begin
          NewStackCapacity:=RoundUpToPowerOfTwo(StackPointer+2);
          if NewStackCapacity>length(Stack) then begin
           SetLength(Stack,NewStackCapacity);
          end;
          Stack[StackPointer+0]:=Node^.Children[1];
          Stack[StackPointer+1]:=Node^.Children[0];
          inc(StackPointer,2);
         end;
        end;
       end;
      end;
      begin
       // Pass 2 - Resize arrays
       SetLength(fNodes,fCountNodes);
       SetLength(fSkipListNodes,fCountSkipListNodes);
      end;
      begin
       // Pass 3 - Fill arrays
       for Index:=0 to CountNodes-1 do begin
        Node:=@Nodes[Index];
        fNodes[Index].Children[0]:=Node^.Children[0];
        fNodes[Index].Children[1]:=Node^.Children[1];
        fNodes[Index].TriangleIndex:=Node^.TriangleIndex;
        fNodes[Index].AABB:=Node^.AABB;
       end;          
       fCountSkipListNodes:=0;
       Stack[0]:=Root;
       Stack[1]:=0;
       Stack[2]:=0;
       StackPointer:=3;
       while StackPointer>0 do begin
        dec(StackPointer,3);
        NodeID:=Stack[StackPointer];
        Pass:=Stack[StackPointer+1];
        Index:=Stack[StackPointer+2];
        if (NodeID>=0) and (NodeID<CountNodes) then begin
         Node:=@Nodes[NodeID];
         case Pass of
          0:begin
           Index:=fCountSkipListNodes;
           inc(fCountSkipListNodes);
           SkipListNode:=@fSkipListNodes[Index];
           SkipListNode^.AABB.Min:=Node^.AABB.Min;
           SkipListNode^.AABB.Max:=Node^.AABB.Max;
           SkipListNode^.SkipToNodeIndex:=-1;
           if Node^.TriangleIndex>=0 then begin
            SkipListNode^.TriangleIndex:=Node^.TriangleIndex;
           end else begin
            SkipListNode^.TriangleIndex:=-1;
           end;
           if Node^.Children[0]>=0 then begin
            NewStackCapacity:=RoundUpToPowerOfTwo(StackPointer+9);
            if NewStackCapacity>length(Stack) then begin
             SetLength(Stack,NewStackCapacity);
            end;
            Stack[StackPointer+0]:=NodeID;
            Stack[StackPointer+1]:=1;
            Stack[StackPointer+2]:=Index;
            Stack[StackPointer+3]:=Node^.Children[1];
            Stack[StackPointer+4]:=0;
            Stack[StackPointer+5]:=0;
            Stack[StackPointer+6]:=Node^.Children[0];
            Stack[StackPointer+7]:=0;
            Stack[StackPointer+8]:=0;
            inc(StackPointer,9);
           end else begin
            NewStackCapacity:=RoundUpToPowerOfTwo(StackPointer+3);
            if NewStackCapacity>length(Stack) then begin
             SetLength(Stack,NewStackCapacity);
            end;
            Stack[StackPointer+0]:=NodeID;
            Stack[StackPointer+1]:=1;
            Stack[StackPointer+2]:=Index;
            inc(StackPointer,3);
           end;
          end;
          1:begin
           SkipListNode:=@fSkipListNodes[Index];
           SkipListNode^.SkipToNodeIndex:=fCountSkipListNodes;
          end;
         end;
        end;
       end;
      end;
     end;
    end;
   finally
    SetLength(Stack,0);
   end;
  end else begin
   SetLength(TriangleAABBs,0);
   SetLength(Nodes,0);
   SetLength(Points[0],0);
   SetLength(Points[1],0);
   SetLength(Points[2],0);
  end;
  for Index:=0 to fCountVertices-1 do begin
   if Index=0 then begin
    fAABB.Min:=fVertices[Index];
    fAABB.Max:=fVertices[Index];
   end else begin
    fAABB:=AABBCombineVector3(fAABB,fVertices[Index]);
   end;
  end;
 end;
end;

function TKraftMesh.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
{$ifdef KraftSingleThreadedUsage}
type PStackItem=^TStackItem;
     TStackItem=record
      NodeIndex:longint;
      SquaredDistance:TKraftScalar;
     end;
     TStackItems=array[0..31] of TStackItem;
var LocalStack:TStackItems;
    LocalStackPointer,TriangleIndex:longint;
    Node:PKraftMeshNode;
    Triangle,BestTriangle:PKraftMeshTriangle;
    StackItem:PStackItem;
    SquaredDistances:array[0..1] of TKraftScalar;
    SquaredDistance,SquaredThickness,SquaredThicknessEpsilon:TKraftScalar;
begin
 if fCountNodes>0 then begin
  result:=MAX_SCALAR;
  BestTriangle:=nil;
  if fDoubleSided then begin
   SquaredThickness:=sqr(2.0*Physics.fLinearSlop);
  end else begin
   SquaredThickness:=0.0;
  end;
  SquaredThicknessEpsilon:=SquaredThickness+sqr(EPSILON);
  LocalStackPointer:=0;
  StackItem:=@LocalStack[LocalStackPointer];
  inc(LocalStackPointer);
  StackItem^.NodeIndex:=0;
  StackItem^.SquaredDistance:=SquaredDistanceFromPointToAABB(fNodes[0].AABB,Position);
  while LocalStackPointer>0 do begin
   dec(LocalStackPointer);
   StackItem:=@LocalStack[LocalStackPointer];
   if (StackItem^.SquaredDistance-SquaredThicknessEpsilon)<result then begin
    if StackItem^.NodeIndex>=0 then begin
     Node:=@fNodes[StackItem^.NodeIndex];
     TriangleIndex:=Node^.TriangleIndex;
     while TriangleIndex>=0 do begin
      Triangle:=@fTriangles[TriangleIndex];
      SquaredDistance:=SquaredDistanceFromPointToTriangle(Position,fVertices[Triangle^.Vertices[0]],fVertices[Triangle^.Vertices[1]],fVertices[Triangle^.Vertices[2]]);
      if result>SquaredDistance then begin
       result:=SquaredDistance;
       BestTriangle:=Triangle;
      end;
      TriangleIndex:=Triangle^.Next;
     end;
     if (Node^.Children[0]>=0) and (Node^.Children[1]>=0) then begin
      SquaredDistances[0]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[0]].AABB,Position);
      SquaredDistances[1]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[1]].AABB,Position);
      if SquaredDistances[0]<SquaredDistances[1] then begin
       if (SquaredDistances[0]-SquaredThicknessEpsilon)<result then begin
        StackItem:=@LocalStack[LocalStackPointer];
        inc(LocalStackPointer);
        StackItem^.NodeIndex:=Node^.Children[0];
        StackItem^.SquaredDistance:=SquaredDistances[0];
       end;
       if (SquaredDistances[1]-SquaredThicknessEpsilon)<result then begin
        StackItem:=@LocalStack[LocalStackPointer];
        inc(LocalStackPointer);
        StackItem^.NodeIndex:=Node^.Children[1];
        StackItem^.SquaredDistance:=SquaredDistances[1];
       end;
      end else begin
       if (SquaredDistances[1]-SquaredThicknessEpsilon)<result then begin
        StackItem:=@LocalStack[LocalStackPointer];
        inc(LocalStackPointer);
        StackItem^.NodeIndex:=Node^.Children[1];
        StackItem^.SquaredDistance:=SquaredDistances[1];
       end;
       if (SquaredDistances[0]-SquaredThicknessEpsilon)<result then begin
        StackItem:=@LocalStack[LocalStackPointer];
        inc(LocalStackPointer);
        StackItem^.NodeIndex:=Node^.Children[0];
        StackItem^.SquaredDistance:=SquaredDistances[0];
       end;
      end;
     end else begin
      if Node^.Children[0]>=0 then begin
       SquaredDistances[0]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[0]].AABB,Position);
       if (SquaredDistances[0]-SquaredThicknessEpsilon)<result then begin
        StackItem:=@LocalStack[LocalStackPointer];
        inc(LocalStackPointer);
        StackItem^.NodeIndex:=Node^.Children[0];
        StackItem^.SquaredDistance:=SquaredDistances[0];
       end;
      end;
      if Node^.Children[1]>=0 then begin
       SquaredDistances[1]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[1]].AABB,Position);
       if (SquaredDistances[1]-SquaredThicknessEpsilon)<result then begin
        StackItem:=@LocalStack[LocalStackPointer];
        inc(LocalStackPointer);
        StackItem^.NodeIndex:=Node^.Children[1];
        StackItem^.SquaredDistance:=SquaredDistances[1];
       end;
      end;
     end;
    end;
   end;
  end;
  result:=sqrt(result);
  if fDoubleSided then begin
   result:=result-(2.0*Physics.fLinearSlop);
  end else begin
   if assigned(BestTriangle) and (PlaneVectorDistance(BestTriangle^.Plane,Position)<0.0) then begin
    result:=-result;
   end;
  end;
 end else begin
  result:=0;
 end;
end;
{$else}
var TriangleIndex:longint;
    Triangle,BestTriangle:PKraftMeshTriangle;
    SquaredDistances:array[0..1] of TKraftScalar;
    SquaredThickness,SquaredThicknessEpsilon:TKraftScalar;
 procedure ProcessNode(const NodeIndex:longint;SquaredDistance:TKraftScalar);
 var Node:PKraftMeshNode;
 begin
  if (NodeIndex>=0) and ((SquaredDistance-SquaredThicknessEpsilon)<result) then begin
   Node:=@fNodes[NodeIndex];
   TriangleIndex:=Node^.TriangleIndex;
   while TriangleIndex>=0 do begin
    Triangle:=@fTriangles[TriangleIndex];
    SquaredDistance:=SquaredDistanceFromPointToTriangle(Position,fVertices[Triangle^.Vertices[0]],fVertices[Triangle^.Vertices[1]],fVertices[Triangle^.Vertices[2]]);
    if result>SquaredDistance then begin
     result:=SquaredDistance;
     BestTriangle:=Triangle;
    end;
    TriangleIndex:=Triangle^.Next;
   end;
   if (Node^.Children[0]>=0) and (Node^.Children[1]>=0) then begin
    SquaredDistances[0]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[0]].AABB,Position);
    SquaredDistances[1]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[1]].AABB,Position);
    if SquaredDistances[0]<SquaredDistances[1] then begin
     if (SquaredDistances[0]-SquaredThicknessEpsilon)<result then begin
      ProcessNode(Node^.Children[0],SquaredDistances[0]);
     end;
     if (SquaredDistances[1]-SquaredThicknessEpsilon)<result then begin
      ProcessNode(Node^.Children[1],SquaredDistances[1]);
     end;
    end else begin
     if (SquaredDistances[1]-SquaredThicknessEpsilon)<result then begin
      ProcessNode(Node^.Children[1],SquaredDistances[1]);
     end;
     if (SquaredDistances[0]-SquaredThicknessEpsilon)<result then begin
      ProcessNode(Node^.Children[0],SquaredDistances[0]);
     end;
    end;
   end else begin
    if Node^.Children[0]>=0 then begin
     SquaredDistances[0]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[0]].AABB,Position);
     if (SquaredDistances[0]-SquaredThicknessEpsilon)<result then begin
      ProcessNode(Node^.Children[0],SquaredDistances[0]);
     end;
    end;
    if Node^.Children[1]>=0 then begin
     SquaredDistances[1]:=SquaredDistanceFromPointToAABB(fNodes[Node^.Children[1]].AABB,Position);
     if (SquaredDistances[1]-SquaredThicknessEpsilon)<result then begin
      ProcessNode(Node^.Children[0],SquaredDistances[1]);
     end;
    end;
   end;
  end;
 end;
begin
 if fCountNodes>0 then begin
  result:=MAX_SCALAR;
  BestTriangle:=nil;
  if fDoubleSided then begin
   SquaredThickness:=sqr(2.0*Physics.fLinearSlop);
  end else begin
   SquaredThickness:=0.0;
  end;
  SquaredThicknessEpsilon:=SquaredThickness+sqr(EPSILON);
  ProcessNode(0,SquaredDistanceFromPointToAABB(fNodes[0].AABB,Position));
  result:=sqrt(result);
  if fDoubleSided then begin
   result:=result-(2.0*Physics.fLinearSlop);
  end else begin
   if assigned(BestTriangle) and (PlaneVectorDistance(BestTriangle^.Plane,Position)<0.0) then begin
    result:=-result;
   end;
  end;
 end else begin
  result:=0;
 end;
end;
{$endif}

constructor TKraftShape.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody);
begin
 inherited Create;

 fShapeType:=kstUnknown;

 fPhysics:=APhysics;

 fPhysics.fNewShapes:=true;

 fRigidBody:=ARigidBody;

 if assigned(fRigidBody) then begin
  if assigned(fRigidBody.fShapeLast) then begin
   fRigidBody.fShapeLast.fShapeNext:=self;
   fShapePrevious:=fRigidBody.fShapeLast;
  end else begin
   fRigidBody.fShapeFirst:=self;
   fShapePrevious:=nil;
  end;
  fRigidBody.fShapeLast:=self;
  fShapeNext:=nil;

  inc(fRigidBody.fShapeCount);
 end;

 fFlags:=[ksfCollision,ksfMass];

 fIsMesh:=false;

 fFriction:=0.5;

 fRestitution:=0.0;

 fDensity:=1.0;

 fUserData:=nil;

 fStaticAABBTreeProxy:=-1;
 fSleepingAABBTreeProxy:=-1;
 fDynamicAABBTreeProxy:=-1;
 fKinematicAABBTreeProxy:=-1;

 fShapeAABB.Min:=Vector3Origin;
 fShapeAABB.Max:=Vector3Origin;

 fWorldAABB.Min:=Vector3Origin;
 fWorldAABB.Max:=Vector3Origin;

 fLocalTransform:=Matrix4x4Identity;

 fWorldTransform:=Matrix4x4Identity;

 fLastWorldTransform:=Matrix4x4Identity;

 fInterpolatedWorldTransform:=Matrix4x4Identity;

 fLocalCentroid:=Vector3Origin;

 fLocalCenterOfMass:=Vector3Origin;

 FillChar(fMassData,SizeOf(TKraftMassData),AnsiChar(#0));

 fFeatureRadius:=0.0;

 fContinuousMinimumRadiusScaleFactor:=0.0;

{$ifdef DebugDraw}
 fDrawDisplayList:=0;
{$endif}

 fOnContactBegin:=nil;
 fOnContactEnd:=nil;
 fOnContactStay:=nil;

end;

destructor TKraftShape.Destroy;
var ContactPairEdge,NextContactPairEdge:PKraftContactPairEdge;
    ContactPair:PKraftContactPair;
begin

{$ifdef DebugDraw}
 if fDrawDisplayList<>0 then begin
  glDeleteLists(fDrawDisplayList,1);
  fDrawDisplayList:=0;
 end;
{$endif}

 if assigned(fRigidBody) then begin
  ContactPairEdge:=fRigidBody.fContactPairEdgeFirst;
  while assigned(ContactPairEdge) do begin
   ContactPair:=ContactPairEdge^.ContactPair;
   NextContactPairEdge:=ContactPairEdge^.Next;
   if (ContactPair^.Shapes[0]=self) or (ContactPair^.Shapes[1]=self) then begin
    fPhysics.fContactManager.RemoveContact(ContactPair);
   end;
   ContactPairEdge:=NextContactPairEdge;
  end;
 end;

 if fStaticAABBTreeProxy>=0 then begin
  fPhysics.fStaticAABBTree.DestroyProxy(fStaticAABBTreeProxy);
  fStaticAABBTreeProxy:=-1;
 end;

 if fSleepingAABBTreeProxy>=0 then begin
  fPhysics.fSleepingAABBTree.DestroyProxy(fSleepingAABBTreeProxy);
  fSleepingAABBTreeProxy:=-1;
 end;

 if fDynamicAABBTreeProxy>=0 then begin
  fPhysics.fDynamicAABBTree.DestroyProxy(fDynamicAABBTreeProxy);
  fDynamicAABBTreeProxy:=-1;
 end;

 if fKinematicAABBTreeProxy>=0 then begin
  fPhysics.fKinematicAABBTree.DestroyProxy(fKinematicAABBTreeProxy);
  fKinematicAABBTreeProxy:=-1;
 end;

 if assigned(fRigidBody) then begin
  if assigned(fShapePrevious) then begin
   fShapePrevious.fShapeNext:=fShapeNext;
  end else if fRigidBody.fShapeFirst=self then begin
   fRigidBody.fShapeFirst:=fShapeNext;
  end;
  if assigned(fShapeNext) then begin
   fShapeNext.fShapePrevious:=fShapePrevious;
  end else if fRigidBody.fShapeLast=self then begin
   fRigidBody.fShapeLast:=fShapePrevious;
  end;
  fShapePrevious:=nil;
  fShapeNext:=nil;

  dec(fRigidBody.fShapeCount);
 end;

 inherited Destroy;
end;

function TKraftShape.GetProxyFatWorldAABB:PKraftAABB;
begin
 if fStaticAABBTreeProxy>=0 then begin
  result:=@fPhysics.fStaticAABBTree.fNodes[fStaticAABBTreeProxy].AABB;
 end else if fSleepingAABBTreeProxy>=0 then begin
  result:=@fPhysics.fSleepingAABBTree.fNodes[fSleepingAABBTreeProxy].AABB;
 end else if fDynamicAABBTreeProxy>=0 then begin
  result:=@fPhysics.fDynamicAABBTree.fNodes[fDynamicAABBTreeProxy].AABB;
 end else if fKinematicAABBTreeProxy>=0 then begin
  result:=@fPhysics.fKinematicAABBTree.fNodes[fKinematicAABBTreeProxy].AABB;
 end else begin
  result:=@fWorldAABB;
 end;
end;

procedure TKraftShape.UpdateShapeAABB;
begin
end;

procedure TKraftShape.CalculateMassData;
begin
end;

procedure TKraftShape.SynchronizeTransform;
begin
 if assigned(fRigidBody) then begin
  fWorldTransform:=Matrix4x4TermMul(fLocalTransform,fRigidBody.fWorldTransform);
 end else begin
  fWorldTransform:=fLocalTransform;
 end;
end;

procedure TKraftShape.SynchronizeProxies;
var NeedUpdate:boolean;
    WorldCenterOfMass,WorldDisplacement,WorldBoundsExpansion,TempPoint:TKraftVector3;
    TempAABB:TKraftAABB;
begin

 if assigned(fRigidBody) then begin
  fWorldTransform:=Matrix4x4TermMul(fLocalTransform,fRigidBody.fWorldTransform);
 end else begin
  fWorldTransform:=fLocalTransform;
 end;
 WorldCenterOfMass:=Vector3TermMatrixMul(fLocalCenterOfMass,fWorldTransform);

 case fShapeType of
  kstSphere:begin
   fWorldAABB.Min:=Vector3Sub(WorldCenterOfMass,Vector3(TKraftShapeSphere(self).fRadius,TKraftShapeSphere(self).fRadius,TKraftShapeSphere(self).fRadius));
   fWorldAABB.Max:=Vector3Add(WorldCenterOfMass,Vector3(TKraftShapeSphere(self).fRadius,TKraftShapeSphere(self).fRadius,TKraftShapeSphere(self).fRadius));
  end;
  kstCapsule:begin
   TempPoint:=Vector3Sub(WorldCenterOfMass,Vector3ScalarMul(Vector3(fWorldTransform[1,0],fWorldTransform[1,1],fWorldTransform[1,2]),(TKraftShapeCapsule(self).fHeight*0.5)));
   fWorldAABB.Min:=Vector3Sub(TempPoint,Vector3(TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius));
   fWorldAABB.Max:=Vector3Add(TempPoint,Vector3(TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius));
   TempPoint:=Vector3Add(WorldCenterOfMass,Vector3ScalarMul(Vector3(fWorldTransform[1,0],fWorldTransform[1,1],fWorldTransform[1,2]),(TKraftShapeCapsule(self).fHeight*0.5)));
   TempAABB.Min:=Vector3Sub(TempPoint,Vector3(TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius));
   TempAABB.Max:=Vector3Add(TempPoint,Vector3(TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius,TKraftShapeCapsule(self).fRadius));
   fWorldAABB:=AABBCombine(fWorldAABB,TempAABB);
  end;
  else begin
   fWorldAABB:=AABBTransform(fShapeAABB,fWorldTransform);
  end;
 end;

 if assigned(fRigidBody) then begin

  WorldDisplacement:=Vector3ScalarMul(fRigidBody.fLinearVelocity,fPhysics.fWorldDeltaTime);
  if Vector3LengthSquared(WorldDisplacement)<Vector3LengthSquared(fRigidBody.fWorldDisplacement) then begin
   WorldDisplacement:=fRigidBody.fWorldDisplacement;
  end;

  WorldBoundsExpansion:=Vector3ScalarMul(Vector3(fAngularMotionDisc,fAngularMotionDisc,fAngularMotionDisc),Vector3Length(fRigidBody.fAngularVelocity)*fPhysics.fWorldDeltaTime);

  if (fRigidBody.fRigidBodyType<>krbtStatic) and (fStaticAABBTreeProxy>=0) then begin
   fPhysics.fStaticAABBTree.DestroyProxy(fStaticAABBTreeProxy);
   fStaticAABBTreeProxy:=-1;
  end else if fRigidBody.fRigidBodyType=krbtStatic then begin
   NeedUpdate:=false;
   if fStaticAABBTreeProxy<0 then begin
    fStaticAABBTreeProxy:=fPhysics.fStaticAABBTree.CreateProxy(fWorldAABB,self);
    NeedUpdate:=true;
   end;
   if fPhysics.fStaticAABBTree.MoveProxy(fStaticAABBTreeProxy,fWorldAABB,Vector3Origin,Vector3Origin) then begin
    NeedUpdate:=true;
   end;
   if NeedUpdate then begin
    fPhysics.fBroadPhase.StaticBufferMove(fStaticAABBTreeProxy);
   end;
  end;

  if (fRigidBody.fRigidBodyType<>krbtDynamic) and (fDynamicAABBTreeProxy>=0) then begin
   fPhysics.fDynamicAABBTree.DestroyProxy(fDynamicAABBTreeProxy);
   fDynamicAABBTreeProxy:=-1;
  end else if fRigidBody.fRigidBodyType=krbtDynamic then begin
   NeedUpdate:=false;
   if fDynamicAABBTreeProxy<0 then begin
    fDynamicAABBTreeProxy:=fPhysics.fDynamicAABBTree.CreateProxy(fWorldAABB,self);
    NeedUpdate:=true;
   end;
   if fPhysics.fDynamicAABBTree.MoveProxy(fDynamicAABBTreeProxy,fWorldAABB,WorldDisplacement,WorldBoundsExpansion) then begin
    NeedUpdate:=true;
   end;
   if NeedUpdate then begin
    fPhysics.fBroadPhase.DynamicBufferMove(fDynamicAABBTreeProxy);
   end;
  end;

  if (fRigidBody.fRigidBodyType<>krbtKinematic) and (fKinematicAABBTreeProxy>=0) then begin
   fPhysics.fKinematicAABBTree.DestroyProxy(fKinematicAABBTreeProxy);
   fKinematicAABBTreeProxy:=-1;
  end else if fRigidBody.fRigidBodyType=krbtKinematic then begin
   NeedUpdate:=false;
   if fKinematicAABBTreeProxy<0 then begin
    fKinematicAABBTreeProxy:=fPhysics.fKinematicAABBTree.CreateProxy(fWorldAABB,self);
    NeedUpdate:=true;
   end;
   if fPhysics.fKinematicAABBTree.MoveProxy(fKinematicAABBTreeProxy,fWorldAABB,fRigidBody.fWorldDisplacement,Vector3Origin) then begin
    NeedUpdate:=true;
   end;
   if NeedUpdate then begin
    fPhysics.fBroadPhase.KinematicBufferMove(fKinematicAABBTreeProxy);
   end;
  end;

 end;

end;

procedure TKraftShape.Finish;
begin
 CalculateMassData;
 UpdateShapeAABB;
 fShapeSphere:=SphereFromAABB(fShapeAABB);
 fAngularMotionDisc:=Vector3Length(fShapeSphere.Center)+fShapeSphere.Radius;
end;

function TKraftShape.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
begin
 result:=0.0;
end;

function TKraftShape.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftShape.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftShape.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
begin
 result:=-1;
end;

function TKraftShape.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalCentroid,Transform);
end;

function TKraftShape.TestPoint(const p:TKraftVector3):boolean;
begin
 result:=false;
end;

function TKraftShape.RayCast(var RayCastData:TKraftRaycastData):boolean;
begin
 result:=false;
end;

procedure TKraftShape.StoreWorldTransform;
begin
 fLastWorldTransform:=fWorldTransform;
end;

procedure TKraftShape.InterpolateWorldTransform(const Alpha:TKraftScalar);
begin
 fInterpolatedWorldTransform:=Matrix4x4Slerp(fLastWorldTransform,fWorldTransform,Alpha);
end;

{$ifdef DebugDraw}
procedure TKraftShape.Draw(const CameraMatrix:TKraftMatrix4x4);
begin
end;
{$endif}

constructor TKraftShapeSphere.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const ARadius:TKraftScalar);
begin
 fRadius:=ARadius;
 inherited Create(APhysics,ARigidBody);
 fShapeType:=kstSphere;
 fFeatureRadius:=ARadius;
end;

destructor TKraftShapeSphere.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftShapeSphere.UpdateShapeAABB;
begin
 fShapeAABB.Min.x:=-fRadius;
 fShapeAABB.Min.y:=-fRadius;
 fShapeAABB.Min.z:=-fRadius;
 fShapeAABB.Max.x:=fRadius;
 fShapeAABB.Max.y:=fRadius;
 fShapeAABB.Max.z:=fRadius;
end;

procedure TKraftShapeSphere.CalculateMassData;
begin
 fMassData.Volume:=((4.0*pi)*(fRadius*fRadius*fRadius))/3.0;
 fMassData.Mass:=fMassData.Volume*fDensity;
 fMassData.Inertia[0,0]:=(2.0*(sqr(fRadius)*fMassData.Mass))/5.0;
 fMassData.Inertia[0,1]:=0.0;
 fMassData.Inertia[0,2]:=0.0;
 fMassData.Inertia[1,0]:=0.0;
 fMassData.Inertia[1,1]:=(2.0*(sqr(fRadius)*fMassData.Mass))/5.0;
 fMassData.Inertia[1,2]:=0.0;
 fMassData.Inertia[2,0]:=0.0;
 fMassData.Inertia[2,1]:=0.0;
 fMassData.Inertia[2,2]:=(2.0*(sqr(fRadius)*fMassData.Mass))/5.0;
 fMassData.Center.x:=fLocalTransform[3,0];
 fMassData.Center.y:=fLocalTransform[3,1];
 fMassData.Center.z:=fLocalTransform[3,2];
 Matrix3x3Add(fMassData.Inertia,InertiaTensorParallelAxisTheorem(fMassData.Center,fMassData.Mass));
end;

function TKraftShapeSphere.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
begin
 result:=Vector3Length(Vector3TermMatrixMulInverted(Position,fWorldTransform))-fRadius;
end;

function TKraftShapeSphere.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
begin
 result:=Vector3ScalarMul(Vector3SafeNorm(Direction),fRadius);
end;

function TKraftShapeSphere.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftShapeSphere.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
begin
 result:=0;
end;

function TKraftShapeSphere.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=PKraftVector3(pointer(@Transform[3,0]))^;
end;

function TKraftShapeSphere.TestPoint(const p:TKraftVector3):boolean;
begin
 result:=Vector3Length(Vector3TermMatrixMulInverted(p,fWorldTransform))<=fRadius;
end;

function TKraftShapeSphere.RayCast(var RayCastData:TKraftRaycastData):boolean;
var Origin,Direction,m:TKraftVector3;
    p,d,s1,s2,t:TKraftScalar;
begin
 result:=false;
 Origin:=Vector3TermMatrixMulInverted(RayCastData.Origin,fWorldTransform);
 Direction:=Vector3SafeNorm(Vector3TermMatrixMulTransposedBasis(RayCastData.Direction,fWorldTransform));
 m:=Vector3Sub(Origin,Vector3Origin);
 p:=-Vector3Dot(m,Direction);
 d:=(sqr(p)-Vector3LengthSquared(m))+sqr(fRadius);
 if d>0.0 then begin
  d:=sqrt(d);
  s1:=p-d;
  s2:=p+d;
  if s2>0.0 then begin
   if s1<0.0 then begin
    t:=s2;
   end else begin
    t:=s1;
   end;
   if (t>=0.0) and (t<=RayCastData.MaxTime) then begin
    RayCastData.TimeOfImpact:=t;
    RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,t)),fWorldTransform);
    RayCastData.Normal:=Vector3NormEx(Vector3Sub(RayCastData.Point,Vector3TermMatrixMul(Vector3Origin,fWorldTransform)));
    result:=true;
   end;
  end;
 end;
end;

{$ifdef DebugDraw}
procedure TKraftShapeSphere.Draw(const CameraMatrix:TKraftMatrix4x4);
const lats=16;
      longs=16;
      pi2=pi*2.0;
var i,j:longint;
    lat0,z0,zr0,lat1,z1,zr1,lng,x,y:TKraftScalar;
    ModelViewMatrix:TKraftMatrix4x4;
begin
 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);
 ModelViewMatrix:=Matrix4x4TermMul(fInterpolatedWorldTransform,CameraMatrix);
{$ifdef UseDouble}
 glLoadMatrixd(pointer(@ModelViewMatrix));
{$else}
 glLoadMatrixf(pointer(@ModelViewMatrix));
{$endif}

 if fDrawDisplayList=0 then begin
  fDrawDisplayList:=glGenLists(1);
  glNewList(fDrawDisplayList,GL_COMPILE);

  for i:=0 to lats do begin
   lat0:=pi*(((i-1)/lats)-0.5);
   z0:=sin(lat0)*fRadius;
   zr0:=cos(lat0)*fRadius;
   lat1:=pi*((i/lats)-0.5);
   z1:=sin(lat1)*fRadius;
   zr1:=cos(lat1)*fRadius;
   glBegin(GL_QUAD_STRIP);
   for j:=0 to longs do begin
    lng:=pi2*((j-1)/longs);
    x:=cos(lng);
    y:=sin(lng);
    glNormal3f(x*zr1,y*zr1,z1);
    glVertex3f(x*zr1,y*zr1,z1);
    glNormal3f(x*zr0,y*zr0,z0);
    glVertex3f(x*zr0,y*zr0,z0);
   end;
   glEnd;
  end;

  glEndList;
 end;

 if fDrawDisplayList<>0 then begin
  glCallList(fDrawDisplayList);
 end;

 glPopMatrix;
end;
{$endif}

constructor TKraftShapeCapsule.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const ARadius,AHeight:TKraftScalar);
begin
 fRadius:=ARadius;
 fHeight:=AHeight;
 inherited Create(APhysics,ARigidBody);
 fShapeType:=kstCapsule;
 fFeatureRadius:=ARadius;
end;

destructor TKraftShapeCapsule.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftShapeCapsule.UpdateShapeAABB;
begin
 fShapeAABB.Min.x:=-fRadius;
 fShapeAABB.Min.y:=-(fRadius+(fHeight*0.5));
 fShapeAABB.Min.z:=-fRadius;
 fShapeAABB.Max.x:=fRadius;
 fShapeAABB.Max.y:=fRadius+(fHeight*0.5);
 fShapeAABB.Max.z:=fRadius;
end;

//{$define UseTKraftShapeCapsuleCalculateMassDataReferenceTestImplementation}
{$ifdef UseTKraftShapeCapsuleCalculateMassDataReferenceTestImplementation}
procedure TKraftShapeCapsule.CalculateMassData;
var RadiusSquared:TKraftScalar;
    CapMassData,CylinderMassData:TKraftMassData;
begin
 RadiusSquared:=sqr(fRadius);
 begin
  CapMassData.Volume:=(2.0/3.0)*pi*RadiusSquared*fRadius;
  CapMassData.Mass:=CapMassData.Volume*Density;
  CapMassData.Inertia[0,0]:=((2.0/5.0)*CapMassData.Mass*RadiusSquared)+(CapMassData.Mass*fHeight*(((3.0*fRadius)+(2.0*fHeight))/8.0));
  CapMassData.Inertia[0,1]:=0.0;
  CapMassData.Inertia[0,2]:=0.0;
  CapMassData.Inertia[1,0]:=0.0;
  CapMassData.Inertia[1,1]:=(2.0/5.0)*CapMassData.Mass*RadiusSquared;
  CapMassData.Inertia[1,2]:=0.0;
  CapMassData.Inertia[2,0]:=0.0;
  CapMassData.Inertia[2,1]:=0.0;
  CapMassData.Inertia[2,2]:=((2.0/5.0)*CapMassData.Mass*RadiusSquared)+(CapMassData.Mass*fHeight*(((3.0*fRadius)+(2.0*fHeight))/8.0));
 end;
 begin
  CylinderMassData.Volume:=pi*RadiusSquared*fHeight;
  CylinderMassData.Mass:=CylinderMassData.Volume*Density;
  CylinderMassData.Inertia[0,0]:=(1.0/12.0)*CylinderMassData.Mass*((3.0*RadiusSquared)+sqr(fHeight));
  CylinderMassData.Inertia[0,1]:=0.0;
  CylinderMassData.Inertia[0,2]:=0.0;
  CylinderMassData.Inertia[1,0]:=0.0;
  CylinderMassData.Inertia[1,1]:=(1.0/2.0)*CylinderMassData.Mass*RadiusSquared;
  CylinderMassData.Inertia[1,2]:=0.0;
  CylinderMassData.Inertia[2,0]:=0.0;
  CylinderMassData.Inertia[2,1]:=0.0;
  CylinderMassData.Inertia[2,2]:=(1.0/12.0)*CylinderMassData.Mass*((3.0*RadiusSquared)+sqr(fHeight));
 end;
 fMassData.Volume:=(CapMassData.Volume*2.0)+CylinderMassData.Volume;
 fMassData.Mass:=(CapMassData.Mass*2.0)+CylinderMassData.Mass;
 fMassData.Inertia[0,0]:=(CapMassData.Inertia[0,0]*2.0)+CylinderMassData.Inertia[0,0];
 fMassData.Inertia[0,1]:=(CapMassData.Inertia[0,1]*2.0)+CylinderMassData.Inertia[0,1];
 fMassData.Inertia[0,2]:=(CapMassData.Inertia[0,2]*2.0)+CylinderMassData.Inertia[0,2];
 fMassData.Inertia[1,0]:=(CapMassData.Inertia[1,0]*2.0)+CylinderMassData.Inertia[1,0];
 fMassData.Inertia[1,1]:=(CapMassData.Inertia[1,1]*2.0)+CylinderMassData.Inertia[1,1];
 fMassData.Inertia[1,2]:=(CapMassData.Inertia[1,2]*2.0)+CylinderMassData.Inertia[1,2];
 fMassData.Inertia[2,0]:=(CapMassData.Inertia[2,0]*2.0)+CylinderMassData.Inertia[2,0];
 fMassData.Inertia[2,1]:=(CapMassData.Inertia[2,1]*2.0)+CylinderMassData.Inertia[2,1];
 fMassData.Inertia[2,2]:=(CapMassData.Inertia[2,2]*2.0)+CylinderMassData.Inertia[2,2];
 fMassData.Center.x:=fLocalTransform[3,0];
 fMassData.Center.y:=fLocalTransform[3,1];
 fMassData.Center.z:=fLocalTransform[3,2];
 fMassData.Inertia:=Matrix3x3TermAdd(InertiaTensorTransform(fMassData.Inertia,Matrix3x3(fLocalTransform)),
                                    InertiaTensorParallelAxisTheorem(fMassData.Center,fMassData.Mass));
end;
{$else}
procedure TKraftShapeCapsule.CalculateMassData;
begin
 fMassData.Volume:=(pi*(fRadius*fRadius))*(fHeight+((4.0*fRadius)/3.0));
 fMassData.Mass:=fMassData.Volume*fDensity;
 fMassData.Inertia[0,0]:=(((5.0*(fHeight*fHeight*fHeight))+(20.0*(fHeight*fHeight)*fRadius)+(45.0*fHeight*(fRadius*fRadius))+(32.0*(fRadius*fRadius*fRadius)))/((60.0*fHeight)+(80.0*fRadius)))*fMassData.Mass;
 fMassData.Inertia[0,1]:=0.0;
 fMassData.Inertia[0,2]:=0.0;
 fMassData.Inertia[1,0]:=0.0;
 fMassData.Inertia[1,1]:=(((fRadius*fRadius)*((15.0*fHeight)+(16.0*fRadius)))/((30.0*fHeight)+(40.0*fRadius)))*fMassData.Mass;
 fMassData.Inertia[1,2]:=0.0;
 fMassData.Inertia[2,0]:=0.0;
 fMassData.Inertia[2,1]:=0.0;
 fMassData.Inertia[2,2]:=(((5.0*(fHeight*fHeight*fHeight))+(20.0*(fHeight*fHeight)*fRadius)+(45.0*fHeight*(fRadius*fRadius))+(32.0*(fRadius*fRadius*fRadius)))/((60.0*fHeight)+(80.0*fRadius)))*fMassData.Mass;
 fMassData.Center.x:=fLocalTransform[3,0];
 fMassData.Center.y:=fLocalTransform[3,1];
 fMassData.Center.z:=fLocalTransform[3,2];
 fMassData.Inertia:=Matrix3x3TermAdd(InertiaTensorTransform(fMassData.Inertia,Matrix3x3(fLocalTransform)),
                                    InertiaTensorParallelAxisTheorem(fMassData.Center,fMassData.Mass));
end;
{$endif}

function TKraftShapeCapsule.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
var p,a,b,pa,ba:TKraftVector3;
    HalfHeight:TKraftScalar;
begin
 HalfHeight:=fHeight*0.5;
 p:=Vector3TermMatrixMulInverted(Position,fWorldTransform);
 a.x:=0.0;
 a.y:=-HalfHeight;
 a.z:=0.0;
 b.x:=0.0;
 b.y:=HalfHeight;
 b.z:=0.0;
 pa:=Vector3Sub(p,a);
 ba:=Vector3Sub(b,a);
 result:=Vector3Length(Vector3Sub(pa,Vector3ScalarMul(b,Min(Max(Vector3Dot(pa,ba)/Vector3Dot(ba,ba),0.0),1.0))))-fRadius;
end;

function TKraftShapeCapsule.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
var Normal,p0,p1:TKraftVector3;
    HalfHeight:TKraftScalar;
begin
 Normal:=Vector3SafeNorm(Direction);
 HalfHeight:=fHeight*0.5;
 p0.x:=(Normal.x*fRadius);
 p0.y:=(Normal.y*fRadius)-HalfHeight;
 p0.z:=(Normal.z*fRadius);
 p1.x:=(Normal.x*fRadius);
 p1.y:=(Normal.y*fRadius)+HalfHeight;
 p1.z:=(Normal.z*fRadius);
 if Vector3Dot(p0,Normal)<Vector3Dot(p1,Normal) then begin
  result:=p1;
 end else begin
  result:=p0;
 end;
end;

function TKraftShapeCapsule.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 result.x:=0.0;
 case Index of
  0:begin
   result.y:=-fHeight*0.5;
  end;
  else begin
   result.y:=fHeight*0.5;
  end;
 end;
 result.z:=0.0;
end;

function TKraftShapeCapsule.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
begin
 if Direction.y<0.0 then begin
  result:=0;
 end else begin
  result:=1;
 end;
end;

function TKraftShapeCapsule.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=PKraftVector3(pointer(@Transform[3,0]))^;
end;

function TKraftShapeCapsule.TestPoint(const p:TKraftVector3):boolean;
var v:TKraftVector3;
    HalfHeight:TKraftScalar;
begin
 v:=Vector3TermMatrixMulInverted(p,fWorldTransform);
 HalfHeight:=fHeight*0.5;
 result:=(abs(v.y)<=(HalfHeight+fRadius)) and (Vector3Length(Vector3(v.x,Min(Max(v.y,-HalfHeight),HalfHeight),v.z))<=fRadius);
end;

function TKraftShapeCapsule.RayCast(var RayCastData:TKraftRaycastData):boolean;
var Origin,Direction,p,m:TKraftVector3;
    Aq,Bq,Cq,t,t0,t1,y0,y1,HalfHeight,pp,d,s1,s2:TKraftScalar;
begin
 result:=false;
 Origin:=Vector3TermMatrixMulInverted(RayCastData.Origin,fWorldTransform);
 Direction:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(RayCastData.Direction,fWorldTransform));
 HalfHeight:=fHeight*0.5;
 Aq:=sqr(Direction.x)+sqr(Direction.z);
 Bq:=((2.0*Origin.x)*Direction.x)+((2.0*Origin.z)*Direction.z);
 Cq:=(sqr(Origin.x)+sqr(Origin.z))-sqr(fRadius);
 if SolveQuadraticRoots(Aq,Bq,Cq,t0,t1) then begin
  if t0>t1 then begin
   t:=t0;
   t0:=t1;
   t1:=t;
  end;
  y0:=Origin.y+(Direction.y*t0);
  y1:=Origin.y+(Direction.y*t1);
  if y0<(-HalfHeight) then begin
   if y1>=(-HalfHeight) then begin
    // if y0 < -HalfHeight and y1 >= -HalfHeight, then the ray hits the bottom Capsule cap
    t:=y0-y1;
    if t<>0.0 then begin
     t:=t0+(((t1-t0)*(y0+HalfHeight))/t);
     if (t>=0.0) and (t<=RayCastData.MaxTime) then begin
      RayCastData.TimeOfImpact:=t;
      RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,t)),fWorldTransform);
      RayCastData.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(Vector3(0.0,-1.0,0.0),fWorldTransform));
      result:=true;
      exit;
     end;
    end;
   end;
  end else if y0>HalfHeight then begin
   if y1<=HalfHeight then begin
    // if y0 > HalfHeight and y1 <= HalfHeight, then the ray hits the top Capsule cap
    t:=y0-y1;
    if t<>0.0 then begin
     t:=t0+(((t1-t0)*(y0-HalfHeight))/t);
     if (t>=0.0) and (t<=RayCastData.MaxTime) then begin
      RayCastData.TimeOfImpact:=t;
      RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,t)),fWorldTransform);
      RayCastData.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(Vector3(0.0,1.0,0.0),fWorldTransform));
      result:=true;
      exit;
     end;
    end;
   end;
  end else begin
   if (t0>=0.0) and (t0<=RayCastData.MaxTime) then begin
    // if y0 < HalfHeight and y0 > -HalfHeight and t0 >= 0.0, then the ray intersects then Capsule wall
    RayCastData.TimeOfImpact:=t0;
    p:=Vector3Add(Origin,Vector3ScalarMul(Direction,t0));
    RayCastData.Point:=Vector3TermMatrixMul(p,fWorldTransform);
    RayCastData.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(p,fWorldTransform));
    result:=true;
    exit;
   end;
  end;
 end;
 begin
  m:=Vector3Sub(Origin,Vector3(0.0,-HalfHeight,0.0));
  pp:=-Vector3Dot(m,Direction);
  d:=sqr(pp)-Vector3LengthSquared(m)+sqr(fRadius);
  if d>0.0 then begin
   d:=sqrt(d);
   s1:=pp-d;
   s2:=pp+d;
   if s2>0.0 then begin
    if s1<0.0 then begin
     t:=s2;
    end else begin
     t:=s1;
    end;
    if (t>=0.0) and (t<=RayCastData.MaxTime) then begin
     RayCastData.TimeOfImpact:=t;
     RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,t)),fWorldTransform);
     RayCastData.Normal:=Vector3NormEx(Vector3Sub(RayCastData.Point,Vector3TermMatrixMul(Vector3(0.0,-HalfHeight,0.0),fWorldTransform)));
     result:=true;
     exit;
    end;
   end;
  end;
 end;
 begin
  m:=Vector3Sub(Origin,Vector3(0.0,HalfHeight,0.0));
  pp:=-Vector3Dot(m,Direction);
  d:=sqr(pp)-Vector3LengthSquared(m)+sqr(fRadius);
  if d>0.0 then begin
   d:=sqrt(d);
   s1:=pp-d;
   s2:=pp+d;
   if s2>0.0 then begin
    if s1<0.0 then begin
     t:=s2;
    end else begin
     t:=s1;
    end;
    if (t>=0.0) and (t<=RayCastData.MaxTime) then begin
     RayCastData.TimeOfImpact:=t;
     RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,t)),fWorldTransform);
     RayCastData.Normal:=Vector3NormEx(Vector3Sub(RayCastData.Point,Vector3TermMatrixMul(Vector3(0.0,HalfHeight,0.0),fWorldTransform)));
     result:=true;
    end;
   end;
  end;
 end;
end;

{$ifdef DebugDraw}
procedure TKraftShapeCapsule.Draw(const CameraMatrix:TKraftMatrix4x4);
const lats=16;
      longs=16;
     pi2=pi*2.0;
var ModelViewMatrix:TKraftMatrix4x4;
    i,j:longint;
    HalfHeight,lat0,y0,yr0,lat1,y1,yr1,lng,x,z,yo0,yo1:TKraftScalar;
begin
 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);
 ModelViewMatrix:=Matrix4x4TermMul(fInterpolatedWorldTransform,CameraMatrix);
{$ifdef UseDouble}
 glLoadMatrixd(pointer(@ModelViewMatrix));
{$else}
 glLoadMatrixf(pointer(@ModelViewMatrix));
{$endif}

 if fDrawDisplayList=0 then begin
  fDrawDisplayList:=glGenLists(1);
  glNewList(fDrawDisplayList,GL_COMPILE);

  HalfHeight:=fHeight*0.5;

  for i:=0 to lats do begin
   lat0:=pi*(((i-1)/lats)-0.5);
   y0:=sin(lat0);
   yr0:=cos(lat0);
   lat1:=pi*((i/lats)-0.5);
   y1:=sin(lat1);
   yr1:=cos(lat1);
   if y0<0.0 then begin
    yo0:=-HalfHeight;
   end else begin
    yo0:=HalfHeight;
   end;
   if y1<0.0 then begin
    yo1:=-HalfHeight;
   end else begin
    yo1:=HalfHeight;
   end;
   glBegin(GL_QUAD_STRIP);
   for j:=0 to longs do begin
    lng:=pi2*((j-1)/longs);
    x:=cos(lng);
    z:=sin(lng);
    glNormal3f(x*yr0,y0,(z*yr0));
    glVertex3f(x*yr0*fRadius,yo0+(y0*fRadius),z*yr0*fRadius);
    glNormal3f(x*yr1,y1,(z*yr1));
    glVertex3f(x*yr1*fRadius,yo1+(y1*fRadius),z*yr1*fRadius);
   end;
   glEnd;
  end;

  glEndList;
 end;

 if fDrawDisplayList<>0 then begin
  glCallList(fDrawDisplayList);
 end;

 glPopMatrix;
end;
{$endif}

constructor TKraftShapeConvexHull.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AConvexHull:TKraftConvexHull);
begin

 fConvexHull:=AConvexHull;

 inherited Create(APhysics,ARigidBody);

 fShapeType:=kstConvexHull;

 fFeatureRadius:=0.0;

//fLocalCentroid:=fConvexHull.fMassData.Center;
 fLocalCentroid:=fConvexHull.fCentroid;

 fLocalCenterOfMass:=fConvexHull.fMassData.Center;

 fAngularMotionDisc:=fConvexHull.fAngularMotionDisc;

end;

destructor TKraftShapeConvexHull.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftShapeConvexHull.UpdateShapeAABB;
begin
 fShapeAABB:=fConvexHull.fAABB;
end;

procedure TKraftShapeConvexHull.CalculateMassData;
begin
 fMassData:=fConvexHull.fMassData;
 fMassData.Mass:=fMassData.Mass*fDensity;
 fMassData.Inertia[0,0]:=fMassData.Inertia[0,0]*fDensity;
 fMassData.Inertia[0,1]:=fMassData.Inertia[0,1]*fDensity;
 fMassData.Inertia[0,2]:=fMassData.Inertia[0,2]*fDensity;
 fMassData.Inertia[1,0]:=fMassData.Inertia[1,0]*fDensity;
 fMassData.Inertia[1,1]:=fMassData.Inertia[1,1]*fDensity;
 fMassData.Inertia[1,2]:=fMassData.Inertia[1,2]*fDensity;
 fMassData.Inertia[2,0]:=fMassData.Inertia[2,0]*fDensity;
 fMassData.Inertia[2,1]:=fMassData.Inertia[2,1]*fDensity;
 fMassData.Inertia[2,2]:=fMassData.Inertia[2,2]*fDensity;
end;

function TKraftShapeConvexHull.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
begin
 result:=fConvexHull.GetSignedDistance(Vector3TermMatrixMulInverted(Position,fWorldTransform));
end;

function TKraftShapeConvexHull.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
begin
 result:=fConvexHull.GetLocalFullSupport(Vector3SafeNorm(Direction));
end;

function TKraftShapeConvexHull.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 result:=fConvexHull.GetLocalFeatureSupportVertex(Index);
end;

function TKraftShapeConvexHull.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
begin
 result:=fConvexHull.GetLocalFeatureSupportIndex(Direction);
end;

function TKraftShapeConvexHull.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalCentroid,Transform);
end;

function TKraftShapeConvexHull.TestPoint(const p:TKraftVector3):boolean;
var i:longint;
begin
 result:=true;
 for i:=0 to fConvexHull.fCountFaces-1 do begin
  if PlaneVectorDistance(fConvexHull.fFaces[i].Plane,p)>0.0 then begin
   result:=false;
   exit;
  end;
 end;
end;

function TKraftShapeConvexHull.RayCast(var RayCastData:TKraftRaycastData):boolean;
var FaceIndex,BestFaceIndex:longint;
    Face:PKraftConvexHullFace;
    TimeFirst,TimeLast,Numerator,Denominator,Time:TKraftScalar;
    Origin,Direction:TKraftVector3;
begin
 result:=false;
 Origin:=Vector3TermMatrixMulInverted(RayCastData.Origin,fWorldTransform);
 Direction:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(RayCastData.Direction,fWorldTransform));
 if Vector3LengthSquared(Direction)>EPSILON then begin
  BestFaceIndex:=-1;
  TimeFirst:=0.0;
  TimeLast:=RayCastData.MaxTime+EPSILON;
  for FaceIndex:=0 to fConvexHull.fCountFaces-1 do begin
   Face:=@fConvexHull.fFaces[FaceIndex];
   Numerator:=-PlaneVectorDistance(Face^.Plane,Origin);
   Denominator:=Vector3Dot(Face^.Plane.Normal,Direction);
   if abs(Denominator)<EPSILON then begin
    if Numerator<0.0 then begin
     exit;
    end;
   end else begin
    Time:=Numerator/Denominator;
    if Denominator<0.0 then begin
     if TimeFirst<Time then begin
      TimeFirst:=Time;
      BestFaceIndex:=FaceIndex;
     end;
    end else begin
     if TimeLast>Time then begin
      TimeLast:=Time;
     end;
    end;
    if TimeFirst>TimeLast then begin
     exit;
    end;
   end;
  end;
  if (BestFaceIndex>=0) and (TimeFirst<=TimeLast) and (TimeFirst<=RayCastData.MaxTime) then begin
   RayCastData.TimeOfImpact:=TimeFirst;
   RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,TimeFirst)),fWorldTransform);
   RayCastData.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(fConvexHull.fFaces[BestFaceIndex].Plane.Normal,fWorldTransform));
   result:=true;
  end;
 end;
end;

{$ifdef DebugDraw}
procedure TKraftShapeConvexHull.Draw(const CameraMatrix:TKraftMatrix4x4);
var i,j:longint;
    ModelViewMatrix:TKraftMatrix4x4;
    Face:PKraftConvexHullFace;
begin
 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);
 ModelViewMatrix:=Matrix4x4TermMul(fInterpolatedWorldTransform,CameraMatrix);
{$ifdef UseDouble}
 glLoadMatrixd(pointer(@ModelViewMatrix));
{$else}
 glLoadMatrixf(pointer(@ModelViewMatrix));
{$endif}

 if fDrawDisplayList=0 then begin
  fDrawDisplayList:=glGenLists(1);
  glNewList(fDrawDisplayList,GL_COMPILE);

{$ifdef UseConvexHullTriangleDebugDraw}
  glBegin(GL_TRIANGLES);
  for i:=0 to fConvexHull.CountFaces-1 do begin
   Face:=@fConvexHull.fFaces[i];
{$ifdef UseDouble}
   glNormal3dv(@Face^.Plane.Normal);
   for j:=1 to Face^.CountVertices-2 do begin
    glVertex3dv(@fConvexHull.fVertices[Face^.Vertices[0]].Position);
    glVertex3dv(@fConvexHull.fVertices[Face^.Vertices[j]].Position);
    glVertex3dv(@fConvexHull.fVertices[Face^.Vertices[j+1]].Position);
   end;
{$else}
   glNormal3fv(@Face^.Plane.Normal);
   for j:=1 to Face^.CountVertices-2 do begin
    glVertex3fv(@fConvexHull.fVertices[Face^.Vertices[0]].Position);
    glVertex3fv(@fConvexHull.fVertices[Face^.Vertices[j]].Position);
    glVertex3fv(@fConvexHull.fVertices[Face^.Vertices[j+1]].Position);
   end;
{$endif}
  end;
  glEnd;
{$else}
  for i:=0 to fConvexHull.fCountFaces-1 do begin
   Face:=@fConvexHull.fFaces[i];
   glBegin(GL_POLYGON);
   glNormal3fv(@Face^.Plane.Normal);
   for j:=0 to Face^.CountVertices-1 do begin
    glVertex3fv(@fConvexHull.fVertices[Face^.Vertices[j]].Position);
   end;
   glEnd;
  end;
{$endif}

  glEndList;
 end;

 if fDrawDisplayList<>0 then begin
  glCallList(fDrawDisplayList);
 end;

 glPopMatrix;
end;
{$endif}

constructor TKraftShapeBox.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AExtents:TKraftVector3);
var i:longint;
    BoxPoints:array[0..7] of TKraftVector3;
begin
 fExtents:=AExtents;
 for i:=0 to length(BoxPoints)-1 do begin
  if (i and 1)<>0 then begin
   BoxPoints[i].x:=fExtents.x;
  end else begin
   BoxPoints[i].x:=-fExtents.x;
  end;
  if (i and 2)<>0 then begin
   BoxPoints[i].y:=fExtents.y;
  end else begin
   BoxPoints[i].y:=-fExtents.y;
  end;
  if (i and 4)<>0 then begin
   BoxPoints[i].z:=fExtents.z;
  end else begin
   BoxPoints[i].z:=-fExtents.z;
  end;
 end;
 fShapeConvexHull:=TKraftConvexHull.Create(APhysics);
 fShapeConvexHull.Load(pointer(@BoxPoints[0]),length(BoxPoints));
 fShapeConvexHull.Build;
 fShapeConvexHull.Finish;
 fAngularMotionDisc:=fShapeConvexHull.fAngularMotionDisc;
 inherited Create(APhysics,ARigidBody,fShapeConvexHull);
 fShapeType:=kstBox;
 fFeatureRadius:=0.0;
end;

destructor TKraftShapeBox.Destroy;
begin
 fShapeConvexHull.Free;
 inherited Destroy;
end;

procedure TKraftShapeBox.UpdateShapeAABB;
begin
 fShapeAABB.Min.x:=-fExtents.x;
 fShapeAABB.Min.y:=-fExtents.y;
 fShapeAABB.Min.z:=-fExtents.z;
 fShapeAABB.Max.x:=fExtents.x;
 fShapeAABB.Max.y:=fExtents.y;
 fShapeAABB.Max.z:=fExtents.z;
end;

procedure TKraftShapeBox.CalculateMassData;
begin                                                   
 fMassData.Volume:=fExtents.x*fExtents.y*fExtents.z;
 fMassData.Mass:=fMassData.Volume*fDensity;
 fMassData.Inertia[0,0]:=((sqr(fExtents.y)+sqr(fExtents.z))*fMassData.Mass)/12.0;
 fMassData.Inertia[0,1]:=0.0;
 fMassData.Inertia[0,2]:=0.0;
 fMassData.Inertia[1,0]:=0.0;
 fMassData.Inertia[1,1]:=((sqr(fExtents.x)+sqr(fExtents.z))*fMassData.Mass)/12.0;
 fMassData.Inertia[1,2]:=0.0;
 fMassData.Inertia[2,0]:=0.0;
 fMassData.Inertia[2,1]:=0.0;
 fMassData.Inertia[2,2]:=((sqr(fExtents.x)+sqr(fExtents.y))*fMassData.Mass)/12.0;
 fMassData.Center.x:=fLocalTransform[3,0];
 fMassData.Center.y:=fLocalTransform[3,1];
 fMassData.Center.z:=fLocalTransform[3,2];
 fMassData.Inertia:=Matrix3x3TermAdd(InertiaTensorTransform(fMassData.Inertia,Matrix3x3(fLocalTransform)),
                                    InertiaTensorParallelAxisTheorem(fMassData.Center,fMassData.Mass));
end;

function TKraftShapeBox.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
var ClosestBoxPoint,Temp,Direction:TKraftVector3;
    Overlap:longint;
begin
 result:=0;
 ClosestBoxPoint:=Vector3TermMatrixMulInverted(Position,fWorldTransform);
 if ClosestBoxPoint.x<-Extents.x then begin
  result:=result+sqr(ClosestBoxPoint.x-(-Extents.x));
  ClosestBoxPoint.x:=-Extents.x;
  Overlap:=0;
 end else if ClosestBoxPoint.x>Extents.x then begin
  result:=result+sqr(ClosestBoxPoint.x-Extents.x);
  ClosestBoxPoint.x:=Extents.x;
  Overlap:=0;
 end else begin
  Overlap:=1;
 end;
 if ClosestBoxPoint.y<-Extents.y then begin
  result:=result+sqr(ClosestBoxPoint.y-(-Extents.y));
  ClosestBoxPoint.y:=-Extents.y;
 end else if ClosestBoxPoint.y>Extents.y then begin
  result:=result+sqr(ClosestBoxPoint.y-Extents.y);
  ClosestBoxPoint.y:=Extents.y;
 end else begin
  Overlap:=Overlap or 2;
 end;
 if ClosestBoxPoint.z<-Extents.z then begin
  result:=result+sqr(ClosestBoxPoint.z-(-Extents.z));
  ClosestBoxPoint.z:=-Extents.z;
 end else if ClosestBoxPoint.z>Extents.z then begin
  result:=result+sqr(ClosestBoxPoint.z-Extents.z);
  ClosestBoxPoint.z:=Extents.z;
 end else begin
  Overlap:=Overlap or 3;
 end;
 if Overlap<>7 then begin
  result:=sqrt(result);
 end else begin
  Temp:=ClosestBoxPoint;
  Direction.x:=ClosestBoxPoint.x/Extents.x;
  Direction.y:=ClosestBoxPoint.y/Extents.y;
  Direction.z:=ClosestBoxPoint.z/Extents.z;
  if (abs(Direction.x)>abs(Direction.y)) and (abs(Direction.x)>abs(Direction.z)) then begin
   if Direction.x<0.0 then begin
    ClosestBoxPoint.x:=-Extents.x;
   end else begin
    ClosestBoxPoint.x:=Extents.x;
   end;
  end else if (abs(Direction.y)>abs(Direction.x)) and (abs(Direction.y)>abs(Direction.z)) then begin
   if Direction.y<0.0 then begin
    ClosestBoxPoint.y:=-Extents.y;
   end else begin
    ClosestBoxPoint.y:=Extents.y;
   end;
  end else begin
   if Direction.z<0.0 then begin
    ClosestBoxPoint.z:=-Extents.z;
   end else begin
    ClosestBoxPoint.z:=Extents.z;
   end;
  end;
  result:=-Vector3Dist(ClosestBoxPoint,Temp);
 end;
end;

function TKraftShapeBox.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
var Normal:TKraftVector3;
begin
 Normal:=Direction;
{$ifdef cpuarm}
 if Normal.x<0.0 then begin
  result.x:=-fExtents.x;
 end else begin
  result.x:=fExtents.x;
 end;
 if Normal.y<0.0 then begin
  result.y:=-fExtents.y;
 end else begin
  result.y:=fExtents.y;
 end;
 if Normal.z<0.0 then begin
  result.z:=-fExtents.z;
 end else begin
  result.z:=fExtents.z;
 end;
{$else}
 result.x:=fExtents.x*(1-longint(longword(longword(pointer(@Normal.x)^) shr 31) shl 1));
 result.y:=fExtents.y*(1-longint(longword(longword(pointer(@Normal.y)^) shr 31) shl 1));
 result.z:=fExtents.z*(1-longint(longword(longword(pointer(@Normal.z)^) shr 31) shl 1));
{$endif}
end;

function TKraftShapeBox.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 result:=inherited GetLocalFeatureSupportVertex(Index);
end;

function TKraftShapeBox.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
begin
 result:=inherited GetLocalFeatureSupportIndex(Direction);
end;

function TKraftShapeBox.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=PKraftVector3(pointer(@Transform[3,0]))^;
end;

function TKraftShapeBox.TestPoint(const p:TKraftVector3):boolean;
var v:TKraftVector3;
begin
 v:=Vector3TermMatrixMulInverted(p,fWorldTransform);
 result:=((v.x>=(-fExtents.x)) and (v.x<=fExtents.x)) and
         ((v.y>=(-fExtents.y)) and (v.x<=fExtents.y)) and
         ((v.z>=(-fExtents.z)) and (v.x<=fExtents.z));
end;

function TKraftShapeBox.RayCast(var RayCastData:TKraftRaycastData):boolean;
var s,v,h:TKraftVector3;
    lo,hi,k,Alpha:TKraftScalar;
    nlo,nhi,n:longint;
    sign:array[0..2] of TKraftScalar;
begin
 result:=false;
 s:=Vector3TermMatrixMulInverted(RayCastData.Origin,fWorldTransform);
 v:=Vector3TermMatrixMulTransposedBasis(RayCastData.Direction,fWorldTransform);
 if v.x<0 then begin
  s.x:=-s.x;
  v.x:=-v.x;
  sign[0]:=1;
 end else begin
  sign[0]:=-1;
 end;
 if v.y<0 then begin
  s.y:=-s.y;
  v.y:=-v.y;
  sign[1]:=1;
 end else begin
  sign[1]:=-1;
 end;
 if v.z<0 then begin
  s.z:=-s.z;
  v.z:=-v.z;
  sign[2]:=1;
 end else begin
  sign[2]:=-1;
 end;
 h:=fExtents;
 if (((s.x<-h.x) and (v.x<=0)) or (s.x>h.x)) or
    (((s.y<-h.y) and (v.y<=0)) or (s.y>h.y)) or
    (((s.z<-h.z) and (v.z<=0)) or (s.z>h.z)) then begin
  exit;
 end;
 lo:=-INFINITY;
 hi:=INFINITY;
 nlo:=0;
 nhi:=0;
 if v.x<>0.0 then begin
  k:=((-h.x)-s.x)/v.x;
  if k>lo then begin
   lo:=k;
   nlo:=0;
  end;
  k:=(h.x-s.x)/v.x;
  if k<hi then begin
   hi:=k;
   nhi:=0;
  end;
 end;
 if v.y<>0.0 then begin
  k:=((-h.y)-s.y)/v.y;
  if k>lo then begin
   lo:=k;
   nlo:=1;
  end;
  k:=(h.y-s.y)/v.y;
  if k<hi then begin
   hi:=k;
   nhi:=1;
  end;
 end;
 if v.z<>0.0 then begin
  k:=((-h.z)-s.z)/v.z;
  if k>lo then begin
   lo:=k;
   nlo:=2;
  end;
  k:=(h.z-s.z)/v.z;
  if k<hi then begin
   hi:=k;
   nhi:=2;
  end;
 end;
 if lo>hi then begin
  exit;
 end;
 if lo>=0 then begin
  Alpha:=lo;
  n:=nlo;
 end else begin
  Alpha:=hi;
  n:=nhi;
 end;
 if (Alpha<0) or (Alpha>RayCastData.MaxTime) then begin
  exit;
 end;
 RayCastData.TimeOfImpact:=Alpha;
 RayCastData.Point:=Vector3Add(RayCastData.Origin,Vector3ScalarMul(RayCastData.Direction,Alpha));
 RayCastData.Normal:=Vector3ScalarMul(Vector3(fWorldTransform[n,0],fWorldTransform[n,1],fWorldTransform[n,2]),sign[n]);
 result:=true;
end;

{$ifdef DebugDraw}
procedure TKraftShapeBox.Draw(const CameraMatrix:TKraftMatrix4x4);
var ModelViewMatrix:TKraftMatrix4x4;
begin
 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);
 ModelViewMatrix:=Matrix4x4TermMul(fInterpolatedWorldTransform,CameraMatrix);
{$ifdef UseDouble}
 glLoadMatrixd(pointer(@ModelViewMatrix));
{$else}
 glLoadMatrixf(pointer(@ModelViewMatrix));
{$endif}

 if fDrawDisplayList=0 then begin
  fDrawDisplayList:=glGenLists(1);
  glNewList(fDrawDisplayList,GL_COMPILE);

  glBegin(GL_TRIANGLE_STRIP);
  glNormal3f(-1.0,0.0,0.0);
  glVertex3f(-fExtents.x,-fExtents.y,-fExtents.z);
  glVertex3f(-fExtents.x,-fExtents.y,fExtents.z);
  glVertex3f(-fExtents.x,fExtents.y,-fExtents.z);
  glVertex3f(-fExtents.x,fExtents.y,fExtents.z);
  glNormal3f(0.0,1.0,0.0);
  glVertex3f(fExtents.x,fExtents.y,-fExtents.z);
  glVertex3f(fExtents.x,fExtents.y,fExtents.z);
  glNormal3f(1.0,0.0,0.0);
  glVertex3f(fExtents.x,-fExtents.y,-fExtents.z);
  glVertex3f(fExtents.x,-fExtents.y,fExtents.z);
  glNormal3f(0.0,-1.0,0.0);
  glVertex3f(-fExtents.x,-fExtents.y,-fExtents.z);
  glVertex3f(-fExtents.x,-fExtents.y,fExtents.z);
  glEnd;
  glBegin(GL_TRIANGLE_FAN);
  glNormal3f(0.0,0.0,1.0);
  glVertex3f(-fExtents.x,-fExtents.y,fExtents.z);
  glVertex3f(fExtents.x,-fExtents.y,fExtents.z);
  glVertex3f(fExtents.x,fExtents.y,fExtents.z);
  glVertex3f(-fExtents.x,fExtents.y,fExtents.z);
  glEnd;
  glBegin(GL_TRIANGLE_FAN);
  glNormal3f(0.0,0.0,-1.0);
  glVertex3f(-fExtents.x,-fExtents.y,-fExtents.z);
  glVertex3f(-fExtents.x,fExtents.y,-fExtents.z);
  glVertex3f(fExtents.x,fExtents.y,-fExtents.z);
  glVertex3f(fExtents.x,-fExtents.y,-fExtents.z);
  glEnd;

  glEndList;
 end;

 if fDrawDisplayList<>0 then begin
  glCallList(fDrawDisplayList);
 end;

 glPopMatrix;
end;
{$endif}

const PlaneSize=32768.0;

constructor TKraftShapePlane.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const APlane:TKraftPlane);
var HullPlaneVertices:array[0..7] of TKraftVector3;
    b,p,q:TKraftVector3;
begin
 fStaticAABBTreeProxy:=-1;
 fSleepingAABBTreeProxy:=-1;
 fDynamicAABBTreeProxy:=-1;
 fKinematicAABBTreeProxy:=-1;
 fPlane:=APlane;
 GetPlaneSpace(fPlane.Normal,p,q);
 fPlaneCenter:=Vector3ScalarMul(fPlane.Normal,fPlane.Distance-(PlaneSize*0.5));
 b:=Vector3ScalarMul(fPlane.Normal,fPlane.Distance);
 fPlaneVertices[0]:=Vector3Add(b,Vector3Add(Vector3ScalarMul(p,PlaneSize),Vector3ScalarMul(q,PlaneSize)));
 fPlaneVertices[1]:=Vector3Add(b,Vector3Add(Vector3ScalarMul(p,PlaneSize),Vector3ScalarMul(q,-PlaneSize)));
 fPlaneVertices[2]:=Vector3Add(b,Vector3Add(Vector3ScalarMul(p,-PlaneSize),Vector3ScalarMul(q,-PlaneSize)));
 fPlaneVertices[3]:=Vector3Add(b,Vector3Add(Vector3ScalarMul(p,-PlaneSize),Vector3ScalarMul(q,PlaneSize)));
 b:=Vector3ScalarMul(fPlane.Normal,PlaneSize);
 HullPlaneVertices[0]:=fPlaneVertices[0];
 HullPlaneVertices[1]:=fPlaneVertices[1];
 HullPlaneVertices[2]:=fPlaneVertices[2];
 HullPlaneVertices[3]:=fPlaneVertices[3];
 HullPlaneVertices[4]:=Vector3Sub(fPlaneVertices[0],b);
 HullPlaneVertices[5]:=Vector3Sub(fPlaneVertices[1],b);
 HullPlaneVertices[6]:=Vector3Sub(fPlaneVertices[2],b);
 HullPlaneVertices[7]:=Vector3Sub(fPlaneVertices[3],b);
//PlaneCenter:=Vector3Avg(@HullPlaneVertices[0],length(HullPlaneVertices));
 fShapeConvexHull:=TKraftConvexHull.Create(APhysics);
 fShapeConvexHull.Load(pointer(@HullPlaneVertices[0]),length(HullPlaneVertices));
 fShapeConvexHull.Build;
 fShapeConvexHull.Finish;
 fAngularMotionDisc:=fShapeConvexHull.fAngularMotionDisc;
 inherited Create(APhysics,ARigidBody,fShapeConvexHull);
 fShapeType:=kstPlane;
 fFeatureRadius:=0.0;
end;

destructor TKraftShapePlane.Destroy;
begin
 fShapeConvexHull.Free;
 inherited Destroy;
end;

procedure TKraftShapePlane.UpdateShapeAABB;
var b:TKraftVector3;
begin
 b:=Vector3ScalarMul(fPlane.Normal,fPlane.Distance-PlaneSize);
 fShapeAABB.Min.x:=Min(Min(Min(Min(fPlaneVertices[0].x,fPlaneVertices[1].x),fPlaneVertices[2].x),fPlaneVertices[3].x),b.x)-0.1;
 fShapeAABB.Min.y:=Min(Min(Min(Min(fPlaneVertices[0].y,fPlaneVertices[1].y),fPlaneVertices[2].y),fPlaneVertices[3].y),b.y)-0.1;
 fShapeAABB.Min.z:=Min(Min(Min(Min(fPlaneVertices[0].z,fPlaneVertices[1].z),fPlaneVertices[2].z),fPlaneVertices[3].z),b.z)-0.1;
 fShapeAABB.Max.x:=Max(Max(Max(Max(fPlaneVertices[0].x,fPlaneVertices[1].x),fPlaneVertices[2].x),fPlaneVertices[3].x),b.x)+0.1;
 fShapeAABB.Max.y:=Max(Max(Max(Max(fPlaneVertices[0].y,fPlaneVertices[1].y),fPlaneVertices[2].y),fPlaneVertices[3].y),b.y)+0.1;
 fShapeAABB.Max.z:=Max(Max(Max(Max(fPlaneVertices[0].z,fPlaneVertices[1].z),fPlaneVertices[2].z),fPlaneVertices[3].z),b.z)+0.1;
end;

procedure TKraftShapePlane.CalculateMassData;
begin
end;

function TKraftShapePlane.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
begin
 result:=Vector3Dot(Plane.Normal,Vector3TermMatrixMulInverted(Position,fWorldTransform))+Plane.Distance;
end;

function TKraftShapePlane.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
var Index,BestIndex:longint;
    BestDistance,NewDistance:TKraftScalar;
    Normal:TKraftVector3;
begin
 Normal:=Vector3SafeNorm(Direction);
 BestDistance:=Vector3Dot(Normal,fPlaneVertices[0]);
 BestIndex:=0;
 for Index:=1 to 3 do begin
  NewDistance:=Vector3Dot(Normal,fPlaneVertices[Index]);
  if BestDistance<NewDistance then begin
   BestDistance:=NewDistance;
   BestIndex:=Index;
  end;
 end;
 result:=fPlaneVertices[BestIndex];
 if Vector3Dot(fPlane.Normal,Normal)<0.0 then begin
  result:=Vector3Sub(result,Vector3ScalarMul(fPlane.Normal,PlaneSize));
 end;
end;

function TKraftShapePlane.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 if (Index>=0) and (Index<4) then begin
  result:=fPlaneVertices[Index];
 end else begin
  result:=Vector3Origin;
 end;
end;

function TKraftShapePlane.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
var Index:longint;
    BestDistance,NewDistance:TKraftScalar;
    Normal:TKraftVector3;
begin
 result:=0;
 Normal:=Vector3SafeNorm(Direction);
 BestDistance:=Vector3Dot(Normal,fPlaneVertices[0]);
 for Index:=1 to 3 do begin
  NewDistance:=Vector3Dot(Normal,fPlaneVertices[Index]);
  if BestDistance<NewDistance then begin
   BestDistance:=NewDistance;
   result:=Index;
  end;
 end;
end;

function TKraftShapePlane.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fPlaneCenter,Transform);
end;

function TKraftShapePlane.TestPoint(const p:TKraftVector3):boolean;
begin
 result:=false;
end;

function TKraftShapePlane.RayCast(var RayCastData:TKraftRaycastData):boolean;
var Origin,Direction:TKraftVector3;
    Time:TKraftScalar;
begin
 result:=false;
 Origin:=Vector3TermMatrixMulInverted(RayCastData.Origin,fWorldTransform);
 Direction:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(RayCastData.Direction,fWorldTransform));
 if Vector3LengthSquared(Direction)>EPSILON then begin
  Time:=-Vector3Dot(fPlane.Normal,Direction);
  if abs(Time)>EPSILON then begin
   Time:=PlaneVectorDistance(fPlane,Origin)/Time;
   if Time>=0.0 then begin
    if Time<RayCastData.MaxTime then begin
     RayCastData.TimeOfImpact:=Time;
     RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,Time)),fWorldTransform);
     RayCastData.Normal:=Vector3TermMatrixMulBasis(fPlane.Normal,fWorldTransform);
     result:=true;
    end;
   end;
  end;
 end;
end;

{$ifdef DebugDraw}
procedure TKraftShapePlane.Draw(const CameraMatrix:TKraftMatrix4x4);
var ModelViewMatrix:TKraftMatrix4x4;
    n:TKraftVector3;
begin

 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);
 ModelViewMatrix:=Matrix4x4TermMul(fInterpolatedWorldTransform,CameraMatrix);
{$ifdef UseDouble}
 glLoadMatrixd(pointer(@ModelViewMatrix));
{$else}
 glLoadMatrixf(pointer(@ModelViewMatrix));
{$endif}

 if fDrawDisplayList=0 then begin
  fDrawDisplayList:=glGenLists(1);
  glNewList(fDrawDisplayList,GL_COMPILE);

  n:=Vector3NormEx(Vector3Cross(Vector3Sub(fPlaneVertices[1],fPlaneVertices[0]),Vector3Sub(fPlaneVertices[2],fPlaneVertices[0])));

{$ifdef UseDouble}
  glBegin(GL_TRIANGLES);
  glNormal3dv(@n);
  glVertex3dv(@fPlaneVertices[0]);
  glVertex3dv(@fPlaneVertices[1]);
  glVertex3dv(@fPlaneVertices[2]);
  glVertex3dv(@fPlaneVertices[2]);
  glVertex3dv(@fPlaneVertices[3]);
  glVertex3dv(@fPlaneVertices[0]);
  n:=Vector3Neg(n);
  glNormal3dv(@n);
  glVertex3dv(@fPlaneVertices[2]);
  glVertex3dv(@fPlaneVertices[1]);
  glVertex3dv(@fPlaneVertices[0]);
  glVertex3dv(@fPlaneVertices[0]);
  glVertex3dv(@fPlaneVertices[3]);
  glVertex3dv(@fPlaneVertices[2]);
  glEnd;
{$else}
  glBegin(GL_TRIANGLES);
  glNormal3fv(@n);
  glVertex3fv(@fPlaneVertices[0]);
  glVertex3fv(@fPlaneVertices[1]);
  glVertex3fv(@fPlaneVertices[2]);
  glVertex3fv(@fPlaneVertices[2]);
  glVertex3fv(@fPlaneVertices[3]);
  glVertex3fv(@fPlaneVertices[0]);
  n:=Vector3Neg(n);
  glNormal3fv(@n);
  glVertex3fv(@fPlaneVertices[2]);
  glVertex3fv(@fPlaneVertices[1]);
  glVertex3fv(@fPlaneVertices[0]);
  glVertex3fv(@fPlaneVertices[0]);
  glVertex3fv(@fPlaneVertices[3]);
  glVertex3fv(@fPlaneVertices[2]);
  glEnd;
{$endif}

  glEndList;
 end;

 if fDrawDisplayList<>0 then begin
  glCallList(fDrawDisplayList);
 end;

 glPopMatrix;
end;
{$endif}

constructor TKraftShapeTriangle.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AVertex0,AVertex1,AVertex2:TKraftVector3);
//var TriangleVertices:array[0..2] of TKraftVector3;
var Vertices:PPKraftConvexHullVertices;
begin

{TriangleVertices[0]:=AVertex0;
 TriangleVertices[1]:=AVertex1;
 TriangleVertices[2]:=AVertex2;

 ShapeConvexHull:=TKraftConvexHull.Create(APhysics);
 ShapeConvexHull.Load(pointer(@TriangleVertices[0]),length(TriangleVertices));
 ShapeConvexHull.Build;
 ShapeConvexHull.Finish;{}

 fShapeConvexHull:=TKraftConvexHull.Create(APhysics);

 fShapeConvexHull.fCountVertices:=3;
 SetLength(fShapeConvexHull.fVertices,fShapeConvexHull.fCountVertices);
 fShapeConvexHull.fVertices[0].Position:=AVertex0;
 fShapeConvexHull.fVertices[0].CountAdjacencies:=2;
 SetLength(fShapeConvexHull.fVertices[0].Adjacencies,2);
 fShapeConvexHull.fVertices[0].Adjacencies[0]:=1;
 fShapeConvexHull.fVertices[0].Adjacencies[1]:=2;
 fShapeConvexHull.fVertices[1].Position:=AVertex1;
 fShapeConvexHull.fVertices[1].CountAdjacencies:=2;
 SetLength(fShapeConvexHull.fVertices[1].Adjacencies,2);
 fShapeConvexHull.fVertices[1].Adjacencies[0]:=2;
 fShapeConvexHull.fVertices[1].Adjacencies[1]:=0;
 fShapeConvexHull.fVertices[2].Position:=AVertex2;
 fShapeConvexHull.fVertices[2].CountAdjacencies:=2;
 SetLength(fShapeConvexHull.fVertices[2].Adjacencies,2);
 fShapeConvexHull.fVertices[2].Adjacencies[0]:=0;
 fShapeConvexHull.fVertices[2].Adjacencies[1]:=1;

 Vertices:=@fShapeConvexHull.fVertices[0];

 fShapeConvexHull.fCountFaces:=2;
 SetLength(fShapeConvexHull.fFaces,fShapeConvexHull.fCountFaces);
 fShapeConvexHull.fFaces[0].Plane.Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(Vertices^[1].Position,Vertices^[0].Position),Vector3Sub(Vertices^[2].Position,Vertices^[0].Position)));
 fShapeConvexHull.fFaces[0].Plane.Distance:=-Vector3Dot(fShapeConvexHull.fFaces[0].Plane.Normal,Vertices^[0].Position);
 fShapeConvexHull.fFaces[0].CountVertices:=3;
 SetLength(fShapeConvexHull.fFaces[0].Vertices,fShapeConvexHull.fFaces[0].CountVertices);
 fShapeConvexHull.fFaces[0].Vertices[0]:=0;
 fShapeConvexHull.fFaces[0].Vertices[1]:=1;
 fShapeConvexHull.fFaces[0].Vertices[2]:=2;
 fShapeConvexHull.fFaces[0].EdgeVertexOffset:=0;
 fShapeConvexHull.fFaces[1].Plane.Normal:=Vector3Neg(fShapeConvexHull.fFaces[0].Plane.Normal);
 fShapeConvexHull.fFaces[1].Plane.Distance:=-fShapeConvexHull.fFaces[0].Plane.Distance;
 fShapeConvexHull.fFaces[1].CountVertices:=3;
 SetLength(fShapeConvexHull.fFaces[1].Vertices,fShapeConvexHull.fFaces[1].CountVertices);
 fShapeConvexHull.fFaces[1].Vertices[0]:=0;
 fShapeConvexHull.fFaces[1].Vertices[1]:=2;
 fShapeConvexHull.fFaces[1].Vertices[2]:=1;
 fShapeConvexHull.fFaces[1].EdgeVertexOffset:=3;

 fShapeConvexHull.fCountEdges:=3;
 SetLength(fShapeConvexHull.fEdges,fShapeConvexHull.fCountEdges);
 fShapeConvexHull.fEdges[0].Vertices[0]:=2;
 fShapeConvexHull.fEdges[0].Vertices[1]:=0;
 fShapeConvexHull.fEdges[0].Faces[0]:=0;
 fShapeConvexHull.fEdges[0].Faces[1]:=1;
 fShapeConvexHull.fEdges[1].Vertices[0]:=1;
 fShapeConvexHull.fEdges[1].Vertices[1]:=2;
 fShapeConvexHull.fEdges[1].Faces[0]:=0;
 fShapeConvexHull.fEdges[1].Faces[1]:=1;
 fShapeConvexHull.fEdges[2].Vertices[0]:=0;
 fShapeConvexHull.fEdges[2].Vertices[1]:=1;
 fShapeConvexHull.fEdges[2].Faces[0]:=0;
 fShapeConvexHull.fEdges[2].Faces[1]:=1;

 fShapeAABB.Min.x:=Min(Min(Vertices^[0].Position.x,Vertices^[1].Position.x),Vertices^[2].Position.x)-0.1;
 fShapeAABB.Min.y:=Min(Min(Vertices^[0].Position.y,Vertices^[1].Position.y),Vertices^[2].Position.y)-0.1;
 fShapeAABB.Min.z:=Min(Min(Vertices^[0].Position.z,Vertices^[1].Position.z),Vertices^[2].Position.z)-0.1;
 fShapeAABB.Max.x:=Max(Max(Vertices^[0].Position.x,Vertices^[1].Position.x),Vertices^[2].Position.x)+0.1;
 fShapeAABB.Max.y:=Max(Max(Vertices^[0].Position.y,Vertices^[1].Position.y),Vertices^[2].Position.y)+0.1;
 fShapeAABB.Max.z:=Max(Max(Vertices^[0].Position.z,Vertices^[1].Position.z),Vertices^[2].Position.z)+0.1;

 fShapeConvexHull.fAABB:=fShapeAABB;

//fShapeSphere.Center:=Vector3Add(Vertices^[0].Position,Vector3Add(Vector3ScalarMul(Vector3Sub(Vertices^[1].Position,Vertices^[0].Position),0.5),Vector3ScalarMul(Vector3Sub(Vertices^[2].Position,Vertices^[0].Position),0.5)));
 fShapeSphere.Center.x:=(Vertices^[0].Position.x+Vertices^[1].Position.x+Vertices^[2].Position.x)/3.0;
 fShapeSphere.Center.y:=(Vertices^[0].Position.y+Vertices^[1].Position.y+Vertices^[2].Position.y)/3.0;
 fShapeSphere.Center.z:=(Vertices^[0].Position.z+Vertices^[1].Position.z+Vertices^[2].Position.z)/3.0;{}
 fShapeSphere.Radius:=sqrt(Max(Max(Vector3DistSquared(fShapeSphere.Center,Vertices^[0].Position),
                                  Vector3DistSquared(fShapeSphere.Center,Vertices^[1].Position)),
                                  Vector3DistSquared(fShapeSphere.Center,Vertices^[2].Position)));

 fShapeConvexHull.fAngularMotionDisc:=Vector3Length(fShapeSphere.Center)+fShapeSphere.Radius;
 fAngularMotionDisc:=fShapeConvexHull.fAngularMotionDisc;

 inherited Create(APhysics,ARigidBody,fShapeConvexHull);

 fShapeType:=kstTriangle;

 fFeatureRadius:=0.0;

end;

destructor TKraftShapeTriangle.Destroy;
begin
 fShapeConvexHull.Free;
 inherited Destroy;
end;

procedure TKraftShapeTriangle.UpdateShapeAABB;
var Vertices:PPKraftConvexHullVertices;
begin
 Vertices:=@fConvexHull.fVertices[0];
 fShapeAABB.Min.x:=Min(Min(Vertices^[0].Position.x,Vertices^[1].Position.x),Vertices^[2].Position.x)-0.1;
 fShapeAABB.Min.y:=Min(Min(Vertices^[0].Position.y,Vertices^[1].Position.y),Vertices^[2].Position.y)-0.1;
 fShapeAABB.Min.z:=Min(Min(Vertices^[0].Position.z,Vertices^[1].Position.z),Vertices^[2].Position.z)-0.1;
 fShapeAABB.Max.x:=Max(Max(Vertices^[0].Position.x,Vertices^[1].Position.x),Vertices^[2].Position.x)+0.1;
 fShapeAABB.Max.y:=Max(Max(Vertices^[0].Position.y,Vertices^[1].Position.y),Vertices^[2].Position.y)+0.1;
 fShapeAABB.Max.z:=Max(Max(Vertices^[0].Position.z,Vertices^[1].Position.z),Vertices^[2].Position.z)+0.1;
end;

procedure TKraftShapeTriangle.CalculateMassData;
begin
end;

procedure TKraftShapeTriangle.UpdateData;
const f1d3=1.0/3.0;
var Vertices:PPKraftConvexHullVertices;
begin
 Vertices:=@fConvexHull.fVertices[0];
//fShapeSphere.Center:=Vector3Add(Vertices^[0].Position,Vector3Add(Vector3ScalarMul(Vector3Sub(Vertices^[1].Position,Vertices^[0].Position),0.5),Vector3ScalarMul(Vector3Sub(Vertices^[2].Position,Vertices^[0].Position),0.5)));
 fShapeSphere.Center.x:=(Vertices^[0].Position.x+Vertices^[1].Position.x+Vertices^[2].Position.x)*f1d3;
 fShapeSphere.Center.y:=(Vertices^[0].Position.y+Vertices^[1].Position.y+Vertices^[2].Position.y)*f1d3;
 fShapeSphere.Center.z:=(Vertices^[0].Position.z+Vertices^[1].Position.z+Vertices^[2].Position.z)*f1d3;
 fShapeSphere.Radius:=sqrt(Max(Max(Vector3DistSquared(fShapeSphere.Center,Vertices^[0].Position),
                                  Vector3DistSquared(fShapeSphere.Center,Vertices^[1].Position)),
                                  Vector3DistSquared(fShapeSphere.Center,Vertices^[2].Position)));
 fShapeConvexHull.fFaces[0].Plane.Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(Vertices^[1].Position,Vertices^[0].Position),Vector3Sub(Vertices^[2].Position,Vertices^[0].Position)));
 fShapeConvexHull.fFaces[0].Plane.Distance:=-Vector3Dot(fShapeConvexHull.fFaces[0].Plane.Normal,Vertices^[0].Position);
 fShapeConvexHull.fFaces[1].Plane.Normal:=Vector3Neg(fShapeConvexHull.fFaces[0].Plane.Normal);
 fShapeConvexHull.fFaces[1].Plane.Distance:=-fShapeConvexHull.fFaces[0].Plane.Distance;
 fLocalCentroid:=fShapeSphere.Center;
 fLocalCenterOfMass:=fShapeSphere.Center;
 fAngularMotionDisc:=Vector3Length(fShapeSphere.Center)+fShapeSphere.Radius;
 fFeatureRadius:=0.0;
end;

function TKraftShapeTriangle.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
begin
 result:=sqrt(SquaredDistanceFromPointToTriangle(Vector3TermMatrixMulInverted(Position,fWorldTransform),
                                                 fConvexHull.fVertices[0].Position,
                                                 fConvexHull.fVertices[1].Position,
                                                 fConvexHull.fVertices[2].Position))-(2.0*Physics.fLinearSlop);
end;

function TKraftShapeTriangle.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
var i:longint;
    Vertices:PPKraftConvexHullVertices;
    d0,d1,d2:TKraftScalar;
    Normal:TKraftVector3;
begin
 Vertices:=@fConvexHull.fVertices[0];
 Normal:=Vector3SafeNorm(Direction);
 d0:=Vector3Dot(Normal,Vertices^[0].Position);
 d1:=Vector3Dot(Normal,Vertices^[1].Position);
 d2:=Vector3Dot(Normal,Vertices^[2].Position);
 if d0>d1 then begin
  if d2>d0 then begin
   i:=2;
  end else begin
   i:=0;
  end;
 end else begin
  if d2>d1 then begin
   i:=2;
  end else begin
   i:=1;
  end;
 end;
 result:=Vertices^[i].Position;
end;

function TKraftShapeTriangle.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 if (Index>=0) and (Index<3) then begin
  result:=fConvexHull.fVertices[Index].Position;
 end else begin
  result:=Vector3Origin;
 end;
end;

function TKraftShapeTriangle.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
var Vertices:PPKraftConvexHullVertices;
    d0,d1,d2:TKraftScalar;
    Normal:TKraftVector3;
begin
 Vertices:=@fConvexHull.fVertices[0];
 Normal:=Vector3SafeNorm(Direction);
 d0:=Vector3Dot(Normal,Vertices^[0].Position);
 d1:=Vector3Dot(Normal,Vertices^[1].Position);
 d2:=Vector3Dot(Normal,Vertices^[2].Position);
 if d0>d1 then begin
  if d2>d0 then begin
   result:=2;
  end else begin
   result:=0;
  end;
 end else begin
  if d2>d1 then begin
   result:=2;
  end else begin
   result:=1;
  end;
 end;
end;

function TKraftShapeTriangle.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fShapeSphere.Center,Transform);
end;

function TKraftShapeTriangle.TestPoint(const p:TKraftVector3):boolean;
begin
 result:=false;
end;

function TKraftShapeTriangle.RayCast(var RayCastData:TKraftRaycastData):boolean;
var Origin,Direction:TKraftVector3;
    Vertices:PPKraftConvexHullVertices;
    Time,u,v:TKraftScalar;
begin
 result:=false;
 Origin:=Vector3TermMatrixMulInverted(RayCastData.Origin,fWorldTransform);
 Direction:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(RayCastData.Direction,fWorldTransform));
 if Vector3LengthSquared(Direction)>EPSILON then begin
  Vertices:=@fConvexHull.fVertices[0];
  if RayIntersectTriangle(Origin,Direction,Vertices^[0].Position,Vertices^[1].Position,Vertices^[2].Position,Time,u,v) then begin
   if (Time>=0.0) and (Time<=RayCastData.MaxTime) then begin
    RayCastData.TimeOfImpact:=Time;
    RayCastData.Point:=Vector3TermMatrixMul(Vector3Add(Origin,Vector3ScalarMul(Direction,Time)),fWorldTransform);
    RayCastData.Normal:=Vector3TermMatrixMulBasis(Vector3NormEx(Vector3Cross(Vector3Sub(Vertices^[1].Position,Vertices^[0].Position),Vector3Sub(Vertices^[2].Position,Vertices^[0].Position))),fWorldTransform);
    result:=true;
   end;
  end;
 end;
end;

{$ifdef DebugDraw}
procedure TKraftShapeTriangle.Draw(const CameraMatrix:TKraftMatrix4x4);
var ModelViewMatrix:TKraftMatrix4x4;
    Vertices:PPKraftConvexHullVertices;
    n:TKraftVector3;
begin

 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);
 ModelViewMatrix:=Matrix4x4TermMul(fInterpolatedWorldTransform,CameraMatrix);
{$ifdef UseDouble}
 glLoadMatrixd(pointer(@ModelViewMatrix));
{$else}
 glLoadMatrixf(pointer(@ModelViewMatrix));
{$endif}

 if fDrawDisplayList=0 then begin
  Vertices:=@fConvexHull.fVertices[0];

  fDrawDisplayList:=glGenLists(1);
  glNewList(fDrawDisplayList,GL_COMPILE);

  n:=Vector3NormEx(Vector3Cross(Vector3Sub(Vertices^[1].Position,Vertices^[0].Position),Vector3Sub(Vertices^[2].Position,Vertices^[0].Position)));

{$ifdef UseDouble}
  glBegin(GL_TRIANGLES);
  glNormal3dv(@n);
  glVertex3dv(@Vertices^[0].Position);
  glVertex3dv(@Vertices^[1].Position);
  glVertex3dv(@Vertices^[2].Position);
  n:=Vector3Neg(n);
  glNormal3dv(@n);
  glVertex3dv(@Vertices^[0].Position);
  glVertex3dv(@Vertices^[2].Position);
  glVertex3dv(@Vertices^[1].Position);
  glEnd;
{$else}
  glBegin(GL_TRIANGLES);
  glNormal3fv(@n);
  glVertex3fv(@Vertices^[0].Position);
  glVertex3fv(@Vertices^[1].Position);
  glVertex3fv(@Vertices^[2].Position);
  n:=Vector3Neg(n);
  glNormal3fv(@n);
  glVertex3fv(@Vertices^[0].Position);
  glVertex3fv(@Vertices^[2].Position);
  glVertex3fv(@Vertices^[1].Position);
  glEnd;
{$endif}

  glEndList;
 end;

 if fDrawDisplayList<>0 then begin
  glCallList(fDrawDisplayList);
 end;

 glPopMatrix;
end;
{$endif}

constructor TKraftShapeMesh.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AMesh:TKraftMesh);
begin

 fMesh:=AMesh;

 inherited Create(APhysics,ARigidBody);

 fIsMesh:=true;

 fShapeType:=kstMesh;

 fFeatureRadius:=0.0;

end;

destructor TKraftShapeMesh.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftShapeMesh.UpdateShapeAABB;
begin
 fShapeAABB:=fMesh.fAABB;
end;

procedure TKraftShapeMesh.CalculateMassData;
begin
end;

function TKraftShapeMesh.GetSignedDistance(const Position:TKraftVector3):TKraftScalar;
begin
 result:=Mesh.GetSignedDistance(Vector3TermMatrixMulInverted(Position,fWorldTransform));
end;

function TKraftShapeMesh.GetLocalFullSupport(const Direction:TKraftVector3):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftShapeMesh.GetLocalFeatureSupportVertex(const Index:longint):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftShapeMesh.GetLocalFeatureSupportIndex(const Direction:TKraftVector3):longint;
begin
 result:=-1;
end;

function TKraftShapeMesh.GetCenter(const Transform:TKraftMatrix4x4):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftShapeMesh.TestPoint(const p:TKraftVector3):boolean;
begin
 result:=false;
end;

function TKraftShapeMesh.RayCast(var RayCastData:TKraftRaycastData):boolean;
var SkipListNodeIndex,TriangleIndex:longint;
    SkipListNode:PKraftMeshSkipListNode;
    Triangle:PKraftMeshTriangle;
    First:boolean;
    Nearest,Time,u,v:TKraftScalar;
    Origin,Direction,p,Normal:TKraftVector3;
begin
 result:=false;
 Origin:=Vector3TermMatrixMulInverted(RayCastData.Origin,fWorldTransform);
 Direction:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(RayCastData.Direction,fWorldTransform));
 if Vector3LengthSquared(Direction)>EPSILON then begin
  Nearest:=MAX_SCALAR;
  First:=true;
  SkipListNodeIndex:=0;  
  while SkipListNodeIndex<fMesh.fCountSkipListNodes do begin
   SkipListNode:=@fMesh.fSkipListNodes[SkipListNodeIndex];
   if AABBRayIntersect(SkipListNode^.AABB,Origin,Direction) then begin
    TriangleIndex:=SkipListNode^.TriangleIndex;
    while TriangleIndex>=0 do begin
     Triangle:=@fMesh.fTriangles[TriangleIndex];
     if RayIntersectTriangle(Origin,
                             Direction,
                             fMesh.fVertices[Triangle^.Vertices[0]],
                             fMesh.fVertices[Triangle^.Vertices[1]],
                             fMesh.fVertices[Triangle^.Vertices[2]],
                             Time,
                             u,
                             v) then begin
      p:=Vector3Add(Origin,Vector3ScalarMul(Direction,Time));
      if ((Time>=0.0) and (Time<=RayCastData.MaxTime)) and (First or (Time<Nearest)) then begin
       First:=false;
       Nearest:=Time;
       Normal:=Vector3Norm(Vector3Add(Vector3ScalarMul(fMesh.fNormals[Triangle^.Normals[0]],1.0-(u+v)),
                           Vector3Add(Vector3ScalarMul(fMesh.fNormals[Triangle^.Normals[1]],u),
                                      Vector3ScalarMul(fMesh.fNormals[Triangle^.Normals[2]],v))));
       RayCastData.TimeOfImpact:=Time;
       RayCastData.Point:=p;
       RayCastData.Normal:=Normal;
       result:=true;
      end;
     end;
     TriangleIndex:=Triangle^.Next;
    end;
    inc(SkipListNodeIndex);
   end else begin
    SkipListNodeIndex:=SkipListNode^.SkipToNodeIndex;
   end;
  end;
  if result then begin
   RayCastData.Point:=Vector3TermMatrixMul(RayCastData.Point,fWorldTransform);
   RayCastData.Normal:=Vector3NormEx(Vector3TermMatrixMulBasis(RayCastData.Normal,fWorldTransform));
  end;
 end;
end;

{$ifdef DebugDraw}
procedure TKraftShapeMesh.Draw(const CameraMatrix:TKraftMatrix4x4);
var i:longint;
    ModelViewMatrix:TKraftMatrix4x4;
    Triangle:PKraftMeshTriangle;
begin
 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);
 ModelViewMatrix:=Matrix4x4TermMul(fInterpolatedWorldTransform,CameraMatrix);
{$ifdef UseDouble}
 glLoadMatrixd(pointer(@ModelViewMatrix));
{$else}
 glLoadMatrixf(pointer(@ModelViewMatrix));
{$endif}

 if fDrawDisplayList=0 then begin
  fDrawDisplayList:=glGenLists(1);
  glNewList(fDrawDisplayList,GL_COMPILE);

  glBegin(GL_TRIANGLES);
  for i:=0 to fMesh.fCountTriangles-1 do begin
   Triangle:=@fMesh.fTriangles[i];
{$ifdef UseDouble}
   glNormal3dv(@fMesh.fNormals[Triangle^.Normals[0]]);
   glVertex3dv(@fMesh.fVertices[Triangle^.Vertices[0]]);
   glNormal3dv(@fMesh.fNormals[Triangle^.Normals[1]]);
   glVertex3dv(@fMesh.fVertices[Triangle^.Vertices[1]]);
   glNormal3dv(@fMesh.fNormals[Triangle^.Normals[2]]);
   glVertex3dv(@fMesh.fVertices[Triangle^.Vertices[2]]);
{$else}
   glNormal3fv(@fMesh.fNormals[Triangle^.Normals[0]]);
   glVertex3fv(@fMesh.fVertices[Triangle^.Vertices[0]]);
   glNormal3fv(@fMesh.fNormals[Triangle^.Normals[1]]);
   glVertex3fv(@fMesh.fVertices[Triangle^.Vertices[1]]);
   glNormal3fv(@fMesh.fNormals[Triangle^.Normals[2]]);
   glVertex3fv(@fMesh.fVertices[Triangle^.Vertices[2]]);
{$endif}

  end;
  glEnd;

  glEndList;
 end;

 if fDrawDisplayList<>0 then begin
  glCallList(fDrawDisplayList);
 end;

 glPopMatrix;
end;
{$endif}

procedure TKraftContactPair.GetSolverContactManifold(out SolverContactManifold:TKraftSolverContactManifold;const WorldTransformA,WorldTransformB:TKraftMatrix4x4;const ContactManifoldMode:TKraftContactPairContactManifoldMode);
var ContactIndex:longint;
    Contact:PKraftContact;
    SolverContact:PKraftSolverContact;
    PointA,PointB,PlanePoint,ClipPoint,cA,cB:TKraftVector3;
    tA,tB:TKraftMatrix4x4;
begin
 tA:=Matrix4x4TermMul(Shapes[0].fLocalTransform,WorldTransformA);
 tB:=Matrix4x4TermMul(Shapes[1].fLocalTransform,WorldTransformB);
 SolverContactManifold.CountContacts:=Manifold.CountContacts;
 case Manifold.ContactManifoldType of
  kcmtImplicit,kcmtImplicitNormal:begin
   case Manifold.ContactManifoldType of
    kcmtImplicit:begin
     SolverContactManifold.Normal:=Vector3Origin;
     for ContactIndex:=0 to Manifold.CountContacts-1 do begin
      Contact:=@Manifold.Contacts[ContactIndex];
      PointA:=Vector3TermMatrixMul(Contact^.LocalPoints[0],tA);
      PointB:=Vector3TermMatrixMul(Contact^.LocalPoints[1],tB);
      SolverContactManifold.Normal:=Vector3Add(SolverContactManifold.Normal,Vector3Sub(PointB,PointA));
     end;
     if Vector3LengthSquared(SolverContactManifold.Normal)<EPSILON then begin
      SolverContactManifold.Normal:=Vector3YAxis;
     end else begin
      SolverContactManifold.Normal:=Vector3NormEx(SolverContactManifold.Normal);
     end;
    end;
    else {kcmtImplicitNormal:}begin
     SolverContactManifold.Normal:=Vector3TermMatrixMulBasis(Manifold.LocalNormal,tB);
    end;
   end;
   for ContactIndex:=0 to Manifold.CountContacts-1 do begin
    Contact:=@Manifold.Contacts[ContactIndex];
    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];
    PointA:=Vector3TermMatrixMul(Contact^.LocalPoints[0],tA);
    PointB:=Vector3TermMatrixMul(Contact^.LocalPoints[1],tB);
    case ContactManifoldMode of
     kcpcmmVelocitySolver,kcpcmmBaumgarte,kcpcmmTemporalCoherence:begin
      cA:=Vector3Add(PointA,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[0]));
      cB:=Vector3Sub(PointB,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[1]));
      SolverContact^.Point:=Vector3Avg(cA,cB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(cB,cA),SolverContactManifold.Normal);
     end;
     kcpcmmPositionSolver:begin
      SolverContact^.Point:=Vector3Avg(PointA,PointB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(PointB,PointA),SolverContactManifold.Normal)-(Manifold.LocalRadius[0]+Manifold.LocalRadius[1]);{}
     end;
    end;
   end;
  end;
  kcmtFaceA:begin
   SolverContactManifold.Normal:=Vector3TermMatrixMulBasis(Manifold.LocalNormal,tA);
   for ContactIndex:=0 to Manifold.CountContacts-1 do begin
    Contact:=@Manifold.Contacts[ContactIndex];
    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];
    PlanePoint:=Vector3TermMatrixMul(Contact^.LocalPoints[0],tA);
    ClipPoint:=Vector3TermMatrixMul(Contact^.LocalPoints[1],tB);
    case ContactManifoldMode of
     kcpcmmVelocitySolver,kcpcmmBaumgarte,kcpcmmTemporalCoherence:begin
      cA:=Vector3Add(ClipPoint,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[0]-Vector3Dot(Vector3Sub(ClipPoint,PlanePoint),SolverContactManifold.Normal)));
      cB:=Vector3Sub(ClipPoint,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[1]));
      SolverContact^.Point:=Vector3Avg(cA,cB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(cB,cA),SolverContactManifold.Normal);
     end;
     kcpcmmPositionSolver:begin
      SolverContact^.Point:=ClipPoint;
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(ClipPoint,PlanePoint),SolverContactManifold.Normal)-(Manifold.LocalRadius[0]+Manifold.LocalRadius[1]);
     end;
    end;
   end;
  end;
  kcmtFaceB:begin
   SolverContactManifold.Normal:=Vector3TermMatrixMulBasis(Manifold.LocalNormal,tB);
   for ContactIndex:=0 to Manifold.CountContacts-1 do begin
    Contact:=@Manifold.Contacts[ContactIndex];
    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];
    ClipPoint:=Vector3TermMatrixMul(Contact^.LocalPoints[0],tA);
    PlanePoint:=Vector3TermMatrixMul(Contact^.LocalPoints[1],tB);
    case ContactManifoldMode of
     kcpcmmVelocitySolver,kcpcmmBaumgarte,kcpcmmTemporalCoherence:begin
      cA:=Vector3Sub(ClipPoint,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[0]));
      cB:=Vector3Add(ClipPoint,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[1]-Vector3Dot(Vector3Sub(ClipPoint,PlanePoint),SolverContactManifold.Normal)));
      SolverContact^.Point:=Vector3Avg(cA,cB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(cA,cB),SolverContactManifold.Normal);
     end;
     kcpcmmPositionSolver:begin
      SolverContact^.Point:=ClipPoint;
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(ClipPoint,PlanePoint),SolverContactManifold.Normal)-(Manifold.LocalRadius[0]+Manifold.LocalRadius[1]);
     end;
    end;
   end;
   SolverContactManifold.Normal:=Vector3Neg(SolverContactManifold.Normal);
  end;
  kcmtEdges,kcmtImplicitEdge:begin
   case Manifold.ContactManifoldType of
    kcmtEdges:begin
     SolverContactManifold.Normal:=Vector3TermMatrixMulBasis(Manifold.LocalNormal,tA);
    end;
    else {kcmtImplicitEdge:}begin
     SolverContactManifold.Normal:=Vector3TermMatrixMulBasis(Manifold.LocalNormal,tB);
    end;
   end;
   for ContactIndex:=0 to Manifold.CountContacts-1 do begin
    Contact:=@Manifold.Contacts[ContactIndex];
    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];
    PointA:=Vector3TermMatrixMul(Contact^.LocalPoints[0],tA);
    PointB:=Vector3TermMatrixMul(Contact^.LocalPoints[1],tB);
    case ContactManifoldMode of
     kcpcmmVelocitySolver,kcpcmmBaumgarte,kcpcmmTemporalCoherence:begin
      cA:=Vector3Add(PointA,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[0]));
      cB:=Vector3Sub(PointB,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[1]));
      SolverContact^.Point:=Vector3Avg(cA,cB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(cB,cA),SolverContactManifold.Normal);
     end;
     kcpcmmPositionSolver:begin
      SolverContact^.Point:=Vector3Avg(PointA,PointB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(PointB,PointA),SolverContactManifold.Normal)-(Manifold.LocalRadius[0]+Manifold.LocalRadius[1]);
     end;
    end;
   end;
  end;
  kcmtPersistentImplicit:begin
   SolverContactManifold.Normal:=Vector3TermMatrixMulBasis(Manifold.LocalNormal,tB);
   for ContactIndex:=0 to Manifold.CountContacts-1 do begin
    Contact:=@Manifold.Contacts[ContactIndex];
    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];
    PointA:=Vector3TermMatrixMul(Contact^.LocalPoints[0],tA);
    PointB:=Vector3TermMatrixMul(Contact^.LocalPoints[1],tB);
    case ContactManifoldMode of
     kcpcmmVelocitySolver,kcpcmmBaumgarte,kcpcmmTemporalCoherence:begin
      cA:=Vector3Add(PointA,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[0]));
      cB:=Vector3Sub(PointB,Vector3ScalarMul(SolverContactManifold.Normal,Manifold.LocalRadius[1]));
      SolverContact^.Point:=Vector3Avg(cA,cB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(cB,cA),SolverContactManifold.Normal);
     end;
     kcpcmmPositionSolver:begin
      SolverContact^.Point:=Vector3Avg(PointA,PointB);
      SolverContact^.Separation:=Vector3Dot(Vector3Sub(PointB,PointA),SolverContactManifold.Normal)-(Manifold.LocalRadius[0]+Manifold.LocalRadius[1]);{}
     end;
    end;
    SolverContact^.Separation:=SolverContact^.Separation+Contact.Penetration;
   end;
  end;
  kcmtSpeculative:begin
   SolverContactManifold.Normal:=Vector3TermMatrixMulBasis(Manifold.LocalNormal,tB);
   for ContactIndex:=0 to Manifold.CountContacts-1 do begin
    Contact:=@Manifold.Contacts[ContactIndex];
    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];
    SolverContactManifold.Points[0]:=Vector3TermMatrixMul(Contact^.LocalPoints[0],tA);
    SolverContactManifold.Points[1]:=Vector3TermMatrixMul(Contact^.LocalPoints[1],tB);
    SolverContact^.Point:=Vector3Avg(SolverContactManifold.Points[0],SolverContactManifold.Points[1]);
    SolverContact^.Separation:=Contact^.Penetration;
   end;
  end;
 end;
end;

procedure TKraftContactPair.DetectCollisions(const ContactManager:TKraftContactManager;const TriangleShape:TKraftShape=nil;const ThreadIndex:longint=0;const SpeculativeContacts:boolean=true;const DeltaTime:double=0.0);
var OldManifoldCountContacts:longint;
    OldContactManifoldType:TKraftContactManifoldType;
    ShapeTriangle:TKraftShapeTriangle;
 function CreateFeatureID(const ElementA,ElementB:longword):TKraftContactFeatureID; overload; {$ifdef caninline}inline;{$endif}
 begin
  result.ElementA:=ElementA;
  result.ElementB:=ElementB;
 end;
 function CreateFeatureID(const Key:int64):TKraftContactFeatureID; overload; {$ifdef caninline}inline;{$endif}
 begin
  result.Key:=Key;
 end;
 procedure AddImplicitContact(const p0,p1:TKraftVector3;const r0,r1:TKraftScalar;const FeatureID:TKraftContactFeatureID;const IsLocal:boolean); {$ifdef caninline}inline;{$endif}
 var Contact:PKraftContact;
 begin
  if Manifold.CountContacts<MAX_CONTACTS then begin
   Manifold.ContactManifoldType:=kcmtImplicit;
   Manifold.LocalRadius[0]:=r0;
   Manifold.LocalRadius[1]:=r1;
   Contact:=@Manifold.Contacts[Manifold.CountContacts];
   inc(Manifold.CountContacts);
   if IsLocal then begin
    Contact^.LocalPoints[0]:=p0;
    Contact^.LocalPoints[1]:=p1;
   end else begin
    Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(p0,Shapes[0].fWorldTransform);
    Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(p1,Shapes[1].fWorldTransform);
   end;
   Contact^.FeatureID:=FeatureID;
  end;
 end;
 procedure AddFaceAContact(const Normal,p0,p1:TKraftVector3;const r0,r1:TKraftScalar;const FeatureID:TKraftContactFeatureID;const IsLocal:boolean); {$ifdef caninline}inline;{$endif}
 var Contact:PKraftContact;
 begin
  if Manifold.CountContacts<MAX_CONTACTS then begin
   Manifold.ContactManifoldType:=kcmtFaceA;
   Manifold.LocalNormal:=Normal;
   Manifold.LocalRadius[0]:=r0;
   Manifold.LocalRadius[1]:=r1;
   Contact:=@Manifold.Contacts[Manifold.CountContacts];
   inc(Manifold.CountContacts);
   if IsLocal then begin
    Contact^.LocalPoints[0]:=p0;
    Contact^.LocalPoints[1]:=p1;
   end else begin
    Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(p0,Shapes[0].fWorldTransform);
    Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(p1,Shapes[1].fWorldTransform);
   end;
   Contact^.FeatureID:=FeatureID;
  end;
 end;
 procedure AddFaceBContact(const Normal,p0,p1:TKraftVector3;const r0,r1:TKraftScalar;const FeatureID:TKraftContactFeatureID;const IsLocal:boolean); {$ifdef caninline}inline;{$endif}
 var Contact:PKraftContact;
 begin
  if Manifold.CountContacts<MAX_CONTACTS then begin
   Manifold.ContactManifoldType:=kcmtFaceB;
   Manifold.LocalNormal:=Normal;
   Manifold.LocalRadius[0]:=r0;
   Manifold.LocalRadius[1]:=r1;
   Contact:=@Manifold.Contacts[Manifold.CountContacts];
   inc(Manifold.CountContacts);
   if IsLocal then begin
    Contact^.LocalPoints[0]:=p0;
    Contact^.LocalPoints[1]:=p1;
   end else begin
    Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(p0,Shapes[0].fWorldTransform);
    Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(p1,Shapes[1].fWorldTransform);
   end;
   Contact^.FeatureID:=FeatureID;
  end;
 end;
 procedure AddImplicitEdgeContact(const Normal,p0,p1:TKraftVector3;const r0,r1:TKraftScalar;const FeatureID:TKraftContactFeatureID;const IsLocal:boolean); {$ifdef caninline}inline;{$endif}
 var Contact:PKraftContact;
 begin
  if Manifold.CountContacts<MAX_CONTACTS then begin
   Manifold.ContactManifoldType:=kcmtImplicitEdge;
   Manifold.LocalNormal:=Normal;
   Manifold.LocalRadius[0]:=r0;
   Manifold.LocalRadius[1]:=r1;
   Contact:=@Manifold.Contacts[Manifold.CountContacts];
   inc(Manifold.CountContacts);
   if IsLocal then begin
    Contact^.LocalPoints[0]:=p0;
    Contact^.LocalPoints[1]:=p1;
   end else begin
    Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(p0,Shapes[0].fWorldTransform);
    Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(p1,Shapes[1].fWorldTransform);
   end;
   Contact^.FeatureID:=FeatureID;
  end;
 end;
 procedure AddImplicitNormalContact(const Normal,p0,p1:TKraftVector3;const r0,r1:TKraftScalar;const FeatureID:TKraftContactFeatureID;const IsLocal:boolean); {$ifdef caninline}inline;{$endif}
 var Contact:PKraftContact;
 begin
  if Manifold.CountContacts<MAX_CONTACTS then begin
   Manifold.ContactManifoldType:=kcmtImplicitNormal;
   Manifold.LocalNormal:=Normal;
   Manifold.LocalRadius[0]:=r0;
   Manifold.LocalRadius[1]:=r1;
   Contact:=@Manifold.Contacts[Manifold.CountContacts];
   inc(Manifold.CountContacts);
   if IsLocal then begin
    Contact^.LocalPoints[0]:=p0;
    Contact^.LocalPoints[1]:=p1;
   end else begin
    Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(p0,Shapes[0].fWorldTransform);
    Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(p1,Shapes[1].fWorldTransform);
   end;
   Contact^.FeatureID:=FeatureID;
  end;
 end;
 procedure CollideSphereWithSphere(ShapeA,ShapeB:TKraftShapeSphere); {$ifdef caninline}inline;{$endif}
 var Distance:TKraftScalar;
     CenterA,CenterB:TKraftVector3;
 begin
  CenterA:=Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform);
  CenterB:=Vector3TermMatrixMul(ShapeB.fLocalCentroid,ShapeB.fWorldTransform);
  Distance:=Vector3Length(Vector3Sub(CenterB,CenterA));
  if Distance<(ShapeA.fRadius+ShapeB.fRadius) then begin
   AddImplicitContact(ShapeA.fLocalCentroid,ShapeB.fLocalCentroid,ShapeA.fRadius,ShapeB.fRadius,CreateFeatureID(0),true);
  end;
 end;
 procedure CollideSphereWithCapsule(ShapeA:TKraftShapeSphere;ShapeB:TKraftShapeCapsule); {$ifdef caninline}inline;{$endif}
 var Alpha,HalfLength,Distance:TKraftScalar;
     CenterA,CenterB,Position,GeometryDirection:TKraftVector3;
 begin
  GeometryDirection:=PKraftVector3(pointer(@ShapeB.fWorldTransform[1,0]))^;
  CenterA:=Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform);
  CenterB:=Vector3TermMatrixMul(ShapeB.fLocalCentroid,ShapeB.fWorldTransform);
  Alpha:=(GeometryDirection.x*(CenterA.x-CenterB.x))+(GeometryDirection.y*(Centera.y-CenterB.y))+(GeometryDirection.z*(CenterA.z-CenterB.z));
  HalfLength:=ShapeB.fHeight*0.5;
  if Alpha>HalfLength then begin
   Alpha:=HalfLength;
  end else if alpha<-HalfLength then begin
   Alpha:=-HalfLength;
  end;
  Position:=Vector3Add(CenterB,Vector3ScalarMul(GeometryDirection,Alpha));
  Distance:=Vector3DistSquared(Position,CenterA);
  if Distance<=sqr(ShapeA.fRadius+ShapeB.fRadius) then begin
   AddImplicitContact(CenterA,Position,ShapeA.fRadius,ShapeB.fRadius,CreateFeatureID(0),false);
  end;
 end;
 procedure CollideSphereWithConvexHull(ShapeA:TKraftShapeSphere;ShapeB:TKraftShapeConvexHull); {$ifdef caninline}inline;{$endif}
 var FaceIndex,ClosestFaceIndex,VertexIndex,BestClosestFaceIndex:longint;
     Distance,ClosestDistance,BestClosestPointDistance,d:TKraftScalar;
     Center,SphereCenter,Normal,ClosestPoint,BestClosestPointOnHull,BestClosestPointNormal,ab,ap,a,b,v,n:TKraftVector3;
     InsideSphere,InsidePolygon,HasBestClosestPoint:boolean;
     Face:PKraftConvexHullFace;
     GJK:TKraftGJK;
 begin

  GJK.CachedSimplex:=@Manifold.GJKCachedSimplex;
  GJK.Simplex.Count:=0;
  GJK.Shapes[0]:=ShapeA;
  GJK.Shapes[1]:=ShapeB;
  GJK.Transforms[0]:=@ShapeA.fWorldTransform;
  GJK.Transforms[1]:=@ShapeB.fWorldTransform;
  GJK.UseRadii:=false;

  GJK.Run;

  if (GJK.Distance>0.0) and not GJK.Failed then begin

   // Shallow contact, the more simple way

   if GJK.Distance<=ShapeA.fRadius then begin
    AddImplicitNormalContact(Vector3Neg(Vector3TermMatrixMulTransposedBasis(GJK.Normal,Shapes[1].fWorldTransform)),
                             GJK.ClosestPoints[0],
                             GJK.ClosestPoints[1],
                             ShapeA.fRadius,
                             0.0,
                             CreateFeatureID(-2),  
                             false);
   end;

  end else begin

   // Deep contact, the more hard way, the followed code works also for shallow contacts, but GJK should be faster for shallow
   // contacts, I think.

   BestClosestPointNormal:=Vector3Origin;
   BestClosestPointOnHull:=Vector3Origin;
   BestClosestPointDistance:=MAX_SCALAR;
   HasBestClosestPoint:=false;
   BestClosestFaceIndex:=-1;
   ClosestDistance:=MAX_SCALAR;
   ClosestFaceIndex:=-1;
   InsideSphere:=true;
   Center:=Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform);
   SphereCenter:=Vector3TermMatrixMulInverted(Center,ShapeB.fWorldTransform);
   for FaceIndex:=0 to ShapeB.fConvexHull.fCountFaces-1 do begin
    Face:=@ShapeB.fConvexHull.fFaces[FaceIndex];
    Distance:=PlaneVectorDistance(Face^.Plane,SphereCenter);
    if Distance>0.0 then begin
     // sphere center is not inside in the convex hull . . .
     if Distance<ShapeA.fRadius then begin
      // but touching . . .
      if Face^.CountVertices>0 then begin
       InsidePolygon:=true;
       n:=Face^.Plane.Normal;
       b:=ShapeB.fConvexHull.fVertices[Face^.Vertices[Face^.CountVertices-1]].Position;
       ClosestPoint:=Vector3Origin;
       for VertexIndex:=0 to Face^.CountVertices-1 do begin
        a:=b;
        b:=ShapeB.fConvexHull.fVertices[Face^.Vertices[VertexIndex]].Position;
        ab:=Vector3Sub(b,a);
        ap:=Vector3Sub(SphereCenter,a);
        v:=Vector3Cross(ab,n);
        if Vector3Dot(ap,v)>0.0 then begin
         d:=Vector3LengthSquared(ab);
         if d<>0.0 then begin
          d:=Vector3Dot(ab,ap)/d;
         end else begin
          d:=0.0;
         end;
         ClosestPoint:=Vector3Lerp(a,b,d);
         InsidePolygon:=false;
         break;
        end;
       end;
       if InsidePolygon then begin
        // sphere is directly touching the convex hull . . .
        AddFaceBContact(n,
                        Center,
                        Vector3TermMatrixMul(Vector3Sub(SphereCenter,Vector3ScalarMul(n,Distance)),ShapeB.fWorldTransform),
                        ShapeA.fRadius,
                        0.0,
                        CreateFeatureID(0,FaceIndex),
                        false);
        exit;
       end else begin
        // the sphere may not be directly touching the polyhedron, but it may be touching a point or an edge, if the distance between
        // the closest point on the poly and the center of the sphere is less than the sphere radius we have a hit.
        Normal:=Vector3Sub(SphereCenter,ClosestPoint);
        if Vector3LengthSquared(Normal)<sqr(ShapeA.fRadius) then begin
         Distance:=Vector3LengthNormalize(Normal);
         if (not HasBestClosestPoint) or (BestClosestPointDistance>Distance) then begin
          HasBestClosestPoint:=true;
          BestClosestPointDistance:=Distance;
          BestClosestPointOnHull:=ClosestPoint;
          BestClosestPointNormal:=Normal;
          BestClosestFaceIndex:=FaceIndex;
         end;
        end;
       end;
      end;
     end;
     InsideSphere:=false;
    end else if InsideSphere and ((ClosestFaceIndex<0) or (ClosestDistance>abs(Distance))) then begin
     ClosestDistance:=abs(Distance);
     ClosestFaceIndex:=FaceIndex;
    end;
   end;
   if InsideSphere and (ClosestFaceIndex>=0) then begin
    // the sphere center is inside the convex hull . . .
    n:=ShapeB.fConvexHull.fFaces[ClosestFaceIndex].Plane.Normal;
    AddImplicitNormalContact(n,
                             Center,
                             Vector3TermMatrixMul(Vector3Add(SphereCenter,Vector3ScalarMul(n,-ClosestDistance)),ShapeB.fWorldTransform),
                             ShapeA.fRadius,
                             0.0,
                             CreateFeatureID(1,ClosestFaceIndex),
                             false);
   end else if HasBestClosestPoint then begin
    AddImplicitNormalContact(Vector3Neg(BestClosestPointNormal),
                             Center,
                             Vector3TermMatrixMul(BestClosestPointOnHull,ShapeB.fWorldTransform),
                             ShapeA.fRadius,
                             0.0,
                             CreateFeatureID(2,BestClosestFaceIndex),
                             false);
   end;
  end;
 end;
 procedure CollideSphereWithBox(ShapeA:TKraftShapeSphere;ShapeB:TKraftShapeBox); {$ifdef caninline}inline;{$endif}
 //const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
 var IntersectionDist,ContactDist,DistSqr,FaceDist,MinDist:TKraftScalar;
     Center,SphereRelativePosition,ClosestPoint,Normal:TKraftVector3;
     Axis,AxisSign,BestFaceIndex:longint;
 begin
  Center:=Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform);
  SphereRelativePosition:=Vector3TermMatrixMulInverted(Center,ShapeB.fWorldTransform);
  ClosestPoint.x:=Min(Max(SphereRelativePosition.x,-ShapeB.fExtents.x),ShapeB.fExtents.x);
  ClosestPoint.y:=Min(Max(SphereRelativePosition.y,-ShapeB.fExtents.y),ShapeB.fExtents.y);
  ClosestPoint.z:=Min(Max(SphereRelativePosition.z,-ShapeB.fExtents.z),ShapeB.fExtents.z);
  Normal:=Vector3Sub(SphereRelativePosition,ClosestPoint);
  DistSqr:=Vector3LengthSquared(Normal);
  IntersectionDist:=ShapeA.fRadius;
  ContactDist:=IntersectionDist+EPSILON;
  BestFaceIndex:=0;
  if DistSqr<=sqr(ContactDist) then begin
   if DistSqr<=EPSILON then begin
    begin
     FaceDist:=ShapeB.fExtents.x-SphereRelativePosition.x;
     MinDist:=FaceDist;
     Axis:=0;
     AxisSign:=1;
     BestFaceIndex:=1;
    end;
    begin
     FaceDist:=ShapeB.fExtents.x+SphereRelativePosition.x;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=0;
      AxisSign:=-1;
      BestFaceIndex:=2;
     end;
    end;
    begin
     FaceDist:=ShapeB.fExtents.y-SphereRelativePosition.y;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=1;
      AxisSign:=1;
      BestFaceIndex:=3;
     end;
    end;
    begin
     FaceDist:=ShapeB.fExtents.y+SphereRelativePosition.y;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=1;
      AxisSign:=-1;
      BestFaceIndex:=4;
     end;
    end;
    begin
     FaceDist:=ShapeB.fExtents.z-SphereRelativePosition.z;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=2;
      AxisSign:=1;
      BestFaceIndex:=5;
     end;
    end;
    begin
     FaceDist:=ShapeB.fExtents.z+SphereRelativePosition.z;
     if FaceDist<MinDist then begin
//    MinDist:=FaceDist;
      Axis:=2;
      AxisSign:=-1;
      BestFaceIndex:=6;
     end;
    end;
    ClosestPoint:=SphereRelativePosition;
    ClosestPoint.xyz[Axis]:=ShapeB.fExtents.xyz[Axis]*AxisSign;
    Normal:=Vector3Origin;
    Normal.xyz[Axis]:=AxisSign;
//  Distance:=-MinDist;
   end else begin
    {Distance:=}Vector3NormalizeEx(Normal);
   end;
   AddFaceBContact(Normal,Center,Vector3TermMatrixMul(ClosestPoint,ShapeB.fWorldTransform),ShapeA.fRadius,0.0,CreateFeatureID(BestFaceIndex),false);
  end;
 end;
 procedure CollideSphereWithPlane(ShapeA:TKraftShapeSphere;ShapeB:TKraftShapePlane); {$ifdef caninline}inline;{$endif}
 var Distance:TKraftScalar;
     Center,SphereCenter,Normal:TKraftVector3;
 begin
  Center:=Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform);
  SphereCenter:=Vector3TermMatrixMulInverted(Center,ShapeB.fWorldTransform);
  Distance:=PlaneVectorDistance(ShapeB.fPlane,SphereCenter);
  if Distance<=ShapeA.fRadius then begin
   Normal:=ShapeB.fPlane.Normal;
   AddFaceBContact(Normal,Center,Vector3TermMatrixMul(Vector3Sub(SphereCenter,Vector3ScalarMul(Normal,Distance)),ShapeB.fWorldTransform),ShapeA.fRadius,0.0,CreateFeatureID(0),false);
  end;
 end;
 procedure CollideSphereWithTriangle(ShapeA:TKraftShapeSphere;ShapeB:TKraftShapeTriangle); {$ifdef caninline}inline;{$endif}
 const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
 var i:longint;
     Radius,RadiusWithThreshold,DistanceFromPlane,ContactRadiusSqr,DistanceSqr:TKraftScalar;
     Center,SphereCenter,Normal,P0ToCenter,ContactPointOnTriangle,NearestOnEdge,ContactToCenter:TKraftVector3;
     IsInsideContactPlane,HasContact,IsEdge:boolean;
     v:array[0..2] of PKraftVector3;
 begin
  v[0]:=@ShapeB.fConvexHull.fVertices[0].Position;
  v[1]:=@ShapeB.fConvexHull.fVertices[1].Position;
  v[2]:=@ShapeB.fConvexHull.fVertices[2].Position;
  Center:=Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform);
  SphereCenter:=Vector3TermMatrixMulInverted(Center,ShapeB.fWorldTransform);
  Radius:=ShapeA.fRadius;
  RadiusWithThreshold:=Radius+EPSILON;
  Normal:=Vector3SafeNorm(Vector3Cross(Vector3Sub(v[1]^,v[0]^),Vector3Sub(v[2]^,v[0]^)));
  P0ToCenter:=Vector3Sub(SphereCenter,v[0]^);
  DistanceFromPlane:=Vector3Dot(P0ToCenter,Normal);
  if DistanceFromPlane<0.0 then begin
   DistanceFromPlane:=-DistanceFromPlane;
   Normal:=Vector3Neg(Normal);
  end;
  IsInsideContactPlane:=DistanceFromPlane<RadiusWithThreshold;
  HasContact:=false;
  IsEdge:=false;
  ContactPointOnTriangle:=Vector3Origin;
  ContactRadiusSqr:=sqr(RadiusWithThreshold);
  if IsInsideContactPlane then begin
   if PointInTriangle(v[0]^,v[1]^,v[2]^,Normal,SphereCenter) then begin
    HasContact:=true;
    ContactPointOnTriangle:=Vector3Sub(SphereCenter,Vector3ScalarMul(Normal,DistanceFromPlane));
   end else begin
    for i:=0 to 2 do begin
     DistanceSqr:=SegmentSqrDistance(v[i]^,v[ModuloThree[i+1]]^,SphereCenter,NearestOnEdge);
     if DistanceSqr<ContactRadiusSqr then begin
      HasContact:=true;
      IsEdge:=true;
      ContactPointOnTriangle:=NearestOnEdge;
     end;
    end;
   end;
  end;
  if HasContact then begin
   ContactToCenter:=Vector3Sub(SphereCenter,ContactPointOnTriangle);
   DistanceSqr:=Vector3LengthSquared(ContactToCenter);
   if DistanceSqr<ContactRadiusSqr then begin
    if DistanceSqr>EPSILON then begin
     if IsEdge then begin
      AddImplicitNormalContact(Vector3Neg(Vector3Norm(ContactToCenter)),Center,Vector3TermMatrixMul(ContactPointOnTriangle,ShapeB.fWorldTransform),ShapeA.fRadius,0.0,CreateFeatureID(2),false);
     end else begin
      AddImplicitContact(Center,Vector3TermMatrixMul(ContactPointOnTriangle,ShapeB.fWorldTransform),ShapeA.fRadius,0.0,CreateFeatureID(0),false);
     end;
    end else begin
     AddFaceBContact(Normal,Center,Vector3TermMatrixMul(ContactPointOnTriangle,ShapeB.fWorldTransform),ShapeA.fRadius,0.0,CreateFeatureID(1),false);
    end;
   end;
  end;
 end;
 procedure CollideCapsuleWithCapsule(ShapeA,ShapeB:TKraftShapeCapsule); {$ifdef caninline}inline;{$endif}
 const Tolerance=0.005;
 var RadiusA,RadiusB,SquaredRadiiWithTolerance,HalfLengthA,HalfLengthB,TimeA,TimeB,SquaredDistance:TKraftScalar;
     CenterA,CenterB,GeometryDirectionA,GeometryDirectionB,HalfAxis,ClosestPointA,ClosestPointB:TKraftVector3;
     SegmentA,SegmentB:TKraftSegment;
 begin

  CenterA:=Vector3TermMatrixMul(ShapeA.fLocalCenterOfMass,ShapeA.fWorldTransform);
  CenterB:=Vector3TermMatrixMul(ShapeB.fLocalCenterOfMass,ShapeB.fWorldTransform);

  GeometryDirectionA:=PKraftVector3(pointer(@ShapeA.fWorldTransform[1,0]))^;
  GeometryDirectionB:=PKraftVector3(pointer(@ShapeB.fWorldTransform[1,0]))^;

  RadiusA:=ShapeA.fRadius;
  RadiusB:=ShapeB.fRadius;

  SquaredRadiiWithTolerance:=sqr((RadiusA+RadiusB)+EPSILON);

  HalfLengthA:=ShapeA.fHeight*0.5;
  HalfLengthB:=ShapeB.fHeight*0.5;

  HalfAxis:=Vector3ScalarMul(GeometryDirectionA,HalfLengthA);
  SegmentA.Points[0]:=Vector3Sub(CenterA,HalfAxis);
  SegmentA.Points[1]:=Vector3Add(CenterA,HalfAxis);

  HalfAxis:=Vector3ScalarMul(GeometryDirectionB,HalfLengthB);
  SegmentB.Points[0]:=Vector3Sub(CenterB,HalfAxis);
  SegmentB.Points[1]:=Vector3Add(CenterB,HalfAxis);

  // Find the closest points between the two capsules
  SIMDSegmentClosestPoints(SegmentA,SegmentB,TimeA,ClosestPointA,TimeB,ClosestPointB);

  SquaredDistance:=Vector3DistSquared(ClosestPointA,ClosestPointB);

  if SquaredDistance<SquaredRadiiWithTolerance then begin

   AddImplicitContact(ClosestPointA,ClosestPointB,RadiusA,RadiusB,CreateFeatureID(0),false);

   // If the two capsules are nearly parallel, an additional support point provides stability
   {if (Vector3Length(Vector3Cross(GeometryDirectionA,GeometryDirectionB))<(sqrt(Vector3LengthSquared(GeometryDirectionA)*Vector3LengthSquared(GeometryDirectionB))*Tolerance)) then{}begin

    if abs(TimeA)<EPSILON then begin
     ClosestPointA:=SegmentA.Points[1];
     SIMDSegmentClosestPointTo(SegmentB,ClosestPointA,TimeB,ClosestPointB);
    end else if abs(1.0-TimeA)<EPSILON then begin
     ClosestPointA:=SegmentA.Points[0];
     SIMDSegmentClosestPointTo(SegmentB,ClosestPointA,TimeB,ClosestPointB);
    end else if abs(TimeB)<EPSILON then begin
     ClosestPointB:=SegmentB.Points[1];
     SIMDSegmentClosestPointTo(SegmentA,ClosestPointB,TimeA,ClosestPointA);
    end else if abs(1.0-TimeB)<EPSILON then begin
     ClosestPointB:=SegmentB.Points[0];
     SIMDSegmentClosestPointTo(SegmentA,ClosestPointB,TimeA,ClosestPointA);
    end else begin
     exit;
    end;

    SquaredDistance:=Vector3DistSquared(ClosestPointA,ClosestPointB);
    if SquaredDistance<SquaredRadiiWithTolerance then begin
     AddImplicitContact(ClosestPointA,ClosestPointB,RadiusA,RadiusB,CreateFeatureID(1),false);
    end;
    
   end;

  end;


 end;
 procedure CollideCapsuleWithConvexHull(ShapeA:TKraftShapeCapsule;ShapeB:TKraftShapeConvexHull); {$ifdef caninline}inline;{$endif}
 const Tolerance=0.005;
 var FaceIndex,VertexIndex,OtherVertexIndex,PointIndex,MaxFaceIndex,MaxEdgeIndex,EdgeIndex:longint;
     CapsuleRadius,Distance,MaxFaceSeparation,MaxEdgeSeparation,Separation,L:TKraftScalar;
     CapsulePosition,CapsuleAxis,CapsulePointStart,CapsulePointEnd,Normal,FaceNormal,{MaxFaceSeparateAxis,}
     MaxEdgeSeparateAxis,CenterB,Ea,Eb,Ea_x_Eb:TKraftVector3;
     OK:boolean;
     Face:PKraftConvexHullFace;
     Edge:PKraftConvexHullEdge;
     Plane:TKraftPlane;
     ClosestPoints:array[0..1] of TKraftVector3;
     GJK:TKraftGJK;
     Transform:TKraftMatrix4x4;
  function GetEdgeContact(var CA,CB:TKraftVector3;const PA,QA,PB,QB:TKraftVector3):boolean;
  var DA,DB,r:TKraftVector3;
      a,e,f,c,b,d,TA,TB:TKraftScalar;
  begin
   DA:=Vector3Sub(QA,PA);
   DB:=Vector3Sub(QB,PB);
   r:=Vector3Sub(PA,PB);
	 a:=Vector3LengthSquared(DA);
	 e:=Vector3LengthSquared(DB);
	 f:=Vector3Dot(DB,r);
	 c:=Vector3Dot(DA,r);
   b:=Vector3Dot(DA,DB);
   d:=(a*e)-sqr(b);
   if (d<>0.0) and (e<>0.0) then begin
    TA:=Min(Max(((b*f)-(c*e))/d,0.0),1.0);
    TB:=Min(Max(((b*TA)+f)/e,0.0),1.0);
    CA:=Vector3Add(PA,Vector3ScalarMul(DA,TA));
    CB:=Vector3Add(PB,Vector3ScalarMul(DB,TB));
    result:=true;
   end else begin
    result:=false;
   end;
  end;
 begin

  Manifold.CountContacts:=0;

  GJK.CachedSimplex:=@Manifold.GJKCachedSimplex;
  GJK.Simplex.Count:=0;
  GJK.Shapes[0]:=ShapeA;
  GJK.Shapes[1]:=ShapeB;
  GJK.Transforms[0]:=@ShapeA.fWorldTransform;
  GJK.Transforms[1]:=@ShapeB.fWorldTransform;
  GJK.UseRadii:=false;

  GJK.Run;
                                
  if (GJK.Distance>0.0) and not GJK.Failed then begin

   // Shallow contact

   if GJK.Distance<=ShapeA.fRadius then begin

    // Check for a parallel face first
    Normal:=GJK.Normal;
    for FaceIndex:=0 to ShapeB.fConvexHull.fCountFaces-1 do begin
     Face:=@ShapeB.fConvexHull.fFaces[FaceIndex];
     FaceNormal:=Vector3Norm(Vector3TermMatrixMulBasis(Face^.Plane.Normal,ShapeB.fWorldTransform));
     if (Vector3Dot(FaceNormal,Normal)>0.0) and
        (Vector3Length(Vector3Cross(FaceNormal,Normal))<(sqrt(Vector3LengthSquared(FaceNormal)*Vector3LengthSquared(Normal))*Tolerance)) then begin
      CapsulePosition:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform),ShapeB.fWorldTransform);
      CapsuleAxis:=Vector3Norm(Vector3TermMatrixMulTransposedBasis(Vector3(ShapeA.fWorldTransform[1,0],ShapeA.fWorldTransform[1,1],ShapeA.fWorldTransform[1,2]),ShapeB.fWorldTransform));
      ClosestPoints[0]:=Vector3Sub(CapsulePosition,Vector3ScalarMul(CapsuleAxis,ShapeA.fHeight*0.5));
      ClosestPoints[1]:=Vector3Add(CapsulePosition,Vector3ScalarMul(CapsuleAxis,ShapeA.fHeight*0.5));
      if Face^.CountVertices>0 then begin
       OK:=true;
       OtherVertexIndex:=Face^.CountVertices-1;
       for VertexIndex:=0 to Face^.CountVertices-1 do begin
        Plane.Normal:=Vector3Norm(Vector3Cross(Face^.Plane.Normal,Vector3Sub(ShapeB.fConvexHull.fVertices[Face^.Vertices[VertexIndex]].Position,ShapeB.fConvexHull.fVertices[Face^.Vertices[OtherVertexIndex]].Position)));
        Plane.Distance:=-Vector3Dot(Plane.Normal,ShapeB.fConvexHull.fVertices[Face^.Vertices[VertexIndex]].Position);
        if not ClipSegmentToPlane(Plane,ClosestPoints[0],ClosestPoints[1]) then begin
         OK:=false;
         break;
        end;
        OtherVertexIndex:=VertexIndex;
       end;
       if OK then begin
        for PointIndex:=0 to 1 do begin
         Distance:=PlaneVectorDistance(Face^.Plane,ClosestPoints[PointIndex]);
         if Distance<=ShapeA.fRadius then begin
          FaceNormal:=Face^.Plane.Normal;
          AddFaceBContact(FaceNormal,
                          Vector3TermMatrixMul(ClosestPoints[PointIndex],ShapeB.fWorldTransform),
                          Vector3TermMatrixMul(Vector3Sub(ClosestPoints[PointIndex],Vector3ScalarMul(FaceNormal,Distance)),ShapeB.fWorldTransform),
                          ShapeA.fRadius,
                          0.0,
                          CreateFeatureID(1,PointIndex),
                          false);
         end;
        end;
        if Manifold.CountContacts>1 then begin
         exit;
        end else begin
         Manifold.CountContacts:=0;
        end;
       end;
      end;
     end;
    end;

    // No parallel face plane with two contacts found, so use GJK closest points for one single implicit surface contact
    AddImplicitNormalContact(Vector3Neg(Vector3TermMatrixMulTransposedBasis(GJK.Normal,Shapes[1].fWorldTransform)),
                             GJK.ClosestPoints[0],
                             GJK.ClosestPoints[1],
                             ShapeA.fRadius,
                             0.0,
                             CreateFeatureID(0),
                             false);
   end;

  end else begin

   // Deep contact

   CapsuleRadius:=ShapeA.fRadius;

   CapsulePosition:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform),ShapeB.fWorldTransform);
   CapsuleAxis:=Vector3Norm(Vector3TermMatrixMulTransposedBasis(Vector3(ShapeA.fWorldTransform[1,0],ShapeA.fWorldTransform[1,1],ShapeA.fWorldTransform[1,2]),ShapeB.fWorldTransform));

   CapsulePointStart:=Vector3Sub(CapsulePosition,Vector3ScalarMul(CapsuleAxis,ShapeA.fHeight*0.5));
   CapsulePointEnd:=Vector3Add(CapsulePosition,Vector3ScalarMul(CapsuleAxis,ShapeA.fHeight*0.5));

   Transform:=Matrix4x4TermMulInverted(ShapeB.fWorldTransform,ShapeA.fWorldTransform);
   MaxFaceIndex:=-1;
   MaxFaceSeparation:=-MAX_SCALAR;
   for FaceIndex:=0 to ShapeB.fConvexHull.fCountFaces-1 do begin
    Face:=@ShapeB.fConvexHull.fFaces[FaceIndex];
    Plane:=PlaneFastTransform(Face^.Plane,Transform);
    Separation:=PlaneVectorDistance(Plane,ShapeA.GetLocalFullSupport(Vector3Neg(Plane.Normal)));
    if Separation>0.0 then begin
     exit;
    end else if MaxFaceSeparation<Separation then begin
     MaxFaceIndex:=FaceIndex;
     MaxFaceSeparation:=Separation;
     //MaxFaceSeparateAxis:=Face^.Plane.Normal;
    end;
   end;

   MaxEdgeIndex:=-1;
   MaxEdgeSeparation:=-MAX_SCALAR;
   MaxEdgeSeparateAxis:=Vector3Origin;
   Ea:=Vector3Sub(CapsulePointEnd,CapsulePointStart);
   CenterB:=ShapeB.fLocalCenterOfMass;
   for EdgeIndex:=0 to ShapeB.fConvexHull.fCountEdges-1 do begin
    Edge:=@ShapeB.fConvexHull.fEdges[EdgeIndex];
    if (Vector3Dot(Ea,ShapeB.fConvexHull.fFaces[Edge^.Faces[0]].Plane.Normal)*Vector3Dot(Ea,ShapeB.fConvexHull.fFaces[Edge^.Faces[1]].Plane.Normal))<0.0 then begin

     Eb:=Vector3Sub(ShapeB.fConvexHull.fVertices[Edge^.Vertices[1]].Position,ShapeB.fConvexHull.fVertices[Edge^.Vertices[0]].Position);

     // Build search direction
     Ea_x_Eb:=Vector3Cross(Ea,Eb);

     // Skip near parallel edges: |Ea x Eb| = sin(alpha) * |Ea| * |Eb|
     L:=Vector3Length(Ea_x_Eb);
     if L<(sqrt(Vector3LengthSquared(Ea)*Vector3LengthSquared(Eb))*Tolerance) then begin
      continue;
     end;

     // Assure consistent normal orientation (here: HullA -> HullB)
     Normal:=Vector3ScalarMul(Ea_x_Eb,1.0/L);
     if Vector3Dot(Normal,Vector3Sub(ShapeB.fConvexHull.fVertices[Edge^.Vertices[0]].Position,CenterB))<0.0 then begin
      Normal:=Vector3Neg(Normal);
     end;

     Separation:=Vector3Dot(Normal,Vector3Sub(CapsulePointStart,ShapeB.fConvexHull.fVertices[Edge^.Vertices[0]].Position))-CapsuleRadius;
     if Separation>0.0 then begin
      exit;
     end else if MaxEdgeSeparation<Separation then begin
      MaxEdgeSeparation:=Separation;
      MaxEdgeSeparateAxis:=Normal;
      MaxEdgeIndex:=EdgeIndex;
     end;
    end;

   end;
         
   if (MaxEdgeIndex>=0) and (MaxEdgeSeparation>(MaxFaceSeparation+0.05)) then begin
    Edge:=@ShapeB.fConvexHull.fEdges[MaxEdgeIndex];
    if GetEdgeContact(ClosestPoints[0],
                      ClosestPoints[1],
                      CapsulePointStart,
                      CapsulePointEnd,
                      ShapeB.fConvexHull.fVertices[Edge^.Vertices[0]].Position,
                      ShapeB.fConvexHull.fVertices[Edge^.Vertices[1]].Position) then begin
     AddImplicitEdgeContact(Vector3Neg(MaxEdgeSeparateAxis),
                            ClosestPoints[0],
                            ClosestPoints[1],
                            CapsuleRadius,
                            0.0,
                            CreateFeatureID(2,MaxEdgeIndex),
                            false);
     exit;
    end;
   end;

   if MaxFaceIndex>=0 then begin
    ClosestPoints[0]:=CapsulePointStart;
    ClosestPoints[1]:=CapsulePointEnd;
    Face:=@ShapeB.fConvexHull.fFaces[MaxFaceIndex];
    if Face^.CountVertices>0 then begin
     OK:=true;
     OtherVertexIndex:=Face^.CountVertices-1;
     for VertexIndex:=0 to Face^.CountVertices-1 do begin
      Plane.Normal:=Vector3Norm(Vector3Cross(Face^.Plane.Normal,Vector3Sub(ShapeB.fConvexHull.fVertices[Face^.Vertices[VertexIndex]].Position,ShapeB.fConvexHull.fVertices[Face^.Vertices[OtherVertexIndex]].Position)));
      Plane.Distance:=-Vector3Dot(Plane.Normal,ShapeB.fConvexHull.fVertices[Face^.Vertices[VertexIndex]].Position);
      if not ClipSegmentToPlane(Plane,ClosestPoints[0],ClosestPoints[1]) then begin
       OK:=false;
       break;
      end;
      OtherVertexIndex:=VertexIndex;
     end;
     if OK then begin
      FaceNormal:=Face^.Plane.Normal;
      for PointIndex:=0 to 1 do begin
       Distance:=PlaneVectorDistance(Face^.Plane,ClosestPoints[PointIndex]);
       if Distance<=ShapeA.fRadius then begin
        AddFaceBContact(FaceNormal,
                        Vector3TermMatrixMul(ClosestPoints[PointIndex],ShapeB.fWorldTransform),
                        Vector3TermMatrixMul(Vector3Sub(ClosestPoints[PointIndex],Vector3ScalarMul(FaceNormal,Distance)),ShapeB.fWorldTransform),
                        CapsuleRadius,
                        0.0,
                        CreateFeatureID(3,PointIndex),
                        false);
       end;
      end;
     end;
    end;
   end;
  end;

 end;
 procedure CollideCapsuleWithTriangle(ShapeA:TKraftShapeCapsule;ShapeB:TKraftShapeTriangle); {$ifdef caninline}inline;{$endif}
 var Index,Count:longint;
     Radius,HalfLength,SquaredDistance,SquaredRadius,d:TKraftScalar;
     Center,GeometryDirection,HalfAxis,pa,pb,Normal:TKraftVector3;
     Segment:TKraftSegment;
     Triangle:TKraftTriangle;
     UseTriangleNormal:boolean;
 begin

  Manifold.CountContacts:=0;

  Center:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(ShapeA.fLocalCentroid,ShapeA.fWorldTransform),ShapeB.fWorldTransform);

  GeometryDirection:=Vector3TermMatrixMulTransposedBasis(PKraftVector3(pointer(@ShapeA.fWorldTransform[1,0]))^,ShapeB.fWorldTransform);

  Triangle.Points[0]:=ShapeB.fConvexHull.fVertices[0].Position;
  Triangle.Points[1]:=ShapeB.fConvexHull.fVertices[1].Position;
  Triangle.Points[2]:=ShapeB.fConvexHull.fVertices[2].Position;
  Triangle.Normal:=ShapeB.fConvexHull.fFaces[0].Plane.Normal;

  Radius:=ShapeA.fRadius;

  SquaredRadius:=sqr(Radius);

  HalfLength:=ShapeA.fHeight*0.5;

  HalfAxis:=Vector3ScalarMul(GeometryDirection,HalfLength);
  Segment.Points[0]:=Vector3Sub(Center,HalfAxis);
  Segment.Points[1]:=Vector3Add(Center,HalfAxis);

  Count:=0;
  for Index:=0 to 1 do begin
   pa:=Segment.Points[Index];
   UseTriangleNormal:=SIMDTriangleClosestPointTo(Triangle,pa,pb);
   SquaredDistance:=Vector3DistSquared(pa,pb);
   if SquaredDistance<(SquaredRadius+EPSILON) then begin
    if UseTriangleNormal then begin
     Normal:=Triangle.Normal;
    end else begin
     Normal:=Vector3Sub(pa,pb);
     d:=Vector3Dot(Normal,Triangle.Normal);
     if d<-EPSILON then begin
      Normal:=Vector3Sub(pa,Vector3ScalarMul(Triangle.Normal,2.0*d));
     end;
     Vector3NormalizeEx(Normal);
    end;
    Normal:=Vector3TermMatrixMulBasis(Normal,ShapeB.fWorldTransform);
    AddFaceBContact(Normal,
                    Vector3TermMatrixMul(pa,ShapeB.fWorldTransform),
                    Vector3TermMatrixMul(pb,ShapeB.fWorldTransform),
                    Radius,
                    0.0,
                    CreateFeatureID(1,Index),
                    false);
    inc(Count);
   end;
  end;

  if Count<2 then begin

   UseTriangleNormal:=SIMDTriangleClosestPointTo(Triangle,Segment,d,pa,pb);
   SquaredDistance:=Vector3DistSquared(pa,pb);
   if ((d>=EPSILON) and (d<=(1.0-EPSILON))) and (SquaredDistance<(SquaredRadius+EPSILON)) then begin
    if UseTriangleNormal then begin
     Normal:=Triangle.Normal;
    end else begin
     Normal:=Vector3Sub(pa,pb);
     d:=Vector3Dot(Normal,Triangle.Normal);
     if d<-EPSILON then begin
      Normal:=Vector3Sub(pa,Vector3ScalarMul(Triangle.Normal,2.0*d));
     end;
     Vector3NormalizeEx(Normal);
    end;
    Normal:=Vector3TermMatrixMulBasis(Normal,ShapeB.fWorldTransform);
    AddFaceBContact(Normal,
                    Vector3TermMatrixMul(pa,ShapeB.fWorldTransform),
                    Vector3TermMatrixMul(pb,ShapeB.fWorldTransform),
                    Radius,
                    0.0,
                    CreateFeatureID(0),
                    false);
   end;

  end;

 end;
 procedure CollideConvexHullWithConvexHull(ShapeA,ShapeB:TKraftShapeConvexHull);
 const kTolerance=0.005; // Skip near parallel edges: |Ea x Eb| = sin(alpha) * |Ea| * |Eb|
       RelativeEdgeTolerance=0.90;
       RelativeFaceTolerance=0.98;
       TemporalCoherenceRelativeTransformTolerance=0.0001;
 var LinearSlop,SeparationTolerance,AbsoluteTolerance:single;
  function IsMinkowskiFace(const A,B,B_x_A,C,D,D_x_C:TKraftVector3):boolean; {$ifdef caninline}inline;{$endif}
  var CBA,DBA,ADC,BDC:TKraftScalar;
  begin
   // Test if arcs AB and CD intersect on the unit sphere
   CBA:=Vector3Dot(C,B_x_A);
   DBA:=Vector3Dot(D,B_x_A);
   ADC:=Vector3Dot(A,D_x_C);
   BDC:=Vector3Dot(B,D_x_C);
   result:=((CBA*DBA<0.0)) and ((ADC*BDC)<0.0) and ((CBA*BDC)>0.0);
  end;
  function TestEarlyFaceDirection(const HullA,HullB:TKraftShapeConvexHull;var FaceQuery:TKraftContactFaceQuery):boolean; {$ifdef caninline}inline;{$endif}
  var Plane:TKraftPlane;
      Transform:TKraftMatrix4x4;
  begin
   Transform:=Matrix4x4TermMulSimpleInverted(HullA.fWorldTransform,HullB.fWorldTransform);
   Plane:=PlaneFastTransform(HullA.fConvexHull.fFaces[FaceQuery.Index].Plane,Transform);
   FaceQuery.Separation:=PlaneVectorDistance(Plane,HullB.GetLocalFullSupport(Vector3Neg(Plane.Normal)));
   result:=FaceQuery.Separation>0.0;
  end;
  function TestEarlyEdgeDirection(const HullA,HullB:TKraftShapeConvexHull;var EdgeQuery:TKraftContactEdgeQuery):boolean; {$ifdef caninline}inline;{$endif}
  var EdgeA,EdgeB:PKraftConvexHullEdge;
      L:TKraftScalar;
      CenterA,Pa,Qa,Ea,Ua,Va,Pb,Qb,Eb,Ub,Vb,Ea_x_Eb,Normal:TKraftVector3;
      Transform:TKraftMatrix4x4;
  begin
   result:=false;
   Transform:=Matrix4x4TermMulSimpleInverted(HullA.fWorldTransform,HullB.fWorldTransform);
   CenterA:=HullA.GetCenter(Transform);
   EdgeA:=@HullA.fConvexHull.fEdges[EdgeQuery.IndexA];
   Pa:=Vector3TermMatrixMul(HullA.fConvexHull.fVertices[EdgeA^.Vertices[0]].Position,Transform);
   Qa:=Vector3TermMatrixMul(HullA.fConvexHull.fVertices[EdgeA^.Vertices[1]].Position,Transform);
   Ea:=Vector3Sub(Qa,Pa);
   Ua:=Vector3Norm(Vector3TermMatrixMulBasis(HullA.fConvexHull.fFaces[EdgeA^.Faces[0]].Plane.Normal,Transform));
   Va:=Vector3Norm(Vector3TermMatrixMulBasis(HullA.fConvexHull.fFaces[EdgeA^.Faces[1]].Plane.Normal,Transform));
   EdgeB:=@HullB.fConvexHull.fEdges[EdgeQuery.IndexB];
   Pb:=HullB.fConvexHull.fVertices[EdgeB^.Vertices[0]].Position;
   Qb:=HullB.fConvexHull.fVertices[EdgeB^.Vertices[1]].Position;
   Eb:=Vector3Sub(Qb,Pb);
   Ub:=HullB.fConvexHull.fFaces[EdgeB^.Faces[0]].Plane.Normal;
   Vb:=HullB.fConvexHull.fFaces[EdgeB^.Faces[1]].Plane.Normal;
   if IsMinkowskiFace(Ua,Va,Vector3Neg(Ea),Vector3Neg(Ub),Vector3Neg(Vb),Vector3Neg(Eb)) then begin
    // Build search direction
    Ea_x_Eb:=Vector3Cross(Ea,Eb);

    // Skip near parallel edges: |Ea x Eb| = sin(alpha) * |Ea| * |Eb|
    L:=Vector3Length(Ea_x_Eb);
    if L<(sqrt(Vector3LengthSquared(Ea)*Vector3LengthSquared(Eb))*kTolerance) then begin
     result:=false;
     exit;
    end;

    // Assure consistent normal orientation (here: HullA -> HullB)
    Normal:=Vector3ScalarMul(Ea_x_Eb,1.0/L);
    if Vector3Dot(Normal,Vector3Sub(Pa,CenterA))<0.0 then begin
     Normal:=Vector3Neg(Normal);
    end;

    // s = Dot(Normal, Pb) - d = Dot(Normal, Pb) - Dot(Normal, Pa) = Dot(Normal, Pb - Pa)
    EdgeQuery.Separation:=Vector3Dot(Normal,Vector3Sub(Pb,Pa));
    if EdgeQuery.Separation>0.0 then begin
     EdgeQuery.Normal:=Normal;
     result:=true;
    end;

   end;
  end;
  procedure QueryFaceDirections(const HullA,HullB:TKraftShapeConvexHull;out OutFaceQuery:TKraftContactFaceQuery); {$ifdef caninline}inline;{$endif}
  var MaxIndex,Index:longint;
      MaxSeparation,Separation:TKraftScalar;
      Plane:TKraftPlane;
      Transform:TKraftMatrix4x4;
  begin
   Transform:=Matrix4x4TermMulSimpleInverted(HullA.fWorldTransform,HullB.fWorldTransform);
   MaxIndex:=-1;
   MaxSeparation:=-MAX_SCALAR;
   for Index:=0 to HullA.fConvexHull.fCountFaces-1 do begin
    Plane:=PlaneFastTransform(HullA.fConvexHull.fFaces[Index].Plane,Transform);
    Separation:=PlaneVectorDistance(Plane,HullB.GetLocalFullSupport(Vector3Neg(Plane.Normal)));
    if (Index=0) or (MaxSeparation<Separation) then begin
     MaxSeparation:=Separation;
     MaxIndex:=Index;
     if MaxSeparation>0.0 then begin
      break;
     end;
    end;
   end;
   OutFaceQuery.Index:=MaxIndex;
   OutFaceQuery.Separation:=MaxSeparation;
  end;
  procedure QueryEdgeDirections(const HullA,HullB:TKraftShapeConvexHull;out OutEdgeQuery:TKraftContactEdgeQuery); {$ifdef caninline}inline;{$endif}
  var EdgeA,EdgeB:PKraftConvexHullEdge;
      IndexA,IndexB,MaxIndexA,MaxIndexB:longint;
      MaxSeparation,Separation,L:TKraftScalar;
      CenterA,Pa,Qa,Ea,Ua,Va,Pb,Qb,Eb,Ub,Vb,Ea_x_Eb,Normal,MaxNormal:TKraftVector3;
      Transform:TKraftMatrix4x4;
      First:boolean;
  begin
   MaxIndexA:=-1;
   MaxIndexB:=-1;
   MaxSeparation:=-MAX_SCALAR;
   MaxNormal:=Vector3Origin;
   Transform:=Matrix4x4TermMulSimpleInverted(HullA.fWorldTransform,HullB.fWorldTransform);
   CenterA:=HullA.GetCenter(Transform);
   First:=true;
   for IndexA:=0 to HullA.fConvexHull.fCountEdges-1 do begin
    EdgeA:=@HullA.fConvexHull.fEdges[IndexA];
    Pa:=Vector3TermMatrixMul(HullA.fConvexHull.fVertices[EdgeA^.Vertices[0]].Position,Transform);
    Qa:=Vector3TermMatrixMul(HullA.fConvexHull.fVertices[EdgeA^.Vertices[1]].Position,Transform);
    Ea:=Vector3Sub(Qa,Pa);
    Ua:=Vector3Norm(Vector3TermMatrixMulBasis(HullA.fConvexHull.fFaces[EdgeA^.Faces[0]].Plane.Normal,Transform));
    Va:=Vector3Norm(Vector3TermMatrixMulBasis(HullA.fConvexHull.fFaces[EdgeA^.Faces[1]].Plane.Normal,Transform));
    for IndexB:=0 to HullB.fConvexHull.fCountEdges-1 do begin
     EdgeB:=@HullB.fConvexHull.fEdges[IndexB];
     Pb:=HullB.fConvexHull.fVertices[EdgeB^.Vertices[0]].Position;
     Qb:=HullB.fConvexHull.fVertices[EdgeB^.Vertices[1]].Position;
     Eb:=Vector3Sub(Qb,Pb);
     Ub:=HullB.fConvexHull.fFaces[EdgeB^.Faces[0]].Plane.Normal;
     Vb:=HullB.fConvexHull.fFaces[EdgeB^.Faces[1]].Plane.Normal;
     if IsMinkowskiFace(Ua,Va,Vector3Neg(Ea),Vector3Neg(Ub),Vector3Neg(Vb),Vector3Neg(Eb)) then begin
      // Build search direction
      Ea_x_Eb:=Vector3Cross(Ea,Eb);

      // Skip near parallel edges: |Ea x Eb| = sin(alpha) * |Ea| * |Eb|
      L:=Vector3Length(Ea_x_Eb);
      if L<(sqrt(Vector3LengthSquared(Ea)*Vector3LengthSquared(Eb))*kTolerance) then begin
       continue;
      end;

      // Assure consistent normal orientation (here: HullA -> HullB)
      Normal:=Vector3ScalarMul(Ea_x_Eb,1.0/L);
      if Vector3Dot(Normal,Vector3Sub(Pa,CenterA))<0.0 then begin
       Normal:=Vector3Neg(Normal);
      end;

      // s = Dot(Normal, Pb) - d = Dot(Normal, Pb) - Dot(Normal, Pa) = Dot(Normal, Pb - Pa)
      Separation:=Vector3Dot(Normal,Vector3Sub(Pb,Pa));
      if First or (MaxSeparation<Separation) then begin
       First:=false;
       MaxSeparation:=Separation;
       MaxIndexA:=IndexA;
       MaxIndexB:=IndexB;
       MaxNormal:=Normal;
       if MaxSeparation>0.0 then begin
        break;
       end;
      end;

     end;
    end;
   end;
   OutEdgeQuery.IndexA:=MaxIndexA;
   OutEdgeQuery.IndexB:=MaxIndexB;
   OutEdgeQuery.Separation:=MaxSeparation;
   OutEdgeQuery.Normal:=MaxNormal;
  end;
  function GetEdgeContact(var CA,CB:TKraftVector3;const PA,QA,PB,QB:TKraftVector3):boolean; {$ifdef caninline}inline;{$endif}
  var DA,DB,r:TKraftVector3;
      a,e,f,c,b,d,TA,TB:TKraftScalar;
  begin
   DA:=Vector3Sub(QA,PA);
   DB:=Vector3Sub(QB,PB);
   r:=Vector3Sub(PA,PB);
	 a:=Vector3LengthSquared(DA);
	 e:=Vector3LengthSquared(DB);
	 f:=Vector3Dot(DB,r);
	 c:=Vector3Dot(DA,r);
   b:=Vector3Dot(DA,DB);
   d:=(a*e)-sqr(b);
   if (d<>0.0) and (e<>0.0) then begin
    TA:=Min(Max(((b*f)-(c*e))/d,0.0),1.0);
    TB:=Min(Max(((b*TA)+f)/e,0.0),1.0);
    CA:=Vector3Add(PA,Vector3ScalarMul(DA,TA));
    CB:=Vector3Add(PB,Vector3ScalarMul(DB,TB));
    result:=true;
   end else begin
    result:=false;
   end;
  end;
  function FindIncidentFaceIndex(const ReferenceHull:TKraftShapeConvexHull;const ReferenceFaceIndex:longint;const IncidentHull:TKraftShapeConvexHull):longint; {$ifdef caninline}inline;{$endif}
  var i:longint;
      MinDot,Dot:TKraftScalar;
      ReferenceNormal:TKraftVector3;
  begin
   ReferenceNormal:=Vector3TermMatrixMulTransposedBasis(Vector3TermMatrixMulBasis(ReferenceHull.fConvexHull.fFaces[ReferenceFaceIndex].Plane.Normal,
                                                                        ReferenceHull.fWorldTransform),
                                                        IncidentHull.fWorldTransform);
   result:=-1;
   MinDot:=MAX_SCALAR;
   for i:=0 to IncidentHull.fConvexHull.fCountFaces-1 do begin
    Dot:=Vector3Dot(ReferenceNormal,IncidentHull.fConvexHull.fFaces[i].Plane.Normal);
		if MinDot>Dot then begin
		 MinDot:=Dot;
     result:=i;
    end;
   end;
  end;
  procedure ClipFaceContactPoints(const ReferenceHull:TKraftShapeConvexHull;const ReferenceFaceIndex:longint;const IncidentHull:TKraftShapeConvexHull;const IncidentFaceIndex:longint;const Flip:boolean);
  var Contact:PKraftContact;
      ReferenceVertexIndex,OtherReferenceVertexIndex,IncidentVertexIndex,CliVertexIndex,ReferenceEdgeIndexOffset,IncidentEdgeIndexOffset:longint;
      ReferenceFace,IncidentFace:PKraftConvexHullFace;
      ClipVertex,PreviousClipVertex,CurrentClipVertex:PKraftClipVertex;
      PreviousClipVertexDistance,CurrentClipVertexDistance,Distance:TKraftScalar;
      ClipVertices:array[0..2] of TKraftClipVertexList;
      ReferencePoint:TKraftVector3;
      ReferenceWorldPlane,ReferenceEdgePlane:TKraftPlane;
      FeatureID:TKraftContactFeatureID;
  begin

   ContactManager.fCountTemporaryContacts[ThreadIndex]:=0;

   ReferenceFace:=@ReferenceHull.fConvexHull.fFaces[ReferenceFaceIndex];
   ReferenceWorldPlane:=PlaneFastTransform(ReferenceFace^.Plane,ReferenceHull.fWorldTransform);

   IncidentFace:=@IncidentHull.fConvexHull.fFaces[IncidentFaceIndex];

   ClipVertices[0]:=ContactManager.fClipVertexLists[ThreadIndex,0];
   ClipVertices[0].Clear;

   ReferenceEdgeIndexOffset:=ReferenceFace^.EdgeVertexOffset;
   IncidentEdgeIndexOffset:=IncidentFace^.EdgeVertexOffset;

   for IncidentVertexIndex:=0 to IncidentFace^.CountVertices-1 do begin
    FeatureID.ElementA:=IncidentEdgeIndexOffset+IncidentVertexIndex;
    FeatureID.ElementB:=IncidentEdgeIndexOffset+(IncidentVertexIndex+1);
    ClipVertices[0].Add(Vector3TermMatrixMul(IncidentHull.fConvexHull.fVertices[IncidentFace^.Vertices[IncidentVertexIndex]].Position,IncidentHull.fWorldTransform),FeatureID);
   end;

{$ifdef DebugDraw}
    if (ContactManager.fPhysics.SingleThreaded or (ContactManager.fPhysics.fCountThreads<2)) and (ContactManager.fCountDebugClipVertexLists<length(ContactManager.fDebugClipVertexLists)) then begin
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.r:=0.5;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.g:=1.0;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.b:=0.5;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.a:=1.0;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].Clear;
     for CliVertexIndex:=0 to ClipVertices[0].Count-1 do begin
      ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].Add(ClipVertices[0].Vertices[CliVertexIndex]);
     end;
     inc(ContactManager.fCountDebugClipVertexLists);
    end;
    if (ContactManager.fPhysics.SingleThreaded or (ContactManager.fPhysics.fCountThreads<2)) and (ContactManager.fCountDebugClipVertexLists<length(ContactManager.fDebugClipVertexLists)) then begin
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.r:=1.0;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.g:=0.5;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.b:=1.0;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.a:=1.0;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].Clear;
     for ReferenceVertexIndex:=0 to ReferenceFace^.CountVertices-1 do begin
      ReferencePoint:=Vector3TermMatrixMul(ReferenceHull.fConvexHull.fVertices[ReferenceFace^.Vertices[ReferenceVertexIndex]].Position,ReferenceHull.fWorldTransform);
      FeatureID.ElementA:=ReferenceEdgeIndexOffset+ReferenceVertexIndex;
      FeatureID.ElementB:=ReferenceEdgeIndexOffset+(ReferenceVertexIndex+1);
      ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].Add(ReferencePoint,FeatureID);
     end;
     inc(ContactManager.fCountDebugClipVertexLists);
    end;
{$endif}

   ClipVertices[1]:=ContactManager.fClipVertexLists[ThreadIndex,1];
   ClipVertices[1].Clear;

   OtherReferenceVertexIndex:=ReferenceFace^.CountVertices-1;
   for ReferenceVertexIndex:=0 to ReferenceFace^.CountVertices-1 do begin
    if ClipVertices[0].Count>=2 then begin
     ReferencePoint:=Vector3TermMatrixMul(ReferenceHull.fConvexHull.fVertices[ReferenceFace^.Vertices[ReferenceVertexIndex]].Position,ReferenceHull.fWorldTransform);
     ReferenceEdgePlane.Normal:=Vector3Neg(Vector3NormEx(Vector3Cross(ReferenceWorldPlane.Normal,Vector3Sub(ReferencePoint,Vector3TermMatrixMul(ReferenceHull.fConvexHull.fVertices[ReferenceFace^.Vertices[OtherReferenceVertexIndex]].Position,ReferenceHull.fWorldTransform)))));
     ReferenceEdgePlane.Distance:=-Vector3Dot(ReferenceEdgePlane.Normal,ReferencePoint);
     PreviousClipVertex:=@ClipVertices[0].Vertices[ClipVertices[0].Count-1];
     CurrentClipVertex:=@ClipVertices[0].Vertices[0];
     PreviousClipVertexDistance:=PlaneVectorDistance(ReferenceEdgePlane,PreviousClipVertex^.Position);
     for CliVertexIndex:=0 to ClipVertices[0].Count-1 do begin
      CurrentClipVertex:=@ClipVertices[0].Vertices[CliVertexIndex];
      CurrentClipVertexDistance:=PlaneVectorDistance(ReferenceEdgePlane,CurrentClipVertex^.Position);
      if PreviousClipVertexDistance<=0.0 then begin
       if CurrentClipVertexDistance<=0.0 then begin
        // Both vertices are behind the plane => keep CurrentClipVertex
        ClipVertices[1].Add(CurrentClipVertex^.Position,CurrentClipVertex^.FeatureID);
       end else begin
        // PreviousClipVertex is behind the plane, CurrentClipVertex is in front => intersection point
        FeatureID.ElementA:=CurrentClipVertex^.FeatureID.ElementA;
        FeatureID.ElementB:=ReferenceEdgeIndexOffset+ReferenceVertexIndex;
        ClipVertices[1].Add(Vector3Lerp(PreviousClipVertex^.Position,CurrentClipVertex^.Position,PreviousClipVertexDistance/(PreviousClipVertexDistance-CurrentClipVertexDistance)),FeatureID);
       end;
      end else if CurrentClipVertexDistance<=0.0 then begin
       // CurrentClipVertex is behind the plane, PreviousClipVertex is in front => intersection point
       FeatureID.ElementA:=ReferenceEdgeIndexOffset+ReferenceVertexIndex;
       FeatureID.ElementB:=CurrentClipVertex^.FeatureID.ElementA;
       ClipVertices[1].Add(Vector3Lerp(PreviousClipVertex^.Position,CurrentClipVertex^.Position,PreviousClipVertexDistance/(PreviousClipVertexDistance-CurrentClipVertexDistance)),FeatureID);
       ClipVertices[1].Add(CurrentClipVertex^.Position,CurrentClipVertex^.FeatureID);
      end;
      PreviousClipVertex:=CurrentClipVertex;
      PreviousClipVertexDistance:=CurrentClipVertexDistance;
     end;
    end;
    if ClipVertices[1].Count=0 then begin
     exit;
    end else begin
     ClipVertices[2]:=ClipVertices[0];
     ClipVertices[0]:=ClipVertices[1];
     ClipVertices[1]:=ClipVertices[2];
     ClipVertices[1].Clear;
     OtherReferenceVertexIndex:=ReferenceVertexIndex;
    end;
   end;

{$ifdef DebugDraw}
    if (ContactManager.fPhysics.SingleThreaded or (ContactManager.fPhysics.fCountThreads<2)) and (ContactManager.fCountDebugClipVertexLists<length(ContactManager.fDebugClipVertexLists)) then begin
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.r:=0.5;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.g:=0.5;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.b:=1.0;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].fColor.a:=1.0;
     ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].Clear;
     for CliVertexIndex:=0 to ClipVertices[0].Count-1 do begin
      ContactManager.fDebugClipVertexLists[ContactManager.fCountDebugClipVertexLists].Add(ClipVertices[0].Vertices[CliVertexIndex]);
     end;
     inc(ContactManager.fCountDebugClipVertexLists);
    end;
{$endif}

   for CliVertexIndex:=0 to ClipVertices[0].Count-1 do begin
    ClipVertex:=@ClipVertices[0].Vertices[CliVertexIndex];
    Distance:=PlaneVectorDistance(ReferenceWorldPlane,ClipVertex^.Position);
    if Distance<0.0 then begin
     if ContactManager.fCountTemporaryContacts[ThreadIndex]<MAX_TEMPORARY_CONTACTS then begin
      Contact:=@ContactManager.fTemporaryContacts[ThreadIndex,ContactManager.fCountTemporaryContacts[ThreadIndex]];
      inc(ContactManager.fCountTemporaryContacts[ThreadIndex]);
      Contact^.Penetration:=Distance;
      if Flip then begin
       // Face BA contact, where B is the reference face and where A is the incident face
       Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(ClipVertex^.Position,Shapes[0].fWorldTransform);
       Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(Vector3Sub(ClipVertex^.Position,
                                                                        Vector3ScalarMul(ReferenceWorldPlane.Normal,
                                                                                         Distance)),
                                                             Shapes[1].fWorldTransform);
       Contact^.FeatureID.ElementA:=ClipVertex^.FeatureID.ElementB;
       Contact^.FeatureID.ElementB:=ClipVertex^.FeatureID.ElementA;
      end else begin
       // Face AB contact, where A is the reference face and where B is the incident face
       Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(Vector3Sub(ClipVertex^.Position,
                                                                        Vector3ScalarMul(ReferenceWorldPlane.Normal,
                                                                                         Distance)),
                                                             Shapes[0].fWorldTransform);
       Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(ClipVertex^.Position,Shapes[1].fWorldTransform);
       Contact^.FeatureID.ElementA:=ClipVertex^.FeatureID.ElementA;
       Contact^.FeatureID.ElementB:=ClipVertex^.FeatureID.ElementB;
      end;
     end else begin
      break;
     end;
    end;
   end;

  end;
 var Physics:TKraft;
     Contact:PKraftContact;
     ReferenceFaceIndex,IncidentFaceIndex,Index,ValidContacts:longint;
     EdgeA,EdgeB:PKraftConvexHullEdge;
     PenetrationDepth:TKraftScalar;
     a0,a1,b0,b1,pa,pb,Normal:TKraftVector3;
     SolverContactManifold:TKraftSolverContactManifold;
 begin

  Physics:=ContactManager.fPhysics;

  LinearSlop:=Physics.fLinearSlop;

  SeparationTolerance:=LinearSlop;

  AbsoluteTolerance:=LinearSlop*0.5;

  Manifold.CountContacts:=0;

  Manifold.LocalRadius[0]:=0.0;
  Manifold.LocalRadius[1]:=0.0;

  // Temporal coherence
  if Manifold.HaveData then begin

   // If we detected a separating axis in the last frame we try this first in the current frame.
   // Chances are high it will still be a separating axis and we have an early out!
   if (Manifold.FaceQueryAB.Index>=0) and (Manifold.FaceQueryAB.Separation>SeparationTolerance) then begin
    if TestEarlyFaceDirection(ShapeA,ShapeB,Manifold.FaceQueryAB) then begin
     // Still existent seperating axis from last frame found, so exit!
     exit;
    end;
   end else if (Manifold.FaceQueryBA.Index>=0) and (Manifold.FaceQueryBA.Separation>SeparationTolerance) then begin
    if TestEarlyFaceDirection(ShapeB,ShapeA,Manifold.FaceQueryBA) then begin
     // Still existent seperating axis from last frame found, so exit!
     exit;
    end;
   end else if ((Manifold.EdgeQuery.IndexA>=0) and (Manifold.EdgeQuery.IndexB>=0)) and (Manifold.EdgeQuery.Separation>SeparationTolerance) then begin
    if TestEarlyEdgeDirection(ShapeA,ShapeB,Manifold.EdgeQuery) then begin
     // Still existent seperating axis from last frame found, so exit!
     exit;
    end;
   end;

   // If we detected overlap in the last frame we try to rebuild the contact manifold from the last features again.
   // Chance are high they still realize the contact manifold and we can skip the whole SAT!
   if (OldContactManifoldType<>kcmtUnknown) and
      (OldManifoldCountContacts>0) and
      (Matrix4x4Difference(Manifold.RelativeTransform,
                           Matrix4x4TermMulSimpleInverted(ShapeB.fWorldTransform,
                                                          ShapeA.fWorldTransform))<TemporalCoherenceRelativeTransformTolerance) then begin

    // Restore manifold meta data
    Manifold.ContactManifoldType:=OldContactManifoldType;
    Manifold.CountContacts:=OldManifoldCountContacts;

    // Verify last frame manifold contacts
    GetSolverContactManifold(SolverContactManifold,ShapeA.fWorldTransform,ShapeB.fWorldTransform,kcpcmmTemporalCoherence);
    if SolverContactManifold.CountContacts>0 then begin

     ValidContacts:=0;
     for Index:=0 to SolverContactManifold.CountContacts-1 do begin
      if SolverContactManifold.Contacts[Index].Separation<SeparationTolerance then begin
       inc(ValidContacts);
      end;
     end;

     if ValidContacts=SolverContactManifold.CountContacts then begin
      // We've still valid contacts, so we can reuse the contact manifold directly from the last frame and we can skip the whole SAT
//    writeln(ContactManager.fPhysics.HighResolutionTimer.GetTime);
      exit;
     end;

    end;

   end;

   // We must process a full seperating axis test, when there are no last frame contact manifold data yet, or when the
   // temporal coherence checks have failed

  end;

  // Seperating axis test
  begin

   Manifold.ContactManifoldType:=kcmtUnknown;
   Manifold.CountContacts:=0;

   Manifold.FaceQueryAB.Index:=-1;
   Manifold.FaceQueryAB.Separation:=MAX_SCALAR;

   Manifold.FaceQueryBA.Index:=-1;
   Manifold.FaceQueryBA.Separation:=MAX_SCALAR;

   Manifold.EdgeQuery.IndexA:=-1;
   Manifold.EdgeQuery.IndexB:=-1;
   Manifold.EdgeQuery.Separation:=MAX_SCALAR;

   Manifold.HaveData:=true;

   Manifold.RelativeTransform:=Matrix4x4TermMulSimpleInverted(ShapeB.fWorldTransform,ShapeA.fWorldTransform);

   QueryFaceDirections(ShapeA,ShapeB,Manifold.FaceQueryAB);
   if Manifold.FaceQueryAB.Separation>SeparationTolerance then begin
    exit;
   end;

   QueryFaceDirections(ShapeB,ShapeA,Manifold.FaceQueryBA);
   if Manifold.FaceQueryBA.Separation>SeparationTolerance then begin
    exit;
   end;

   QueryEdgeDirections(ShapeA,ShapeB,Manifold.EdgeQuery);
   if Manifold.EdgeQuery.Separation>SeparationTolerance then begin
    exit;
   end;

  end;

  // Contact generation
  begin

   ContactManager.fCountTemporaryContacts[ThreadIndex]:=0;

   if ((Manifold.EdgeQuery.IndexA>=0) and
       (Manifold.EdgeQuery.IndexB>=0)) and
      (Manifold.EdgeQuery.Separation>((Max(Manifold.FaceQueryAB.Separation,Manifold.FaceQueryBA.Separation)*RelativeEdgeTolerance)+AbsoluteTolerance)) then begin

    // Edge contact

    Manifold.HaveData:=false;

    EdgeA:=@ShapeA.fConvexHull.fEdges[Manifold.EdgeQuery.IndexA];
    EdgeB:=@ShapeB.fConvexHull.fEdges[Manifold.EdgeQuery.IndexB];

    a0:=Vector3TermMatrixMul(ShapeA.fConvexHull.fVertices[EdgeA^.Vertices[0]].Position,ShapeA.fWorldTransform);
    a1:=Vector3TermMatrixMul(ShapeA.fConvexHull.fVertices[EdgeA^.Vertices[1]].Position,ShapeA.fWorldTransform);
    b0:=Vector3TermMatrixMul(ShapeB.fConvexHull.fVertices[EdgeB^.Vertices[0]].Position,ShapeB.fWorldTransform);
    b1:=Vector3TermMatrixMul(ShapeB.fConvexHull.fVertices[EdgeB^.Vertices[1]].Position,ShapeB.fWorldTransform);

    if GetEdgeContact(pa,pb,a0,a1,b0,b1) then begin
 {   Normal:=Vector3NormEx(Vector3Cross(Vector3Sub(a1,a0),Vector3Sub(b1,b0)));
     if Vector3Dot(Normal,Vector3Sub(ShapeB.GetCenter(ShapeB.fWorldTransform),ShapeA.GetCenter(ShapeA.fWorldTransform)))<0.0 then begin
      Normal:=Vector3Neg(Normal);
     end;
 //{}Normal:=Vector3TermMatrixMulBasis(Manifold.EdgeQuery.Normal,Shapes[1].fWorldTransform);
     PenetrationDepth:=Vector3Dot(Vector3Sub(pb,pa),Normal);
     if PenetrationDepth<0.0 then begin
      Manifold.ContactManifoldType:=kcmtEdges;
      Manifold.LocalNormal:=Vector3TermMatrixMulTransposedBasis(Normal,Shapes[0].fWorldTransform);
      Contact:=@ContactManager.fTemporaryContacts[ThreadIndex,ContactManager.fCountTemporaryContacts[ThreadIndex]];
      inc(ContactManager.fCountTemporaryContacts[ThreadIndex]);
      Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(pa,Shapes[0].fWorldTransform);
      Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(pb,Shapes[1].fWorldTransform);
      Contact^.Penetration:=PenetrationDepth;
      Contact^.FeatureID.ElementA:=longword(Manifold.EdgeQuery.IndexA) or longword($80000000);
      Contact^.FeatureID.ElementB:=longword(Manifold.EdgeQuery.IndexB) or longword($80000000);
     end;
    end;

   end else begin

    // Face contact

    if Manifold.FaceQueryBA.Separation>((Manifold.FaceQueryAB.Separation*RelativeFaceTolerance)+AbsoluteTolerance) then begin

     // Face contact BA

     ReferenceFaceIndex:=FindIncidentFaceIndex(ShapeB,Manifold.FaceQueryBA.Index,ShapeA);
     IncidentFaceIndex:=Manifold.FaceQueryBA.Index;

     Manifold.ContactManifoldType:=kcmtFaceB;
     Manifold.LocalNormal:=ShapeB.fConvexHull.fFaces[Manifold.FaceQueryBA.Index].Plane.Normal;

     ClipFaceContactPoints(ShapeB,IncidentFaceIndex,ShapeA,ReferenceFaceIndex,true);

    end else begin

     // Face contact AB

     ReferenceFaceIndex:=Manifold.FaceQueryAB.Index;
     IncidentFaceIndex:=FindIncidentFaceIndex(ShapeA,Manifold.FaceQueryAB.Index,ShapeB);

     Manifold.ContactManifoldType:=kcmtFaceA;
     Manifold.LocalNormal:=ShapeA.fConvexHull.fFaces[Manifold.FaceQueryAB.Index].Plane.Normal;

     ClipFaceContactPoints(ShapeA,ReferenceFaceIndex,ShapeB,IncidentFaceIndex,false);

    end;

   end;

   if ContactManager.fCountTemporaryContacts[ThreadIndex]>0 then begin
    // Contacts found, reduce these down to four contacts with the largest area
    Manifold.CountContacts:=ContactManager.ReduceContacts(pointer(@ContactManager.fTemporaryContacts[ThreadIndex,0]),ContactManager.fCountTemporaryContacts[ThreadIndex],pointer(@Manifold.Contacts[0]));
   end;

  end;

 end;
 procedure CollideWithMPRAndPerturbation(ShapeA,ShapeB:TKraftShape);
 const PerturbationAngleLimit=pi*0.125;
       pi2=pi*2.0;
 var PerturbationIterations,PreturbIteration,ContactIndex:longint;
     Contact:PKraftContact;
     PenetrationDepth,RadiusA,RadiusB,PerturbationAngle,IterationAngle:TKraftScalar;
     Normal,pa,pb,tpa,tpb,tp,v0,v1,SeperateNormalWorldSpace:TKraftVector3;
     PerturbationA,OK:boolean;
     UnperturbatedTransform,PerturbatedTransform:TKraftMatrix4x4;
     ShapeTransformMatrices:array[0..1] of PKraftMatrix4x4;
     PerturbationRotationQuaternion0,PerturbationRotationQuaternion1,PerturbationRotationQuaternion2,UnpreturbedQuaternion:TKraftQuaternion;
 begin
  Manifold.Persistent:=false;

  PerturbationIterations:=ContactManager.fPhysics.fPerturbationIterations;
  if PerturbationIterations<MAX_CONTACTS then begin
   PerturbationIterations:=MAX_CONTACTS;
  end;

  Manifold.CountContacts:=0;

  if MPRPenetration(ShapeA,ShapeB,ShapeA.fWorldTransform,ShapeB.fWorldTransform,pa,pb,Normal,PenetrationDepth) then begin

   Manifold.LocalNormal:=Vector3TermMatrixMulTransposedBasis(Vector3Neg(Normal),Shapes[1].fWorldTransform);

   SeperateNormalWorldSpace:=Normal;
   GetPlaneSpace(SeperateNormalWorldSpace,v0,v1);
   if Vector3LengthSquared(v0)>EPSILON then begin
    RadiusA:=ShapeA.fAngularMotionDisc;
    RadiusB:=ShapeB.fAngularMotionDisc;
    if {$ifndef UseTriangleMeshFullPerturbation}assigned(MeshContactPair) or{$endif} (RadiusA<RadiusB) then begin
     PerturbationAngle:=Min(PerturbationAngleLimit,ContactManager.fPhysics.fContactBreakingThreshold/RadiusA);
     PerturbationA:=true;
     UnperturbatedTransform:=ShapeA.fWorldTransform;
    end else begin
     PerturbationAngle:=Min(PerturbationAngleLimit,ContactManager.fPhysics.fContactBreakingThreshold/RadiusB);
     PerturbationA:=false;
     UnperturbatedTransform:=ShapeB.fWorldTransform;
    end;
    UnpreturbedQuaternion:=QuaternionFromMatrix4x4(UnperturbatedTransform);
   end else begin
    Contact:=@Manifold.Contacts[0];
    Manifold.CountContacts:=1;
    Manifold.ContactManifoldType:=kcmtPersistentImplicit;
    Manifold.LocalRadius[0]:=0.0;
    Manifold.LocalRadius[1]:=0.0;
    tp:=Vector3Avg(pa,pb);
    Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(tp,Shapes[0].fWorldTransform);
    Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(tp,Shapes[1].fWorldTransform);
    Contact^.Penetration:=-PenetrationDepth;
    Contact^.FeatureID:=CreateFeatureID(-1);
    exit;
   end;

   ContactManager.fCountTemporaryContacts[ThreadIndex]:=0;
   for PreturbIteration:=0 to PerturbationIterations-1 do begin
    IterationAngle:=(PreturbIteration*pi2)/PerturbationIterations;
    PerturbationRotationQuaternion0:=QuaternionFromAxisAngle(v0,PerturbationAngle);
    PerturbationRotationQuaternion1:=QuaternionFromAxisAngle(SeperateNormalWorldSpace,IterationAngle);
    PerturbationRotationQuaternion2:=QuaternionMul(QuaternionMul(QuaternionInverse(PerturbationRotationQuaternion1),PerturbationRotationQuaternion0),PerturbationRotationQuaternion1);
    PerturbatedTransform:=QuaternionToMatrix4x4(QuaternionMul(UnpreturbedQuaternion,PerturbationRotationQuaternion2));
    if PerturbationA then begin
     PKraftVector3(pointer(@PerturbatedTransform[3,0]))^:=PKraftVector3(pointer(@ShapeA.fWorldTransform[3,0]))^;
     ShapeTransformMatrices[0]:=@PerturbatedTransform;
     ShapeTransformMatrices[1]:=@ShapeB.fWorldTransform;
    end else begin
     PKraftVector3(pointer(@PerturbatedTransform[3,0]))^:=PKraftVector3(pointer(@ShapeB.fWorldTransform[3,0]))^;
     ShapeTransformMatrices[0]:=@ShapeA.fWorldTransform;
     ShapeTransformMatrices[1]:=@PerturbatedTransform;
    end;
    if MPRPenetration(ShapeA,ShapeB,ShapeTransformMatrices[0]^,ShapeTransformMatrices[1]^,tpa,tpb,Normal,PenetrationDepth) then begin
     if PerturbationA then begin
      Vector3MatrixMul(tpa,Matrix4x4TermMul(Matrix4x4TermInverse(PerturbatedTransform),ShapeA.fWorldTransform));
     end else begin
      Vector3MatrixMul(tpb,Matrix4x4TermMul(Matrix4x4TermInverse(PerturbatedTransform),ShapeB.fWorldTransform));
     end;
     if ContactManager.fCountTemporaryContacts[ThreadIndex]<MAX_TEMPORARY_CONTACTS then begin
      OK:=true;
      tp:=Vector3Avg(tpa,tpb);
      v0:=Vector3TermMatrixMulInverted(tp,Shapes[0].fWorldTransform);
      for ContactIndex:=0 to ContactManager.fCountTemporaryContacts[ThreadIndex]-1 do begin
       Contact:=@ContactManager.fTemporaryContacts[ThreadIndex,ContactIndex];
       if Vector3Dist(Contact^.LocalPoints[0],v0)<ContactManager.fPhysics.fContactBreakingThreshold then begin
        OK:=false;
        break;
       end;
      end;
      if OK then begin
       Contact:=@ContactManager.fTemporaryContacts[ThreadIndex,ContactManager.fCountTemporaryContacts[ThreadIndex]];
       inc(ContactManager.fCountTemporaryContacts[ThreadIndex]);
       Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(tp,Shapes[0].fWorldTransform);
       Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(tp,Shapes[1].fWorldTransform);
       Contact^.Penetration:=-PenetrationDepth;
       Contact^.FeatureID:=CreateFeatureID(-1);
      end;
     end else begin
      break;
     end;
    end;
   end;

   if ContactManager.fCountTemporaryContacts[ThreadIndex]>0 then begin
    Manifold.CountContacts:=ContactManager.ReduceContacts(pointer(@ContactManager.fTemporaryContacts[ThreadIndex,0]),ContactManager.fCountTemporaryContacts[ThreadIndex],pointer(@Manifold.Contacts[0]));
    Manifold.ContactManifoldType:=kcmtPersistentImplicit;
    Manifold.LocalRadius[0]:=0.0;
    Manifold.LocalRadius[1]:=0.0;
   end else begin
    Manifold.CountContacts:=0;
   end;

   if Manifold.CountContacts=0 then begin
    Contact:=@Manifold.Contacts[0];
    Manifold.CountContacts:=1;
    Manifold.ContactManifoldType:=kcmtPersistentImplicit;
    Manifold.LocalRadius[0]:=0.0;
    Manifold.LocalRadius[1]:=0.0;
    tp:=Vector3Avg(pa,pb);
    Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(tp,Shapes[0].fWorldTransform);
    Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(tp,Shapes[1].fWorldTransform);
    Contact^.Penetration:=-PenetrationDepth;
    Contact^.FeatureID:=CreateFeatureID(-1);
   end;

  end;
 end;
 procedure ProcessPersistentContactManifold(ShapeA,ShapeB:TKraftShape);
  function FindReplacableContact(const NewContact:TKraftContact):longint;
  const Indices:array[0..3,0..2] of longint=((1,2,3),(0,2,3),(0,1,3),(0,1,2));
  var i,MaxPenetrationIndex:longint;
      MaxPenetration,Area,BiggestArea:single;
      Contact:PKraftContact;
  begin
   MaxPenetrationIndex:=-1;
   MaxPenetration:=NewContact.Penetration;
   for i:=0 to 3 do begin
    Contact:=@Manifold.Contacts[i];
    if MaxPenetration<Contact^.Penetration then begin
     MaxPenetrationIndex:=i;
     MaxPenetration:=Contact^.Penetration;
    end;
   end;
   result:=-1;
   BiggestArea:=CalculateAreaFromFourPoints(Manifold.Contacts[0].LocalPoints[0],
                                            Manifold.Contacts[1].LocalPoints[0],
                                            Manifold.Contacts[2].LocalPoints[0],
                                            Manifold.Contacts[3].LocalPoints[0]);
   for i:=0 to 3 do begin
    if MaxPenetrationIndex<>i then begin
     Area:=CalculateAreaFromFourPoints(NewContact.LocalPoints[0],
                                       Manifold.Contacts[Indices[i,0]].LocalPoints[0],
                                       Manifold.Contacts[Indices[i,1]].LocalPoints[0],
                                       Manifold.Contacts[Indices[i,2]].LocalPoints[0]);
     if BiggestArea<Area then begin
      BiggestArea:=Area;
      result:=i;
     end;
    end;
   end;
  end;
{$ifdef UseTriangleMeshFullPerturbation}
  procedure PreprocessTriangle;
  var Center:TKraftVector3;
  begin
   Center.x:=(ShapeTriangle.fConvexHull.fVertices[0].Position.x+ShapeTriangle.fConvexHull.fVertices[1].Position.x+ShapeTriangle.fConvexHull.fVertices[2].Position.x)/3.0;
   Center.y:=(ShapeTriangle.fConvexHull.fVertices[0].Position.y+ShapeTriangle.fConvexHull.fVertices[1].Position.y+ShapeTriangle.fConvexHull.fVertices[2].Position.y)/3.0;
   Center.z:=(ShapeTriangle.fConvexHull.fVertices[0].Position.z+ShapeTriangle.fConvexHull.fVertices[1].Position.z+ShapeTriangle.fConvexHull.fVertices[2].Position.z)/3.0;
   ShapeTriangle.fConvexHull.fVertices[0].Position:=Vector3Sub(ShapeTriangle.fConvexHull.fVertices[0].Position,Center);
   ShapeTriangle.fConvexHull.fVertices[1].Position:=Vector3Sub(ShapeTriangle.fConvexHull.fVertices[1].Position,Center);
   ShapeTriangle.fConvexHull.fVertices[2].Position:=Vector3Sub(ShapeTriangle.fConvexHull.fVertices[2].Position,Center);
   Center:=Vector3TermMatrixMulBasis(Center,ShapeTriangle.fWorldTransform);
   ShapeTriangle.fWorldTransform[3,0]:=ShapeTriangle.fWorldTransform[3,0]+Center.x;
   ShapeTriangle.fWorldTransform[3,1]:=ShapeTriangle.fWorldTransform[3,1]+Center.y;
   ShapeTriangle.fWorldTransform[3,2]:=ShapeTriangle.fWorldTransform[3,2]+Center.z;
   ShapeTriangle.UpdateData;
  end;
{$endif}
 var WorldPositions,LocalPoints:array[0..1] of TKraftVector3;
     Normal,Point:TKraftVector3;
     PenetrationDepth,ShortestDistance,Distance:TKraftScalar;
     Nearest,Index,SubIndex:longint;
     Contact:PKraftContact;
     NewContact:TKraftContact;
 begin
{$ifdef UseTriangleMeshFullPerturbation}
  if ShapeB=TriangleShape then begin
   PreprocessTriangle;
  end;
{$endif}
  if ((OldManifoldCountContacts=0) or ContactManager.fPhysics.fAlwaysPerturbating) and (ContactManager.fPhysics.fPerturbationIterations>0) then begin
   CollideWithMPRAndPerturbation(ShapeA,ShapeB);
  end else begin

   Manifold.CountContacts:=OldManifoldCountContacts;
   Manifold.Persistent:=true;

   if MPRPenetration(ShapeA,ShapeB,ShapeA.fWorldTransform,ShapeB.fWorldTransform,WorldPositions[0],WorldPositions[1],Normal,PenetrationDepth) then begin

    Point:=Vector3Avg(WorldPositions[0],WorldPositions[1]);
    WorldPositions[0]:=Point;
    WorldPositions[1]:=Point;{}

    Normal:=Vector3Neg(Normal);
    PenetrationDepth:=-PenetrationDepth;

    Manifold.LocalNormal:=Vector3TermMatrixMulTransposedBasis(Normal,Shapes[1].fWorldTransform);

//  writeln(PenetrationDepth:1:8,' ',Vector3Dot(Manifold.Normal,Vector3Sub(WorldPositions[1],WorldPositions[0])):1:8);

    // Find the nearest point to replace with it, if possible
    Nearest:=-1;
    LocalPoints[0]:=Vector3TermMatrixMulInverted(WorldPositions[0],Shapes[0].fWorldTransform);
    LocalPoints[1]:=Vector3TermMatrixMulInverted(WorldPositions[1],Shapes[1].fWorldTransform);
    if Manifold.CountContacts>0 then begin
     ShortestDistance:=ContactManager.fPhysics.fContactBreakingThreshold;
     for Index:=0 to Manifold.CountContacts-1 do begin
      Contact:=@Manifold.Contacts[Index];
      Distance:=Vector3Dist(LocalPoints[0],Contact^.LocalPoints[0]);
      if ShortestDistance>Distance then begin
       ShortestDistance:=Distance;
       Nearest:=Index;
      end;
     end;
    end;

    // Select contact data target
    if (Manifold.CountContacts=0) or (Nearest<0) then begin
     if Manifold.CountContacts=0 then begin
      // First contact
      Contact:=@Manifold.Contacts[0];
      Manifold.CountContacts:=1;
      Manifold.ContactManifoldType:=kcmtPersistentImplicit;
      Manifold.LocalRadius[0]:=0.0;
      Manifold.LocalRadius[1]:=0.0;
      Contact^.FeatureID:=CreateFeatureID(1);
     end else begin
      // Non-first contact without old nearest found contact
      Contact:=@NewContact;
     end;
     Contact^.NormalImpulse:=0.0;
     Contact^.TangentImpulse[0]:=0.0;
     Contact^.TangentImpulse[1]:=0.0;
     Contact^.WarmStartState:=0;
    end else begin
     // Non-first contact with old nearest found contact, for replace it with this new contact
     Contact:=@Manifold.Contacts[Nearest];
     Contact^.FeatureID:=CreateFeatureID(Nearest+1);
    end;

    if assigned(Contact) then begin
     // Generate contact
     Contact^.LocalPoints[0]:=LocalPoints[0];
     Contact^.LocalPoints[1]:=LocalPoints[1];
     Contact^.Penetration:=PenetrationDepth;
     if Contact=@NewContact then begin
      // Replace old nearest found contact with this new contact
      Index:=Manifold.CountContacts;
      if Index>=MAX_CONTACTS then begin
       Index:=FindReplacableContact(NewContact);
      end else begin
       inc(Manifold.CountContacts);
      end;
      if (Index>=0) and (Index<MAX_CONTACTS) then begin
       Manifold.Contacts[Index]:=NewContact;
       Manifold.Contacts[Index].FeatureID:=CreateFeatureID(Index+1);
      end;
     end;
    end;

   end;

   // Contacts becomes invalid when signed distance exceeds margin (projected on contact normal direction) or
   // when relative movement orthogonal to normal exceeds margin
   Index:=0;
   while Index<Manifold.CountContacts do begin
    Contact:=@Manifold.Contacts[Index];

    // First refresh world-space positions
    WorldPositions[0]:=Vector3TermMatrixMul(Contact^.LocalPoints[0],Shapes[0].fWorldTransform);
    WorldPositions[1]:=Vector3TermMatrixMul(Contact^.LocalPoints[1],Shapes[1].fWorldTransform);

    // Then check
    if (Vector3Dot(Normal,Vector3Sub(WorldPositions[0],WorldPositions[1]))>ContactManager.fPhysics.fContactBreakingThreshold) or
       (Vector3Dist(Vector3Add(WorldPositions[0],
                               Vector3ScalarMul(Normal,Vector3Dot(Normal,
                                                                  Vector3Sub(WorldPositions[1],
                                                                             WorldPositions[0])))),
                    WorldPositions[1])>ContactManager.fPhysics.fContactBreakingThreshold) then begin
     for SubIndex:=Index to Manifold.CountContacts-2 do begin
      Manifold.Contacts[SubIndex]:=Manifold.Contacts[SubIndex+1];
     end;
     dec(Manifold.CountContacts);
    end else begin
     inc(Index);                                 
    end;
   end;

  end;

  if Manifold.CountContacts>0 then begin
   Manifold.ContactManifoldType:=kcmtPersistentImplicit;
   Manifold.LocalRadius[0]:=0.0;
   Manifold.LocalRadius[1]:=0.0;
  end else begin
   Manifold.ContactManifoldType:=kcmtUnknown;
  end;
  
 end;
 procedure FindSpeculativeContacts(ShapeA,ShapeB:TKraftShape);
 var Index:longint;
     VelocityLength,t0,t1:TKraftScalar;
     Sweeps:array[0..1] of TKraftSweep;
     Transforms:array[0..1,0..1] of TKraftMatrix4x4;
     Contact:PKraftContact;
     Velocity:TKraftVector3;
     LinearVelocities:array[0..1] of TKraftVector3;
     GJK:TKraftGJK;
 begin
  if (Manifold.CountContacts=0) and SpeculativeContacts and (ContactManager.fPhysics.fContinuousMode=kcmSpeculativeContacts) and
     (assigned(Shapes[0].fRigidBody) and assigned(Shapes[1].fRigidBody)) and
     ((krbfContinuous in Shapes[0].fRigidBody.fFlags) or (krbfContinuous in Shapes[1].fRigidBody.fFlags)) then begin
   if not (((Shapes[0].fRigidBody.fRigidBodyType=krbtDYNAMIC) and (Shapes[1].fRigidBody.fRigidBodyType=krbtDYNAMIC)) and not
           (ContactManager.fPhysics.fContinuousAgainstDynamics and
            ((krbfContinuousAgainstDynamics in Shapes[0].fRigidBody.fFlags) or (krbfContinuousAgainstDynamics in Shapes[1].fRigidBody.fFlags)))) then begin
    for Index:=0 to 1 do begin
     Sweeps[Index]:=Shapes[Index].fRigidBody.fSweep;
     Sweeps[Index].c0:=Sweeps[Index].c;
     Sweeps[Index].q0:=Sweeps[Index].q;
     ContactManager.fPhysics.Integrate(Sweeps[Index].c,Sweeps[Index].q,Shapes[Index].fRigidBody.fLinearVelocity,Shapes[Index].fRigidBody.fAngularVelocity,DeltaTime);
     Sweeps[Index].Alpha0:=0.0;
     Transforms[Index,0]:=Matrix4x4TermMul(Shapes[Index].fLocalTransform,SweepTransform(Sweeps[Index],0.0));
     Transforms[Index,1]:=Matrix4x4TermMul(Shapes[Index].fLocalTransform,SweepTransform(Sweeps[Index],1.0));
     LinearVelocities[Index]:=Vector3Sub(Vector3TermMatrixMul(Vector3Origin,Transforms[Index,1]),Vector3TermMatrixMul(Vector3Origin,Transforms[Index,0]));
    end;
    Velocity:=Vector3Sub(LinearVelocities[0],LinearVelocities[1]);
    VelocityLength:=Vector3Length(Velocity);
    if VelocityLength>Max(1e-5,Min(ShapeA.fAngularMotionDisc,ShapeB.fAngularMotionDisc)*0.125) then begin
     if SweepSphereSphere(Vector3TermMatrixMul(ShapeA.fLocalCenterOfMass,Transforms[0,0]),
                          Vector3TermMatrixMul(ShapeA.fLocalCenterOfMass,Transforms[0,1]),
                          ShapeA.fShapeSphere.Radius,
                          Vector3TermMatrixMul(ShapeB.fLocalCenterOfMass,Transforms[1,0]),
                          Vector3TermMatrixMul(ShapeB.fLocalCenterOfMass,Transforms[1,1]),
                          ShapeB.fShapeSphere.Radius,
                          t0,
                          t1) then begin
      GJK.CachedSimplex:=@Manifold.GJKCachedSimplex;
      GJK.Simplex.Count:=0;
      GJK.Shapes[0]:=ShapeA;
      GJK.Shapes[1]:=ShapeB;
      GJK.Transforms[0]:=@ShapeA.fWorldTransform;
      GJK.Transforms[1]:=@ShapeB.fWorldTransform;
      GJK.UseRadii:=false;
      GJK.Run;
      if (GJK.Distance>0.0) and (GJK.Distance<(VelocityLength+EPSILON)) and not GJK.Failed then begin
       if MPRSweep(ShapeA,ShapeB,Sweeps[0],Sweeps[1]) then begin
        Manifold.ContactManifoldType:=kcmtSpeculative;
        Manifold.CountContacts:=1;
        Manifold.LocalNormal:=Vector3SafeNorm(Vector3TermMatrixMulTransposedBasis(Vector3Neg(GJK.Normal),Shapes[1].fWorldTransform));
        Contact:=@Manifold.Contacts[0];
        Contact^.LocalPoints[0]:=Vector3TermMatrixMulInverted(GJK.ClosestPoints[0],Shapes[0].fWorldTransform);
        Contact^.LocalPoints[1]:=Vector3TermMatrixMulInverted(GJK.ClosestPoints[1],Shapes[1].fWorldTransform);
        Contact^.Penetration:=GJK.Distance;
        Contact^.FeatureID:=CreateFeatureID(-1);
       end;
      end;
     end;
    end;
   end;
  end;
 end;
var Index,SubIndex:longint;
    ShapeA,ShapeB:TKraftShape;
    MeshShape:TKraftShapeMesh;
    HasContact:boolean;
    MeshTriangle:PKraftMeshTriangle;
    Contact,BestOldContact,OldContact:PKraftContact;
    BestContactDistance,ContactDistance:TKraftScalar;
    OldManifoldContacts:array[0..MAX_CONTACTS-1] of TKraftContact;
begin

 Flags:=Flags+[kcfEnabled];

 OldManifoldCountContacts:=Manifold.CountContacts;
 OldContactManifoldType:=Manifold.ContactManifoldType;
 for Index:=0 to OldManifoldCountContacts-1 do begin
  OldManifoldContacts[Index]:=Manifold.Contacts[Index];
 end;

 Manifold.ContactManifoldType:=kcmtUnknown;

 Manifold.CountContacts:=0;

 ShapeA:=Shapes[0];
 ShapeB:=Shapes[1];

 if assigned(MeshContactPair) then begin
  if (ElementIndex>=0) and assigned(TriangleShape) and (ShapeB is TKraftShapeMesh) then begin
   MeshShape:=TKraftShapeMesh(ShapeB);
   ShapeB:=TriangleShape;
   ShapeTriangle:=TKraftShapeTriangle(TriangleShape);
   MeshTriangle:=@MeshShape.fMesh.fTriangles[ElementIndex];
   ShapeTriangle.fWorldTransform:=MeshShape.fWorldTransform;
   ShapeTriangle.fConvexHull.fVertices[0].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[0]];
   ShapeTriangle.fConvexHull.fVertices[1].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[1]];
   ShapeTriangle.fConvexHull.fVertices[2].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[2]];
   ShapeTriangle.UpdateData;
  end else begin
   exit;
  end;
 end;

 HasContact:=false;

 if (Shapes[0]<>Shapes[1]) and
    (Shapes[0].fRigidBody<>Shapes[1].fRigidBody) and
    (ksfCollision in Shapes[0].fFlags) and
    (ksfCollision in Shapes[1].fFlags) then begin

  if ContactManager.fPhysics.fPersistentContactManifold then begin

   // Incremental persistent contact manifold
   ProcessPersistentContactManifold(ShapeA,ShapeB);

  end else begin

   // Full one-shot contact manifold (default)

   case ShapeA.fShapeType of
    kstSphere:begin
     case ShapeB.fShapeType of
      kstSphere:begin
       CollideSphereWithSphere(TKraftShapeSphere(ShapeA),TKraftShapeSphere(ShapeB));
      end;
      kstCapsule:begin
       CollideSphereWithCapsule(TKraftShapeSphere(ShapeA),TKraftShapeCapsule(ShapeB));
      end;
      kstConvexHull:begin
       CollideSphereWithConvexHull(TKraftShapeSphere(ShapeA),TKraftShapeConvexHull(ShapeB));
      end;
      kstBox:begin
       CollideSphereWithBox(TKraftShapeSphere(ShapeA),TKraftShapeBox(ShapeB));
      end;
      kstPlane:begin
       CollideSphereWithPlane(TKraftShapeSphere(ShapeA),TKraftShapePlane(ShapeB));
      end;
      kstTriangle:begin
       CollideSphereWithTriangle(TKraftShapeSphere(ShapeA),TKraftShapeTriangle(ShapeB));
      end;
     end;
    end;
    kstCapsule:begin
     case ShapeB.fShapeType of
      kstCapsule:begin
       CollideCapsuleWithCapsule(TKraftShapeCapsule(ShapeA),TKraftShapeCapsule(ShapeB));
      end;
      kstConvexHull,kstBox,kstPlane,kstTriangle:begin
       CollideCapsuleWithConvexHull(TKraftShapeCapsule(ShapeA),TKraftShapeConvexHull(ShapeB));
      end;{}
 {    kstConvexHull,kstBox,kstPlane:begin
       CollideCapsuleWithConvexHull(TKraftShapeCapsule(ShapeA),TKraftShapeConvexHull(ShapeB));
      end;
      kstTriangle:begin
       CollideCapsuleWithTriangle(TKraftShapeCapsule(ShapeA),TKraftShapeTriangle(ShapeB));
      end;{}
     end;
    end;
    kstConvexHull,kstBox,kstPlane,kstTriangle:begin
     case ShapeB.fShapeType of
      kstConvexHull,kstBox,kstPlane,kstTriangle:begin
       CollideConvexHullWithConvexHull(TKraftShapeConvexHull(ShapeA),TKraftShapeConvexHull(ShapeB));
      end;
     end;
    end;
   end;

  end;

  if SpeculativeContacts then begin
   FindSpeculativeContacts(ShapeA,ShapeB);
  end;

  if ((ksfSensor in Shapes[0].fFlags) or (ksfSensor in Shapes[1].fFlags)) or
     ((krbfSensor in RigidBodies[0].fFlags) or (krbfSensor in RigidBodies[1].fFlags)) then begin

   HasContact:=Manifold.CountContacts>0;

   Manifold.ContactManifoldType:=kcmtUnknown;
   Manifold.CountContacts:=0;
    
  end else begin

   HasContact:=Manifold.CountContacts>0;

   if Manifold.Persistent then begin

    for Index:=0 to Manifold.CountContacts-1 do begin
     Contact:=@Manifold.Contacts[Index];
     Contact^.WarmStartState:=Max(Contact^.WarmStartState,Contact^.WarmStartState+1);
    end;

   end else begin

    for Index:=0 to Manifold.CountContacts-1 do begin
     Contact:=@Manifold.Contacts[Index];
     BestOldContact:=nil;
     if Contact^.FeatureID.Key=-1 then begin
      BestContactDistance:=ContactManager.fPhysics.fContactBreakingThreshold;
      for SubIndex:=0 to OldManifoldCountContacts-1 do begin
       OldContact:=@OldManifoldContacts[SubIndex];
       ContactDistance:=Vector3Dist(Contact^.LocalPoints[0],OldContact^.LocalPoints[0]);
       if (OldContact^.FeatureID.Key=-1) and (BestContactDistance>ContactDistance) then begin
        BestContactDistance:=ContactDistance;
        BestOldContact:=OldContact;
       end;
      end;
     end else begin
      for SubIndex:=0 to OldManifoldCountContacts-1 do begin
       OldContact:=@OldManifoldContacts[SubIndex];
       if Contact^.FeatureID.Key=OldContact^.FeatureID.Key then begin
        BestOldContact:=OldContact;
        break;
       end;
      end;
     end;
     if assigned(BestOldContact) then begin
      Contact^.NormalImpulse:=BestOldContact^.NormalImpulse;
      Contact^.TangentImpulse[0]:=BestOldContact^.TangentImpulse[0];
      Contact^.TangentImpulse[1]:=BestOldContact^.TangentImpulse[1];
      Contact^.WarmStartState:=Max(BestOldContact^.WarmStartState,BestOldContact^.WarmStartState+1);
     end else begin
      Contact^.NormalImpulse:=0.0;
      Contact^.TangentImpulse[0]:=0.0;
      Contact^.TangentImpulse[1]:=0.0;
      Contact^.WarmStartState:=0;
     end;
    end;
   end;

  end; 

 end;

 if HasContact then begin
  if kcfColliding in Flags then begin
   Include(Flags,kcfWasColliding);
  end else begin
   Include(Flags,kcfColliding);
  end;
 end else begin
  if kcfColliding in Flags then begin
   Flags:=(Flags-[kcfColliding])+[kcfWasColliding];
  end else begin
   Exclude(Flags,kcfWasColliding);
  end;
 end;

{if ShapeB.fShapeType=kstPlane then begin
  writeln(ContactManager.fPhysics.HighResolutionTimer.GetTime:16,' ',Manifold.CountContacts:4);
 end;{}

end;

constructor TKraftMeshContactPair.Create(const AContactManager:TKraftContactManager);
begin
 inherited Create;

 fContactManager:=AContactManager;

 if assigned(fContactManager.fMeshContactPairLast) then begin
  fContactManager.fMeshContactPairLast.fNext:=self;
  fPrevious:=fContactManager.fMeshContactPairLast;
 end else begin
  fContactManager.fMeshContactPairFirst:=self;
  fPrevious:=nil;
 end;
 fContactManager.fMeshContactPairLast:=self;
 fNext:=nil;

 fHashBucket:=-1;
 fHashPrevious:=nil;
 fHashNext:=nil;

 fIsOnFreeList:=false;

 fFlags:=[];

 inc(fContactManager.fCountMeshContactPairs);

 fShapeConvex:=nil;
 fShapeMesh:=nil;

 fRigidBodyConvex:=nil;
 fRigidBodyMesh:=nil;

end;

destructor TKraftMeshContactPair.Destroy;
begin

 RemoveFromHashTable;

 if fIsOnFreeList then begin
  if assigned(fPrevious) then begin
   fPrevious.fNext:=fNext;
  end else if fContactManager.fMeshContactPairFirstFree=self then begin
   fContactManager.fMeshContactPairFirstFree:=fNext;
  end;
  if assigned(fNext) then begin
   fNext.fPrevious:=fPrevious;
  end else if fContactManager.fMeshContactPairLastFree=self then begin
   fContactManager.fMeshContactPairLastFree:=fPrevious;
  end;
  fPrevious:=nil;
  fNext:=nil;
 end else begin
  if assigned(fPrevious) then begin
   fPrevious.fNext:=fNext;
  end else if fContactManager.fMeshContactPairFirst=self then begin
   fContactManager.fMeshContactPairFirst:=fNext;
  end;
  if assigned(fNext) then begin
   fNext.fPrevious:=fPrevious;
  end else if fContactManager.fMeshContactPairLast=self then begin
   fContactManager.fMeshContactPairLast:=fPrevious;
  end;
  fPrevious:=nil;
  fNext:=nil;
 end;

 dec(fContactManager.fCountMeshContactPairs);

 inherited Destroy;
end;

procedure TKraftMeshContactPair.MoveToFreeList;
begin
 if not fIsOnFreeList then begin

  fIsOnFreeList:=true;

  if assigned(fPrevious) then begin
   fPrevious.fNext:=fNext;
  end else if fContactManager.fMeshContactPairFirst=self then begin
   fContactManager.fMeshContactPairFirst:=fNext;
  end;
  if assigned(fNext) then begin
   fNext.fPrevious:=fPrevious;
  end else if fContactManager.fMeshContactPairLast=self then begin
   fContactManager.fMeshContactPairLast:=fPrevious;
  end;

  if assigned(fContactManager.fMeshContactPairLastFree) then begin
   fContactManager.fMeshContactPairLastFree.fNext:=self;
   fPrevious:=fContactManager.fMeshContactPairLastFree;
  end else begin
   fContactManager.fMeshContactPairFirstFree:=self;
   fPrevious:=nil;
  end;
  fContactManager.fMeshContactPairLastFree:=self;
  fNext:=nil;

 end;
end;

procedure TKraftMeshContactPair.MoveFromFreeList;
begin
 if fIsOnFreeList then begin

  fIsOnFreeList:=false;

  fFlags:=[];

  if assigned(fPrevious) then begin
   fPrevious.fNext:=fNext;
  end else if fContactManager.fMeshContactPairFirstFree=self then begin
   fContactManager.fMeshContactPairFirstFree:=fNext;
  end;
  if assigned(fNext) then begin
   fNext.fPrevious:=fPrevious;
  end else if fContactManager.fMeshContactPairLastFree=self then begin
   fContactManager.fMeshContactPairLastFree:=fPrevious;
  end;

  if assigned(fContactManager.fMeshContactPairLast) then begin
   fContactManager.fMeshContactPairLast.fNext:=self;
   fPrevious:=fContactManager.fMeshContactPairLast;
  end else begin
   fContactManager.fMeshContactPairFirst:=self;
   fPrevious:=nil;
  end;
  fContactManager.fMeshContactPairLast:=self;
  fNext:=nil;

 end;
end;

procedure TKraftMeshContactPair.AddToHashTable; {$ifdef caninline}inline;{$endif}
var HashTableBucket:PKraftMeshContactPairHashTableBucket;
begin
 if fHashBucket<0 then begin
  fHashBucket:=HashTwoPointers(fShapeConvex,fShapeMesh) and high(TKraftMeshContactPairHashTable);
  HashTableBucket:=@fContactManager.fMeshContactPairHashTable[fHashBucket];
  if assigned(HashTableBucket^.First) then begin
   HashTableBucket^.First.fHashPrevious:=self;
   fHashNext:=HashTableBucket^.First;
  end else begin
   HashTableBucket^.Last:=self;
   fHashNext:=nil;
  end;
  HashTableBucket^.First:=self;
  fHashPrevious:=nil;
 end;
end;

procedure TKraftMeshContactPair.RemoveFromHashTable; {$ifdef caninline}inline;{$endif}
var HashTableBucket:PKraftMeshContactPairHashTableBucket;
begin
 if fHashBucket>=0 then begin
  HashTableBucket:=@fContactManager.fMeshContactPairHashTable[fHashBucket];
  fHashBucket:=-1;
  if assigned(fHashPrevious) then begin
   fHashPrevious.fHashNext:=fHashNext;
  end else if HashTableBucket^.First=self then begin
   HashTableBucket^.First:=fHashNext;
  end;
  if assigned(fHashNext) then begin
   fHashNext.fHashPrevious:=fHashPrevious;
  end else if HashTableBucket^.Last=self then begin
   HashTableBucket^.Last:=fHashPrevious;
  end;
  fHashPrevious:=nil;
  fHashNext:=nil;
 end;
end;

procedure TKraftMeshContactPair.Query;
var SkipListNodeIndex,TriangleIndex:longint;
    SkipListNode:PKraftMeshSkipListNode;
    Triangle:PKraftMeshTriangle;
begin
 SkipListNodeIndex:=0;
 while SkipListNodeIndex<TKraftShapeMesh(fShapeMesh).fMesh.fCountSkipListNodes do begin
  SkipListNode:=@TKraftShapeMesh(fShapeMesh).fMesh.fSkipListNodes[SkipListNodeIndex];
  if AABBIntersect(SkipListNode^.AABB,fConvexAABBInMeshLocalSpace) then begin
   TriangleIndex:=SkipListNode^.TriangleIndex;
   while TriangleIndex>=0 do begin
    Triangle:=@TKraftShapeMesh(fShapeMesh).fMesh.fTriangles[TriangleIndex];
    if AABBIntersect(Triangle^.AABB,fConvexAABBInMeshLocalSpace) and not fContactManager.HasDuplicateContact(fRigidBodyConvex,fRigidBodyMesh,fShapeConvex,fShapeMesh,TriangleIndex) then begin
     fContactManager.AddConvexContact(fRigidBodyConvex,fRigidBodyMesh,fShapeConvex,fShapeMesh,TriangleIndex,self);
    end;
    TriangleIndex:=Triangle^.Next;
   end;
   inc(SkipListNodeIndex);
  end else begin
   SkipListNodeIndex:=SkipListNode^.SkipToNodeIndex;
  end;
 end;
end;

procedure TKraftMeshContactPair.Update;
var NewConvexAABBInMeshLocalSpace:TKraftAABB;
    Transform:TKraftMatrix4x4;
    Displacement,BoundsExpansion:TKraftVector3;
begin
 Transform:=Matrix4x4TermMulSimpleInverted(fShapeConvex.fWorldTransform,fShapeMesh.fWorldTransform);
 NewConvexAABBInMeshLocalSpace:=AABBTransform(fShapeConvex.fShapeAABB,Transform);
 if not AABBContains(fConvexAABBInMeshLocalSpace,NewConvexAABBInMeshLocalSpace) then begin
  Displacement:=Vector3TermMatrixMulBasis(Vector3ScalarMul(fRigidBodyConvex.fLinearVelocity,fRigidBodyConvex.fPhysics.fWorldDeltaTime),Transform);
  BoundsExpansion:=Vector3ScalarMul(Vector3(fShapeConvex.fAngularMotionDisc,fShapeConvex.fAngularMotionDisc,fShapeConvex.fAngularMotionDisc),Vector3Length(fRigidBodyConvex.fAngularVelocity)*fRigidBodyConvex.fPhysics.fWorldDeltaTime);
  fConvexAABBInMeshLocalSpace:=AABBStretch(NewConvexAABBInMeshLocalSpace,Displacement,BoundsExpansion);
  Query;
 end;
end;

constructor TKraftContactManager.Create(const APhysics:TKraft);
var ThreadIndex:longint;
begin
 inherited Create;

 fPhysics:=APhysics;

 fContactPairFirst:=nil;
 fContactPairLast:=nil;

 fFreeContactPairs:=nil;

 fCountContactPairs:=0;

 fMeshContactPairFirst:=nil;
 fMeshContactPairLast:=nil;

 fMeshContactPairFirstFree:=nil;
 fMeshContactPairLastFree:=nil;

 fCountMeshContactPairs:=0;

 fOnContactBegin:=nil;
 fOnContactEnd:=nil;
 fOnContactStay:=nil;

 fOnCanCollide:=nil;

 for ThreadIndex:=0 to MAX_THREADS-1 do begin
  fClipVertexLists[ThreadIndex,0]:=TKraftClipVertexList.Create;
  fClipVertexLists[ThreadIndex,1]:=TKraftClipVertexList.Create;
 end;

{$ifdef DebugDraw}
 fCountDebugClipVertexLists:=0;
 for ThreadIndex:=0 to high(fDebugClipVertexLists) do begin
  fDebugClipVertexLists[ThreadIndex]:=TKraftClipVertexList.Create;
 end;
{$endif}

 fActiveContactPairs:=nil;
 SetLength(fActiveContactPairs,256);
 fCountActiveContactPairs:=0;

 FillChar(fConvexConvexContactPairHashTable,SizeOf(TKraftContactPairHashTable),AnsiChar(#0));

 FillChar(fConvexMeshTriangleContactPairHashTable,SizeOf(TKraftContactPairHashTable),AnsiChar(#0));

 FillChar(fMeshContactPairHashTable,SizeOf(TKraftMeshContactPairHashTable),AnsiChar(#0));
 
end;

destructor TKraftContactManager.Destroy;
var ThreadIndex:longint;
    NextContactPair:PKraftContactPair;
begin

 SetLength(fActiveContactPairs,0);

 while assigned(fContactPairFirst) do begin
  RemoveContact(fContactPairFirst);
 end;

 while assigned(fFreeContactPairs) do begin
  NextContactPair:=fFreeContactPairs^.Next;
  FreeMem(fFreeContactPairs);
  fFreeContactPairs:=NextContactPair;
 end;

 while assigned(fMeshContactPairFirst) do begin
  fMeshContactPairFirst.Free;
 end;

 while assigned(fMeshContactPairFirstFree) do begin
  fMeshContactPairFirstFree.Free;
 end;

//Assert(CountContactPairs=0);
//Assert(fCountMeshContactPairs=0);

 for ThreadIndex:=0 to MAX_THREADS-1 do begin
  FreeAndNil(fClipVertexLists[ThreadIndex,0]);
  FreeAndNil(fClipVertexLists[ThreadIndex,1]);
 end;

{$ifdef DebugDraw}
 for ThreadIndex:=0 to high(fDebugClipVertexLists) do begin
  fDebugClipVertexLists[ThreadIndex].Free;
 end;
{$endif}

 inherited Destroy;
end;

function TKraftContactManager.HasDuplicateContact(const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AShapeA,AShapeB:TKraftShape;const AElementIndex:longint=-1):boolean;
var HashTableBucket:PKraftContactPairHashTableBucket;
    ContactPair:PKraftContactPair;
begin
 result:=false;
 if AElementIndex<0 then begin
  if ptruint(AShapeA)<ptruint(AShapeB) then begin
   HashTableBucket:=@fConvexConvexContactPairHashTable[HashTwoPointers(AShapeA,AShapeB) and high(TKraftContactPairHashTable)];
  end else begin
   HashTableBucket:=@fConvexConvexContactPairHashTable[HashTwoPointers(AShapeB,AShapeA) and high(TKraftContactPairHashTable)];
  end;
  ContactPair:=HashTableBucket^.First;
  while assigned(ContactPair) do begin
   if ((ContactPair^.Shapes[0]=AShapeA) and (ContactPair^.Shapes[1]=AShapeB)) or
      ((ContactPair^.Shapes[0]=AShapeB) and (ContactPair^.Shapes[1]=AShapeA)) then begin
    result:=true;
    exit;
   end;
   ContactPair:=ContactPair^.Next;
  end;
 end else begin
  if ptruint(AShapeA)<ptruint(AShapeB) then begin
   HashTableBucket:=@fConvexMeshTriangleContactPairHashTable[HashTwoPointersAndOneLongword(AShapeA,AShapeB,AElementIndex) and high(TKraftContactPairHashTable)];
  end else begin
   HashTableBucket:=@fConvexMeshTriangleContactPairHashTable[HashTwoPointersAndOneLongword(AShapeB,AShapeA,AElementIndex) and high(TKraftContactPairHashTable)];
  end;
  ContactPair:=HashTableBucket^.First;
  while assigned(ContactPair) do begin
   if (ContactPair^.ElementIndex=AElementIndex) and
      (((ContactPair^.Shapes[0]=AShapeA) and (ContactPair^.Shapes[1]=AShapeB)) or
       ((ContactPair^.Shapes[0]=AShapeB) and (ContactPair^.Shapes[1]=AShapeA))) then begin
    result:=true;
    exit;
   end;
   ContactPair:=ContactPair^.Next;
  end;
 end;
end;            

procedure TKraftContactManager.AddConvexContact(const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AShapeA,AShapeB:TKraftShape;const AElementIndex:longint=-1;const AMeshContactPair:TKraftMeshContactPair=nil);
var i:longint;
    ContactPair:PKraftContactPair;
    HashTableBucket:PKraftContactPairHashTableBucket;
begin

 if assigned(fFreeContactPairs) then begin
  ContactPair:=fFreeContactPairs;
  fFreeContactPairs:=ContactPair^.Next;
 end else begin
  GetMem(ContactPair,SizeOf(TKraftContactPair));
 end;
 FillChar(ContactPair^,SizeOf(TKraftContactPair),AnsiChar(#0));

 ContactPair^.Manifold.HaveData:=false;

 ContactPair^.Manifold.Persistent:=false;

 ContactPair^.Manifold.GJKCachedSimplex.Count:=0;

 ContactPair^.Island:=nil;

 ContactPair^.Shapes[0]:=AShapeA;
 ContactPair^.Shapes[1]:=AShapeB;

 ContactPair^.ElementIndex:=AElementIndex;

 if AElementIndex<0 then begin
  if ptruint(AShapeA)<ptruint(AShapeB) then begin
   ContactPair^.HashBucket:=HashTwoPointers(AShapeA,AShapeB) and high(TKraftContactPairHashTable);
  end else begin
   ContactPair^.HashBucket:=HashTwoPointers(AShapeB,AShapeA) and high(TKraftContactPairHashTable);
  end;
  HashTableBucket:=@fConvexConvexContactPairHashTable[ContactPair^.HashBucket];
 end else begin
  if ptruint(AShapeA)<ptruint(AShapeB) then begin
   ContactPair^.HashBucket:=HashTwoPointersAndOneLongword(AShapeA,AShapeB,AElementIndex) and high(TKraftContactPairHashTable);
  end else begin
   ContactPair^.HashBucket:=HashTwoPointersAndOneLongword(AShapeB,AShapeA,AElementIndex) and high(TKraftContactPairHashTable);
  end;
  HashTableBucket:=@fConvexMeshTriangleContactPairHashTable[ContactPair^.HashBucket];
 end;
 if assigned(HashTableBucket^.First) then begin
  HashTableBucket^.First.HashPrevious:=ContactPair;
  ContactPair^.HashNext:=HashTableBucket^.First;
 end else begin
  HashTableBucket^.Last:=ContactPair;
  ContactPair^.HashNext:=nil;
 end;
 HashTableBucket^.First:=ContactPair;
 ContactPair^.HashPrevious:=nil;

 ContactPair^.MeshContactPair:=AMeshContactPair;

 ContactPair^.RigidBodies[0]:=ARigidBodyA;
 ContactPair^.RigidBodies[1]:=ARigidBodyB;

 ContactPair^.Manifold.CountContacts:=0;

 for i:=low(ContactPair^.Manifold.Contacts) to high(ContactPair^.Manifold.Contacts) do begin
  ContactPair^.Manifold.Contacts[i].WarmStartState:=0;
 end;

 ContactPair^.Flags:=[kcfEnabled];

 ContactPair^.Friction:=sqrt(AShapeA.fFriction*AShapeB.fFriction);

 ContactPair^.Restitution:=max(AShapeA.fRestitution,AShapeB.fRestitution);

 if assigned(fContactPairLast) then begin
  ContactPair^.Previous:=fContactPairLast;
  fContactPairLast^.Next:=ContactPair;
 end else begin
  ContactPair^.Previous:=nil;
  fContactPairFirst:=ContactPair;
 end;
 ContactPair^.Next:=nil;
 fContactPairLast:=ContactPair;

 ContactPair^.Edges[0].OtherRigidBody:=ARigidBodyB;
 ContactPair^.Edges[0].ContactPair:=ContactPair;

 if assigned(ARigidBodyA.fContactPairEdgeLast) then begin
  ContactPair^.Edges[0].Previous:=ARigidBodyA.fContactPairEdgeLast;
  ARigidBodyA.fContactPairEdgeLast^.Next:=@ContactPair^.Edges[0];
 end else begin
  ContactPair^.Edges[0].Previous:=nil;
  ARigidBodyA.fContactPairEdgeFirst:=@ContactPair^.Edges[0];
 end;
 ContactPair^.Edges[0].Next:=nil;
 ARigidBodyA.fContactPairEdgeLast:=@ContactPair^.Edges[0];

 ContactPair^.Edges[1].OtherRigidBody:=ARigidBodyA;
 ContactPair^.Edges[1].ContactPair:=ContactPair;

 if assigned(ARigidBodyB.fContactPairEdgeLast) then begin
  ContactPair^.Edges[1].Previous:=ARigidBodyB.fContactPairEdgeLast;
  ARigidBodyB.fContactPairEdgeLast^.Next:=@ContactPair^.Edges[1];
 end else begin
  ContactPair^.Edges[1].Previous:=nil;
  ARigidBodyB.fContactPairEdgeFirst:=@ContactPair^.Edges[1];                                  
 end;
 ContactPair^.Edges[1].Next:=nil;
 ARigidBodyB.fContactPairEdgeLast:=@ContactPair^.Edges[1];

 if not ((ksfSensor in AShapeA.Flags) or (ksfSensor in AShapeB.Flags)) then begin
  ARigidBodyB.SetToAwake;
  ARigidBodyA.SetToAwake;
 end;

 inc(fCountContactPairs);

end;

procedure TKraftContactManager.AddMeshContact(const ARigidBodyConvex,ARigidBodyMesh:TKraftRigidBody;const AShapeConvex,AShapeMesh:TKraftShape);
var HashTableBucket:PKraftMeshContactPairHashTableBucket;
    MeshContactPair:TKraftMeshContactPair;
    Transform:TKraftMatrix4x4;
    Displacement,BoundsExpansion:TKraftVector3;
begin

 HashTableBucket:=@fMeshContactPairHashTable[HashTwoPointers(AShapeConvex,AShapeMesh) and high(TKraftMeshContactPairHashTable)];
 MeshContactPair:=HashTableBucket.First;
 while assigned(MeshContactPair) do begin
  if (MeshContactPair.fShapeConvex=AShapeConvex) and (MeshContactPair.fShapeMesh=AShapeMesh) then begin
   exit;
  end;
  MeshContactPair:=MeshContactPair.fHashNext;
 end;

 if assigned(fMeshContactPairFirstFree) then begin
  MeshContactPair:=fMeshContactPairFirstFree;
  MeshContactPair.MoveFromFreeList;
 end else begin
  MeshContactPair:=TKraftMeshContactPair.Create(self);
 end;

 MeshContactPair.fRigidBodyConvex:=ARigidBodyConvex;
 MeshContactPair.fRigidBodyMesh:=ARigidBodyMesh;

 MeshContactPair.fShapeConvex:=AShapeConvex;
 MeshContactPair.fShapeMesh:=AShapeMesh;

 MeshContactPair.AddToHashTable;

 Transform:=Matrix4x4TermMulSimpleInverted(MeshContactPair.fShapeConvex.fWorldTransform,MeshContactPair.fShapeMesh.fWorldTransform);
 Displacement:=Vector3TermMatrixMulBasis(Vector3ScalarMul(MeshContactPair.fRigidBodyConvex.fLinearVelocity,MeshContactPair.fRigidBodyConvex.fPhysics.fWorldDeltaTime),Transform);
 BoundsExpansion:=Vector3ScalarMul(Vector3(MeshContactPair.fShapeConvex.fAngularMotionDisc,MeshContactPair.fShapeConvex.fAngularMotionDisc,MeshContactPair.fShapeConvex.fAngularMotionDisc),Vector3Length(MeshContactPair.fRigidBodyConvex.fAngularVelocity)*MeshContactPair.fRigidBodyConvex.fPhysics.fWorldDeltaTime);
 MeshContactPair.fConvexAABBInMeshLocalSpace:=AABBStretch(AABBTransform(MeshContactPair.fShapeConvex.fShapeAABB,Transform),Displacement,BoundsExpansion);

 MeshContactPair.Query;

end;

procedure TKraftContactManager.AddContact(const AShapeA,AShapeB:TKraftShape);
var RigidBodyA,RigidBodyB:TKraftRigidBody;
begin

 RigidBodyA:=AShapeA.fRigidBody;
 RigidBodyB:=AShapeB.fRigidBody;

 if (not RigidBodyA.CanCollideWith(RigidBodyB)) or
    (assigned(fOnCanCollide) and not fOnCanCollide(AShapeA,AShapeB)) then begin
  exit;
 end;

 if (ksfCollision in AShapeA.fFlags) and
    (ksfCollision in AShapeB.fFlags) then begin

  if AShapeA.fIsMesh then begin

   AddMeshContact(RigidBodyB,RigidBodyA,AShapeB,AShapeA);

  end else if AShapeB.fIsMesh then begin

   AddMeshContact(RigidBodyA,RigidBodyB,AShapeA,AShapeB);

  end else begin

   if not HasDuplicateContact(RigidBodyA,RigidBodyB,AShapeA,AShapeB,-1) then begin
    AddConvexContact(RigidBodyA,RigidBodyB,AShapeA,AShapeB,-1);
   end;

  end;

 end;

end;

procedure TKraftContactManager.RemoveContact(AContactPair:PKraftContactPair);
var RigidBodyA,RigidBodyB:TKraftRigidBody;
    HashTableBucket:PKraftContactPairHashTableBucket;
begin

 if AContactPair^.ElementIndex<0 then begin
  HashTableBucket:=@fConvexConvexContactPairHashTable[AContactPair^.HashBucket];
 end else begin
  HashTableBucket:=@fConvexMeshTriangleContactPairHashTable[AContactPair^.HashBucket];
 end;
 AContactPair^.HashBucket:=-1;
 if assigned(AContactPair^.HashPrevious) then begin
  AContactPair^.HashPrevious^.HashNext:=AContactPair^.HashNext;
 end else if HashTableBucket^.First=AContactPair then begin
  HashTableBucket^.First:=AContactPair^.HashNext;
 end;
 if assigned(AContactPair^.HashNext) then begin
  AContactPair^.HashNext^.HashPrevious:=AContactPair^.HashPrevious;
 end else if HashTableBucket^.Last=AContactPair then begin
  HashTableBucket^.Last:=AContactPair^.HashPrevious;
 end;
 AContactPair^.HashPrevious:=nil;
 AContactPair^.HashNext:=nil;

 RigidBodyA:=AContactPair.Shapes[0].fRigidBody;
 RigidBodyB:=AContactPair.Shapes[1].fRigidBody;

 if assigned(AContactPair^.Edges[0].Previous) then begin
  AContactPair^.Edges[0].Previous^.Next:=AContactPair^.Edges[0].Next;
 end else if RigidBodyA.fContactPairEdgeFirst=@AContactPair^.Edges[0] then begin
  RigidBodyA.fContactPairEdgeFirst:=AContactPair^.Edges[0].Next;
 end;
 if assigned(AContactPair^.Edges[0].Next) then begin
  AContactPair^.Edges[0].Next^.Previous:=AContactPair^.Edges[0].Previous;
 end else if RigidBodyA.fContactPairEdgeLast=@AContactPair^.Edges[0] then begin
  RigidBodyA.fContactPairEdgeLast:=AContactPair^.Edges[0].Previous;
 end;
 AContactPair^.Edges[0].Previous:=nil;
 AContactPair^.Edges[0].Next:=nil;

 if assigned(AContactPair^.Edges[1].Previous) then begin
  AContactPair^.Edges[1].Previous^.Next:=AContactPair^.Edges[1].Next;
 end else if RigidBodyB.fContactPairEdgeFirst=@AContactPair^.Edges[1] then begin
  RigidBodyB.fContactPairEdgeFirst:=AContactPair^.Edges[1].Next;
 end;
 if assigned(AContactPair^.Edges[1].Next) then begin
  AContactPair^.Edges[1].Next^.Previous:=AContactPair^.Edges[1].Previous;
 end else if RigidBodyB.fContactPairEdgeLast=@AContactPair^.Edges[1] then begin
  RigidBodyB.fContactPairEdgeLast:=AContactPair^.Edges[1].Previous;
 end;
 AContactPair^.Edges[1].Previous:=nil;
 AContactPair^.Edges[1].Next:=nil;

 RigidBodyA.SetToAwake;
 RigidBodyB.SetToAwake;

 if assigned(AContactPair^.Previous) then begin
  AContactPair^.Previous^.Next:=AContactPair^.Next;
 end else if fContactPairFirst=AContactPair then begin
  fContactPairFirst:=AContactPair^.Next;
 end;
 if assigned(AContactPair^.Next) then begin
  AContactPair^.Next^.Previous:=AContactPair^.Previous;
 end else if fContactPairLast=AContactPair then begin
  fContactPairLast:=AContactPair^.Previous;
 end;
 AContactPair^.Previous:=nil;
 AContactPair^.Next:=fFreeContactPairs;
 fFreeContactPairs:=AContactPair;

 dec(fCountContactPairs);

end;

procedure TKraftContactManager.RemoveMeshContact(AMeshContactPair:TKraftMeshContactPair);
begin
 AMeshContactPair.RemoveFromHashTable;
 AMeshContactPair.MoveToFreeList;
end;

procedure TKraftContactManager.RemoveContactsFromRigidBody(ARigidBody:TKraftRigidBody);
var ContactPairEdge,NextContactPairEdge:PKraftContactPairEdge;
    MeshContactPair,NextMeshContactPair:TKraftMeshContactPair;
begin
 ContactPairEdge:=ARigidBody.fContactPairEdgeFirst;
 while assigned(ContactPairEdge) do begin
  NextContactPairEdge:=ContactPairEdge^.Next;
  RemoveContact(ContactPairEdge^.ContactPair);
  ContactPairEdge:=NextContactPairEdge;
 end;
 ARigidBody.fContactPairEdgeFirst:=nil;
 ARigidBody.fContactPairEdgeLast:=nil;
 MeshContactPair:=fMeshContactPairFirst;
 while assigned(MeshContactPair) do begin
  NextMeshContactPair:=MeshContactPair.fNext;
  if (MeshContactPair.fRigidBodyConvex=ARigidBody) or (MeshContactPair.fRigidBodyMesh=ARigidBody) then begin
   RemoveMeshContact(MeshContactPair);
  end;
  MeshContactPair:=NextMeshContactPair;
 end;
end;

procedure TKraftContactManager.DoBroadPhase;
var StartTime:int64;
begin
 StartTime:=fPhysics.fHighResolutionTimer.GetTime;
 fPhysics.fBroadPhase.UpdatePairs;
 inc(fPhysics.fBroadPhaseTime,fPhysics.fHighResolutionTimer.GetTime-StartTime);
end;

procedure TKraftContactManager.DoMidPhase;
var MeshContactPair:TKraftMeshContactPair;
    StartTime:int64;
begin
 StartTime:=fPhysics.fHighResolutionTimer.GetTime;
 MeshContactPair:=fMeshContactPairFirst;
 while assigned(MeshContactPair) do begin
  MeshContactPair.Update;
  MeshContactPair:=MeshContactPair.fNext;
 end;
 inc(fPhysics.fMidPhaseTime,fPhysics.fHighResolutionTimer.GetTime-StartTime);
end;

procedure TKraftContactManager.ProcessContactPair(const ContactPair:PKraftContactPair;const ThreadIndex:longint=0);
begin
 ContactPair^.DetectCollisions(self,fPhysics.fTriangleShapes[ThreadIndex],ThreadIndex,true,fPhysics.fWorldDeltaTime);
end;

{$ifdef KraftPasMP}
procedure TKraftContactManager.ProcessContactPairParallelForFunction(const Job:PPasMPJob;const ThreadIndex:longint;const Data:pointer;const FromIndex,ToIndex:TPasMPNativeInt);
var Index:longint;
begin
 for Index:=FromIndex to ToIndex do begin
  ProcessContactPair(fActiveContactPairs[Index],ThreadIndex);
 end;
end;
{$else}
procedure TKraftContactManager.ProcessContactPairJob(const JobIndex,ThreadIndex:longint);
begin
 ProcessContactPair(fActiveContactPairs[JobIndex],ThreadIndex);
end;
{$endif}

procedure TKraftContactManager.DoNarrowPhase;
var ActiveContactPairIndex:longint;
    ContactPair,NextContactPair:PKraftContactPair;
    MeshContactPair,NextMeshContactPair:TKraftMeshContactPair;
    ShapeA,ShapeB:TKraftShape;
    RigidBodyA,RigidBodyB:TKraftRigidBody;
    StartTime:int64;
    Flags:TKraftContactFlags;
begin

 StartTime:=fPhysics.fHighResolutionTimer.GetTime;

 fCountActiveContactPairs:=0;

 ContactPair:=fContactPairFirst;

 while assigned(ContactPair) do begin

  ShapeA:=ContactPair^.Shapes[0];
  ShapeB:=ContactPair^.Shapes[1];

  RigidBodyA:=ContactPair^.RigidBodies[0];
  RigidBodyB:=ContactPair^.RigidBodies[1];

  if kcfFiltered in ContactPair^.Flags then begin
   if (not RigidBodyA.CanCollideWith(RigidBodyB)) or (assigned(fOnCanCollide) and not fOnCanCollide(ShapeA,ShapeB)) then begin
    if (ContactPair^.Flags*[kcfColliding,kcfWasColliding])<>[] then begin
     if (ContactPair^.Flags*[kcfColliding,kcfWasColliding])=[kcfColliding] then begin
      if assigned(fOnContactBegin) then begin
       fOnContactBegin(ContactPair);
      end;
      if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactBegin) then begin
       ContactPair^.Shapes[0].fOnContactBegin(ContactPair,ContactPair^.Shapes[1]);
      end;
      if assigned(ContactPair^.Shapes[1]) and assigned(ContactPair^.Shapes[1].fOnContactBegin) then begin
       ContactPair^.Shapes[1].fOnContactBegin(ContactPair,ContactPair^.Shapes[0]);
      end;
     end;
     if assigned(fOnContactEnd) then begin
      fOnContactEnd(ContactPair);
     end;
     if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactEnd) then begin
      ContactPair^.Shapes[0].fOnContactEnd(ContactPair,ContactPair^.Shapes[1]);
     end;
     if assigned(ContactPair^.Shapes[1]) and assigned(ContactPair^.Shapes[1].fOnContactEnd) then begin
      ContactPair^.Shapes[1].fOnContactEnd(ContactPair,ContactPair^.Shapes[0]);
     end;
    end;
    NextContactPair:=ContactPair^.Next;
    RemoveContact(ContactPair);
    ContactPair:=NextContactPair;
    continue;
   end;
   ContactPair^.Flags:=ContactPair^.Flags-[kcfFiltered];
  end;

  if ((RigidBodyA.fFlags*[krbfAwake,krbfActive])<>[krbfAwake,krbfActive]) and
     ((RigidBodyB.fFlags*[krbfAwake,krbfActive])<>[krbfAwake,krbfActive]) then begin
   ContactPair:=ContactPair^.Next;
   continue;
  end;

  if not AABBIntersect(ShapeA.ProxyFatWorldAABB^,ShapeB.ProxyFatWorldAABB^) then begin
   if (ContactPair^.Flags*[kcfColliding,kcfWasColliding])<>[] then begin
    if (ContactPair^.Flags*[kcfColliding,kcfWasColliding])=[kcfColliding] then begin
     if assigned(fOnContactBegin) then begin
      fOnContactBegin(ContactPair);
     end;
     if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactBegin) then begin
      ContactPair^.Shapes[0].fOnContactBegin(ContactPair,ContactPair^.Shapes[1]);
     end;
     if assigned(ContactPair^.Shapes[1]) and assigned(ContactPair^.Shapes[1].fOnContactBegin) then begin
      ContactPair^.Shapes[1].fOnContactBegin(ContactPair,ContactPair^.Shapes[0]);
     end;
    end;
    if assigned(fOnContactEnd) then begin
     fOnContactEnd(ContactPair);
    end;
    if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactEnd) then begin
     ContactPair^.Shapes[0].fOnContactEnd(ContactPair,ContactPair^.Shapes[1]);
    end;
    if assigned(ContactPair^.Shapes[1]) and assigned(ContactPair^.Shapes[1].fOnContactEnd) then begin
     ContactPair^.Shapes[1].fOnContactEnd(ContactPair,ContactPair^.Shapes[0]);
    end;
   end;
   NextContactPair:=ContactPair^.Next;
   RemoveContact(ContactPair);
   ContactPair:=NextContactPair;
   continue;
  end;

  if assigned(ContactPair^.MeshContactPair) and (ContactPair^.ElementIndex>=0) then begin
   if not AABBIntersect(TKraftShapeMesh(ContactPair^.MeshContactPair.fShapeMesh).fMesh.fTriangles[ContactPair^.ElementIndex].AABB,
                        ContactPair^.MeshContactPair.fConvexAABBInMeshLocalSpace) then begin
    if (ContactPair^.Flags*[kcfColliding,kcfWasColliding])<>[] then begin
     if (ContactPair^.Flags*[kcfColliding,kcfWasColliding])=[kcfColliding] then begin
      if assigned(fOnContactBegin) then begin
       fOnContactBegin(ContactPair);
      end;
      if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactBegin) then begin
       ContactPair^.Shapes[0].fOnContactBegin(ContactPair,ContactPair^.MeshContactPair.fShapeMesh);
      end;
      if assigned(ContactPair^.MeshContactPair.fShapeMesh) and assigned(ContactPair^.MeshContactPair.fShapeMesh.fOnContactBegin) then begin
       ContactPair^.Shapes[1].fOnContactBegin(ContactPair,ContactPair^.Shapes[0]);
      end;
     end;
     if assigned(fOnContactEnd) then begin
      fOnContactEnd(ContactPair);
     end;
     if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactEnd) then begin
      ContactPair^.Shapes[0].fOnContactEnd(ContactPair,ContactPair^.MeshContactPair.fShapeMesh);
     end;
     if assigned(ContactPair^.MeshContactPair.fShapeMesh) and assigned(ContactPair^.MeshContactPair.fShapeMesh.fOnContactEnd) then begin
      ContactPair^.MeshContactPair.fShapeMesh.fOnContactEnd(ContactPair,ContactPair^.Shapes[0]);
     end;
    end;
    NextContactPair:=ContactPair^.Next;
    RemoveContact(ContactPair);
    ContactPair:=NextContactPair;
    continue;
   end;
  end;

  ActiveContactPairIndex:=fCountActiveContactPairs;
  inc(fCountActiveContactPairs);
  if fCountActiveContactPairs>length(fActiveContactPairs) then begin
   SetLength(fActiveContactPairs,fCountActiveContactPairs*2);
  end;
  fActiveContactPairs[ActiveContactPairIndex]:=ContactPair;

  ContactPair:=ContactPair^.Next;

 end;

 fCountRemainActiveContactPairsToDo:=fCountActiveContactPairs;

{$ifdef KraftPasMP}
 if assigned(fPhysics.fPasMP) and (fCountActiveContactPairs>64) and not fPhysics.fSingleThreaded then begin
  fPhysics.fPasMP.Invoke(fPhysics.fPasMP.ParallelFor(nil,0,fCountActiveContactPairs-1,ProcessContactPairParallelForFunction,Max(64,fCountActiveContactPairs div (fPhysics.fCountThreads*16)),4));
{$else}
 if assigned(fPhysics.fJobManager) and (fCountActiveContactPairs>64) and not fPhysics.fSingleThreaded then begin
  fPhysics.fJobManager.fOnProcessJob:=ProcessContactPairJob;
  fPhysics.fJobManager.fCountRemainJobs:=fCountActiveContactPairs;
  fPhysics.fJobManager.fGranularity:=Max(64,fCountActiveContactPairs div (fPhysics.fCountThreads*16));
  fPhysics.fJobManager.ProcessJobs;
{$endif}
 end else begin
  for ActiveContactPairIndex:=0 to fCountActiveContactPairs-1 do begin
   ProcessContactPair(fActiveContactPairs[ActiveContactPairIndex],0);
  end;
 end;

 for ActiveContactPairIndex:=0 to fCountActiveContactPairs-1 do begin
  ContactPair:=fActiveContactPairs[ActiveContactPairIndex];
  Flags:=ContactPair^.Flags*[kcfColliding,kcfWasColliding];
  if Flags=[kcfColliding] then begin
   if assigned(fOnContactBegin) then begin
    fOnContactBegin(ContactPair);
   end;
   if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactBegin) then begin
    ContactPair^.Shapes[0].fOnContactBegin(ContactPair,ContactPair^.Shapes[1]);
   end;
   if assigned(ContactPair^.Shapes[1]) and assigned(ContactPair^.Shapes[1].fOnContactBegin) then begin
    ContactPair^.Shapes[1].fOnContactBegin(ContactPair,ContactPair^.Shapes[0]);
   end;
  end else if Flags=[kcfWasColliding] then begin
   if assigned(fOnContactEnd) then begin
    fOnContactEnd(ContactPair);
   end;
   if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactEnd) then begin
    ContactPair^.Shapes[0].fOnContactEnd(ContactPair,ContactPair^.Shapes[1]);
   end;
   if assigned(ContactPair^.Shapes[1]) and assigned(ContactPair^.Shapes[1].fOnContactEnd) then begin
    ContactPair^.Shapes[1].fOnContactEnd(ContactPair,ContactPair^.Shapes[0]);
   end;
  end else if Flags=[kcfColliding,kcfWasColliding] then begin
   if assigned(fOnContactStay) then begin
    fOnContactStay(ContactPair);
   end;
   if assigned(ContactPair^.Shapes[0]) and assigned(ContactPair^.Shapes[0].fOnContactStay) then begin
    ContactPair^.Shapes[0].fOnContactStay(ContactPair,ContactPair^.Shapes[1]);
   end;
   if assigned(ContactPair^.Shapes[1]) and assigned(ContactPair^.Shapes[1].fOnContactStay) then begin
    ContactPair^.Shapes[1].fOnContactStay(ContactPair,ContactPair^.Shapes[0]);
   end;
  end;
 end;

 MeshContactPair:=fMeshContactPairFirst;
 while assigned(MeshContactPair) do begin

  ShapeA:=MeshContactPair.fShapeConvex;
  ShapeB:=MeshContactPair.fShapeMesh;

  RigidBodyA:=MeshContactPair.fRigidBodyConvex;
  RigidBodyB:=MeshContactPair.fRigidBodyMesh;

  if kcfFiltered in MeshContactPair.fFlags then begin
   if (not RigidBodyA.CanCollideWith(RigidBodyB)) or (assigned(fOnCanCollide) and not fOnCanCollide(ShapeA,ShapeB)) then begin
    NextMeshContactPair:=MeshContactPair.fNext;
    RemoveMeshContact(MeshContactPair);
    MeshContactPair:=NextMeshContactPair;
    continue;
   end;
   MeshContactPair.fFlags:=MeshContactPair.fFlags-[kcfFiltered];
  end;

  if ((RigidBodyA.fFlags*[krbfAwake,krbfActive])<>[krbfAwake,krbfActive]) and
     ((RigidBodyB.fFlags*[krbfAwake,krbfActive])<>[krbfAwake,krbfActive]) then begin
   MeshContactPair:=MeshContactPair.fNext;
   continue;
  end;

  if not AABBIntersect(ShapeA.ProxyFatWorldAABB^,ShapeB.ProxyFatWorldAABB^) then begin
   NextMeshContactPair:=MeshContactPair.fNext;
   RemoveMeshContact(MeshContactPair);
   MeshContactPair:=NextMeshContactPair;
   continue;
  end;

  MeshContactPair:=MeshContactPair.fNext;

 end;

 inc(fPhysics.fNarrowPhaseTime,fPhysics.fHighResolutionTimer.GetTime-StartTime);

end;

{$ifdef DebugDraw}
procedure TKraftContactManager.DebugDraw(const CameraMatrix:TKraftMatrix4x4);
var i,j:longint;
    ContactPair:PKraftContactPair;
    ContactManifold:PKraftContactManifold;
    Contact:PKraftContact;
    SolverContact:PKraftSolverContact;
    SolverContactManifold:TKraftSolverContactManifold;
    f:TKraftScalar;
begin

 glPushMatrix;
 glMatrixMode(GL_MODELVIEW);

{$ifdef UseDouble}
 glLoadMatrixd(pointer(@CameraMatrix));
{$else}
 glLoadMatrixf(pointer(@CameraMatrix));
{$endif}

 ContactPair:=fContactPairFirst;

 while assigned(ContactPair) do begin

  if kcfColliding in ContactPair^.Flags then begin

   ContactManifold:=@ContactPair^.Manifold;
                                                              
   ContactPair^.GetSolverContactManifold(SolverContactManifold,ContactPair^.RigidBodies[0].fWorldTransform,ContactPair^.RigidBodies[1].fWorldTransform,kcpcmmBaumgarte);

   for i:=0 to ContactManifold^.CountContacts-1 do begin

    SolverContact:=@SolverContactManifold.Contacts[i];

    Contact:=@ContactManifold.Contacts[i];

    f:=(1024-Min(Max(Contact^.WarmStartState,0),1024))/1024.0;
                                                  
    if ContactPair^.Manifold.ContactManifoldType=kcmtSPECULATIVE then begin

     if krbfAwake in ContactPair^.Shapes[0].fRigidBody.fFlags then begin
      glColor4f(0.0,f,1.0-f,1.0);
     end else begin
      glColor4f(0.0,1.0,0.0,1.0);
     end;
     glBegin(GL_POINTS);
{$ifdef UseDouble}
     glVertex3fd(@SolverContactManifold.Points[0]);
     glVertex3fd(@SolverContactManifold.Points[1]);
{$else}
     glVertex3fv(@SolverContactManifold.Points[0]);
     glVertex3fv(@SolverContactManifold.Points[1]);
{$endif}
     glEnd;

     if krbfAwake in ContactPair^.Shapes[0].fRigidBody.fFlags then begin
      glColor4f(1.0,1.0,1.0,1.0);
     end else begin
      glColor4f(0.2,0.2,0.2,1.0);
     end;
     glBegin(GL_LINES);
{$ifdef UseDouble}
     glVertex3fd(@SolverContactManifold.Points[0]);
     glVertex3fd(@SolverContactManifold.Points[1]);
{$else}
     glVertex3fv(@SolverContactManifold.Points[0]);
     glVertex3fv(@SolverContactManifold.Points[1]);
{$endif}
     glEnd;

    end else begin

     if krbfAwake in ContactPair^.Shapes[0].fRigidBody.fFlags then begin
      glColor4f(1.0-f,f,f,1.0);
     end else begin
      glColor4f(1.0,1.0,0.0,1.0);
     end;
     glBegin(GL_POINTS);
{$ifdef UseDouble}
     glVertex3dv(@SolverContact^.Point);
{$else}
     glVertex3fv(@SolverContact^.Point);
{$endif}
     glEnd;

     if krbfAwake in ContactPair^.Shapes[0].fRigidBody.fFlags then begin
      glColor4f(1.0,1.0,1.0,1.0);
     end else begin
      glColor4f(0.2,0.2,0.2,1.0);
     end;
     glBegin(GL_LINES);
{$ifdef UseDouble}
     glVertex3dv(@SolverContact^.Point);
{$else}
     glVertex3fv(@SolverContact^.Point);
{$endif}
     glVertex3f(SolverContact^.Point.x+(SolverContactManifold.Normal.x*SolverContact^.Separation),
                SolverContact^.Point.y+(SolverContactManifold.Normal.y*SolverContact^.Separation),
                SolverContact^.Point.z+(SolverContactManifold.Normal.z*SolverContact^.Separation));
     glEnd;

    end;

   end;

  end;

  ContactPair:=ContactPair^.Next;

 end;

 glLineWidth(2);
 glBegin(GL_LINES);
 for i:=0 to fCountDebugClipVertexLists-1 do begin
{$ifdef UseDouble}
  glColor4dv(@fDebugClipVertexLists[i].Color);
  for j:=0 to fDebugClipVertexLists[i].Count-1 do begin
   if j=0 then begin
    glVertex3dv(@fDebugClipVertexLists[i].Vertices[fDebugClipVertexLists[i].Count-1].Position);
   end else begin
    glVertex3dv(@fDebugClipVertexLists[i].Vertices[j-1].Position);
   end;
   glVertex3dv(@fDebugClipVertexLists[i].Vertices[j].Position);
  end;
{$else}
  glColor4fv(@fDebugClipVertexLists[i].Color);
  for j:=0 to fDebugClipVertexLists[i].Count-1 do begin
   if j=0 then begin
    glVertex3fv(@fDebugClipVertexLists[i].Vertices[fDebugClipVertexLists[i].Count-1].Position);
   end else begin
    glVertex3fv(@fDebugClipVertexLists[i].Vertices[j-1].Position);
   end;
   glVertex3fv(@fDebugClipVertexLists[i].Vertices[j].Position);
  end;
{$endif}
 end;
 glEnd;

 glPopMatrix;

end;
{$endif}

function TKraftContactManager.ReduceContacts(const AInputContacts:PKraftContacts;const ACountInputContacts:longint;const AOutputContacts:PKraftContacts):longint;
var Index,MaxPenetrationIndex:longint;
    MaxPenetration,MaxDistance,Distance,MaxArea,Area:TKraftScalar;
    Contact:PKraftContact;
    Contacts:array[0..MAX_CONTACTS-1] of PKraftContact;
begin
 if ACountInputContacts<=0 then begin

  result:=0;

 end else if ACountInputContacts<=MAX_CONTACTS then begin

  result:=ACountInputContacts;

  for Index:=0 to ACountInputContacts-1 do begin
   AOutputContacts^[Index]:=AInputContacts^[Index];
  end;

 end else begin

  result:=MAX_CONTACTS;

  MaxPenetrationIndex:=0;
  MaxPenetration:=AInputContacts^[0].Penetration;
  for Index:=1 to ACountInputContacts-1 do begin
   Contact:=@AInputContacts^[Index];
   if MaxPenetration<Contact^.Penetration then begin
    MaxPenetrationIndex:=Index;
    MaxPenetration:=Contact^.Penetration;
   end;
  end;
  Contacts[0]:=@AInputContacts^[MaxPenetrationIndex];

  Contacts[1]:=nil;
  MaxDistance:=0.0;
  for Index:=0 to ACountInputContacts-1 do begin
   Contact:=@AInputContacts^[Index];
   if Contact<>Contacts[0] then begin
    Distance:=Vector3DistSquared(Contact^.LocalPoints[0],Contacts[0]^.LocalPoints[0]);
    if (not assigned(Contacts[1])) or (MaxDistance<Distance) then begin
     MaxDistance:=Distance;
     Contacts[1]:=Contact;
    end;
   end;
  end;

  Contacts[2]:=nil;              
  MaxArea:=0.0;
  for Index:=0 to ACountInputContacts-1 do begin
   Contact:=@AInputContacts^[Index];
   if (Contact<>Contacts[0]) and (Contact<>Contacts[1]) then begin
    Area:=CalculateAreaFromThreePoints(Contact^.LocalPoints[0],Contacts[0]^.LocalPoints[0],Contacts[1]^.LocalPoints[0]);
    if (not assigned(Contacts[2])) or (MaxArea<Area) then begin
     MaxArea:=Area;
     Contacts[2]:=Contact;
    end;
   end;
  end;

  Contacts[3]:=nil;
  MaxArea:=0.0;
  for Index:=0 to ACountInputContacts-1 do begin
   Contact:=@AInputContacts^[Index];
   if (Contact<>Contacts[0]) and (Contact<>Contacts[1]) and (Contact<>Contacts[2]) then begin
    Area:=CalculateAreaFromFourPoints(Contact^.LocalPoints[0],Contacts[0]^.LocalPoints[0],Contacts[1]^.LocalPoints[0],Contacts[2]^.LocalPoints[0]);
    if (not assigned(Contacts[3])) or (MaxArea<Area) then begin
     MaxArea:=Area;
     Contacts[3]:=Contact;
    end;
   end;
  end;

  for Index:=0 to MAX_CONTACTS-1 do begin
   AOutputContacts^[Index]:=Contacts[Index]^;
  end;

 end;
end;

function TKraftContactManager.GetMaximizedAreaReducedContactIndices(const AInputContactPositions:PPKraftVector3s;const ACountInputContactPositions:longint;var AOutputContactIndices:TKraftContactIndices):longint;
var Index,StartIndex:longint;
    MaxDistance,Distance,MaxArea,Area:TKraftScalar;
    Position:PKraftVector3;
    Positions:array[0..MAX_CONTACTS-1] of PKraftVector3;
    Contacts:array[0..MAX_CONTACTS-1] of longint;
begin
 if ACountInputContactPositions<=0 then begin

  result:=0;

 end else if ACountInputContactPositions<=MAX_CONTACTS then begin

  result:=ACountInputContactPositions;

  for Index:=0 to ACountInputContactPositions-1 do begin
   AOutputContactIndices[Index]:=Index;
  end;

 end else begin

  result:=MAX_CONTACTS-1;

  StartIndex:=0;
  Positions[0]:=AInputContactPositions^[StartIndex];
  for Index:=1 to ACountInputContactPositions-1 do begin
   Position:=AInputContactPositions^[Index];
   if (Position^.x<Positions[0]^.x) or (Position^.y<Positions[0]^.y) or (Position^.z<Positions[0]^.z) then begin
    StartIndex:=Index;
    Positions[0]:=AInputContactPositions^[Index];
   end;
  end;
  Contacts[0]:=StartIndex;

  Contacts[1]:=-1;
  MaxDistance:=0.0;
  for Index:=0 to ACountInputContactPositions-1 do begin
   Position:=AInputContactPositions^[Index];
   if Index<>Contacts[0] then begin
    Distance:=Vector3DistSquared(Position^,Positions[0]^);
    if (Contacts[1]<0) or (MaxDistance<Distance) then begin
     MaxDistance:=Distance;
     Contacts[1]:=Index;
    end;
   end;
  end;
  Positions[1]:=AInputContactPositions^[Contacts[1]];

  Contacts[2]:=-1;
  MaxArea:=0.0;
  for Index:=0 to ACountInputContactPositions-1 do begin
   Position:=AInputContactPositions^[Index];
   if (Index<>Contacts[0]) and (Index<>Contacts[1]) then begin
    Area:=CalculateAreaFromThreePoints(Position^,Positions[0]^,Positions[1]^);
    if (Contacts[2]<0) or (MaxArea<Area) then begin
     MaxArea:=Area;
     Contacts[2]:=Index;
    end;
   end;
  end;
  Positions[2]:=AInputContactPositions^[Contacts[2]];

  Contacts[3]:=-1;
  MaxArea:=0.0;
  for Index:=0 to ACountInputContactPositions-1 do begin
   Position:=AInputContactPositions^[Index];
   if (Index<>Contacts[0]) and (Index<>Contacts[1]) and (Index<>Contacts[2]) then begin
    Area:=CalculateAreaFromFourPoints(Position^,Positions[0]^,Positions[1]^,Positions[2]^);
    if (Contacts[3]<0) or (MaxArea<Area) then begin
     MaxArea:=Area;
     Contacts[3]:=Index;
    end;
   end;
  end;

  for Index:=0 to MAX_CONTACTS-1 do begin
   AOutputContactIndices[Index]:=Contacts[Index];
  end;

 end;
end;

constructor TKraftBroadPhase.Create(const APhysics:TKraft);
var ThreadIndex,CountThreads:longint;
begin
 inherited Create;

 fPhysics:=APhysics;

 CountThreads:=Max(1,fPhysics.fCountThreads);

 for ThreadIndex:=0 to MAX_THREADS-1 do begin

  if ThreadIndex<CountThreads then begin
   fStackCapacity[ThreadIndex]:=16;
   GetMem(fStack[ThreadIndex],fStackCapacity[ThreadIndex]*SizeOf(longint));
  end else begin
   fStack[ThreadIndex]:=nil;
   fStackCapacity[ThreadIndex]:=0;
  end;

  fContactPairs[ThreadIndex]:=nil;
  if ThreadIndex<CountThreads then begin
   SetLength(fContactPairs[ThreadIndex],4096);
  end else begin
   SetLength(fContactPairs[ThreadIndex],0);
  end;
  fCountContactPairs[ThreadIndex]:=0;
  
 end;

 fStaticMoveBuffer:=nil;
 SetLength(fStaticMoveBuffer,64);
 fStaticMoveBufferSize:=0;

 fDynamicMoveBuffer:=nil;
 SetLength(fDynamicMoveBuffer,64);
 fDynamicMoveBufferSize:=0;

 fKinematicMoveBuffer:=nil;
 SetLength(fKinematicMoveBuffer,64);
 fKinematicMoveBufferSize:=0;

end;

destructor TKraftBroadPhase.Destroy;
var ThreadIndex:longint;
begin
 for ThreadIndex:=0 to MAX_THREADS-1 do begin
  if assigned(fStack[ThreadIndex]) then begin
   FreeMem(fStack[ThreadIndex]);
   fStack[ThreadIndex]:=nil;
  end;
  SetLength(fContactPairs[ThreadIndex],0);
 end;
 SetLength(fStaticMoveBuffer,0);
 SetLength(fDynamicMoveBuffer,0);
 SetLength(fKinematicMoveBuffer,0);
 inherited Destroy;
end;

function CompareContactPairs(const a,b:pointer):longint;
begin
 result:=PtrInt(PKraftBroadPhaseContactPair(a)^[0])-PtrInt(PKraftBroadPhaseContactPair(b)^[0]);
 if result=0 then begin
  result:=PtrInt(PKraftBroadPhaseContactPair(a)^[1])-PtrInt(PKraftBroadPhaseContactPair(b)^[1]);
 end;
end;

procedure TKraftBroadPhase.AddPair(const ThreadIndex:longint;ShapeA,ShapeB:TKraftShape); {$ifdef caninline}inline;{$endif}
var TempShape:TKraftShape;
    Index:longint;
    ContactPair:PKraftBroadPhaseContactPair;
begin
 if (ShapeA<>ShapeB) and (ShapeA.RigidBody<>ShapeB.RigidBody) then begin
  if (ShapeA.fShapeType>ShapeB.fShapeType) or ((ShapeA.fShapeType=ShapeB.fShapeType) and (ptruint(ShapeA)>ptruint(ShapeB))) then begin
   TempShape:=ShapeA;
   ShapeA:=ShapeB;
   ShapeB:=TempShape;
  end;
  Index:=fCountContactPairs[ThreadIndex];
  inc(fCountContactPairs[ThreadIndex]);
  if fCountContactPairs[ThreadIndex]>length(fContactPairs[ThreadIndex]) then begin
   SetLength(fContactPairs[ThreadIndex],fCountContactPairs[ThreadIndex]*2);
  end;
  ContactPair:=@fContactPairs[ThreadIndex,Index];
  ContactPair[0]:=ShapeA;
  ContactPair[1]:=ShapeB;
 end;
end;

procedure TKraftBroadPhase.QueryShapeWithTree(const ThreadIndex:longint;const Shape:TKraftShape;const AABBTree:TKraftDynamicAABBTree);
{$ifdef KraftSingleThreadedUsage}
var ShapeAABB:PKraftAABB;
    LocalStack:PKraftDynamicAABBTreeLongintArray;
    LocalStackPointer,NodeID:longint;
    Node:PKraftDynamicAABBTreeNode;
    OtherShape:TKraftShape;
begin
 if assigned(Shape) and assigned(AABBTree) then begin
  ShapeAABB:=Shape.ProxyFatWorldAABB;
  if AABBTree.fRoot>=0 then begin
   LocalStack:=fStack[ThreadIndex];
   LocalStack^[0]:=AABBTree.fRoot;
   LocalStackPointer:=1;
   while LocalStackPointer>0 do begin
    dec(LocalStackPointer);
    NodeID:=LocalStack^[LocalStackPointer];
    if NodeID>=0 then begin
     Node:=@AABBTree.fNodes[NodeID];
     if AABBIntersect(Node^.AABB,ShapeAABB^) then begin
      if Node^.Children[0]<0 then begin
       OtherShape:=Node^.UserData;
       if assigned(OtherShape) and (Shape<>OtherShape) then begin
        AddPair(ThreadIndex,Shape,OtherShape);
       end;
      end else begin
       if fStackCapacity[ThreadIndex]<=(LocalStackPointer+2) then begin
        fStackCapacity[ThreadIndex]:=RoundUpToPowerOfTwo(LocalStackPointer+2);
        ReallocMem(fStack[ThreadIndex],fStackCapacity[ThreadIndex]*SizeOf(longint));
        LocalStack:=fStack[ThreadIndex];
       end;
       LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
       LocalStack^[LocalStackPointer+1]:=Node^.Children[1];
       inc(LocalStackPointer,2);
      end;
     end;
    end;
   end;
  end;
 end;
end;
{$else}
var ShapeAABB:PKraftAABB;
 procedure ProcessNode(const NodeID:longint);
 var Node:PKraftDynamicAABBTreeNode;
     OtherShape:TKraftShape;
 begin
  if NodeID>=0 then begin
   Node:=@AABBTree.fNodes[NodeID];
   if AABBIntersect(Node^.AABB,ShapeAABB^) then begin
    if Node^.Children[0]<0 then begin
     OtherShape:=Node^.UserData;
     if assigned(OtherShape) and (Shape<>OtherShape) then begin
      AddPair(ThreadIndex,Shape,OtherShape);
     end;
    end else begin
     ProcessNode(Node^.Children[0]);
     ProcessNode(Node^.Children[1]);
    end;
   end;
  end;
 end;
begin
 if assigned(Shape) and assigned(AABBTree) then begin
  ShapeAABB:=Shape.ProxyFatWorldAABB;
  if AABBTree.fRoot>=0 then begin
   ProcessNode(AABBTree.fRoot);
  end;
 end;
end;
{$endif}

{$ifdef KraftPasMP}
procedure TKraftBroadPhase.ProcessMoveBufferItemParallelForFunction(const Job:PPasMPJob;const ThreadIndex:longint;const Data:pointer;const FromIndex,ToIndex:TPasMPNativeInt);
{$else}
procedure TKraftBroadPhase.ProcessMoveBufferItem(const JobIndex,ThreadIndex:longint);
{$endif}
var {$ifdef KraftPasMP}JobIndex,{$endif}Index:longint;
    Shape:TKraftShape;
begin
{$ifdef KraftPasMP}for JobIndex:=FromIndex to ToIndex do{$endif}begin
  Index:=JobIndex;
  if Index<fStaticMoveBufferSize then begin
   // Static shapes against dynamic shapes
   if fStaticMoveBuffer[Index]>=0 then begin
    Shape:=fPhysics.fStaticAABBTree.fNodes[fStaticMoveBuffer[Index]].UserData;
    if assigned(Shape) then begin
     QueryShapeWithTree(ThreadIndex,Shape,fPhysics.fDynamicAABBTree);
    end;
   end;
  end else if Index<(fStaticMoveBufferSize+fDynamicMoveBufferSize) then begin
   // Dynamic shapes against static, dynamic and kinematic shapes
   dec(Index,fStaticMoveBufferSize);
   if fDynamicMoveBuffer[Index]>=0 then begin
    Shape:=fPhysics.fDynamicAABBTree.fNodes[fDynamicMoveBuffer[Index]].UserData;
    if assigned(Shape) then begin
     QueryShapeWithTree(ThreadIndex,Shape,fPhysics.fStaticAABBTree);
     QueryShapeWithTree(ThreadIndex,Shape,fPhysics.fDynamicAABBTree);
     QueryShapeWithTree(ThreadIndex,Shape,fPhysics.fKinematicAABBTree);
    end;
   end;
  end else if Index<(fStaticMoveBufferSize+fDynamicMoveBufferSize+fKinematicMoveBufferSize) then begin
   // Kinematic shapes against dynamic shapes
   dec(Index,fStaticMoveBufferSize+fDynamicMoveBufferSize);
   if fKinematicMoveBuffer[Index]>=0 then begin
    Shape:=fPhysics.fKinematicAABBTree.fNodes[fKinematicMoveBuffer[Index]].UserData;
    if assigned(Shape) then begin
     QueryShapeWithTree(ThreadIndex,Shape,fPhysics.fDynamicAABBTree);
    end;
   end;
  end;
 end;
end;

procedure TKraftBroadPhase.UpdatePairs;
var ThreadIndex,Count,Index:longint;
    ContactPair,OtherContactPair:PKraftBroadPhaseContactPair;
begin

 // Reset found thread contact pair arrays
 for ThreadIndex:=0 to Max(1,fPhysics.CountThreads)-1 do begin
  fCountContactPairs[ThreadIndex]:=0;
 end;

 // Run the thread jobs
 fAllMoveBufferSize:=fStaticMoveBufferSize+fDynamicMoveBufferSize+fKinematicMoveBufferSize;
{$ifdef KraftPasMP}
 if assigned(fPhysics.fPasMP) and (fAllMoveBufferSize>1024) and not fPhysics.fSingleThreaded then begin
  fPhysics.fPasMP.Invoke(fPhysics.fPasMP.ParallelFor(nil,0,fAllMoveBufferSize-1,ProcessMoveBufferItemParallelForFunction,Max(64,fAllMoveBufferSize div (fPhysics.fCountThreads*16)),4));
 end else begin
  ProcessMoveBufferItemParallelForFunction(nil,0,nil,0,fAllMoveBufferSize-1);
 end;
{$else}
 if assigned(fPhysics.fJobManager) and (fAllMoveBufferSize>1024) and not fPhysics.fSingleThreaded then begin
  fPhysics.fJobManager.fOnProcessJob:=ProcessMoveBufferItem;
  fPhysics.fJobManager.fCountRemainJobs:=fAllMoveBufferSize;
  fPhysics.fJobManager.fGranularity:=Max(64,fAllMoveBufferSize div (fPhysics.fCountThreads*16));
  fPhysics.fJobManager.ProcessJobs;
 end else begin
  for Index:=0 to fAllMoveBufferSize-1 do begin
   ProcessMoveBufferItem(Index,0);
  end;
 end;
{$endif}

 // Reset move buffer sizes
 fStaticMoveBufferSize:=0;
 fDynamicMoveBufferSize:=0;
 fKinematicMoveBufferSize:=0;

 // Merge thread contact pair lists into the first single contact pair list
 for ThreadIndex:=1 to fPhysics.CountThreads-1 do begin
  if fCountContactPairs[ThreadIndex]>0 then begin
   Count:=fCountContactPairs[0]+fCountContactPairs[ThreadIndex];
   if Count>length(fContactPairs[0]) then begin
    SetLength(fContactPairs[0],RoundUpToPowerOfTwo(Count+1));
   end;
   Move(fContactPairs[ThreadIndex,0],fContactPairs[0,fCountContactPairs[0]],fCountContactPairs[ThreadIndex]*SizeOf(TKraftBroadPhaseContactPair));
   fCountContactPairs[0]:=Count;
  end;
 end;

 // Process the found contact pairs, when there are any...
 if fCountContactPairs[0]>0 then begin

  // Sort pairs to expose duplicates
  DirectIntroSort(@fContactPairs[0,0],0,fCountContactPairs[0]-1,SizeOf(TKraftBroadPhaseContactPair),CompareContactPairs);

  // Queue manifolds for solving
  Index:=0;
  while Index<fCountContactPairs[0] do begin

   ContactPair:=@fContactPairs[0,Index];
   inc(Index);

   // Add contact pair to contact manager
   fPhysics.fContactManager.AddContact(ContactPair^[0],ContactPair^[1]);

   // Skip duplicate pairs until we find a unique pair
   while Index<fCountContactPairs[0] do begin
    OtherContactPair:=@fContactPairs[0,Index];
    if (ContactPair^[0]<>OtherContactPair^[0]) or (ContactPair^[1]<>OtherContactPair^[1]) then begin
     break;
    end;
    inc(Index);
   end;

  end;

 end;

end;

procedure TKraftBroadPhase.StaticBufferMove(ProxyID:longint);
var Index:longint;
begin
 Index:=fStaticMoveBufferSize;
 inc(fStaticMoveBufferSize);
 if fStaticMoveBufferSize>length(fStaticMoveBuffer) then begin
  SetLength(fStaticMoveBuffer,fStaticMoveBufferSize*2);
 end;
 fStaticMoveBuffer[Index]:=ProxyID;
end;

procedure TKraftBroadPhase.DynamicBufferMove(ProxyID:longint);
var Index:longint;
begin
 Index:=fDynamicMoveBufferSize;
 inc(fDynamicMoveBufferSize);
 if fDynamicMoveBufferSize>length(fDynamicMoveBuffer) then begin
  SetLength(fDynamicMoveBuffer,fDynamicMoveBufferSize*2);
 end;
 fDynamicMoveBuffer[Index]:=ProxyID;
end;

procedure TKraftBroadPhase.KinematicBufferMove(ProxyID:longint);
var Index:longint;
begin
 Index:=fKinematicMoveBufferSize;
 inc(fKinematicMoveBufferSize);
 if fKinematicMoveBufferSize>length(fKinematicMoveBuffer) then begin
  SetLength(fKinematicMoveBuffer,fKinematicMoveBufferSize*2);
 end;
 fKinematicMoveBuffer[Index]:=ProxyID;
end;

constructor TKraftRigidBody.Create(const APhysics:TKraft);
begin
 inherited Create;

 fPhysics:=APhysics;

 fIsland:=nil;

 fIslandIndices:=nil;
 SetLength(fIslandIndices,4);

 inc(fPhysics.fCountRigidBodies);

 fID:=fPhysics.fRigidBodyIDCounter;
 inc(fPhysics.fRigidBodyIDCounter);

 fRigidBodyType:=krbtUnknown;

 if assigned(fPhysics.fRigidBodyLast) then begin
  fPhysics.fRigidBodyLast.fRigidBodyNext:=self;
  fRigidBodyPrevious:=fPhysics.fRigidBodyLast;
 end else begin
  fPhysics.fRigidBodyFirst:=self;
  fRigidBodyPrevious:=nil;
 end;
 fPhysics.fRigidBodyLast:=self;
 fRigidBodyNext:=nil;

 fStaticRigidBodyIsOnList:=false;
 fStaticRigidBodyPrevious:=nil;
 fStaticRigidBodyNext:=nil;

 fDynamicRigidBodyIsOnList:=false;
 fDynamicRigidBodyPrevious:=nil;
 fDynamicRigidBodyNext:=nil;

 fKinematicRigidBodyIsOnList:=false;
 fKinematicRigidBodyPrevious:=nil;
 fKinematicRigidBodyNext:=nil;

 fShapeFirst:=nil;
 fShapeLast:=nil;

 fShapeCount:=0;

 fFlags:=[krbfContinuous,krbfAllowSleep,krbfAwake,krbfActive];

 fWorldDisplacement:=Vector3Origin;

 fSweep.LocalCenter:=Vector3Origin;
 fSweep.c0:=Vector3Origin;
 fSweep.c:=Vector3Origin;
 fSweep.q0:=QuaternionIdentity;
 fSweep.q:=QuaternionIdentity;
 fSweep.Alpha0:=0.0;

 fWorldTransform:=Matrix4x4Identity;

 fGravity.x:=0.0;
 fGravity.y:=-9.83;
 fGravity.z:=0.0;

 fGravityProperty:=TKraftVector3Property.Create(@fGravity);

 fLinearFactor.x:=1.0;
 fLinearFactor.y:=1.0;
 fLinearFactor.z:=1.0;

 fUserData:=nil;

 fNextOnIslandBuildStack:=nil;
 fNextStaticRigidBody:=nil;

 fBodyInertiaTensor:=Matrix3x3Identity;
 fBodyInverseInertiaTensor:=Matrix3x3Identity;

 fWorldInertiaTensor:=Matrix3x3Identity;
 fWorldInverseInertiaTensor:=Matrix3x3Identity;

 fForcedMass:=0.0;

 fMass:=0.0;
 fInverseMass:=0.0;

 fLinearVelocity:=Vector3Origin;
 fAngularVelocity:=Vector3Origin;

 fMaximalLinearVelocity:=0.0;
 fMaximalAngularVelocity:=0.0;

 fLinearVelocityDamp:=0.1;
 fAngularVelocityDamp:=0.1;
 fAdditionalDamping:=false;
 fAdditionalDamp:=0.005;
 fLinearVelocityAdditionalDamp:=0.01;
 fAngularVelocityAdditionalDamp:=0.01;
 fLinearVelocityAdditionalDampThresholdSqr:=0.01;
 fAngularVelocityAdditionalDampThresholdSqr:=0.01;
 
 fForce:=Vector3Origin;
 fTorque:=Vector3Origin;

 fSleepTime:=0.0;

 fGravityScale:=1.0;

 fEnableGyroscopicForce:=false;

 fMaximalGyroscopicForce:=0.0;

 fCollisionGroups:=[0];

 fCollideWithCollisionGroups:=[low(TKraftRigidBodyCollisionGroup)..high(TKraftRigidBodyCollisionGroup)];

 fCountConstraints:=0;

 fConstraintEdgeFirst:=nil;
 fConstraintEdgeLast:=nil;

 fContactPairEdgeFirst:=nil;
 fContactPairEdgeLast:=nil;

 fOnPreStep:=nil;
 fOnPostStep:=nil;

end;

destructor TKraftRigidBody.Destroy;
var ConstraintEdge,NextConstraintEdge:PKraftConstraintEdge;
    Constraint:TKraftConstraint;
begin

 ConstraintEdge:=fConstraintEdgeFirst;
 while assigned(ConstraintEdge) do begin
  NextConstraintEdge:=ConstraintEdge^.Next;
  Constraint:=ConstraintEdge^.Constraint;
  if assigned(Constraint) then begin
   Constraint.Free;
  end;
  ConstraintEdge:=NextConstraintEdge;
 end;

 fCountConstraints:=0;

 while assigned(fShapeLast) do begin
  fShapeLast.Free;
 end;

 fPhysics.fContactManager.RemoveContactsFromRigidBody(self);

 if assigned(fRigidBodyPrevious) then begin
  fRigidBodyPrevious.fRigidBodyNext:=fRigidBodyNext;
 end else if fPhysics.fRigidBodyFirst=self then begin
  fPhysics.fRigidBodyFirst:=fRigidBodyNext;
 end;
 if assigned(fRigidBodyNext) then begin
  fRigidBodyNext.fRigidBodyPrevious:=fRigidBodyPrevious;
 end else if fPhysics.fRigidBodyLast=self then begin
  fPhysics.fRigidBodyLast:=fRigidBodyPrevious;
 end;
 fRigidBodyPrevious:=nil;
 fRigidBodyNext:=nil;

 if fStaticRigidBodyIsOnList then begin
  fStaticRigidBodyIsOnList:=false;
  if assigned(fStaticRigidBodyPrevious) then begin
   fStaticRigidBodyPrevious.fStaticRigidBodyNext:=fStaticRigidBodyNext;
  end else if fPhysics.fStaticRigidBodyFirst=self then begin
   fPhysics.fStaticRigidBodyFirst:=fStaticRigidBodyNext;
  end;
  if assigned(fStaticRigidBodyNext) then begin
   fStaticRigidBodyNext.fStaticRigidBodyPrevious:=fStaticRigidBodyPrevious;
  end else if fPhysics.fStaticRigidBodyLast=self then begin
   fPhysics.fStaticRigidBodyLast:=fStaticRigidBodyPrevious;
  end;
  fStaticRigidBodyPrevious:=nil;
  fStaticRigidBodyNext:=nil;
 end;

 if fDynamicRigidBodyIsOnList then begin
  fDynamicRigidBodyIsOnList:=false;
  if assigned(fDynamicRigidBodyPrevious) then begin
   fDynamicRigidBodyPrevious.fDynamicRigidBodyNext:=fDynamicRigidBodyNext;
  end else if fPhysics.fDynamicRigidBodyFirst=self then begin
   fPhysics.fDynamicRigidBodyFirst:=fDynamicRigidBodyNext;
  end;
  if assigned(fDynamicRigidBodyNext) then begin
   fDynamicRigidBodyNext.fDynamicRigidBodyPrevious:=fDynamicRigidBodyPrevious;
  end else if fPhysics.fDynamicRigidBodyLast=self then begin
   fPhysics.fDynamicRigidBodyLast:=fDynamicRigidBodyPrevious;
  end;
  fDynamicRigidBodyPrevious:=nil;
  fDynamicRigidBodyNext:=nil;
 end;

 if fKinematicRigidBodyIsOnList then begin
  fKinematicRigidBodyIsOnList:=false;
  if assigned(fKinematicRigidBodyPrevious) then begin
   fKinematicRigidBodyPrevious.fKinematicRigidBodyNext:=fKinematicRigidBodyNext;
  end else if fPhysics.fKinematicRigidBodyFirst=self then begin
   fPhysics.fKinematicRigidBodyFirst:=fKinematicRigidBodyNext;
  end;
  if assigned(fKinematicRigidBodyNext) then begin
   fKinematicRigidBodyNext.fKinematicRigidBodyPrevious:=fKinematicRigidBodyPrevious;
  end else if fPhysics.fKinematicRigidBodyLast=self then begin
   fPhysics.fKinematicRigidBodyLast:=fKinematicRigidBodyPrevious;
  end;
  fKinematicRigidBodyPrevious:=nil;
  fKinematicRigidBodyNext:=nil;
 end;

 SetLength(fIslandIndices,0);

 fRigidBodyType:=krbtUnknown;

 fGravityProperty.Free;

 inherited Destroy;
end;

function TKraftRigidBody.SetRigidBodyType(ARigidBodyType:TKraftRigidBodyType):TKraftRigidBody;
var Shape:TKraftShape;
begin

 if fRigidBodyType<>ARigidBodyType then begin

  case fRigidBodyType of
   krbtStatic:begin

    dec(fPhysics.fStaticRigidBodyCount);

    if fStaticRigidBodyIsOnList then begin
     fStaticRigidBodyIsOnList:=false;
     if assigned(fStaticRigidBodyPrevious) then begin
      fStaticRigidBodyPrevious.fStaticRigidBodyNext:=fStaticRigidBodyNext;
     end else if fPhysics.fStaticRigidBodyFirst=self then begin
      fPhysics.fStaticRigidBodyFirst:=fStaticRigidBodyNext;
     end;
     if assigned(fStaticRigidBodyNext) then begin
      fStaticRigidBodyNext.fStaticRigidBodyPrevious:=fStaticRigidBodyPrevious;
     end else if fPhysics.fStaticRigidBodyLast=self then begin
      fPhysics.fStaticRigidBodyLast:=fStaticRigidBodyPrevious;
     end;
     fStaticRigidBodyPrevious:=nil;
     fStaticRigidBodyNext:=nil;
    end;

   end;
   krbtDynamic:begin

    dec(fPhysics.fDynamicRigidBodyCount);

    if fDynamicRigidBodyIsOnList then begin
     fDynamicRigidBodyIsOnList:=false;
     if assigned(fDynamicRigidBodyPrevious) then begin
      fDynamicRigidBodyPrevious.fDynamicRigidBodyNext:=fDynamicRigidBodyNext;
     end else if fPhysics.fDynamicRigidBodyFirst=self then begin
      fPhysics.fDynamicRigidBodyFirst:=fDynamicRigidBodyNext;
     end;
     if assigned(fDynamicRigidBodyNext) then begin
      fDynamicRigidBodyNext.fDynamicRigidBodyPrevious:=fDynamicRigidBodyPrevious;
     end else if fPhysics.fDynamicRigidBodyLast=self then begin
      fPhysics.fDynamicRigidBodyLast:=fDynamicRigidBodyPrevious;
     end;
     fDynamicRigidBodyPrevious:=nil;
     fDynamicRigidBodyNext:=nil;
    end;

   end;
   krbtKinematic:begin

    dec(fPhysics.fKinematicRigidBodyCount);

    if fKinematicRigidBodyIsOnList then begin
     fKinematicRigidBodyIsOnList:=false;
     if assigned(fKinematicRigidBodyPrevious) then begin
      fKinematicRigidBodyPrevious.fKinematicRigidBodyNext:=fKinematicRigidBodyNext;
     end else if fPhysics.fKinematicRigidBodyFirst=self then begin
      fPhysics.fKinematicRigidBodyFirst:=fKinematicRigidBodyNext;
     end;
     if assigned(fKinematicRigidBodyNext) then begin
      fKinematicRigidBodyNext.fKinematicRigidBodyPrevious:=fKinematicRigidBodyPrevious;
     end else if fPhysics.fKinematicRigidBodyLast=self then begin
      fPhysics.fKinematicRigidBodyLast:=fKinematicRigidBodyPrevious;
     end;
     fKinematicRigidBodyPrevious:=nil;
     fKinematicRigidBodyNext:=nil;
    end;

   end;
  end;

  fRigidBodyType:=ARigidBodyType;

  case fRigidBodyType of
   krbtStatic:begin

    if assigned(fPhysics.fStaticRigidBodyLast) then begin
     fPhysics.fStaticRigidBodyLast.fStaticRigidBodyNext:=self;
     fStaticRigidBodyPrevious:=fPhysics.fStaticRigidBodyLast;
    end else begin
     fPhysics.fStaticRigidBodyFirst:=self;
     fStaticRigidBodyPrevious:=nil;
    end;
    fPhysics.fStaticRigidBodyLast:=self;
    fStaticRigidBodyNext:=nil;
    fStaticRigidBodyIsOnList:=true;

    inc(fPhysics.fStaticRigidBodyCount);

   end;
   krbtDynamic:begin

    if assigned(fPhysics.fDynamicRigidBodyLast) then begin
     fPhysics.fDynamicRigidBodyLast.fDynamicRigidBodyNext:=self;
     fDynamicRigidBodyPrevious:=fPhysics.fDynamicRigidBodyLast;
    end else begin
     fPhysics.fDynamicRigidBodyFirst:=self;
     fDynamicRigidBodyPrevious:=nil;
    end;
    fPhysics.fDynamicRigidBodyLast:=self;
    fDynamicRigidBodyNext:=nil;
    fDynamicRigidBodyIsOnList:=true;

    inc(fPhysics.fDynamicRigidBodyCount);

   end;
   krbtKinematic:begin

    if assigned(fPhysics.fKinematicRigidBodyLast) then begin
     fPhysics.fKinematicRigidBodyLast.fKinematicRigidBodyNext:=self;
     fKinematicRigidBodyPrevious:=fPhysics.fKinematicRigidBodyLast;
    end else begin
     fPhysics.fKinematicRigidBodyFirst:=self;
     fKinematicRigidBodyPrevious:=nil;
    end;
    fPhysics.fKinematicRigidBodyLast:=self;
    fKinematicRigidBodyNext:=nil;
    fKinematicRigidBodyIsOnList:=true;

    inc(fPhysics.fKinematicRigidBodyCount);

   end;
  end;

  Shape:=fShapeFirst;
  while assigned(Shape) do begin
   Shape.SynchronizeProxies;
   Shape:=Shape.fShapeNext;
  end;

 end;

 result:=self;
end;

function TKraftRigidBody.IsStatic:boolean;
begin
 result:=fRigidBodyType=krbtStatic;
end;

function TKraftRigidBody.IsDynamic:boolean;
begin
 result:=fRigidBodyType=krbtDynamic;
end;

function TKraftRigidBody.IsKinematic:boolean;
begin
 result:=fRigidBodyType=krbtKinematic;
end;

procedure TKraftRigidBody.SynchronizeTransform;
begin
 fWorldTransform:=QuaternionToMatrix4x4(fSweep.q);
 PKraftVector3(pointer(@fWorldTransform[3,0]))^.xyz:=Vector3Sub(fSweep.c,Vector3TermMatrixMulBasis(fSweep.LocalCenter,fWorldTransform)).xyz;
end;

procedure TKraftRigidBody.SynchronizeTransformIncludingShapes;
var Shape:TKraftShape;
begin
 SynchronizeTransform;
 Shape:=fShapeFirst;
 while assigned(Shape) do begin
  Shape.SynchronizeTransform;
  Shape:=Shape.fShapeNext;
 end;
end;

procedure TKraftRigidBody.StoreWorldTransform;
var Shape:TKraftShape;
begin
 Shape:=fShapeFirst;
 while assigned(Shape) do begin
  Shape.StoreWorldTransform;
  Shape:=Shape.fShapeNext;
 end;
end;

procedure TKraftRigidBody.InterpolateWorldTransform(const Alpha:TKraftScalar);
var Shape:TKraftShape;
begin
 Shape:=fShapeFirst;
 while assigned(Shape) do begin
  Shape.InterpolateWorldTransform(Alpha);
  Shape:=Shape.fShapeNext;
 end;
end;

procedure TKraftRigidBody.Advance(Alpha:TKraftScalar);
begin
 SweepAdvance(fSweep,Alpha);
 fSweep.c:=fSweep.c0;
 fSweep.q:=fSweep.q0;
 SynchronizeTransformIncludingShapes;
end;

procedure TKraftRigidBody.UpdateWorldInertiaTensor;
var Orientation:TKraftMatrix3x3;
begin
 if fRigidBodyType=krbtDynamic then begin
  Orientation:=QuaternionToMatrix3x3(fSweep.q0);
  fWorldInverseInertiaTensor:=Matrix3x3TermMulTranspose(Matrix3x3TermMul(Orientation,fBodyInverseInertiaTensor),Orientation);
//fWorldInverseInertiaTensor:=Matrix3x3TermMul(Matrix3x3TermMul(Orientation,fBodyInverseInertiaTensor),Matrix3x3TermTranspose(Orientation));
  Matrix3x3Inverse(fWorldInertiaTensor,fWorldInverseInertiaTensor);
 end;
end;

procedure TKraftRigidBody.Finish;
 procedure CalculateMassData; {$ifdef caninline}inline;{$endif}
 var Shape:TKraftShape;
     TempLocalCenter:TKraftVector3;
 begin

  FillChar(fBodyInertiaTensor,SizeOf(TKraftMatrix3x3),AnsiChar(#0));
  FillChar(fBodyInverseInertiaTensor,SizeOf(TKraftMatrix3x3),AnsiChar(#0));

  FillChar(fWorldInertiaTensor,SizeOf(TKraftMatrix3x3),AnsiChar(#0));
  FillChar(fWorldInverseInertiaTensor,SizeOf(TKraftMatrix3x3),AnsiChar(#0));

  fMass:=0.0;
  fInverseMass:=0.0;

  if fRigidBodyType<>krbtDynamic then begin

   fSweep.LocalCenter:=Vector3Origin;
   fSweep.c0.x:=fWorldTransform[3,0];
   fSweep.c0.y:=fWorldTransform[3,1];
   fSweep.c0.z:=fWorldTransform[3,2];
   fSweep.c:=fSweep.c0;

  end else begin

   TempLocalCenter:=Vector3Origin;

   Shape:=fShapeFirst;
   while assigned(Shape) do begin
    if Shape is TKraftShapePlane then begin
     raise EKraftShapeTypeOnlyForStaticRigidBody.Create('Plane shapes are allowed only at static rigidbodies');
    end else if Shape is TKraftShapeTriangle then begin
     raise EKraftShapeTypeOnlyForStaticRigidBody.Create('Triangle shapes are allowed only at static rigidbodies');
    end else if Shape is TKraftShapeMesh then begin
    raise EKraftShapeTypeOnlyForStaticRigidBody.Create('Mesh shapes are allowed only at static rigidbodies');
    end;
    if (ksfMass in Shape.fFlags) and (Shape.fDensity>EPSILON) then begin
     fMass:=fMass+Shape.fMassData.Mass;
     Matrix3x3Add(fBodyInertiaTensor,Shape.fMassData.Inertia);
     TempLocalCenter:=Vector3Add(TempLocalCenter,Vector3ScalarMul(Shape.fMassData.Center,Shape.fMassData.Mass));
    end;
    Shape:=Shape.fShapeNext;
   end;

   if fMass>EPSILON then begin

    TempLocalCenter.x:=TempLocalCenter.x/fMass;
    TempLocalCenter.y:=TempLocalCenter.y/fMass;
    TempLocalCenter.z:=TempLocalCenter.z/fMass;

    Matrix3x3Sub(fBodyInertiaTensor,InertiaTensorParallelAxisTheorem(TempLocalCenter,fMass));

{   fBodyInertiaTensor[1,0]:=fBodyInertiaTensor[0,1];
    fBodyInertiaTensor[2,0]:=fBodyInertiaTensor[0,2];
    fBodyInertiaTensor[2,1]:=fBodyInertiaTensor[1,2];{}

    if fForcedMass>EPSILON then begin
     Matrix3x3ScalarMul(fBodyInertiaTensor,fForcedMass/fMass);
     fMass:=fForcedMass;
    end;

    fInverseMass:=1.0/fMass;

    if krbfLockTranslationAxisX in fFlags then begin
     fLinearFactor.x:=0.0;
    end else begin
     fLinearFactor.x:=1.0;
    end;

    if krbfLockTranslationAxisY in fFlags then begin
     fLinearFactor.y:=0.0;
    end else begin
     fLinearFactor.y:=1.0;
    end;

    if krbfLockTranslationAxisZ in fFlags then begin
     fLinearFactor.z:=0.0;
    end else begin
     fLinearFactor.z:=1.0;
    end;

    Matrix3x3Inverse(fBodyInverseInertiaTensor,fBodyInertiaTensor);

    if (fFlags*[krbfLockRotationAxisX,
                krbfLockRotationAxisY,
                krbfLockRotationAxisZ])<>[] then begin

     if krbfLockRotationAxisX in fFlags then begin
      fBodyInverseInertiaTensor[0,0]:=0.0;
      fBodyInverseInertiaTensor[0,1]:=0.0;
      fBodyInverseInertiaTensor[0,2]:=0.0;
     end;

     if krbfLockRotationAxisY in fFlags then begin
      fBodyInverseInertiaTensor[1,0]:=0.0;
      fBodyInverseInertiaTensor[1,1]:=0.0;
      fBodyInverseInertiaTensor[1,2]:=0.0;
     end;

     if krbfLockRotationAxisZ in fFlags then begin
      fBodyInverseInertiaTensor[2,0]:=0.0;
      fBodyInverseInertiaTensor[2,1]:=0.0;
      fBodyInverseInertiaTensor[2,2]:=0.0;
     end;

     Matrix3x3Inverse(fBodyInertiaTensor,fBodyInverseInertiaTensor);

    end;

   end else begin

    fInverseMass:=1.0;

   end;

   fSweep.LocalCenter:=TempLocalCenter;
   fSweep.c0:=Vector3TermMatrixMul(TempLocalCenter,fWorldTransform);
   fSweep.c:=fSweep.c0;

  end;

 end;
var Shape:TKraftShape;
begin

 Shape:=fShapeFirst;
 while assigned(Shape) do begin
  Shape.Finish;
  Shape:=Shape.fShapeNext;
 end;

 CalculateMassData;

 SynchronizeTransform;

 SynchronizeProxies;

 UpdateWorldInertiaTensor;

end;

procedure TKraftRigidBody.SynchronizeProxies;
var Shape:TKraftShape;
    NewTransform:TKraftMatrix4x4;
begin
 NewTransform:=QuaternionToMatrix4x4(fSweep.q0);
 PKraftVector3(pointer(@NewTransform[3,0]))^.xyz:=Vector3Sub(fSweep.c0,Vector3TermMatrixMulBasis(fSweep.LocalCenter,NewTransform)).xyz;
 fWorldDisplacement:=Vector3Sub(PKraftVector3(pointer(@NewTransform[3,0]))^,PKraftVector3(pointer(@fWorldTransform[3,0]))^);
 Shape:=fShapeFirst;
 while assigned(Shape) do begin
  Shape.SynchronizeProxies;
  Shape:=Shape.fShapeNext;
 end;
end;

procedure TKraftRigidBody.Refilter;
var ContactPairEdge:PKraftContactPairEdge;
    ContactPair:PKraftContactPair;
begin
 ContactPairEdge:=fContactPairEdgeFirst;
 while assigned(ContactPairEdge) do begin
  ContactPair:=ContactPairEdge^.ContactPair;
  if assigned(ContactPair) then begin
   ContactPair^.Flags:=ContactPair^.Flags+[kcfFiltered];
   if assigned(ContactPair^.MeshContactPair) then begin
    ContactPair^.MeshContactPair.fFlags:=ContactPair^.MeshContactPair.fFlags+[kcfFiltered];
   end;
  end;
  ContactPairEdge:=ContactPairEdge^.Next;
 end;
end;

function TKraftRigidBody.CanCollideWith(OtherRigidBody:TKraftRigidBody):boolean;
var ConstraintEdge:PKraftConstraintEdge;
    Constraint:TKraftConstraint;
begin
 if (assigned(OtherRigidBody) and
    (self<>OtherRigidBody)) and // Don't collide with itself
    (((fRigidBodyType=krbtDynamic) or // Every collision must have at least one dynamic body involved
      (OtherRigidBody.fRigidBodyType=krbtDynamic)) and
     (((fCollisionGroups*OtherRigidBody.fCollideWithCollisionGroups)<>[]) or
      ((OtherRigidBody.fCollisionGroups*fCollideWithCollisionGroups)<>[])
     )
    ) then begin
  ConstraintEdge:=fConstraintEdgeFirst;
  while assigned(ConstraintEdge) do begin
   Constraint:=ConstraintEdge^.Constraint;     
   if (assigned(Constraint) and not (kcfCollideConnected in Constraint.fFlags)) and (ConstraintEdge^.OtherRigidBody=OtherRigidBody) then begin
    result:=false;
    exit;
   end;
   ConstraintEdge:=ConstraintEdge^.Next;
  end;
  result:=true;
 end else begin
  result:=false;
 end;
end;

procedure TKraftRigidBody.SetToAwake;
var ConstraintEdge:PKraftConstraintEdge;
begin
 if not (krbfAwake in fFlags) then begin
  Include(fFlags,krbfAwake);
  fSleepTime:=0.0;
  fWorldDisplacement:=Vector3Origin;
  ConstraintEdge:=fConstraintEdgeFirst;
  while assigned(ConstraintEdge) do begin
   if assigned(ConstraintEdge^.OtherRigidBody) and (ConstraintEdge^.OtherRigidBody<>self) then begin
    ConstraintEdge^.OtherRigidBody.SetToAwake;
   end;
   ConstraintEdge:=ConstraintEdge^.Next;
  end;
 end;
end;

procedure TKraftRigidBody.SetToSleep;
begin
 Exclude(fFlags,krbfAwake);
 fSleepTime:=0.0;
 fLinearVelocity:=Vector3Origin;
 fAngularVelocity:=Vector3Origin;
 fForce:=Vector3Origin;
 fTorque:=Vector3Origin;
 fWorldDisplacement:=Vector3Origin;
end;

procedure TKraftRigidBody.SetWorldTransformation(const AWorldTransformation:TKraftMatrix4x4);
begin
 fWorldTransform:=AWorldTransformation;
 UpdateWorldInertiaTensor;
 fSweep.c0:=Vector3TermMatrixMul(fSweep.LocalCenter,fWorldTransform);
 fSweep.c:=fSweep.c0;
 fSweep.q0:=QuaternionFromMatrix4x4(fWorldTransform);
 fSweep.q:=fSweep.q0;
 SynchronizeProxies;
 SetToAwake;
end;

procedure TKraftRigidBody.SetWorldPosition(const AWorldPosition:TKraftVector3);
begin
 PKraftVector3(pointer(@fWorldTransform[3,0]))^.xyz:=AWorldPosition.xyz;
 fSweep.c0:=Vector3Add(AWorldPosition,Vector3TermMatrixMulBasis(fSweep.LocalCenter,fWorldTransform));
 fSweep.c:=fSweep.c0;
 SynchronizeProxies;
 SetToAwake;
end;

procedure TKraftRigidBody.SetOrientation(const AOrientation:TKraftMatrix3x3);
begin
 PKraftVector3(pointer(@fWorldTransform[0,0]))^.xyz:=PKraftVector3(pointer(@AOrientation[0,0]))^.xyz;
 PKraftVector3(pointer(@fWorldTransform[1,0]))^.xyz:=PKraftVector3(pointer(@AOrientation[1,0]))^.xyz;
 PKraftVector3(pointer(@fWorldTransform[2,0]))^.xyz:=PKraftVector3(pointer(@AOrientation[2,0]))^.xyz;
 UpdateWorldInertiaTensor;
 fSweep.q0:=QuaternionFromMatrix3x3(AOrientation);
 fSweep.q:=fSweep.q0;
 SynchronizeProxies;
 SetToAwake;
end;

procedure TKraftRigidBody.SetOrientation(const x,y,z:TKraftScalar);
var Orientation:TKraftMatrix3x3;
begin
 Orientation:=Matrix3x3RotateZ(z);
 Matrix3x3Mul(Orientation,Matrix3x3RotateY(y));
 Matrix3x3Mul(Orientation,Matrix3x3RotateX(x));
 PKraftVector3(pointer(@fWorldTransform[0,0]))^.xyz:=PKraftVector3(pointer(@Orientation[0,0]))^.xyz;
 PKraftVector3(pointer(@fWorldTransform[1,0]))^.xyz:=PKraftVector3(pointer(@Orientation[1,0]))^.xyz;
 PKraftVector3(pointer(@fWorldTransform[2,0]))^.xyz:=PKraftVector3(pointer(@Orientation[2,0]))^.xyz;
 UpdateWorldInertiaTensor;
 fSweep.q0:=QuaternionFromMatrix3x3(Orientation);
 fSweep.q:=fSweep.q0;
 SynchronizeProxies;
 SetToAwake;
end;

procedure TKraftRigidBody.AddOrientation(const x,y,z:TKraftScalar);
var Orientation:TKraftMatrix3x3;
begin
 Orientation:=Matrix3x3RotateZ(z);
 Matrix3x3Mul(Orientation,Matrix3x3RotateY(y));
 Matrix3x3Mul(Orientation,Matrix3x3RotateX(x));
 Orientation:=Matrix3x3TermMul(Orientation,QuaternionToMatrix3x3(fSweep.q));
 PKraftVector3(pointer(@fWorldTransform[0,0]))^.xyz:=PKraftVector3(pointer(@Orientation[0,0]))^.xyz;
 PKraftVector3(pointer(@fWorldTransform[1,0]))^.xyz:=PKraftVector3(pointer(@Orientation[1,0]))^.xyz;
 PKraftVector3(pointer(@fWorldTransform[2,0]))^.xyz:=PKraftVector3(pointer(@Orientation[2,0]))^.xyz;
 UpdateWorldInertiaTensor;
 fSweep.q0:=QuaternionFromMatrix3x3(Orientation);
 fSweep.q:=fSweep.q0;
 SynchronizeProxies;
 SetToAwake;
end;

procedure TKraftRigidBody.LimitVelocities;
var Velocity:TKraftScalar;
begin
 if fMaximalLinearVelocity>EPSILON then begin
  Velocity:=Vector3Length(fLinearVelocity);
  if Velocity>fMaximalLinearVelocity then begin
   Vector3Scale(fLinearVelocity,fMaximalLinearVelocity/Velocity);
  end;
 end;
 if fMaximalAngularVelocity>EPSILON then begin
  Velocity:=Vector3Length(fAngularVelocity);
  if Velocity>fMaximalAngularVelocity then begin
   Vector3Scale(fAngularVelocity,fMaximalAngularVelocity/Velocity);
  end;
 end;
 if fPhysics.fMaximalLinearVelocity>EPSILON then begin
  Velocity:=Vector3Length(fLinearVelocity);
  if Velocity>fPhysics.fMaximalLinearVelocity then begin
   Vector3Scale(fLinearVelocity,fPhysics.fMaximalLinearVelocity/Velocity);
  end;
 end;
 if fPhysics.fMaximalAngularVelocity>EPSILON then begin
  Velocity:=Vector3Length(fAngularVelocity);
  if Velocity>fPhysics.fMaximalAngularVelocity then begin
   Vector3Scale(fAngularVelocity,fPhysics.fMaximalAngularVelocity/Velocity);
  end;
 end;
end;

procedure TKraftRigidBody.ApplyImpulseAtPosition(const Point,Impulse:TKraftVector3);
begin
 fLinearVelocity:=Vector3Add(fLinearVelocity,Vector3Mul(Impulse,Vector3ScalarMul(fLinearFactor,fInverseMass)));
 fAngularVelocity:=Vector3Add(fAngularVelocity,Vector3TermMatrixMul(Vector3Cross(Vector3Sub(Point,fSweep.c),Impulse),fWorldInverseInertiaTensor));
end;

procedure TKraftRigidBody.ApplyImpulseAtRelativePosition(const RelativePosition,Impulse:TKraftVector3);
begin
 fLinearVelocity:=Vector3Add(fLinearVelocity,Vector3Mul(Impulse,Vector3ScalarMul(fLinearFactor,fInverseMass)));
 fAngularVelocity:=Vector3Add(fAngularVelocity,Vector3TermMatrixMul(Vector3Cross(RelativePosition,Impulse),fWorldInverseInertiaTensor));
end;

procedure TKraftRigidBody.SetForceAtPosition(const AForce,APosition:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 SetWorldForce(AForce,AForceMode);
 SetWorldTorque(Vector3TermMatrixMul(Vector3Cross(Vector3Sub(APosition,fSweep.c),AForce),fWorldInverseInertiaTensor),AForceMode);
end;

procedure TKraftRigidBody.AddForceAtPosition(const AForce,APosition:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 AddWorldForce(AForce,AForceMode);
 AddWorldTorque(Vector3TermMatrixMul(Vector3Cross(Vector3Sub(APosition,fSweep.c),AForce),fWorldInverseInertiaTensor),AForceMode);
end;

procedure TKraftRigidBody.SetWorldForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 fForce:=Vector3Origin;
 AddWorldForce(AForce,AForceMode);
end;

procedure TKraftRigidBody.AddWorldForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 case AForceMode of
  kfmForce:begin
   // The unit of the Force parameter is applied to the rigidbody as mass*distance/time^2.
   fForce:=Vector3Add(fForce,AForce);
  end;
  kfmAcceleration:begin
   // The unit of the Force parameter is applied to the rigidbody as distance/time^2.
   fForce:=Vector3Add(fForce,Vector3ScalarMul(AForce,fMass));
  end;
  kfmImpulse:begin
   // The unit of the Force parameter is applied to the rigidbody as mass*distance/time.
   fForce:=Vector3Add(fForce,Vector3ScalarMul(AForce,fPhysics.fWorldInverseDeltaTime));
  end;
  kfmVelocity:begin
   // The unit of the Force parameter is applied to the rigidbody as distance/time.
   fForce:=Vector3Add(fForce,Vector3ScalarMul(AForce,fMass*fPhysics.fWorldInverseDeltaTime));
  end;
 end;
end;

procedure TKraftRigidBody.SetBodyForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 SetWorldForce(Vector3TermMatrixMulBasis(AForce,fWorldTransform),AForceMode);
end;

procedure TKraftRigidBody.AddBodyForce(const AForce:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 AddWorldForce(Vector3TermMatrixMulBasis(AForce,fWorldTransform),AForceMode);
end;

procedure TKraftRigidBody.SetWorldTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 fTorque:=Vector3Origin;
 AddWorldTorque(ATorque,AForceMode);
end;

procedure TKraftRigidBody.AddWorldTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 case AForceMode of
  kfmForce:begin
   // The unit of the Torque parameter is applied to the rigidbody as mass*distance/time^2.
   fTorque:=Vector3Add(fTorque,ATorque);
  end;
  kfmAcceleration:begin
   // The unit of the Torque parameter is applied to the rigidbody as distance/time^2.
   fTorque:=Vector3Add(fTorque,Vector3ScalarMul(ATorque,fMass));
  end;
  kfmImpulse:begin
   // The unit of the Torque parameter is applied to the rigidbody as mass*distance/time.
   fTorque:=Vector3Add(fTorque,Vector3ScalarMul(ATorque,fPhysics.fWorldInverseDeltaTime));
  end;
  kfmVelocity:begin
   // The unit of the Torque parameter is applied to the rigidbody as distance/time.
   fTorque:=Vector3Add(fTorque,Vector3ScalarMul(ATorque,fMass*fPhysics.fWorldInverseDeltaTime));
  end;
 end;
end;

procedure TKraftRigidBody.SetBodyTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 SetWorldTorque(Vector3TermMatrixMulBasis(ATorque,fWorldTransform),AForceMode);
end;

procedure TKraftRigidBody.AddBodyTorque(const ATorque:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 AddWorldTorque(Vector3TermMatrixMulBasis(ATorque,fWorldTransform),AForceMode);
end;

procedure TKraftRigidBody.SetWorldAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 fAngularVelocity:=Vector3Origin;
 AddWorldAngularVelocity(AAngularVelocity,AForceMode);
end;

procedure TKraftRigidBody.AddWorldAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 case AForceMode of
  kfmForce:begin
   // The unit of the Torque parameter is applied to the rigidbody as mass*distance/time^2.
   fAngularVelocity:=Vector3Add(fAngularVelocity,Vector3ScalarMul(AAngularVelocity,fInverseMass*fPhysics.fWorldDeltaTime));
  end;
  kfmAcceleration:begin
   // The unit of the Torque parameter is applied to the rigidbody as distance/time^2.
   fAngularVelocity:=Vector3Add(fAngularVelocity,Vector3ScalarMul(AAngularVelocity,fPhysics.fWorldDeltaTime));
  end;
  kfmImpulse:begin
   // The unit of the Torque parameter is applied to the rigidbody as mass*distance/time.
   fAngularVelocity:=Vector3Add(fAngularVelocity,Vector3ScalarMul(AAngularVelocity,fInverseMass));
  end;
  kfmVelocity:begin
   // The unit of the Torque parameter is applied to the rigidbody as distance/time.
   fAngularVelocity:=Vector3Add(fAngularVelocity,AAngularVelocity);
  end;
 end;
end;

procedure TKraftRigidBody.SetBodyAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 SetWorldAngularVelocity(Vector3TermMatrixMulBasis(AAngularVelocity,fWorldTransform),AForceMode);
end;

procedure TKraftRigidBody.AddBodyAngularVelocity(const AAngularVelocity:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 AddWorldAngularVelocity(Vector3TermMatrixMulBasis(AAngularVelocity,fWorldTransform),AForceMode);
end;

procedure TKraftRigidBody.SetWorldAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 SetWorldAngularVelocity(Vector3TermMatrixMul(AAngularMomentum,fWorldInverseInertiaTensor),AForceMode);
end;

procedure TKraftRigidBody.AddWorldAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 AddWorldAngularVelocity(Vector3TermMatrixMul(AAngularMomentum,fWorldInverseInertiaTensor),AForceMode);
end;

procedure TKraftRigidBody.SetBodyAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 SetBodyAngularVelocity(Vector3TermMatrixMul(AAngularMomentum,fBodyInverseInertiaTensor),AForceMode);
end;

procedure TKraftRigidBody.AddBodyAngularMomentum(const AAngularMomentum:TKraftVector3;const AForceMode:TKraftForceMode=kfmForce);
begin
 AddBodyAngularVelocity(Vector3TermMatrixMul(AAngularMomentum,fBodyInverseInertiaTensor),AForceMode);
end;

function TKraftRigidBody.GetAngularMomentum:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fAngularVelocity,fWorldInertiaTensor);
end;

procedure TKraftRigidBody.SetAngularMomentum(const NewAngularMomentum:TKraftVector3);
begin
 fAngularVelocity:=Vector3TermMatrixMul(NewAngularMomentum,fWorldInverseInertiaTensor);
end;

constructor TKraftConstraint.Create(const APhysics:TKraft);
var RigidBodyIndex:longint;
    RigidBody:TKraftRigidBody;
    ConstraintEdge:PKraftConstraintEdge;
    ContactPairEdge:PKraftContactPairEdge;
    ContactPair:PKraftContactPair;
begin

 inherited Create;

 fPhysics:=APhysics;

 fParent:=nil;

 fChildren:=nil;
 fCountChildren:=0;

 fUserData:=nil;

 fFlags:=(fFlags-[kcfVisited])+[kcfActive];

 if assigned(fPhysics.fConstraintLast) then begin
  fPhysics.fConstraintLast.fNext:=self;
  fPrevious:=fPhysics.fConstraintLast;
 end else begin
  fPhysics.fConstraintFirst:=self;
  fPrevious:=nil;
 end;
 fPhysics.fConstraintLast:=self;
 fNext:=nil;

 for RigidBodyIndex:=0 to 1 do begin
  ConstraintEdge:=@fConstraintEdges[RigidBodyIndex];
  RigidBody:=fRigidBodies[RigidBodyIndex];
  if assigned(RigidBody) then begin
   inc(RigidBody.fCountConstraints);
   if assigned(RigidBody.fConstraintEdgeLast) then begin
    RigidBody.fConstraintEdgeLast.Next:=ConstraintEdge;
    ConstraintEdge^.Previous:=RigidBody.fConstraintEdgeLast;
   end else begin
    RigidBody.fConstraintEdgeFirst:=ConstraintEdge;
    ConstraintEdge^.Previous:=nil;
   end;
   RigidBody.fConstraintEdgeLast:=ConstraintEdge;
   ConstraintEdge^.Next:=nil;
   ConstraintEdge^.Constraint:=self;
   ConstraintEdge^.OtherRigidBody:=fRigidBodies[(RigidBodyIndex+1) and 1];
  end else begin
   ConstraintEdge^.Previous:=nil;
   ConstraintEdge^.Next:=nil;
   ConstraintEdge^.Constraint:=nil;
   ConstraintEdge^.OtherRigidBody:=nil;
  end;
 end;

 // If the constraint prevents collisions, then flag any contacts for filtering.
 if not (kcfCollideConnected in fFlags) then begin
  for RigidBodyIndex:=0 to 1 do begin
   ConstraintEdge:=@fConstraintEdges[RigidBodyIndex];
   RigidBody:=fRigidBodies[RigidBodyIndex];
   if assigned(RigidBody) then begin
    ContactPairEdge:=RigidBody.fContactPairEdgeFirst;
    while assigned(ContactPairEdge) do begin
     ContactPair:=ContactPairEdge^.ContactPair;
     if assigned(ContactPair) then begin
      if ContactPairEdge^.OtherRigidBody=ConstraintEdge^.OtherRigidBody then begin
       // Flag the contact for filtering at the next time step (where either rigid body is awake).
       ContactPair^.Flags:=ContactPair^.Flags+[kcfFiltered];
       if assigned(ContactPair^.MeshContactPair) then begin
        ContactPair^.MeshContactPair.fFlags:=ContactPair^.MeshContactPair.fFlags+[kcfFiltered];
       end;
      end;
     end;
     ContactPairEdge:=ContactPairEdge^.Next;
    end;
   end;
  end;
 end;

 for RigidBodyIndex:=0 to 1 do begin
  RigidBody:=fRigidBodies[RigidBodyIndex];
  if assigned(RigidBody) then begin
   RigidBody.SetToAwake;
  end;
 end;

 fBreakThresholdForce:=MAX_SCALAR;

 fBreakThresholdTorque:=MAX_SCALAR;

 fOnBreak:=nil;

end;

destructor TKraftConstraint.Destroy;
var RigidBodyIndex,ConstraintIndex:longint;
    RigidBody:TKraftRigidBody;
    Constraint:TKraftConstraint;
    ConstraintEdge:PKraftConstraintEdge;
    ContactPairEdge:PKraftContactPairEdge;
    ContactPair:PKraftContactPair;
begin

 if assigned(fParent) then begin
  for ConstraintIndex:=0 to fParent.fCountChildren-1 do begin
   if fParent.fChildren[ConstraintIndex]=self then begin
    fParent.fChildren[ConstraintIndex]:=nil;
   end;
  end;
 end;

 if fCountChildren>0 then begin
  for ConstraintIndex:=0 to fCountChildren-1 do begin
   Constraint:=fChildren[ConstraintIndex];
   if assigned(Constraint) then begin
    fChildren[ConstraintIndex]:=nil;
    Constraint.Free;
   end;
  end;
  SetLength(fChildren,0);
 end;

 if assigned(fPrevious) then begin
  fPrevious.fNext:=fNext;
 end else if fPhysics.fConstraintFirst=self then begin
  fPhysics.fConstraintFirst:=fNext;
 end;
 if assigned(fNext) then begin
  fNext.fPrevious:=fPrevious;
 end else if fPhysics.fConstraintLast=self then begin
  fPhysics.fConstraintLast:=fPrevious;
 end;
 fPrevious:=nil;
 fNext:=nil;

 for RigidBodyIndex:=0 to 1 do begin
  RigidBody:=fRigidBodies[RigidBodyIndex];
  if assigned(RigidBody) then begin
   RigidBody.SetToAwake;
  end;
 end;

 for RigidBodyIndex:=0 to 1 do begin
  RigidBody:=fRigidBodies[RigidBodyIndex];
  if assigned(RigidBody) then begin
   dec(RigidBody.fCountConstraints);
   ConstraintEdge:=@fConstraintEdges[RigidBodyIndex];
   if assigned(ConstraintEdge^.Previous) then begin
    ConstraintEdge^.Previous.Next:=ConstraintEdge^.Next;
   end else if RigidBody.fConstraintEdgeFirst=ConstraintEdge then begin
    RigidBody.fConstraintEdgeFirst:=ConstraintEdge^.Next;
   end;
   if assigned(ConstraintEdge^.Next) then begin
    ConstraintEdge^.Next.Previous:=ConstraintEdge^.Previous;
   end else if RigidBody.fConstraintEdgeLast=ConstraintEdge then begin
    RigidBody.fConstraintEdgeLast:=ConstraintEdge^.Previous;
   end;
   ConstraintEdge^.Previous:=nil;
   ConstraintEdge^.Next:=nil;
  end;
 end;

 // If the constraint prevents collisions, then flag any contacts for filtering.
 if not (kcfCollideConnected in fFlags) then begin
  for RigidBodyIndex:=0 to 1 do begin
   ConstraintEdge:=@fConstraintEdges[RigidBodyIndex];
   RigidBody:=fRigidBodies[RigidBodyIndex];
   if assigned(RigidBody) then begin
    ContactPairEdge:=RigidBody.fContactPairEdgeFirst;
    while assigned(ContactPairEdge) do begin
     ContactPair:=ContactPairEdge^.ContactPair;
     if assigned(ContactPair) then begin
      if ContactPairEdge^.OtherRigidBody=ConstraintEdge^.OtherRigidBody then begin
       // Flag the contact for filtering at the next time step (where either rigid body is awake).
       ContactPair^.Flags:=ContactPair^.Flags+[kcfFiltered];
       if assigned(ContactPair^.MeshContactPair) then begin
        ContactPair^.MeshContactPair.fFlags:=ContactPair^.MeshContactPair.fFlags+[kcfFiltered];
       end;
      end;
     end;
     ContactPairEdge:=ContactPairEdge^.Next;
    end;
   end;
  end;
 end;

 inherited Destroy;
end;

procedure TKraftConstraint.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
begin
end;

procedure TKraftConstraint.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
begin
end;

function TKraftConstraint.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
begin
 result:=true;
end;

function TKraftConstraint.GetAnchorA:TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftConstraint.GetAnchorB:TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftConstraint.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftConstraint.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

constructor TKraftConstraintJointGrab.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const AWorldPoint:TKraftVector3;const AFrequencyHz:TKraftScalar=5.0;const ADampingRatio:TKraftScalar=0.7;const AMaximalForce:TKraftScalar=MAX_SCALAR;const ACollideConnected:boolean=false);
begin

 fWorldPoint:=AWorldPoint;

 fLocalAnchor:=Vector3TermMatrixMulInverted(AWorldPoint,ARigidBody.fWorldTransform);

 fFrequencyHz:=AFrequencyHz;
 fDampingRatio:=ADampingRatio;
 fAccumulatedImpulse:=Vector3Origin;
 fBeta:=0.0;
 fGamma:=0.0;

 fMaximalForce:=AMaximalForce;

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBody;
 fRigidBodies[1]:=nil;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointGrab.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftConstraintJointGrab.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
{$define TKraftConstraintJointGrabMassMatrixCodeVariantA}
var cA,vA,wA:PKraftVector3;
    qA:PKraftQuaternion;
    Omega,k,h,d:TKraftScalar;
    {$if defined(TKraftConstraintJointGrabMassMatrixCodeVariantC)}SkewSymmetricMatrix,{$ifend}MassMatrix:TKraftMatrix3x3;
begin

 fIslandIndex:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];

 fLocalCenter:=fRigidBodies[0].fSweep.LocalCenter;

 fInverseMass:=fRigidBodies[0].fInverseMass;

 fWorldInverseInertiaTensor:=fRigidBodies[0].fWorldInverseInertiaTensor;

 fSolverVelocity:=@Island.fSolver.fVelocities[fIslandIndex];

 fSolverPosition:=@Island.fSolver.fPositions[fIslandIndex];

 fSolverLinearFactor:=@Island.fSolver.fLinearFactors[fIslandIndex];

 cA:=@fSolverPosition^.Position;
 qA:=@fSolverPosition^.Orientation;
 vA:=@fSolverVelocity^.LinearVelocity;
 wA:=@fSolverVelocity^.AngularVelocity;

 // Compute the effective mass matrix
 if fInverseMass<>0.0 then begin
  fMass:=1.0/fInverseMass;
 end else begin
  fMass:=0.0;
 end;

 // fFrequency
 Omega:=pi2*fFrequencyHz;

 // Damping coefficient
 d:=2.0*fMass*fDampingRatio*Omega;

 // Spring stiffness
 k:=fMass*sqr(Omega);

 // Magic formulas
 h:=TimeStep.DeltaTime;
 fGamma:=h*(d+(h*k));
 if fGamma<>0.0 then begin
  fGamma:=1.0/fGamma;
 end else begin
  fGamma:=0.0;
 end;
 fBeta:=h*k*fGamma;

 fRelativePosition:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchor,fLocalCenter),qA^);

{$if defined(TKraftConstraintJointGrabMassMatrixCodeVariantA)}

 MassMatrix[0,0]:=((fInverseMass+fGamma)-
                   (fRelativePosition.z*((fWorldInverseInertiaTensor[1,2]*fRelativePosition.y)-(fWorldInverseInertiaTensor[1,1]*fRelativePosition.z))))+
                  (fRelativePosition.y*((fWorldInverseInertiaTensor[2,2]*fRelativePosition.y)-(fWorldInverseInertiaTensor[2,1]*fRelativePosition.z)));
 MassMatrix[0,1]:=(fRelativePosition.z*((fWorldInverseInertiaTensor[0,2]*fRelativePosition.y)-(fWorldInverseInertiaTensor[0,1]*fRelativePosition.z)))-
                  (fRelativePosition.x*((fWorldInverseInertiaTensor[2,2]*fRelativePosition.y)-(fWorldInverseInertiaTensor[2,1]*fRelativePosition.z)));
 MassMatrix[0,2]:=(fRelativePosition.x*((fWorldInverseInertiaTensor[1,2]*fRelativePosition.y)-(fWorldInverseInertiaTensor[1,1]*fRelativePosition.z)))-
                  (fRelativePosition.y*((fWorldInverseInertiaTensor[0,2]*fRelativePosition.y)-(fWorldInverseInertiaTensor[0,1]*fRelativePosition.z)));
 MassMatrix[1,0]:=(fRelativePosition.z*((fWorldInverseInertiaTensor[1,2]*fRelativePosition.x)-(fWorldInverseInertiaTensor[1,0]*fRelativePosition.z)))-
                  (fRelativePosition.y*((fWorldInverseInertiaTensor[2,2]*fRelativePosition.x)-(fWorldInverseInertiaTensor[2,0]*fRelativePosition.z)));
 MassMatrix[1,1]:=((fInverseMass+fGamma)-
                   (fRelativePosition.z*((fWorldInverseInertiaTensor[0,2]*fRelativePosition.x)-(fWorldInverseInertiaTensor[0,0]*fRelativePosition.z))))+
                  (fRelativePosition.x*((fWorldInverseInertiaTensor[2,2]*fRelativePosition.x)-(fWorldInverseInertiaTensor[2,0]*fRelativePosition.z)));
 MassMatrix[1,2]:=(fRelativePosition.y*((fWorldInverseInertiaTensor[0,2]*fRelativePosition.x)-(fWorldInverseInertiaTensor[0,0]*fRelativePosition.z)))-
                  (fRelativePosition.x*((fWorldInverseInertiaTensor[1,2]*fRelativePosition.x)-(fWorldInverseInertiaTensor[1,0]*fRelativePosition.z)));
 MassMatrix[2,0]:=(fRelativePosition.y*((fWorldInverseInertiaTensor[2,1]*fRelativePosition.x)-(fWorldInverseInertiaTensor[2,0]*fRelativePosition.y)))-
                  (fRelativePosition.z*((fWorldInverseInertiaTensor[1,1]*fRelativePosition.x)-(fWorldInverseInertiaTensor[1,0]*fRelativePosition.y)));
 MassMatrix[2,1]:=(fRelativePosition.z*((fWorldInverseInertiaTensor[0,1]*fRelativePosition.x)-(fWorldInverseInertiaTensor[0,0]*fRelativePosition.y)))-
                  (fRelativePosition.x*((fWorldInverseInertiaTensor[2,1]*fRelativePosition.x)-(fWorldInverseInertiaTensor[2,0]*fRelativePosition.y)));
 MassMatrix[2,2]:=((fInverseMass+fGamma)-
                   (fRelativePosition.y*((fWorldInverseInertiaTensor[0,1]*fRelativePosition.x)-(fWorldInverseInertiaTensor[0,0]*fRelativePosition.y))))+
                  (fRelativePosition.x*((fWorldInverseInertiaTensor[1,1]*fRelativePosition.x)-(fWorldInverseInertiaTensor[1,0]*fRelativePosition.y)));

{$elseif defined(TKraftConstraintJointGrabMassMatrixCodeVariantB)}

 MassMatrix[0,0]:=(((sqr(fRelativePosition.z)*fWorldInverseInertiaTensor[1,1])-
                    ((fRelativePosition.y*fRelativePosition.z)*(fWorldInverseInertiaTensor[1,2]+fWorldInverseInertiaTensor[2,1])))+
                   (sqr(fRelativePosition.y)*fWorldInverseInertiaTensor[2,2]))+
                  (fInverseMass+fGamma);
 MassMatrix[0,1]:=(((-(sqr(fRelativePosition.z)*fWorldInverseInertiaTensor[1,0]))+
                    ((fRelativePosition.x*fRelativePosition.z)*fWorldInverseInertiaTensor[1,2]))+
                   ((fRelativePosition.y*fRelativePosition.z)*fWorldInverseInertiaTensor[2,0]))-
                  ((fRelativePosition.x*fRelativePosition.y)*fWorldInverseInertiaTensor[2,2]);
 MassMatrix[0,2]:=((((fRelativePosition.y*fRelativePosition.z)*fWorldInverseInertiaTensor[1,0])-
                    ((fRelativePosition.x*fRelativePosition.z)*fWorldInverseInertiaTensor[1,1]))-
                   (sqr(fRelativePosition.y)*fWorldInverseInertiaTensor[2,0]))+
                  ((fRelativePosition.x*fRelativePosition.y)*fWorldInverseInertiaTensor[2,1]);
 MassMatrix[1,0]:=(((-(sqr(fRelativePosition.z)*fWorldInverseInertiaTensor[0,1]))+
                    ((fRelativePosition.y*fRelativePosition.z)*fWorldInverseInertiaTensor[0,2]))+
                   ((fRelativePosition.x*fRelativePosition.z)*fWorldInverseInertiaTensor[2,1]))-
                  ((fRelativePosition.x*fRelativePosition.y)*fWorldInverseInertiaTensor[2,2]);
 MassMatrix[1,1]:=(((sqr(fRelativePosition.z)*fWorldInverseInertiaTensor[0,0])-
                    ((fRelativePosition.x*fRelativePosition.z)*(fWorldInverseInertiaTensor[0,2]+fWorldInverseInertiaTensor[2,0])))+
                   (sqr(fRelativePosition.x)*fWorldInverseInertiaTensor[2,2]))+
                  (fInverseMass+fGamma);
 MassMatrix[1,2]:=(((-((fRelativePosition.y*fRelativePosition.z)*fWorldInverseInertiaTensor[0,0]))+
                    ((fRelativePosition.x*fRelativePosition.z)*fWorldInverseInertiaTensor[0,1]))+
                   ((fRelativePosition.x*fRelativePosition.y)*fWorldInverseInertiaTensor[2,0]))-
                  (sqr(fRelativePosition.x)*fWorldInverseInertiaTensor[2,1]);
 MassMatrix[2,0]:=((((fRelativePosition.y*fRelativePosition.z)*fWorldInverseInertiaTensor[0,1])-
                    (sqr(fRelativePosition.y)*fWorldInverseInertiaTensor[0,2]))-
                   ((fRelativePosition.x*fRelativePosition.z)*fWorldInverseInertiaTensor[1,1]))+
                  ((fRelativePosition.x*fRelativePosition.y)*fWorldInverseInertiaTensor[1,2]);
 MassMatrix[2,1]:=(((-((fRelativePosition.y*fRelativePosition.z)*fWorldInverseInertiaTensor[0,0]))+
                    ((fRelativePosition.x*fRelativePosition.y)*fWorldInverseInertiaTensor[0,2])+
                   ((fRelativePosition.x*fRelativePosition.z)*fWorldInverseInertiaTensor[1,0])))-
                  (sqr(fRelativePosition.x)*fWorldInverseInertiaTensor[1,2]);
 MassMatrix[2,2]:=(((sqr(fRelativePosition.y)*fWorldInverseInertiaTensor[0,0])-
                    ((fRelativePosition.x*fRelativePosition.y)*(fWorldInverseInertiaTensor[0,1]+fWorldInverseInertiaTensor[1,0])))+
                   (sqr(fRelativePosition.x)*fWorldInverseInertiaTensor[1,1]))+
                  (fInverseMass+fGamma);

{$else}

 MassMatrix[0,0]:=fInverseMass+fGamma;
 MassMatrix[0,1]:=0.0;
 MassMatrix[0,2]:=0.0;
{$ifdef SIMD}
 MassMatrix[0,3]:=0.0;
{$endif}
 MassMatrix[1,0]:=0.0;
 MassMatrix[1,1]:=MassMatrix[0,0];
 MassMatrix[1,2]:=0.0;
{$ifdef SIMD}
 MassMatrix[1,3]:=0.0;
{$endif}
 MassMatrix[2,0]:=0.0;
 MassMatrix[2,1]:=0.0;
 MassMatrix[2,2]:=MassMatrix[0,0];
{$ifdef SIMD}
 MassMatrix[2,3]:=0.0;
{$endif}

 SkewSymmetricMatrix:=GetSkewSymmetricMatrixPlus(fRelativePosition);

 Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrix,fWorldInverseInertiaTensor),SkewSymmetricMatrix));

{$ifend}

 Matrix3x3Inverse(fEffectiveMass,MassMatrix);

 fmC:=Vector3ScalarMul(Vector3Sub(Vector3Add(cA^,fRelativePosition),fWorldPoint),fBeta);

 Vector3Scale(wA^,0.98);

 if fPhysics.fWarmStarting then begin

  fAccumulatedImpulse:=Vector3ScalarMul(fAccumulatedImpulse,TimeStep.DeltaTimeRatio);

  Vector3DirectAdd(vA^,Vector3Mul(fAccumulatedImpulse,Vector3ScalarMul(fSolverLinearFactor^,fInverseMass)));
  Vector3DirectAdd(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePosition,fAccumulatedImpulse),fWorldInverseInertiaTensor));

 end else begin

  fAccumulatedImpulse:=Vector3Origin;

 end;

end;

procedure TKraftConstraintJointGrab.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA:PKraftVector3;
    Cdot,Impulse,OldImpulse:TKraftVector3;
    MaximalImpulse:TKraftScalar;
begin

 vA:=@fSolverVelocity^.LinearVelocity;
 wA:=@fSolverVelocity^.AngularVelocity;

 // Cdot = dot(u, v + cross(w, r))
 Cdot:=Vector3Add(vA^,Vector3Cross(wA^,fRelativePosition));

 Impulse:=Vector3TermMatrixMul(Vector3Neg(Vector3Add(Cdot,Vector3Add(fmC,Vector3ScalarMul(fAccumulatedImpulse,fGamma)))),fEffectiveMass);

 OldImpulse:=fAccumulatedImpulse;
 Vector3DirectAdd(fAccumulatedImpulse,Impulse);
 MaximalImpulse:=fMaximalForce*TimeStep.DeltaTime;
 if Vector3Length(fAccumulatedImpulse)>MaximalImpulse then begin
  Vector3Scale(fAccumulatedImpulse,MaximalImpulse/Vector3Length(fAccumulatedImpulse));
 end;
 Impulse:=Vector3Sub(fAccumulatedImpulse,OldImpulse);

 Vector3DirectAdd(vA^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactor^,fInverseMass)));
 Vector3DirectAdd(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePosition,Impulse),fWorldInverseInertiaTensor));

end;

function TKraftConstraintJointGrab.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
begin
 result:=true;
end;

function TKraftConstraintJointGrab.GetAnchor:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchor,fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointGrab.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fAccumulatedImpulse,InverseDeltaTime);
end;

function TKraftConstraintJointGrab.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftConstraintJointGrab.GetWorldPoint:TKraftVector3;
begin
 result:=fWorldPoint;
end;

function TKraftConstraintJointGrab.GetMaximalForce:TKraftScalar;
begin
 result:=fMaximalForce;
end;

procedure TKraftConstraintJointGrab.SetWorldPoint(AWorldPoint:TKraftVector3);
begin
 fWorldPoint:=AWorldPoint;
end;

procedure TKraftConstraintJointGrab.SetMaximalForce(AMaximalForce:TKraftScalar);
begin
 fMaximalForce:=AMaximalForce;
end;

constructor TKraftConstraintJointWorldPlaneDistance.Create(const APhysics:TKraft;const ARigidBody:TKraftRigidBody;const ALocalAnchorPoint:TKraftVector3;const AWorldPlane:TKraftPlane;const ADoubleSidedWorldPlane:boolean=true;const AWorldDistance:single=1.0;const ALimitBehavior:TKraftConstraintLimitBehavior=kclbLimitDistance;const AFrequencyHz:TKraftScalar=0.0;const ADampingRatio:TKraftScalar=0.0;const AInverseInertiaTensorRatio:TKraftScalar=1.0;const ACollideConnected:boolean=false);
begin

 fLocalAnchor:=ALocalAnchorPoint;

 fWorldPlane:=AWorldPlane;

 fWorldPoint:=Vector3ScalarMul(AWorldPlane.Normal,-AWorldPlane.Distance);

 fDoubleSidedWorldPlane:=ADoubleSidedWorldPlane;

 fWorldDistance:=AWorldDistance;

//fAnchorDistanceLength:=AWorldDistance;//Vector3Dist(fWorldPoint,Vector3TermMatrixMul(ALocalAnchorPoint,ARigidBody.fWorldTransform));

 fLimitBehavior:=ALimitBehavior;

 fFrequencyHz:=AFrequencyHz;
 fDampingRatio:=ADampingRatio;
 fInverseInertiaTensorRatio:=AInverseInertiaTensorRatio;
 fAccumulatedImpulse:=0.0;
 fGamma:=0.0;
 fBias:=0.0;

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBody;
 fRigidBodies[1]:=nil;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointWorldPlaneDistance.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftConstraintJointWorldPlaneDistance.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA:PKraftVector3;
    qA:PKraftQuaternion;
    crAu,P,AbsolutePosition,u:TKraftVector3;
    l,TotalInverseMass,C,Omega,k,h,d,BiasFactor:TKraftScalar;
begin

 fIslandIndex:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];

 fLocalCenter:=fRigidBodies[0].fSweep.LocalCenter;

 fInverseMass:=fRigidBodies[0].fInverseMass;

 fWorldInverseInertiaTensor:=Matrix3x3TermScalarMul(fRigidBodies[0].fWorldInverseInertiaTensor,fInverseInertiaTensorRatio);

 fSolverVelocity:=@Island.fSolver.fVelocities[fIslandIndex];

 fSolverPosition:=@Island.fSolver.fPositions[fIslandIndex];

 cA:=@fSolverPosition^.Position;
 qA:=@fSolverPosition^.Orientation;
 vA:=@fSolverVelocity^.LinearVelocity;
 wA:=@fSolverVelocity^.AngularVelocity;

 fRelativePosition:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchor,fLocalCenter),qA^);
 AbsolutePosition:=Vector3Add(cA^,fRelativePosition);

 fWorldPoint:=Vector3Add(AbsolutePosition,Vector3ScalarMul(fWorldPlane.Normal,-PlaneVectorDistance(fWorldPlane,AbsolutePosition)));
                         
 if fDoubleSidedWorldPlane then begin

  fmU:=Vector3Sub(fWorldPoint,AbsolutePosition);

  // Handle singularity
  l:=Vector3Length(fmU);
  if l>fPhysics.fLinearSlop then begin
   Vector3Scale(fmU,1.0/l);
  end else begin
   fmU:=Vector3Origin;
  end;

 end else begin

  l:=PlaneVectorDistance(fWorldPlane,AbsolutePosition);

  if abs(l)>fPhysics.fLinearSlop then begin
   fmU:=Vector3Neg(fWorldPlane.Normal);
  end else begin
   fmU:=Vector3Origin;
  end;

 end;

 fSoftConstraint:=fFrequencyHz>EPSILON;

 if fSoftConstraint then begin

  // No limit state skipping for soft contraints
  fSkip:=false;

 end else begin

  // Limit state skipping for non-soft contraints
  case fLimitBehavior of
   kclbLimitMinimumDistance:begin
    fSkip:=l>(fWorldDistance+fPhysics.fLinearSlop);
   end;
   kclbLimitMaximumDistance:begin
    fSkip:=l<(fWorldDistance-fPhysics.fLinearSlop);
   end;
   else begin
    fSkip:=false;
   end;
  end;

 end;

 if not fSkip then begin

  crAu:=Vector3Cross(fRelativePosition,fmU);

  TotalInverseMass:=fRigidBodies[0].fInverseMass+
                    Vector3Dot(Vector3TermMatrixMul(crAu,fWorldInverseInertiaTensor),crAu);

  // Compute the effective mass matrix
  if TotalInverseMass<>0.0 then begin
   fMass:=1.0/TotalInverseMass;
  end else begin
   fMass:=0.0;
  end;

  if fSoftConstraint then begin

   C:=l-fWorldDistance;

   // fFrequency
   Omega:=pi2*fFrequencyHz;

   // Damping coefficient
   d:=2.0*fMass*fDampingRatio*Omega;

   // Spring stiffness
   k:=fMass*sqr(Omega);

   // Magic formulas
   h:=TimeStep.DeltaTime;
   fGamma:=h*(d+(h*k));
   if fGamma<>0.0 then begin
    fGamma:=1.0/fGamma;
   end else begin
    fGamma:=0.0;
   end;
   fBias:=C*h*k*fGamma;

   TotalInverseMass:=TotalInverseMass+fGamma;

   if TotalInverseMass<>0.0 then begin
    fMass:=1.0/TotalInverseMass;
   end else begin
    fMass:=0.0;
   end;

  end else begin

   fGamma:=0.0;
   fBias:=0.0;

  end;

  if fPhysics.fConstraintPositionCorrectionMode=kpcmBaumgarte then begin
   AbsolutePosition:=Vector3Add(cA^,fRelativePosition);
   if fDoubleSidedWorldPlane then begin
    fWorldPoint:=Vector3Add(AbsolutePosition,Vector3ScalarMul(fWorldPlane.Normal,-PlaneVectorDistance(fWorldPlane,AbsolutePosition)));
    u:=Vector3Sub(fWorldPoint,AbsolutePosition);
    l:=Vector3LengthNormalize(u);
    C:=Min(Max(l-fWorldDistance,-fPhysics.fMaximalLinearCorrection),fPhysics.fMaximalLinearCorrection);
   end else begin
    C:=Min(Max(fWorldDistance-PlaneVectorDistance(fWorldPlane,AbsolutePosition),-fPhysics.fMaximalLinearCorrection),fPhysics.fMaximalLinearCorrection);
   end;
   BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;
   fBias:=fBias+(C*BiasFactor);
  end;

 end;

 if fPhysics.fWarmStarting and not fSkip then begin

  fAccumulatedImpulse:=fAccumulatedImpulse*TimeStep.DeltaTimeRatio;

  P:=Vector3ScalarMul(fmU,fAccumulatedImpulse);

  Vector3DirectSub(vA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactor^,fInverseMass)));
  Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePosition,P),fWorldInverseInertiaTensor));

 end else begin

  fAccumulatedImpulse:=0.0;

 end;


end;

procedure TKraftConstraintJointWorldPlaneDistance.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA:PKraftVector3;
    vpA,P:TKraftVector3;
    Cdot,Impulse,OldImpulse:TKraftScalar;
begin

 if not fSkip then begin

  vA:=@fSolverVelocity^.LinearVelocity;
  wA:=@fSolverVelocity^.AngularVelocity;

  // Cdot = dot(u, v + cross(w, r))
  vpA:=Vector3Add(vA^,Vector3Cross(wA^,fRelativePosition));
  Cdot:=Vector3Dot(fmU,Vector3Neg(vpA));

  Impulse:=-(fMass*((Cdot+fBias)+(fGamma*fAccumulatedImpulse)));

  if fSoftConstraint then begin
   case fLimitBehavior of
    kclbLimitMinimumDistance:begin
     OldImpulse:=fAccumulatedImpulse;
     fAccumulatedImpulse:=Max(0.0,fAccumulatedImpulse+Impulse);
     Impulse:=fAccumulatedImpulse-OldImpulse;
    end;
    kclbLimitMaximumDistance:begin
     OldImpulse:=fAccumulatedImpulse;
     fAccumulatedImpulse:=Min(0.0,fAccumulatedImpulse+Impulse);
     Impulse:=fAccumulatedImpulse-OldImpulse;
    end;
    else begin
     fAccumulatedImpulse:=fAccumulatedImpulse+Impulse;
    end;
   end;
  end else begin
   fAccumulatedImpulse:=fAccumulatedImpulse+Impulse;
  end;
  
  P:=Vector3ScalarMul(fmU,Impulse);

  Vector3DirectSub(vA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactor^,fInverseMass)));
  Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePosition,P),fWorldInverseInertiaTensor));

 end;

end;

function TKraftConstraintJointWorldPlaneDistance.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA:PKraftVector3;
    qA:PKraftQuaternion;
    rA,AbsolutePosition,u,P:TKraftVector3;
    l,C,Impulse:TKraftScalar;
begin

 if (fPhysics.fConstraintPositionCorrectionMode<>kpcmNonLinearGaussSeidel) or fSoftConstraint or fSkip then begin

  // There is no position correction for soft distance constraints or invalid limit states
  result:=true;

 end else begin

  cA:=@fSolverPosition^.Position;
  qA:=@fSolverPosition^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchor,fLocalCenter),qA^);

  AbsolutePosition:=Vector3Add(cA^,rA);

  if fDoubleSidedWorldPlane then begin

   fWorldPoint:=Vector3Add(AbsolutePosition,Vector3ScalarMul(fWorldPlane.Normal,-PlaneVectorDistance(fWorldPlane,AbsolutePosition)));

   u:=Vector3Sub(fWorldPoint,AbsolutePosition);

   l:=Vector3LengthNormalize(u);

   C:=Min(Max(l-fWorldDistance,-fPhysics.fMaximalLinearCorrection),fPhysics.fMaximalLinearCorrection);

   Impulse:=-(fMass*C);

   P:=Vector3ScalarMul(u,Impulse);

  end else begin

   C:=Min(Max(fWorldDistance-PlaneVectorDistance(fWorldPlane,AbsolutePosition),-fPhysics.fMaximalLinearCorrection),fPhysics.fMaximalLinearCorrection);

   Impulse:=-(fMass*C);

   P:=Vector3ScalarMul(fWorldPlane.Normal,Impulse);

  end;

  Vector3DirectSub(cA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactor^,fInverseMass)));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(P)),fWorldInverseInertiaTensor),1.0);

  result:=abs(C)<fPhysics.fLinearSlop;

 end;
end;

function TKraftConstraintJointWorldPlaneDistance.GetAnchor:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchor,fRigidBodies[0].fWorldTransform);
end;
                                         
function TKraftConstraintJointWorldPlaneDistance.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fmU,fAccumulatedImpulse*InverseDeltaTime);
end;

function TKraftConstraintJointWorldPlaneDistance.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftConstraintJointWorldPlaneDistance.GetWorldPoint:TKraftVector3;
begin
 result:=fWorldPoint;
end;

function TKraftConstraintJointWorldPlaneDistance.GetWorldPlane:TKraftPlane;
begin
 result:=fWorldPlane;
end;

procedure TKraftConstraintJointWorldPlaneDistance.SetWorldPlane(const AWorldPlane:TKraftPlane);
begin
 fWorldPlane:=AWorldPlane;
end;

function TKraftConstraintJointWorldPlaneDistance.GetWorldDistance:TKraftScalar;
begin
 result:=fWorldDistance;
end;

procedure TKraftConstraintJointWorldPlaneDistance.SetWorldDistance(const AWorldDistance:TKraftScalar);
begin
 fWorldDistance:=AWorldDistance;
end;

constructor TKraftConstraintJointDistance.Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const ALocalAnchorPointA,ALocalAnchorPointB:TKraftVector3;const AFrequencyHz:TKraftScalar=0.0;const ADampingRatio:TKraftScalar=0.0;const ACollideConnected:boolean=false);
begin

 fLocalAnchors[0]:=ALocalAnchorPointA;
 fLocalAnchors[1]:=ALocalAnchorPointB;

 fAnchorDistanceLength:=Vector3Dist(Vector3TermMatrixMul(ALocalAnchorPointB,ARigidBodyB.fWorldTransform),Vector3TermMatrixMul(ALocalAnchorPointA,ARigidBodyA.fWorldTransform));
 fFrequencyHz:=AFrequencyHz;
 fDampingRatio:=ADampingRatio;
 fAccumulatedImpulse:=0.0;
 fGamma:=0.0;
 fBias:=0.0;

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointDistance.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftConstraintJointDistance.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA,cB,vB,wB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    crAu,crBu,P,u:TKraftVector3;
    l,fInverseMass,C,Omega,k,h,d,BiasFactor:TKraftScalar;
begin

 fIslandIndices[0]:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];
 fIslandIndices[1]:=fRigidBodies[1].fIslandIndices[Island.fIslandIndex];

 fLocalCenters[0]:=fRigidBodies[0].fSweep.LocalCenter;
 fLocalCenters[1]:=fRigidBodies[1].fSweep.LocalCenter;

 fInverseMasses[0]:=fRigidBodies[0].fInverseMass;
 fInverseMasses[1]:=fRigidBodies[1].fInverseMass;

 fWorldInverseInertiaTensors[0]:=fRigidBodies[0].fWorldInverseInertiaTensor;
 fWorldInverseInertiaTensors[1]:=fRigidBodies[1].fWorldInverseInertiaTensor;

 fSolverVelocities[0]:=@Island.fSolver.fVelocities[fIslandIndices[0]];
 fSolverVelocities[1]:=@Island.fSolver.fVelocities[fIslandIndices[1]];

 fSolverPositions[0]:=@Island.fSolver.fPositions[fIslandIndices[0]];
 fSolverPositions[1]:=@Island.fSolver.fPositions[fIslandIndices[1]];

 fSolverLinearFactors[0]:=@Island.fSolver.fLinearFactors[fIslandIndices[0]];
 fSolverLinearFactors[1]:=@Island.fSolver.fLinearFactors[fIslandIndices[1]];

 cA:=@fSolverPositions[0]^.Position;
 qA:=@fSolverPositions[0]^.Orientation;
 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 cB:=@fSolverPositions[1]^.Position;
 qB:=@fSolverPositions[1]^.Orientation;
 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 fRelativePositions[0]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
 fRelativePositions[1]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);
 fmU:=Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0]));

 // Handle singularity
 l:=Vector3Length(fmU);
 if l>fPhysics.fLinearSlop then begin
  Vector3Scale(fmU,1.0/l);
 end else begin
  fmU:=Vector3Origin;
 end;

 crAu:=Vector3Cross(fRelativePositions[0],fmU);
 crBu:=Vector3Cross(fRelativePositions[1],fmU);

 fInverseMass:=fRigidBodies[0].fInverseMass+
               fRigidBodies[1].fInverseMass+
               Vector3Dot(Vector3TermMatrixMul(crAu,fWorldInverseInertiaTensors[0]),crAu)+
               Vector3Dot(Vector3TermMatrixMul(crBu,fWorldInverseInertiaTensors[1]),crBu);

 // Compute the effective mass matrix
 if fInverseMass<>0.0 then begin
  fMass:=1.0/fInverseMass;
 end else begin
  fMass:=0.0;
 end;

 if fFrequencyHz>EPSILON then begin

  C:=l-fAnchorDistanceLength;

  // fFrequency
  Omega:=pi2*fFrequencyHz;

  // Damping coefficient
  d:=2.0*fMass*fDampingRatio*Omega;

  // Spring stiffness
	k:=fMass*sqr(Omega);

  // Magic formulas
  h:=TimeStep.DeltaTime;
  fGamma:=h*(d+(h*k));
  if fGamma<>0.0 then begin
   fGamma:=1.0/fGamma;
  end else begin
   fGamma:=0.0;
  end;
	fBias:=C*h*k*fGamma;

  fInverseMass:=fInverseMass+fGamma;

  if fInverseMass<>0.0 then begin
   fMass:=1.0/fInverseMass;
  end else begin
   fMass:=0.0;
  end;

 end else begin

  fGamma:=0.0;
  fBias:=0.0;

 end;

 if fPhysics.fConstraintPositionCorrectionMode=kpcmBaumgarte then begin
  u:=Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0]));
  l:=Vector3LengthNormalize(u);
  C:=Min(Max(l-fAnchorDistanceLength,-fPhysics.fMaximalLinearCorrection),fPhysics.fMaximalLinearCorrection);
  BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;
  fBias:=fBias+(C*BiasFactor);
 end;

 if fPhysics.fWarmStarting then begin

  fAccumulatedImpulse:=fAccumulatedImpulse*TimeStep.DeltaTimeRatio;

  P:=Vector3ScalarMul(fmU,fAccumulatedImpulse);

  Vector3DirectSub(vA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],P),fWorldInverseInertiaTensors[0]));

  Vector3DirectAdd(vB^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],P),fWorldInverseInertiaTensors[1]));

 end else begin

  fAccumulatedImpulse:=0.0;

 end;

end;

procedure TKraftConstraintJointDistance.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA,vB,wB:PKraftVector3;
    vpA,vpB,P:TKraftVector3;
    Cdot,Impulse:TKraftScalar;
begin

 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 // Cdot = dot(u, v + cross(w, r))
 vpA:=Vector3Add(vA^,Vector3Cross(wA^,fRelativePositions[0]));
 vpB:=Vector3Add(vB^,Vector3Cross(wB^,fRelativePositions[1]));
 Cdot:=Vector3Dot(fmU,Vector3Sub(vpB,vpA));

 Impulse:=-(fMass*((Cdot+fBias)+(fGamma*fAccumulatedImpulse)));
 fAccumulatedImpulse:=fAccumulatedImpulse+Impulse;

 P:=Vector3ScalarMul(fmU,Impulse);

 Vector3DirectSub(vA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
 Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],P),fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(vB^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],P),fWorldInverseInertiaTensors[1]));

end;

function TKraftConstraintJointDistance.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA,cB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    rA,rB,u,P:TKraftVector3;
    l,C,Impulse:TKraftScalar;
begin

 if (fPhysics.fConstraintPositionCorrectionMode<>kpcmNonLinearGaussSeidel) or (fFrequencyHz>EPSILON) then begin

  // There is no position correction for soft distance constraints
  result:=true;

 end else begin

  cA:=@fSolverPositions[0]^.Position;
  qA:=@fSolverPositions[0]^.Orientation;

  cB:=@fSolverPositions[1]^.Position;
  qB:=@fSolverPositions[1]^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
  rB:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);
  u:=Vector3Sub(Vector3Add(cB^,rB),Vector3Add(cA^,rA));

  l:=Vector3LengthNormalize(u);
  C:=Min(Max(l-fAnchorDistanceLength,-fPhysics.fMaximalLinearCorrection),fPhysics.fMaximalLinearCorrection);

  Impulse:=-(fMass*C);

  P:=Vector3ScalarMul(u,Impulse);

  Vector3DirectSub(cA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(P)),fWorldInverseInertiaTensors[0]),1.0);

  Vector3DirectAdd(cB^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Vector3Cross(rB,P),fWorldInverseInertiaTensors[1]),1.0);

  result:=abs(C)<fPhysics.fLinearSlop;

 end;

end;

function TKraftConstraintJointDistance.GetAnchorA:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointDistance.GetAnchorB:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform);
end;

function TKraftConstraintJointDistance.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fmU,fAccumulatedImpulse*InverseDeltaTime);
end;

function TKraftConstraintJointDistance.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

constructor TKraftConstraintJointRope.Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const ALocalAnchorPointA,ALocalAnchorPointB:TKraftVector3;const AMaximalLength:TKraftScalar=1.0;const ACollideConnected:boolean=false);
begin

 fLocalAnchors[0]:=ALocalAnchorPointA;
 fLocalAnchors[1]:=ALocalAnchorPointB;

 fMaximalLength:=AMaximalLength;

 fAccumulatedImpulse:=0.0;

 fLimitState:=kclsInactiveLimit;

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointRope.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftConstraintJointRope.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA,cB,vB,wB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    crAu,crBu,P,u:TKraftVector3;
    l,fInverseMass,C,BiasFactor:TKraftScalar;
begin

 fIslandIndices[0]:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];
 fIslandIndices[1]:=fRigidBodies[1].fIslandIndices[Island.fIslandIndex];

 fLocalCenters[0]:=fRigidBodies[0].fSweep.LocalCenter;
 fLocalCenters[1]:=fRigidBodies[1].fSweep.LocalCenter;

 fInverseMasses[0]:=fRigidBodies[0].fInverseMass;
 fInverseMasses[1]:=fRigidBodies[1].fInverseMass;

 fWorldInverseInertiaTensors[0]:=fRigidBodies[0].fWorldInverseInertiaTensor;
 fWorldInverseInertiaTensors[1]:=fRigidBodies[1].fWorldInverseInertiaTensor;

 fSolverVelocities[0]:=@Island.fSolver.fVelocities[fIslandIndices[0]];
 fSolverVelocities[1]:=@Island.fSolver.fVelocities[fIslandIndices[1]];

 fSolverPositions[0]:=@Island.fSolver.fPositions[fIslandIndices[0]];
 fSolverPositions[1]:=@Island.fSolver.fPositions[fIslandIndices[1]];

 fSolverLinearFactors[0]:=@Island.fSolver.fLinearFactors[fIslandIndices[0]];
 fSolverLinearFactors[1]:=@Island.fSolver.fLinearFactors[fIslandIndices[1]];

 cA:=@fSolverPositions[0]^.Position;
 qA:=@fSolverPositions[0]^.Orientation;
 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 cB:=@fSolverPositions[1]^.Position;
 qB:=@fSolverPositions[1]^.Orientation;
 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 fRelativePositions[0]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
 fRelativePositions[1]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);
 fmU:=Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0]));

 fCurrentLength:=Vector3Length(fmU);

 C:=fCurrentLength-fMaximalLength;
 if C>0.0 then begin
  fLimitState:=kclsAtUpperLimit;
 end else begin
  fLimitState:=kclsInactiveLimit;
 end;

 // Handle singularity
 if fCurrentLength>fPhysics.fLinearSlop then begin

  Vector3Scale(fmU,1.0/fCurrentLength);

  crAu:=Vector3Cross(fRelativePositions[0],fmU);
  crBu:=Vector3Cross(fRelativePositions[1],fmU);

  fInverseMass:=fRigidBodies[0].fInverseMass+
                fRigidBodies[1].fInverseMass+
                Vector3Dot(Vector3TermMatrixMul(crAu,fWorldInverseInertiaTensors[0]),crAu)+
                Vector3Dot(Vector3TermMatrixMul(crBu,fWorldInverseInertiaTensors[1]),crBu);

  // Compute the effective mass matrix
  if fInverseMass<>0.0 then begin
   fMass:=1.0/fInverseMass;
  end else begin
   fMass:=0.0;
  end;

  if fPhysics.fWarmStarting then begin

   fAccumulatedImpulse:=fAccumulatedImpulse*TimeStep.DeltaTimeRatio;

   P:=Vector3ScalarMul(fmU,fAccumulatedImpulse);

   Vector3DirectSub(vA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
   Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],P),fWorldInverseInertiaTensors[0]));

   Vector3DirectAdd(vB^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
   Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],P),fWorldInverseInertiaTensors[1]));

  end else begin

   fAccumulatedImpulse:=0.0;

  end;
 
 end else begin

  fmU:=Vector3Origin;
  fMass:=0.0;
  fAccumulatedImpulse:=0.0;

 end;

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  fBias:=0.0;

 end else begin

  BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;

  u:=Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0]));

  l:=Vector3LengthNormalize(u);
  C:=Min(Max(l-fMaximalLength,0.0),fPhysics.fMaximalLinearCorrection);

  fBias:=C*BiasFactor;

 end;

end;

procedure TKraftConstraintJointRope.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA,vB,wB:PKraftVector3;
    vpA,vpB,P:TKraftVector3;
    C,Cdot,Impulse,OldImpulse:TKraftScalar;
begin

 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 // Cdot = dot(u, v + cross(w, r))
 vpA:=Vector3Add(vA^,Vector3Cross(wA^,fRelativePositions[0]));
 vpB:=Vector3Add(vB^,Vector3Cross(wB^,fRelativePositions[1]));
 C:=fCurrentLength-fMaximalLength;
 Cdot:=Vector3Dot(fmU,Vector3Sub(vpB,vpA));

 // Predictive constraint
 if C<0.0 then begin
  Cdot:=Cdot+(C*TimeStep.InverseDeltaTime);
 end;

 Impulse:=-(fMass*(Cdot+fBias));
 OldImpulse:=fAccumulatedImpulse;
 fAccumulatedImpulse:=Min(0.0,fAccumulatedImpulse+Impulse);
 Impulse:=fAccumulatedImpulse-OldImpulse;

 P:=Vector3ScalarMul(fmU,Impulse);

 Vector3DirectSub(vA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
 Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],P),fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(vB^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],P),fWorldInverseInertiaTensors[1]));

end;

function TKraftConstraintJointRope.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA,cB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    rA,rB,u,P:TKraftVector3;
    Len,C,Impulse:TKraftScalar;
begin

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  cA:=@fSolverPositions[0]^.Position;
  qA:=@fSolverPositions[0]^.Orientation;

  cB:=@fSolverPositions[1]^.Position;
  qB:=@fSolverPositions[1]^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
  rB:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);
  u:=Vector3Sub(Vector3Add(cB^,rB),Vector3Add(cA^,rA));

  Len:=Vector3LengthNormalize(u);
  C:=Min(Max(Len-fMaximalLength,0.0),fPhysics.fMaximalLinearCorrection);

  Impulse:=-(fMass*C);

  P:=Vector3ScalarMul(u,Impulse);

  Vector3DirectSub(cA^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(P)),fWorldInverseInertiaTensors[0]),1.0);

  Vector3DirectAdd(cB^,Vector3Mul(P,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Vector3Cross(rB,P),fWorldInverseInertiaTensors[1]),1.0);

  result:=(Len-fMaximalLength)<fPhysics.fLinearSlop;

 end else begin

  result:=true;

 end;

end;

function TKraftConstraintJointRope.GetAnchorA:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointRope.GetAnchorB:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform);
end;

function TKraftConstraintJointRope.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fmU,fAccumulatedImpulse*InverseDeltaTime);
end;

function TKraftConstraintJointRope.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

constructor TKraftConstraintJointPulley.Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AWorldGroundAnchorA,AWorldGroundAnchorB,AWorldAnchorPointA,AWorldAnchorPointB:TKraftVector3;const ARatio:TKraftScalar=1.0;const ACollideConnected:boolean=false);
begin

 fGroundAnchors[0]:=AWorldGroundAnchorA;
 fGroundAnchors[1]:=AWorldGroundAnchorB;

 fLocalAnchors[0]:=Vector3TermMatrixMulInverted(AWorldAnchorPointA,ARigidBodyA.fWorldTransform);
 fLocalAnchors[1]:=Vector3TermMatrixMulInverted(AWorldAnchorPointB,ARigidBodyB.fWorldTransform);

 fLengths[0]:=Vector3Dist(AWorldAnchorPointA,AWorldGroundAnchorA);
 fLengths[1]:=Vector3Dist(AWorldAnchorPointB,AWorldGroundAnchorB);

 fRatio:=ARatio;

 fConstant:=fLengths[0]+(fLengths[1]*fRatio);

 fAccumulatedImpulse:=0.0;

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointPulley.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftConstraintJointPulley.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA,cB,vB,wB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    ruA,ruB,PA,PB:TKraftVector3;
    LengthA,LengthB,mA,mB,fInverseMass,BiasFactor,C:TKraftScalar;
begin

 fIslandIndices[0]:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];
 fIslandIndices[1]:=fRigidBodies[1].fIslandIndices[Island.fIslandIndex];

 fLocalCenters[0]:=fRigidBodies[0].fSweep.LocalCenter;
 fLocalCenters[1]:=fRigidBodies[1].fSweep.LocalCenter;

 fInverseMasses[0]:=fRigidBodies[0].fInverseMass;
 fInverseMasses[1]:=fRigidBodies[1].fInverseMass;

 fWorldInverseInertiaTensors[0]:=fRigidBodies[0].fWorldInverseInertiaTensor;
 fWorldInverseInertiaTensors[1]:=fRigidBodies[1].fWorldInverseInertiaTensor;

 fSolverVelocities[0]:=@Island.fSolver.fVelocities[fIslandIndices[0]];
 fSolverVelocities[1]:=@Island.fSolver.fVelocities[fIslandIndices[1]];

 fSolverPositions[0]:=@Island.fSolver.fPositions[fIslandIndices[0]];
 fSolverPositions[1]:=@Island.fSolver.fPositions[fIslandIndices[1]];

 fSolverLinearFactors[0]:=@Island.fSolver.fLinearFactors[fIslandIndices[0]];
 fSolverLinearFactors[1]:=@Island.fSolver.fLinearFactors[fIslandIndices[1]];

 cA:=@fSolverPositions[0]^.Position;
 qA:=@fSolverPositions[0]^.Orientation;
 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 cB:=@fSolverPositions[1]^.Position;
 qB:=@fSolverPositions[1]^.Orientation;
 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 fRelativePositions[0]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
 fRelativePositions[1]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

 fmU[0]:=Vector3Add(cA^,fRelativePositions[0]);
 fmU[1]:=Vector3Add(cB^,fRelativePositions[1]);

 LengthA:=Vector3Length(fmU[0]);
 LengthB:=Vector3Length(fmU[1]);

 if LengthA>(10.0*fPhysics.fLinearSlop) then begin
  Vector3Scale(fmU[0],1.0/LengthA);
 end else begin
  fmU[0]:=Vector3Origin;
 end;

 if LengthB>(10.0*fPhysics.fLinearSlop) then begin
  Vector3Scale(fmU[1],1.0/LengthB);
 end else begin
  fmU[1]:=Vector3Origin;
 end;

 ruA:=Vector3Cross(fRelativePositions[0],fmU[0]);
 ruB:=Vector3Cross(fRelativePositions[1],fmU[1]);

 mA:=fRigidBodies[0].fInverseMass+Vector3Dot(Vector3TermMatrixMul(ruA,fWorldInverseInertiaTensors[0]),ruA);
 mB:=fRigidBodies[1].fInverseMass+Vector3Dot(Vector3TermMatrixMul(ruB,fWorldInverseInertiaTensors[1]),ruB);

 fInverseMass:=mA+(mB*sqr(fRatio));

 // Compute the effective mass matrix
 if fInverseMass<>0.0 then begin
  fMass:=1.0/fInverseMass;
 end else begin
  fMass:=0.0;
 end;

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  fBias:=0.0;

 end else begin

  BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;

  C:=fConstant-(LengthA+(LengthB*fRatio));

  fBias:=C*BiasFactor;

 end;

 if fPhysics.fWarmStarting then begin

  fAccumulatedImpulse:=fAccumulatedImpulse*TimeStep.DeltaTimeRatio;

  PA:=Vector3ScalarMul(fmU[0],-fAccumulatedImpulse);
  PB:=Vector3ScalarMul(fmU[1],-(fAccumulatedImpulse*fRatio));

  Vector3DirectAdd(vA^,Vector3Mul(PA,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  Vector3DirectAdd(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],PA),fWorldInverseInertiaTensors[0]));

  Vector3DirectAdd(vB^,Vector3Mul(PB,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],PB),fWorldInverseInertiaTensors[1]));

 end else begin

  fAccumulatedImpulse:=0.0;

 end;

end;

procedure TKraftConstraintJointPulley.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA,vB,wB:PKraftVector3;
    vpA,vpB,PA,PB:TKraftVector3;
    Cdot,Impulse:TKraftScalar;
begin

 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 vpA:=Vector3Add(vA^,Vector3Cross(wA^,fRelativePositions[0]));
 vpB:=Vector3Add(vB^,Vector3Cross(wB^,fRelativePositions[1]));

 Cdot:=(-(Vector3Dot(fmU[0],vpA)))-(Vector3Dot(fmU[1],vpB)*fRatio);
 Impulse:=-(fMass*(Cdot+fBias));
 fAccumulatedImpulse:=fAccumulatedImpulse+Impulse;

 PA:=Vector3ScalarMul(fmU[0],-Impulse);
 PB:=Vector3ScalarMul(fmU[1],-(Impulse*fRatio));

 Vector3DirectAdd(vA^,Vector3Mul(PA,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
 Vector3DirectAdd(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],PA),fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(vB^,Vector3Mul(PB,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],PB),fWorldInverseInertiaTensors[1]));

end;

function TKraftConstraintJointPulley.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA,cB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    rA,rB,uA,uB,ruA,ruB,PA,PB:TKraftVector3;
    LengthA,LengthB,mA,mB,fInverseMass,Mass,C,LinearError,Impulse:TKraftScalar;
begin

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  cA:=@fSolverPositions[0]^.Position;
  qA:=@fSolverPositions[0]^.Orientation;

  cB:=@fSolverPositions[1]^.Position;
  qB:=@fSolverPositions[1]^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
  rB:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

  // Get the pulley axes
  uA:=Vector3Add(cA^,rA);
  uB:=Vector3Add(cB^,rB);

  LengthA:=Vector3Length(uA);
  LengthB:=Vector3Length(uB);

  if LengthA>(10.0*fPhysics.fLinearSlop) then begin
   Vector3Scale(uA,1.0/LengthA);
  end else begin
   uA:=Vector3Origin;
  end;

  if LengthB>(10.0*fPhysics.fLinearSlop) then begin
   Vector3Scale(uB,1.0/LengthB);
  end else begin
   uB:=Vector3Origin;
  end;

  ruA:=Vector3Cross(fRelativePositions[0],fmU[0]);
  ruB:=Vector3Cross(fRelativePositions[1],fmU[1]);

  mA:=fRigidBodies[0].fInverseMass+Vector3Dot(Vector3TermMatrixMul(ruA,fWorldInverseInertiaTensors[0]),ruA);
  mB:=fRigidBodies[1].fInverseMass+Vector3Dot(Vector3TermMatrixMul(ruB,fWorldInverseInertiaTensors[1]),ruB);

  fInverseMass:=mA+(mB*sqr(fRatio));

  // Compute the effective mass matrix
  if fInverseMass<>0.0 then begin
   Mass:=1.0/fInverseMass;
  end else begin
   Mass:=0.0;
  end;

  C:=fConstant-(LengthA+(LengthB*fRatio));

  LinearError:=abs(c);

  Impulse:=-(Mass*C);

  PA:=Vector3ScalarMul(uA,-Impulse);
  PB:=Vector3ScalarMul(uB,-(Impulse*fRatio));

  Vector3DirectAdd(cA^,Vector3Mul(PA,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Cross(rA,PA),fWorldInverseInertiaTensors[0]),1.0);

  Vector3DirectAdd(cB^,Vector3Mul(PB,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Vector3Cross(rB,PB),fWorldInverseInertiaTensors[1]),1.0);

  result:=LinearError<fPhysics.fLinearSlop;

 end else begin

  result:=true;

 end;
end;

function TKraftConstraintJointPulley.GetAnchorA:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointPulley.GetAnchorB:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform);
end;

function TKraftConstraintJointPulley.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fmU[1],fAccumulatedImpulse*InverseDeltaTime);
end;

function TKraftConstraintJointPulley.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

function TKraftConstraintJointPulley.GetCurrentLengthA:TKraftScalar;
begin
 result:=Vector3Dist(Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform),fGroundAnchors[0]);
end;

function TKraftConstraintJointPulley.GetCurrentLengthB:TKraftScalar;
begin
 result:=Vector3Dist(Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform),fGroundAnchors[1]);
end;

constructor TKraftConstraintJointBallSocket.Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AWorldAnchorPoint:TKraftVector3;const ACollideConnected:boolean=false);
begin

 fLocalAnchors[0]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyA.fWorldTransform);
 fLocalAnchors[1]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyB.fWorldTransform);

 fAccumulatedImpulse:=Vector3Origin;

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;

constructor TKraftConstraintJointBallSocket.Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const ALocalAnchorPointA,ALocalAnchorPointB:TKraftVector3;const ACollideConnected:boolean=false);
begin

 fLocalAnchors[0]:=ALocalAnchorPointA;
 fLocalAnchors[1]:=ALocalAnchorPointB;

 fAccumulatedImpulse:=Vector3Origin;

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;


destructor TKraftConstraintJointBallSocket.Destroy;
begin
 inherited Destroy;
end;
                                                   
procedure TKraftConstraintJointBallSocket.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA,cB,vB,wB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    InverseMassOfBodies,BiasFactor:TKraftScalar;
    SkewSymmetricMatrices:array[0..1] of TKraftMatrix3x3;
    MassMatrix:TKraftMatrix3x3;
begin

 fIslandIndices[0]:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];
 fIslandIndices[1]:=fRigidBodies[1].fIslandIndices[Island.fIslandIndex];

 fLocalCenters[0]:=fRigidBodies[0].fSweep.LocalCenter;
 fLocalCenters[1]:=fRigidBodies[1].fSweep.LocalCenter;

 fInverseMasses[0]:=fRigidBodies[0].fInverseMass;
 fInverseMasses[1]:=fRigidBodies[1].fInverseMass;

 fWorldInverseInertiaTensors[0]:=fRigidBodies[0].fWorldInverseInertiaTensor;
 fWorldInverseInertiaTensors[1]:=fRigidBodies[1].fWorldInverseInertiaTensor;

 fSolverVelocities[0]:=@Island.fSolver.fVelocities[fIslandIndices[0]];
 fSolverVelocities[1]:=@Island.fSolver.fVelocities[fIslandIndices[1]];

 fSolverPositions[0]:=@Island.fSolver.fPositions[fIslandIndices[0]];
 fSolverPositions[1]:=@Island.fSolver.fPositions[fIslandIndices[1]];

 fSolverLinearFactors[0]:=@Island.fSolver.fLinearFactors[fIslandIndices[0]];
 fSolverLinearFactors[1]:=@Island.fSolver.fLinearFactors[fIslandIndices[1]];

 cA:=@fSolverPositions[0]^.Position;
 qA:=@fSolverPositions[0]^.Orientation;
 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 cB:=@fSolverPositions[1]^.Position;
 qB:=@fSolverPositions[1]^.Orientation;
 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 fRelativePositions[0]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
 fRelativePositions[1]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

 SkewSymmetricMatrices[0]:=GetSkewSymmetricMatrixPlus(fRelativePositions[0]);
 SkewSymmetricMatrices[1]:=GetSkewSymmetricMatrixPlus(fRelativePositions[1]);

 InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

 if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
  MassMatrix[0,0]:=InverseMassOfBodies;
  MassMatrix[0,1]:=0.0;
  MassMatrix[0,2]:=0.0;
{$ifdef SIMD}
  MassMatrix[0,3]:=0.0;
{$endif}
  MassMatrix[1,0]:=0.0;
  MassMatrix[1,1]:=InverseMassOfBodies;
  MassMatrix[1,2]:=0.0;
{$ifdef SIMD}
  MassMatrix[1,3]:=0.0;
{$endif}
  MassMatrix[2,0]:=0.0;
  MassMatrix[2,1]:=0.0;
  MassMatrix[2,2]:=InverseMassOfBodies;
{$ifdef SIMD}
  MassMatrix[2,3]:=0.0;
{$endif}
  Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[0],fWorldInverseInertiaTensors[0]),SkewSymmetricMatrices[0]));
  Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[1],fWorldInverseInertiaTensors[1]),SkewSymmetricMatrices[1]));
  fInverseMassMatrix:=Matrix3x3TermInverse(MassMatrix);
 end else begin
  fInverseMassMatrix:=Matrix3x3Null;
 end;

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin
  fBiasVector:=Vector3Origin;
 end else begin
  BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;
  fBiasVector:=Vector3ScalarMul(Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0])),BiasFactor);
 end;

 if fPhysics.fWarmStarting then begin

  fAccumulatedImpulse:=Vector3ScalarMul(fAccumulatedImpulse,TimeStep.DeltaTimeRatio);

//writeln('BallWarm ',Vector3Length(fAccumulatedImpulse):1:8);

  Vector3DirectSub(vA^,Vector3Mul(fAccumulatedImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],fAccumulatedImpulse),fWorldInverseInertiaTensors[0]));

  Vector3DirectAdd(vB^,Vector3Mul(fAccumulatedImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],fAccumulatedImpulse),fWorldInverseInertiaTensors[1]));

 end else begin

  fAccumulatedImpulse:=Vector3Origin;

 end;

end;

procedure TKraftConstraintJointBallSocket.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA,vB,wB:PKraftVector3;
    vpA,vpB,Jv,Impulse:TKraftVector3;
begin

 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 // Cdot = dot(u, v + cross(w, r))
 vpA:=Vector3Add(vA^,Vector3Cross(wA^,fRelativePositions[0]));
 vpB:=Vector3Add(vB^,Vector3Cross(wB^,fRelativePositions[1]));
 Jv:=Vector3Sub(vpB,vpA);

 Impulse:=Vector3TermMatrixMul(Vector3Sub(Vector3Neg(Jv),fBiasVector),fInverseMassMatrix);

 fAccumulatedImpulse:=Vector3Add(fAccumulatedImpulse,Impulse);

//writeln('BallVelo ',Vector3Length(Impulse):1:8);

 Vector3DirectSub(vA^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
 Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],Impulse),fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(vB^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],Impulse),fWorldInverseInertiaTensors[1]));

end;

function TKraftConstraintJointBallSocket.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA,cB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    rA,rB,TranslationError,Impulse:TKraftVector3;
    InverseMassOfBodies:TKraftScalar;
    SkewSymmetricMatrices:array[0..1] of TKraftMatrix3x3;
    MassMatrix:TKraftMatrix3x3;
begin

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  cA:=@fSolverPositions[0]^.Position;
  qA:=@fSolverPositions[0]^.Orientation;

  cB:=@fSolverPositions[1]^.Position;
  qB:=@fSolverPositions[1]^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
  rB:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

  SkewSymmetricMatrices[0]:=GetSkewSymmetricMatrixPlus(rA);
  SkewSymmetricMatrices[1]:=GetSkewSymmetricMatrixPlus(rB);

  InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

  if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
   MassMatrix[0,0]:=InverseMassOfBodies;
   MassMatrix[0,1]:=0.0;
   MassMatrix[0,2]:=0.0;
{$ifdef SIMD}
   MassMatrix[0,3]:=0.0;
{$endif}
   MassMatrix[1,0]:=0.0;
   MassMatrix[1,1]:=InverseMassOfBodies;
   MassMatrix[1,2]:=0.0;
{$ifdef SIMD}
   MassMatrix[1,3]:=0.0;
{$endif}
   MassMatrix[2,0]:=0.0;
   MassMatrix[2,1]:=0.0;
   MassMatrix[2,2]:=InverseMassOfBodies;
{$ifdef SIMD}
   MassMatrix[2,3]:=0.0;
{$endif}
   Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[0],fWorldInverseInertiaTensors[0]),SkewSymmetricMatrices[0]));
   Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[1],fWorldInverseInertiaTensors[1]),SkewSymmetricMatrices[1]));
   fInverseMassMatrix:=Matrix3x3TermInverse(MassMatrix);
  end else begin
   fInverseMassMatrix:=Matrix3x3Null;
  end;

  TranslationError:=Vector3Sub(Vector3Add(cB^,rB),Vector3Add(cA^,rA));

  Impulse:=Vector3TermMatrixMul(Vector3Neg(TranslationError),fInverseMassMatrix);

 // writeln('BallPos ',Vector3Length(fAccumulatedImpulse):1:8,' ',TranslationError.x:1:8,' ',TranslationError.y:1:8,' ',TranslationError.z:1:8);

  Vector3DirectSub(cA^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(Impulse)),fWorldInverseInertiaTensors[0]),1.0);

  Vector3DirectAdd(cB^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Vector3Cross(rB,Impulse),fWorldInverseInertiaTensors[1]),1.0);

  result:=Vector3Length(TranslationError)<fPhysics.fLinearSlop;

 end else begin

  result:=true;

 end;
end;

function TKraftConstraintJointBallSocket.GetAnchorA:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointBallSocket.GetAnchorB:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform);
end;

function TKraftConstraintJointBallSocket.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fAccumulatedImpulse,InverseDeltaTime);
end;

function TKraftConstraintJointBallSocket.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3Origin;
end;

constructor TKraftConstraintJointFixed.Create(const APhysics:TKraft;const ARigidBodyA,ARigidBodyB:TKraftRigidBody;const AWorldAnchorPoint:TKraftVector3;const ACollideConnected:boolean=false);
begin

 fLocalAnchors[0]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyA.fWorldTransform);
 fLocalAnchors[1]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyB.fWorldTransform);

 fAccumulatedImpulseTranslation:=Vector3Origin;
 fAccumulatedImpulseRotation:=Vector3Origin;

 fInverseInitialOrientationDifference:=QuaternionInverse(QuaternionTermNormalize(QuaternionMul(ARigidBodyB.fSweep.q0,QuaternionInverse(ARigidBodyA.fSweep.q0))));

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointFixed.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftConstraintJointFixed.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA,cB,vB,wB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    InverseMassOfBodies,BiasFactor:TKraftScalar;
    SkewSymmetricMatrices:array[0..1] of TKraftMatrix3x3;
    MassMatrix:TKraftMatrix3x3;
    CurrentOrientationDifference,qError:TKraftQuaternion;
begin

 fIslandIndices[0]:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];
 fIslandIndices[1]:=fRigidBodies[1].fIslandIndices[Island.fIslandIndex];

 fLocalCenters[0]:=fRigidBodies[0].fSweep.LocalCenter;
 fLocalCenters[1]:=fRigidBodies[1].fSweep.LocalCenter;

 fInverseMasses[0]:=fRigidBodies[0].fInverseMass;
 fInverseMasses[1]:=fRigidBodies[1].fInverseMass;

 fWorldInverseInertiaTensors[0]:=fRigidBodies[0].fWorldInverseInertiaTensor;
 fWorldInverseInertiaTensors[1]:=fRigidBodies[1].fWorldInverseInertiaTensor;

 fSolverVelocities[0]:=@Island.fSolver.fVelocities[fIslandIndices[0]];
 fSolverVelocities[1]:=@Island.fSolver.fVelocities[fIslandIndices[1]];

 fSolverPositions[0]:=@Island.fSolver.fPositions[fIslandIndices[0]];
 fSolverPositions[1]:=@Island.fSolver.fPositions[fIslandIndices[1]];

 fSolverLinearFactors[0]:=@Island.fSolver.fLinearFactors[fIslandIndices[0]];
 fSolverLinearFactors[1]:=@Island.fSolver.fLinearFactors[fIslandIndices[1]];

 cA:=@fSolverPositions[0]^.Position;
 qA:=@fSolverPositions[0]^.Orientation;
 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 cB:=@fSolverPositions[1]^.Position;
 qB:=@fSolverPositions[1]^.Orientation;
 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 fRelativePositions[0]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
 fRelativePositions[1]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

 SkewSymmetricMatrices[0]:=GetSkewSymmetricMatrixPlus(fRelativePositions[0]);
 SkewSymmetricMatrices[1]:=GetSkewSymmetricMatrixPlus(fRelativePositions[1]);

 InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

 if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
  MassMatrix[0,0]:=InverseMassOfBodies;
  MassMatrix[0,1]:=0.0;
  MassMatrix[0,2]:=0.0;
{$ifdef SIMD}
  MassMatrix[0,3]:=0.0;
{$endif}
  MassMatrix[1,0]:=0.0;
  MassMatrix[1,1]:=InverseMassOfBodies;
  MassMatrix[1,2]:=0.0;
{$ifdef SIMD}
  MassMatrix[1,3]:=0.0;
{$endif}
  MassMatrix[2,0]:=0.0;
  MassMatrix[2,1]:=0.0;
  MassMatrix[2,2]:=InverseMassOfBodies;
{$ifdef SIMD}
  MassMatrix[2,3]:=0.0;
{$endif}
  Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[0],fWorldInverseInertiaTensors[0]),SkewSymmetricMatrices[0]));
  Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[1],fWorldInverseInertiaTensors[1]),SkewSymmetricMatrices[1]));
  fInverseMassMatrixTranslation:=Matrix3x3TermInverse(MassMatrix);
  fInverseMassMatrixRotation:=Matrix3x3TermInverse(Matrix3x3TermAdd(fWorldInverseInertiaTensors[0],fWorldInverseInertiaTensors[1]));
 end else begin
  fInverseMassMatrixTranslation:=Matrix3x3Null;
  fInverseMassMatrixRotation:=Matrix3x3Null;
 end;

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  fBiasTranslation:=Vector3Origin;
  fBiasRotation:=Vector3Origin;

 end else begin

  BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;

  fBiasTranslation:=Vector3ScalarMul(Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0])),BiasFactor);

  CurrentOrientationDifference:=QuaternionTermNormalize(QuaternionMul(qB^,QuaternionInverse(qA^)));
  qError:=QuaternionMul(CurrentOrientationDifference,fInverseInitialOrientationDifference);
  fBiasRotation:=Vector3ScalarMul(Vector3(qError.x,qError.y,qError.z),BiasFactor*2.0);

 end;

 if fPhysics.fWarmStarting then begin

  fAccumulatedImpulseTranslation:=Vector3ScalarMul(fAccumulatedImpulseTranslation,TimeStep.DeltaTimeRatio);
  fAccumulatedImpulseRotation:=Vector3ScalarMul(fAccumulatedImpulseRotation,TimeStep.DeltaTimeRatio);

  Vector3DirectSub(vA^,Vector3Mul(fAccumulatedImpulseTranslation,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Add(Vector3Cross(fRelativePositions[0],fAccumulatedImpulseTranslation),fAccumulatedImpulseRotation),fWorldInverseInertiaTensors[0]));

  Vector3DirectAdd(vB^,Vector3Mul(fAccumulatedImpulseTranslation,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Add(Vector3Cross(fRelativePositions[1],fAccumulatedImpulseTranslation),fAccumulatedImpulseRotation),fWorldInverseInertiaTensors[1]));

 end else begin

  fAccumulatedImpulseTranslation:=Vector3Origin;
  fAccumulatedImpulseRotation:=Vector3Origin;

 end;

end;

procedure TKraftConstraintJointFixed.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA,vB,wB:PKraftVector3;
    Jv,Impulse:TKraftVector3;
begin

 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 (**** Translation ****)

 // Cdot = dot(u, v + cross(w, r))
 Jv:=Vector3Sub(Vector3Add(vB^,Vector3Cross(wB^,fRelativePositions[1])),
                Vector3Add(vA^,Vector3Cross(wA^,fRelativePositions[0])));

 Impulse:=Vector3TermMatrixMul(Vector3Sub(Vector3Neg(Jv),fBiasTranslation),fInverseMassMatrixTranslation);

 fAccumulatedImpulseTranslation:=Vector3Add(fAccumulatedImpulseTranslation,Impulse);

 Vector3DirectSub(vA^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
 Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],Impulse),fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(vB^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],Impulse),fWorldInverseInertiaTensors[1]));

 (**** Rotation ****)

 Jv:=Vector3Sub(wB^,wA^);

 Impulse:=Vector3TermMatrixMul(Vector3Sub(Vector3Neg(Jv),fBiasRotation),fInverseMassMatrixRotation);

 fAccumulatedImpulseRotation:=Vector3Add(fAccumulatedImpulseRotation,Impulse);

 Vector3DirectSub(wA^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]));

end;

function TKraftConstraintJointFixed.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA,cB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    rA,rB,TranslationError,RotationError,Impulse:TKraftVector3;
    InverseMassOfBodies:TKraftScalar;
    SkewSymmetricMatrices:array[0..1] of TKraftMatrix3x3;
    MassMatrix:TKraftMatrix3x3;
    CurrentOrientationDifference,qError:TKraftQuaternion;
begin

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  cA:=@fSolverPositions[0]^.Position;
  qA:=@fSolverPositions[0]^.Orientation;

  cB:=@fSolverPositions[1]^.Position;
  qB:=@fSolverPositions[1]^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
  rB:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

  SkewSymmetricMatrices[0]:=GetSkewSymmetricMatrixPlus(rA);
  SkewSymmetricMatrices[1]:=GetSkewSymmetricMatrixPlus(rB);

  InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

  if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
   MassMatrix[0,0]:=InverseMassOfBodies;
   MassMatrix[0,1]:=0.0;
   MassMatrix[0,2]:=0.0;
{$ifdef SIMD}
   MassMatrix[0,3]:=0.0;
{$endif}
   MassMatrix[1,0]:=0.0;
   MassMatrix[1,1]:=InverseMassOfBodies;
   MassMatrix[1,2]:=0.0;
{$ifdef SIMD}
   MassMatrix[1,3]:=0.0;
{$endif}
   MassMatrix[2,0]:=0.0;
   MassMatrix[2,1]:=0.0;
   MassMatrix[2,2]:=InverseMassOfBodies;
{$ifdef SIMD}
   MassMatrix[2,3]:=0.0;
{$endif}
   Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[0],fWorldInverseInertiaTensors[0]),SkewSymmetricMatrices[0]));
   Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[1],fWorldInverseInertiaTensors[1]),SkewSymmetricMatrices[1]));
   fInverseMassMatrixTranslation:=Matrix3x3TermInverse(MassMatrix);
  end else begin
   fInverseMassMatrixTranslation:=Matrix3x3Null;
  end;

  (**** Translation ****)

  TranslationError:=Vector3Sub(Vector3Add(cB^,rB),Vector3Add(cA^,rA));

  result:=Vector3Length(TranslationError)<fPhysics.fLinearSlop;

  Impulse:=Vector3TermMatrixMul(Vector3Neg(TranslationError),fInverseMassMatrixTranslation);

  Vector3DirectSub(cA^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(Impulse)),fWorldInverseInertiaTensors[0]),1.0);

  Vector3DirectAdd(cB^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Vector3Cross(rB,Impulse),fWorldInverseInertiaTensors[1]),1.0);

  (**** Rotation ****)

  if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
   fInverseMassMatrixRotation:=Matrix3x3TermInverse(Matrix3x3TermAdd(fWorldInverseInertiaTensors[0],fWorldInverseInertiaTensors[1]));
  end else begin
   fInverseMassMatrixRotation:=Matrix3x3Null;
  end;

  CurrentOrientationDifference:=QuaternionTermNormalize(QuaternionMul(qB^,QuaternionInverse(qA^)));
  qError:=QuaternionMul(CurrentOrientationDifference,fInverseInitialOrientationDifference);
  RotationError.x:=qError.x*2.0;
  RotationError.y:=qError.y*2.0;
  RotationError.z:=qError.z*2.0;

  result:=result and (Vector3Length(RotationError)<fPhysics.fAngularSlop);

  Impulse:=Vector3TermMatrixMul(Vector3Neg(RotationError),fInverseMassMatrixRotation);

  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(Impulse),fWorldInverseInertiaTensors[0]),1.0);

  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]),1.0);

 end else begin

  result:=true;

 end;

end;

function TKraftConstraintJointFixed.GetAnchorA:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointFixed.GetAnchorB:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform);
end;

function TKraftConstraintJointFixed.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fAccumulatedImpulseTranslation,InverseDeltaTime);
end;

function TKraftConstraintJointFixed.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fAccumulatedImpulseRotation,InverseDeltaTime);
end;

constructor TKraftConstraintJointHinge.Create(const APhysics:TKraft;
                                              const ARigidBodyA,ARigidBodyB:TKraftRigidBody;
                                              const AWorldAnchorPoint:TKraftVector3;
                                              const AWorldRotationAxis:TKraftVector3;
                                              const ALimitEnabled:boolean=false;
                                              const AMotorEnabled:boolean=false;
                                              const AMinimumAngleLimit:TKraftScalar=-1.0;
                                              const AMaximumAngleLimit:TKraftScalar=1.0;
                                              const AMotorSpeed:TKraftScalar=0.0;
                                              const AMaximalMotorTorque:TKraftScalar=0.0;
                                              const ACollideConnected:boolean=false);
begin

 fLimitState:=ALimitEnabled;

 fMotorState:=AMotorEnabled;

 fLowerLimit:=AMinimumAngleLimit;

 fUpperLimit:=AMaximumAngleLimit;

 Assert((fLowerLimit<=EPSILON) and (fLowerLimit>=(-(pi2+EPSILON))));

 Assert((fUpperLimit>=(-EPSILON)) and (fUpperLimit<=(pi2+EPSILON)));

 fMotorSpeed:=AMotorSpeed;

 fMaximalMotorTorque:=AMaximalMotorTorque;

 fLocalAnchors[0]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyA.fWorldTransform);
 fLocalAnchors[1]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyB.fWorldTransform);

 fLocalAxes[0]:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(AWorldRotationAxis,ARigidBodyA.fWorldTransform));
 fLocalAxes[1]:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(AWorldRotationAxis,ARigidBodyB.fWorldTransform));

 fAccumulatedImpulseLowerLimit:=0.0;
 fAccumulatedImpulseUpperLimit:=0.0;
 fAccumulatedImpulseMotor:=0.0;
 fAccumulatedImpulseTranslation:=Vector3Origin;
 fAccumulatedImpulseRotation:=Vector2Origin;

 fBiasLowerLimit:=0.0;
 fBiasUpperLimit:=0.0;

 fInverseInitialOrientationDifference:=QuaternionInverse(QuaternionTermNormalize(QuaternionMul(ARigidBodyB.fSweep.q0,QuaternionInverse(ARigidBodyA.fSweep.q0))));

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointHinge.Destroy;
begin
 inherited Destroy;
end;

function TKraftConstraintJointHinge.ComputeCurrentHingeAngle(const OrientationA,OrientationB:TKraftQuaternion):TKraftScalar;
 function ComputeNormalizedAngle(a:TKraftScalar):TKraftScalar;
 begin
  result:=a-(floor(a/pi2)*pi2); // ModuloPos(ModuloPos(a+pi,pi2)+pi2,pi2)-pi;
  while result<-pi do begin
   result:=result+pi2;
  end;
  while result>pi do begin
   result:=result-pi2;
  end;
 end;
var CurrentOrientationDifference,RelativeRotation:TKraftQuaternion;
    CosHalfAngle,SinHalfAngleAbs,DotProduct:TKraftScalar;
begin
 // Compute the current orientation difference between the two bodies
 CurrentOrientationDifference:=QuaternionTermNormalize(QuaternionMul(OrientationB,QuaternionInverse(OrientationA)));

 // Compute the relative rotation considering the initial orientation difference
 RelativeRotation:=QuaternionTermNormalize(QuaternionMul(CurrentOrientationDifference,fInverseInitialOrientationDifference));

 // Extract cos(theta/2) and |sin(theta/2)|
 CosHalfAngle:=RelativeRotation.w;
 SinHalfAngleAbs:=Vector3Length(PKraftVector3(pointer(@RelativeRotation))^);

 // Compute the dot product of the relative rotation axis and the hinge axis
 DotProduct:=Vector3Dot(PKraftVector3(pointer(@RelativeRotation))^,PKraftVector3(pointer(@fA1))^);

 // If the relative rotation axis and the hinge axis are pointing the same direction
 if DotProduct>=0.0 then begin
  result:=2.0*ArcTan2(SinHalfAngleAbs,CosHalfAngle);
 end else begin
  result:=2.0*ArcTan2(SinHalfAngleAbs,-CosHalfAngle);
 end;

 // Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
 result:=ComputeNormalizedAngle(result);

 // Compute and return the corresponding angle near one the two limits
 if fLowerLimit<fUpperLimit then begin
  if result>fUpperLimit then begin
   if abs(ComputeNormalizedAngle(result-fUpperLimit))>abs(ComputeNormalizedAngle(result-fLowerLimit)) then begin
    result:=result-pi2;
   end;
  end else if result<fLowerLimit then begin
   if abs(ComputeNormalizedAngle(fUpperLimit-result))<=abs(ComputeNormalizedAngle(fLowerLimit-result)) then begin
    result:=result+pi2;
   end;
  end;
 end;

end;

procedure TKraftConstraintJointHinge.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA,cB,vB,wB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    HingeAngle,LowerLimitError,UpperLimitError,InverseMassOfBodies,BiasFactor:TKraftScalar;
    SkewSymmetricMatrices:array[0..1] of TKraftMatrix3x3;
    MassMatrix:TKraftMatrix3x3;
    RotationKMatrix:TKraftMatrix2x2;
    OldIsLowerLimitViolated,OldIsUpperLimitViolated:boolean;
    a2,b2,c2,I1B2CrossA1,I1C2CrossA1,I2B2CrossA1,I2C2CrossA1,RotationImpulse,LimitsImpulse,MotorImpulse,AngularImpulse:TKraftVector3;
begin

 fIslandIndices[0]:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];
 fIslandIndices[1]:=fRigidBodies[1].fIslandIndices[Island.fIslandIndex];

 fLocalCenters[0]:=fRigidBodies[0].fSweep.LocalCenter;
 fLocalCenters[1]:=fRigidBodies[1].fSweep.LocalCenter;

 fInverseMasses[0]:=fRigidBodies[0].fInverseMass;
 fInverseMasses[1]:=fRigidBodies[1].fInverseMass;

 fWorldInverseInertiaTensors[0]:=fRigidBodies[0].fWorldInverseInertiaTensor;
 fWorldInverseInertiaTensors[1]:=fRigidBodies[1].fWorldInverseInertiaTensor;

 fSolverVelocities[0]:=@Island.fSolver.fVelocities[fIslandIndices[0]];
 fSolverVelocities[1]:=@Island.fSolver.fVelocities[fIslandIndices[1]];

 fSolverPositions[0]:=@Island.fSolver.fPositions[fIslandIndices[0]];
 fSolverPositions[1]:=@Island.fSolver.fPositions[fIslandIndices[1]];

 fSolverLinearFactors[0]:=@Island.fSolver.fLinearFactors[fIslandIndices[0]];
 fSolverLinearFactors[1]:=@Island.fSolver.fLinearFactors[fIslandIndices[1]];

 cA:=@fSolverPositions[0]^.Position;
 qA:=@fSolverPositions[0]^.Orientation;
 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 cB:=@fSolverPositions[1]^.Position;
 qB:=@fSolverPositions[1]^.Orientation;
 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 fRelativePositions[0]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
 fRelativePositions[1]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

 HingeAngle:=ComputeCurrentHingeAngle(qA^,qB^);

 LowerLimitError:=HingeAngle-fLowerLimit;
 UpperLimitError:=fUpperLimit-HingeAngle;
 OldIsLowerLimitViolated:=fIsLowerLimitViolated;
 fIsLowerLimitViolated:=LowerLimitError<=0.0;
 if fIsLowerLimitViolated<>OldIsLowerLimitViolated then begin
  fAccumulatedImpulseLowerLimit:=0.0;
 end;
 OldIsUpperLimitViolated:=fIsUpperLimitViolated;
 fIsUpperLimitViolated:=UpperLimitError<=0.0;
 if fIsUpperLimitViolated<>OldIsUpperLimitViolated then begin
  fAccumulatedImpulseUpperLimit:=0.0;
 end;

 fA1:=Vector3NormEx(Vector3TermQuaternionRotate(fLocalAxes[0],qA^));
 a2:=Vector3NormEx(Vector3TermQuaternionRotate(fLocalAxes[1],qB^));
 b2:=Vector3GetOneUnitOrthogonalVector(a2);
 c2:=Vector3Cross(a2,b2);
 fB2CrossA1:=Vector3Cross(b2,fA1);
 fC2CrossA1:=Vector3Cross(c2,fA1);

 SkewSymmetricMatrices[0]:=GetSkewSymmetricMatrixPlus(fRelativePositions[0]);
 SkewSymmetricMatrices[1]:=GetSkewSymmetricMatrixPlus(fRelativePositions[1]);

 InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

 if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
  MassMatrix[0,0]:=InverseMassOfBodies;
  MassMatrix[0,1]:=0.0;
  MassMatrix[0,2]:=0.0;
{$ifdef SIMD}
  MassMatrix[0,3]:=0.0;
{$endif}
  MassMatrix[1,0]:=0.0;
  MassMatrix[1,1]:=InverseMassOfBodies;
  MassMatrix[1,2]:=0.0;
{$ifdef SIMD}
  MassMatrix[1,3]:=0.0;
{$endif}
  MassMatrix[2,0]:=0.0;
  MassMatrix[2,1]:=0.0;
  MassMatrix[2,2]:=InverseMassOfBodies;
{$ifdef SIMD}
  MassMatrix[2,3]:=0.0;
{$endif}
  Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[0],fWorldInverseInertiaTensors[0]),SkewSymmetricMatrices[0]));
  Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[1],fWorldInverseInertiaTensors[1]),SkewSymmetricMatrices[1]));
  fInverseMassMatrixTranslation:=Matrix3x3TermInverse(MassMatrix);
 end else begin
  fInverseMassMatrixTranslation:=Matrix3x3Null;
 end;

 I1B2CrossA1:=Vector3TermMatrixMul(fB2CrossA1,fWorldInverseInertiaTensors[0]);
 I1C2CrossA1:=Vector3TermMatrixMul(fC2CrossA1,fWorldInverseInertiaTensors[0]);
 I2B2CrossA1:=Vector3TermMatrixMul(fB2CrossA1,fWorldInverseInertiaTensors[1]);
 I2C2CrossA1:=Vector3TermMatrixMul(fC2CrossA1,fWorldInverseInertiaTensors[1]);
 RotationKMatrix[0,0]:=Vector3Dot(fB2CrossA1,I1B2CrossA1)+Vector3Dot(fB2CrossA1,I2B2CrossA1);
 RotationKMatrix[0,1]:=Vector3Dot(fB2CrossA1,I1C2CrossA1)+Vector3Dot(fB2CrossA1,I2C2CrossA1);
 RotationKMatrix[1,0]:=Vector3Dot(fC2CrossA1,I1B2CrossA1)+Vector3Dot(fC2CrossA1,I2B2CrossA1);
 RotationKMatrix[1,1]:=Vector3Dot(fC2CrossA1,I1C2CrossA1)+Vector3Dot(fC2CrossA1,I2C2CrossA1);
 if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
  Matrix2x2Inverse(fInverseMassMatrixRotation,RotationKMatrix);
 end else begin
  fInverseMassMatrixRotation:=Matrix2x2Null;
 end;

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin
  BiasFactor:=0.0;
  fBiasTranslation:=Vector3Origin;
  fBiasRotation:=Vector2Origin;
 end else begin
  BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;
  fBiasTranslation:=Vector3ScalarMul(Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0])),BiasFactor);
  fBiasRotation:=Vector2ScalarMul(Vector2(Vector3Dot(fA1,b2),Vector3Dot(fA1,c2)),BiasFactor);
 end;

 if fMotorState or (fLimitState and (fIsLowerLimitViolated or fIsUpperLimitViolated)) then begin
  // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
  fInverseMassMatrixLimitMotor:=Vector3Dot(fA1,Vector3TermMatrixMul(fA1,fWorldInverseInertiaTensors[0]))+
                                Vector3Dot(fA1,Vector3TermMatrixMul(fA1,fWorldInverseInertiaTensors[1]));
  if fInverseMassMatrixLimitMotor>0.0 then begin
   fInverseMassMatrixLimitMotor:=1.0/fInverseMassMatrixLimitMotor;
  end else begin
   fInverseMassMatrixLimitMotor:=0.0;
  end;
  if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin
   fBiasLowerLimit:=0.0;
   fBiasUpperLimit:=0.0;
  end else begin
   fBiasLowerLimit:=LowerLimitError*BiasFactor;
   fBiasUpperLimit:=UpperLimitError*BiasFactor;
  end;
 end;

 if fPhysics.fWarmStarting then begin

  // Compute the impulse P=J^T * lambda for the 2 rotation constraints
  RotationImpulse:=Vector3Add(Vector3ScalarMul(fB2CrossA1,fAccumulatedImpulseRotation.x),
                              Vector3ScalarMul(fC2CrossA1,fAccumulatedImpulseRotation.y));

  // Compute the impulse P=J^T * lambda for the lower and upper limits constraints
  LimitsImpulse:=Vector3ScalarMul(fA1,fAccumulatedImpulseLowerLimit-fAccumulatedImpulseUpperLimit);

  // Compute the impulse P=J^T * lambda for the motor constraint
  MotorImpulse:=Vector3ScalarMul(fA1,fAccumulatedImpulseMotor);

  fAccumulatedImpulseTranslation:=Vector3ScalarMul(fAccumulatedImpulseTranslation,TimeStep.DeltaTimeRatio);

  AngularImpulse:=Vector3ScalarMul(Vector3Add(Vector3Add(RotationImpulse,LimitsImpulse),MotorImpulse),TimeStep.DeltaTimeRatio);

  Vector3DirectSub(vA^,Vector3Mul(fAccumulatedImpulseTranslation,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Add(Vector3Cross(fRelativePositions[0],fAccumulatedImpulseTranslation),AngularImpulse),fWorldInverseInertiaTensors[0]));

  Vector3DirectAdd(vB^,Vector3Mul(fAccumulatedImpulseTranslation,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Add(Vector3Cross(fRelativePositions[1],fAccumulatedImpulseTranslation),AngularImpulse),fWorldInverseInertiaTensors[1]));

 end else begin

  fAccumulatedImpulseTranslation:=Vector3Origin;
  fAccumulatedImpulseRotation:=Vector2Origin;
  fAccumulatedImpulseLowerLimit:=0.0;
  fAccumulatedImpulseUpperLimit:=0.0;
  fAccumulatedImpulseMotor:=0.0;

 end;

end;

procedure TKraftConstraintJointHinge.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA,vB,wB:PKraftVector3;
    vpA,vpB,Jv,Impulse:TKraftVector3;
    JvRotation,RotationImpulse:TKraftVector2;
    JvLowerLimit,ImpulseLower,JvUpperLimit,ImpulseUpper,JvMotor,ImpulseMotor,LambdaTemp,MaximalMotorImpulse:TKraftScalar;
begin

 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 (**** Translation ****)

 // Cdot = dot(u, v + cross(w, r))
 vpA:=Vector3Add(vA^,Vector3Cross(wA^,fRelativePositions[0]));
 vpB:=Vector3Add(vB^,Vector3Cross(wB^,fRelativePositions[1]));
 Jv:=Vector3Sub(vpB,vpA);

 Impulse:=Vector3TermMatrixMul(Vector3Sub(Vector3Neg(Jv),fBiasTranslation),fInverseMassMatrixTranslation);

 fAccumulatedImpulseTranslation:=Vector3Add(fAccumulatedImpulseTranslation,Impulse);

 Vector3DirectSub(vA^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
 Vector3DirectSub(wA^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[0],Impulse),fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(vB^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Vector3Cross(fRelativePositions[1],Impulse),fWorldInverseInertiaTensors[1]));

 (**** Rotation ****)
       
 JvRotation.x:=(Vector3Dot(fB2CrossA1,wB^)-Vector3Dot(fB2CrossA1,wA^))+fBiasRotation.x;
 JvRotation.y:=(Vector3Dot(fC2CrossA1,wB^)-Vector3Dot(fC2CrossA1,wA^))+fBiasRotation.y;

 RotationImpulse.x:=-((JvRotation.x*fInverseMassMatrixRotation[0,0])+(JvRotation.y*fInverseMassMatrixRotation[0,1]));
 RotationImpulse.y:=-((JvRotation.x*fInverseMassMatrixRotation[1,0])+(JvRotation.y*fInverseMassMatrixRotation[1,1]));

 fAccumulatedImpulseRotation.x:=fAccumulatedImpulseRotation.x+RotationImpulse.x;
 fAccumulatedImpulseRotation.y:=fAccumulatedImpulseRotation.y+RotationImpulse.y;

 Impulse:=Vector3Add(Vector3ScalarMul(fB2CrossA1,RotationImpulse.x),
                     Vector3ScalarMul(fC2CrossA1,RotationImpulse.y));

 Vector3DirectSub(wA^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(wB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]));

 (**** Limits ****)

 if fLimitState then begin
  if fIsLowerLimitViolated then begin
   JvLowerLimit:=Vector3Dot(Vector3Sub(wB^,wA^),fA1);
   ImpulseLower:=fInverseMassMatrixLimitMotor*(-(JvLowerLimit+fBiasLowerLimit));
   LambdaTemp:=fAccumulatedImpulseLowerLimit;
   fAccumulatedImpulseLowerLimit:=Max(0.0,fAccumulatedImpulseLowerLimit+ImpulseLower);
   ImpulseLower:=fAccumulatedImpulseLowerLimit-LambdaTemp;
   Impulse:=Vector3ScalarMul(fA1,ImpulseLower);
   Vector3DirectSub(wA^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[0]));
   Vector3DirectAdd(wB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]));
  end;
  if fIsUpperLimitViolated then begin
   JvUpperLimit:=-Vector3Dot(Vector3Sub(wB^,wA^),fA1);
   ImpulseUpper:=fInverseMassMatrixLimitMotor*(-(JvUpperLimit++fBiasUpperLimit));
   LambdaTemp:=fAccumulatedImpulseUpperLimit;
   fAccumulatedImpulseUpperLimit:=Max(0.0,fAccumulatedImpulseUpperLimit+ImpulseUpper);
   ImpulseUpper:=-(fAccumulatedImpulseUpperLimit-LambdaTemp);
   Impulse:=Vector3ScalarMul(fA1,ImpulseUpper);
   Vector3DirectSub(wA^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[0]));
   Vector3DirectAdd(wB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]));
  end;
 end;

 (**** Motor ****)

 if fMotorState then begin
  JvMotor:=Vector3Dot(Vector3Sub(wB^,wA^),fA1);
  ImpulseMotor:=fInverseMassMatrixLimitMotor*(-JvMotor);
  LambdaTemp:=fAccumulatedImpulseMotor;
  MaximalMotorImpulse:=fMaximalMotorTorque*TimeStep.DeltaTime;
  fAccumulatedImpulseMotor:=Min(Max(fAccumulatedImpulseMotor+ImpulseMotor,-MaximalMotorImpulse),MaximalMotorImpulse);
  ImpulseMotor:=fAccumulatedImpulseMotor-LambdaTemp;
  Impulse:=Vector3ScalarMul(fA1,ImpulseMotor);
  Vector3DirectSub(wA^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[0]));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]));
 end;

end;

function TKraftConstraintJointHinge.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA,cB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    rA,rB,TranslationError,Impulse,a2,b2,c2,I1B2CrossA1,I1C2CrossA1,I2B2CrossA1,I2C2CrossA1:TKraftVector3;
    RotationError,RotationImpulse:TKraftVector2;
    InverseMassOfBodies,HingeAngle,LowerLimitError,UpperLimitError,ImpulseLower,ImpulseUpper:TKraftScalar;
    SkewSymmetricMatrices:array[0..1] of TKraftMatrix3x3;
    MassMatrix:TKraftMatrix3x3;
    RotationKMatrix:TKraftMatrix2x2;
begin

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  cA:=@fSolverPositions[0]^.Position;
  qA:=@fSolverPositions[0]^.Orientation;

  cB:=@fSolverPositions[1]^.Position;
  qB:=@fSolverPositions[1]^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
  rB:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

  HingeAngle:=ComputeCurrentHingeAngle(qA^,qB^);

  LowerLimitError:=HingeAngle-fLowerLimit;
  UpperLimitError:=fUpperLimit-HingeAngle;
  fIsLowerLimitViolated:=LowerLimitError<=0.0;
  fIsUpperLimitViolated:=UpperLimitError<=0.0;

  fA1:=Vector3NormEx(Vector3TermQuaternionRotate(fLocalAxes[0],qA^));
  a2:=Vector3NormEx(Vector3TermQuaternionRotate(fLocalAxes[1],qB^));
  b2:=Vector3GetOneUnitOrthogonalVector(a2);
  c2:=Vector3Cross(a2,b2);
  fB2CrossA1:=Vector3Cross(b2,fA1);
  fC2CrossA1:=Vector3Cross(c2,fA1);

  SkewSymmetricMatrices[0]:=GetSkewSymmetricMatrixPlus(rA);
  SkewSymmetricMatrices[1]:=GetSkewSymmetricMatrixPlus(rB);

  (**** Translation ****)

  InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

  if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
   MassMatrix[0,0]:=InverseMassOfBodies;
   MassMatrix[0,1]:=0.0;
   MassMatrix[0,2]:=0.0;
 {$ifdef SIMD}
   MassMatrix[0,3]:=0.0;
 {$endif}
   MassMatrix[1,0]:=0.0;
   MassMatrix[1,1]:=InverseMassOfBodies;
   MassMatrix[1,2]:=0.0;
 {$ifdef SIMD}
   MassMatrix[1,3]:=0.0;
 {$endif}
   MassMatrix[2,0]:=0.0;
   MassMatrix[2,1]:=0.0;
   MassMatrix[2,2]:=InverseMassOfBodies;
 {$ifdef SIMD}
   MassMatrix[2,3]:=0.0;
 {$endif}
   Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[0],fWorldInverseInertiaTensors[0]),SkewSymmetricMatrices[0]));
   Matrix3x3Add(MassMatrix,Matrix3x3TermMulTranspose(Matrix3x3TermMul(SkewSymmetricMatrices[1],fWorldInverseInertiaTensors[1]),SkewSymmetricMatrices[1]));
   fInverseMassMatrixTranslation:=Matrix3x3TermInverse(MassMatrix);
  end else begin
   fInverseMassMatrixTranslation:=Matrix3x3Null;
  end;

  TranslationError:=Vector3Sub(Vector3Add(cB^,rB),Vector3Add(cA^,rA));

  result:=Vector3Length(TranslationError)<fPhysics.fLinearSlop;

  Impulse:=Vector3TermMatrixMul(Vector3Neg(TranslationError),fInverseMassMatrixTranslation);

  Vector3DirectSub(cA^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(Impulse)),fWorldInverseInertiaTensors[0]),1.0);

  Vector3DirectAdd(cB^,Vector3Mul(Impulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Vector3Cross(rB,Impulse),fWorldInverseInertiaTensors[1]),1.0);

  (**** Rotation ****)

  I1B2CrossA1:=Vector3TermMatrixMul(fB2CrossA1,fWorldInverseInertiaTensors[0]);
  I1C2CrossA1:=Vector3TermMatrixMul(fC2CrossA1,fWorldInverseInertiaTensors[0]);
  I2B2CrossA1:=Vector3TermMatrixMul(fB2CrossA1,fWorldInverseInertiaTensors[1]);
  I2C2CrossA1:=Vector3TermMatrixMul(fC2CrossA1,fWorldInverseInertiaTensors[1]);
  RotationKMatrix[0,0]:=Vector3Dot(fB2CrossA1,I1B2CrossA1)+Vector3Dot(fB2CrossA1,I2B2CrossA1);
  RotationKMatrix[0,1]:=Vector3Dot(fB2CrossA1,I1C2CrossA1)+Vector3Dot(fB2CrossA1,I2C2CrossA1);
  RotationKMatrix[1,0]:=Vector3Dot(fC2CrossA1,I1B2CrossA1)+Vector3Dot(fC2CrossA1,I2B2CrossA1);
  RotationKMatrix[1,1]:=Vector3Dot(fC2CrossA1,I1C2CrossA1)+Vector3Dot(fC2CrossA1,I2C2CrossA1);
  if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
   Matrix2x2Inverse(fInverseMassMatrixRotation,RotationKMatrix);
  end else begin
   fInverseMassMatrixRotation:=Matrix2x2Null;
  end;

  RotationError.x:=Vector3Dot(fA1,b2);
  RotationError.y:=Vector3Dot(fA1,c2);

  result:=result and (Vector2Length(RotationError)<fPhysics.fAngularSlop);

  RotationImpulse.x:=-((RotationError.x*fInverseMassMatrixRotation[0,0])+(RotationError.y*fInverseMassMatrixRotation[0,1]));
  RotationImpulse.y:=-((RotationError.x*fInverseMassMatrixRotation[1,0])+(RotationError.y*fInverseMassMatrixRotation[1,1]));

  Impulse:=Vector3Add(Vector3ScalarMul(fB2CrossA1,RotationImpulse.x),Vector3ScalarMul(fC2CrossA1,RotationImpulse.y));

  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(Impulse),fWorldInverseInertiaTensors[0]),1.0);

  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]),1.0);

  (**** Limits ****)
 
  if fLimitState then begin
   if fIsLowerLimitViolated then begin
    ImpulseLower:=fInverseMassMatrixLimitMotor*(-LowerLimitError);
    result:=result and (ImpulseLower<fPhysics.fAngularSlop);
    Impulse:=Vector3ScalarMul(fA1,ImpulseLower);
    QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(Impulse),fWorldInverseInertiaTensors[0]),1.0);
    QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]),1.0);
   end;
   if fIsUpperLimitViolated then begin
    ImpulseUpper:=fInverseMassMatrixLimitMotor*(-UpperLimitError);
    result:=result and (ImpulseUpper<fPhysics.fAngularSlop);
    Impulse:=Vector3ScalarMul(fA1,-ImpulseUpper);
    QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(Impulse),fWorldInverseInertiaTensors[0]),1.0);
    QuaternionDirectSpin(qB^,Vector3TermMatrixMul(Impulse,fWorldInverseInertiaTensors[1]),1.0);
   end;
  end;

 end else begin

  result:=true;
  
 end;

end;

function TKraftConstraintJointHinge.GetAnchorA:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointHinge.GetAnchorB:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform);
end;

function TKraftConstraintJointHinge.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fAccumulatedImpulseTranslation,InverseDeltaTime);
end;

function TKraftConstraintJointHinge.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
var RotationImpulse,LimitsImpulse,MotorImpulse:TKraftVector3;
begin
 RotationImpulse:=Vector3Sub(Vector3ScalarMul(fC2CrossA1,fAccumulatedImpulseRotation.y),
                                  Vector3ScalarMul(fB2CrossA1,-fAccumulatedImpulseRotation.x));
 LimitsImpulse:=Vector3ScalarMul(fA1,fAccumulatedImpulseLowerLimit-fAccumulatedImpulseUpperLimit);
 MotorImpulse:=Vector3ScalarMul(fA1,fAccumulatedImpulseMotor);
 result:=Vector3ScalarMul(Vector3Add(Vector3Add(RotationImpulse,LimitsImpulse),MotorImpulse),InverseDeltaTime);
end;

function TKraftConstraintJointHinge.GetWorldRotationAxis:TKraftVector3;
begin
 result:=Vector3TermMatrixMulBasis(fLocalAxes[0],fRigidBodies[0].fWorldTransform);
end;

procedure TKraftConstraintJointHinge.SetWorldRotationAxis(AWorldRotationAxis:TKraftVector3);
begin
 fLocalAxes[0]:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(AWorldRotationAxis,fRigidBodies[0].fWorldTransform));
 fLocalAxes[1]:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(AWorldRotationAxis,fRigidBodies[1].fWorldTransform));
end;

function TKraftConstraintJointHinge.IsLimitEnabled:boolean;
begin
 result:=fLimitState;
end;

function TKraftConstraintJointHinge.IsMotorEnabled:boolean;
begin
 result:=fMotorState;
end;

function TKraftConstraintJointHinge.GetMinimumAngleLimit:TKraftScalar;
begin
 result:=fLowerLimit;
end;

function TKraftConstraintJointHinge.GetMaximumAngleLimit:TKraftScalar;
begin
 result:=fUpperLimit;
end;

function TKraftConstraintJointHinge.GetMotorSpeed:TKraftScalar;
begin
 result:=fMotorSpeed;
end;

function TKraftConstraintJointHinge.GetMaximalMotorTorque:TKraftScalar;
begin
 result:=fMaximalMotorTorque;
end;

function TKraftConstraintJointHinge.GetMotorTorque(const DeltaTime:TKraftScalar):TKraftScalar;
begin
 result:=fAccumulatedImpulseMotor/DeltaTime;
end;

procedure TKraftConstraintJointHinge.ResetLimits;
begin
 fAccumulatedImpulseLowerLimit:=0.0;
 fAccumulatedImpulseUpperLimit:=0.0;
 fRigidBodies[0].SetToAwake;
 fRigidBodies[1].SetToAwake;
end;

procedure TKraftConstraintJointHinge.EnableLimit(const ALimitEnabled:boolean);
begin
 if fLimitState<>ALimitEnabled then begin
  fLimitState:=ALimitEnabled;
  ResetLimits;
 end;
end;

procedure TKraftConstraintJointHinge.EnableMotor(const AMotorEnabled:boolean);
begin
 if fMotorState<>AMotorEnabled then begin
  fMotorState:=AMotorEnabled;
  fAccumulatedImpulseMotor:=0.0;
  fRigidBodies[0].SetToAwake;
  fRigidBodies[1].SetToAwake;
 end;
end;

procedure TKraftConstraintJointHinge.SetMinimumAngleLimit(const AMinimumAngleLimit:TKraftScalar);
begin
 if fLowerLimit<>AMinimumAngleLimit then begin
  fLowerLimit:=AMinimumAngleLimit;
  Assert((fLowerLimit<=EPSILON) and (fLowerLimit>=(-(pi2+EPSILON))));
  ResetLimits;
 end;
end;

procedure TKraftConstraintJointHinge.SetMaximumAngleLimit(const AMaximumAngleLimit:TKraftScalar);
begin
 if fUpperLimit<>AMaximumAngleLimit then begin
  fUpperLimit:=AMaximumAngleLimit;
  Assert((fUpperLimit>=(-EPSILON)) and (fUpperLimit<=(pi2+EPSILON)));
  ResetLimits;
 end;
end;

procedure TKraftConstraintJointHinge.SetMotorSpeed(const AMotorSpeed:TKraftScalar);
begin
 if fMotorSpeed<>AMotorSpeed then begin
  fMotorSpeed:=AMotorSpeed;
  fRigidBodies[0].SetToAwake;
  fRigidBodies[1].SetToAwake;
 end;
end;

procedure TKraftConstraintJointHinge.SetMaximalMotorTorque(const AMaximalMotorTorque:TKraftScalar);
begin
 if fMaximalMotorTorque<>AMaximalMotorTorque then begin
  fMaximalMotorTorque:=AMaximalMotorTorque;
  Assert(fMaximalMotorTorque>=(-EPSILON));
  fRigidBodies[0].SetToAwake;
  fRigidBodies[1].SetToAwake;
 end;
end;

constructor TKraftConstraintJointSlider.Create(const APhysics:TKraft;
                                               const ARigidBodyA,ARigidBodyB:TKraftRigidBody;
                                               const AWorldAnchorPoint:TKraftVector3;
                                               const AWorldSliderAxis:TKraftVector3;
                                               const ALimitEnabled:boolean=false;
                                               const AMotorEnabled:boolean=false;
                                               const AMinimumTranslationLimit:TKraftScalar=-1.0;
                                               const AMaximumTranslationLimit:TKraftScalar=1.0;
                                               const AMotorSpeed:TKraftScalar=0.0;
                                               const AMaximalMotorForce:TKraftScalar=0.0;
                                               const ACollideConnected:boolean=false);
begin

 fLimitState:=ALimitEnabled;

 fMotorState:=AMotorEnabled;

 fLowerLimit:=AMinimumTranslationLimit;

 fUpperLimit:=AMaximumTranslationLimit;

 fMotorSpeed:=AMotorSpeed;

 fMaximalMotorForce:=AMaximalMotorForce;

 fLocalAnchors[0]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyA.fWorldTransform);
 fLocalAnchors[1]:=Vector3TermMatrixMulInverted(AWorldAnchorPoint,ARigidBodyB.fWorldTransform);

 fSliderAxisBodyA:=Vector3NormEx(Vector3TermMatrixMulTransposedBasis(AWorldSliderAxis,ARigidBodyA.fWorldTransform));

 fAccumulatedImpulseLowerLimit:=0.0;
 fAccumulatedImpulseUpperLimit:=0.0;
 fAccumulatedImpulseMotor:=0.0;
 fAccumulatedImpulseTranslation:=Vector2Origin;
 fAccumulatedImpulseRotation:=Vector3Origin;

 fBiasLowerLimit:=0.0;
 fBiasUpperLimit:=0.0;

 fInverseInitialOrientationDifference:=QuaternionInverse(QuaternionTermNormalize(QuaternionMul(ARigidBodyB.fSweep.q0,QuaternionInverse(ARigidBodyA.fSweep.q0))));

 if ACollideConnected then begin
  Include(fFlags,kcfCollideConnected);
 end else begin
  Exclude(fFlags,kcfCollideConnected);
 end;

 fRigidBodies[0]:=ARigidBodyA;
 fRigidBodies[1]:=ARigidBodyB;

 inherited Create(APhysics);

end;

destructor TKraftConstraintJointSlider.Destroy;
begin
 inherited Destroy;
end;

procedure TKraftConstraintJointSlider.InitializeConstraintsAndWarmStart(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var cA,vA,wA,cB,vB,wB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    uDotSliderAxis,LowerLimitError,UpperLimitError,InverseMassOfBodies,ImpulseLimits,BiasFactor:TKraftScalar;
    TranslationKMatrix:TKraftMatrix2x2;
    OldIsLowerLimitViolated,OldIsUpperLimitViolated:boolean;
    u,R1PlusU,I1R1PlusUCrossN1,I1R1PlusUCrossN2,I2R2CrossN1,I2R2CrossN2,
    LinearImpulseLimits,ImpulseMotor,LinearImpulse,AngularImpulseA,AngularImpulseB:TKraftVector3;
    CurrentOrientationDifference,qError:TKraftQuaternion;
begin

 fIslandIndices[0]:=fRigidBodies[0].fIslandIndices[Island.fIslandIndex];
 fIslandIndices[1]:=fRigidBodies[1].fIslandIndices[Island.fIslandIndex];

 fLocalCenters[0]:=fRigidBodies[0].fSweep.LocalCenter;
 fLocalCenters[1]:=fRigidBodies[1].fSweep.LocalCenter;

 fInverseMasses[0]:=fRigidBodies[0].fInverseMass;
 fInverseMasses[1]:=fRigidBodies[1].fInverseMass;

 fWorldInverseInertiaTensors[0]:=fRigidBodies[0].fWorldInverseInertiaTensor;
 fWorldInverseInertiaTensors[1]:=fRigidBodies[1].fWorldInverseInertiaTensor;

 fSolverVelocities[0]:=@Island.fSolver.fVelocities[fIslandIndices[0]];
 fSolverVelocities[1]:=@Island.fSolver.fVelocities[fIslandIndices[1]];

 fSolverPositions[0]:=@Island.fSolver.fPositions[fIslandIndices[0]];
 fSolverPositions[1]:=@Island.fSolver.fPositions[fIslandIndices[1]];

 fSolverLinearFactors[0]:=@Island.fSolver.fLinearFactors[fIslandIndices[0]];
 fSolverLinearFactors[1]:=@Island.fSolver.fLinearFactors[fIslandIndices[1]];

 cA:=@fSolverPositions[0]^.Position;
 qA:=@fSolverPositions[0]^.Orientation;
 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 cB:=@fSolverPositions[1]^.Position;
 qB:=@fSolverPositions[1]^.Orientation;
 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 fRelativePositions[0]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
 fRelativePositions[1]:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);
 u:=Vector3Sub(Vector3Add(cB^,fRelativePositions[1]),Vector3Add(cA^,fRelativePositions[0]));

 fSliderAxisWorld:=Vector3NormEx(Vector3TermQuaternionRotate(fSliderAxisBodyA,qA^));

 fN1:=Vector3GetOneUnitOrthogonalVector(fSliderAxisWorld);
 fN2:=Vector3Cross(fSliderAxisWorld,fN1);

 uDotSliderAxis:=Vector3Dot(u,fSliderAxisWorld);

 LowerLimitError:=uDotSliderAxis-fLowerLimit;
 UpperLimitError:=fUpperLimit-uDotSliderAxis;
 OldIsLowerLimitViolated:=fIsLowerLimitViolated;
 fIsLowerLimitViolated:=LowerLimitError<=0.0;
 if fIsLowerLimitViolated<>OldIsLowerLimitViolated then begin
  fAccumulatedImpulseLowerLimit:=0.0;
 end;
 OldIsUpperLimitViolated:=fIsUpperLimitViolated;
 fIsUpperLimitViolated:=UpperLimitError<=0.0;
 if fIsUpperLimitViolated<>OldIsUpperLimitViolated then begin
  fAccumulatedImpulseUpperLimit:=0.0;
 end;

 fR2CrossN1:=Vector3Cross(fRelativePositions[1],fN1);
 fR2CrossN2:=Vector3Cross(fRelativePositions[1],fN2);
 fR2CrossSliderAxis:=Vector3Cross(fRelativePositions[1],fSliderAxisWorld);
 R1PlusU:=Vector3Cross(fRelativePositions[0],u);
 fR1PlusUCrossN1:=Vector3Cross(R1PlusU,fN1);
 fR1PlusUCrossN2:=Vector3Cross(R1PlusU,fN2);
 fR1PlusUCrossSliderAxis:=Vector3Cross(R1PlusU,fSliderAxisWorld);

 InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

 I1R1PlusUCrossN1:=Vector3TermMatrixMul(fR1PlusUCrossN1,fWorldInverseInertiaTensors[0]);
 I1R1PlusUCrossN2:=Vector3TermMatrixMul(fR1PlusUCrossN2,fWorldInverseInertiaTensors[0]);
 I2R2CrossN1:=Vector3TermMatrixMul(fR2CrossN1,fWorldInverseInertiaTensors[1]);
 I2R2CrossN2:=Vector3TermMatrixMul(fR2CrossN2,fWorldInverseInertiaTensors[1]);
 TranslationKMatrix[0,0]:=InverseMassOfBodies+Vector3Dot(fR1PlusUCrossN1,I1R1PlusUCrossN1)+Vector3Dot(fR2CrossN1,I2R2CrossN1);
 TranslationKMatrix[0,1]:=Vector3Dot(fR1PlusUCrossN1,I1R1PlusUCrossN2)+Vector3Dot(fR2CrossN1,I2R2CrossN2);
 TranslationKMatrix[1,0]:=Vector3Dot(fR1PlusUCrossN2,I1R1PlusUCrossN1)+Vector3Dot(fR2CrossN2,I2R2CrossN1);
 TranslationKMatrix[1,1]:=InverseMassOfBodies+Vector3Dot(fR1PlusUCrossN2,I1R1PlusUCrossN2)+Vector3Dot(fR2CrossN2,I2R2CrossN2);
 if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
  Matrix2x2Inverse(fInverseMassMatrixTranslationConstraint,TranslationKMatrix);
  fInverseMassMatrixRotationConstraint:=Matrix3x3TermInverse(Matrix3x3TermAdd(fWorldInverseInertiaTensors[0],fWorldInverseInertiaTensors[1]));
 end else begin
  fInverseMassMatrixTranslationConstraint:=Matrix2x2Null;
  fInverseMassMatrixRotationConstraint:=Matrix3x3Null;
 end;

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  BiasFactor:=0.0;

  fBiasTranslation:=Vector2Origin;

  fBiasRotation:=Vector3Origin;

 end else begin

  BiasFactor:=fPhysics.fConstraintBaumgarte/TimeStep.DeltaTime;

  fBiasTranslation:=Vector2ScalarMul(Vector2(Vector3Dot(u,fN1),Vector3Dot(u,fN2)),BiasFactor);

  CurrentOrientationDifference:=QuaternionTermNormalize(QuaternionMul(qB^,QuaternionInverse(qA^)));
  qError:=QuaternionMul(CurrentOrientationDifference,fInverseInitialOrientationDifference);
  fBiasRotation:=Vector3ScalarMul(Vector3(qError.x,qError.y,qError.z),BiasFactor*2.0);

 end;

 if fLimitState and (fIsLowerLimitViolated or fIsUpperLimitViolated) then begin
  // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
  fInverseMassMatrixLimit:=InverseMassOfBodies+
                           Vector3Dot(fR1PlusUCrossSliderAxis,Vector3TermMatrixMul(fR1PlusUCrossSliderAxis,fWorldInverseInertiaTensors[0]))+
                           Vector3Dot(fR2CrossSliderAxis,Vector3TermMatrixMul(fR2CrossSliderAxis,fWorldInverseInertiaTensors[1]));
  if fInverseMassMatrixLimit>0.0 then begin
   fInverseMassMatrixLimit:=1.0/fInverseMassMatrixLimit;
  end else begin
   fInverseMassMatrixLimit:=0.0;
  end;
  if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin
   fBiasLowerLimit:=0.0;
   fBiasUpperLimit:=0.0;
  end else begin
   fBiasLowerLimit:=LowerLimitError*BiasFactor;
   fBiasUpperLimit:=UpperLimitError*BiasFactor;
  end;
 end;

 if fMotorState then begin
  // Compute the inverse of the mass matrix K=JM^-1J^t for the motor (1x1 matrix)
  fInverseMassMatrixMotor:=InverseMassOfBodies;
  if fInverseMassMatrixMotor>0.0 then begin
   fInverseMassMatrixMotor:=1.0/fInverseMassMatrixMotor;
  end else begin
   fInverseMassMatrixMotor:=0.0;
  end;
 end;

 if fPhysics.fWarmStarting then begin

  // Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body A
  ImpulseLimits:=fAccumulatedImpulseLowerLimit-fAccumulatedImpulseUpperLimit;
  LinearImpulseLimits:=Vector3ScalarMul(fSliderAxisWorld,ImpulseLimits);

  // Compute the impulse P=J^T * lambda for the motor constraint of body 1
  ImpulseMotor:=Vector3ScalarMul(fSliderAxisWorld,-fAccumulatedImpulseMotor);

  // Compute the linear impulse P=J^T * lambda for the 2 translation constraints for bodies A and B
  LinearImpulse:=Vector3ScalarMul(Vector3Add(Vector3Add(Vector3ScalarMul(fN1,fAccumulatedImpulseTranslation.x),
                                                        Vector3ScalarMul(fN2,fAccumulatedImpulseTranslation.y)),
                                             Vector3Add(LinearImpulseLimits,ImpulseMotor)),TimeStep.DeltaTimeRatio);

  // Compute the angular impulse P=J^T * lambda for the 2 translation constraints for body A
  AngularImpulseA:=Vector3ScalarMul(Vector3Add(Vector3Add(Vector3ScalarMul(fR1PlusUCrossN1,fAccumulatedImpulseTranslation.x),
                                                          Vector3ScalarMul(fR1PlusUCrossN2,fAccumulatedImpulseTranslation.y)),
                                               Vector3Add(Vector3ScalarMul(fR1PlusUCrossSliderAxis,ImpulseLimits),
                                                                           fAccumulatedImpulseRotation)),TimeStep.DeltaTimeRatio);

  // Compute the angular impulse P=J^T * lambda for the 2 translation constraints for body B
  AngularImpulseB:=Vector3ScalarMul(Vector3Add(Vector3Add(Vector3ScalarMul(fR2CrossN1,fAccumulatedImpulseTranslation.x),
                                                          Vector3ScalarMul(fR2CrossN2,fAccumulatedImpulseTranslation.y)),
                                               Vector3Add(Vector3ScalarMul(fR2CrossSliderAxis,ImpulseLimits),
                                                                           fAccumulatedImpulseRotation)),TimeStep.DeltaTimeRatio);

  // Apply impulses
  Vector3DirectSub(vA^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  Vector3DirectSub(wA^,Vector3TermMatrixMul(AngularImpulseA,fWorldInverseInertiaTensors[0]));

  Vector3DirectAdd(vB^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(AngularImpulseB,fWorldInverseInertiaTensors[1]));

 end else begin

  fAccumulatedImpulseTranslation:=Vector2Origin;
  fAccumulatedImpulseRotation:=Vector3Origin;
  fAccumulatedImpulseLowerLimit:=0.0;
  fAccumulatedImpulseUpperLimit:=0.0;
  fAccumulatedImpulseMotor:=0.0;

 end;

end;

procedure TKraftConstraintJointSlider.SolveVelocityConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep);
var vA,wA,vB,wB:PKraftVector3;
    LinearImpulse,AngularImpulseA,AngularImpulseB,JvRotation,RotationImpulse:TKraftVector3;
    JvTranslation,TranslationImpulse:TKraftVector2;
    JvLowerLimit,ImpulseLower,JvUpperLimit,ImpulseUpper,JvMotor,ImpulseMotor,LambdaTemp,MaximalMotorImpulse:TKraftScalar;
begin

 vA:=@fSolverVelocities[0]^.LinearVelocity;
 wA:=@fSolverVelocities[0]^.AngularVelocity;

 vB:=@fSolverVelocities[1]^.LinearVelocity;
 wB:=@fSolverVelocities[1]^.AngularVelocity;

 (**** Translation ****)

 // Compute J*v for the 2 translation constraints
 JvTranslation.x:=((Vector3Dot(fN1,vB^)+Vector3Dot(wB^,fR2CrossN1))-(Vector3Dot(fN1,vA^)+Vector3Dot(wA^,fR1PlusUCrossN1)))+fBiasTranslation.x;
 JvTranslation.y:=((Vector3Dot(fN2,vB^)+Vector3Dot(wB^,fR2CrossN2))-(Vector3Dot(fN2,vA^)+Vector3Dot(wA^,fR1PlusUCrossN2)))+fBiasTranslation.y;

 TranslationImpulse.x:=-((JvTranslation.x*fInverseMassMatrixTranslationConstraint[0,0])+(JvTranslation.y*fInverseMassMatrixTranslationConstraint[0,1]));
 TranslationImpulse.y:=-((JvTranslation.x*fInverseMassMatrixTranslationConstraint[1,0])+(JvTranslation.y*fInverseMassMatrixTranslationConstraint[1,1]));

 fAccumulatedImpulseTranslation.x:=fAccumulatedImpulseTranslation.x+TranslationImpulse.x;
 fAccumulatedImpulseTranslation.y:=fAccumulatedImpulseTranslation.y+TranslationImpulse.y;

 LinearImpulse:=Vector3Add(Vector3ScalarMul(fN1,TranslationImpulse.x),
                           Vector3ScalarMul(fN2,TranslationImpulse.y));

 AngularImpulseA:=Vector3Add(Vector3ScalarMul(fR1PlusUCrossN1,TranslationImpulse.x),
                                 Vector3ScalarMul(fR1PlusUCrossN2,TranslationImpulse.y));

 AngularImpulseB:=Vector3Add(Vector3ScalarMul(fR2CrossN1,TranslationImpulse.x),
                                 Vector3ScalarMul(fR2CrossN2,TranslationImpulse.y));

 Vector3DirectSub(vA^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
 Vector3DirectSub(wA^,Vector3TermMatrixMul(AngularImpulseA,fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(vB^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
 Vector3DirectAdd(wB^,Vector3TermMatrixMul(AngularImpulseB,fWorldInverseInertiaTensors[1]));

 (**** Rotation ****)

 JvRotation:=Vector3Sub(wB^,wA^);

 RotationImpulse:=Vector3TermMatrixMul(Vector3Sub(Vector3Neg(JvRotation),fBiasRotation),fInverseMassMatrixRotationConstraint);

 Vector3DirectAdd(fAccumulatedImpulseRotation,RotationImpulse);

 Vector3DirectSub(wA^,Vector3TermMatrixMul(RotationImpulse,fWorldInverseInertiaTensors[0]));

 Vector3DirectAdd(wB^,Vector3TermMatrixMul(RotationImpulse,fWorldInverseInertiaTensors[1]));

 (**** Limits ****)

 if fLimitState then begin
  if fIsLowerLimitViolated then begin
   JvLowerLimit:=(Vector3Dot(fSliderAxisWorld,vB^)+Vector3Dot(fR2CrossSliderAxis,wB^))-(Vector3Dot(fSliderAxisWorld,vA^)+Vector3Dot(fR1PlusUCrossSliderAxis,wA^));
   ImpulseLower:=fInverseMassMatrixLimit*(-(JvLowerLimit+fBiasLowerLimit));
   LambdaTemp:=fAccumulatedImpulseLowerLimit;
   fAccumulatedImpulseLowerLimit:=Max(0.0,fAccumulatedImpulseLowerLimit+ImpulseLower);
   ImpulseLower:=fAccumulatedImpulseLowerLimit-LambdaTemp;
   LinearImpulse:=Vector3ScalarMul(fSliderAxisWorld,ImpulseLower);
   AngularImpulseA:=Vector3ScalarMul(fR1PlusUCrossSliderAxis,ImpulseLower);
   AngularImpulseB:=Vector3ScalarMul(fR2CrossSliderAxis,ImpulseLower);
   Vector3DirectSub(vA^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
   Vector3DirectSub(wA^,Vector3TermMatrixMul(AngularImpulseA,fWorldInverseInertiaTensors[0]));
   Vector3DirectAdd(vB^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
   Vector3DirectAdd(wB^,Vector3TermMatrixMul(AngularImpulseB,fWorldInverseInertiaTensors[1]));
  end;
  if fIsUpperLimitViolated then begin
   JvUpperLimit:=(Vector3Dot(fSliderAxisWorld,vA^)+Vector3Dot(fR1PlusUCrossSliderAxis,wA^))-(Vector3Dot(fSliderAxisWorld,vB^)+Vector3Dot(fR2CrossSliderAxis,wB^));
   ImpulseUpper:=fInverseMassMatrixLimit*(-(JvUpperLimit+fBiasUpperLimit));
   LambdaTemp:=fAccumulatedImpulseUpperLimit;
   fAccumulatedImpulseUpperLimit:=Max(0.0,fAccumulatedImpulseUpperLimit+ImpulseUpper);
   ImpulseUpper:=-(fAccumulatedImpulseUpperLimit-LambdaTemp);
   LinearImpulse:=Vector3ScalarMul(fSliderAxisWorld,ImpulseUpper);
   AngularImpulseA:=Vector3ScalarMul(fR1PlusUCrossSliderAxis,ImpulseUpper);
   AngularImpulseB:=Vector3ScalarMul(fR2CrossSliderAxis,ImpulseUpper);
   Vector3DirectSub(vA^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
   Vector3DirectSub(wA^,Vector3TermMatrixMul(AngularImpulseA,fWorldInverseInertiaTensors[0]));
   Vector3DirectAdd(vB^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
   Vector3DirectAdd(wB^,Vector3TermMatrixMul(AngularImpulseB,fWorldInverseInertiaTensors[1]));
  end;
 end;

 (**** Motor ****)

 if fMotorState then begin
  JvMotor:=Vector3Dot(fSliderAxisWorld,vA^)-Vector3Dot(fSliderAxisWorld,vB^);
  ImpulseMotor:=fInverseMassMatrixMotor*(-JvMotor);
  LambdaTemp:=fAccumulatedImpulseMotor;
  MaximalMotorImpulse:=fMaximalMotorForce*TimeStep.DeltaTime;
  fAccumulatedImpulseMotor:=Min(Max(fAccumulatedImpulseMotor+ImpulseMotor,-MaximalMotorImpulse),MaximalMotorImpulse);
  ImpulseMotor:=-(fAccumulatedImpulseMotor-LambdaTemp);
  LinearImpulse:=Vector3ScalarMul(fSliderAxisWorld,ImpulseMotor);
  Vector3DirectSub(wA^,Vector3TermMatrixMul(LinearImpulse,fWorldInverseInertiaTensors[0]));
  Vector3DirectAdd(wB^,Vector3TermMatrixMul(LinearImpulse,fWorldInverseInertiaTensors[1]));
 end;

end;

function TKraftConstraintJointSlider.SolvePositionConstraint(const Island:TKraftIsland;const TimeStep:TKraftTimeStep):boolean;
var cA,cB:PKraftVector3;
    qA,qB:PKraftQuaternion;
    rA,rB,u,R1PlusU,I1R1PlusUCrossN1,I1R1PlusUCrossN2,I2R2CrossN1,I2R2CrossN2,
    LinearImpulse,AngularImpulseA,AngularImpulseB,RotationError,RotationImpulse:TKraftVector3;
    TranslationError,TranslationImpulse:TKraftVector2;
    uDotSliderAxis,InverseMassOfBodies,LowerLimitError,UpperLimitError,ImpulseLower,ImpulseUpper:TKraftScalar;
    TranslationKMatrix:TKraftMatrix2x2;
    CurrentOrientationDifference,qError:TKraftQuaternion;
begin

 if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  cA:=@fSolverPositions[0]^.Position;
  qA:=@fSolverPositions[0]^.Orientation;

  cB:=@fSolverPositions[1]^.Position;
  qB:=@fSolverPositions[1]^.Orientation;

  rA:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),qA^);
  rB:=Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),qB^);

  u:=Vector3Sub(Vector3Add(cB^,rB),Vector3Add(cA^,rA));

  fSliderAxisWorld:=Vector3NormEx(Vector3TermQuaternionRotate(fSliderAxisBodyA,qA^));

  fN1:=Vector3GetOneUnitOrthogonalVector(fSliderAxisWorld);
  fN2:=Vector3Cross(fSliderAxisWorld,fN1);

  uDotSliderAxis:=Vector3Dot(u,fSliderAxisWorld);

  LowerLimitError:=uDotSliderAxis-fLowerLimit;
  UpperLimitError:=fUpperLimit-uDotSliderAxis;
  fIsLowerLimitViolated:=LowerLimitError<=0.0;
  fIsUpperLimitViolated:=UpperLimitError<=0.0;

  fR2CrossN1:=Vector3Cross(fRelativePositions[1],fN1);
  fR2CrossN2:=Vector3Cross(fRelativePositions[1],fN2);
  fR2CrossSliderAxis:=Vector3Cross(fRelativePositions[1],fSliderAxisWorld);
  R1PlusU:=Vector3Cross(fRelativePositions[0],u);
  fR1PlusUCrossN1:=Vector3Cross(R1PlusU,fN1);
  fR1PlusUCrossN2:=Vector3Cross(R1PlusU,fN2);
  fR1PlusUCrossSliderAxis:=Vector3Cross(R1PlusU,fSliderAxisWorld);

  (**** Translation ****)

  InverseMassOfBodies:=fRigidBodies[0].fInverseMass+fRigidBodies[1].fInverseMass;

  I1R1PlusUCrossN1:=Vector3TermMatrixMul(fR1PlusUCrossN1,fWorldInverseInertiaTensors[0]);
  I1R1PlusUCrossN2:=Vector3TermMatrixMul(fR1PlusUCrossN2,fWorldInverseInertiaTensors[0]);
  I2R2CrossN1:=Vector3TermMatrixMul(fR2CrossN1,fWorldInverseInertiaTensors[1]);
  I2R2CrossN2:=Vector3TermMatrixMul(fR2CrossN2,fWorldInverseInertiaTensors[1]);
  TranslationKMatrix[0,0]:=InverseMassOfBodies+Vector3Dot(fR1PlusUCrossN1,I1R1PlusUCrossN1)+Vector3Dot(fR2CrossN1,I2R2CrossN1);
  TranslationKMatrix[0,1]:=Vector3Dot(fR1PlusUCrossN1,I1R1PlusUCrossN2)+Vector3Dot(fR2CrossN1,I2R2CrossN2);
  TranslationKMatrix[1,0]:=Vector3Dot(fR1PlusUCrossN2,I1R1PlusUCrossN1)+Vector3Dot(fR2CrossN2,I2R2CrossN1);
  TranslationKMatrix[1,1]:=InverseMassOfBodies+Vector3Dot(fR1PlusUCrossN2,I1R1PlusUCrossN2)+Vector3Dot(fR2CrossN2,I2R2CrossN2);
  if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
   Matrix2x2Inverse(fInverseMassMatrixTranslationConstraint,TranslationKMatrix);
  end else begin
   fInverseMassMatrixRotationConstraint:=Matrix3x3Null;
  end;

  TranslationError.x:=Vector3Dot(u,fN1);
  TranslationError.y:=Vector3Dot(u,fN2);

  TranslationImpulse.x:=-((TranslationError.x*fInverseMassMatrixTranslationConstraint[0,0])+(TranslationError.y*fInverseMassMatrixTranslationConstraint[0,1]));
  TranslationImpulse.y:=-((TranslationError.x*fInverseMassMatrixTranslationConstraint[1,0])+(TranslationError.y*fInverseMassMatrixTranslationConstraint[1,1]));

  LinearImpulse:=Vector3Add(Vector3ScalarMul(fN1,TranslationImpulse.x),
                            Vector3ScalarMul(fN2,TranslationImpulse.y));

  AngularImpulseA:=Vector3Add(Vector3ScalarMul(fR1PlusUCrossN1,TranslationImpulse.x),
                              Vector3ScalarMul(fR1PlusUCrossN2,TranslationImpulse.y));

  AngularImpulseB:=Vector3Add(Vector3ScalarMul(fR2CrossN1,TranslationImpulse.x),
                              Vector3ScalarMul(fR2CrossN2,TranslationImpulse.y));

  result:=Vector2Length(TranslationError)<fPhysics.fLinearSlop;

  Vector3DirectSub(cA^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(AngularImpulseA),fWorldInverseInertiaTensors[0]),1.0);

  Vector3DirectAdd(cB^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(AngularImpulseB,fWorldInverseInertiaTensors[1]),1.0);

  (**** Rotation ****)

  if (fRigidBodies[0].fRigidBodyType=krbtDynamic) or (fRigidBodies[1].fRigidBodyType=krbtDynamic) then begin
   fInverseMassMatrixRotationConstraint:=Matrix3x3TermInverse(Matrix3x3TermAdd(fWorldInverseInertiaTensors[0],fWorldInverseInertiaTensors[1]));
  end else begin
   fInverseMassMatrixRotationConstraint:=Matrix3x3Null;
  end;

  CurrentOrientationDifference:=QuaternionTermNormalize(QuaternionMul(qB^,QuaternionInverse(qA^)));
  qError:=QuaternionMul(CurrentOrientationDifference,fInverseInitialOrientationDifference);
  RotationError.x:=qError.x*2.0;
  RotationError.y:=qError.y*2.0;
  RotationError.z:=qError.z*2.0;

  RotationImpulse:=Vector3TermMatrixMul(Vector3Neg(RotationError),fInverseMassMatrixRotationConstraint);

  result:=result and (Vector3Length(RotationError)<fPhysics.fAngularSlop);

  QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(RotationImpulse),fWorldInverseInertiaTensors[0]),1.0);

  QuaternionDirectSpin(qB^,Vector3TermMatrixMul(RotationImpulse,fWorldInverseInertiaTensors[1]),1.0);

  (**** Limits ****)

  if fLimitState then begin
   if fIsLowerLimitViolated or fIsUpperLimitViolated then begin
    fInverseMassMatrixLimit:=InverseMassOfBodies+
                            Vector3Dot(fR1PlusUCrossSliderAxis,Vector3TermMatrixMul(fR1PlusUCrossSliderAxis,fWorldInverseInertiaTensors[0]))+
                            Vector3Dot(fR2CrossSliderAxis,Vector3TermMatrixMul(fR2CrossSliderAxis,fWorldInverseInertiaTensors[1]));
    if fInverseMassMatrixLimit>0.0 then begin
     fInverseMassMatrixLimit:=1.0/fInverseMassMatrixLimit;
    end else begin
     fInverseMassMatrixLimit:=0.0;
    end;
   end;
   if fIsLowerLimitViolated then begin
    ImpulseLower:=fInverseMassMatrixLimit*(-LowerLimitError);
    LinearImpulse:=Vector3ScalarMul(fSliderAxisWorld,ImpulseLower);
    AngularImpulseA:=Vector3ScalarMul(fR1PlusUCrossSliderAxis,ImpulseLower);
    AngularImpulseB:=Vector3ScalarMul(fR2CrossSliderAxis,ImpulseLower);
    result:=result and (LowerLimitError<fPhysics.fLinearSlop);
    Vector3DirectSub(cA^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
    QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(AngularImpulseA),fWorldInverseInertiaTensors[0]),1.0);
    Vector3DirectAdd(cB^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
    QuaternionDirectSpin(qB^,Vector3TermMatrixMul(AngularImpulseB,fWorldInverseInertiaTensors[1]),1.0);
   end;
   if fIsUpperLimitViolated then begin
    ImpulseUpper:=-(fInverseMassMatrixLimit*(-UpperLimitError));
    LinearImpulse:=Vector3ScalarMul(fSliderAxisWorld,ImpulseUpper);
    AngularImpulseA:=Vector3ScalarMul(fR1PlusUCrossSliderAxis,ImpulseUpper);
    AngularImpulseB:=Vector3ScalarMul(fR2CrossSliderAxis,ImpulseUpper);
    result:=result and (UpperLimitError<fPhysics.fLinearSlop);
    Vector3DirectSub(cA^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[0]^,fInverseMasses[0])));
    QuaternionDirectSpin(qA^,Vector3TermMatrixMul(Vector3Neg(AngularImpulseA),fWorldInverseInertiaTensors[0]),1.0);
    Vector3DirectAdd(cB^,Vector3Mul(LinearImpulse,Vector3ScalarMul(fSolverLinearFactors[1]^,fInverseMasses[1])));
    QuaternionDirectSpin(qB^,Vector3TermMatrixMul(AngularImpulseB,fWorldInverseInertiaTensors[1]),1.0);
   end;
  end;

 end else begin

  result:=true;

 end;
 
end;

function TKraftConstraintJointSlider.GetAnchorA:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[0],fRigidBodies[0].fWorldTransform);
end;

function TKraftConstraintJointSlider.GetAnchorB:TKraftVector3;
begin
 result:=Vector3TermMatrixMul(fLocalAnchors[1],fRigidBodies[1].fWorldTransform);
end;

function TKraftConstraintJointSlider.GetReactionForce(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(Vector3Add(Vector3ScalarMul(fN1,fAccumulatedImpulseTranslation.x),
                                     Vector3ScalarMul(fN2,fAccumulatedImpulseTranslation.y)),InverseDeltaTime);
end;

function TKraftConstraintJointSlider.GetReactionTorque(const InverseDeltaTime:TKraftScalar):TKraftVector3;
begin
 result:=Vector3ScalarMul(fAccumulatedImpulseRotation,InverseDeltaTime);
end;

function TKraftConstraintJointSlider.IsLimitEnabled:boolean;
begin
 result:=fLimitState;
end;

function TKraftConstraintJointSlider.IsMotorEnabled:boolean;
begin
 result:=fMotorState;
end;

function TKraftConstraintJointSlider.GetMinimumTranslationLimit:TKraftScalar;
begin
 result:=fLowerLimit;
end;

function TKraftConstraintJointSlider.GetMaximumTranslationLimit:TKraftScalar;
begin
 result:=fUpperLimit;
end;

function TKraftConstraintJointSlider.GetMotorSpeed:TKraftScalar;
begin
 result:=fMotorSpeed;
end;

function TKraftConstraintJointSlider.GetMaximalMotorForce:TKraftScalar;
begin
 result:=fMaximalMotorForce;
end;

function TKraftConstraintJointSlider.GetMotorForce(const DeltaTime:TKraftScalar):TKraftScalar;
begin
 result:=fAccumulatedImpulseMotor/DeltaTime;
end;

function TKraftConstraintJointSlider.GetTranslation:TKraftScalar;
begin
 result:=Vector3Dot(Vector3Sub(Vector3Add(fSolverPositions[1]^.Position,Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[1],fLocalCenters[1]),fSolverPositions[1]^.Orientation)),
                                       Vector3Add(fSolverPositions[0]^.Position,Vector3TermQuaternionRotate(Vector3Sub(fLocalAnchors[0],fLocalCenters[0]),fSolverPositions[0]^.Orientation))),
                        Vector3TermQuaternionRotate(fSliderAxisBodyA,fSolverPositions[0]^.Orientation));
end;

procedure TKraftConstraintJointSlider.ResetLimits;
begin
 fAccumulatedImpulseLowerLimit:=0.0;
 fAccumulatedImpulseUpperLimit:=0.0;
 fRigidBodies[0].SetToAwake;
 fRigidBodies[1].SetToAwake;
end;

procedure TKraftConstraintJointSlider.EnableLimit(const ALimitEnabled:boolean);
begin
 if fLimitState<>ALimitEnabled then begin
  fLimitState:=ALimitEnabled;
  ResetLimits;
 end;
end;

procedure TKraftConstraintJointSlider.EnableMotor(const AMotorEnabled:boolean);
begin
 if fMotorState<>AMotorEnabled then begin
  fMotorState:=AMotorEnabled;
  fAccumulatedImpulseMotor:=0.0;
  fRigidBodies[0].SetToAwake;
  fRigidBodies[1].SetToAwake;
 end;
end;

procedure TKraftConstraintJointSlider.SetMinimumTranslationLimit(const AMinimumTranslationLimit:TKraftScalar);
begin
 if fLowerLimit<>AMinimumTranslationLimit then begin
  fLowerLimit:=AMinimumTranslationLimit;
  ResetLimits;
 end;
end;

procedure TKraftConstraintJointSlider.SetMaximumTranslationLimit(const AMaximumTranslationLimit:TKraftScalar);
begin
 if fUpperLimit<>AMaximumTranslationLimit then begin
  fUpperLimit:=AMaximumTranslationLimit;
  ResetLimits;
 end;
end;

procedure TKraftConstraintJointSlider.SetMotorSpeed(const AMotorSpeed:TKraftScalar);
begin
 if fMotorSpeed<>AMotorSpeed then begin
  fMotorSpeed:=AMotorSpeed;
  fRigidBodies[0].SetToAwake;
  fRigidBodies[1].SetToAwake;
 end;
end;

procedure TKraftConstraintJointSlider.SetMaximalMotorForce(const AMaximalMotorForce:TKraftScalar);
begin
 if fMaximalMotorForce<>AMaximalMotorForce then begin
  fMaximalMotorForce:=AMaximalMotorForce;
  fRigidBodies[0].SetToAwake;
  fRigidBodies[1].SetToAwake;
 end;
end;

constructor TKraftSolver.Create(const APhysics:TKraft;const AIsland:TKraftIsland);
begin
 inherited Create;

 fPhysics:=APhysics;

 fIsland:=AIsland;

 fVelocities:=nil;
 SetLength(fVelocities,64);
 fCountVelocities:=0;

 fPositions:=nil;
 SetLength(fPositions,64);
 fCountPositions:=0;

 fLinearFactors:=nil;
 SetLength(fLinearFactors,64);
 fCountLinearFactors:=0;

 fVelocityStates:=nil;
 SetLength(fVelocityStates,64);
 fCountVelocityStates:=0;

 fPositionStates:=nil;
 SetLength(fPositionStates,64);
 fCountPositionStates:=0;

 fSpeculativeContactStates:=nil;
 SetLength(fSpeculativeContactStates,64);
 fCountSpeculativeContactStates:=0;

 fCountContacts:=0;

end;

destructor TKraftSolver.Destroy;
begin
 SetLength(fVelocities,0);
 SetLength(fPositions,0);
 SetLength(fLinearFactors,0);
 SetLength(fVelocityStates,0);
 SetLength(fPositionStates,0);
 SetLength(fSpeculativeContactStates,0);
 inherited Destroy;
end;

procedure TKraftSolver.Initialize(const TimeStep:TKraftTimeStep);
begin
 fDeltaTime:=TimeStep.DeltaTime;
 fDeltaTimeRatio:=TimeStep.DeltaTimeRatio;
 fEnableFriction:=fPhysics.fEnableFriction;
 fPositionCorrectionMode:=fPhysics.fContactPositionCorrectionMode;
end;

procedure TKraftSolver.Store;
var ContactPairIndex,ContactIndex:longint;
    VelocityState:PKraftSolverVelocityState;
    PositionState:PKraftSolverPositionState;
    ContactPair:PKraftContactPair;
    Contact:PKraftContact;
    ContactPoint:PKraftSolverVelocityStateContactPoint;
    SpeculativeContactState:PKraftSolverSpeculativeContactState;
begin

 if fIsland.fCountRigidBodies>length(fVelocities) then begin
  SetLength(fVelocities,fIsland.fCountRigidBodies*2);
 end;
 fCountVelocities:=fIsland.fCountRigidBodies;

 if fIsland.fCountRigidBodies>length(fPositions) then begin
  SetLength(fPositions,fIsland.fCountRigidBodies*2);
 end;
 fCountPositions:=fIsland.fCountRigidBodies;

 if fIsland.fCountRigidBodies>length(fLinearFactors) then begin
  SetLength(fLinearFactors,fIsland.fCountRigidBodies*2);
 end;
 fCountLinearFactors:=fIsland.fCountRigidBodies;

 fCountContacts:=fIsland.fCountContactPairs;

 if fCountContacts>length(fVelocityStates) then begin
  SetLength(fVelocityStates,fCountContacts*2);
 end;
 fCountVelocityStates:=fCountContacts;

 if fCountContacts>length(fPositionStates) then begin
  SetLength(fPositionStates,fCountContacts*2);
 end;
 fCountPositionStates:=fCountContacts;

 fCountSpeculativeContacts:=fIsland.fCountSpeculativeContactPairs;

 if fCountSpeculativeContacts>length(fSpeculativeContactStates) then begin
  SetLength(fSpeculativeContactStates,fCountSpeculativeContacts*2);
 end;
 fCountSpeculativeContactStates:=fCountSpeculativeContacts;

 for ContactPairIndex:=0 to fCountContacts-1 do begin

  ContactPair:=fIsland.fContactPairs[ContactPairIndex];

  VelocityState:=@fVelocityStates[ContactPairIndex];
  VelocityState^.Centers[0]:=ContactPair^.RigidBodies[0].fSweep.c0;
  VelocityState^.Centers[1]:=ContactPair^.RigidBodies[1].fSweep.c0;
  VelocityState^.WorldInverseInertiaTensors[0]:=ContactPair^.RigidBodies[0].fWorldInverseInertiaTensor;
  VelocityState^.WorldInverseInertiaTensors[1]:=ContactPair^.RigidBodies[1].fWorldInverseInertiaTensor;
  VelocityState^.NormalMass:=0.0;
  VelocityState^.TangentMass[0]:=0.0;
  VelocityState^.TangentMass[1]:=0.0;
  VelocityState^.InverseMasses[0]:=ContactPair^.RigidBodies[0].fInverseMass;
  VelocityState^.InverseMasses[1]:=ContactPair^.RigidBodies[1].fInverseMass;
  VelocityState^.Restitution:=ContactPair^.Restitution;
  VelocityState^.Friction:=ContactPair^.Friction;
  VelocityState^.Indices[0]:=ContactPair^.RigidBodies[0].fIslandIndices[fIsland.fIslandIndex];
  VelocityState^.Indices[1]:=ContactPair^.RigidBodies[1].fIslandIndices[fIsland.fIslandIndex];
  VelocityState^.CountPoints:=ContactPair^.Manifold.CountContacts;

  PositionState:=@fPositionStates[ContactPairIndex];
  PositionState^.LocalNormal:=ContactPair^.Manifold.LocalNormal;
  PositionState^.LocalCenters[0]:=ContactPair^.RigidBodies[0].fSweep.LocalCenter;
  PositionState^.LocalCenters[1]:=ContactPair^.RigidBodies[1].fSweep.LocalCenter;
  PositionState^.WorldInverseInertiaTensors[0]:=ContactPair^.RigidBodies[0].fWorldInverseInertiaTensor;
  PositionState^.WorldInverseInertiaTensors[1]:=ContactPair^.RigidBodies[1].fWorldInverseInertiaTensor;
  PositionState^.InverseMasses[0]:=ContactPair^.RigidBodies[0].fInverseMass;
  PositionState^.InverseMasses[1]:=ContactPair^.RigidBodies[1].fInverseMass;
  PositionState^.Indices[0]:=ContactPair^.RigidBodies[0].fIslandIndices[fIsland.fIslandIndex];
  PositionState^.Indices[1]:=ContactPair^.RigidBodies[1].fIslandIndices[fIsland.fIslandIndex];
  PositionState^.CountPoints:=ContactPair^.Manifold.CountContacts;

  VelocityState^.LostSpeculativeBounce:=ContactPair^.Manifold.LostSpeculativeBounce;
  ContactPair^.Manifold.LostSpeculativeBounce:=0;
    
  for ContactIndex:=0 to ContactPair^.Manifold.CountContacts-1 do begin

   Contact:=@ContactPair^.Manifold.Contacts[ContactIndex];

   ContactPoint:=@VelocityState^.Points[ContactIndex];

   ContactPoint^.RelativePositions[0]:=Vector3Origin;
   ContactPoint^.RelativePositions[1]:=Vector3Origin;
   if fPhysics.fWarmStarting then begin
    ContactPoint^.NormalImpulse:=Contact^.NormalImpulse*fDeltaTimeRatio;
    ContactPoint^.TangentImpulse[0]:=Contact^.TangentImpulse[0]*fDeltaTimeRatio;
    ContactPoint^.TangentImpulse[1]:=Contact^.TangentImpulse[1]*fDeltaTimeRatio;
   end else begin
    ContactPoint^.NormalImpulse:=0.0;
    ContactPoint^.TangentImpulse[0]:=0.0;
    ContactPoint^.TangentImpulse[1]:=0.0;
   end;
   ContactPoint^.Bias:=0.0;
   ContactPoint^.NormalMass:=0.0;
   ContactPoint^.TangentMass[0]:=0.0;
   ContactPoint^.TangentMass[1]:=0.0;

   PositionState^.LocalPoints[ContactIndex]:=Contact^.LocalPoints[ContactIndex];

  end;

 end;

 for ContactPairIndex:=0 to fCountSpeculativeContacts-1 do begin
  ContactPair:=fIsland.fSpeculativeContactPairs[ContactPairIndex];
  Contact:=@ContactPair^.Manifold.Contacts[0];
  SpeculativeContactState:=@fSpeculativeContactStates[ContactPairIndex];
  SpeculativeContactState^.Centers[0]:=ContactPair^.RigidBodies[0].fSweep.c0;
  SpeculativeContactState^.Centers[1]:=ContactPair^.RigidBodies[1].fSweep.c0;
  SpeculativeContactState^.LocalCenters[0]:=ContactPair^.RigidBodies[0].fSweep.LocalCenter;
  SpeculativeContactState^.LocalCenters[1]:=ContactPair^.RigidBodies[1].fSweep.LocalCenter;
  SpeculativeContactState^.WorldInverseInertiaTensors[0]:=ContactPair^.RigidBodies[0].fWorldInverseInertiaTensor;
  SpeculativeContactState^.WorldInverseInertiaTensors[1]:=ContactPair^.RigidBodies[1].fWorldInverseInertiaTensor;
  SpeculativeContactState^.InverseMasses[0]:=ContactPair^.RigidBodies[0].fInverseMass;
  SpeculativeContactState^.InverseMasses[1]:=ContactPair^.RigidBodies[1].fInverseMass;
  SpeculativeContactState^.Restitution:=ContactPair^.Restitution;
  SpeculativeContactState^.Friction:=ContactPair^.Friction;
  SpeculativeContactState^.Indices[0]:=ContactPair^.RigidBodies[0].fIslandIndices[fIsland.fIslandIndex];
  SpeculativeContactState^.Indices[1]:=ContactPair^.RigidBodies[1].fIslandIndices[fIsland.fIslandIndex];
  SpeculativeContactState^.LostSpeculativeBounce:=ContactPair^.Manifold.LostSpeculativeBounce;
  SpeculativeContactState^.RelativePositions[0]:=Vector3Origin;
  SpeculativeContactState^.RelativePositions[1]:=Vector3Origin;
  SpeculativeContactState^.NormalImpulse:=Contact^.NormalImpulse;
  SpeculativeContactState^.TangentImpulse[0]:=Contact^.TangentImpulse[0];
  SpeculativeContactState^.TangentImpulse[1]:=Contact^.TangentImpulse[1];
  SpeculativeContactState^.NormalMass:=0.0;
  SpeculativeContactState^.TangentMass[0]:=0.0;
  SpeculativeContactState^.TangentMass[1]:=0.0;
 end;

end;

procedure TKraftSolver.InitializeConstraints;
var ContactPairIndex,ContactIndex,TangentIndex,IndexA,IndexB:longint;
    VelocityState:PKraftSolverVelocityState;
    PositionState:PKraftSolverPositionState;
    ContactPoint:PKraftSolverVelocityStateContactPoint;
    SolverContact:PKraftSolverContact;
    iA,iB:PKraftMatrix3x3;
    mA,mB,NormalMass,TangentMass,dv,LostSpeculativeBounce,RestitutionBias:TKraftScalar;
    LocalCenterA,LocalCenterB,cA,vA,wA,rnA,rtA,cB,vB,wB,rnB,rtB,P,Temp:TKraftVector3;
    TangentVectors:array[0..1] of TKraftVector3;
    qA,qB:TKraftQuaternion;
    tA,tB:TKraftMatrix4x4;
    SolverContactManifold:TKraftSolverContactManifold;
    //Normal,t0,t1:TKraftVector3;
    SpeculativeContactState:PKraftSolverSpeculativeContactState;
begin

 TangentVectors[0]:=Vector3Origin;
 TangentVectors[1]:=Vector3Origin;

 for ContactPairIndex:=0 to fCountContacts-1 do begin

  VelocityState:=@fVelocityStates[ContactPairIndex];

  PositionState:=@fPositionStates[ContactPairIndex];

  IndexA:=VelocityState^.Indices[0];
  IndexB:=VelocityState^.Indices[1];

  iA:=@VelocityState^.WorldInverseInertiaTensors[0];
  iB:=@VelocityState^.WorldInverseInertiaTensors[1];

  mA:=VelocityState^.InverseMasses[0];
  mB:=VelocityState^.InverseMasses[1];

  LocalCenterA:=PositionState^.LocalCenters[0];
  LocalCenterB:=PositionState^.LocalCenters[1];

  cA:=fPositions[IndexA].Position;
  qA:=fPositions[IndexA].Orientation;
  tA:=QuaternionToMatrix4x4(qA);
  Temp:=Vector3Sub(cA,Vector3TermMatrixMulBasis(LocalCenterA,tA));
  PKraftVector3(pointer(@tA[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

  cB:=fPositions[IndexB].Position;
  qB:=fPositions[IndexB].Orientation;
  tB:=QuaternionToMatrix4x4(qB);
  Temp:=Vector3Sub(cB,Vector3TermMatrixMulBasis(LocalCenterB,tB));
  PKraftVector3(pointer(@tB[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

  vA:=fVelocities[IndexA].LinearVelocity;
  wA:=fVelocities[IndexA].AngularVelocity;

  vB:=fVelocities[IndexB].LinearVelocity;
  wB:=fVelocities[IndexB].AngularVelocity;

  if fPositionCorrectionMode=kpcmBaumgarte then begin
   fIsland.fContactPairs[ContactPairIndex]^.GetSolverContactManifold(SolverContactManifold,tA,tB,kcpcmmBaumgarte);
  end else begin
   fIsland.fContactPairs[ContactPairIndex]^.GetSolverContactManifold(SolverContactManifold,tA,tB,kcpcmmVelocitySolver);
  end;

  VelocityState^.CountPoints:=SolverContactManifold.CountContacts;

  VelocityState^.Normal:=SolverContactManifold.Normal;

  LostSpeculativeBounce:=VelocityState^.LostSpeculativeBounce/Max(1,SolverContactManifold.CountContacts);

  ComputeBasis(VelocityState^.Normal,TangentVectors[0],TangentVectors[1]);

  for ContactIndex:=0 to SolverContactManifold.CountContacts-1 do begin

   ContactPoint:=@VelocityState^.Points[ContactIndex];

   SolverContact:=@SolverContactManifold.Contacts[ContactIndex];

   P:=SolverContact^.Point;
   ContactPoint^.RelativePositions[0]:=Vector3Sub(P,cA);
   ContactPoint^.RelativePositions[1]:=Vector3Sub(P,cB);

   // Precalculate JM^-1JT for contact and friction constraints
   rnA:=Vector3Cross(ContactPoint^.RelativePositions[0],VelocityState^.Normal);
   rnB:=Vector3Cross(ContactPoint^.RelativePositions[1],VelocityState^.Normal);

   NormalMass:=mA+mB+Vector3Dot(rnA,Vector3TermMatrixMul(rnA,iA^))+Vector3Dot(rnB,Vector3TermMatrixMul(rnB,iB^));

   if NormalMass>0.0 then begin
    ContactPoint^.NormalMass:=1.0/NormalMass;
   end else begin
    ContactPoint^.NormalMass:=0.0;
   end;

   for TangentIndex:=0 to 1 do begin
    rtA:=Vector3Cross(TangentVectors[TangentIndex],ContactPoint^.RelativePositions[0]);
    rtB:=Vector3Cross(TangentVectors[TangentIndex],ContactPoint^.RelativePositions[1]);
    TangentMass:=mA+mB+Vector3Dot(rtA,Vector3TermMatrixMul(rtA,iA^))+Vector3Dot(rtB,Vector3TermMatrixMul(rtB,iB^));
    if TangentMass>0.0 then begin
     ContactPoint^.TangentMass[TangentIndex]:=1.0/TangentMass;
    end else begin
     ContactPoint^.TangentMass[TangentIndex]:=0.0;
    end;    
   end;

   if fPositionCorrectionMode=kpcmBaumgarte then begin
    ContactPoint^.BaumgarteBias:=(-fPhysics.fContactBaumgarte)*(1.0/fDeltaTime)*min(0.0,SolverContact^.Separation+fPhysics.fPenetrationSlop);
   end else begin
    ContactPoint^.BaumgarteBias:=0.0;
   end;

   ContactPoint^.BaumgarteBias:=ContactPoint^.BaumgarteBias+LostSpeculativeBounce;

   dv:=Vector3Dot(Vector3Sub(Vector3Add(vB,
                                        Vector3Cross(wB,
                                                     ContactPoint^.RelativePositions[1])),
                             Vector3Add(vA,
                                        Vector3Cross(wA,
                                                     ContactPoint^.RelativePositions[0]))),
                  VelocityState^.Normal);
   if dv<(-fPhysics.fVelocityThreshold) then begin
    ContactPoint^.Bias:=ContactPoint^.BaumgarteBias-(VelocityState^.Restitution*dv);
   end else begin
    ContactPoint^.Bias:=ContactPoint^.BaumgarteBias;
   end;

  end;

 end;

 for ContactPairIndex:=0 to fCountSpeculativeContacts-1 do begin

  SpeculativeContactState:=@fSpeculativeContactStates[ContactPairIndex];

  IndexA:=SpeculativeContactState^.Indices[0];
  IndexB:=SpeculativeContactState^.Indices[1];

  iA:=@SpeculativeContactState^.WorldInverseInertiaTensors[0];
  iB:=@SpeculativeContactState^.WorldInverseInertiaTensors[1];            

  mA:=SpeculativeContactState^.InverseMasses[0];
  mB:=SpeculativeContactState^.InverseMasses[1];

  LocalCenterA:=SpeculativeContactState^.LocalCenters[0];
  LocalCenterB:=SpeculativeContactState^.LocalCenters[1];

  cA:=fPositions[IndexA].Position;
  qA:=fPositions[IndexA].Orientation;
  tA:=QuaternionToMatrix4x4(qA);
  Temp:=Vector3Sub(cA,Vector3TermMatrixMulBasis(LocalCenterA,tA));
  PKraftVector3(pointer(@tA[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

  cB:=fPositions[IndexB].Position;
  qB:=fPositions[IndexB].Orientation;
  tB:=QuaternionToMatrix4x4(qB);
  Temp:=Vector3Sub(cB,Vector3TermMatrixMulBasis(LocalCenterB,tB));
  PKraftVector3(pointer(@tB[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

  vA:=fVelocities[IndexA].LinearVelocity;
  wA:=fVelocities[IndexA].AngularVelocity;

  vB:=fVelocities[IndexB].LinearVelocity;
  wB:=fVelocities[IndexB].AngularVelocity;

  fIsland.fSpeculativeContactPairs[ContactPairIndex]^.GetSolverContactManifold(SolverContactManifold,tA,tB,kcpcmmVelocitySolver);

  SpeculativeContactState^.Normal:=SolverContactManifold.Normal;

  SolverContact:=@SolverContactManifold.Contacts[0];

  SpeculativeContactState^.RelativePositions[0]:=Vector3Sub(SolverContactManifold.Points[0],cA);
  SpeculativeContactState^.RelativePositions[1]:=Vector3Sub(SolverContactManifold.Points[1],cB);

  SpeculativeContactState^.Separation:=SolverContact^.Separation;

  // Precalculate JM^-1JT for contact and friction constraints
  rnA:=Vector3Cross(SpeculativeContactState^.RelativePositions[0],SpeculativeContactState^.Normal);
  rnB:=Vector3Cross(SpeculativeContactState^.RelativePositions[1],SpeculativeContactState^.Normal);

  NormalMass:=mA+mB+Vector3Dot(rnA,Vector3TermMatrixMul(rnA,iA^))+Vector3Dot(rnB,Vector3TermMatrixMul(rnB,iB^));

  if NormalMass>0.0 then begin
   SpeculativeContactState^.NormalMass:=1.0/NormalMass;
  end else begin
   SpeculativeContactState^.NormalMass:=0.0;
  end;

  for TangentIndex:=0 to 1 do begin
   rtA:=Vector3Cross(TangentVectors[TangentIndex],SpeculativeContactState^.RelativePositions[0]);
   rtB:=Vector3Cross(TangentVectors[TangentIndex],SpeculativeContactState^.RelativePositions[1]);
   TangentMass:=mA+mB+Vector3Dot(rtA,Vector3TermMatrixMul(rtA,iA^))+Vector3Dot(rtB,Vector3TermMatrixMul(rtB,iB^));
   if TangentMass>0.0 then begin
    SpeculativeContactState^.TangentMass[TangentIndex]:=1.0/TangentMass;
   end else begin
    SpeculativeContactState^.TangentMass[TangentIndex]:=0.0;
   end;
  end;

  // Precalculate speculative velocity
  SpeculativeContactState^.SpeculativeVelocity:=-(SpeculativeContactState^.Separation/fDeltaTime);

  // Add in restitution bias to lost speculative bounce
  RestitutionBias:=SpeculativeContactState^.LostSpeculativeBounce;
  dv:=Vector3Dot(Vector3Sub(Vector3Add(vB,
                                       Vector3Cross(wB,
                                                    SpeculativeContactState^.RelativePositions[1])),
                            Vector3Add(vA,
                                       Vector3Cross(wA,
                                                    SpeculativeContactState^.RelativePositions[0]))),
                 SpeculativeContactState^.Normal);
  SpeculativeContactState^.RestitutionBias:=RestitutionBias;
  if dv<(-fPhysics.fVelocityThreshold) then begin
   RestitutionBias:=Max(RestitutionBias,-(SpeculativeContactState^.Restitution*dv));
  end;
  SpeculativeContactState^.LostSpeculativeBounce:=RestitutionBias;

 end;


end;

procedure TKraftSolver.WarmStart;
var ContactPairIndex,ContactIndex,IndexA,IndexB,CountPoints:longint;
    VelocityState:PKraftSolverVelocityState;
    ContactPoint:PKraftSolverVelocityStateContactPoint;
    iA,iB:PKraftMatrix3x3;
    mA,mB,dv,RestitutionBias:TKraftScalar;
    Normal,vA,lA,wA,vB,lB,wB,P:TKraftVector3;
    TangentVectors:array[0..1] of TKraftVector3;
    SpeculativeContactState:PKraftSolverSpeculativeContactState;
begin

 for ContactPairIndex:=0 to fCountContacts-1 do begin

  VelocityState:=@fVelocityStates[ContactPairIndex];
  IndexA:=VelocityState^.Indices[0];
  IndexB:=VelocityState^.Indices[1];
  iA:=@VelocityState^.WorldInverseInertiaTensors[0];
  iB:=@VelocityState^.WorldInverseInertiaTensors[1];
  mA:=VelocityState^.InverseMasses[0];
  mB:=VelocityState^.InverseMasses[1];
  CountPoints:=VelocityState^.CountPoints;
  Normal:=VelocityState^.Normal;

  ComputeBasis(Normal,TangentVectors[0],TangentVectors[1]);

  vA:=fVelocities[IndexA].LinearVelocity;
  wA:=fVelocities[IndexA].AngularVelocity;
  vB:=fVelocities[IndexB].LinearVelocity;
  wB:=fVelocities[IndexB].AngularVelocity;

  lA:=fLinearFactors[IndexA];
  lB:=fLinearFactors[IndexB];

  for ContactIndex:=0 to CountPoints-1 do begin

   ContactPoint:=@VelocityState^.Points[ContactIndex];

   P:=Vector3Add(Vector3ScalarMul(Normal,ContactPoint^.NormalImpulse),
                 Vector3Add(Vector3ScalarMul(TangentVectors[0],ContactPoint^.TangentImpulse[0]),
                            Vector3ScalarMul(TangentVectors[1],ContactPoint^.TangentImpulse[1])));

   Vector3DirectSub(vA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
   Vector3DirectSub(wA,Vector3TermMatrixMul(Vector3Cross(ContactPoint^.RelativePositions[0],P),iA^));

   Vector3DirectAdd(vB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
   Vector3DirectAdd(wB,Vector3TermMatrixMul(Vector3Cross(ContactPoint^.RelativePositions[1],P),iB^));

   if fPositionCorrectionMode=kpcmBaumgarte then begin
    dv:=Vector3Dot(Vector3Sub(Vector3Add(vB,
                                         Vector3Cross(wB,
                                                      ContactPoint^.RelativePositions[1])),
                              Vector3Add(vA,
                                         Vector3Cross(wA,
                                                      ContactPoint^.RelativePositions[0]))),
                   VelocityState^.Normal);
    if dv<(-fPhysics.fVelocityThreshold) then begin
     ContactPoint^.Bias:=ContactPoint^.BaumgarteBias-(VelocityState^.Restitution*dv);
    end;
   end;

  end;

  fVelocities[IndexA].LinearVelocity:=vA;
  fVelocities[IndexA].AngularVelocity:=wA;
  fVelocities[IndexB].LinearVelocity:=vB;
  fVelocities[IndexB].AngularVelocity:=wB;

 end;

 for ContactPairIndex:=0 to fCountSpeculativeContacts-1 do begin

  SpeculativeContactState:=@fSpeculativeContactStates[ContactPairIndex];

  IndexA:=SpeculativeContactState^.Indices[0];
  IndexB:=SpeculativeContactState^.Indices[1];
  iA:=@SpeculativeContactState^.WorldInverseInertiaTensors[0];
  iB:=@SpeculativeContactState^.WorldInverseInertiaTensors[1];
  mA:=SpeculativeContactState^.InverseMasses[0];
  mB:=SpeculativeContactState^.InverseMasses[1];
  Normal:=SpeculativeContactState^.Normal;

  ComputeBasis(Normal,TangentVectors[0],TangentVectors[1]);

  vA:=fVelocities[IndexA].LinearVelocity;
  wA:=fVelocities[IndexA].AngularVelocity;
  vB:=fVelocities[IndexB].LinearVelocity;
  wB:=fVelocities[IndexB].AngularVelocity;

  lA:=fLinearFactors[IndexA];
  lB:=fLinearFactors[IndexB];

  P:=Vector3Add(Vector3ScalarMul(Normal,SpeculativeContactState^.NormalImpulse),
                Vector3Add(Vector3ScalarMul(TangentVectors[0],SpeculativeContactState^.TangentImpulse[0]),
                           Vector3ScalarMul(TangentVectors[1],SpeculativeContactState^.TangentImpulse[1])));

  Vector3DirectSub(vA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
  Vector3DirectSub(wA,Vector3TermMatrixMul(Vector3Cross(SpeculativeContactState^.RelativePositions[0],P),iA^));

  Vector3DirectAdd(vB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
  Vector3DirectAdd(wB,Vector3TermMatrixMul(Vector3Cross(SpeculativeContactState^.RelativePositions[1],P),iB^));

  // Add in restitution bias to lost speculative bounce
  RestitutionBias:=SpeculativeContactState^.RestitutionBias;
  dv:=Vector3Dot(Vector3Sub(Vector3Add(vB,
                                       Vector3Cross(wB,
                                                    SpeculativeContactState^.RelativePositions[1])),
                            Vector3Add(vA,
                                       Vector3Cross(wA,
                                                    SpeculativeContactState^.RelativePositions[0]))),
                 SpeculativeContactState^.Normal);
  if dv<(-fPhysics.fVelocityThreshold) then begin
   RestitutionBias:=Max(RestitutionBias,-(SpeculativeContactState^.Restitution*dv));
  end;
  SpeculativeContactState^.LostSpeculativeBounce:=RestitutionBias;

  fVelocities[IndexA].LinearVelocity:=vA;
  fVelocities[IndexA].AngularVelocity:=wA;
  fVelocities[IndexB].LinearVelocity:=vB;
  fVelocities[IndexB].AngularVelocity:=wB;

 end;

end;

procedure TKraftSolver.SolveVelocityConstraints;
var ContactPairIndex,ContactIndex,TangentIndex,IndexA,IndexB,CountPoints:longint;
    VelocityState:PKraftSolverVelocityState;
    ContactPoint:PKraftSolverVelocityStateContactPoint;
    iA,iB:PKraftMatrix3x3;
    mA,mB,Friction,Lambda,MaxLambda,vn,Old:TKraftScalar;
    Normal,vA,lA,wA,rA,vB,lB,wB,rB,dv,P:TKraftVector3;
    TangentVectors:array[0..1] of TKraftVector3;
begin

 for ContactPairIndex:=0 to fCountContacts-1 do begin

  VelocityState:=@fVelocityStates[ContactPairIndex];
  IndexA:=VelocityState^.Indices[0];
  IndexB:=VelocityState^.Indices[1];
  iA:=@VelocityState^.WorldInverseInertiaTensors[0];
  iB:=@VelocityState^.WorldInverseInertiaTensors[1];
  mA:=VelocityState^.InverseMasses[0];
  mB:=VelocityState^.InverseMasses[1];
  CountPoints:=VelocityState^.CountPoints;
  Normal:=VelocityState^.Normal;
  Friction:=VelocityState^.Friction;
  ComputeBasis(Normal,TangentVectors[0],TangentVectors[1]);

  vA:=fVelocities[IndexA].LinearVelocity;
  wA:=fVelocities[IndexA].AngularVelocity;
  vB:=fVelocities[IndexB].LinearVelocity;
  wB:=fVelocities[IndexB].AngularVelocity;

  lA:=fLinearFactors[IndexA];
  lB:=fLinearFactors[IndexB];

  for ContactIndex:=0 to CountPoints-1 do begin

   ContactPoint:=@VelocityState^.Points[ContactIndex];

   rA:=ContactPoint^.RelativePositions[0];
   rB:=ContactPoint^.RelativePositions[1];

   if fEnableFriction then begin

    MaxLambda:=Friction*ContactPoint^.NormalImpulse;

    for TangentIndex:=0 to 1 do begin

     dv:=Vector3Sub(Vector3Add(vB,Vector3Cross(wB,rB)),Vector3Add(vA,Vector3Cross(wA,rA)));

     Lambda:=(-Vector3Dot(dv,TangentVectors[TangentIndex]))*ContactPoint^.TangentMass[TangentIndex];

     Old:=ContactPoint^.TangentImpulse[TangentIndex];
     ContactPoint^.TangentImpulse[TangentIndex]:=Min(Max(Old+Lambda,-MaxLambda),MaxLambda);
     Lambda:=ContactPoint^.TangentImpulse[TangentIndex]-Old;

     P:=Vector3ScalarMul(TangentVectors[TangentIndex],Lambda);

     Vector3DirectSub(vA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
     Vector3DirectSub(wA,Vector3TermMatrixMul(Vector3Cross(rA,P),iA^));

     Vector3DirectAdd(vB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
     Vector3DirectAdd(wB,Vector3TermMatrixMul(Vector3Cross(rB,P),iB^));

    end;

   end;

   dv:=Vector3Sub(Vector3Add(vB,Vector3Cross(wB,rB)),Vector3Add(vA,Vector3Cross(wA,rA)));

   vn:=Vector3Dot(dv,Normal);

   Lambda:=ContactPoint^.NormalMass*(ContactPoint^.Bias-vn);

   Old:=ContactPoint^.NormalImpulse;
   ContactPoint^.NormalImpulse:=Max(0.0,Old+Lambda);
   Lambda:=ContactPoint^.NormalImpulse-Old;

   P:=Vector3ScalarMul(Normal,Lambda);

   Vector3DirectSub(vA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
   Vector3DirectSub(wA,Vector3TermMatrixMul(Vector3Cross(rA,P),iA^));

   Vector3DirectAdd(vB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
   Vector3DirectAdd(wB,Vector3TermMatrixMul(Vector3Cross(rB,P),iB^));

  end;

  fVelocities[IndexA].LinearVelocity:=vA;
  fVelocities[IndexA].AngularVelocity:=wA;
  fVelocities[IndexB].LinearVelocity:=vB;
  fVelocities[IndexB].AngularVelocity:=wB;

 end;

end;

function TKraftSolver.SolvePositionConstraints:boolean;
var ContactPairIndex,ContactIndex,IndexA,IndexB:longint;
    PositionState:PKraftSolverPositionState;
    SolverContact:PKraftSolverContact;
    iA,iB:PKraftMatrix3x3;
    MinSeparation,mA,mB,Separation,C,K,Impulse:TKraftScalar;
    LocalCenterA,LocalCenterB,cA,cB,Normal,Point,rA,rB,rnA,rnB,P,Temp,lA,lB:TKraftVector3;
    qA,qB:TKraftQuaternion;
    tA,tB:TKraftMatrix4x4;
    SolverContactManifold:TKraftSolverContactManifold;
begin

 if fPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  MinSeparation:=0.0;
  for ContactPairIndex:=0 to fCountContacts-1 do begin

   PositionState:=@fPositionStates[ContactPairIndex];

   IndexA:=PositionState^.Indices[0];
   IndexB:=PositionState^.Indices[1];

   iA:=@PositionState^.WorldInverseInertiaTensors[0];
   iB:=@PositionState^.WorldInverseInertiaTensors[1];

   mA:=PositionState^.InverseMasses[0];
   mB:=PositionState^.InverseMasses[1];

   LocalCenterA:=PositionState^.LocalCenters[0];
   LocalCenterB:=PositionState^.LocalCenters[1];

   lA:=fLinearFactors[IndexA];
   lB:=fLinearFactors[IndexB];

   cA:=fPositions[IndexA].Position;
   qA:=fPositions[IndexA].Orientation;
   tA:=QuaternionToMatrix4x4(qA);
   Temp:=Vector3Sub(cA,Vector3TermMatrixMulBasis(LocalCenterA,tA));
   PKraftVector3(pointer(@tA[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

   cB:=fPositions[IndexB].Position;
   qB:=fPositions[IndexB].Orientation;
   tB:=QuaternionToMatrix4x4(qB);
   Temp:=Vector3Sub(cB,Vector3TermMatrixMulBasis(LocalCenterB,tB));
   PKraftVector3(pointer(@tB[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

   fIsland.fContactPairs[ContactPairIndex]^.GetSolverContactManifold(SolverContactManifold,tA,tB,kcpcmmPositionSolver);

   Normal:=SolverContactManifold.Normal;

   for ContactIndex:=0 to SolverContactManifold.CountContacts-1 do begin

    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];

    Point:=SolverContact^.Point;
    Separation:=SolverContact^.Separation;

    rA:=Vector3Sub(Point,cA);
    rB:=Vector3Sub(Point,cB);

    if MinSeparation>Separation then begin
     MinSeparation:=Separation;
    end;

    C:=Min(Max(fPhysics.fContactBaumgarte*(Separation+fPhysics.fLinearSlop),-fPhysics.fMaximalLinearCorrection),0.0);

    rnA:=Vector3Cross(rA,Normal);
    rnB:=Vector3Cross(rB,Normal);

    K:=mA+mB+Vector3Dot(rnA,Vector3TermMatrixMul(rnA,iA^))+Vector3Dot(rnB,Vector3TermMatrixMul(rnB,iB^));

    if K>0.0 then begin
     Impulse:=-(C/K);   
    end else begin
     Impulse:=0.0;
    end;

    P:=Vector3ScalarMul(Normal,Impulse);

    Vector3DirectSub(cA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
    QuaternionDirectSpin(qA,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(P)),iA^),1.0);

    Vector3DirectAdd(cB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
    QuaternionDirectSpin(qB,Vector3TermMatrixMul(Vector3Cross(rB,P),iB^),1.0);

   end;

   fPositions[IndexA].Position:=cA;
   fPositions[IndexA].Orientation:=qA;

   fPositions[IndexB].Position:=cB;
   fPositions[IndexB].Orientation:=qB;

  end;

  result:=MinSeparation>=((-3.0)*fPhysics.fLinearSlop);

 end else begin

  result:=true;

 end;

end;

function TKraftSolver.SolveTimeOfImpactConstraints(IndexA,IndexB:longint):boolean;
var ContactPairIndex,ContactIndex,CurrentIndexA,CurrentIndexB:longint;
    PositionState:PKraftSolverPositionState;
    SolverContact:PKraftSolverContact;
    iA,iB:PKraftMatrix3x3;
    MinSeparation,mA,mB,Separation,C,K,Impulse:TKraftScalar;
    LocalCenterA,LocalCenterB,cA,cB,Normal,Point,rA,rB,rnA,rnB,P,Temp,lA,lB:TKraftVector3;
    qA,qB:TKraftQuaternion;
    tA,tB:TKraftMatrix4x4;
    SolverContactManifold:TKraftSolverContactManifold;
begin

 if fPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin

  MinSeparation:=0.0;

  for ContactPairIndex:=0 to fCountContacts-1 do begin

   PositionState:=@fPositionStates[ContactPairIndex];

   CurrentIndexA:=PositionState^.Indices[0];
   CurrentIndexB:=PositionState^.Indices[1];

   LocalCenterA:=PositionState^.LocalCenters[0];
   LocalCenterB:=PositionState^.LocalCenters[1];

   if (CurrentIndexA=IndexA) or (CurrentIndexA=IndexB) then begin
    iA:=@PositionState^.WorldInverseInertiaTensors[0];
    mA:=PositionState^.InverseMasses[0];
   end else begin
    mA:=0.0;
    iA:=@Matrix3x3Null;
   end;

   if (CurrentIndexB=IndexA) or (CurrentIndexB=IndexB) then begin
    iB:=@PositionState^.WorldInverseInertiaTensors[1];
    mB:=PositionState^.InverseMasses[1];
   end else begin
    mB:=0.0;
    iB:=@Matrix3x3Null;
   end;

   lA:=fLinearFactors[IndexA];
   lB:=fLinearFactors[IndexB];

   cA:=fPositions[IndexA].Position;
   qA:=fPositions[IndexA].Orientation;
   tA:=QuaternionToMatrix4x4(qA);
   Temp:=Vector3Sub(cA,Vector3TermMatrixMulBasis(LocalCenterA,tA));
   PKraftVector3(pointer(@tA[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

   cB:=fPositions[IndexB].Position;
   qB:=fPositions[IndexB].Orientation;
   tB:=QuaternionToMatrix4x4(qB);
   Temp:=Vector3Sub(cB,Vector3TermMatrixMulBasis(LocalCenterB,tB));
   PKraftVector3(pointer(@tB[3,0]))^.xyz:=PKraftVector3(pointer(@Temp))^.xyz;

   fIsland.fContactPairs[ContactPairIndex]^.GetSolverContactManifold(SolverContactManifold,tA,tB,kcpcmmPositionSolver);

   Normal:=SolverContactManifold.Normal;

   for ContactIndex:=0 to SolverContactManifold.CountContacts-1 do begin

    SolverContact:=@SolverContactManifold.Contacts[ContactIndex];

    Point:=SolverContact^.Point;
    Separation:=SolverContact^.Separation;

    rA:=Vector3Sub(Point,cA);
    rB:=Vector3Sub(Point,cB);

    if MinSeparation>Separation then begin
     MinSeparation:=Separation;
    end;

    C:=Min(Max(fPhysics.fTimeOfImpactBaumgarte*(Separation+fPhysics.fLinearSlop),-fPhysics.fMaximalLinearCorrection),0.0);

    rnA:=Vector3Cross(rA,Normal);
    rnB:=Vector3Cross(rB,Normal);

    K:=mA+mB+Vector3Dot(rnA,Vector3TermMatrixMul(rnA,iA^))+Vector3Dot(rnB,Vector3TermMatrixMul(rnB,iB^));

    if K>0.0 then begin
     Impulse:=-(C/K);
    end else begin
     Impulse:=0.0;
    end;

    P:=Vector3ScalarMul(Normal,Impulse);

    Vector3DirectSub(cA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
    QuaternionDirectSpin(qA,Vector3TermMatrixMul(Vector3Cross(rA,Vector3Neg(P)),iA^),1.0);

    Vector3DirectAdd(cB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
    QuaternionDirectSpin(qB,Vector3TermMatrixMul(Vector3Cross(rB,P),iB^),1.0);

   end;

   fPositions[IndexA].Position:=cA;
   fPositions[IndexA].Orientation:=qA;

   fPositions[IndexB].Position:=cB;
   fPositions[IndexB].Orientation:=qB;

  end;

  result:=MinSeparation>=((-1.5)*fPhysics.fLinearSlop);

 end else begin

  result:=true;
  
 end;

end;

procedure TKraftSolver.SolveSpeculativeContactConstraints;
var ContactPairIndex,TangentIndex,IndexA,IndexB:longint;
    SpeculativeContactState:PKraftSolverSpeculativeContactState;
    iA,iB:PKraftMatrix3x3;
    mA,mB,Friction,Lambda,MaxLambda,vn,Old:TKraftScalar;
    Normal,vA,lA,wA,rA,vB,lB,wB,rB,dv,P:TKraftVector3;
    TangentVectors:array[0..1] of TKraftVector3;
begin

 for ContactPairIndex:=0 to fCountSpeculativeContacts-1 do begin

  SpeculativeContactState:=@fSpeculativeContactStates[ContactPairIndex];
  IndexA:=SpeculativeContactState^.Indices[0];
  IndexB:=SpeculativeContactState^.Indices[1];
  iA:=@SpeculativeContactState^.WorldInverseInertiaTensors[0];
  iB:=@SpeculativeContactState^.WorldInverseInertiaTensors[1];
  mA:=SpeculativeContactState^.InverseMasses[0];
  mB:=SpeculativeContactState^.InverseMasses[1];
  Normal:=SpeculativeContactState^.Normal;
  Friction:=SpeculativeContactState^.Friction;
  ComputeBasis(Normal,TangentVectors[0],TangentVectors[1]);

  vA:=fVelocities[IndexA].LinearVelocity;
  wA:=fVelocities[IndexA].AngularVelocity;
  vB:=fVelocities[IndexB].LinearVelocity;
  wB:=fVelocities[IndexB].AngularVelocity;

  lA:=fLinearFactors[IndexA];
  lB:=fLinearFactors[IndexB];

  rA:=SpeculativeContactState^.RelativePositions[0];
  rB:=SpeculativeContactState^.RelativePositions[1];

  if fEnableFriction then begin

   MaxLambda:=Friction*SpeculativeContactState^.NormalImpulse;

   for TangentIndex:=0 to 1 do begin

    dv:=Vector3Sub(Vector3Add(vB,Vector3Cross(wB,rB)),Vector3Add(vA,Vector3Cross(wA,rA)));

    Lambda:=(-Vector3Dot(dv,TangentVectors[TangentIndex]))*SpeculativeContactState^.TangentMass[TangentIndex];

    Old:=SpeculativeContactState^.TangentImpulse[TangentIndex];
    SpeculativeContactState^.TangentImpulse[TangentIndex]:=Min(Max(Old+Lambda,-MaxLambda),MaxLambda);
    Lambda:=SpeculativeContactState^.TangentImpulse[TangentIndex]-Old;

    P:=Vector3ScalarMul(TangentVectors[TangentIndex],Lambda);

    Vector3DirectSub(vA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
    Vector3DirectSub(wA,Vector3TermMatrixMul(Vector3Cross(rA,P),iA^));

    Vector3DirectAdd(vB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
    Vector3DirectAdd(wB,Vector3TermMatrixMul(Vector3Cross(rB,P),iB^));

   end;

  end;

  dv:=Vector3Sub(Vector3Add(vB,Vector3Cross(wB,rB)),Vector3Add(vA,Vector3Cross(wA,rA)));

  vn:=Vector3Dot(dv,Normal);

  Lambda:=SpeculativeContactState^.NormalMass*(SpeculativeContactState^.SpeculativeVelocity-vn);

  Old:=SpeculativeContactState^.NormalImpulse;
  SpeculativeContactState^.NormalImpulse:=Max(0.0,Old+Lambda);
  Lambda:=SpeculativeContactState^.NormalImpulse-Old;

  P:=Vector3ScalarMul(Normal,Lambda);

  Vector3DirectSub(vA,Vector3Mul(P,Vector3ScalarMul(lA,mA)));
  Vector3DirectSub(wA,Vector3TermMatrixMul(Vector3Cross(rA,P),iA^));

  Vector3DirectAdd(vB,Vector3Mul(P,Vector3ScalarMul(lB,mB)));
  Vector3DirectAdd(wB,Vector3TermMatrixMul(Vector3Cross(rB,P),iB^));

  fVelocities[IndexA].LinearVelocity:=vA;
  fVelocities[IndexA].AngularVelocity:=wA;
  fVelocities[IndexB].LinearVelocity:=vB;
  fVelocities[IndexB].AngularVelocity:=wB;

 end;

end;

procedure TKraftSolver.StoreImpulses;
var i,j:longint;
    VelocityState:PKraftSolverVelocityState;
    ContactPair:PKraftContactPair;
    Contact:PKraftContact;
    ContactPoint:PKraftSolverVelocityStateContactPoint;
    SpeculativeContactState:PKraftSolverSpeculativeContactState;
begin
 for i:=0 to fCountContacts-1 do begin
  VelocityState:=@fVelocityStates[i];
  ContactPair:=fIsland.fContactPairs[i];
  ContactPair^.Manifold.LostSpeculativeBounce:=0.0;
  for j:=0 to VelocityState^.CountPoints-1 do begin
   Contact:=@ContactPair^.Manifold.Contacts[j];
   ContactPoint:=@VelocityState^.Points[j];
   Contact^.NormalImpulse:=ContactPoint^.NormalImpulse;
   Contact^.TangentImpulse[0]:=ContactPoint^.TangentImpulse[0];
   Contact^.TangentImpulse[1]:=ContactPoint^.TangentImpulse[1];
  end;
 end;
 for i:=0 to fCountSpeculativeContacts-1 do begin
  SpeculativeContactState:=@fSpeculativeContactStates[i];
  ContactPair:=fIsland.fSpeculativeContactPairs[i];
  Contact:=@ContactPair^.Manifold.Contacts[0];
  ContactPair^.Manifold.LostSpeculativeBounce:=SpeculativeContactState^.LostSpeculativeBounce;
  Contact^.NormalImpulse:=SpeculativeContactState^.NormalImpulse;
  Contact^.TangentImpulse[0]:=SpeculativeContactState^.TangentImpulse[0];
  Contact^.TangentImpulse[1]:=SpeculativeContactState^.TangentImpulse[1];
 end;
end;

constructor TKraftIsland.Create(const APhysics:TKraft;const AIndex:longint);
begin
 inherited Create;

 fPhysics:=APhysics;

 fIslandIndex:=AIndex;

 fRigidBodies:=nil;
 SetLength(fRigidBodies,64);
 fCountRigidBodies:=0;

 fConstraints:=nil;
 SetLength(fConstraints,64);
 fCountConstraints:=0;

 fContactPairs:=nil;
 SetLength(fContactPairs,64);
 fCountContactPairs:=0;

 fStaticContactPairs:=nil;
 SetLength(fStaticContactPairs,64);
 fCountStaticContactPairs:=0;

 fSpeculativeContactPairs:=nil;
 SetLength(fSpeculativeContactPairs,4096);
 fCountSpeculativeContactPairs:=0;

 fSolver:=TKraftSolver.Create(fPhysics,self);

end;

destructor TKraftIsland.Destroy;
begin
 SetLength(fRigidBodies,0);
 SetLength(fConstraints,0);
 SetLength(fContactPairs,0);
 SetLength(fStaticContactPairs,0);
 SetLength(fSpeculativeContactPairs,0);
 fSolver.Free;
 inherited Destroy;
end;

procedure TKraftIsland.Clear;
begin
 fCountRigidBodies:=0;
 fCountConstraints:=0;
 fCountContactPairs:=0;
 fCountStaticContactPairs:=0;
 fCountSpeculativeContactPairs:=0;
end;

function TKraftIsland.AddRigidBody(RigidBody:TKraftRigidBody):longint;
begin
 RigidBody.fIsland:=self;
 if (fCountRigidBodies+1)>length(fRigidBodies) then begin
  SetLength(fRigidBodies,(fCountRigidBodies+1)*2);
 end;
 fRigidBodies[fCountRigidBodies]:=RigidBody;
 if fIslandIndex>=length(RigidBody.fIslandIndices) then begin
  SetLength(RigidBody.fIslandIndices,(fIslandIndex+1)*2);
 end;
 RigidBody.fIslandIndices[fIslandIndex]:=fCountRigidBodies;
 result:=fCountRigidBodies;
 inc(fCountRigidBodies);
end;

procedure TKraftIsland.AddConstraint(Constraint:TKraftConstraint);
begin
 if (fCountConstraints+1)>length(fConstraints) then begin
  SetLength(fConstraints,(fCountConstraints+1)*2);
 end;
 fConstraints[fCountConstraints]:=Constraint;
 inc(fCountConstraints);
end;

procedure TKraftIsland.AddContactPair(ContactPair:PKraftContactPair);
begin
 ContactPair^.Island:=self;
 if ContactPair^.Manifold.ContactManifoldType=kcmtSpeculative then begin
  if (fCountSpeculativeContactPairs+1)>length(fSpeculativeContactPairs) then begin
   SetLength(fSpeculativeContactPairs,(fCountSpeculativeContactPairs+1)*2);
  end;
  fSpeculativeContactPairs[fCountSpeculativeContactPairs]:=ContactPair;
  inc(fCountSpeculativeContactPairs);
 end else begin
  if (assigned(ContactPair^.RigidBodies[0]) and (ContactPair^.RigidBodies[0].fRigidBodyType=krbtDynamic)) and
     (assigned(ContactPair^.RigidBodies[1]) and (ContactPair^.RigidBodies[1].fRigidBodyType=krbtDynamic)) then begin
   // Dynamic vs dynamic (solving before dynamic vs static in a solver iteration)
   if (fCountContactPairs+1)>length(fContactPairs) then begin
    SetLength(fContactPairs,(fCountContactPairs+1)*2);
   end;
   fContactPairs[fCountContactPairs]:=ContactPair;
   inc(fCountContactPairs);
  end else begin
   // Dynamic vs static (solving after dynamic vs dynamic in a solver iteration)
   if (fCountStaticContactPairs+1)>length(fStaticContactPairs) then begin
    SetLength(fStaticContactPairs,(fCountStaticContactPairs+1)*2);
   end;
   fStaticContactPairs[fCountStaticContactPairs]:=ContactPair;
   inc(fCountStaticContactPairs);
  end;
 end;
end;

procedure TKraftIsland.MergeContactPairs;
var NewCountContactPairs:longint;
begin
 NewCountContactPairs:=fCountContactPairs+fCountStaticContactPairs;
 if length(fContactPairs)<NewCountContactPairs then begin
  SetLength(fContactPairs,NewCountContactPairs*2);
 end;
 if fCountStaticContactPairs>0 then begin
  Move(fStaticContactPairs[0],fContactPairs[fCountContactPairs],fCountStaticContactPairs*SizeOf(PKraftContactPair));
  inc(fCountContactPairs,fCountStaticContactPairs);
 end;
end;

procedure TKraftIsland.Solve(const TimeStep:TKraftTimeStep);
var Iteration,Index:longint;
    RigidBody:TKraftRigidBody;
    Constraint:TKraftConstraint;
    MinSleepTime,s:TKraftScalar;
    First,OK:boolean;
    SolverVelocity:PKraftSolverVelocity;
    SolverPosition:PKraftSolverPosition;
    GyroscopicForce:TKraftVector3;
    Position,LinearVelocity,AngularVelocity,Translation,Rotation:TKraftVector3;
    Orientation:TKraftQuaternion;
begin

 fSolver.Store;

 // Integrate velocities and create state buffers, calculate world inertia
 for Index:=0 to fCountRigidBodies-1 do begin

  RigidBody:=fRigidBodies[Index];

  Position:=RigidBody.fSweep.c;
  Orientation:=RigidBody.fSweep.q;
  RigidBody.fSweep.c0:=RigidBody.fSweep.c;
  RigidBody.fSweep.q0:=RigidBody.fSweep.q;

  if RigidBody.fRigidBodyType=krbtDynamic then begin

   // Apply gravity force
   if krbfHasOwnGravity in RigidBody.fFlags then begin
    RigidBody.fForce:=Vector3Add(RigidBody.fForce,Vector3ScalarMul(RigidBody.fGravity,RigidBody.fMass*RigidBody.fGravityScale));
   end else begin
    RigidBody.fForce:=Vector3Add(RigidBody.fForce,Vector3ScalarMul(fPhysics.fGravity,RigidBody.fMass*RigidBody.fGravityScale));
   end;

   // Calculate world space inertia tensor
   RigidBody.UpdateWorldInertiaTensor;

   // Apply gyroscopic force
   if RigidBody.fEnableGyroscopicForce then begin
    // Gyroscopic force calculation using full newton-euler equations with implicit euler step, so it's stable.
    GyroscopicForce:=Vector3Sub(Vector3Sub(RigidBody.fAngularVelocity,
                                           Vector3TermMatrixMulInverse(EvaluateEulerEquation(RigidBody.fAngularVelocity,
                                                                                             RigidBody.fAngularVelocity,
                                                                                             Vector3Origin,
                                                                                             TimeStep.DeltaTime,
                                                                                             RigidBody.fWorldInertiaTensor),
                                                                       EvaluateEulerEquationDerivation(RigidBody.fAngularVelocity,
                                                                                                       RigidBody.fAngularVelocity,
                                                                                                       TimeStep.DeltaTime,
                                                                                                       RigidBody.fWorldInertiaTensor))),
                                RigidBody.fAngularVelocity);
    if (RigidBody.fMaximalGyroscopicForce>EPSILON) and (Vector3LengthSquared(GyroscopicForce)>sqr(RigidBody.fMaximalGyroscopicForce)) then begin
     Vector3Scale(GyroscopicForce,RigidBody.fMaximalGyroscopicForce/Vector3Length(GyroscopicForce));
    end;
    RigidBody.fTorque:=Vector3Add(RigidBody.fTorque,GyroscopicForce);
   end;       

   // Integrate linear velocity
   RigidBody.fLinearVelocity:=Vector3Add(RigidBody.fLinearVelocity,VEctor3Mul(RigidBody.fForce,Vector3ScalarMul(RigidBody.fLinearFactor,RigidBody.InverseMass*TimeStep.DeltaTime)));

   // Integrate angular velocity
   RigidBody.fAngularVelocity:=Vector3Add(RigidBody.fAngularVelocity,Vector3ScalarMul(Vector3TermMatrixMul(RigidBody.fTorque,RigidBody.fWorldInverseInertiaTensor),TimeStep.DeltaTime));

   if assigned(RigidBody.fOnDamping) then begin
    RigidBody.fOnDamping(RigidBody,TimeStep);
   end;

   // From Box2D
   // Apply damping.
   // ODE: dv/dt + c * v = 0
   // Solution: v(t) = v0 * exp(-c * t)
   // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
   // v2 = exp(-c * dt) * v1
   // Pade approximation:
   // v2 = v1 * 1 / (1 + c * dt)
   Vector3Scale(RigidBody.fLinearVelocity,1.0/(1.0+(RigidBody.fLinearVelocityDamp*TimeStep.DeltaTime)));
   Vector3Scale(RigidBody.fAngularVelocity,1.0/(1.0+(RigidBody.fAngularVelocityDamp*TimeStep.DeltaTime)));

   // From PAPPE 1.0
   if RigidBody.fAdditionalDamping then begin
    if (Vector3LengthSquared(RigidBody.fLinearVelocity)<RigidBody.fLinearVelocityAdditionalDampThresholdSqr) and
       (Vector3LengthSquared(RigidBody.fAngularVelocity)<RigidBody.fAngularVelocityAdditionalDampThresholdSqr) then begin
     Vector3Scale(RigidBody.fLinearVelocity,RigidBody.fAdditionalDamp);
     Vector3Scale(RigidBody.fAngularVelocity,RigidBody.fAdditionalDamp);
    end;
    s:=Vector3Length(RigidBody.fLinearVelocity);
    if s<RigidBody.fLinearVelocityDamp then begin
     if s>RigidBody.fAdditionalDamp then begin
      RigidBody.fLinearVelocity:=Vector3Sub(RigidBody.fLinearVelocity,Vector3ScalarMul(Vector3NormEx(RigidBody.fLinearVelocity),RigidBody.fAdditionalDamp));
     end else begin
      RigidBody.fLinearVelocity:=Vector3Origin;
     end;
    end;
    s:=Vector3Length(RigidBody.fAngularVelocity);
    if s<RigidBody.fAngularVelocityDamp then begin
     if s>RigidBody.fAdditionalDamp then begin
      RigidBody.fAngularVelocity:=Vector3Sub(RigidBody.fAngularVelocity,Vector3ScalarMul(Vector3NormEx(RigidBody.fAngularVelocity),RigidBody.fAdditionalDamp));
     end else begin
      RigidBody.fAngularVelocity:=Vector3Origin;
     end;
    end;
   end;

	end;

  // Transfer velocities
  SolverVelocity:=@fSolver.fVelocities[Index];
  SolverVelocity^.LinearVelocity:=RigidBody.fLinearVelocity;
  SolverVelocity^.AngularVelocity:=RigidBody.fAngularVelocity;

  // Transfer positions
  SolverPosition:=@fSolver.fPositions[Index];
  SolverPosition^.Position:=Position;
  SolverPosition^.Orientation:=Orientation;

  // Transfer linear factors
  fSolver.fLinearFactors[Index]:=RigidBody.fLinearFactor;

 end;

 fSolver.Initialize(TimeStep);

 fSolver.InitializeConstraints;

 if fPhysics.fWarmStarting then begin
  fSolver.WarmStart;
 end;

 for Index:=0 to fCountConstraints-1 do begin
  Constraint:=fConstraints[Index];
  if assigned(Constraint) then begin
   Constraint.InitializeConstraintsAndWarmStart(self,TimeStep);
  end;
 end;
 for Iteration:=1 to Max(fPhysics.fVelocityIterations,fPhysics.fSpeculativeIterations) do begin
  if Iteration<=fPhysics.fVelocityIterations then begin
   for Index:=0 to fCountConstraints-1 do begin
    Constraint:=fConstraints[Index];
    if assigned(Constraint) and not (kcfBreaked in Constraint.fFlags) then begin
     if ((Constraint.fFlags*[kcfActive,kcfBreakable,kcfBreaked])=[kcfActive,kcfBreakable]) and
        ((Vector3Length(Constraint.GetReactionForce(TimeStep.InverseDeltaTime))>Constraint.fBreakThresholdForce) or
         (Vector3Length(Constraint.GetReactionTorque(TimeStep.InverseDeltaTime))>Constraint.fBreakThresholdTorque)) then begin
      Constraint.fFlags:=Constraint.fFlags+[kcfBreaked,kcfFreshBreaked];
      continue;
     end;
     Constraint.SolveVelocityConstraint(self,TimeStep);
    end;
   end;
   fSolver.SolveVelocityConstraints;
  end;
  if (fSolver.fCountSpeculativeContacts>0) and (Iteration<=fPhysics.fSpeculativeIterations) then begin
   fSolver.SolveSpeculativeContactConstraints;
  end;
 end;
 fSolver.StoreImpulses;

 for Index:=0 to fCountRigidBodies-1 do begin

  RigidBody:=fRigidBodies[Index];

  if RigidBody.fRigidBodyType=krbtDynamic then begin

   SolverPosition:=@fSolver.fPositions[Index];
   Position:=SolverPosition^.Position;
   Orientation:=SolverPosition^.Orientation;

   SolverVelocity:=@fSolver.fVelocities[Index];
   LinearVelocity:=SolverVelocity^.LinearVelocity;
   AngularVelocity:=SolverVelocity^.AngularVelocity;

// writeln(Vector3Length(LinearVelocity)*TimeStep.DeltaTime:1:8);

   if fPhysics.fMaximalLinearVelocity>EPSILON then begin
    Translation:=Vector3ScalarMul(LinearVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Translation)>sqr(fPhysics.fMaximalLinearVelocity) then begin
     Vector3Scale(LinearVelocity,fPhysics.fMaximalLinearVelocity/Vector3Length(Translation));
    end;
   end;
   if RigidBody.fMaximalLinearVelocity>EPSILON then begin
    Translation:=Vector3ScalarMul(LinearVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Translation)>sqr(RigidBody.fMaximalLinearVelocity) then begin
     Vector3Scale(LinearVelocity,RigidBody.fMaximalLinearVelocity/Vector3Length(Translation));
    end;
   end;

   if fPhysics.fMaximalAngularVelocity>EPSILON then begin
    Rotation:=Vector3ScalarMul(AngularVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Rotation)>sqr(fPhysics.fMaximalAngularVelocity) then begin
     Vector3Scale(AngularVelocity,fPhysics.fMaximalAngularVelocity/Vector3Length(Rotation));
    end;
   end;
   if RigidBody.fMaximalAngularVelocity>EPSILON then begin
    Rotation:=Vector3ScalarMul(AngularVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Rotation)>sqr(RigidBody.fMaximalAngularVelocity) then begin
     Vector3Scale(AngularVelocity,RigidBody.fMaximalAngularVelocity/Vector3Length(Rotation));
    end;
   end;

   fPhysics.Integrate(Position,Orientation,LinearVelocity,AngularVelocity,TimeStep.DeltaTime);

   SolverPosition^.Position:=Position;
   SolverPosition^.Orientation:=Orientation;
   SolverVelocity^.LinearVelocity:=LinearVelocity;
   SolverVelocity^.AngularVelocity:=AngularVelocity;

  end;

 end;

 for Index:=0 to fCountConstraints-1 do begin
  Constraint:=fConstraints[Index];
  if assigned(Constraint) and
     (((Constraint.fFlags*[kcfActive,kcfBreakable,kcfBreaked])=[kcfActive,kcfBreakable]) and
      ((Vector3Length(Constraint.GetReactionForce(TimeStep.InverseDeltaTime))>Constraint.fBreakThresholdForce) or
       (Vector3Length(Constraint.GetReactionTorque(TimeStep.InverseDeltaTime))>Constraint.fBreakThresholdTorque))) then begin
   Constraint.fFlags:=Constraint.fFlags+[kcfBreaked,kcfFreshBreaked];
  end;
 end;

 if (fPhysics.fContactPositionCorrectionMode=kpcmNonLinearGaussSeidel) or
    (fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel) then begin
  for Iteration:=1 to fPhysics.fPositionIterations do begin
   OK:=true;
   if (fPhysics.fContactPositionCorrectionMode=kpcmNonLinearGaussSeidel) and not fSolver.SolvePositionConstraints then begin
    OK:=false;
   end;
   if fPhysics.fConstraintPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin
    for Index:=0 to fCountConstraints-1 do begin
     Constraint:=fConstraints[Index];
     if assigned(Constraint) and not (kcfBreaked in Constraint.fFlags) then begin
      if not Constraint.SolvePositionConstraint(self,TimeStep) then begin
       OK:=false;
      end;
     end;
    end;
   end;
   if OK then begin
    break;
   end;
  end;
 end;

 for Index:=0 to fCountRigidBodies-1 do begin
  RigidBody:=fRigidBodies[Index];
  if RigidBody.fRigidBodyType=krbtDynamic then begin

   SolverPosition:=@fSolver.fPositions[Index];
   RigidBody.fSweep.c:=SolverPosition^.Position;
   RigidBody.fSweep.q:=SolverPosition^.Orientation;

   SolverVelocity:=@fSolver.fVelocities[Index];
   RigidBody.fLinearVelocity:=SolverVelocity^.LinearVelocity;
   RigidBody.fAngularVelocity:=SolverVelocity^.AngularVelocity;

   RigidBody.SynchronizeTransformIncludingShapes;
   RigidBody.UpdateWorldInertiaTensor;
  end;
 end;

 if fPhysics.fAllowSleep then begin

  // Find minimum sleep time of the entire island
  MinSleepTime:=3.40e+38;
  First:=true;
  for Index:=0 to fCountRigidBodies-1 do begin
   RigidBody:=fRigidBodies[Index];
   if RigidBody.fRigidBodyType<>krbtStatic then begin
    if (Vector3LengthSquared(RigidBody.fLinearVelocity)>sqr(fPhysics.fLinearVelocityThreshold)) or
       (Vector3LengthSquared(RigidBody.fAngularVelocity)>sqr(fPhysics.fAngularVelocityThreshold)) then begin
     MinSleepTime:=0.0;
     RigidBody.fSleepTime:=0.0;
     First:=false;
    end else if krbfAllowSleep in RigidBody.fFlags then begin
     RigidBody.fSleepTime:=RigidBody.fSleepTime+TimeStep.DeltaTime;
     if First or (MinSleepTime>RigidBody.fSleepTime) then begin
      First:=false;
      MinSleepTime:=RigidBody.fSleepTime;
     end;
    end;
   end;
  end;

	// Put entire island to sleep so long as the minimum found sleep time is below the threshold.
  // If the minimum sleep time reaches below the sleeping threshold, the entire island will be
  // reformed next step and sleep test will be tried again.
  if MinSleepTime>fPhysics.fSleepTimeThreshold then begin
   for Index:=0 to fCountRigidBodies-1 do begin
    if krbfAllowSleep in fRigidBodies[Index].fFlags then begin
     fRigidBodies[Index].SetToSleep;
    end;
   end;
  end;

 end;

end;

procedure TKraftIsland.SolveTimeOfImpact(const TimeStep:TKraftTimeStep;const IndexA,IndexB:longint);
var Iteration,Index:longint;
    RigidBody:TKraftRigidBody;
    SolverVelocity:PKraftSolverVelocity;
    SolverPosition:PKraftSolverPosition;
    Position,LinearVelocity,AngularVelocity,Translation,Rotation:TKraftVector3;
    Orientation:TKraftQuaternion;
begin

 fSolver.Store;

 // Integrate velocities and create state buffers
 for Index:=0 to fCountRigidBodies-1 do begin
  RigidBody:=fRigidBodies[Index];
  SolverVelocity:=@fSolver.fVelocities[Index];
  SolverVelocity^.LinearVelocity:=RigidBody.fLinearVelocity;
  SolverVelocity^.AngularVelocity:=RigidBody.fAngularVelocity;
  SolverPosition:=@fSolver.fPositions[Index];
  SolverPosition^.Position:=RigidBody.fSweep.c;
  SolverPosition^.Orientation:=RigidBody.fSweep.q;
 end;

 fSolver.Initialize(TimeStep);

 for Iteration:=0 to fPhysics.fTimeOfImpactIterations-1 do begin
  if fSolver.SolveTimeOfImpactConstraints(IndexA,IndexB) then begin
   break;
  end;
 end;

 // Leap of faith to new safe state.
 for Index:=0 to fCountRigidBodies-1 do begin
  RigidBody:=fRigidBodies[Index];
  if RigidBody.fRigidBodyType=krbtDynamic then begin
   SolverPosition:=@fSolver.fPositions[Index];
   RigidBody.fSweep.c0:=SolverPosition^.Position;
   RigidBody.fSweep.q0:=SolverPosition^.Orientation;
  end;
 end;

 fSolver.InitializeConstraints;

 // No warm starting is needed for TOI events because warm
 // starting impulses were applied in the discrete solver.

 for Iteration:=0 to fPhysics.fVelocityIterations-1 do begin
  fSolver.SolveVelocityConstraints;
 end;

 // Don't store the TOI contact forces for warm starting
 // because they can be quite large.
                       
 for Index:=0 to fCountRigidBodies-1 do begin

  RigidBody:=fRigidBodies[Index];

  if RigidBody.fRigidBodyType=krbtDynamic then begin

   SolverPosition:=@fSolver.fPositions[Index];
   Position:=SolverPosition^.Position;
   Orientation:=SolverPosition^.Orientation;

   SolverVelocity:=@fSolver.fVelocities[Index];
   LinearVelocity:=SolverVelocity^.LinearVelocity;
   AngularVelocity:=SolverVelocity^.AngularVelocity;

   if fPhysics.fMaximalLinearVelocity>EPSILON then begin
    Translation:=Vector3ScalarMul(LinearVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Translation)>sqr(fPhysics.fMaximalLinearVelocity) then begin
     Vector3Scale(LinearVelocity,fPhysics.fMaximalLinearVelocity/Vector3Length(Translation));
    end;
   end;
   if RigidBody.fMaximalLinearVelocity>EPSILON then begin
    Translation:=Vector3ScalarMul(LinearVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Translation)>sqr(RigidBody.fMaximalLinearVelocity) then begin
     Vector3Scale(LinearVelocity,RigidBody.fMaximalLinearVelocity/Vector3Length(Translation));
    end;
   end;

   if fPhysics.fMaximalAngularVelocity>EPSILON then begin
    Rotation:=Vector3ScalarMul(AngularVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Rotation)>sqr(fPhysics.fMaximalAngularVelocity) then begin
     Vector3Scale(AngularVelocity,fPhysics.fMaximalAngularVelocity/Vector3Length(Rotation));
    end;
   end;
   if RigidBody.fMaximalAngularVelocity>EPSILON then begin
    Rotation:=Vector3ScalarMul(AngularVelocity,TimeStep.DeltaTime);
    if Vector3LengthSquared(Rotation)>sqr(RigidBody.fMaximalAngularVelocity) then begin
     Vector3Scale(AngularVelocity,RigidBody.fMaximalAngularVelocity/Vector3Length(Rotation));
    end;
   end;

   fPhysics.Integrate(Position,Orientation,LinearVelocity,AngularVelocity,TimeStep.DeltaTime);

   RigidBody.fSweep.c:=Position;
   RigidBody.fSweep.q:=Orientation;

   RigidBody.fLinearVelocity:=LinearVelocity;
   RigidBody.fAngularVelocity:=AngularVelocity;

   RigidBody.SynchronizeTransformIncludingShapes;
   RigidBody.UpdateWorldInertiaTensor;
  end;
 end;

end;

{$ifndef KraftPasMP}
constructor TKraftJobThread.Create(const APhysics:TKraft;const AJobManager:TKraftJobManager;const AThreadNumber:longint);
begin
//{$ifdef memdebug}ScanMemoryPoolForCorruptions;{$endif}
 fPhysics:=APhysics;
 fJobManager:=AJobManager;
 fThreadNumber:=AThreadNumber;
 fEvent:=TEvent.Create(nil,false,false,'');
 fDoneEvent:=TEvent.Create(nil,false,false,'');
//{$ifdef memdebug}ScanMemoryPoolForCorruptions;{$endif}
 inherited Create(false);
end;

destructor TKraftJobThread.Destroy;
begin
//{$ifdef memdebug}ScanMemoryPoolForCorruptions;{$endif}
 FreeAndNil(fEvent);
 FreeAndNil(fDoneEvent);
//{$ifdef memdebug}ScanMemoryPoolForCorruptions;{$endif}
 inherited Destroy;
end;

procedure TKraftJobThread.Execute;
var JobIndex:longint;
begin
 SetPrecisionMode(PhysicsFPUPrecisionMode);
 SetExceptionMask(PhysicsFPUExceptionMask);
 SIMDSetOurFlags;
 InterlockedIncrement(fJobManager.fCountAliveThreads);
 while not (Terminated or fJobManager.fThreadsTerminated) do begin
  fEvent.WaitFor(INFINITE);
  if Terminated or fJobManager.fThreadsTerminated then begin
   break;
  end else begin
   repeat
    JobIndex:=InterlockedDecrement(fJobManager.fCountRemainJobs);
    if JobIndex>=0 then begin
     if assigned(fJobManager.fOnProcessJob) then begin
      fJobManager.fOnProcessJob(JobIndex,fThreadNumber);
     end;
    end else begin
     break;
    end;
   until false;
   fDoneEvent.SetEvent;
  end;
 end;
 InterlockedDecrement(fJobManager.fCountAliveThreads);
end;

constructor TKraftJobManager.Create(const APhysics:TKraft);
var Index:longint;
begin
 inherited Create;
 fPhysics:=APhysics;
 fThreads:=nil;
 fCountThreads:=APhysics.fCountThreads;
 SetLength(fThreads,fCountThreads);
 fCountAliveThreads:=0;
 fGranularity:=1;
 fThreadsTerminated:=false;
 fOnProcessJob:=nil;
 for Index:=0 to fCountThreads-1 do begin
  fThreads[Index]:=TKraftJobThread.Create(fPhysics,self,Index);
  fThreads[Index].Priority:=tpHigher;
 end;
end;

destructor TKraftJobManager.Destroy;
var Index:longint;
begin
 fThreadsTerminated:=true;
 for Index:=0 to fCountThreads-1 do begin
  fThreads[Index].Terminate;
  fThreads[Index].fEvent.SetEvent;
  fThreads[Index].WaitFor;
  FreeAndNil(fThreads[Index]);
 end;
 SetLength(fThreads,0);
 inherited Destroy;
end;

procedure TKraftJobManager.WakeUp;
var Index:longint;
begin
 for Index:=0 to fCountThreads-1 do begin
  fThreads[Index].fEvent.SetEvent;
 end;
end;

procedure TKraftJobManager.WaitFor;
var Index:longint;
begin
 for Index:=0 to fCountThreads-1 do begin
  fThreads[Index].fDoneEvent.WaitFor(INFINITE);
 end;
end;

procedure TKraftJobManager.ProcessJobs;
begin
 WakeUp;
 WaitFor;
end;
{$endif}

{$ifdef KraftPasMP}
constructor TKraft.Create(const APasMP:TPasMP);
{$else}
constructor TKraft.Create(const ACountThreads:longint=-1);
{$endif}
const TriangleVertex0:TKraftVector3=(x:0.0;y:0.0;z:0.0{$ifdef SIMD};w:0.0{$endif});
      TriangleVertex1:TKraftVector3=(x:1.0;y:0.0;z:0.01{$ifdef SIMD};w:0.0{$endif});
      TriangleVertex2:TKraftVector3=(x:0.0;y:1.0;z:0.02{$ifdef SIMD};w:0.0{$endif});
var Index:longint;
{$ifndef KraftPasMP}
{$ifdef win32}
    i,j:longint;
    sinfo:SYSTEM_INFO;
    dwProcessAffinityMask,dwSystemAffinityMask:ptruint;
 function GetRealCountOfCPUCores:longint; {$ifdef caninline}inline;{$endif}
 const RelationProcessorCore=0;
       RelationNumaNode=1;
       RelationCache=2;
       RelationProcessorPackage=3;
       RelationGroup=4;
       RelationAll=$ffff;
       CacheUnified=0;
       CacheInstruction=1;
       CacheData=2;
       CacheTrace=3;
 type TLogicalProcessorRelationship=dword;
      TProcessorCacheType=dword;
      TCacheDescriptor=packed record
       Level:byte;
       Associativity:byte;
       LineSize:word;
       Size:dword;
       pcType:TProcessorCacheType;
      end;
      PSystemLogicalProcessorInformation=^TSystemLogicalProcessorInformation;
      TSystemLogicalProcessorInformation=packed record
       ProcessorMask:ptruint;
       case Relationship:TLogicalProcessorRelationship of
        0:(
         Flags:byte;
        );
        1:(
         NodeNumber:dword;
        );
        2:(
         Cache:TCacheDescriptor;
        );
        3:(
         Reserved:array[0..1] of int64;
        );
      end;
      TGetLogicalProcessorInformation=function(Buffer:PSystemLogicalProcessorInformation;out ReturnLength:DWORD):BOOL; stdcall;
 var GetLogicalProcessorInformation:TGetLogicalProcessorInformation;
     Buffer:array of TSystemLogicalProcessorInformation;
     ReturnLength:dword;
     Index,Count:longint;
 begin
  result:=-1;
  Buffer:=nil;
  try
   GetLogicalProcessorInformation:=GetProcAddress(GetModuleHandle('kernel32'),'GetLogicalProcessorInformation');
   if assigned(GetLogicalProcessorInformation) then begin
    SetLength(Buffer,16);
    Count:=0;
    repeat
     ReturnLength:=length(Buffer)*SizeOf(TSystemLogicalProcessorInformation);
     if GetLogicalProcessorInformation(@Buffer[0],ReturnLength) then begin
      Count:=ReturnLength div SizeOf(TSystemLogicalProcessorInformation);
     end else begin
      if GetLastError=ERROR_INSUFFICIENT_BUFFER then begin
       SetLength(Buffer,(ReturnLength div SizeOf(TSystemLogicalProcessorInformation))+1);
       continue;
      end;
     end;
     break;
    until false;
    if Count>0 then begin
     result:=0;
     for Index:=0 to Count-1 do begin
      if Buffer[Index].Relationship=RelationProcessorCore then begin
       inc(result);
      end;
     end;
    end;
   end;
  finally
   SetLength(Buffer,0);
  end;
 end;
{$endif}
{$endif}
begin
 inherited Create;

 fSingleThreaded:=false;

 fHighResolutionTimer:=TKraftHighResolutionTimer.Create;

 fIsSolving:=false;

{$ifdef KraftPasMP}
 fPasMP:=APasMP;

 fCountThreads:=fPasMP.CountJobWorkerThreads;
{$else}

 fCountThreads:=ACountThreads;

{$ifdef win32}
 if fCountThreads<0 then begin
  fCountThreads:=GetRealCountOfCPUCores;
  GetSystemInfo(sinfo);
  GetProcessAffinityMask(GetCurrentProcess,dwProcessAffinityMask,dwSystemAffinityMask);
  j:=0;
  for i:=0 to sinfo.dwNumberOfProcessors-1 do begin
   if (dwProcessAffinityMask and (1 shl i))<>0 then begin
    inc(j);
    if j>=MAX_THREADS then begin
     break;
    end;
   end;
  end;
  if (fCountThreads<0) or (fCountThreads>j) then begin
   fCountThreads:=j;
  end;
 end;
//SetThreadIdealProcessor(GetCurrentThread,0);
{$endif}

 fCountThreads:=Min(Max(fCountThreads,0),MAX_THREADS);
{$endif}

 fNewShapes:=false;

 fConvexHullFirst:=nil;
 fConvexHullLast:=nil;

 fMeshFirst:=nil;
 fMeshLast:=nil;

 fConstraintFirst:=nil;
 fConstraintLast:=nil;

 fCountRigidBodies:=0;
 fRigidBodyIDCounter:=0;

 fRigidBodyFirst:=nil;
 fRigidBodyLast:=nil;

 fStaticRigidBodyCount:=0;

 fStaticRigidBodyFirst:=nil;
 fStaticRigidBodyLast:=nil;

 fDynamicRigidBodyCount:=0;

 fDynamicRigidBodyFirst:=nil;
 fDynamicRigidBodyLast:=nil;

 fKinematicRigidBodyCount:=0;

 fKinematicRigidBodyFirst:=nil;
 fKinematicRigidBodyLast:=nil;

 fStaticAABBTree:=TKraftDynamicAABBTree.Create;
 fSleepingAABBTree:=TKraftDynamicAABBTree.Create;
 fDynamicAABBTree:=TKraftDynamicAABBTree.Create;
 fKinematicAABBTree:=TKraftDynamicAABBTree.Create;

 fIslands:=nil;
 SetLength(fIslands,16);
 for Index:=0 to length(fIslands)-1 do begin
  fIslands[Index]:=TKraftIsland.Create(self,Index);
 end;
 fCountIslands:=0;

 fBroadPhase:=TKraftBroadPhase.Create(self);

 fContactManager:=TKraftContactManager.Create(self);

 fWorldFrequency:=60.0;

 fWorldDeltaTime:=1.0/fWorldFrequency;

 fWorldInverseDeltaTime:=fWorldFrequency;

 fLastInverseDeltaTime:=0.0;

 fAllowSleep:=true;

 fAllowedPenetration:=0.0;

 fGravity.x:=0.0;
 fGravity.y:=-9.83;
 fGravity.z:=0.0;

 fGravityProperty:=TKraftVector3Property.Create(@fGravity);

 fMaximalLinearVelocity:=2.0;

 fMaximalAngularVelocity:=pi*0.5;

 fLinearVelocityThreshold:=0.1;

 fAngularVelocityThreshold:=2.0*(pi/180.0);

 fSleepTimeThreshold:=0.5;

 fVelocityThreshold:=1.0;

 fContactBaumgarte:=0.2;

 fConstraintBaumgarte:=0.2;

 fTimeOfImpactBaumgarte:=0.75;

 fPenetrationSlop:=0.05;

 fLinearSlop:=0.005;

 fAngularSlop:=(2.0/180.0)*pi;

 fMaximalLinearCorrection:=0.2;

 fMaximalAngularCorrection:=(8.0/180.0)*pi;

 fWarmStarting:=true;

 fContinuousMode:=kcmNone;
//fContinuousMode:=kcmMotionClamping;
//fContinuousMode:=kcmTimeOfImpactSubSteps;

 fContinuousAgainstDynamics:=false;

//fTimeOfImpactAlgorithm:=ktoiaConservativeAdvancement;
 fTimeOfImpactAlgorithm:=ktoiaBilateralAdvancement;

 fMaximalSubSteps:=16;

 fContactPositionCorrectionMode:=kpcmNonLinearGaussSeidel;

 fConstraintPositionCorrectionMode:=kpcmNonLinearGaussSeidel;

 fVelocityIterations:=8;

 fPositionIterations:=3;

 fSpeculativeIterations:=8;

 fTimeOfImpactIterations:=20;

 fPerturbationIterations:=MAX_CONTACTS;

 fPersistentContactManifold:=false;

 fAlwaysPerturbating:=false;

 fEnableFriction:=true;

 fLinearVelocityRK4Integration:=false;

 fAngularVelocityRK4Integration:=false;

 fContactBreakingThreshold:=0.02;

 fTriangleShapes:=nil;
 SetLength(fTriangleShapes,Max(1,fCountThreads));
 for Index:=0 to length(fTriangleShapes)-1 do begin
  fTriangleShapes[Index]:=TKraftShapeTriangle.Create(self,nil,TriangleVertex0,TriangleVertex1,TriangleVertex2);
  fTriangleShapes[Index].UpdateShapeAABB;
  fTriangleShapes[Index].CalculateMassData;
 end;

{$ifndef KraftPasMP}
 if fCountThreads>1 then begin
  fJobManager:=TKraftJobManager.Create(self);
 end else begin
  fJobManager:=nil;
 end;
{$endif}

 SIMDSetOurFlags;

end;

destructor TKraft.Destroy;
var Index:longint;
begin

{$ifndef KraftPasMP}
 if assigned(fJobManager) then begin
  FreeAndNil(fJobManager);
 end;
{$endif}

 for Index:=0 to length(fTriangleShapes)-1 do begin
  fTriangleShapes[Index].Free;
 end;
 SetLength(fTriangleShapes,0);

 while assigned(fConstraintLast) do begin
  fConstraintLast.Free;
 end;

 while assigned(fRigidBodyLast) do begin
  fRigidBodyLast.Free;
 end;

 while assigned(fMeshLast) do begin
  fMeshLast.Free;
 end;

 while assigned(fConvexHullLast) do begin
  fConvexHullLast.Free;
 end;

 fBroadPhase.Free;

 fStaticAABBTree.Free;
 fSleepingAABBTree.Free;
 fDynamicAABBTree.Free;
 fKinematicAABBTree.Free;

 for Index:=0 to length(fIslands)-1 do begin
  fIslands[Index].Free;
 end;
 SetLength(fIslands,0);

 fContactManager.Free;

 fHighResolutionTimer.Free;

 fGravityProperty.Free;

 inherited Destroy;
end;

procedure TKraft.SetFrequency(const AFrequency:TKraftScalar);
begin
 fWorldFrequency:=AFrequency;
 fWorldDeltaTime:=1.0/fWorldFrequency;
 fWorldInverseDeltaTime:=fWorldFrequency;
end;

procedure TKraft.Integrate(var Position:TKraftVector3;var Orientation:TKraftQuaternion;const LinearVelocity,AngularVelocity:TKraftVector3;const DeltaTime:TKraftScalar);
const OneDiv3=1.0/3.0;
      OneDiv6=1.0/6.0;
      OneDiv24=1.0/24.0;
var {ThetaLenSquared,ThetaLen,s,w,}DeltaTimeDiv6,DeltaTimeDiv3:TKraftScalar;
//  Theta:TKraftVector3;
    HalfSpinQuaternion:TKraftQuaternion;
    Quaternions:array[0..3] of TKraftQuaternion;
    Positions:array[0..3] of TKraftVector3;
begin

 DeltaTimeDiv6:=DeltaTime*OneDiv6;
 DeltaTimeDiv3:=DeltaTime*OneDiv3;

 if fLinearVelocityRK4Integration then begin
  Positions[0]:=LinearVelocity;
  Positions[1]:=Vector3Add(Positions[0],Vector3ScalarMul(LinearVelocity,DeltaTime*0.5));
  Positions[2]:=Vector3Add(Positions[1],Vector3ScalarMul(LinearVelocity,DeltaTime*0.5));
  Positions[3]:=Vector3Add(Positions[2],Vector3ScalarMul(LinearVelocity,DeltaTime));
  Vector3DirectAdd(Position,
                   Vector3Add(Vector3ScalarMul(Vector3Add(Positions[0],Positions[3]),DeltaTimeDiv6),
                              Vector3ScalarMul(Vector3Add(Positions[1],Positions[2]),DeltaTimeDiv3)));
 end else begin
  Vector3DirectAdd(Position,Vector3ScalarMul(LinearVelocity,DeltaTime));
 end;

 if fAngularVelocityRK4Integration then begin
  HalfSpinQuaternion.x:=AngularVelocity.x*0.5;
  HalfSpinQuaternion.y:=AngularVelocity.y*0.5;
  HalfSpinQuaternion.z:=AngularVelocity.z*0.5;
  HalfSpinQuaternion.w:=0;
  Quaternions[0]:=QuaternionMul(HalfSpinQuaternion,QuaternionTermNormalize(Orientation));
  Quaternions[1]:=QuaternionMul(HalfSpinQuaternion,QuaternionTermNormalize(QuaternionAdd(Orientation,QuaternionScalarMul(Quaternions[0],DeltaTime*0.5))));
  Quaternions[2]:=QuaternionMul(HalfSpinQuaternion,QuaternionTermNormalize(QuaternionAdd(Orientation,QuaternionScalarMul(Quaternions[1],DeltaTime*0.5))));
  Quaternions[3]:=QuaternionMul(HalfSpinQuaternion,QuaternionTermNormalize(QuaternionAdd(Orientation,QuaternionScalarMul(Quaternions[2],DeltaTime))));
  Orientation:=QuaternionTermNormalize(QuaternionAdd(Orientation,
                                                     QuaternionAdd(QuaternionScalarMul(QuaternionAdd(Quaternions[0],Quaternions[3]),DeltaTimeDiv6),
                                                     QuaternionScalarMul(QuaternionAdd(Quaternions[1],Quaternions[2]),DeltaTimeDiv3))));
 end else begin
  HalfSpinQuaternion.x:=AngularVelocity.x;
  HalfSpinQuaternion.y:=AngularVelocity.y;
  HalfSpinQuaternion.z:=AngularVelocity.z;
  HalfSpinQuaternion.w:=0;
  Orientation:=QuaternionTermNormalize(QuaternionAdd(Orientation,QuaternionMul(QuaternionScalarMul(HalfSpinQuaternion,DeltaTime*0.5),Orientation)));{}
//Orientation:=QuaternionTermNormalize(QuaternionIntegrate(Orientation,AngularVelocity,DeltaTime));
{ Theta:=Vector3ScalarMul(AngularVelocity,DeltaTime*0.5);
  ThetaLenSquared:=Vector3LengthSquared(Theta);
  if (sqr(ThetaLenSquared)*OneDiv24)<EPSILON then begin
   w:=1.0-(ThetaLenSquared*0.5);
   s:=1.0-(ThetaLenSquared*OneDiv6);
  end else begin
   ThetaLen:=sqrt(ThetaLenSquared);
   w:=cos(ThetaLen);
   s:=sin(ThetaLen)/ThetaLen;
  end;
  Quaternions[0].x:=Theta.x*s;
  Quaternions[0].y:=Theta.y*s;
  Quaternions[0].z:=Theta.z*s;
  Quaternions[0].w:=w;
  Orientation:=QuaternionTermNormalize(QuaternionMul(Quaternions[0],Orientation));{}
 end;

end;

procedure TKraft.BuildIslands;
var IslandIndex,LastCount,SubIndex:longint;
    SeedRigidBody,SeedRigidBodyStack,CurrentRigidBody,OtherRigidBody,StaticRigidBodiesList:TKraftRigidBody;
    CurrentConstraintEdge:PKraftConstraintEdge;
    CurrentConstraint:TKraftConstraint;
    Island:TKraftIsland;
    ContactPairEdge:PKraftContactPairEdge;
    ContactPair:PKraftContactPair;
begin

 if fCountRigidBodies>length(fIslands) then begin
  LastCount:=length(fIslands);
  SetLength(fIslands,fCountRigidBodies*2);
  for SubIndex:=LastCount to length(fIslands)-1 do begin
   fIslands[SubIndex]:=TKraftIsland.Create(self,SubIndex);
  end;
 end;

 CurrentRigidBody:=fRigidBodyFirst;
 while assigned(CurrentRigidBody) do begin
  CurrentRigidBody.fIsland:=nil;
  CurrentRigidBody.fFlags:=CurrentRigidBody.fFlags-[krbfIslandVisited,krbfIslandStatic];
//Exclude(ContactPair^.Flags,[krbfIslandVisited,krbfIslandStatic]);
  CurrentRigidBody:=CurrentRigidBody.fRigidBodyNext;
 end;

 CurrentConstraint:=fConstraintFirst;
 while assigned(CurrentConstraint) do begin
  Exclude(CurrentConstraint.fFlags,kcfVisited);
  CurrentConstraint:=CurrentConstraint.fNext;
 end;

 ContactPair:=fContactManager.fContactPairFirst;
 while assigned(ContactPair) do begin
  ContactPair.Island:=nil;
  Exclude(ContactPair^.Flags,kcfInIsland);
  ContactPair:=ContactPair^.Next;
 end;

 fCountIslands:=0;

 SeedRigidBody:=fRigidBodyFirst;
 while assigned(SeedRigidBody) do begin

  if (krbfIslandVisited in SeedRigidBody.fFlags) or                               // Seed can't be visited and apart of an island already
     ((SeedRigidBody.fFlags*[krbfAwake,krbfActive])<>[krbfAwake,krbfActive]) or   // Seed must be awake
     (SeedRigidBody.fRigidBodyType=krbtStatic) then begin                         // Seed can't be a static body in order to keep islands as small as possible
   SeedRigidBody:=SeedRigidBody.fRigidBodyNext;
   continue;
  end;

  // Allocate island
  IslandIndex:=fCountIslands;
  inc(fCountIslands);
  if fCountIslands>length(fIslands) then begin
   LastCount:=length(fIslands);
   SetLength(fIslands,fCountIslands*2);
   for SubIndex:=LastCount to length(fIslands)-1 do begin
    fIslands[SubIndex]:=TKraftIsland.Create(self,SubIndex);
   end;
  end;
  Island:=fIslands[IslandIndex];
  Island.Clear;

  StaticRigidBodiesList:=nil;

  // Add first rigid body to the stack
  Include(SeedRigidBody.fFlags,krbfIslandVisited);
  SeedRigidBody.fNextOnIslandBuildStack:=nil;
  SeedRigidBodyStack:=SeedRigidBody;

  // Process seed rigid body loop
  while assigned(SeedRigidBodyStack) do begin

   // Pop next rigid body from the stack
   CurrentRigidBody:=SeedRigidBodyStack;
   SeedRigidBodyStack:=CurrentRigidBody.fNextOnIslandBuildStack;
   CurrentRigidBody.fNextOnIslandBuildStack:=nil;

   // Add to the island
   Island.AddRigidBody(CurrentRigidBody);

   // Awaken all bodies connected to the island
   CurrentRigidBody.SetToAwake;  

   // Do not search across static bodies to keep island formations as small as possible, however the static
   // body itself should be apart of the island in order to properly represent a full contact
   if CurrentRigidBody.fRigidBodyType=krbtStatic then begin

    if not (krbfIslandStatic in CurrentRigidBody.fFlags) then begin
     Include(CurrentRigidBody.fFlags,krbfIslandStatic);
     CurrentRigidBody.fNextStaticRigidBody:=StaticRigidBodiesList;
     StaticRigidBodiesList:=CurrentRigidBody;
    end;

    continue;
   end;

   // Process all contact pairs
   if not (krbfSensor in CurrentRigidBody.fFlags) then begin
    ContactPairEdge:=CurrentRigidBody.fContactPairEdgeFirst;
    while assigned(ContactPairEdge) do begin
     ContactPair:=ContactPairEdge^.ContactPair;
     // Skip contacts that have been added to an island already and we can safely skip contacts if these didn't actually collide with anything,
     // and skip also sensors
     if ((ContactPair^.Flags*[kcfColliding,kcfInIsland])=[kcfColliding]) and
        not ((ksfSensor in ContactPair^.Shapes[0].fFlags) or
             (ksfSensor in ContactPair^.Shapes[1].fFlags)) then begin
      ContactPair^.Flags:=ContactPair^.Flags+[kcfInIsland];
      Island.AddContactPair(ContactPair);
      OtherRigidBody:=ContactPairEdge^.OtherRigidBody;
      if assigned(OtherRigidBody) and ((OtherRigidBody.fFlags*[krbfIslandVisited,krbfSensor])=[]) then begin
       Include(OtherRigidBody.fFlags,krbfIslandVisited);
       OtherRigidBody.fNextOnIslandBuildStack:=SeedRigidBodyStack;
       SeedRigidBodyStack:=OtherRigidBody;
      end;
     end;
     ContactPairEdge:=ContactPairEdge^.Next;
    end;
   end;

   // Process all collected constraints
   CurrentConstraintEdge:=CurrentRigidBody.fConstraintEdgeFirst;
   while assigned(CurrentConstraintEdge) do begin
    CurrentConstraint:=CurrentConstraintEdge^.Constraint;
    if (assigned(CurrentConstraint) and not (kcfVisited in CurrentConstraint.fFlags)) and ((CurrentConstraint.fFlags*[kcfActive,kcfBreaked])=[kcfActive]) then begin
     Include(CurrentConstraint.fFlags,kcfVisited);
     Island.AddConstraint(CurrentConstraint);
     OtherRigidBody:=CurrentConstraintEdge^.OtherRigidBody;
     if (OtherRigidBody<>CurrentRigidBody) and assigned(OtherRigidBody) and ((OtherRigidBody.fFlags*[krbfIslandVisited,krbfSensor])=[]) then begin
      Include(OtherRigidBody.fFlags,krbfIslandVisited);
      OtherRigidBody.fNextOnIslandBuildStack:=SeedRigidBodyStack;
      SeedRigidBodyStack:=OtherRigidBody;
     end;
    end;
    CurrentConstraintEdge:=CurrentConstraintEdge^.Next;
   end;

  end;

  Island.MergeContactPairs;

  // Allow static bodies and with these collected constraints to participate in other islands
  while assigned(StaticRigidBodiesList) do begin
   CurrentRigidBody:=StaticRigidBodiesList;
   CurrentConstraintEdge:=CurrentRigidBody.fConstraintEdgeFirst;
   while assigned(CurrentConstraintEdge) do begin
    CurrentConstraint:=CurrentConstraintEdge^.Constraint;
    if assigned(CurrentConstraint) then begin
     Exclude(CurrentConstraint.fFlags,kcfVisited);
    end;
    CurrentConstraintEdge:=CurrentConstraintEdge^.Next;
   end;
   StaticRigidBodiesList:=CurrentRigidBody.fNextStaticRigidBody;
   CurrentRigidBody.fFlags:=CurrentRigidBody.fFlags-[krbfIslandVisited,krbfIslandStatic];
  end;

  SeedRigidBody:=SeedRigidBody.fRigidBodyNext;

 end;

end;

{$ifdef KraftPasMP}
procedure TKraft.ProcessSolveIslandParallelForFunction(const Job:PPasMPJob;const ThreadIndex:longint;const Data:pointer;const FromIndex,ToIndex:TPasMPNativeInt);
var Index:longint;
begin
 for Index:=FromIndex to ToIndex do begin
  fIslands[Index].Solve(fJobTimeStep);
 end;
end;
{$else}
procedure TKraft.ProcessSolveIslandJob(const JobIndex,ThreadIndex:longint);
begin
 fIslands[JobIndex].Solve(fJobTimeStep);
end;
{$endif}

procedure TKraft.SolveIslands(const TimeStep:TKraftTimeStep);
var Index:longint;
begin
 fJobTimeStep:=TimeStep;
 fIsSolving:=true;
{$ifdef KraftPasMP}
 if assigned(fPasMP) and (fCountIslands>1) and not fSingleThreaded then begin
  fPasMP.Invoke(fPasMP.ParallelFor(nil,0,fCountIslands-1,ProcessSolveIslandParallelForFunction,Max(1,fCountIslands div (fCountThreads*16)),4));
{$else}
 if assigned(fJobManager) and (fCountIslands>1) and not fSingleThreaded then begin
  fJobManager.fOnProcessJob:=ProcessSolveIslandJob;
  fJobManager.fCountRemainJobs:=fCountIslands;
  fJobManager.fGranularity:=Max(1,fCountIslands div (fCountThreads*16));
  fJobManager.ProcessJobs;
{$endif}
 end else begin
  for Index:=0 to fCountIslands-1 do begin
   fIslands[Index].Solve(fJobTimeStep);
  end;
 end;
 fIsSolving:=false;
end;

// Conservative advancement
function TKraft.GetConservativeAdvancementTimeOfImpact(const ShapeA:TKraftShape;const SweepA:TKraftSweep;const ShapeB:TKraftShape;const ShapeBTriangleIndex:longint;const SweepB:TKraftSweep;const TimeStep:TKraftTimeStep;const ThreadIndex:longint;var Beta:TKraftScalar):boolean;
const Radius=1e-3;
var Tries:longint;
    BoundingRadiusA,BoundingRadiusB,MaximumAngularProjectedVelocity,RelativeLinearVelocityLength,Lambda,LastLambda,
    ProjectedLinearVelocity,DistanceLambda,Distance,ContinuousMinimumRadiusScaleFactor:TKraftScalar;
    MeshShape:TKraftShapeMesh;
    MeshTriangle:PKraftMeshTriangle;
    ShapeTriangle:TKraftShapeTriangle;
    Shapes:array[0..1] of TKraftShape;
    RelativeLinearVelocity:TKraftVector3;
    LinearVelocities,AngularVelocities:array[0..1] of TKraftVector3;
    Transforms:array[0..1] of TKraftMatrix4x4;
    GJK:TKraftGJK;
    GJKCachedSimplex:TKraftGJKCachedSimplex;
begin

 result:=false;

 Shapes[0]:=ShapeA;

 if (ShapeBTriangleIndex>=0) and (ShapeB is TKraftShapeMesh) then begin
  MeshShape:=TKraftShapeMesh(ShapeB);
  ShapeTriangle:=TKraftShapeTriangle(fTriangleShapes[ThreadIndex]);
  Shapes[1]:=ShapeTriangle;
  MeshTriangle:=@MeshShape.fMesh.fTriangles[ShapeBTriangleIndex];
  ShapeTriangle.fLocalTransform:=MeshShape.fLocalTransform;
  ShapeTriangle.fWorldTransform:=MeshShape.fWorldTransform;
  ShapeTriangle.fConvexHull.fVertices[0].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[0]];
  ShapeTriangle.fConvexHull.fVertices[1].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[1]];
  ShapeTriangle.fConvexHull.fVertices[2].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[2]];
  ShapeTriangle.UpdateData;
 end else begin
  Shapes[1]:=ShapeB;
 end;

 CalculateVelocity(SweepA.c0,SweepA.q0,SweepA.c,SweepA.q,1.0,LinearVelocities[0],AngularVelocities[0]);
 CalculateVelocity(SweepB.c0,SweepB.q0,SweepB.c,SweepB.q,1.0,LinearVelocities[1],AngularVelocities[1]);

 BoundingRadiusA:=Shapes[0].fAngularMotionDisc;
 BoundingRadiusB:=Shapes[1].fAngularMotionDisc;

 MaximumAngularProjectedVelocity:=((Vector3Length(AngularVelocities[0])*BoundingRadiusA)+(Vector3Length(AngularVelocities[1])*BoundingRadiusB));

 RelativeLinearVelocity:=Vector3Sub(LinearVelocities[1],LinearVelocities[0]);

 RelativeLinearVelocityLength:=Vector3Length(RelativeLinearVelocity);

 if abs(RelativeLinearVelocityLength+MaximumAngularProjectedVelocity)<EPSILON then begin
  exit;
 end;

 ContinuousMinimumRadiusScaleFactor:=Max(ShapeA.fContinuousMinimumRadiusScaleFactor,ShapeB.fContinuousMinimumRadiusScaleFactor);

 if (ContinuousMinimumRadiusScaleFactor>EPSILON) and
    (RelativeLinearVelocityLength<Max(EPSILON,Min(Shapes[0].fShapeSphere.Radius,Shapes[1].fShapeSphere.Radius)*ContinuousMinimumRadiusScaleFactor)) then begin
  exit;
 end;

 Lambda:=0.0;

 LastLambda:=Lambda;

 GJKCachedSimplex.Count:=0;
 
 GJK.CachedSimplex:=@GJKCachedSimplex;
 GJK.Simplex.Count:=0;
 GJK.Shapes[0]:=Shapes[0];
 GJK.Shapes[1]:=Shapes[1];
 GJK.Transforms[0]:=@Transforms[0];
 GJK.Transforms[1]:=@Transforms[1];
 GJK.UseRadii:=true;

 Transforms[0]:=Matrix4x4TermMul(Shapes[0].fLocalTransform,SweepTransform(SweepA,0.0));
 Transforms[1]:=Matrix4x4TermMul(Shapes[1].fLocalTransform,SweepTransform(SweepB,0.0));

 if GJK.Run then begin

  Distance:=GJK.Distance+fAllowedPenetration;

  ProjectedLinearVelocity:=Vector3Dot(RelativeLinearVelocity,GJK.Normal);
  if (ProjectedLinearVelocity+MaximumAngularProjectedVelocity)<=EPSILON then begin
   exit;
  end;

  Tries:=0;

  while Distance>Radius do begin

   ProjectedLinearVelocity:=Vector3Dot(RelativeLinearVelocity,GJK.Normal);
   if (ProjectedLinearVelocity+MaximumAngularProjectedVelocity)<=EPSILON then begin
    exit;
   end;

   DistanceLambda:=Distance/(ProjectedLinearVelocity+MaximumAngularProjectedVelocity);

   Lambda:=Lambda+DistanceLambda;
   if ((Lambda<0.0) or (Lambda>1.0)) or (Lambda<=LastLambda) then begin
    exit;
   end;

   LastLambda:=Lambda;

   Transforms[0]:=Matrix4x4TermMul(Shapes[0].fLocalTransform,SweepTransform(SweepA,Lambda));
   Transforms[1]:=Matrix4x4TermMul(Shapes[1].fLocalTransform,SweepTransform(SweepB,Lambda));

   if GJK.Run then begin

    Distance:=GJK.Distance+fAllowedPenetration;

    inc(Tries);
    if Tries>TimeOfImpactMaximumIterations then begin
     exit;
    end;

   end else begin
    exit;
   end;

  end;

//writeln(Lambda:1:8);

  Beta:=Lambda;
  result:=true;

 end;

end;
  
// Bilateral advancement
function TKraft.GetBilateralAdvancementTimeOfImpact(const ShapeA:TKraftShape;const SweepA:TKraftSweep;const ShapeB:TKraftShape;const ShapeBTriangleIndex:longint;const SweepB:TKraftSweep;const TimeStep:TKraftTimeStep;const ThreadIndex:longint;var Beta:TKraftScalar):boolean;
const sfmNONE=0;
      sfmVERTICES=1;
      sfmEDGEA=2;
      sfmEDGEB=3;
      sfmFACEA=4;
      sfmFACEB=5;
      sfmEDGES=6;
var Iteration,TryIteration,RootIteration,SeparationFunctionMode:longint;
    //Unprocessed,Overlapping:boolean;
    t0,t1,s0,s1,a0,a1,t,s,{tS,tT0,tT1,}TotalRadius,Target,Tolerance,{Distance,CurrentDistance,L,}
    ContinuousMinimumRadiusScaleFactor:TKraftScalar;
    ShapeTriangle:TKraftShapeTriangle;
    MeshShape:TKraftShapeMesh;
    MeshTriangle:PKraftMeshTriangle;
    Axis,{LocalVertex,va,vb,}eA,eB:TKraftVector3;
    LocalPlane:TKraftPlane;
    GJK:TKraftGJK;
    Shapes:array[0..1] of TKraftShape;
    WitnessPoints:array[0..1] of TKraftVector3;
    Transforms:array[0..1] of TKraftMatrix4x4;
    //LinearVelocities,AngularVelocities:array[0..1] of TKraftVector3;
    UniqueGJKVertexIndices:array[0..1,0..2] of longint;
    UniqueGJKVertices:array[0..1,0..2] of TKraftVector3;
    CountUniqueGJKVertices:array[0..1] of longint;
    GJKCachedSimplex:TKraftGJKCachedSimplex;
 function Evaluate:TKraftScalar; //{$ifdef caninline}inline;{$endif}
 begin
  case SeparationFunctionMode of
   sfmVERTICES:begin
    result:=Vector3Dot(Axis,
                       Vector3Sub(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),
                                  Vector3TermMatrixMul(WitnessPoints[0],Transforms[0])));
   end;
   sfmEDGEA,sfmFACEA,sfmEDGES:begin
    result:=PlaneVectorDistance(LocalPlane,Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[1],Transforms[1]),Transforms[0]));
   end;
   sfmEDGEB,sfmFACEB:begin
    result:=PlaneVectorDistance(LocalPlane,Vector3TermMatrixMulInverted(Vector3TermMatrixMul(WitnessPoints[0],Transforms[0]),Transforms[1]));
   end;
   else begin
    result:=0.0;
    Assert(false);
   end;
  end;
 end;
 function FindMinSeparation:TKraftScalar; //{$ifdef caninline}inline;{$endif}
 begin
  case SeparationFunctionMode of
   sfmVERTICES:begin
    WitnessPoints[0]:=Shapes[0].GetLocalFeatureSupportVertex(Shapes[0].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Axis,Transforms[0])));
    WitnessPoints[1]:=Shapes[1].GetLocalFeatureSupportVertex(Shapes[1].GetLocalFeatureSupportIndex(Vector3TermMatrixMulTransposedBasis(Vector3Neg(Axis),Transforms[1])));
   end;
   sfmEDGEA,sfmFACEA,sfmEDGES:begin
    WitnessPoints[1]:=Shapes[1].GetLocalFeatureSupportVertex(Shapes[1].GetLocalFeatureSupportIndex(Vector3Neg(Vector3TermMatrixMulTransposedBasis(Vector3TermMatrixMulBasis(Axis,Transforms[0]),Transforms[1]))));
   end;
   sfmEDGEB,sfmFACEB:begin
    WitnessPoints[0]:=Shapes[0].GetLocalFeatureSupportVertex(Shapes[0].GetLocalFeatureSupportIndex(Vector3Neg(Vector3TermMatrixMulTransposedBasis(Vector3TermMatrixMulBasis(Axis,Transforms[1]),Transforms[0]))));
   end;
  end;
  result:=Evaluate;
 end;
begin

 result:=false;

 Shapes[0]:=ShapeA;

 if (ShapeBTriangleIndex>=0) and (ShapeB is TKraftShapeMesh) then begin
  MeshShape:=TKraftShapeMesh(ShapeB);
  ShapeTriangle:=TKraftShapeTriangle(fTriangleShapes[ThreadIndex]);
  Shapes[1]:=ShapeTriangle;
  MeshTriangle:=@MeshShape.fMesh.fTriangles[ShapeBTriangleIndex];
  ShapeTriangle.fLocalTransform:=MeshShape.fLocalTransform;
  ShapeTriangle.fWorldTransform:=MeshShape.fWorldTransform;
  ShapeTriangle.fConvexHull.fVertices[0].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[0]];
  ShapeTriangle.fConvexHull.fVertices[1].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[1]];
  ShapeTriangle.fConvexHull.fVertices[2].Position:=MeshShape.fMesh.fVertices[MeshTriangle^.Vertices[2]];
  ShapeTriangle.UpdateData;
 end else begin
  Shapes[1]:=ShapeB;
 end;

 ContinuousMinimumRadiusScaleFactor:=Max(ShapeA.fContinuousMinimumRadiusScaleFactor,ShapeB.fContinuousMinimumRadiusScaleFactor);
 if (ContinuousMinimumRadiusScaleFactor>EPSILON) and
    (Vector3Length(Vector3Sub(Vector3Sub(SweepB.c0,SweepB.c),Vector3Sub(SweepA.c0,SweepA.c)))<Max(EPSILON,Min(Shapes[0].fShapeSphere.Radius,Shapes[1].fShapeSphere.Radius)*ContinuousMinimumRadiusScaleFactor)) then begin
  exit;
 end;

 TotalRadius:=Shapes[0].fFeatureRadius+Shapes[1].fFeatureRadius;

 Target:=Max(fLinearSlop,TotalRadius-(3.0*fLinearSlop));

 Tolerance:=fLinearSlop*0.25;
 
 GJKCachedSimplex.Count:=0;

 GJK.CachedSimplex:=@GJKCachedSimplex;
 GJK.Shapes[0]:=Shapes[0];
 GJK.Shapes[1]:=Shapes[1];
 GJK.Transforms[0]:=@Transforms[0];
 GJK.Transforms[1]:=@Transforms[1];
 GJK.UseRadii:=false;

 // Compute current closest features. Setup a separation function to evaluate
 // overlap on the axis between the closest features. Terminate if closest
 // features are repeated.

 t0:=0.0;

 Axis:=Vector3Origin;

 SeparationFunctionMode:=sfmNONE;

 UniqueGJKVertexIndices[0,0]:=0;
 UniqueGJKVertexIndices[0,1]:=0;
 UniqueGJKVertexIndices[1,0]:=0;
 UniqueGJKVertexIndices[1,1]:=0;

 CountUniqueGJKVertices[0]:=0;
 CountUniqueGJKVertices[1]:=0;

 for Iteration:=1 to TimeOfImpactMaximumIterations do begin

  Transforms[0]:=Matrix4x4TermMul(Shapes[0].fLocalTransform,SweepTransform(SweepA,t0));
  Transforms[1]:=Matrix4x4TermMul(Shapes[1].fLocalTransform,SweepTransform(SweepB,t0));

  if not GJK.Run then begin
   Beta:=0.0;
   result:=false;
   exit;
  end;

  // TOI is not defined if shapes began in an overlapping configuration
  if GJK.Distance<EPSILON then begin
   Beta:=0.0;
   result:=false;
   exit;
  end;

  // Check for initial convergent state
  if GJK.Distance<(Target+Tolerance) then begin
   Beta:=t0;
   result:=true;
   exit;
  end;

  // Extract features from GJK
  case GJK.Simplex.Count of
   1:begin

    // GJK point simplex

    UniqueGJKVertexIndices[0,0]:=GJK.Simplex.Vertices[0]^.iA;
    UniqueGJKVertexIndices[1,0]:=GJK.Simplex.Vertices[0]^.iB;
    CountUniqueGJKVertices[0]:=1;
    CountUniqueGJKVertices[1]:=1;

   end;
   2:begin

    // GJK line simplex

    UniqueGJKVertexIndices[0,0]:=GJK.Simplex.Vertices[0]^.iA;
    UniqueGJKVertexIndices[0,1]:=GJK.Simplex.Vertices[1]^.iA;
    if UniqueGJKVertexIndices[0,0]<>UniqueGJKVertexIndices[0,1] then begin
     CountUniqueGJKVertices[0]:=2;
    end else begin
     CountUniqueGJKVertices[0]:=1;
    end;

    UniqueGJKVertexIndices[1,0]:=GJK.Simplex.Vertices[0]^.iB;
    UniqueGJKVertexIndices[1,1]:=GJK.Simplex.Vertices[1]^.iB;
    if UniqueGJKVertexIndices[1,0]<>UniqueGJKVertexIndices[1,1] then begin
     CountUniqueGJKVertices[1]:=2;
    end else begin
     CountUniqueGJKVertices[1]:=1;
    end;

   end;
   3:begin

    // GJK triangle simplex

    UniqueGJKVertexIndices[0,0]:=GJK.Simplex.Vertices[0]^.iA;
    if UniqueGJKVertexIndices[0,0]<>GJK.Simplex.Vertices[1]^.iA then begin
     UniqueGJKVertexIndices[0,1]:=GJK.Simplex.Vertices[1]^.iA;
     if UniqueGJKVertexIndices[0,1]<>GJK.Simplex.Vertices[2]^.iA then begin
      UniqueGJKVertexIndices[0,2]:=GJK.Simplex.Vertices[2]^.iA;
      CountUniqueGJKVertices[0]:=3;
     end else begin
      CountUniqueGJKVertices[0]:=2;
     end;
    end else if UniqueGJKVertexIndices[0,0]<>GJK.Simplex.Vertices[2]^.iA then begin
     UniqueGJKVertexIndices[0,1]:=GJK.Simplex.Vertices[2]^.iA;
     CountUniqueGJKVertices[0]:=2;
    end else begin
     CountUniqueGJKVertices[0]:=1;
    end;

    UniqueGJKVertexIndices[1,0]:=GJK.Simplex.Vertices[0]^.iB;
    if UniqueGJKVertexIndices[1,0]<>GJK.Simplex.Vertices[1]^.iB then begin
     UniqueGJKVertexIndices[1,1]:=GJK.Simplex.Vertices[1]^.iB;
     if UniqueGJKVertexIndices[1,1]<>GJK.Simplex.Vertices[2]^.iB then begin
      UniqueGJKVertexIndices[1,2]:=GJK.Simplex.Vertices[2]^.iB;
      CountUniqueGJKVertices[1]:=3;
     end else begin
      CountUniqueGJKVertices[1]:=2;
     end;
    end else if UniqueGJKVertexIndices[1,0]<>GJK.Simplex.Vertices[2]^.iB then begin
     UniqueGJKVertexIndices[1,1]:=GJK.Simplex.Vertices[2]^.iB;
     CountUniqueGJKVertices[1]:=2;
    end else begin
     CountUniqueGJKVertices[1]:=1;
    end;

   end;
   4:begin
    // GJK tetrahedron simplex, which contains the origin, which means that there is a overlapping configuration, where
    // the time of impact is not defined if shapes are already in an overlapping configuration
    Beta:=0.0;
    result:=false;
    exit;
   end;
   else begin
    // This might happen if the GJK implementation is faulty.
   end;
  end;

  // Initialize the found features for the separation function
  case CountUniqueGJKVertices[0] of
   1:begin
    // Vertex on A
    case CountUniqueGJKVertices[1] of
     1:begin
      // Vertex on A and vertex on B
      Axis:=Vector3NormEx(Vector3Sub(Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]),
                                             Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0])));
      SeparationFunctionMode:=sfmVERTICES;
     end;
     2:begin
      // Vertex on A and edge on B
      UniqueGJKVertices[0,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]),Transforms[0]),Transforms[1]);
      UniqueGJKVertices[1,0]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]);
      UniqueGJKVertices[1,1]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,1]);
      Axis:=Vector3Sub(UniqueGJKVertices[1,1],UniqueGJKVertices[1,0]);
      LocalPlane.Normal:=Vector3NormEx(Vector3Cross(Vector3Cross(Axis,Vector3Sub(UniqueGJKVertices[0,0],UniqueGJKVertices[1,0])),Axis));
      LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[1,0]);
      SeparationFunctionMode:=sfmEDGEB;
     end;
     3:begin
      // Vertex on A and face on B
      UniqueGJKVertices[0,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]),Transforms[0]),Transforms[1]);
      UniqueGJKVertices[1,0]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]);
      UniqueGJKVertices[1,1]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,1]);
      UniqueGJKVertices[1,2]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,2]);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(UniqueGJKVertices[1,0],UniqueGJKVertices[1,1]),Vector3Sub(UniqueGJKVertices[1,2],UniqueGJKVertices[1,1])));
      if Vector3Dot(UniqueGJKVertices[0,0],Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalPlane.Normal:=Axis;
      LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[1,0]);
      SeparationFunctionMode:=sfmFACEB;
     end;
    end;
   end;
   2:begin
    // Edge A
    case CountUniqueGJKVertices[1] of
     1:begin
      // Edge on A and vertex on B
      UniqueGJKVertices[0,0]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]);
      UniqueGJKVertices[0,1]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,1]);
      UniqueGJKVertices[1,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]),Transforms[1]),Transforms[0]);
      Axis:=Vector3Sub(UniqueGJKVertices[0,1],UniqueGJKVertices[0,0]);
      LocalPlane.Normal:=Vector3NormEx(Vector3Cross(Vector3Cross(Axis,Vector3Sub(UniqueGJKVertices[1,0],UniqueGJKVertices[0,0])),Axis));
      LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[0,0]);
      SeparationFunctionMode:=sfmEDGEA;
     end;
     2:begin
      // Edge on A and edge on B
      UniqueGJKVertices[0,0]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]);
      UniqueGJKVertices[0,1]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,1]);
      UniqueGJKVertices[1,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]),Transforms[1]),Transforms[0]);
      UniqueGJKVertices[1,1]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,1]),Transforms[1]),Transforms[0]);
      eA:=Vector3Sub(UniqueGJKVertices[0,1],UniqueGJKVertices[0,0]);
      eB:=Vector3Sub(UniqueGJKVertices[1,1],UniqueGJKVertices[1,0]);
      Axis:=Vector3NormEx(Vector3Cross(eA,eB));
      if Vector3Dot(Vector3Sub(eB,eA),Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalPlane.Normal:=Axis;
      LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,eA);
      SeparationFunctionMode:=sfmEDGES;
     end;
     3:begin
      // Edge on A and face on B
      UniqueGJKVertices[0,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]),Transforms[0]),Transforms[1]);
      UniqueGJKVertices[0,1]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,1]),Transforms[0]),Transforms[1]);
      UniqueGJKVertices[1,0]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]);
      UniqueGJKVertices[1,1]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,1]);
      UniqueGJKVertices[1,2]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,2]);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(UniqueGJKVertices[1,0],UniqueGJKVertices[1,1]),Vector3Sub(UniqueGJKVertices[1,2],UniqueGJKVertices[1,1])));
      if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,0],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,1],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2]) then begin
       if Vector3Dot(UniqueGJKVertices[0,0],Axis)<0.0 then begin
        Axis:=Vector3Neg(Axis);
       end;
      end else begin
       if Vector3Dot(UniqueGJKVertices[0,1],Axis)<0.0 then begin
        Axis:=Vector3Neg(Axis);
       end;
      end;
      LocalPlane.Normal:=Axis;
      LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[1,0]);
      SeparationFunctionMode:=sfmFACEB;
     end;
    end;
   end;
   3:begin
    // Face on A
    case CountUniqueGJKVertices[1] of
     1:begin
      // Face on A and vertex on B
      UniqueGJKVertices[0,0]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]);
      UniqueGJKVertices[0,1]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,1]);
      UniqueGJKVertices[0,2]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,2]);
      UniqueGJKVertices[1,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]),Transforms[1]),Transforms[0]);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(UniqueGJKVertices[0,0],UniqueGJKVertices[0,1]),Vector3Sub(UniqueGJKVertices[0,2],UniqueGJKVertices[0,1])));
      if Vector3Dot(UniqueGJKVertices[1,0],Axis)<0.0 then begin
       Axis:=Vector3Neg(Axis);
      end;
      LocalPlane.Normal:=Axis;
      LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[0,0]);
      SeparationFunctionMode:=sfmFACEA;
     end;
     2:begin
      // Face on A and edge on B
      UniqueGJKVertices[0,0]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]);
      UniqueGJKVertices[0,1]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,1]);
      UniqueGJKVertices[0,2]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,2]);
      UniqueGJKVertices[1,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]),Transforms[1]),Transforms[0]);
      UniqueGJKVertices[1,1]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,1]),Transforms[1]),Transforms[0]);
      Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(UniqueGJKVertices[0,0],UniqueGJKVertices[0,1]),Vector3Sub(UniqueGJKVertices[0,2],UniqueGJKVertices[0,1])));
      if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,0],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,1],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2]) then begin
       if Vector3Dot(UniqueGJKVertices[1,0],Axis)<0.0 then begin
        Axis:=Vector3Neg(Axis);
       end;
      end else begin
       if Vector3Dot(UniqueGJKVertices[1,1],Axis)<0.0 then begin
        Axis:=Vector3Neg(Axis);
       end;
      end;
      LocalPlane.Normal:=Axis;
      LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[0,0]);
      SeparationFunctionMode:=sfmFACEA;
     end;
     3:begin
      // Face on A and face on B
      UniqueGJKVertices[0,0]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,0]);
      UniqueGJKVertices[0,1]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,1]);
      UniqueGJKVertices[0,2]:=Shapes[0].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[0,2]);
      UniqueGJKVertices[1,0]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,0]);
      UniqueGJKVertices[1,1]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,1]);
      UniqueGJKVertices[1,2]:=Shapes[1].GetLocalFeatureSupportVertex(UniqueGJKVertexIndices[1,2]);
      if CalculateArea(UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2])<CalculateArea(UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2]) then begin
       // Face B has a larger area than face A
       UniqueGJKVertices[0,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(UniqueGJKVertices[0,0],Transforms[0]),Transforms[1]);
       UniqueGJKVertices[0,1]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(UniqueGJKVertices[0,1],Transforms[0]),Transforms[1]);
       UniqueGJKVertices[0,2]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(UniqueGJKVertices[0,2],Transforms[0]),Transforms[1]);
       Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(UniqueGJKVertices[1,0],UniqueGJKVertices[1,1]),Vector3Sub(UniqueGJKVertices[1,2],UniqueGJKVertices[1,1])));
       if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,0],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,1],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2]) then begin
        if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,0],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,2],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2]) then begin
         if Vector3Dot(UniqueGJKVertices[0,0],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end else begin
         if Vector3Dot(UniqueGJKVertices[0,2],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end;
       end else begin
        if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,1],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[0,2],UniqueGJKVertices[1,0],UniqueGJKVertices[1,1],UniqueGJKVertices[1,2]) then begin
         if Vector3Dot(UniqueGJKVertices[0,1],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end else begin
         if Vector3Dot(UniqueGJKVertices[0,2],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end;
       end;
       LocalPlane.Normal:=Axis;
       LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[1,0]);
       SeparationFunctionMode:=sfmFACEB;
      end else begin
       // Face A has a larger area than face B
       UniqueGJKVertices[1,0]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(UniqueGJKVertices[1,0],Transforms[1]),Transforms[0]);
       UniqueGJKVertices[1,1]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(UniqueGJKVertices[1,1],Transforms[1]),Transforms[0]);
       UniqueGJKVertices[1,2]:=Vector3TermMatrixMulInverted(Vector3TermMatrixMul(UniqueGJKVertices[1,2],Transforms[1]),Transforms[0]);
       Axis:=Vector3NormEx(Vector3Cross(Vector3Sub(UniqueGJKVertices[0,0],UniqueGJKVertices[0,1]),Vector3Sub(UniqueGJKVertices[0,2],UniqueGJKVertices[0,1])));
       if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,0],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,1],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2]) then begin
        if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,0],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,2],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2]) then begin
         if Vector3Dot(UniqueGJKVertices[1,0],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end else begin
         if Vector3Dot(UniqueGJKVertices[1,2],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end;
       end else begin
        if SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,1],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2])<SquaredDistanceFromPointToTriangle(UniqueGJKVertices[1,2],UniqueGJKVertices[0,0],UniqueGJKVertices[0,1],UniqueGJKVertices[0,2]) then begin
         if Vector3Dot(UniqueGJKVertices[1,1],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end else begin
         if Vector3Dot(UniqueGJKVertices[1,2],Axis)<0.0 then begin
          Axis:=Vector3Neg(Axis);
         end;
        end;
       end;
       LocalPlane.Normal:=Axis;
       LocalPlane.Distance:=-Vector3Dot(LocalPlane.Normal,UniqueGJKVertices[0,0]);
       SeparationFunctionMode:=sfmFACEA;
      end;
     end;
    end;
   end;
  end;

  // Successively resolve the deepest point to compute the time of impact, loop is bounded by the number of Vertices to be resolved.
  t1:=1.0;
  for TryIteration:=1 to 64 do begin

   // Compute deepest witness points at t1
   Transforms[0]:=Matrix4x4TermMul(Shapes[0].fLocalTransform,SweepTransform(SweepA,t1));
   Transforms[1]:=Matrix4x4TermMul(Shapes[1].fLocalTransform,SweepTransform(SweepB,t1));
   s1:=FindMinSeparation;

   // Is the final configuration separated?
   if s1>(Target+Tolerance) then begin
    exit;
   end;

   // Has the separation reached tolerance?
   if s1>(Target-Tolerance) then begin
    // Advance the sweeps
    t0:=t1;
    if t0>=(1.0-EPSILON) then begin
     exit;
    end else begin
     break;
    end;
   end;

   // Compute the initial separation of the witness points
   Transforms[0]:=Matrix4x4TermMul(Shapes[0].fLocalTransform,SweepTransform(SweepA,t0));
   Transforms[1]:=Matrix4x4TermMul(Shapes[1].fLocalTransform,SweepTransform(SweepB,t0));
   s0:=Evaluate;

   // Check for initial overlap
   if s0<(Target-Tolerance) then begin
    // This might happen if the root finder runs out of iterations. Also more likely to happen with a poor separation function.
    Beta:=t0;
    result:=false;
    exit;
   end;

   // Check for touching, t0 should hold the time of impact (could be 0.0)
   if s0<=(Target+Tolerance) then begin
    Beta:=t0;
    result:=true;
    exit;
   end;

   // Compute the 1D root of: f(x)-Target=0
   a0:=t0;
   a1:=t1;
   for RootIteration:=0 to 63 do begin

    if (RootIteration and 1)<>0 then begin
     t:=a1+((Target-s0)*((a1-a0)/(s1-s0)));
    end else begin
     t:=(a0+a1)*0.5;
    end;

    Transforms[0]:=Matrix4x4TermMul(Shapes[0].fLocalTransform,SweepTransform(SweepA,t));
    Transforms[1]:=Matrix4x4TermMul(Shapes[1].fLocalTransform,SweepTransform(SweepB,t));
    s:=Evaluate;

    if abs(s-Target)<Tolerance then begin
     // t1 holds a tentative value for t0
     t1:=t;
     break;
    end else if s>Target then begin
     a0:=t;
     s0:=s;
    end else begin
     a1:=t;
     s1:=s;
    end;

   end;

  end;

 end;

end;

function TKraft.GetTimeOfImpact(const ShapeA:TKraftShape;const SweepA:TKraftSweep;const ShapeB:TKraftShape;const ShapeBTriangleIndex:longint;const SweepB:TKraftSweep;const TimeStep:TKraftTimeStep;const ThreadIndex:longint;var Beta:TKraftScalar):boolean;
begin
 case fTimeOfImpactAlgorithm of
  ktoiaConservativeAdvancement:begin
   result:=GetConservativeAdvancementTimeOfImpact(ShapeA,SweepA,ShapeB,ShapeBTriangleIndex,SweepB,TimeStep,ThreadIndex,Beta);
  end;
  else {ktoiaBilateralAdvancement:}begin
   result:=GetBilateralAdvancementTimeOfImpact(ShapeA,SweepA,ShapeB,ShapeBTriangleIndex,SweepB,TimeStep,ThreadIndex,Beta);
  end;
 end;
end;

procedure TKraft.Solve(const TimeStep:TKraftTimeStep);
var RigidBody:TKraftRigidBody;
begin

 BuildIslands;

 SolveIslands(TimeStep);

 RigidBody:=fDynamicRigidBodyFirst;
 while assigned(RigidBody) do begin
  RigidBody.SynchronizeProxies;
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 RigidBody:=fKinematicRigidBodyFirst;
 while assigned(RigidBody) do begin
  RigidBody.SynchronizeProxies;
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 fContactManager.DoBroadPhase;

 fContactManager.DoMidPhase;

end;

procedure TKraft.SolveContinuousMotionClamping(const TimeStep:TKraftTimeStep);
var RigidBody:TKraftRigidBody;
    ContactPair:PKraftContactPair;
    Beta:TKraftScalar;
    NeedUpdate:boolean;
    ContactPairEdge:PKraftContactPairEdge;
    RigidBodies:array[0..1] of TKraftRigidBody;
    Sweeps:array[0..1] of TKraftSweep;
begin

 NeedUpdate:=false;

 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  RigidBody.fSweep.Alpha0:=0.0;
  RigidBody.fTimeOfImpact:=1.0;
  RigidBody.fIsland:=nil;
  RigidBody.fFlags:=RigidBody.fFlags-[krbfIslandVisited,krbfIslandStatic];
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 ContactPair:=fContactManager.fContactPairFirst;
 while assigned(ContactPair) do begin
  ContactPair^.Island:=nil;
  ContactPair^.Flags:=ContactPair^.Flags-[kcfInIsland,kcfTimeOfImpact];
  ContactPair^.TimeOfImpactCount:=0;
  ContactPair^.TimeOfImpact:=1.0;
  ContactPair:=ContactPair.Next;
 end;

 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  if RigidBody.fRigidBodyType=krbtDynamic then begin
   ContactPairEdge:=RigidBody.fContactPairEdgeFirst;
   while assigned(ContactPairEdge) do begin
    ContactPair:=ContactPairEdge^.ContactPair;
    if (not (kcfInIsland in ContactPair^.Flags)) and
       (((RigidBody.fFlags*[krbfAwake,krbfActive])=[krbfAwake,krbfActive]) or
        ((ContactPairEdge^.OtherRigidBody.fFlags*[krbfAwake,krbfActive])=[krbfAwake,krbfActive])) and
       (not (((ksfSensor in ContactPair^.Shapes[0].fFlags) or
              (ksfSensor in ContactPair^.Shapes[1].fFlags)) or
             ((krbfSensor in RigidBody.fFlags) or
              (krbfSensor in ContactPairEdge^.OtherRigidBody.fFlags)))) and
       (not (((RigidBody.fRigidBodyType=krbtDynamic) and (ContactPairEdge^.OtherRigidBody.fRigidBodyType=krbtDynamic)) and not
             (fContinuousAgainstDynamics and
              ((krbfContinuousAgainstDynamics in RigidBody.fFlags) or
               (krbfContinuousAgainstDynamics in ContactPairEdge^.OtherRigidBody.fFlags)
              )
             )
            )
       ) then begin
     ContactPair^.Flags:=ContactPair^.Flags+[kcfInIsland];
     if not (kcfColliding in ContactPair^.Flags) then begin
      RigidBodies[0]:=ContactPair^.Shapes[0].fRigidBody;
      RigidBodies[1]:=ContactPair^.Shapes[1].fRigidBody;
      if assigned(RigidBodies[0]) and assigned(RigidBodies[1]) then begin
       Sweeps[0]:=SweepTermNormalize(RigidBodies[0].fSweep);
       Sweeps[1]:=SweepTermNormalize(RigidBodies[1].fSweep);
       if GetTimeOfImpact(ContactPair^.Shapes[0],
                          Sweeps[0],
                          ContactPair^.Shapes[1],
                          ContactPair^.ElementIndex,
                          Sweeps[1],
                          TimeStep,
                          0,
                          Beta) then begin
        // Check for that the object will not get stucking, when it is alreading colliding at beginning
        if Beta>0.0 then begin
         RigidBodies[0].fTimeOfImpact:=Min(RigidBodies[0].fTimeOfImpact,Beta);
         RigidBodies[1].fTimeOfImpact:=Min(RigidBodies[1].fTimeOfImpact,Beta);
        end;
       end;
      end;
     end;
    end;
    ContactPairEdge:=ContactPairEdge^.Next;
   end;
  end;
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  if (RigidBody.fRigidBodyType=krbtDynamic) and ({(RigidBody.fTimeOfImpact>0.0) and} (RigidBody.fTimeOfImpact<1.0)) then begin
// writeln(RigidBody.fTimeOfImpact:1:8);
   RigidBody.Advance(RigidBody.fTimeOfImpact);
   RigidBody.SynchronizeProxies;
   NeedUpdate:=true;
  end;
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 if NeedUpdate then begin
  fContactManager.DoBroadPhase;
  fContactManager.DoMidPhase;
  fContactManager.DoNarrowPhase;
 end;

end;

procedure TKraft.SolveContinuousTimeOfImpactSubSteps(const TimeStep:TKraftTimeStep);
const FLAG_VISITED=1 shl 0;
      FLAG_STATIC=1 shl 1;
var TryIndex,Index,SubIndex,LastCount,Count,IndexA,IndexB{,c{}:longint;
    NeedUpdate:boolean;
    RigidBody,CurrentRigidBody,OtherRigidBody:TKraftRigidBody;
    ContactPair,MinimumContactPair:PKraftContactPair;
    MinimumAlpha,Alpha,Alpha0,Beta:TKraftScalar;
    Island:TKraftIsland;
    ContactPairEdge:PKraftContactPairEdge;
    RigidBodies:array[0..1] of TKraftRigidBody;
    BackupSweeps:array[0..1] of TKraftSweep;
    SubTimeStep:TKraftTimeStep;
begin

 NeedUpdate:=false;

 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  RigidBody.fSweep.Alpha0:=0.0;
  RigidBody.fIsland:=nil;
  RigidBody.fFlags:=RigidBody.fFlags-[krbfIslandVisited,krbfIslandStatic];
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 Count:=0;
 ContactPair:=fContactManager.fContactPairFirst;
 while assigned(ContactPair) do begin
  ContactPair^.Island:=nil;
  ContactPair^.Flags:=ContactPair^.Flags-[kcfInIsland,kcfTimeOfImpact];
  ContactPair^.TimeOfImpactCount:=0;
  ContactPair^.TimeOfImpact:=1.0;
  inc(Count);
  ContactPair:=ContactPair.Next;
 end;

 for TryIndex:=1 to Count do begin

  MinimumContactPair:=nil;
  MinimumAlpha:=1.0;

//c:=0;

  ContactPair:=fContactManager.fContactPairFirst;
  while assigned(ContactPair) do begin

   if (ContactPair^.TimeOfImpactCount>=fMaximalSubSteps) or not (kcfEnabled in ContactPair^.Flags) then begin
    ContactPair:=ContactPair^.Next;
    continue;
   end;

   if kcfTimeOfImpact in ContactPair^.Flags then begin

    Alpha:=ContactPair^.TimeOfImpact;

   end else begin

    Alpha:=1.0;

    if (ksfSensor in ContactPair^.Shapes[0].fFlags) or
       (ksfSensor in ContactPair^.Shapes[1].fFlags) then begin
     ContactPair:=ContactPair^.Next;
     continue;
    end;

    RigidBodies[0]:=ContactPair^.Shapes[0].fRigidBody;
    RigidBodies[1]:=ContactPair^.Shapes[1].fRigidBody;

    if not ((assigned(RigidBodies[0]) and assigned(RigidBodies[1])) and
            (((RigidBodies[0].fRigidBodyType=krbtDynamic) or (RigidBodies[1].fRigidBodyType=krbtDynamic)) and
             (((RigidBodies[0].fFlags*[krbfAwake,krbfActive])=[krbfAwake,krbfActive]) or
              ((RigidBodies[1].fFlags*[krbfAwake,krbfActive])=[krbfAwake,krbfActive])) and
             ((krbfContinuous in RigidBodies[0].fFlags) and (krbfContinuous in RigidBodies[1].fFlags)))) then begin
     ContactPair:=ContactPair^.Next;
     continue;
    end;

    if ((krbfSensor in ContactPair^.RigidBodies[0].fFlags) or
        (krbfSensor in ContactPair^.RigidBodies[1].fFlags)) or
       (((RigidBodies[0].fRigidBodyType=krbtDynamic) and (RigidBodies[1].fRigidBodyType=krbtDynamic)) and not
        (fContinuousAgainstDynamics and
         ((krbfContinuousAgainstDynamics in RigidBodies[0].fFlags) or
          (krbfContinuousAgainstDynamics in RigidBodies[1].fFlags)))) then begin
     ContactPair:=ContactPair^.Next;
     continue;
    end;

{   if (Vector3Length(RigidBodies[0].LinearVelocity)<EPSILON) and
       (Vector3Length(RigidBodies[0].AngularVelocity)<EPSILON) and
       (Vector3Length(RigidBodies[1].LinearVelocity)<EPSILON) and
       (Vector3Length(RigidBodies[1].AngularVelocity)<EPSILON) then begin
     ContactPair:=ContactPair^.Next;
     continue;
    end;{}

    // Compute TOI for this elligable contact and place sweeps onto the same time interval
    if RigidBodies[0].fSweep.Alpha0<RigidBodies[1].fSweep.Alpha0 then begin
     Alpha0:=RigidBodies[1].fSweep.Alpha0;
     SweepAdvance(RigidBodies[0].fSweep,Alpha0);
    end else if RigidBodies[1].fSweep.Alpha0<RigidBodies[0].fSweep.Alpha0 then begin
     Alpha0:=RigidBodies[0].fSweep.Alpha0;
     SweepAdvance(RigidBodies[1].fSweep,Alpha0);
    end else begin
     Alpha0:=RigidBodies[0].fSweep.Alpha0;
    end;

    Assert(Alpha0<1.0);

    Beta:=0.0;
    if GetTimeOfImpact(ContactPair^.Shapes[0],
                       SweepTermNormalize(RigidBodies[0].fSweep),
                       ContactPair^.Shapes[1],
                       ContactPair^.ElementIndex,
                       SweepTermNormalize(RigidBodies[1].fSweep),
                       TimeStep,
                       0,
                       Beta) then begin
     Alpha:=Min(Alpha0+((1.0-Alpha0)*Beta),1.0);
    end;

    ContactPair^.TimeOfImpact:=Alpha;
    ContactPair^.Flags:=ContactPair^.Flags+[kcfTimeOfImpact];

   end;

   if MinimumAlpha>Alpha then begin
    MinimumAlpha:=Alpha;
    MinimumContactPair:=ContactPair;
   end;

// inc(c);
   ContactPair:=ContactPair^.Next;

  end;

  // End loop if no minimum contact exists (all have been processed, if any existed) or if time is almost exhausted
  if (not assigned(MinimumContactPair)) or ((1.0-EPSILON)<MinimumAlpha) then begin
   break;
  end;

{ if c>=0 then begin
   writeln(TryIndex:4,' ',c:4,' ',MinimumAlpha:1:8);
  end;{}

  RigidBodies[0]:=MinimumContactPair^.Shapes[0].fRigidBody;
  RigidBodies[1]:=MinimumContactPair^.Shapes[1].fRigidBody;

  BackupSweeps[0]:=RigidBodies[0].fSweep;
  BackupSweeps[1]:=RigidBodies[1].fSweep;

  RigidBodies[0].Advance(MinimumAlpha);
  RigidBodies[1].Advance(MinimumAlpha);

  MinimumContactPair^.DetectCollisions(fContactManager,fTriangleShapes[0],0,false,0.0);

  MinimumContactPair^.Flags:=MinimumContactPair^.Flags-[kcfTimeOfImpact];
  inc(MinimumContactPair^.TimeOfImpactCount);

  if (not (kcfColliding in MinimumContactPair^.Flags)) or not (kcfEnabled in MinimumContactPair^.Flags) then begin
   MinimumContactPair^.Flags:=MinimumContactPair^.Flags-[kcfEnabled];
   RigidBodies[0].fSweep:=BackupSweeps[0];
   RigidBodies[1].fSweep:=BackupSweeps[1];
   RigidBodies[0].SynchronizeTransformIncludingShapes;
   RigidBodies[1].SynchronizeTransformIncludingShapes;
   continue;
  end;

  RigidBodies[0].SetToAwake;
  RigidBodies[1].SetToAwake;

  if length(fIslands)<1 then begin
   LastCount:=length(fIslands);
   SetLength(fIslands,2);
   for SubIndex:=LastCount to length(fIslands)-1 do begin
    fIslands[SubIndex]:=TKraftIsland.Create(self,SubIndex);
   end;
  end;

  CurrentRigidBody:=fRigidBodyFirst;
  while assigned(CurrentRigidBody) do begin
   CurrentRigidBody.fIsland:=nil;
   CurrentRigidBody.fFlags:=CurrentRigidBody.fFlags-[krbfIslandVisited,krbfIslandStatic];
   CurrentRigidBody:=CurrentRigidBody.fRigidBodyNext;
  end;

  Island:=fIslands[0];
  Island.Clear;
  IndexA:=Island.AddRigidBody(RigidBodies[0]);
  IndexB:=Island.AddRigidBody(RigidBodies[1]);
  Island.AddContactPair(MinimumContactPair);

  Include(RigidBodies[0].fFlags,krbfIslandVisited);
  Include(RigidBodies[1].fFlags,krbfIslandVisited);
  Include(MinimumContactPair^.Flags,kcfInIsland);

  for Index:=0 to 1 do begin

   RigidBody:=RigidBodies[Index];

   if RigidBody.fRigidBodyType=krbtDynamic then begin

    ContactPairEdge:=RigidBody.fContactPairEdgeFirst;
    while assigned(ContactPairEdge) do begin
     ContactPair:=ContactPairEdge^.ContactPair;
     OtherRigidBody:=ContactPairEdge^.OtherRigidBody;
     if (not (kcfInIsland in ContactPair^.Flags)) and not
        ((((ksfSensor in ContactPair^.Shapes[0].fFlags) or
           (ksfSensor in ContactPair^.Shapes[1].fFlags)) or
          ((krbfSensor in RigidBody.fFlags) or
           (krbfSensor in OtherRigidBody.fFlags))) or
         ((OtherRigidBody.fRigidBodyType=krbtDynamic) and not
          (fContinuousAgainstDynamics and
           ((krbfContinuousAgainstDynamics in RigidBody.fFlags) or
            (krbfContinuousAgainstDynamics in OtherRigidBody.fFlags))))) then begin

      BackupSweeps[0]:=OtherRigidBody.fSweep;
      if (not (krbfIslandVisited in OtherRigidBody.fFlags)) and not assigned(OtherRigidBody.fIsland) then begin
       OtherRigidBody.Advance(MinimumAlpha);
      end;

      ContactPair^.DetectCollisions(fContactManager,fTriangleShapes[0],0,false,0.0);

      if (not (kcfEnabled in ContactPair^.Flags)) or not (kcfColliding in ContactPair^.Flags) then begin
       OtherRigidBody.fSweep:=BackupSweeps[0];
       OtherRigidBody.SynchronizeTransformIncludingShapes;
      end else begin

       Island.AddContactPair(ContactPair);
       ContactPair^.Flags:=ContactPair^.Flags+[kcfInIsland];

       if (krbfIslandVisited in OtherRigidBody.fFlags) or assigned(OtherRigidBody.fIsland) then begin
        Island.AddRigidBody(OtherRigidBody);
        Include(OtherRigidBody.fFlags,krbfIslandVisited);
        if OtherRigidBody.fRigidBodyType<>krbtStatic then begin
         OtherRigidBody.SetToAwake;
        end;
       end;

      end;

     end;
     ContactPairEdge:=ContactPairEdge^.Next;
    end;

   end;

  end;

  Island.MergeContactPairs;

  SubTimeStep.DeltaTime:=(1.0-MinimumAlpha)*TimeStep.DeltaTime;
  SubTimeStep.InverseDeltaTime:=1.0/SubTimeStep.DeltaTime;
  SubTimeStep.DeltaTimeRatio:=1.0;
  SubTimeStep.WarmStarting:=false;
  Island.SolveTimeOfImpact(SubTimeStep,IndexA,IndexB);

  for Index:=0 to Island.fCountRigidBodies-1 do begin
   RigidBody:=Island.fRigidBodies[Index];
   RigidBody.fIsland:=nil;
   RigidBody.fFlags:=RigidBody.fFlags-[krbfIslandVisited,krbfIslandStatic];
   if RigidBody.fRigidBodyType=krbtDynamic then begin
    RigidBody.SynchronizeProxies;
    ContactPairEdge:=RigidBody.fContactPairEdgeFirst;
    while assigned(ContactPairEdge) do begin
     ContactPair:=ContactPairEdge^.ContactPair;
     ContactPair^.Flags:=ContactPair^.Flags-[kcfInIsland,kcfTimeOfImpact];
     ContactPairEdge:=ContactPairEdge^.Next;
    end;
   end;
  end;

  fContactManager.DoBroadPhase;
  fContactManager.DoMidPhase;
  NeedUpdate:=true;

 end;

 if NeedUpdate then begin
  fContactManager.DoNarrowPhase;
 end;

end;

procedure TKraft.StoreWorldTransforms;
var RigidBody:TKraftRigidBody;
begin
 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  RigidBody.StoreWorldTransform;
  RigidBody:=RigidBody.fRigidBodyNext;
 end;
end;

procedure TKraft.InterpolateWorldTransforms(const Alpha:TKraftScalar);
var RigidBody:TKraftRigidBody;
begin
 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  RigidBody.InterpolateWorldTransform(Alpha);
  RigidBody:=RigidBody.fRigidBodyNext;
 end;
end;

procedure TKraft.Step(const ADeltaTime:TKraftScalar=0);
var RigidBody:TKraftRigidBody;
    Constraint,NextConstraint:TKraftConstraint;
    OldFPUPrecisionMode:TFPUPrecisionMode;
    OldFPUExceptionMask:TFPUExceptionMask;
    OldSIMDFlags:longword;
    StartTime:int64;
    TimeStep:TKraftTimeStep;
begin

 fBroadPhaseTime:=0;
 fMidPhaseTime:=0;
 fNarrowPhaseTime:=0;
 fSolverTime:=0;
 fContinuousTime:=0;
 fTotalTime:=fHighResolutionTimer.GetTime;

 if abs(ADeltaTime)<EPSILON then begin
  TimeStep.DeltaTime:=fWorldDeltaTime;
 end else begin
  TimeStep.DeltaTime:=ADeltaTime;
 end;
 if IsZero(TimeStep.DeltaTime) then begin
  TimeStep.InverseDeltaTime:=1.0;
 end else begin
  TimeStep.InverseDeltaTime:=1.0/TimeStep.DeltaTime;
 end;                     
 TimeStep.DeltaTimeRatio:=fLastInverseDeltaTime*TimeStep.DeltaTime;
 TimeStep.WarmStarting:=fWarmStarting;

{$ifdef DebugDraw}
 fContactManager.fCountDebugClipVertexLists:=0;
{$endif}

 OldFPUPrecisionMode:=GetPrecisionMode;
 if OldFPUPrecisionMode<>PhysicsFPUPrecisionMode then begin
  SetPrecisionMode(PhysicsFPUPrecisionMode);
 end;

 OldFPUExceptionMask:=GetExceptionMask;
 if OldFPUExceptionMask<>PhysicsFPUExceptionMask then begin
  SetExceptionMask(PhysicsFPUExceptionMask);
 end;

 OldSIMDFlags:=SIMDGetFlags;

 SIMDSetOurFlags;

 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  if assigned(RigidBody.fOnPreStep) then begin
   RigidBody.fOnPreStep(RigidBody,TimeStep);
  end;
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 if fNewShapes then begin
  fNewShapes:=false;
  fContactManager.DoBroadPhase;
  fContactManager.DoMidPhase;
 end;

 fContactManager.DoNarrowPhase;

 StartTime:=fHighResolutionTimer.GetTime;
 Solve(TimeStep);
 inc(fSolverTime,fHighResolutionTimer.GetTime-StartTime);

 case fContinuousMode of
  kcmMotionClamping:begin
   StartTime:=fHighResolutionTimer.GetTime;
   SolveContinuousMotionClamping(TimeStep);
   inc(fContinuousTime,fHighResolutionTimer.GetTime-StartTime);
  end;
  kcmTimeOfImpactSubSteps:begin
   if fContactPositionCorrectionMode=kpcmNonLinearGaussSeidel then begin
    StartTime:=fHighResolutionTimer.GetTime;
    SolveContinuousTimeOfImpactSubSteps(TimeStep);
    inc(fContinuousTime,fHighResolutionTimer.GetTime-StartTime);
   end else begin
    StartTime:=fHighResolutionTimer.GetTime;
    SolveContinuousMotionClamping(TimeStep);
    inc(fContinuousTime,fHighResolutionTimer.GetTime-StartTime);
   end;
  end;
 end;

 Constraint:=fConstraintFirst;
 while assigned(Constraint) do begin
  NextConstraint:=Constraint.fNext;
  if kcfFreshBreaked in Constraint.fFlags then begin
   Exclude(Constraint.fFlags,kcfFreshBreaked);
   if assigned(Constraint.fOnBreak) then begin
    Constraint.fOnBreak(self,Constraint);
   end;
  end;
  Constraint:=NextConstraint;
 end;

 RigidBody:=fRigidBodyFirst;
 while assigned(RigidBody) do begin
  if RigidBody.fRigidBodyType<>krbtSTATIC then begin
   RigidBody.fForce:=Vector3Origin;
   RigidBody.fTorque:=Vector3Origin;
  end;
  if assigned(RigidBody.fOnPostStep) then begin
   RigidBody.fOnPostStep(RigidBody,TimeStep);
  end;
  RigidBody:=RigidBody.fRigidBodyNext;
 end;

 if TimeStep.DeltaTime>0.0 then begin
  fLastInverseDeltaTime:=TimeStep.InverseDeltaTime;
 end;

 SIMDSetFlags(OldSIMDFlags);

 if OldFPUExceptionMask<>PhysicsFPUExceptionMask then begin
  SetExceptionMask(OldFPUExceptionMask);
 end;

 if OldFPUPrecisionMode<>PhysicsFPUPrecisionMode then begin
  SetPrecisionMode(OldFPUPrecisionMode);
 end;

 fTotalTime:=fHighResolutionTimer.GetTime-fTotalTime;

end;

function TKraft.TestPoint(const Point:TKraftVector3):TKraftShape;
var Hit:TKraftShape;
{$ifdef KraftSingleThreadedUsage}
 procedure QueryTree(AABBTree:TKraftDynamicAABBTree);
 var LocalStack:PKraftDynamicAABBTreeLongintArray;
     LocalStackPointer,NodeID:longint;
     Node:PKraftDynamicAABBTreeNode;
     CurrentShape:TKraftShape;
 begin
  if assigned(AABBTree) then begin
   if AABBTree.fRoot>=0 then begin
    LocalStack:=AABBTree.fStack;
    LocalStack^[0]:=AABBTree.fRoot;
    LocalStackPointer:=1;
    while LocalStackPointer>0 do begin
     dec(LocalStackPointer);
     NodeID:=LocalStack^[LocalStackPointer];
     if NodeID>=0 then begin
      Node:=@AABBTree.fNodes[NodeID];
      if AABBContains(Node^.AABB,Point) then begin
       if Node^.Children[0]<0 then begin
        CurrentShape:=Node^.UserData;
        if assigned(CurrentShape) and CurrentShape.TestPoint(Point) then begin
         Hit:=CurrentShape;
         exit;
        end;
       end else begin
        if AABBTree.fStackCapacity<=(LocalStackPointer+2) then begin
         AABBTree.fStackCapacity:=RoundUpToPowerOfTwo(LocalStackPointer+2);
         ReallocMem(AABBTree.fStack,AABBTree.fStackCapacity*SizeOf(longint));
         LocalStack:=AABBTree.fStack;
        end;
        LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
        LocalStack^[LocalStackPointer+1]:=Node^.Children[1];
        inc(LocalStackPointer,2);
       end;
      end;
     end;
    end;
   end;
  end;
 end;
{$else}
 procedure QueryTree(AABBTree:TKraftDynamicAABBTree);
  procedure ProcessNode(NodeID:longint);
  var Node:PKraftDynamicAABBTreeNode;
      CurrentShape:TKraftShape;
  begin
   if (NodeID>=0) and not assigned(Hit) then begin
    Node:=@AABBTree.fNodes[NodeID];
    if AABBContains(Node^.AABB,Point) then begin
     if Node^.Children[0]<0 then begin
      CurrentShape:=Node^.UserData;
      if assigned(CurrentShape) and CurrentShape.TestPoint(Point) then begin
       Hit:=CurrentShape;
       exit;
      end;
     end else begin
      ProcessNode(Node^.Children[0]);
      ProcessNode(Node^.Children[1]);
     end;
    end;
   end;
  end;
 begin
  ProcessNode(AABBTree.fRoot);
 end;
{$endif}
begin
 Hit:=nil;
 QueryTree(fStaticAABBTree);
 if not assigned(Hit) then begin
  QueryTree(fSleepingAABBTree);
 end;
 if not assigned(Hit) then begin
  QueryTree(fDynamicAABBTree);
 end;
 if not assigned(Hit) then begin
  QueryTree(fKinematicAABBTree);
 end;
 result:=Hit;
end;

function TKraft.RayCast(const Origin,Direction:TKraftVector3;const MaxTime:TKraftScalar;var Shape:TKraftShape;var Time:TKraftScalar;var Point,Normal:TKraftVector3;const CollisionGroups:TKraftRigidBodyCollisionGroups=[low(TKraftRigidBodyCollisionGroup)..high(TKraftRigidBodyCollisionGroup)]):boolean;
var Hit:boolean;
{$ifdef KraftSingleThreadedUsage}
 procedure QueryTree(AABBTree:TKraftDynamicAABBTree);
 var LocalStack:PKraftDynamicAABBTreeLongintArray;
     LocalStackPointer,NodeID:longint;
     Node:PKraftDynamicAABBTreeNode;
     CurrentShape:TKraftShape;
     RayCastData:TKraftRaycastData;
 begin
  if assigned(AABBTree) then begin
   if AABBTree.fRoot>=0 then begin
    LocalStack:=AABBTree.fStack;
    LocalStack^[0]:=AABBTree.fRoot;
    LocalStackPointer:=1;
    while LocalStackPointer>0 do begin
     dec(LocalStackPointer);
     NodeID:=LocalStack^[LocalStackPointer];
     if NodeID>=0 then begin
      Node:=@AABBTree.fNodes[NodeID];
      if AABBRayIntersect(Node^.AABB,Origin,Direction) then begin
       if Node^.Children[0]<0 then begin
        CurrentShape:=Node^.UserData;
        RayCastData.Origin:=Origin;
        RayCastData.Direction:=Direction;
        RayCastData.MaxTime:=MaxTime;
        if (assigned(CurrentShape) and (assigned(CurrentShape.fRigidBody) and ((CurrentShape.fRigidBody.fCollisionGroups*CollisionGroups)<>[]))) and CurrentShape.RayCast(RayCastData) then begin
         if (Hit and (RayCastData.TimeOfImpact<Time)) or not Hit then begin
          Hit:=true;
          Time:=RayCastData.TimeOfImpact;
          Point:=RayCastData.Point;
          Normal:=RayCastData.Normal;
          Shape:=CurrentShape;
         end;
        end;
       end else begin
        if AABBTree.fStackCapacity<=(LocalStackPointer+2) then begin
         AABBTree.fStackCapacity:=RoundUpToPowerOfTwo(LocalStackPointer+2);
         ReallocMem(AABBTree.fStack,AABBTree.fStackCapacity*SizeOf(longint));
         LocalStack:=AABBTree.fStack;
        end;
        LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
        LocalStack^[LocalStackPointer+1]:=Node^.Children[1];
        inc(LocalStackPointer,2);
       end;
      end;
     end;
    end;
   end;
  end;
 end;
{$else}
 procedure QueryTree(AABBTree:TKraftDynamicAABBTree);
  procedure ProcessNode(NodeID:longint);
  var Node:PKraftDynamicAABBTreeNode;
      CurrentShape:TKraftShape;
      RayCastData:TKraftRaycastData;
  begin
   if NodeID>=0 then begin
    Node:=@AABBTree.fNodes[NodeID];
    if AABBRayIntersect(Node^.AABB,Origin,Direction) then begin
     if Node^.Children[0]<0 then begin
      CurrentShape:=Node^.UserData;
      RayCastData.Origin:=Origin;
      RayCastData.Direction:=Direction;
      RayCastData.MaxTime:=MaxTime;
      if (assigned(CurrentShape) and (assigned(CurrentShape.fRigidBody) and ((CurrentShape.fRigidBody.fCollisionGroups*CollisionGroups)<>[]))) and CurrentShape.RayCast(RayCastData) then begin
       if (Hit and (RayCastData.TimeOfImpact<Time)) or not Hit then begin
        Hit:=true;
        Time:=RayCastData.TimeOfImpact;
        Point:=RayCastData.Point;
        Normal:=RayCastData.Normal;
        Shape:=CurrentShape;
       end;
      end;
     end else begin
      ProcessNode(Node^.Children[0]);
      ProcessNode(Node^.Children[1]);
     end;
    end;
   end;
  end;
 begin
  ProcessNode(AABBTree.fRoot);
 end;
{$endif}
begin
 Hit:=false;
 Time:=MaxTime;
 QueryTree(fStaticAABBTree);
 QueryTree(fSleepingAABBTree);
 QueryTree(fDynamicAABBTree);
 QueryTree(fKinematicAABBTree);
 result:=Hit;
end;

function TKraft.PushSphere(var Center:TKraftVector3;const Radius:TKraftScalar;const CollisionGroups:TKraftRigidBodyCollisionGroups=[low(TKraftRigidBodyCollisionGroup)..high(TKraftRigidBodyCollisionGroup)];const TryIterations:longint=4):boolean;
var Hit:boolean;
    AABB:TKraftAABB;
    Sphere:TKraftSphere;
    SumMinimumTranslationVector:TKraftVector3;
    Count:longint;
 procedure CollideSphereWithSphere(Shape:TKraftShapeSphere); {$ifdef caninline}inline;{$endif}
 var Position,Normal:TKraftVector3;
     Depth:TKraftScalar;
 begin
  Position:=Vector3Sub(Sphere.Center,Vector3TermMatrixMul(Shape.fLocalCenterOfMass,Shape.fWorldTransform));
  if Vector3Length(Position)<(Sphere.Radius+Shape.fRadius) then begin
   Normal:=Vector3SafeNorm(Position);
   Depth:=(Sphere.Radius+Shape.fRadius)-Vector3Length(Position);
   SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Normal,Depth));
   inc(Count);
   Hit:=true;
  end;
 end;
 procedure CollideSphereWithCapsule(Shape:TKraftShapeCapsule); {$ifdef caninline}inline;{$endif}
 var Alpha,HalfLength,r1,r2,d,d1:TKraftScalar;
     Center,Position,Normal,GeometryDirection:TKraftVector3;
 begin
  r1:=Shape.fRadius;
  r2:=Sphere.Radius;
  GeometryDirection:=Vector3(Shape.fWorldTransform[1,0],Shape.fWorldTransform[1,1],Shape.fWorldTransform[1,2]);
  Center:=Vector3TermMatrixMul(Shape.fLocalCenterOfMass,Shape.fWorldTransform);
  Alpha:=(GeometryDirection.x*(Sphere.Center.x-Center.x))+
         (GeometryDirection.y*(Sphere.Center.y-Center.y))+
         (GeometryDirection.z*(Sphere.Center.z-Center.z));
  HalfLength:=Shape.fHeight*0.5;
  if Alpha>HalfLength then begin
   Alpha:=HalfLength;
  end else if alpha<-HalfLength then begin
   Alpha:=-HalfLength;
  end;
  Position:=Vector3Add(Center,Vector3ScalarMul(GeometryDirection,Alpha));
  d:=Vector3Dist(Position,Sphere.Center);
  if d<=(r1+r2) then begin
   if d<=EPSILON then begin
    SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Vector3XAxis,r1+r2));
    inc(Count);
    Hit:=true;
   end else begin
    d1:=1.0/d;
    Normal:=Vector3Neg(Vector3ScalarMul(Vector3Sub(Position,Sphere.Center),d1));
    SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Normal,(r1+r2)-d));
    inc(Count);
    Hit:=true;
   end;
  end;
 end;
 procedure CollideSphereWithConvexHull(Shape:TKraftShapeConvexHull); {$ifdef caninline}inline;{$endif}
 var FaceIndex,ClosestFaceIndex,VertexIndex:longint;
     Distance,ClosestDistance,BestClosestPointDistance,d:TKraftScalar;
     SphereCenter,Normal,ClosestPoint,{BestClosestPoint,}BestClosestPointNormal,ab,ap,a,b,v,n:TKraftVector3;
     InsideSphere,InsidePolygon,HasBestClosestPoint:boolean;
     Face:PKraftConvexHullFace;
 begin
  BestClosestPointDistance:=MAX_SCALAR;
  BestClosestPointNormal:=Vector3Origin;
  HasBestClosestPoint:=false;
  ClosestPoint:=Vector3Origin;
  ClosestDistance:=MAX_SCALAR;
  ClosestFaceIndex:=-1;
  InsideSphere:=true;
  SphereCenter:=Vector3TermMatrixMulInverted(Sphere.Center,Shape.fWorldTransform);
  for FaceIndex:=0 to Shape.fConvexHull.fCountFaces-1 do begin
   Face:=@Shape.fConvexHull.fFaces[FaceIndex];
   Distance:=PlaneVectorDistance(Face^.Plane,SphereCenter);
   if Distance>0.0 then begin
    // sphere center is not inside in the convex hull . . .
    if Distance<Sphere.Radius then begin
     // but touching . . .
     if Face^.CountVertices>0 then begin
      InsidePolygon:=true;
      n:=Face^.Plane.Normal;
      b:=Shape.fConvexHull.fVertices[Face^.Vertices[Face^.CountVertices-1]].Position;
      for VertexIndex:=0 to Face^.CountVertices-1 do begin
       a:=b;
       b:=Shape.fConvexHull.fVertices[Face^.Vertices[VertexIndex]].Position;
       ab:=Vector3Sub(b,a);
       ap:=Vector3Sub(SphereCenter,a);
       v:=Vector3Cross(ab,n);
       if Vector3Dot(ap,v)>0.0 then begin
        d:=Vector3LengthSquared(ab);
        if d<>0.0 then begin
         d:=Vector3Dot(ab,ap)/d;
        end else begin
         d:=0.0;
        end;
        ClosestPoint:=Vector3Lerp(a,b,d);
        InsidePolygon:=false;
        break;
       end;
      end;
      if InsidePolygon then begin
       // sphere is directly touching the convex hull . . .
       Normal:=Vector3SafeNorm(Vector3TermMatrixMulBasis(Shape.fConvexHull.fFaces[FaceIndex].Plane.Normal,Shape.fWorldTransform));
       SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Normal,Sphere.Radius-Distance));
       inc(Count);
       Hit:=true;
       exit;
      end else begin
       // the sphere may not be directly touching the polyhedron, but it may be touching a point or an edge, if the distance between
       // the closest point on the poly and the center of the sphere is less than the sphere radius we have a hit.
       Normal:=Vector3Sub(SphereCenter,ClosestPoint);
       if Vector3LengthSquared(Normal)<sqr(Sphere.Radius) then begin
        Normal:=Vector3TermMatrixMulBasis(Normal,Shape.fWorldTransform);
        Distance:=Vector3LengthNormalize(Normal);
        if (not HasBestClosestPoint) or (BestClosestPointDistance>Distance) then begin
         HasBestClosestPoint:=true;
         BestClosestPointDistance:=Distance;
         //BestClosestPoint:=ClosestPoint;
         BestClosestPointNormal:=Normal;
        end;
       end;
      end;
     end;
    end;
    InsideSphere:=false;
   end else if InsideSphere and ((ClosestFaceIndex<0) or (ClosestDistance>abs(Distance))) then begin
    ClosestDistance:=abs(Distance);
    ClosestFaceIndex:=FaceIndex;
   end;
  end;
  if InsideSphere and (ClosestFaceIndex>=0) then begin
   // the sphere center is inside the convex hull . . .
   Normal:=Vector3SafeNorm(Vector3TermMatrixMulBasis(Shape.fConvexHull.fFaces[ClosestFaceIndex].Plane.Normal,Shape.fWorldTransform));
   SumMinimumTranslationVector:=Vector3Sub(SumMinimumTranslationVector,Vector3ScalarMul(Normal,ClosestDistance-Sphere.Radius));
   inc(Count);
   Hit:=true;
  end else if HasBestClosestPoint then begin
   SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Vector3SafeNorm(BestClosestPointNormal),Sphere.Radius-BestClosestPointDistance));
   inc(Count);
   Hit:=true;
  end;
 end;
 procedure CollideSphereWithBox(Shape:TKraftShapeBox); {$ifdef caninline}inline;{$endif}
 //const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
 var IntersectionDist,ContactDist,DistSqr,Distance,FaceDist,MinDist:TKraftScalar;
     SphereRelativePosition,ClosestPoint,Normal:TKraftVector3;
     Axis,AxisSign:longint;
 begin
  SphereRelativePosition:=Vector3TermMatrixMulInverted(Sphere.Center,Shape.fWorldTransform);
  ClosestPoint.x:=Min(Max(SphereRelativePosition.x,-Shape.fExtents.x),Shape.fExtents.x);
  ClosestPoint.y:=Min(Max(SphereRelativePosition.y,-Shape.fExtents.y),Shape.fExtents.y);
  ClosestPoint.z:=Min(Max(SphereRelativePosition.z,-Shape.fExtents.z),Shape.fExtents.z);
  Normal:=Vector3Sub(SphereRelativePosition,ClosestPoint);
  DistSqr:=Vector3LengthSquared(Normal);
  IntersectionDist:=Sphere.Radius;
  ContactDist:=IntersectionDist+EPSILON;
  if DistSqr<=sqr(ContactDist) then begin
   if DistSqr<=EPSILON then begin
    begin
     FaceDist:=Shape.fExtents.x-SphereRelativePosition.x;
     MinDist:=FaceDist;
     Axis:=0;
     AxisSign:=1;
    end;
    begin
     FaceDist:=Shape.fExtents.x+SphereRelativePosition.x;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=0;
      AxisSign:=-1;
     end;
    end;
    begin
     FaceDist:=Shape.fExtents.y-SphereRelativePosition.y;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=1;
      AxisSign:=1;
     end;
    end;
    begin
     FaceDist:=Shape.fExtents.y+SphereRelativePosition.y;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=1;
      AxisSign:=-1;
     end;
    end;
    begin
     FaceDist:=Shape.fExtents.z-SphereRelativePosition.z;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=2;
      AxisSign:=1;
     end;
    end;
    begin
     FaceDist:=Shape.fExtents.z+SphereRelativePosition.z;
     if FaceDist<MinDist then begin
      MinDist:=FaceDist;
      Axis:=2;
      AxisSign:=-1;
     end;
    end;
    ClosestPoint:=SphereRelativePosition;
    ClosestPoint.xyz[Axis]:=Shape.fExtents.xyz[Axis]*AxisSign;
    Normal:=Vector3Origin;
    Normal.xyz[Axis]:=AxisSign;
    Distance:=-MinDist;
   end else begin
    Distance:=Vector3LengthNormalize(Normal);
   end;
   SumMinimumTranslationVector:=Vector3Sub(SumMinimumTranslationVector,Vector3ScalarMul(Vector3SafeNorm(Vector3TermMatrixMulBasis(Normal,Shape.fWorldTransform)),Distance-IntersectionDist));
   inc(Count);
   Hit:=true;
  end;
 end;
 procedure CollideSphereWithPlane(Shape:TKraftShapePlane); {$ifdef caninline}inline;{$endif}
 var Distance:TKraftScalar;
     SphereCenter:TKraftVector3;
 begin
  SphereCenter:=Vector3TermMatrixMulInverted(Sphere.Center,Shape.fWorldTransform);
  Distance:=PlaneVectorDistance(Shape.fPlane,SphereCenter);
  if Distance<=Sphere.Radius then begin
   SumMinimumTranslationVector:=Vector3Sub(SumMinimumTranslationVector,Vector3ScalarMul(Vector3SafeNorm(Vector3TermMatrixMulBasis(Shape.fPlane.Normal,Shape.fWorldTransform)),Distance-Sphere.Radius));
   inc(Count);
   Hit:=true;
  end;
 end;
 procedure CollideSphereWithTriangle(Shape:TKraftShapeTriangle);
 const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
 var i:longint;
     Radius,RadiusWithThreshold,DistanceFromPlane,ContactRadiusSqr,DistanceSqr:TKraftScalar;
     SphereCenter,Normal,P0ToCenter,ContactPoint,NearestOnEdge,ContactToCenter:TKraftVector3;
     IsInsideContactPlane,HasContact:boolean;
     v:array[0..2] of PKraftVector3;
 begin
  v[0]:=@Shape.fConvexHull.fVertices[0].Position;
  v[1]:=@Shape.fConvexHull.fVertices[1].Position;
  v[2]:=@Shape.fConvexHull.fVertices[2].Position;
  SphereCenter:=Vector3TermMatrixMulInverted(Sphere.Center,Shape.fWorldTransform);
  Radius:=Sphere.Radius;
  RadiusWithThreshold:=Radius+EPSILON;
  Normal:=Shape.fConvexHull.fFaces[0].Plane.Normal;// Vector3SafeNorm(Vector3Cross(Vector3Sub(v[1]^,v[0]^),Vector3Sub(v[2]^,v[0]^)));
  P0ToCenter:=Vector3Sub(SphereCenter,v[0]^);
  DistanceFromPlane:=Vector3Dot(P0ToCenter,Normal);
  if DistanceFromPlane<0.0 then begin
   DistanceFromPlane:=-DistanceFromPlane;
   Normal:=Vector3Neg(Normal);
  end;
  IsInsideContactPlane:=DistanceFromPlane<RadiusWithThreshold;
  HasContact:=false;
  ContactPoint:=Vector3Origin;
  ContactRadiusSqr:=sqr(RadiusWithThreshold);
  if IsInsideContactPlane then begin
   if PointInTriangle(v[0]^,v[1]^,v[2]^,Normal,SphereCenter) then begin
    HasContact:=true;
    ContactPoint:=Vector3Sub(SphereCenter,Vector3ScalarMul(Normal,DistanceFromPlane));
   end else begin
    for i:=0 to 2 do begin
     DistanceSqr:=SegmentSqrDistance(v[i]^,v[ModuloThree[i+1]]^,SphereCenter,NearestOnEdge);
     if DistanceSqr<ContactRadiusSqr then begin
      HasContact:=true;
      ContactPoint:=NearestOnEdge;
     end;
    end;
   end;
  end;
  if HasContact then begin
   ContactToCenter:=Vector3Sub(SphereCenter,ContactPoint);
   DistanceSqr:=Vector3LengthSquared(ContactToCenter);
   if DistanceSqr<ContactRadiusSqr then begin
    if DistanceSqr>EPSILON then begin
     SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Vector3SafeNorm(Vector3TermMatrixMulBasis(ContactToCenter,Shape.fWorldTransform)),Radius-sqrt(DistanceSqr)));
     inc(Count);
     Hit:=true;
    end else begin
     SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Vector3SafeNorm(Vector3TermMatrixMulBasis(Normal,Shape.fWorldTransform)),Radius));
     inc(Count);
     Hit:=true;
    end;
   end;
  end;
 end;
 procedure CollideSphereWithMesh(Shape:TKraftShapeMesh); {$ifdef caninline}inline;{$endif}
 const ModuloThree:array[0..5] of longint=(0,1,2,0,1,2);
 var i,SkipListNodeIndex,TriangleIndex:longint;
     Radius,RadiusWithThreshold,DistanceFromPlane,ContactRadiusSqr,DistanceSqr:TKraftScalar;
     SphereCenter,Normal,P0ToCenter,ContactPoint,NearestOnEdge,ContactToCenter:TKraftVector3;
     IsInsideContactPlane,HasContact:boolean;
     SkipListNode:PKraftMeshSkipListNode;
     Triangle:PKraftMeshTriangle;
     AABB:TKraftAABB;
     Vertices:array[0..2] of PKraftVector3;
 begin
  SphereCenter:=Vector3TermMatrixMulInverted(Sphere.Center,Shape.fWorldTransform);
  Radius:=Sphere.Radius;
  RadiusWithThreshold:=Radius+0.1;
  AABB.Min.x:=SphereCenter.x-RadiusWithThreshold;
  AABB.Min.y:=SphereCenter.y-RadiusWithThreshold;
  AABB.Min.z:=SphereCenter.z-RadiusWithThreshold;
  AABB.Max.x:=SphereCenter.x+RadiusWithThreshold;
  AABB.Max.y:=SphereCenter.y+RadiusWithThreshold;
  AABB.Max.z:=SphereCenter.z+RadiusWithThreshold;
  RadiusWithThreshold:=Radius+EPSILON;
  SkipListNodeIndex:=0;
  while SkipListNodeIndex<Shape.fMesh.fCountSkipListNodes do begin
   SkipListNode:=@Shape.fMesh.fSkipListNodes[SkipListNodeIndex];
   if AABBIntersect(SkipListNode^.AABB,AABB) then begin
    TriangleIndex:=SkipListNode^.TriangleIndex;
    while TriangleIndex>=0 do begin
     Triangle:=@Shape.fMesh.fTriangles[TriangleIndex];
     Vertices[0]:=@Shape.fMesh.fVertices[Triangle^.Vertices[0]];
     Vertices[1]:=@Shape.fMesh.fVertices[Triangle^.Vertices[1]];
     Vertices[2]:=@Shape.fMesh.fVertices[Triangle^.Vertices[2]];
     Normal:=Vector3SafeNorm(Vector3Cross(Vector3Sub(Vertices[1]^,Vertices[0]^),Vector3Sub(Vertices[2]^,Vertices[0]^)));
     P0ToCenter:=Vector3Sub(SphereCenter,Vertices[0]^);
     DistanceFromPlane:=Vector3Dot(P0ToCenter,Normal);
     if DistanceFromPlane<0.0 then begin
      DistanceFromPlane:=-DistanceFromPlane;
      Normal:=Vector3Neg(Normal);
     end;
     IsInsideContactPlane:=DistanceFromPlane<RadiusWithThreshold;
     HasContact:=false;
     ContactPoint:=Vector3Origin;
     ContactRadiusSqr:=sqr(RadiusWithThreshold);
     if IsInsideContactPlane then begin
      if PointInTriangle(Vertices[0]^,Vertices[1]^,Vertices[2]^,Normal,SphereCenter) then begin
       HasContact:=true;
       ContactPoint:=Vector3Sub(SphereCenter,Vector3ScalarMul(Normal,DistanceFromPlane));
      end else begin
       for i:=0 to 2 do begin
        DistanceSqr:=SegmentSqrDistance(Vertices[i]^,Vertices[ModuloThree[i+1]]^,SphereCenter,NearestOnEdge);
        if DistanceSqr<ContactRadiusSqr then begin
         HasContact:=true;
         ContactPoint:=NearestOnEdge;
        end;
       end;
      end;
     end;
     if HasContact then begin
      ContactToCenter:=Vector3Sub(SphereCenter,ContactPoint);
      DistanceSqr:=Vector3LengthSquared(ContactToCenter);
      if DistanceSqr<ContactRadiusSqr then begin
       if DistanceSqr>EPSILON then begin
        SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Vector3SafeNorm(Vector3TermMatrixMulBasis(ContactToCenter,Shape.fWorldTransform)),Radius-sqrt(DistanceSqr)));
        inc(Count);
        Hit:=true;
       end else begin
        SumMinimumTranslationVector:=Vector3Add(SumMinimumTranslationVector,Vector3ScalarMul(Vector3SafeNorm(Vector3TermMatrixMulBasis(Normal,Shape.fWorldTransform)),Radius));
        inc(Count);
        Hit:=true;
       end;
      end;
     end;
     TriangleIndex:=Triangle^.Next;
    end;
    inc(SkipListNodeIndex);
   end else begin
    SkipListNodeIndex:=SkipListNode^.SkipToNodeIndex;
   end;
  end;
 end;
{$ifdef KraftSingleThreadedUsage}
 procedure QueryTree(AABBTree:TKraftDynamicAABBTree);
 var LocalStack:PKraftDynamicAABBTreeLongintArray;
     LocalStackPointer,NodeID:longint;
     Node:PKraftDynamicAABBTreeNode;
     CurrentShape:TKraftShape;
 begin
  if assigned(AABBTree) then begin
   if AABBTree.fRoot>=0 then begin
    LocalStack:=AABBTree.fStack;
    LocalStack^[0]:=AABBTree.fRoot;
    LocalStackPointer:=1;
    while LocalStackPointer>0 do begin
     dec(LocalStackPointer);
     NodeID:=LocalStack^[LocalStackPointer];
     if NodeID>=0 then begin
      Node:=@AABBTree.fNodes[NodeID];
      if AABBIntersect(Node^.AABB,AABB) then begin
       if Node^.Children[0]<0 then begin
        CurrentShape:=Node^.UserData;
        if assigned(CurrentShape) and (assigned(CurrentShape.fRigidBody) and ((CurrentShape.fRigidBody.fCollisionGroups*CollisionGroups)<>[])) then begin
         case CurrentShape.fShapeType of
          kstSphere:begin
           CollideSphereWithSphere(TKraftShapeSphere(CurrentShape));
          end;
          kstCapsule:begin
           CollideSphereWithCapsule(TKraftShapeCapsule(CurrentShape));
          end;
          kstConvexHull:begin
           CollideSphereWithConvexHull(TKraftShapeConvexHull(CurrentShape));
          end;
          kstBox:begin
           CollideSphereWithBox(TKraftShapeBox(CurrentShape));
          end;
          kstPlane:begin
           CollideSphereWithPlane(TKraftShapePlane(CurrentShape));
          end;
          kstTriangle:begin
           CollideSphereWithTriangle(TKraftShapeTriangle(CurrentShape));
          end;
          kstMesh:begin
           CollideSphereWithMesh(TKraftShapeMesh(CurrentShape));
          end;
         end;
        end;
       end else begin
        if AABBTree.fStackCapacity<=(LocalStackPointer+2) then begin
         AABBTree.fStackCapacity:=RoundUpToPowerOfTwo(LocalStackPointer+2);
         ReallocMem(AABBTree.fStack,AABBTree.fStackCapacity*SizeOf(longint));
         LocalStack:=AABBTree.fStack;
        end;
        LocalStack^[LocalStackPointer+0]:=Node^.Children[0];
        LocalStack^[LocalStackPointer+1]:=Node^.Children[1];
        inc(LocalStackPointer,2);
       end;
      end;
     end;
    end;
   end;
  end;
 end;
{$else}
 procedure QueryTree(AABBTree:TKraftDynamicAABBTree);
  procedure ProcessNode(NodeID:longint);
  var Node:PKraftDynamicAABBTreeNode;
      CurrentShape:TKraftShape;
  begin
   if NodeID>=0 then begin
    Node:=@AABBTree.fNodes[NodeID];
    if AABBIntersect(Node^.AABB,AABB) then begin
     if Node^.Children[0]<0 then begin
      CurrentShape:=Node^.UserData;
      if assigned(CurrentShape) and (assigned(CurrentShape.fRigidBody) and ((CurrentShape.fRigidBody.fCollisionGroups*CollisionGroups)<>[])) then begin
       case CurrentShape.fShapeType of
        kstSphere:begin
         CollideSphereWithSphere(TKraftShapeSphere(CurrentShape));
        end;
        kstCapsule:begin
         CollideSphereWithCapsule(TKraftShapeCapsule(CurrentShape));
        end;
        kstConvexHull:begin
         CollideSphereWithConvexHull(TKraftShapeConvexHull(CurrentShape));
        end;
        kstBox:begin
         CollideSphereWithBox(TKraftShapeBox(CurrentShape));
        end;
        kstPlane:begin
         CollideSphereWithPlane(TKraftShapePlane(CurrentShape));
        end;
        kstTriangle:begin
         CollideSphereWithTriangle(TKraftShapeTriangle(CurrentShape));
        end;
        kstMesh:begin
         CollideSphereWithMesh(TKraftShapeMesh(CurrentShape));
        end;
       end;
      end;
     end else begin
      ProcessNode(Node^.Children[0]);
      ProcessNode(Node^.Children[1]);
     end;
    end;
   end;
  end;
 begin
  ProcessNode(AABBTree.fRoot);
 end;
{$endif}
var TryCounter:longint;
begin
 result:=false;
 for TryCounter:=1 to TryIterations do begin
  Hit:=false;
  AABB.Min.x:=Center.x-Radius;
  AABB.Min.y:=Center.y-Radius;
  AABB.Min.z:=Center.z-Radius;
  AABB.Max.x:=Center.x+Radius;
  AABB.Max.y:=Center.y+Radius;
  AABB.Max.z:=Center.z+Radius;
  Sphere.Center:=Center;
  Sphere.Radius:=Radius;
  SumMinimumTranslationVector:=Vector3Origin;
  Count:=0;
  QueryTree(fStaticAABBTree);
  QueryTree(fSleepingAABBTree);
  QueryTree(fDynamicAABBTree);
  QueryTree(fKinematicAABBTree);
  result:=result or Hit;
  if (Count>0) and not IsZero(Vector3LengthSquared(SumMinimumTranslationVector)) then begin
   Center.x:=Center.x+(SumMinimumTranslationVector.x/Count);
   Center.y:=Center.y+(SumMinimumTranslationVector.y/Count);
   Center.z:=Center.z+(SumMinimumTranslationVector.z/Count);
  end else begin
   break;
  end;
 end;
end;

function TKraft.GetDistance(const ShapeA,ShapeB:TKraftShape):TKraftScalar;
var GJK:TKraftGJK;
begin
 GJK.CachedSimplex:=nil;
 GJK.Simplex.Count:=0;
 GJK.Shapes[0]:=ShapeA;
 GJK.Shapes[1]:=ShapeB;
 GJK.Transforms[0]:=@ShapeA.fWorldTransform;
 GJK.Transforms[1]:=@ShapeB.fWorldTransform;
 GJK.UseRadii:=false;
 GJK.Run;
 result:=GJK.Distance;
end;

initialization
 CheckCPU;
end.
