开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 电子书籍 > Delphi/CppBuilder > 查看源码
UPhysics2DTypes.pas
资源名称：box2DDelphiPackage.zip　[点击查看]
上传用户：zkjn0718
上传日期：2021-01-01
资源大小：776k
文件大小：39k
源码类别：

Delphi/CppBuilder

开发平台：

Delphi

UPhysics2DTypes.pas：源码内容
  1. unit UPhysics2DTypes;
  2. { This unit is written based on Box2D whose author is Erin Catto (http://www.gphysics.com)
  3.   All type names follow the Delphi custom Txxx and xxx means the corresponding
  4.   type in cpp source.
  6.   Because versions before Delphi 2007 don't support operator overloading, so
  7.   I write two versions of all math operations for vector and matrix, etc. But
  8.   later I found that the version without operator overloading runs faster.
  9.   So if you want a better performance, DEFINE BETTER_PERFORMANCE in Physics2D.inc
  10.   which will UNDEFINE OP_OVERLOAD even if you are using Delphi 2010.
  12.   This library supports three kinds of floats, Single(32bit), Double(64bit) and
  13.   Extended(80bit).
  14.        flags        EXTENDED_PRECISION        DOUBLE_PRECISION
  15.      Extended               ON                     whatever
  16.       Double(default)       OFF                       ON
  17.       Single                OFF                       OFF
  18.   There is also a flag SINGLE_PRECISION in the include file but it doesn't affect
  19.   Float type definition.
  21.   All assertions are ignored.
  23.   Translator:  Qianyuan Wang(王乾元)
  24.   Contact me: http://hi.baidu.com/wqyfavor
  25.                       wqyfavor@163.com
  26.                       QQ: 466798985   }
  28. interface
  29. {$I Physics2D.inc}
  31. uses
  32.    Math, Classes, SysUtils,
  33.    {$IFDEF HANDLE_EXCEPTION}
  34.    UPhysics2DMessages,
  35.    {$ENDIF}
  36.    Dialogs;
  38. type
  39.    Int8   = ShortInt;
  40.    Int16  = SmallInt;
  41.    Int32  = Integer;
  42.    UInt8  = Byte;
  43.    UInt16 = Word;
  44.    UInt32 = Cardinal;
  45.    PUInt16 = ^UInt16;
  47.    // If handle exception and give localizable messages. Messages are stored in UPhysics2DMessages.pas
  48.    {$IFDEF HANDLE_EXCEPTION}
  49.    TPhysics2DExceptionCode = (ecDivByZero);
  50.    TPhysics2DException = class(Exception)
  51.       constructor Create(ErrorCode: TPhysics2DExceptionCode);
  52.    end;
  53.    {$ENDIF}
  55.    // Float type
  56.    PFloat = ^Float;
  57.    {$IFDEF EXTENDED_PRECISION}
  58.    Float = Extended;
  59.    {$ELSE}
  60.       {$IFDEF DOUBLE_PRECISION}
  61.       Float = Double;
  62.       {$ELSE}
  63.       Float = Single;
  64.       {$ENDIF}
  65.    {$ENDIF}
  66.    Float32 = Single;
  67.    Float64 = Double;
  68.    Float80 = Extended;
  70. const
  71.    {$IFDEF EXTENDED_PRECISION}
  72.    FLT_EPSILON = 1.084202172485504E-19;
  73.    FLT_MAX = MaxExtended;
  74.    {$ELSE}
  75.       {$IFDEF DOUBLE_PRECISION}
  76.       FLT_EPSILON = 2.2204460492503131e-16;
  77.       FLT_MAX = MaxDouble;
  78.       {$ELSE}
  79.       FLT_EPSILON = 1.192092896e-7;
  80.       FLT_MAX = MaxSingle;
  81.       {$ENDIF}
  82.    {$ENDIF}
  84.    function IsValid(f: Float): Boolean; {$IFNDEF OP_OVERLOAD}overload;{$ENDIF}
  86. type
  87.    PVector2 = ^TVector2;
  88.    TVector2 = record
  89.       x, y: Float;
  91.       {$IFDEF OP_OVERLOAD}
  92.       function IsValid: Boolean; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  94.       function Length: Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  95.       function SqrLength: Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  96.       function Normalize: Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  97.       procedure SetZero; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  98.       procedure SetNegative; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  99.       procedure SetValue(x, y: Float); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  101.       class function From(const x, y: Float): TVector2; static; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  102.       // Operators
  103.       class operator Negative(const AValue: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  104.       class operator Add(const Left, Right: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  105.       class operator Subtract(const Left, Right: TVector2): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  107.       class operator Multiply(const Left: TVector2; const Right: Single): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  108.       class operator Multiply(const Left: TVector2; const Right: Double): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  109.       class operator Multiply(const Left: TVector2; const Right: Extended): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  110.       class operator Multiply(const Left: Single; const Right: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  111.       class operator Multiply(const Left: Double; const Right: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  112.       class operator Multiply(const Left: Extended; const Right: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  114.       class operator Divide(const Left: TVector2; const Right: Single): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  115.       class operator Divide(const Left: TVector2; const Right: Double): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  116.       class operator Divide(const Left: TVector2; const Right: Extended): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  118.       procedure AddBy(const Operand: TVector2); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  119.       procedure SubtractBy(const Operand: TVector2); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  120.       procedure MultiplyBy(const Operand: Single); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  121.       procedure MultiplyBy(const Operand: Double); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  122.       procedure MultiplyBy(const Operand: Extended); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  123.       procedure DivideBy(const Operand: Single); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  124.       procedure DivideBy(const Operand: Double); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  125.       procedure DivideBy(const Operand: Extended); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  126.       {$ENDIF}
  127.    end;
  129.    TVectorArray4 = array[0..3] of TVector2;
  131.    TMatrix22 = record
  132.       col1, col2: TVector2;
  134.       {$IFDEF OP_OVERLOAD}
  135.       procedure SetIdentity;
  136.       procedure SetZero;
  137.       procedure SetValue(const _col1, _col2: TVector2); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  138.       /// Initialize this matrix using an angle. This matrix becomes
  139.     /// an orthonormal rotation matrix.
  140.       procedure SetValue(angle: Float); overload;
  142.       function Invert: TMatrix22;
  143.       function Solve(const b: TVector2): TVector2; // Solve A * x = b, where b is a column vector.
  145.       // Operators
  146.       class operator Negative(const AValue: TMatrix22): TMatrix22;
  147.       class operator Add(const Left, Right: TMatrix22): TMatrix22;
  148.       class operator Subtract(const Left, Right: TMatrix22): TMatrix22;
  149.       {$ENDIF}
  150.    end;
  152.    /// A transform contains translation and rotation. It is used to represent
  153.    /// the position and orientation of rigid bodies.
  154.    Tb2XForm = record
  155.       position: TVector2;
  156.       R: TMatrix22;
  158.       {$IFDEF OP_OVERLOAD}
  159.       procedure SetIdentity;
  160.       class function From(const position: TVector2; const R: TMatrix22): Tb2XForm; static;
  161.       {$ENDIF}
  162.    end;
  164.    /// This describes the motion of a body/shape for TOI computation.
  165.    /// Shapes are defined with respect to the body origin, which may
  166.    /// no coincide with the center of mass. However, to support dynamics
  167.    /// we must interpolate the center of mass position.
  168.    Tb2Sweep = record
  169.       localCenter: TVector2; // local center of mass position
  170.       c0, c: TVector2; // center world positions
  171.       a0, a: Float; // world angles
  172.       t0: Float; // time interval = [t0,1], where t0 is in [0,1]
  174.       {$IFDEF OP_OVERLOAD}
  175.       /// Get the interpolated transform at a specific time.
  176.      /// @param t the normalized time in [0,1].
  177.      procedure GetXForm(var xf: Tb2XForm; t: Float);
  179.     /// Advance the sweep forward, yielding a new initial state.
  180.      /// @param t the new initial time.
  181.       procedure Advance(t: Float);
  182.       {$ENDIF}
  183.    end;
  185. const
  186.    b2Vec2_Zero: TVector2 = (X: 0.0; Y: 0.0);
  187.    b2Mat22_identity: TMatrix22 = (col1: (X: 1.0; Y: 0.0); col2: (X: 0.0; Y: 1.0));
  188.    b2XForm_identity: Tb2XForm = (position: (X: 0.0; Y: 0.0); R: (col1: (X: 1.0; Y: 0.0); col2: (X: 0.0; Y: 1.0)));
  190. const
  191.    UInt8_MAX = $FF;
  192.    UINT16_MAX = $FFFF;
  194.    b2_maxManifoldPoints = 2;
  195.    b2_maxPolygonVertices = 8;
  196.    b2_maxProxies = 512; // this must be a power of two
  197.    b2_maxPairs = 8 * b2_maxProxies; // this must be a power of two
  199.    b2_nullFeature: UInt8 = UInt8_MAX;
  200.    b2_nullPair: UInt16 = UINT16_MAX;
  201.    b2_nullProxy: UInt16 = UINT16_MAX;
  202.    b2_tableCapacity = b2_maxPairs; // must be a power of two
  203.    b2_tableMask = b2_tableCapacity - 1; // should be (b2_tableCapacity - 1) but won't compile.
  205.    B2BROADPHASE_MAX = UINT16_MAX;
  206.    b2_invalid: UInt16 = B2BROADPHASE_MAX;
  207.    b2_nullEdge: UInt16 = B2BROADPHASE_MAX;
  209.    // Dynamics
  210.    /// A small length used as a collision and constraint tolerance. Usually it is
  211.    /// chosen to be numerically significant, but visually insignificant.
  212.    b2_linearSlop = 0.005; // 0.5 cm
  214.    /// A small angle used as a collision and constraint tolerance. Usually it is
  215.    /// chosen to be numerically significant, but visually insignificant.
  216.    b2_angularSlop = 2.0 / 180.0 * Pi; // 2 degrees
  218.    /// Continuous collision detection (CCD) works with core, shrunken shapes. This is the
  219.    /// amount by which shapes are automatically shrunk to work with CCD. This must be
  220.    /// larger than b2_linearSlop.
  221.    b2_toiSlop = 8.0 * b2_linearSlop;
  223.    /// Maximum number of contacts to be handled to solve a TOI island.
  224.    b2_maxTOIContactsPerIsland: Int32 = 32;
  226.    /// A velocity threshold for elastic collisions. Any collision with a relative linear
  227.    /// velocity below this threshold will be treated as inelastic.
  228.    b2_velocityThreshold = 1.0; // 1 m/s
  230.    /// The maximum linear position correction used when solving constraints. This helps to
  231.    /// prevent overshoot.
  232.    b2_maxLinearCorrection = 0.2; // 20 cm
  234.    /// The maximum angular position correction used when solving constraints. This helps to
  235.    /// prevent overshoot.
  236.    b2_maxAngularCorrection = 8.0 / 180.0 * Pi; // 8 degrees
  238.    /// The maximum linear velocity of a body. This limit is very large and is used
  239.    /// to prevent numerical problems. You shouldn't need to adjust this.
  240.    b2_maxLinearVelocity = 200.0;
  241.    b2_maxLinearVelocitySquared = b2_maxLinearVelocity * b2_maxLinearVelocity;
  243.    /// The maximum angular velocity of a body. This limit is very large and is used
  244.    /// to prevent numerical problems. You shouldn't need to adjust this.
  245.    b2_maxAngularVelocity = 250.0;
  246.    b2_maxAngularVelocitySquared = b2_maxAngularVelocity * b2_maxAngularVelocity;
  248.    /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
  249.    /// that overlap is removed in one time step. However using values close to 1 often lead
  250.    /// to overshoot.
  251.    b2_contactBaumgarte = 0.2;
  253.    // Sleep
  254.    /// The time that a body must be still before it will go to sleep.
  255.    b2_timeToSleep  = 0.5; // half a second
  257.    /// A body cannot sleep if its linear velocity is above this tolerance.
  258.    b2_linearSleepTolerance = 0.01; // 1 cm/s
  260.    /// A body cannot sleep if its angular velocity is above this tolerance.
  261.    b2_angularSleepTolerance = 2.0 / 180.0; // 2 degrees/s
  263. function MakeVector(x, y: Float): TVector2; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  264. procedure SetZero(var v: TVector2); {$IFNDEF OP_OVERLOAD}overload;{$ENDIF} {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  265. procedure SetValue(var v: TVector2; ax, ay: Float); {$IFNDEF OP_OVERLOAD}overload;{$ENDIF} {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  267. {$IFNDEF OP_OVERLOAD}
  268. // For TVector2
  269. function IsValid(const v: TVector2): Boolean; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  270. function Length(const v: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  272. function SqrLength(const v: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  273. function Normalize(var v: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  274. procedure SetNegative(var v: TVector2); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  276. function Negative(const AValue: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  277. function Add(const Left, Right: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  278. function Add(const p1, p2, p3: TVector2): TVector2; overload;
  279. function Subtract(const Left, Right: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  281. function Multiply(const Left: TVector2; const Right: Single): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  282. function Multiply(const Left: TVector2; const Right: Double): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  283. function Multiply(const Left: TVector2; const Right: Extended): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  285. function Divide(const Left: TVector2; const Right: Single): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  286. function Divide(const Left: TVector2; const Right: Double): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  287. function Divide(const Left: TVector2; const Right: Extended): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  289. procedure AddBy(var v: TVector2; const Operand: TVector2); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  290. procedure SubtractBy(var v: TVector2; const Operand: TVector2); {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  291. procedure MultiplyBy(var v: TVector2; const Operand: Single); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  292. procedure MultiplyBy(var v: TVector2; const Operand: Double); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  293. procedure MultiplyBy(var v: TVector2; const Operand: Extended); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  294. procedure DivideBy(var v: TVector2; const Operand: Single); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  295. procedure DivideBy(var v: TVector2; const Operand: Double); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  296. procedure DivideBy(var v: TVector2; const Operand: Extended); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  298. // For TMatrix22
  299. procedure SetIdentity(var m: TMatrix22); overload;
  300. procedure SetZero(var m: TMatrix22); overload;
  301. procedure SetValue(var m: TMatrix22; const _col1, _col2: TVector2); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  302. procedure SetValue(var m: TMatrix22; angle: Float); overload;
  304. function Invert(const m: TMatrix22): TMatrix22;
  305. function Solve(const m: TMatrix22; const b: TVector2): TVector2; // Solve A * x = b, where b is a column vector.
  307. function Negative(const AValue: TMatrix22): TMatrix22; overload;
  308. function Add(const Left, Right: TMatrix22): TMatrix22; overload;
  309. function Add(const m1, m2, m3: TMatrix22): TMatrix22; overload;
  310. function Subtract(const Left, Right: TMatrix22): TMatrix22; overload;
  312. // For T2bXForm
  313. procedure SetIdentity(var xf: Tb2XForm); overload;
  315. // For Tb2Sweep
  316. procedure GetXForm(const Sweep: Tb2Sweep; var xf: Tb2XForm; t: Float);
  317. procedure Advance(var Sweep: Tb2Sweep; t: Float);
  319. {$ENDIF}
  321. function b2MixFriction(friction1, friction2: Float): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  322. function b2MixRestitution(restitution1, restitution2: Float): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  324. function b2Max(const a, b: Float): Float; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  325. function b2Max(const a, b: TVector2): TVector2; overload;
  326. function b2Min(const a, b: Float): Float; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  327. function b2Min(const a, b: TVector2): TVector2; overload;
  328. function b2Max(const a, b: Int32): Int32; overload;
  329. function b2Min(const a, b: Int32): Int32; overload;
  331. procedure b2Swap(var a, b: Float); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  332. procedure b2Swap(var a, b: Int32); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  333. procedure b2Swap(var a, b: TVector2); overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  335. function b2Clamp(const a, low, high: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  336. function b2Clamp(const a, low, high: Float): Float; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  338. function b2Dot(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  339. function b2Cross(const a, b: TVector2): Float; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  340. function b2Cross(const a: TVector2; s: Float): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  341. function b2Cross(s: Float; const a: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  342. function b2Mul(const A: TMatrix22; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  343. function b2MulT(const A: TMatrix22; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  344. function b2Distance(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  345. function b2DistanceSquared(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  346. function b2Mul(const A, B: TMatrix22): TMatrix22; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  347. function b2MulT(const A, B: TMatrix22): TMatrix22; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  348. function b2Mul(const T: Tb2XForm; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  349. function b2MulT(const T: Tb2XForm; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  350. function b2Abs(const a: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  351. function b2Abs(const a: TMatrix22): TMatrix22; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  353. {$IFDEF EXTENDED_PRECISION}
  354. procedure SinCos(const Theta: Extended; var Sin, Cos: Extended);
  355. {$ELSE}
  356. {$IFDEF DOUBLE_PRECISION}
  357. procedure SinCos(const Theta: Double; var Sin, Cos: Double);
  358. {$ELSE}
  359. procedure SinCos(const Theta: Single; var Sin, Cos: Single);
  360. {$ENDIF}
  361. {$ENDIF}
  363. implementation
  365. {$IFDEF EXTENDED_PRECISION}
  366. procedure SinCos(const Theta: Extended; var Sin, Cos: Extended);
  367. asm
  368.    FLD  Theta
  369.    FSINCOS
  370.    FSTP TBYTE PTR [EDX]    // cosine
  371.    FSTP TBYTE PTR [EAX]    // sine
  372. end;
  374. {$ELSE}
  375. {$IFDEF DOUBLE_PRECISION}
  376. procedure SinCos(const Theta: Double; var Sin, Cos: Double);
  377. asm
  378.    FLD  Theta
  379.    FSINCOS
  380.    FSTP QWORD PTR [EDX]    // cosine
  381.    FSTP QWORD PTR [EAX]    // sine
  382. end;
  383. {$ELSE}
  384. procedure SinCos(const Theta: Single; var Sin, Cos: Single);
  385. asm
  386.    FLD  Theta
  387.    FSINCOS
  388.    FSTP DWORD PTR [EDX]    // cosine
  389.    FSTP DWORD PTR [EAX]    // sine
  390. end;
  391. {$ENDIF}
  392. {$ENDIF}
  394. function MakeVector(x, y: Float): TVector2;
  395. begin
  396.    Result.x := x;
  397.    Result.y := y;
  398. end;
  400. procedure SetZero(var v: TVector2);
  401. begin
  402.    with v do
  403.    begin
  404.       x := 0.0;
  405.       y := 0.0;
  406.    end;
  407. end;
  409. procedure SetValue(var v: TVector2; ax, ay: Float);
  410. begin
  411.    with v do
  412.    begin
  413.       x := ax;
  414.       y := ay;
  415.    end;
  416. end;
  418. {$IFNDEF OP_OVERLOAD}
  419. function IsValid(const v: TVector2): Boolean;
  420. begin
  421.    Result := UPhysics2DTypes.IsValid(v.x) and UPhysics2DTypes.IsValid(v.y);
  422. end;
  424. function Length(const v: TVector2): Float;
  425. begin
  426.    with v do
  427.       Result := Sqrt(x * x + y * y);
  428. end;
  430. function SqrLength(const v: TVector2): Float;
  431. begin
  432.    with v do
  433.       Result := x * x + y * y;
  434. end;
  436. function Normalize(var v: TVector2): Float;
  437. begin
  438.    Result := Length(v);
  439.  if Result < FLT_EPSILON then
  440.    begin
  441.       Result := 0.0;
  442.       Exit;
  443.    end;
  445.    with v do
  446.    begin
  447.       x := x / Result;
  448.       y := y / Result;
  449.    end;
  450. end;
  452. procedure SetNegative(var v: TVector2);
  453. begin
  454.    with v do
  455.    begin
  456.       x := -x;
  457.       y := -y;
  458.    end;
  459. end;
  461. function Negative(const AValue: TVector2): TVector2;
  462. begin
  463.    Result.x := -AValue.x;
  464.    Result.y := -AValue.y;
  465. end;
  467. function Add(const Left, Right: TVector2): TVector2;
  468. begin
  469.    Result.x := Left.x + Right.x;
  470.    Result.y := Left.y + Right.y;
  471. end;
  473. function Add(const p1, p2, p3: TVector2): TVector2;
  474. begin
  475.    Result.x := p1.x + p2.x + p3.x;
  476.    Result.y := p1.y + p2.y + p3.y;
  477. end;
  479. function Subtract(const Left, Right: TVector2): TVector2;
  480. begin
  481.    Result.x := Left.x - Right.x;
  482.    Result.y := Left.y - Right.y;
  483. end;
  485. function Multiply(const Left: TVector2; const Right: Single): TVector2;
  486. begin
  487.    Result.x := Left.x * Right;
  488.    Result.y := Left.y * Right;
  489. end;
  491. function Multiply(const Left: TVector2; const Right: Double): TVector2;
  492. begin
  493.    Result.x := Left.x * Right;
  494.    Result.y := Left.y * Right;
  495. end;
  497. function Multiply(const Left: TVector2; const Right: Extended): TVector2;
  498. begin
  499.    Result.x := Left.x * Right;
  500.    Result.y := Left.y * Right;
  501. end;
  503. function Divide(const Left: TVector2; const Right: Single): TVector2;
  504. begin
  505.    Result.x := Left.x / Right;
  506.    Result.y := Left.y / Right;
  507. end;
  509. function Divide(const Left: TVector2; const Right: Double): TVector2;
  510. begin
  511.    Result.x := Left.x / Right;
  512.    Result.y := Left.y / Right;
  513. end;
  515. function Divide(const Left: TVector2; const Right: Extended): TVector2;
  516. begin
  517.    Result.x := Left.x / Right;
  518.    Result.y := Left.y / Right;
  519. end;
  521. procedure AddBy(var v: TVector2; const Operand: TVector2);
  522. begin
  523.    with v do
  524.    begin
  525.       x := x + Operand.x;
  526.       y := y + Operand.y;
  527.    end;
  528. end;
  530. procedure SubtractBy(var v: TVector2; const Operand: TVector2);
  531. begin
  532.    with v do
  533.    begin
  534.       x := x - Operand.x;
  535.       y := y - Operand.y;
  536.    end;
  537. end;
  539. procedure MultiplyBy(var v: TVector2; const Operand: Single);
  540. begin
  541.    with v do
  542.    begin
  543.       x := x * Operand;
  544.       y := y * Operand;
  545.    end;
  546. end;
  548. procedure MultiplyBy(var v: TVector2; const Operand: Double);
  549. begin
  550.    with v do
  551.    begin
  552.       x := x * Operand;
  553.       y := y * Operand;
  554.    end;
  555. end;
  557. procedure MultiplyBy(var v: TVector2; const Operand: Extended);
  558. begin
  559.    with v do
  560.    begin
  561.       x := x * Operand;
  562.       y := y * Operand;
  563.    end;
  564. end;
  566. procedure DivideBy(var v: TVector2; const Operand: Single);
  567. begin
  568.    with v do
  569.    begin
  570.       x := x / Operand;
  571.       y := y / Operand;
  572.    end;
  573. end;
  575. procedure DivideBy(var v: TVector2; const Operand: Double);
  576. begin
  577.    with v do
  578.    begin
  579.       x := x / Operand;
  580.       y := y / Operand;
  581.    end;
  582. end;
  584. procedure DivideBy(var v: TVector2; const Operand: Extended);
  585. begin
  586.    with v do
  587.    begin
  588.       x := x / Operand;
  589.       y := y / Operand;
  590.    end;
  591. end;
  593. procedure SetIdentity(var m: TMatrix22);
  594. begin
  595.    with m do
  596.    begin
  597.       col1.x := 1.0;
  598.       col2.x := 0.0;
  599.       col1.y := 0.0;
  600.       col2.y := 1.0;
  601.    end;
  602. end;
  604. procedure SetZero(var m: TMatrix22);
  605. begin
  606.    with m do
  607.    begin
  608.       col1.x := 0.0;
  609.       col2.x := 0.0;
  610.       col1.y := 0.0;
  611.       col2.y := 0.0;
  612.    end;
  613. end;
  615. procedure SetValue(var m: TMatrix22; const _col1, _col2: TVector2);
  616. begin
  617.    with m do
  618.    begin
  619.       col1 := _col1;
  620.       col2 := _col2;
  621.    end;
  622. end;
  624. procedure SetValue(var m: TMatrix22; angle: Float);
  625. var
  626.    c, s: Float;
  627. begin
  628.     SinCos(angle, s, c);
  630.     with m do
  631.     begin
  632.        col1.x := c;
  633.        col2.x := -s;
  634.        col1.y := s;
  635.        col2.y := c;
  636.     end;
  637. end;
  639. function Invert(const m: TMatrix22): TMatrix22;
  640. var
  641.    a, b, c, d, det: Float;
  642. begin
  643.    with m do
  644.    begin
  645.       a := col1.x;
  646.       b := col2.x;
  647.       c := col1.y;
  648.       d := col2.y;
  649.    end;
  651.    det := a * d - b * c;
  652.    {$IFDEF HANDLE_EXCEPTION}
  653.    if IsZero(det) then
  654.       raise TPhysics2DException.Create(ecDivByZero);
  655.    {$ENDIF}
  656.    det := 1.0 / det;
  657.    with Result do
  658.    begin
  659.       col1.x :=  det * d;
  660.       col2.x := -det * b;
  661.       col1.y := -det * c;
  662.       col2.y :=  det * a;
  663.    end;
  664. end;
  666. function Solve(const m: TMatrix22; const b: TVector2): TVector2;
  667. var
  668.    a11, a12, a21, a22, det: Float;
  669. begin
  670.    with m do
  671.    begin
  672.       a11 := col1.x;
  673.       a12 := col2.x;
  674.       a21 := col1.y;
  675.       a22 := col2.y;
  676.    end;
  677.    det := a11 * a22 - a12 * a21;
  678.    {$IFDEF HANDLE_EXCEPTION}
  679.    if IsZero(det) then
  680.       raise TPhysics2DException.Create(ecDivByZero);
  681.    {$ENDIF}
  682.    det := 1.0 / det;
  683.    Result.x := det * (a22 * b.x - a12 * b.y);
  684.    Result.y := det * (a11 * b.y - a21 * b.x);
  685. end;
  687. function Negative(const AValue: TMatrix22): TMatrix22;
  688. begin
  689.    with Result do
  690.    begin
  691.       col1.x := -AValue.col1.x;
  692.       col1.y := -AValue.col1.y;
  693.       col2.x := -AValue.col2.x;
  694.       col2.y := -AValue.col2.y;
  695.    end;
  696. end;
  698. function Add(const Left, Right: TMatrix22): TMatrix22;
  699. begin
  700.    with Result do
  701.    begin
  702.       col1.x := Left.col1.x + Right.col1.x;
  703.       col1.y := Left.col1.y + Right.col1.y;
  704.       col2.x := Left.col2.x + Right.col2.x;
  705.       col2.y := Left.col2.y + Right.col2.y;
  706.    end;
  707. end;
  709. function Add(const m1, m2, m3: TMatrix22): TMatrix22;
  710. begin
  711.    with Result do
  712.    begin
  713.       col1.x := m1.col1.x + m2.col1.x + m3.col1.x;
  714.       col1.y := m1.col1.y + m2.col1.y + m3.col1.y;
  715.       col2.x := m1.col2.x + m2.col2.x + m3.col2.x;
  716.       col2.y := m1.col2.y + m2.col2.y + m3.col2.y;
  717.    end;
  718. end;
  720. function Subtract(const Left, Right: TMatrix22): TMatrix22;
  721. begin
  722.    with Result do
  723.    begin
  724.       col1.x := Left.col1.x - Right.col1.x;
  725.       col1.y := Left.col1.y - Right.col1.y;
  726.       col2.x := Left.col2.x - Right.col2.x;
  727.       col2.y := Left.col2.y - Right.col2.y;
  728.    end;
  729. end;
  731. procedure SetIdentity(var xf: Tb2XForm);
  732. begin
  733.    with xf do
  734.    begin
  735.       SetZero(position);
  736.       SetIdentity(R);
  737.    end;
  738. end;
  740. procedure GetXForm(const Sweep: Tb2Sweep; var xf: Tb2XForm; t: Float);
  741. var
  742.    alpha: Float;
  743. begin
  744.    with Sweep do
  745.    begin
  746.       // center = p + R * localCenter
  747.       if (1.0 - t0 > FLT_EPSILON) then
  748.       begin
  749.          alpha := (t - t0) / (1.0 - t0);
  750.          xf.position := Add(Multiply(c0, 1.0 - alpha), Multiply(c, alpha));
  751.          SetValue(xf.R, (1.0 - alpha) * a0 + alpha * a);
  752.       end
  753.       else
  754.       begin
  755.          // center = p + R * localCenter
  756.          xf.position := c;
  757.          SetValue(xf.R, a);
  758.       end;
  759.       // Shift to origin
  760.       SubtractBy(xf.position, b2Mul(xf.R, localCenter));
  761.    end;
  762. end;
  764. procedure Advance(var Sweep: Tb2Sweep; t: Float);
  765. var
  766.    alpha: Float;
  767. begin
  768.    with Sweep do
  769.    begin
  770.       if (t0 < t) and (t0 < 1.0 - FLT_EPSILON) then
  771.       begin
  772.          alpha := (t - t0) / (1.0 - t0);
  773.          c0 := Add(Multiply(c0, 1.0 - alpha), Multiply(c, alpha));
  774.          a0 := (1.0 - alpha) * a0 + alpha * a;
  775.          t0 := t;
  776.       end;
  777.    end;
  778. end;
  780. {$ENDIF}
  782. /// Friction mixing law. Feel free to customize this.
  783. function b2MixFriction(friction1, friction2: Float): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  784. begin
  785.  Result := Sqrt(friction1 * friction2);
  786. end;
  788. /// Restitution mixing law. Feel free to customize this.
  789. function b2MixRestitution(restitution1, restitution2: Float): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  790. begin
  791.    if restitution1 > restitution2 then
  792.       Result := restitution1
  793.    else
  794.       Result := restitution2;
  795. end;
  797. function b2Max(const a, b: Float): Float;
  798. begin
  799.    if a >= b then
  800.       Result := a
  801.    else
  802.       Result := b;
  803. end;
  805. function b2Max(const a, b: TVector2): TVector2;
  806. begin
  807.    Result.x := b2Max(a.x, b.x);
  808.    Result.y := b2Max(a.y, b.y);
  809. end;
  811. function b2Min(const a, b: Float): Float;
  812. begin
  813.    if a >= b then
  814.       Result := b
  815.    else
  816.       Result := a;
  817. end;
  819. function b2Min(const a, b: TVector2): TVector2;
  820. begin
  821.    Result.x := b2Min(a.x, b.x);
  822.    Result.y := b2Min(a.y, b.y);
  823. end;
  825. function b2Max(const a, b: Int32): Int32;
  826. begin
  827.    if a >= b then
  828.       Result := a
  829.    else
  830.       Result := b;
  831. end;
  833. function b2Min(const a, b: Int32): Int32;
  834. begin
  835.    if a >= b then
  836.       Result := b
  837.    else
  838.       Result := a;
  839. end;
  841. procedure b2Swap(var a, b: Float);
  842. var
  843.    tmp: Float;
  844. begin
  845.    tmp := a;
  846.    a := b;
  847.    b := tmp;
  848. end;
  850. procedure b2Swap(var a, b: Int32);
  851. var
  852.    tmp: Int32;
  853. begin
  854.    tmp := a;
  855.    a := b;
  856.    b := tmp;
  857. end;
  859. procedure b2Swap(var a, b: TVector2);
  860. var
  861.    tmp: TVector2;
  862. begin
  863.    tmp := a;
  864.    a := b;
  865.    b := tmp;
  866. end;
  868. function b2Clamp(const a, low, high: TVector2): TVector2;
  869. begin
  870.    Result := b2Max(low, b2Min(a, high));
  871. end;
  873. function b2Clamp(const a, low, high: Float): Float;
  874. begin
  875.    Result := b2Max(low, b2Min(a, high));
  876. end;
  878. function IsValid(f: Float): Boolean; {$IFNDEF OP_OVERLOAD}overload;{$ENDIF}
  879. begin
  880.    Result := not (IsNan(f) or IsInfinite(f));
  881. end;
  883. /// Peform the dot product on two vectors.
  884. function b2Dot(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  885. begin
  886.    Result := a.x * b.x + a.y * b.y;
  887. end;
  889. /// Perform the cross product on two vectors. In 2D this produces a scalar.
  890. function b2Cross(const a, b: TVector2): Float; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  891. begin
  892.  Result := a.x * b.y - a.y * b.x;
  893. end;
  895. /// Perform the cross product on a vector and a scalar. In 2D this produces a vector.
  896. function b2Cross(const a: TVector2; s: Float): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  897. begin
  898.    Result.x := s * a.y;
  899.    Result.y := -s * a.x;
  900. end;
  902. /// Perform the cross product on a scalar and a vector. In 2D this produces a vector.
  903. function b2Cross(s: Float; const a: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  904. begin
  905.    Result.x := -s * a.y;
  906.    Result.y := s * a.x;
  907. end;
  909. /// Multiply a matrix times a vector. If a rotation matrix is provided,
  910. /// then this transforms the vector from one frame to another.
  911. function b2Mul(const A: TMatrix22; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  912. begin
  913.    Result.x := A.col1.x * v.x + A.col2.x * v.y;
  914.    Result.y := A.col1.y * v.x + A.col2.y * v.y;
  915. end;
  917. /// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
  918. /// then this transforms the vector from one frame to another (inverse transform).
  919. function b2MulT(const A: TMatrix22; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  920. begin
  921.    Result.x := b2Dot(v, A.col1);
  922.    Result.y := b2Dot(v, A.col2);
  923. end;
  925. {$IFDEF OP_OVERLOAD}
  926. function b2Distance(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  927. begin
  928.    Result := (a - b).Length;
  929. end;
  931. function b2DistanceSquared(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  932. begin
  933.    Result := (a - b).SqrLength;
  934. end;
  935. {$ELSE}
  936. function b2Distance(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  937. begin
  938.    Result := Length(Subtract(a, b));
  939. end;
  941. function b2DistanceSquared(const a, b: TVector2): Float; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  942. begin
  943.    Result := SqrLength(Subtract(a, b));
  944. end;
  945. {$ENDIF}
  947. // A * B
  948. function b2Mul(const A, B: TMatrix22): TMatrix22; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  949. begin
  950.    Result.col1 := b2Mul(A, B.col1);
  951.    Result.col2 := b2Mul(A, B.col2);
  952. end;
  954. // A^T * B
  955. function b2MulT(const A, B: TMatrix22): TMatrix22; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  956. var
  957.    c1, c2: TVector2;
  958. begin
  959.    with Result do
  960.    begin
  961.       col1.x := b2Dot(A.col1, B.col1);
  962.       col1.y := b2Dot(A.col2, B.col1);
  963.       col2.x := b2Dot(A.col1, B.col2);
  964.       col2.y := b2Dot(A.col2, B.col2);
  965.    end;
  966. end;
  968. {$IFDEF OP_OVERLOAD}
  969. function b2Mul(const T: Tb2XForm; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  970. begin
  971.    Result := T.position + b2Mul(T.R, v);
  972. end;
  973. {$ELSE}
  974. function b2Mul(const T: Tb2XForm; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  975. var
  976.    tmp: TVector2;
  977. begin
  978.    tmp := b2Mul(T.R, v);
  979.    Result.x := T.position.x + tmp.x;
  980.    Result.y := T.position.y + tmp.y;
  981. end;
  982. {$ENDIF}
  984. {$IFDEF OP_OVERLOAD}
  985. function b2MulT(const T: Tb2XForm; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  986. begin
  987.    Result := b2MulT(T.R, v - T.position);
  988. end;
  989. {$ELSE}
  990. // unfinished
  991. function b2MulT(const T: Tb2XForm; const v: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  992. var
  993.    tmp: TVector2;
  994. begin
  995.    tmp := Subtract(v, T.position);
  996.    Result := b2MulT(T.R, tmp);
  997. end;
  998. {$ENDIF}
  1000. function b2Abs(const a: TVector2): TVector2; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  1001. begin
  1002.    Result.x := Abs(a.x);
  1003.    Result.y := Abs(a.y);
  1004. end;
  1006. function b2Abs(const a: TMatrix22): TMatrix22; overload; {$IFDEF INLINE_AVAIL}inline;{$ENDIF}
  1007. begin
  1008.    Result.col1 := b2Abs(a.col1);
  1009.    Result.col2 := b2Abs(a.col2);
  1010. end;
  1012. {$IFDEF HANDLE_EXCEPTION}
  1013. constructor TPhysics2DException.Create(ErrorCode: TPhysics2DExceptionCode);
  1014. begin
  1015.    case ErrorCode of
  1016.       ecDivByZero: ;
  1018.    end;
  1019. end;
  1020. {$ENDIF}
  1022. { TVector2 }
  1024. {$IFDEF OP_OVERLOAD}
  1026. function TVector2.IsValid: Boolean;
  1027. begin
  1028.    Result := UPhysics2DTypes.IsValid(x) and UPhysics2DTypes.IsValid(y);
  1029. end;
  1031. function TVector2.Length: Float;
  1032. begin
  1033.    Result := Sqrt(x * x + y * y);
  1034. end;
  1036. function TVector2.Normalize: Float;
  1037. begin
  1038.    Result := Length();
  1039.  if Result < FLT_EPSILON then
  1040.    begin
  1041.       Result := 0.0;
  1042.       Exit;
  1043.    end;
  1044.    x := x / Result;
  1045.    y := y / Result;
  1046. end;
  1048. procedure TVector2.SetZero;
  1049. begin
  1050.    x := 0.0;
  1051.    y := 0.0;
  1052. end;
  1054. procedure TVector2.SetNegative;
  1055. begin
  1056.    x := -x;
  1057.    y := -y;
  1058. end;
  1060. procedure TVector2.SetValue(x, y: Float);
  1061. begin
  1062.    Self.x := x;
  1063.    Self.y := y;
  1064. end;
  1066. function TVector2.SqrLength: Float;
  1067. begin
  1068.    Result := x * x + y * y;
  1069. end;
  1071. class function TVector2.From(const x, y: Float): TVector2;
  1072. begin
  1073.    Result.x := x;
  1074.    Result.y := y;
  1075. end;
  1077. class operator TVector2.Negative(const AValue: TVector2): TVector2;
  1078. begin
  1079.    Result.x := -AValue.x;
  1080.    Result.y := -AValue.y;
  1081. end;
  1083. class operator TVector2.Add(const Left, Right: TVector2): TVector2;
  1084. begin
  1085.    Result.x := Left.x + Right.x;
  1086.    Result.y := Left.y + Right.y;
  1087. end;
  1089. class operator TVector2.Subtract(const Left, Right: TVector2): TVector2;
  1090. begin
  1091.    Result.x := Left.x - Right.x;
  1092.    Result.y := Left.y - Right.y;
  1093. end;
  1095. class operator TVector2.Multiply(const Left: TVector2; const Right: Single): TVector2;
  1096. begin
  1097.    Result.x := Left.x * Right;
  1098.    Result.y := Left.y * Right;
  1099. end;
  1101. class operator TVector2.Multiply(const Left: TVector2; const Right: Double): TVector2;
  1102. begin
  1103.    Result.x := Left.x * Right;
  1104.    Result.y := Left.y * Right;
  1105. end;
  1107. class operator TVector2.Multiply(const Left: TVector2; const Right: Extended): TVector2;
  1108. begin
  1109.    Result.x := Left.x * Right;
  1110.    Result.y := Left.y * Right;
  1111. end;
  1113. class operator TVector2.Multiply(const Left: Single; const Right: TVector2): TVector2;
  1114. begin
  1115.    Result.x := Left * Right.x;
  1116.    Result.y := Left * Right.y;
  1117. end;
  1119. class operator TVector2.Multiply(const Left: Double; const Right: TVector2): TVector2;
  1120. begin
  1121.    Result.x := Left * Right.x;
  1122.    Result.y := Left * Right.y;
  1123. end;
  1125. class operator TVector2.Multiply(const Left: Extended; const Right: TVector2): TVector2;
  1126. begin
  1127.    Result.x := Left * Right.x;
  1128.    Result.y := Left * Right.y;
  1129. end;
  1131. class operator TVector2.Divide(const Left: TVector2; const Right: Single): TVector2;
  1132. begin
  1133.    Result.x := Left.x / Right;
  1134.    Result.y := Left.y / Right;
  1135. end;
  1137. class operator TVector2.Divide(const Left: TVector2; const Right: Double): TVector2;
  1138. begin
  1139.    Result.x := Left.x / Right;
  1140.    Result.y := Left.y / Right;
  1141. end;
  1143. class operator TVector2.Divide(const Left: TVector2; const Right: Extended): TVector2;
  1144. begin
  1145.    Result.x := Left.x / Right;
  1146.    Result.y := Left.y / Right;
  1147. end;
  1149. procedure TVector2.AddBy(const Operand: TVector2);
  1150. begin
  1151.    x := x + Operand.x;
  1152.    y := y + Operand.y;
  1153. end;
  1155. procedure TVector2.SubtractBy(const Operand: TVector2);
  1156. begin
  1157.    x := x - Operand.x;
  1158.    y := y - Operand.y;
  1159. end;
  1161. procedure TVector2.MultiplyBy(const Operand: Single);
  1162. begin
  1163.    x := x * Operand;
  1164.    y := y * Operand;
  1165. end;
  1167. procedure TVector2.MultiplyBy(const Operand: Double);
  1168. begin
  1169.    x := x * Operand;
  1170.    y := y * Operand;
  1171. end;
  1173. procedure TVector2.MultiplyBy(const Operand: Extended);
  1174. begin
  1175.    x := x * Operand;
  1176.    y := y * Operand;
  1177. end;
  1179. procedure TVector2.DivideBy(const Operand: Single);
  1180. begin
  1181.    x := x / Operand;
  1182.    y := y / Operand;
  1183. end;
  1185. procedure TVector2.DivideBy(const Operand: Double);
  1186. begin
  1187.    x := x / Operand;
  1188.    y := y / Operand;
  1189. end;
  1191. procedure TVector2.DivideBy(const Operand: Extended);
  1192. begin
  1193.    x := x / Operand;
  1194.    y := y / Operand;
  1195. end;
  1197. {$ENDIF}
  1199. { TMatrix22 }
  1201. {$IFDEF OP_OVERLOAD}
  1202. procedure TMatrix22.SetIdentity;
  1203. begin
  1204.  col1.x := 1.0;
  1205.    col2.x := 0.0;
  1206.  col1.y := 0.0;
  1207.    col2.y := 1.0;
  1208. end;
  1210. procedure TMatrix22.SetZero;
  1211. begin
  1212.  col1.x := 0.0;
  1213.    col2.x := 0.0;
  1214.  col1.y := 0.0;
  1215.    col2.y := 0.0;
  1216. end;
  1218. procedure TMatrix22.SetValue(const _col1, _col2: TVector2);
  1219. begin
  1220.    col1 := _col1;
  1221.    col2 := _col2;
  1222. end;
  1224. procedure TMatrix22.SetValue(angle: Float);
  1225. var
  1226.    c, s: Float;
  1227. begin
  1228.     SinCos(angle, s, c);
  1229. col1.x := c;
  1230.     col2.x := -s;
  1231. col1.y := s;
  1232.     col2.y := c;
  1233. end;
  1235. function TMatrix22.Invert: TMatrix22;
  1236. var
  1237.    a, b, c, d, det: Float;
  1238. begin
  1239.     a := col1.x;
  1240.     b := col2.x;
  1241.     c := col1.y;
  1242.     d := col2.y;
  1244. det := a * d - b * c;
  1245.     {$IFDEF HANDLE_EXCEPTION}
  1246.     if IsZero(det) then
  1247.        raise TPhysics2DException.Create(ecDivByZero);
  1248.     {$ENDIF}
  1249. det := 1.0 / det;
  1250.     with Result do
  1251.     begin
  1252.        col1.x :=  det * d;
  1253.        col2.x := -det * b;
  1254.        col1.y := -det * c;
  1255.        col2.y :=  det * a;
  1256.     end;
  1257. end;
  1259. function TMatrix22.Solve(const b: TVector2): TVector2;
  1260. var
  1261.    a11, a12, a21, a22, det: Float;
  1262. begin
  1263. a11 := col1.x;
  1264.     a12 := col2.x;
  1265.     a21 := col1.y;
  1266.     a22 := col2.y;
  1267. det := a11 * a22 - a12 * a21;
  1268.     {$IFDEF HANDLE_EXCEPTION}
  1269.     if IsZero(det) then
  1270.        raise TPhysics2DException.Create(ecDivByZero);
  1271.     {$ENDIF}
  1272. det := 1.0 / det;
  1273. Result.x := det * (a22 * b.x - a12 * b.y);
  1274. Result.y := det * (a11 * b.y - a21 * b.x);
  1275. end;
  1277. class operator TMatrix22.Negative(const AValue: TMatrix22): TMatrix22;
  1278. begin
  1279.    with Result do
  1280.    begin
  1281.       col1.x := -AValue.col1.x;
  1282.       col1.y := -AValue.col1.y;
  1283.       col2.x := -AValue.col2.x;
  1284.       col2.y := -AValue.col2.y;
  1285.    end;
  1286. end;
  1288. class operator TMatrix22.Add(const Left, Right: TMatrix22): TMatrix22;
  1289. begin
  1290.    with Result do
  1291.    begin
  1292.       col1.x := Left.col1.x + Right.col1.x;
  1293.       col1.y := Left.col1.y + Right.col1.y;
  1294.       col2.x := Left.col2.x + Right.col2.x;
  1295.       col2.y := Left.col2.y + Right.col2.y;
  1296.    end;
  1297. end;
  1299. class operator TMatrix22.Subtract(const Left, Right: TMatrix22): TMatrix22;
  1300. begin
  1301.    with Result do
  1302.    begin
  1303.       col1.x := Left.col1.x - Right.col1.x;
  1304.       col1.y := Left.col1.y - Right.col1.y;
  1305.       col2.x := Left.col2.x - Right.col2.x;
  1306.       col2.y := Left.col2.y - Right.col2.y;
  1307.    end;
  1308. end;
  1310. {$ENDIF}
  1312. { Tb2XForm }
  1313. {$IFDEF OP_OVERLOAD}
  1314. procedure Tb2XForm.SetIdentity;
  1315. begin
  1316.    position.SetZero;
  1317.    R.SetIdentity;
  1318. end;
  1320. class function Tb2XForm.From(const position: TVector2; const R: TMatrix22): Tb2XForm;
  1321. begin
  1322.    Result.position := position;
  1323.    Result.R := R;
  1324. end;
  1325. {$ENDIF}
  1327. { Tb2Sweep }
  1328. {$IFDEF OP_OVERLOAD}
  1329. procedure Tb2Sweep.GetXForm(var xf: Tb2XForm; t: Float);
  1330. var
  1331.    alpha: Float;
  1332. begin
  1333.  if (1.0 - t0 > FLT_EPSILON) then
  1334.  begin
  1335.       alpha := (t - t0) / (1.0 - t0);
  1336.       xf.position := (1.0 - alpha) * c0 + alpha * c;
  1337.       xf.R.SetValue((1.0 - alpha) * a0 + alpha * a);
  1338.    end
  1339.    else
  1340.    begin
  1341.       // center = p + R * localCenter
  1342.       xf.position := c;
  1343.       xf.R.SetValue(a);
  1344.    end;
  1345.    // Shift to origin
  1346.    xf.position.SubtractBy(b2Mul(xf.R, localCenter));
  1347. end;
  1349. procedure Tb2Sweep.Advance(t: Float);
  1350. var
  1351.    alpha: Float;
  1352. begin
  1353.    if (t0 < t) and (t0 < 1.0 - FLT_EPSILON) then
  1354.    begin
  1355.       alpha := (t - t0) / (1.0 - t0);
  1356.       c0 := (1.0 - alpha) * c0 + alpha * c;
  1357.       a0 := (1.0 - alpha) * a0 + alpha * a;
  1358.       t0 := t;
  1359.    end;
  1360. end;
  1361. {$ENDIF}
  1363. end.