2
// This unit is part of the GLScene Engine https://github.com/glscene
5
Base Verlet modelling/simulation classes.
6
This unit is generic, GLScene-specific sub-classes are in GLVerletClasses.
8
Note that currently, the SatisfyConstraintForEdge methods push the nodes in
9
the edge uniformly - it should push the closer node more for correct physics.
10
It's a matter of leverage.
13
18/11/14 - PW - Renamed VerletClasses.pas to GLVerletTypes
14
10/11/12 - PW - Added CPP compatibility: changed vector arrays to records
15
25/11/09 - DanB - Fix for TGLVerletGlobalConstraint.TranslateKickbackTorque
16
31/03/07 - DaStr - Added $I GLScene.inc
17
14/04/04 - MF - Fixed force for springs, was referring to deltaP...
18
13/04/04 - MF - Minor drag changes
19
13/04/04 - EG - Added TVCHeightField and TVCSlider, fixed TVCFloor
20
and TVFSpring, altered the way world Drag operates
21
06/03/04 - MF - Small updates to accomodate hair
22
11/07/03 - EG - Optimized TVCCube collider
23
11/07/03 - MF - A bit of a documentation effort
24
10/07/03 - MF - Verlets now use spatial partitioning objects to speed
26
10/07/03 - MF - Renaming TVerletAssembly to TGLVerletWorld
27
24/06/03 - MF - Added force kickbacks for integration with external
28
physics. Needs to be split into force+torque and add
29
friction to the kickback
30
19/06/03 - MF - Added TGLVerletGlobalConstraint.SatisfyConstraintForEdge
31
and implemented for TVCSphere and TVCCapsule
32
19/06/03 - MF - Added friction to TVCCylinder
33
19/06/03 - MF - Added surface normals to all colliders - surface
34
normal is identical to Normalize(Movement)!
35
18/06/03 - MF - Moved FrictionRatio to TGLVerletGlobalFrictionConstraint
36
18/06/03 - EG - Updated TVCCapsule
37
18/06/03 - MF - Updated TVCFloor to use a normal and a point
38
18/06/03 - MF - Added TVCCapsule
39
17/06/03 - MF - Added TVFAirResistance
40
17/06/03 - MF - Added TVCCube collider
41
16/06/03 - MF - Fixed TVFSpring.SetRestlengthToCurrent
42
24/07/02 - EG - Added TVCCylinder
43
18/07/02 - EG - Improved forces & constraints
44
23/06/02 - EG - Stricter encapsulation, fixed some leaks,
45
Various optimizations (+25%)
46
21/06/02 - EG - Creation (original code by Mattias Fagerlund)
56
Classes, SysUtils, Types,
58
GLCrossPlatform, GLVectorGeometry, GLVectorLists, GLSpacePartition,
59
GLGeometryBB, GLVectorTypes;
63
cDEFAULT_CONSTRAINT_FRICTION = 0.6;
66
TGLVerletEdgeList = class;
67
TGLVerletWorld = class;
69
TVerletProgressTimes = packed record
70
deltaTime, newTime : Double;
71
sqrDeltaTime, invSqrDeltaTime : Single;
77
TVerletNode = class(TSpacePartitionLeaf)
80
FForce : TAffineVector;
81
FOwner : TGLVerletWorld;
82
FWeight, FInvWeight : Single;
84
FNailedDown : Boolean;
86
FChangedOnStep : Integer;
87
function GetSpeed: TAffineVector;
91
FLocation, FOldLocation : TAffineVector;
93
procedure SetLocation(const Value: TAffineVector);virtual;
95
procedure SetWeight(const value : Single);
97
procedure AfterProgress; virtual;
101
constructor CreateOwned(const aOwner : TGLVerletWorld); virtual;
102
destructor Destroy; override;
105
procedure ApplyFriction(const friction, penetrationDepth : Single;
106
const surfaceNormal : TAffineVector);
107
{ Simple and less accurate method for friction }108
procedure OldApplyFriction(const friction, penetrationDepth : Single);
110
{ Perform Verlet integration }111
procedure Verlet(const vpt : TVerletProgressTimes); virtual;
113
{ Initlializes the node. For the base class, it just makes sure that114
FOldPosition = FPosition, so that speed is zero }
115
procedure Initialize; dynamic;
117
{ Calculates the distance to another node }118
function DistanceToNode(const node : TVerletNode) : Single;
120
{ Calculates the movement of the node }121
function GetMovement : TAffineVector;
123
{ The TVerletNode inherits from TSpacePartitionLeaf, and it needs to124
know how to publish itself. The owner ( a TGLVerletWorld ) has a spatial
126
procedure UpdateCachedAABBAndBSphere; override;
128
{ The VerletWorld that owns this verlet }129
property Owner : TGLVerletWorld read FOwner;
131
{ The location of the verlet }132
property Location : TAffineVector read FLocation write SetLocation;
134
{ The old location of the verlet. This is used for verlet integration }135
property OldLocation : TAffineVector read FOldLocation write FOldLocation;
137
{ The radius of the verlet node - this has been more or less deprecated }138
property Radius : Single read FRadius write FRadius;
140
{ A sum of all forces that has been applied to this verlet node during a step }141
property Force : TAffineVector read FForce write FForce;
143
{ If the node is nailed down, it can't be moved by either force,144
constraint or verlet integration - but you can still move it by hand }
145
property NailedDown : Boolean read FNailedDown write FNailedDown;
147
{ The weight of a node determines how much it's affected by a force }148
property Weight : Single read FWeight write SetWeight;
150
{ InvWeight is 1/Weight, and is kept up to date automatically }151
property InvWeight : Single read FInvWeight;
153
{ Returns the speed of the verlet node. Speed = Movement / deltatime }154
property Speed : TAffineVector read GetSpeed;
156
{ Each node has a friction that effects how it reacts during contacts.}157
property Friction : Single read FFriction write FFriction;
159
{ What phyisics step was this node last changed? Used to keep track160
of when the spatial partitioning needs to be updated }
161
property ChangedOnStep : Integer read FChangedOnStep;
164
TVerletNodeClass = class of TVerletNode;
168
TVerletNodeList = class(TList)
171
function GetItems(i : Integer): TVerletNode;
172
procedure SetItems(i : Integer; const value : TVerletNode);
176
property Items[i : Integer] : TVerletNode read GetItems write SetItems; default;
181
TVerletConstraint = class (TObject)
184
FOwner : TGLVerletWorld;
190
constructor Create(const aOwner : TGLVerletWorld); virtual;
191
destructor Destroy; override;
193
{ Updates the position of one or several nodes to make sure that they194
don't violate the constraint }
195
procedure SatisfyConstraint(const iteration, maxIterations : Integer); virtual; abstract;
196
{ Notifies removal of a node }197
procedure RemoveNode(const aNode : TVerletNode); virtual; abstract;
198
{ Method that's fired before the physics iterations are performed}199
procedure BeforeIterations; virtual;
201
{ Onwer of the constraint }202
property Owner : TGLVerletWorld read FOwner;
203
{ Determines if the constraint should be enforced or not }204
property Enabled : Boolean read FEnabled write FEnabled;
205
{ Tag field reserved for the user. }206
property Tag : Integer read FTag write FTag;
209
// TGLVerletDualConstraint
211
TGLVerletDualConstraint = class (TVerletConstraint)
214
FNodeA, FNodeB : TVerletNode;
218
procedure RemoveNode(const aNode : TVerletNode); override;
220
{ Reference to NodeA. }221
property NodeA : TVerletNode read FNodeA write FNodeA;
222
{ Reference to NodeB. }223
property NodeB : TVerletNode read FNodeB write FNodeB;
226
// TVerletGroupConstraint
228
TVerletGroupConstraint = class (TVerletConstraint)
231
FNodes : TVerletNodeList;
235
constructor Create(const aOwner : TGLVerletWorld); override;
236
destructor Destroy; override;
238
procedure RemoveNode(const aNode : TVerletNode); override;
240
{ The list of nodes that this constraint will effect}241
property Nodes : TVerletNodeList read FNodes;
245
// Verlet edges simulate rigid collission edges
246
TGLVerletEdge = class(TSpacePartitionLeaf)
254
{ The TGLVerletEdge inherits from TSpacePartitionLeaf, and it needs to255
know how to publish itself. The owner ( a TGLVerletWorld ) has a spatial
257
procedure UpdateCachedAABBAndBSphere; override;
259
constructor CreateEdgeOwned(const aNodeA, aNodeB : TVerletNode);
261
{ One of the nodes in the edge }262
property NodeA : TVerletNode read FNodeA write FNodeA;
264
{ One of the nodes in the edge }265
property NodeB : TVerletNode read FNodeB write FNodeB;
268
TGLVerletEdgeList = class(TList)
271
function GetItems(i: Integer): TGLVerletEdge;
272
procedure SetItems(i: Integer; const Value: TGLVerletEdge);
276
property Items[i : Integer] : TGLVerletEdge read GetItems write SetItems; default;
279
// TGLVerletGlobalConstraint
281
TGLVerletGlobalConstraint = class (TVerletConstraint)
284
FKickbackForce: TAffineVector;
285
FKickbackTorque : TAffineVector;
286
FLocation: TAffineVector;
287
procedure SetLocation(const Value: TAffineVector); virtual;
291
constructor Create(const aOwner : TGLVerletWorld); override;
292
destructor Destroy; override;
294
procedure RemoveNode(const aNode : TVerletNode); override;
295
procedure BeforeIterations; override;
297
procedure SatisfyConstraint(const iteration, maxIterations : Integer); override;
298
procedure SatisfyConstraintForNode(const aNode : TVerletNode;
299
const iteration, maxIterations : Integer); virtual; abstract;
300
procedure SatisfyConstraintForEdge(const aEdge : TGLVerletEdge;
301
const iteration, maxIterations : Integer); virtual;
303
property Location : TAffineVector read FLocation write SetLocation;
305
{ The force that this collider has experienced while correcting the306
verlet possitions. This force can be applied to ODE bodies, for
308
property KickbackForce : TAffineVector read FKickbackForce write FKickbackForce;
309
{ The torque that this collider has experienced while correcting the310
verlet possitions, in reference to the center of the collider. The
311
torque force can be applied to ODE bodies, but it must first be
312
translated. A torque can be trasnalted by
314
EM(b) = EM(a) + EF x VectorSubtract(b, a).
316
Simply adding the torque to the body will NOT work correctly. See
317
TranslateKickbackTorque}
318
property KickbackTorque : TAffineVector read FKickbackTorque write FKickbackTorque;
320
procedure AddKickbackForceAt(const Pos : TAffineVector; const Force : TAffineVector);
322
function TranslateKickbackTorque(const TorqueCenter : TAffineVector) : TAffineVector;
325
// TGLVerletGlobalFrictionConstraint
327
TGLVerletGlobalFrictionConstraint = class (TGLVerletGlobalConstraint)
330
FFrictionRatio: Single;
334
constructor Create(const aOwner : TGLVerletWorld); override;
336
property FrictionRatio : Single read FFrictionRatio write FFrictionRatio;
339
TGLVerletGlobalFrictionConstraintSP = class(TGLVerletGlobalFrictionConstraint)
341
procedure SatisfyConstraint(const iteration, maxIterations : Integer); override;
342
procedure PerformSpaceQuery; virtual; abstract;
345
TGLVerletGlobalFrictionConstraintSphere = class(TGLVerletGlobalFrictionConstraintSP)
347
FCachedBSphere: TBSphere;
349
procedure SetLocation(const Value: TAffineVector); override;
351
procedure UpdateCachedBSphere;
352
procedure PerformSpaceQuery; override;
353
function GetBSphere : TBSphere; virtual; abstract;
355
property CachedBSphere : TBSphere read FCachedBSphere;
358
TGLVerletGlobalFrictionConstraintBox = class(TGLVerletGlobalFrictionConstraintSP)
362
procedure SetLocation(const Value: TAffineVector); override;
364
procedure UpdateCachedAABB;
366
procedure PerformSpaceQuery; override;
367
function GetAABB : TAABB; virtual; abstract;
369
property CachedAABB : TAABB read FCachedAABB;
372
// TVerletConstraintList
374
TVerletConstraintList = class(TList)
377
function GetItems(i : Integer): TVerletConstraint;
378
procedure SetItems(i : Integer; const Value: TVerletConstraint);
382
property Items[i : Integer] : TVerletConstraint read GetItems write SetItems; default;
387
{ Generic verlet force. }388
TVerletForce = class (TObject)
391
FOwner : TGLVerletWorld;
395
constructor Create(const aOwner : TGLVerletWorld); virtual;
396
destructor Destroy; override;
398
// Implementation should add force to force resultant for all relevant nodes
399
procedure AddForce(const vpt : TVerletProgressTimes); virtual; abstract;
401
// Notifies removal of a node
402
procedure RemoveNode(const aNode : TVerletNode); virtual; abstract;
404
property Owner : TGLVerletWorld read FOwner;
407
// TGLVerletDualForce
409
{ A verlet force that applies to two specified nodes. }410
TGLVerletDualForce = class (TVerletForce)
413
FNodeA, FNodeB : TVerletNode;
417
procedure RemoveNode(const aNode : TVerletNode); override;
419
{ Reference to NodeA. }420
property NodeA : TVerletNode read FNodeA write FNodeA;
421
{ Reference to NodeB. }422
property NodeB : TVerletNode read FNodeB write FNodeB;
427
{ A verlet force that applies to a specified group of nodes. }428
TVerletGroupForce = class (TVerletForce)
431
FNodes : TVerletNodeList;
435
constructor Create(const aOwner : TGLVerletWorld); override;
436
destructor Destroy; override;
438
procedure RemoveNode(const aNode : TVerletNode); override;
440
{ Nodes of the force group, referred, NOT owned. }441
property Nodes : TVerletNodeList read FNodes;
444
// TGLVerletGlobalForce
446
{ A global force (applied to all verlet nodes). }447
TGLVerletGlobalForce = class (TVerletForce)
453
procedure RemoveNode(const aNode : TVerletNode); override;
455
procedure AddForce(const vpt : TVerletProgressTimes); override;
456
procedure AddForceToNode(const aNode : TVerletNode); virtual; abstract;
461
TVerletForceList = class (TList)
464
function GetItems(i : Integer): TVerletForce;
465
procedure SetItems(i : Integer; const Value: TVerletForce);
469
property Items[i : Integer] : TVerletForce read GetItems write SetItems; default;
476
TUpdateSpacePartion = (uspEveryIteration, uspEveryFrame, uspNever);
477
TCollisionConstraintTypes = (cctEdge, cctNode);
478
TCollisionConstraintTypesSet = set of TCollisionConstraintTypes;
482
TGLVerletWorld = class (TObject)
485
FIterations : Integer;
486
FNodes : TVerletNodeList;
487
FConstraints : TVerletConstraintList;
488
FForces : TVerletForceList;
489
FMaxDeltaTime, FSimTime : Single;
491
FCurrentDeltaTime: Single;
492
FInvCurrentDeltaTime : Single;
493
FSolidEdges: TGLVerletEdgeList;
494
FSpacePartition: TBaseSpacePartition;
495
FCurrentStepCount: Integer;
496
FUpdateSpacePartion: TUpdateSpacePartion;
497
FCollisionConstraintTypes: TCollisionConstraintTypesSet;
498
FConstraintsWithBeforeIterations: TVerletConstraintList;
499
FVerletNodeClass: TVerletNodeClass;
501
FInertaPauseSteps : Integer;
505
procedure AccumulateForces(const vpt : TVerletProgressTimes); virtual;
506
procedure Verlet(const vpt : TVerletProgressTimes); virtual;
507
procedure SatisfyConstraints(const vpt : TVerletProgressTimes); virtual;
509
procedure DoUpdateSpacePartition;
513
constructor Create; virtual;
514
destructor Destroy; override;
516
function AddNode(const aNode : TVerletNode) : Integer;
517
procedure RemoveNode(const aNode : TVerletNode);
519
function AddConstraint(const aConstraint : TVerletConstraint) : Integer;
520
procedure RemoveConstraint(const aConstraint : TVerletConstraint);
522
function AddForce(const aForce : TVerletForce) : Integer;
523
procedure RemoveForce(const aForce : TVerletForce);
525
procedure AddSolidEdge(const aNodeA, aNodeB : TVerletNode);
527
procedure PauseInertia(const IterationSteps : Integer);
529
function CreateOwnedNode(const location : TAffineVector;
530
const aRadius : Single = 0;
531
const aWeight : Single = 1) : TVerletNode;
532
function CreateStick(const aNodeA, aNodeB : TVerletNode;
533
const Slack : Single = 0) : TVCStick;
534
function CreateSpring(const aNodeA, aNodeB : TVerletNode;
535
const aStrength, aDamping : Single; const aSlack : Single = 0) : TVFSpring;
536
function CreateSlider(const aNodeA, aNodeB : TVerletNode;
537
const aSlideDirection : TAffineVector) : TVCSlider;
539
procedure Initialize; dynamic;
540
procedure CreateOctree(const OctreeMin, OctreeMax : TAffineVector;
541
const LeafThreshold, MaxTreeDepth : Integer);
543
function Progress(const deltaTime, newTime : Double) : Integer; virtual;
545
function FirstNode : TVerletNode;
546
function LastNode : TVerletNode;
548
property Drag : Single read FDrag write FDrag;
549
property Iterations : Integer read FIterations write FIterations;
550
property Nodes : TVerletNodeList read FNodes;
551
property Constraints : TVerletConstraintList read FConstraints;
552
property ConstraintsWithBeforeIterations : TVerletConstraintList read FConstraintsWithBeforeIterations;
554
property SimTime : Single read FSimTime write FSimTime;
555
property MaxDeltaTime : Single read FMaxDeltaTime write FMaxDeltaTime;
557
property CurrentDeltaTime : Single read FCurrentDeltaTime;
558
property SolidEdges : TGLVerletEdgeList read FSolidEdges write FSolidEdges;
559
property CurrentStepCount : Integer read FCurrentStepCount;
560
property SpacePartition: TBaseSpacePartition read FSpacePartition;
561
property UpdateSpacePartion : TUpdateSpacePartion read FUpdateSpacePartion write FUpdateSpacePartion;
562
property CollisionConstraintTypes : TCollisionConstraintTypesSet read FCollisionConstraintTypes write FCollisionConstraintTypes;
563
property VerletNodeClass : TVerletNodeClass read FVerletNodeClass write FVerletNodeClass;
564
property Inertia : boolean read FInertia write FInertia;
569
TVFGravity = class(TGLVerletGlobalForce)
572
FGravity : TAffineVector;
576
constructor Create(const aOwner : TGLVerletWorld); override;
578
procedure AddForceToNode(const aNode : TVerletNode); override;
580
property Gravity : TAffineVector read FGravity write FGravity;
585
TVFAirResistance = class(TGLVerletGlobalForce)
589
FWindDirection: TAffineVector;
590
FWindMagnitude: Single;
592
procedure SetWindDirection(const Value: TAffineVector);
596
constructor Create(const aOwner : TGLVerletWorld); override;
597
procedure AddForceToNode(const aNode : TVerletNode); override;
599
property DragCoeff : Single read FDragCoeff write FDragCoeff;
600
property WindDirection : TAffineVector read FWindDirection write SetWindDirection;
601
property WindMagnitude : Single read FWindMagnitude write FWindMagnitude;
602
{ Measures how chaotic the wind is, as a fraction of the wind magnitude }603
property WindChaos : Single read FWindChaos write FWindChaos;
608
TVFSpring = class (TGLVerletDualForce)
611
FRestLength : Single;
615
FForceFactor : Single;
619
procedure SetSlack(const value : Single);
623
procedure AddForce(const vpt : TVerletProgressTimes); override;
625
// Must be invoked after adjust node locations or strength
626
procedure SetRestLengthToCurrent;
628
property Strength : Single read FStrength write FStrength;
629
property Damping : Single read FDamping write FDamping;
630
property Slack : Single read FSlack write SetSlack;
635
{ Floor collision constraint }636
TVCFloor = class (TGLVerletGlobalFrictionConstraintSP)
639
FBounceRatio, FFloorLevel : Single;
640
FNormal : TAffineVector;
644
procedure SetNormal(const value : TAffineVector);
648
constructor Create(const aOwner : TGLVerletWorld); override;
650
procedure PerformSpaceQuery; override;
651
procedure SatisfyConstraintForNode(const aNode : TVerletNode;
652
const iteration, maxIterations : Integer); override;
654
property BounceRatio : Single read FBounceRatio write FBounceRatio;
655
property FloorLevel : Single read FFloorLevel write FFloorLevel;
656
property Normal : TAffineVector read FNormal write SetNormal;
659
TVCHeightField = class;
660
TVCHeightFieldOnNeedHeight = function (hfConstraint : TVCHeightField; node : TVerletNode) : Single of object;
664
{ HeightField collision constraint (punctual!) }665
TVCHeightField = class (TVCFloor)
668
FOnNeedHeight : TVCHeightFieldOnNeedHeight;
672
procedure SatisfyConstraintForNode(const aNode : TVerletNode;
673
const iteration, maxIterations : Integer); override;
675
property OnNeedHeight : TVCHeightFieldOnNeedHeight read FOnNeedHeight write FOnNeedHeight;
681
Imposes a fixed distance between two nodes. }
682
TVCStick = class (TGLVerletDualConstraint)
686
FRestLength : Single;
690
procedure SatisfyConstraint(const iteration, maxIterations : Integer); override;
691
procedure SetRestLengthToCurrent;
693
property Slack : Single read FSlack write FSlack;
694
property RestLength : Single read FRestLength write FRestLength;
699
{ Rigid body constraint.700
Regroups several nodes in a rigid body conformation, somewhat similar
701
to a stick but for multiple nodes.
702
EXPERIMENTAL, DOES NOT WORK!
704
TVCRigidBody = class (TVerletGroupConstraint)
707
FNodeParams : array of TAffineVector;
708
FNodeCoords : array of TAffineVector;
709
FNatMatrix, FInvNatMatrix : TAffineMatrix;
713
procedure ComputeBarycenter(var barycenter : TAffineVector);
714
procedure ComputeNaturals(const barycenter : TAffineVector;
715
var natX, natY, natZ : TAffineVector);
719
procedure ComputeRigidityParameters;
720
procedure SatisfyConstraint(const iteration, maxIterations : Integer); override;
726
Imposes that two nodes be aligned on a defined direction, on which they
727
can slide freely. Note that the direction is fixed and won't rotate
728
with the verlet assembly!. }
729
TVCSlider = class (TGLVerletDualConstraint)
732
FSlideDirection : TAffineVector;
733
FConstrained : Boolean;
737
procedure SetSlideDirection(const value : TAffineVector);
741
procedure SatisfyConstraint(const iteration, maxIterations : Integer); override;
743
property SlideDirection : TAffineVector read FSlideDirection write SetSlideDirection;
744
{ Constrain NodeB to the halfplane defined by NodeA and SlideDirection. } 745
property Constrained : Boolean read FConstrained write FConstrained;
750
{ Sphere collision constraint. }751
TVCSphere = class (TGLVerletGlobalFrictionConstraintSphere)
758
function GetBSphere : TBSphere; override;
759
procedure SatisfyConstraintForNode(const aNode : TVerletNode;
760
const iteration, maxIterations : Integer); override;
762
procedure SatisfyConstraintForEdge(const aEdge : TGLVerletEdge;
763
const iteration, maxIterations : Integer); override;
765
property Radius : Single read FRadius write FRadius;
770
{ Cylinder collision constraint.771
The cylinder is considered infinite by this constraint. }
772
TVCCylinder = class (TGLVerletGlobalFrictionConstraint)
775
FAxis : TAffineVector;
776
FRadius, FRadius2 : Single;
780
procedure SetRadius(const val : Single);
784
procedure SatisfyConstraintForNode(const aNode : TVerletNode;
785
const iteration, maxIterations : Integer); override;
787
{ A base point on the cylinder axis.788
Can theoretically be anywhere, however, to reduce floating point
789
precision issues, choose it in the area where collision detection
791
//property Base : TAffineVector read FBase write FBase;
792
{ Cylinder axis vector.793
Must be normalized. }
794
property Axis : TAffineVector read FAxis write FAxis;
796
property Radius : Single read FRadius write SetRadius;
801
{ Cube collision constraint. }802
TVCCube = class (TGLVerletGlobalFrictionConstraintBox)
805
FHalfSides : TAffineVector;
806
FSides: TAffineVector;
807
FDirection: TAffineVector;
808
procedure SetSides(const Value: TAffineVector);
812
function GetAABB : TAABB; override;
814
procedure SatisfyConstraintForNode(const aNode : TVerletNode;
815
const iteration, maxIterations : Integer); override;
817
// Broken and very slow!
818
procedure SatisfyConstraintForEdge(const aEdge : TGLVerletEdge;
819
const iteration, maxIterations : Integer); override;//}
821
property Direction : TAffineVector read FDirection write FDirection;
822
property Sides : TAffineVector read FSides write SetSides;
827
{ Capsule collision constraint. }828
TVCCapsule = class (TGLVerletGlobalFrictionConstraintSphere)
831
FAxis : TAffineVector;
832
FRadius, FRadius2, FLength, FLengthDiv2 : Single;
836
procedure SetAxis(const val : TAffineVector);
837
procedure SetRadius(const val : Single);
838
procedure SetLength(const val : Single);
842
function GetBSphere: TBSphere; override;
844
procedure SatisfyConstraintForNode(const aNode : TVerletNode;
845
const iteration, maxIterations : Integer); override;
847
procedure SatisfyConstraintForEdge(const aEdge : TGLVerletEdge;
848
const iteration, maxIterations : Integer); override;
850
// property Base : TAffineVector read FBase write FBase;
851
property Axis : TAffineVector read FAxis write SetAxis;
852
property Radius : Single read FRadius write SetRadius;
853
property Length : Single read FLength write SetLength;
857
// ------------------------------------------------------------------
858
// ------------------------------------------------------------------
859
// ------------------------------------------------------------------
861
// ------------------------------------------------------------------
862
// ------------------------------------------------------------------
863
// ------------------------------------------------------------------
865
// ------------------ TVerletNode ------------------
870
constructor TVerletNode.CreateOwned(const aOwner : TGLVerletWorld);
873
inherited CreateOwned(aOwner.SpacePartition);
874
if Assigned(aOwner) then
875
aOwner.AddNode(Self);
885
destructor TVerletNode.Destroy;
887
if Assigned(FOwner) then
888
FOwner.RemoveNode(Self);
895
{ TODO: Improve the friction calculations897
Friction = - NormalForce * FrictionConstant
899
To compute the NormalForce, which is the force acting on the normal of the
900
collider, we can use the fact that F = m*a.
902
m is the weight of the node, a is the acceleration (retardation) caused by the
905
Acceleration := - PenetrationDepth / Owner.FCurrentDeltaTime;
907
The force with which the node has been "stopped" from penetration
908
NormalForce := Weight * Acceleration;
910
This force should be applied to stopping the movement.
912
procedure TVerletNode.ApplyFriction(const friction, penetrationDepth : Single;
913
const surfaceNormal : TAffineVector);
915
frictionMove, move, moveNormal : TAffineVector;
916
realFriction : Single;
918
if (penetrationDepth>0) then begin
919
realFriction := friction*FFriction;
920
if realFriction>0 then begin
921
VectorSubtract(Location, OldLocation, move);
922
moveNormal:=VectorScale(surfaceNormal, VectorDotProduct(move, surfaceNormal));
923
frictionMove:=VectorSubtract(move, moveNormal);
924
if penetrationDepth>Radius then
925
ScaleVector(frictionMove, realFriction)
926
else ScaleVector(frictionMove, realFriction*Sqrt(penetrationDepth/Radius));
927
VectorAdd(OldLocation, frictionMove, FOldLocation);
934
procedure TVerletNode.OldApplyFriction(const friction, penetrationDepth : Single);
936
frictionMove, move : TAffineVector;
939
VectorSubtract(Location, OldLocation, move);
940
VectorScale(move, friction*FFriction, frictionMove);
941
//pd:=Abs(penetrationDepth);
942
//ScaleVector(frictionMove, friction*pd);
943
VectorAdd(OldLocation, frictionMove, FOldLocation);
948
function TVerletNode.DistanceToNode(const node : TVerletNode) : Single;
950
Result:=VectorDistance(Location, node.Location);
955
function TVerletNode.GetMovement : TAffineVector;
957
Result:=VectorSubtract(Location, OldLocation);
962
procedure TVerletNode.Initialize;
964
FOldLocation:=Location;
969
procedure TVerletNode.SetWeight(const value : Single);
979
procedure TVerletNode.Verlet(const vpt : TVerletProgressTimes);
981
newLocation, temp, move, accel : TAffineVector;
983
if NailedDown then begin
984
FOldLocation:=Location;
986
if Owner.Inertia then begin
988
VectorSubtract(Location, OldLocation, move);
990
ScaleVector(move, 1-Owner.Drag);//*Sqr(deltaTime));
992
VectorAdd(Location, move, newLocation);
994
VectorScale(Force, vpt.sqrDeltaTime*FInvWeight, accel);
995
AddVector(newLocation, accel);
997
Location:=newLocation;
1000
newLocation := Location;
1001
VectorScale(Force, vpt.sqrDeltaTime*FInvWeight, accel);
1002
AddVector(newLocation, accel);
1004
Location := newLocation;
1005
FOldLocation:=Location;
1012
procedure TVerletNode.AfterProgress;
1014
// nothing here, reserved for subclass use
1017
// ------------------
1018
// ------------------ TVerletNodeList ------------------
1019
// ------------------
1023
function TVerletNodeList.GetItems(i : Integer) : TVerletNode;
1030
procedure TVerletNodeList.SetItems(i : Integer; const value : TVerletNode);
1035
function TVerletNode.GetSpeed: TAffineVector;
1037
result := VectorScale(VectorSubtract(FLocation, FOldLocation), 1/Owner.CurrentDeltaTime);
1040
// ------------------
1041
// ------------------ TVerletConstraint ------------------
1042
// ------------------
1046
constructor TVerletConstraint.Create(const aOwner : TGLVerletWorld);
1049
if Assigned(aOwner) then
1050
aOwner.AddConstraint(Self);
1056
destructor TVerletConstraint.Destroy;
1058
if Assigned(FOwner) then
1059
FOwner.RemoveConstraint(Self);
1065
procedure TVerletConstraint.BeforeIterations;
1070
// ------------------
1071
// ------------------ TGLVerletDualConstraint ------------------
1072
// ------------------
1076
procedure TGLVerletDualConstraint.RemoveNode(const aNode : TVerletNode);
1078
if FNodeA=aNode then
1080
if FNodeB=aNode then
1082
if (FNodeA=nil) and (FNodeA=nil) then
1086
// ------------------
1087
// ------------------ TVerletGroupConstraint ------------------
1088
// ------------------
1092
constructor TVerletGroupConstraint.Create(const aOwner : TGLVerletWorld);
1094
inherited Create(aOwner);
1095
FNodes:=TVerletNodeList.Create;
1100
destructor TVerletGroupConstraint.Destroy;
1108
procedure TVerletGroupConstraint.RemoveNode(const aNode : TVerletNode);
1110
FNodes.Remove(aNode);
1113
// ------------------
1114
// ------------------ TGLVerletGlobalConstraint ------------------
1115
// ------------------
1120
procedure TGLVerletGlobalConstraint.AddKickbackForceAt(const Pos : TAffineVector; const Force: TAffineVector);
1122
dPos : TAffineVector;
1125
AddVector(FKickbackForce, Force);
1128
dPos := VectorSubtract(Pos, FLocation);
1129
AddVector(FKickbackTorque, VectorCrossProduct(dPos, Force));
1132
function TGLVerletGlobalConstraint.TranslateKickbackTorque(
1133
const TorqueCenter: TAffineVector): TAffineVector;
1135
// EM(b) = EM(a) + EF x VectorSubtract(b, a).
1136
Result := VectorAdd(FKickbackTorque, VectorCrossProduct(VectorSubtract(TorqueCenter, FLocation), FKickbackForce));
1139
procedure TGLVerletGlobalConstraint.BeforeIterations;
1142
FKickbackForce := NullVector;
1143
FKickbackTorque := NullVector;
1146
procedure TGLVerletGlobalConstraint.RemoveNode(const aNode : TVerletNode);
1148
// nothing to do here
1153
procedure TGLVerletGlobalConstraint.SetLocation(const Value: TAffineVector);
1160
procedure TGLVerletGlobalConstraint.SatisfyConstraint(const iteration, maxIterations : Integer);
1165
if cctNode in Owner.CollisionConstraintTypes then
1166
for i:=0 to Owner.Nodes.Count-1 do begin
1167
node:=TVerletNode(Owner.Nodes[i]);
1168
if not node.NailedDown then
1169
SatisfyConstraintForNode(node, iteration, maxIterations);
1172
if cctEdge in Owner.CollisionConstraintTypes then
1173
for i:=0 to Owner.SolidEdges.Count-1 do begin
1174
SatisfyConstraintForEdge(Owner.SolidEdges[i], iteration, maxIterations);
1178
// SatisfyConstraintForEdge
1180
procedure TGLVerletGlobalConstraint.SatisfyConstraintForEdge(
1181
const aEdge: TGLVerletEdge; const iteration, maxIterations: Integer);
1183
// Purely virtual, but can't be abstract...
1186
// ------------------
1187
// ------------------ TGLVerletGlobalFrictionConstraint ------------------
1188
// ------------------
1192
constructor TGLVerletGlobalFrictionConstraint.Create(const aOwner: TGLVerletWorld);
1195
FFrictionRatio:=cDEFAULT_CONSTRAINT_FRICTION;
1198
// ------------------
1199
// ------------------ TGLVerletGlobalFrictionConstraintSP ------------------
1200
// ------------------
1204
procedure TGLVerletGlobalFrictionConstraintSP.SatisfyConstraint(
1205
const iteration, maxIterations: Integer);
1209
edge : TGLVerletEdge;
1210
SP : TBaseSpacePartition;
1211
Leaf : TSpacePartitionLeaf;
1213
if Owner.SpacePartition=nil then begin
1219
SP := Owner.SpacePartition;
1221
for i:=0 to SP.QueryResult.Count-1 do begin
1222
Leaf:=SP.QueryResult[i];
1223
if Leaf is TVerletNode then begin
1224
if cctNode in Owner.CollisionConstraintTypes then begin
1225
node := Leaf as TVerletNode;
1226
if not node.NailedDown then
1227
SatisfyConstraintForNode(node, iteration, maxIterations);
1229
end else if Leaf is TGLVerletEdge then begin
1230
if cctEdge in Owner.CollisionConstraintTypes then begin
1231
edge := Leaf as TGLVerletEdge;
1232
SatisfyConstraintForEdge(edge, iteration, maxIterations);
1234
end else Assert(False, 'Bad objects in list!');
1238
// ------------------
1239
// ------------------ TVerletConstraintList ------------------
1240
// ------------------
1244
function TVerletConstraintList.GetItems(i : Integer) : TVerletConstraint;
1251
procedure TVerletConstraintList.SetItems(i : Integer;
1252
const value : TVerletConstraint);
1257
// ------------------
1258
// ------------------ TVerletForce ------------------
1259
// ------------------
1263
constructor TVerletForce.Create(const aOwner : TGLVerletWorld);
1266
if Assigned(aOwner) then
1267
aOwner.AddForce(Self);
1272
destructor TVerletForce.Destroy;
1274
if Assigned(FOwner) then
1275
FOwner.RemoveForce(Self);
1279
// ------------------
1280
// ------------------ TVerletGroupForce ------------------
1281
// ------------------
1285
constructor TVerletGroupForce.Create(const aOwner : TGLVerletWorld);
1287
inherited Create(aOwner);
1288
FNodes:=TVerletNodeList.Create;
1293
destructor TVerletGroupForce.Destroy;
1301
procedure TVerletGroupForce.RemoveNode(const aNode : TVerletNode);
1303
FNodes.Remove(aNode);
1306
// ------------------
1307
// ------------------ TGLVerletGlobalForce ------------------
1308
// ------------------
1312
procedure TGLVerletGlobalForce.RemoveNode(const aNode : TVerletNode);
1314
// nothing to do here
1319
procedure TGLVerletGlobalForce.AddForce;
1324
for i:=0 to Owner.Nodes.Count-1 do begin
1325
node:=TVerletNode(Owner.Nodes.List[i]);
1326
if not node.NailedDown then
1327
AddForceToNode(node);
1331
// ------------------
1332
// ------------------ TGLVerletDualForce ------------------
1333
// ------------------
1337
procedure TGLVerletDualForce.RemoveNode(const aNode : TVerletNode);
1339
if FNodeA=aNode then
1341
if FNodeB=aNode then
1345
// ------------------
1346
// ------------------ TVerletForceList ------------------
1347
// ------------------
1351
function TVerletForceList.GetItems(i : Integer) : TVerletForce;
1358
procedure TVerletForceList.SetItems(i : Integer; const value : TVerletForce);
1363
// ------------------
1364
// ------------------ TGLVerletWorld ------------------
1365
// ------------------
1369
constructor TGLVerletWorld.Create;
1373
FNodes:=TVerletNodeList.Create;
1374
FConstraints:=TVerletConstraintList.Create;
1375
FConstraintsWithBeforeIterations:=TVerletConstraintList.Create;
1376
FForces:=TVerletForceList.Create;
1377
FMaxDeltaTime:=0.02;
1379
FSolidEdges := TGLVerletEdgeList.Create;
1380
FCurrentStepCount := 0;
1381
FUpdateSpacePartion := uspNever;
1382
FCollisionConstraintTypes := [cctNode, cctEdge];
1383
FSpacePartition := nil;
1384
FVerletNodeClass := TVerletNode;
1390
destructor TGLVerletWorld.Destroy;
1395
for i:=0 to FNodes.Count-1 do with FNodes[i] do begin
1400
// Delete all constraints
1401
for i:=0 to FConstraints.Count-1 do with FConstraints[i] do begin
1405
FreeAndNil(FConstraints);
1406
// Delete all forces
1407
for i:=0 to FForces.Count-1 do with FForces[i] do begin
1411
FreeAndNil(FForces);
1412
FreeAndNil(FConstraintsWithBeforeIterations);
1414
for i := 0 to FSolidEdges.Count-1 do
1415
FSolidEdges[i].Free;
1416
FreeAndNil(FSolidEdges);
1418
FreeAndNil(FSpacePartition);
1425
procedure TGLVerletWorld.AccumulateForces(const vpt : TVerletProgressTimes);
1429
// First of all, reset all forces
1430
for i:=0 to FNodes.Count-1 do
1431
FNodes[i].FForce:=NullVector;
1432
// Now, update all forces in the assembly!
1433
for i:=0 to FForces.Count-1 do
1434
FForces[i].AddForce(vpt);
1439
function TGLVerletWorld.AddNode(const aNode : TVerletNode) : Integer;
1441
if Assigned(aNode.FOwner) then
1442
aNode.Owner.FNodes.Remove(aNode);
1443
Result:=FNodes.Add(aNode);
1449
procedure TGLVerletWorld.RemoveNode(const aNode : TVerletNode);
1453
if aNode.Owner=Self then begin
1454
FNodes.Remove(aNode);
1456
// drop refs in constraints
1457
for i:=FConstraints.Count-1 downto 0 do
1458
FConstraints[i].RemoveNode(aNode);
1459
// drop refs in forces
1460
for i:=FForces.Count-1 downto 0 do
1461
FForces[i].RemoveNode(aNode);
1467
function TGLVerletWorld.AddConstraint(const aConstraint : TVerletConstraint) : Integer;
1469
if Assigned(aConstraint.FOwner) then
1470
aConstraint.Owner.FConstraints.Remove(aConstraint);
1471
Result:=FConstraints.Add(aConstraint);
1472
aConstraint.FOwner:=Self;
1477
procedure TGLVerletWorld.RemoveConstraint(const aConstraint : TVerletConstraint);
1479
if aConstraint.Owner=Self then begin
1480
FConstraints.Remove(aConstraint);
1481
aConstraint.FOwner:=nil;
1487
function TGLVerletWorld.AddForce(const aForce : TVerletForce) : Integer;
1489
if Assigned(aForce.FOwner) then
1490
aForce.Owner.FForces.Remove(aForce);
1491
Result:=FForces.Add(aForce);
1492
aForce.FOwner:=Self;
1497
procedure TGLVerletWorld.RemoveForce(const aForce : TVerletForce);
1499
if aForce.Owner=Self then begin
1500
FForces.Remove(aForce);
1507
procedure TGLVerletWorld.AddSolidEdge(const aNodeA, aNodeB: TVerletNode);
1509
VerletEdge : TGLVerletEdge;
1511
VerletEdge := TGLVerletEdge.CreateEdgeOwned(aNodeA, aNodeB);
1512
SolidEdges.Add(VerletEdge);
1517
function TGLVerletWorld.FirstNode : TVerletNode;
1519
Assert(FNodes.Count>0, 'There are no nodes in the assembly!');
1525
function TGLVerletWorld.LastNode : TVerletNode;
1527
Assert(FNodes.Count>0, 'There are no nodes in the assembly!');
1528
Result:=FNodes[FNodes.Count-1];
1533
function TGLVerletWorld.CreateOwnedNode(const location : TAffineVector;
1534
const aRadius : Single = 0; const aWeight : Single=1) : TVerletNode;
1536
Result:=VerletNodeClass.CreateOwned(self);
1537
Result.Location:=Location;
1538
Result.OldLocation:=Location;
1539
Result.Weight:=aWeight;
1540
Result.Radius:=aRadius;
1545
function TGLVerletWorld.CreateStick(const aNodeA, aNodeB : TVerletNode; const Slack : Single = 0) : TVCStick;
1547
Assert(aNodeA <> aNodeB, 'Can''t create stick between same node!');
1548
Result:=TVCStick.Create(Self);
1549
Result.NodeA:=aNodeA;
1550
Result.NodeB:=aNodeB;
1551
Result.SetRestLengthToCurrent;
1552
Result.Slack := Slack;
1557
function TGLVerletWorld.CreateSpring(const aNodeA, aNodeB : TVerletNode;
1558
const aStrength, aDamping : Single; const aSlack : Single = 0) : TVFSpring;
1560
Result:=TVFSpring.Create(Self);
1561
Result.NodeA:=aNodeA;
1562
Result.NodeB:=aNodeB;
1563
Result.Strength:=aStrength;
1564
Result.Damping:=aDamping;
1565
Result.Slack:=aSlack;
1566
Result.SetRestLengthToCurrent;
1571
function TGLVerletWorld.CreateSlider(const aNodeA, aNodeB : TVerletNode;
1572
const aSlideDirection : TAffineVector) : TVCSlider;
1574
Result:=TVCSlider.Create(Self);
1575
Result.NodeA:=aNodeA;
1576
Result.NodeB:=aNodeB;
1577
Result.SlideDirection:=aSlideDirection;
1582
procedure TGLVerletWorld.Initialize;
1586
for i:=0 to FNodes.Count-1 do
1587
FNodes[i].Initialize;
1592
function TGLVerletWorld.Progress(const deltaTime, newTime : Double) : Integer;
1596
myDeltaTime : Single;
1597
vpt : TVerletProgressTimes;
1600
myDeltaTime:=FMaxDeltaTime;
1601
FCurrentDeltaTime:=FMaxDeltaTime;
1602
FInvCurrentDeltaTime:=1/FCurrentDeltaTime;
1604
vpt.deltaTime:=myDeltaTime;
1605
vpt.sqrDeltaTime:=Sqr(myDeltaTime);
1606
vpt.invSqrDeltaTime:=1/vpt.sqrDeltaTime;
1608
while FSimTime<newTime do begin
1610
FSimTime:=FSimTime+myDeltaTime;
1611
vpt.newTime:=FSimTime;
1613
AccumulateForces(vpt);
1614
SatisfyConstraints(vpt);
1616
if FInertaPauseSteps>0 then
1618
dec(FInertaPauseSteps);
1619
if FInertaPauseSteps=0 then
1628
for i:=0 to FNodes.Count-1 do
1629
FNodes[i].AfterProgress;
1632
// DoUpdateSpacePartition
1634
procedure TGLVerletWorld.DoUpdateSpacePartition;
1638
if Assigned(SpacePartition) then
1640
for i:=0 to FSolidEdges.Count-1 do
1641
if (FSolidEdges[i].FNodeA.FChangedOnStep=FCurrentStepCount) or
1642
(FSolidEdges[i].FNodeB.FChangedOnStep=FCurrentStepCount) then
1643
FSolidEdges[i].Changed;
1645
for i:=0 to FNodes.Count-1 do
1646
if (FNodes[i].FChangedOnStep=FCurrentStepCount) then
1651
// SatisfyConstraints
1653
procedure TGLVerletWorld.SatisfyConstraints(const vpt : TVerletProgressTimes);
1656
Constraint : TVerletConstraint;
1658
for i:=0 to FConstraintsWithBeforeIterations.Count-1 do
1660
Constraint := FConstraintsWithBeforeIterations[i];
1661
Constraint.BeforeIterations;
1664
if UpdateSpacePartion=uspEveryFrame then
1665
inc(FCurrentStepCount);
1667
for j:=0 to Iterations-1 do
1669
for i:=0 to FConstraints.Count-1 do with FConstraints[i] do
1671
SatisfyConstraint(j, Iterations);//}
1673
if UpdateSpacePartion=uspEveryIteration then
1674
DoUpdateSpacePartition;
1677
if UpdateSpacePartion=uspEveryFrame then
1678
DoUpdateSpacePartition;//}
1683
procedure TGLVerletWorld.Verlet(const vpt : TVerletProgressTimes);
1687
if UpdateSpacePartion<>uspNever then
1688
inc(FCurrentStepCount);
1690
for i:=0 to FNodes.Count-1 do
1691
FNodes[i].Verlet(vpt);
1693
if UpdateSpacePartion<>uspNever then
1694
DoUpdateSpacePartition;
1697
// ------------------
1698
// ------------------ TVFGravity ------------------
1699
// ------------------
1703
constructor TVFGravity.Create(const aOwner : TGLVerletWorld);
1707
FGravity.V[1]:=-9.81;
1713
procedure TVFGravity.AddForceToNode(const aNode : TVerletNode);
1715
CombineVector(aNode.FForce, Gravity, @aNode.Weight);
1718
// ------------------
1719
// ------------------ TVFSpring ------------------
1720
// ------------------
1724
procedure TVFSpring.SetSlack(const value : Single);
1733
procedure TVFSpring.AddForce;
1735
hTerm, dTerm : Single;
1736
deltaV, force : TAffineVector;
1737
deltaLength : Single;
1739
VectorSubtract(NodeA.Location, NodeB.Location, force);
1740
deltaLength:=VectorLength(force);
1742
if deltaLength>FSlack then begin
1743
hTerm:=(FRestLength-deltaLength)*FForceFactor;
1744
force:=VectorScale(force, hTerm/deltaLength);
1745
end else force:=NullVector;
1746
if FDamping<>0 then begin
1747
VectorSubtract(NodeA.GetMovement, NodeB.GetMovement, deltaV);
1748
dTerm:=-0.25*FDamping*vpt.invSqrDeltaTime;
1749
CombineVector(force, deltaV, dTerm);
1752
AddVector(NodeA.FForce, force);
1753
SubtractVector(NodeB.FForce, force);
1756
// SetRestLengthToCurrent
1758
procedure TVFSpring.SetRestLengthToCurrent;
1760
FRestLength:=VectorDistance(NodeA.Location, NodeB.Location);
1761
FForceFactor:=FStrength/FRestLength;
1764
// ------------------
1765
// ------------------ TVFAirResistance ------------------
1766
// ------------------
1768
procedure TVFAirResistance.AddForceToNode(const aNode: TVerletNode);
1770
s, F, FCurrentWindBurst : TAffineVector;
1777
if FWindMagnitude<>0 then
1779
Chaos := FWindMagnitude * FWindChaos;
1780
FCurrentWindBurst.V[0] := FWindDirection.V[0] * FWindMagnitude + Chaos * (random-0.5) * 2;
1781
FCurrentWindBurst.V[1] := FWindDirection.V[1] * FWindMagnitude + Chaos * (random-0.5) * 2;
1782
FCurrentWindBurst.V[2] := FWindDirection.V[2] * FWindMagnitude + Chaos * (random-0.5) * 2;
1784
s := VectorSubtract(s, FCurrentWindBurst);
1787
sMag := VectorLength(s);
1789
r := aNode.Radius + 1;
1793
F := VectorScale(s, - sqr(sMag) * sqr(r) * pi * FDragCoeff);
1795
aNode.FForce := VectorAdd(aNode.FForce, F);
1799
constructor TVFAirResistance.Create(const aOwner: TGLVerletWorld);
1803
FDragCoeff := 0.001;
1804
FWindDirection.V[0] := 0;
1805
FWindDirection.V[1] := 0;
1806
FWindDirection.V[2] := 0;
1807
FWindMagnitude := 0;
1811
procedure TVFAirResistance.SetWindDirection(const Value: TAffineVector);
1813
FWindDirection := VectorNormalize(Value);
1816
// ------------------
1817
// ------------------ TVCFloor ------------------
1818
// ------------------
1822
constructor TVCFloor.Create(const aOwner: TGLVerletWorld);
1825
MakeVector(FNormal, 0, 1, 0);
1826
MakeVector(FLocation, 0, 0, 0);
1831
procedure TVCFloor.PerformSpaceQuery;
1833
Owner.SpacePartition.QueryPlane(FLocation, FNormal);
1836
// SatisfyConstraintForNode
1838
procedure TVCFloor.SatisfyConstraintForNode(const aNode : TVerletNode;
1839
const iteration, maxIterations : Integer);
1841
penetrationDepth : Single;
1842
currentPenetrationDepth : Single;
1844
correction : TAffineVector;
1846
currentPenetrationDepth:=-PointPlaneDistance(aNode.Location, FLocation, FNormal)
1847
+aNode.Radius+FFloorLevel;
1849
// Record how far down the node goes
1850
penetrationDepth:=currentPenetrationDepth;
1851
// Correct the node location
1852
if currentPenetrationDepth>0 then begin
1853
correction:=VectorScale(FNormal, currentPenetrationDepth);
1854
if BounceRatio>0 then begin
1855
d:=VectorSubtract(aNode.FLocation, aNode.FOldLocation);
1856
if FrictionRatio>0 then
1857
aNode.ApplyFriction(FrictionRatio, penetrationDepth, FNormal);
1858
AddVector(aNode.FLocation, correction);
1859
aNode.FOldLocation:=VectorAdd(aNode.FLocation, VectorScale(d, BounceRatio));
1861
AddVector(aNode.FLocation, correction);
1862
if FrictionRatio>0 then
1863
aNode.ApplyFriction(FrictionRatio, penetrationDepth, FNormal);
1864
aNode.FChangedOnStep:=Owner.CurrentStepCount;
1871
procedure TVCFloor.SetNormal(const Value: TAffineVector);
1874
NormalizeVector(FNormal);
1877
// ------------------
1878
// ------------------ TVCHeightField ------------------
1879
// ------------------
1881
// SatisfyConstraintForNode
1883
procedure TVCHeightField.SatisfyConstraintForNode(const aNode : TVerletNode;
1884
const iteration, maxIterations : Integer);
1886
penetrationDepth : Single;
1887
currentPenetrationDepth : Single;
1889
correction : TAffineVector;
1891
currentPenetrationDepth:=-PointPlaneDistance(aNode.Location, FLocation, FNormal)+aNode.Radius;
1892
if Assigned(FOnNeedHeight) then
1893
currentPenetrationDepth:=currentPenetrationDepth+FOnNeedHeight(Self, aNode);
1895
// Record how far down the node goes
1896
penetrationDepth:=currentPenetrationDepth;
1897
// Correct the node location
1898
if currentPenetrationDepth>0 then begin
1899
correction:=VectorScale(FNormal, currentPenetrationDepth);
1900
if BounceRatio>0 then begin
1901
d:=VectorSubtract(aNode.FLocation, aNode.FOldLocation);
1902
if FrictionRatio>0 then
1903
aNode.ApplyFriction(FrictionRatio, penetrationDepth, FNormal);
1904
AddVector(aNode.FLocation, correction);
1905
aNode.FOldLocation:=VectorAdd(aNode.FLocation, VectorScale(d, BounceRatio));
1907
AddVector(aNode.FLocation, correction);
1908
if FrictionRatio>0 then
1909
aNode.ApplyFriction(FrictionRatio, penetrationDepth, FNormal);
1910
aNode.FChangedOnStep:=Owner.CurrentStepCount;
1915
// ------------------
1916
// ------------------ TVCStick ------------------
1917
// ------------------
1921
procedure TVCStick.SatisfyConstraint(const iteration, maxIterations : Integer);
1923
delta : TAffineVector;
1925
deltaLength, diff : Single;
1927
cDefaultDelta : TAffineVector = (X:0.01; Y:0; Z:0);
1929
Assert((NodeA<>NodeB), 'The nodes are identical - that causes division by zero!');
1931
VectorSubtract(NodeB.Location, NodeA.Location, delta);
1932
deltaLength:=VectorLength(delta);
1933
// Avoid div by zero!
1934
if deltaLength<1e-3 then begin
1935
delta:=cDefaultDelta;
1939
diff:=(deltaLength-RestLength)/deltaLength;
1941
if Abs(diff)>Slack then begin
1942
r:=1/(NodeA.InvWeight+NodeB.InvWeight);
1944
diff:=(diff+Slack)*r
1945
else diff:=(diff-Slack)*r;
1947
// Take into acount the different weights of the nodes!
1949
if not NodeA.NailedDown then begin
1950
f:=diff*NodeA.InvWeight;
1951
CombineVector(NodeA.FLocation, delta, f);
1952
NodeA.FChangedOnStep := Owner.CurrentStepCount;
1954
if not NodeB.NailedDown then begin
1955
f:=-diff*NodeB.InvWeight;
1956
CombineVector(NodeB.FLocation, delta, f);
1957
NodeB.FChangedOnStep := Owner.CurrentStepCount;
1962
// SetRestLengthToCurrent
1964
procedure TVCStick.SetRestLengthToCurrent;
1966
FRestLength:=VectorDistance(NodeA.Location, NodeB.Location);
1969
// ------------------
1970
// ------------------ TVCRigidBody ------------------
1971
// ------------------
1975
procedure TVCRigidBody.ComputeBarycenter(var barycenter : TAffineVector);
1980
// first we compute the barycenter
1982
barycenter:=NullVector;
1983
for i:=0 to Nodes.Count-1 do with Nodes[i] do begin
1984
CombineVector(barycenter, Location, @Weight);
1985
totWeight:=totWeight+Weight;
1988
ScaleVector(barycenter, 1/totWeight);
1993
procedure TVCRigidBody.ComputeNaturals(const barycenter : TAffineVector;
1994
var natX, natY, natZ : TAffineVector);
1997
delta : TAffineVector;
2002
for i:=0 to Nodes.Count-1 do begin
2003
delta:=VectorSubtract(Nodes[i].Location, barycenter);
2004
CombineVector(natX, delta, FNodeParams[i].V[0]);
2005
CombineVector(natY, delta, FNodeParams[i].V[1]);
2006
CombineVector(natZ, delta, FNodeParams[i].V[2]);
2010
// ComputeRigidityParameters
2012
procedure TVCRigidBody.ComputeRigidityParameters;
2015
barycenter : TAffineVector;
2018
// first we compute the barycenter
2019
ComputeBarycenter(barycenter);
2020
// next the parameters
2021
SetLength(FNodeParams, Nodes.Count);
2022
SetLength(FNodeCoords, Nodes.Count);
2023
for i:=0 to Nodes.Count-1 do begin
2024
FNodeCoords[i]:=VectorSubtract(Nodes[i].Location, barycenter);
2025
d:=Nodes[i].Weight/VectorLength(FNodeCoords[i]);
2026
FNodeParams[i].V[0]:=FNodeCoords[i].V[0]*d;
2027
FNodeParams[i].V[1]:=FNodeCoords[i].V[1]*d;
2028
FNodeParams[i].V[2]:=FNodeCoords[i].V[2]*d;
2031
ComputeNaturals(barycenter, FNatMatrix.V[0], FNatMatrix.V[1], FNatMatrix.V[2]);
2033
FNatMatrix.V[2]:=VectorCrossProduct(FNatMatrix.V[0], FNatMatrix.V[1]);
2034
FNatMatrix.V[1]:=VectorCrossProduct(FNatMatrix.V[2], FNatMatrix.V[0]);
2035
NormalizeVector(FNatMatrix.V[0]);
2036
NormalizeVector(FNatMatrix.V[1]);
2037
NormalizeVector(FNatMatrix.V[2]);
2039
FInvNatMatrix:=FNatMatrix;
2040
// TransposeMatrix(FInvNatMatrix);
2041
InvertMatrix(FInvNatMatrix);
2046
procedure TVCRigidBody.SatisfyConstraint(const iteration, maxIterations : Integer);
2049
barycenter, delta : TAffineVector;
2050
nrjBase, nrjAdjust : TaffineVector;
2051
natural : array [0..2] of TAffineVector;
2052
deltas : array of TAffineVector;
2054
Assert(Nodes.Count=Length(FNodeParams), 'You forgot to call ComputeRigidityParameters!');
2055
// compute the barycenter
2056
ComputeBarycenter(barycenter);
2057
// compute the natural axises
2058
ComputeNaturals(barycenter, natural[0], natural[1], natural[2]);
2060
natural[2]:=VectorCrossProduct(natural[0], natural[1]);
2061
natural[1]:=VectorCrossProduct(natural[2], natural[0]);
2063
NormalizeVector(natural[i]);
2065
natural[0]:=VectorTransform(natural[0], FInvNatMatrix);
2066
natural[1]:=VectorTransform(natural[1], FInvNatMatrix);
2067
natural[2]:=VectorTransform(natural[2], FInvNatMatrix);
2068
// make the natural axises orthonormal, by picking the longest two
2070
vectNorm[i]:=VectorNorm(natural[i]);
2071
if (vectNorm[0]<vectNorm[1]) and (vectNorm[0]<vectNorm[2]) then begin
2072
natural[0]:=VectorCrossProduct(natural[1], natural[2]);
2073
natural[1]:=VectorCrossProduct(natural[2], natural[0]);
2074
end else if (vectNorm[1]<vectNorm[0]) and (vectNorm[1]<vectNorm[2]) then begin
2075
natural[1]:=VectorCrossProduct(natural[2], natural[0]);
2076
natural[2]:=VectorCrossProduct(natural[0], natural[1]);
2078
natural[2]:=VectorCrossProduct(natural[0], natural[1]);
2079
natural[0]:=VectorCrossProduct(natural[1], natural[2]);
2082
// now the axises are back, recompute the position of all points
2083
SetLength(deltas, Nodes.Count);
2084
nrjBase:=NullVector;
2085
for i:=0 to Nodes.Count-1 do begin
2086
nrjBase:=VectorAdd(nrjBase, VectorCrossProduct(VectorSubtract(Nodes[i].Location, barycenter),
2087
Nodes[i].GetMovement));
2090
nrjAdjust:=NullVector;
2091
for i:=0 to Nodes.Count-1 do begin
2092
delta:=VectorCombine3(natural[0], natural[1], natural[2],
2093
FNodeCoords[i].V[0], FNodeCoords[i].V[1], FNodeCoords[i].V[2]);
2094
deltas[i]:=VectorSubtract(VectorAdd(barycenter, delta), Nodes[i].Location);
2095
nrjAdjust:=VectorAdd(nrjBase, VectorCrossProduct(VectorSubtract(Nodes[i].Location, barycenter),
2097
Nodes[i].Location:=VectorAdd(Nodes[i].Location, deltas[i]);
2098
Nodes[i].FOldLocation:=VectorAdd(Nodes[i].FOldLocation, deltas[i]);
2099
// Nodes[i].FOldLocation:=Nodes[i].Location;
2103
deltas[1]:=nrjAdjust;
2106
// ------------------
2107
// ------------------ TVCSlider ------------------
2108
// ------------------
2112
procedure TVCSlider.SetSlideDirection(const value : TAffineVector);
2114
FSlideDirection:=VectorNormalize(value);
2119
procedure TVCSlider.SatisfyConstraint(const iteration, maxIterations : Integer);
2121
delta : TAffineVector;
2123
projB : TAffineVector;
2125
Assert((NodeA<>NodeB), 'The nodes are identical - that causes division by zero!');
2127
// project B in the plane defined by A and SlideDirection
2128
projB:=VectorSubtract(NodeB.Location, NodeA.Location);
2129
f:=VectorDotProduct(projB, SlideDirection);
2130
projB:=VectorCombine(NodeB.Location, SlideDirection, 1, -f);
2131
if Constrained and (f<0) then
2132
NodeB.Location:=projB;
2134
VectorSubtract(projB, NodeA.Location, delta);
2136
// Take into acount the different weights of the nodes!
2137
r:=1/(NodeA.InvWeight+NodeB.InvWeight);
2139
if not NodeA.NailedDown then begin
2140
f:=r*NodeA.InvWeight;
2141
CombineVector(NodeA.FLocation, delta, f);
2142
NodeA.FChangedOnStep:=Owner.CurrentStepCount;
2144
if not NodeB.NailedDown then begin
2145
f:=-r*NodeB.InvWeight;
2146
CombineVector(NodeB.FLocation, delta, f);
2147
NodeB.FChangedOnStep:=Owner.CurrentStepCount;
2151
// ------------------
2152
// ------------------ TVCSphere ------------------
2153
// ------------------
2156
// SatisfyConstraintForEdge
2158
function TVCSphere.GetBSphere: TBSphere;
2160
result.Center := FLocation;
2161
result.Radius := FRadius;
2164
procedure TVCSphere.SatisfyConstraintForEdge(const aEdge: TGLVerletEdge;
2165
const iteration, maxIterations: Integer);
2167
closestPoint, move, delta, contactNormal : TAffineVector;
2168
deltaLength, diff : Single;
2170
// If the edge penetrates the sphere, try pushing the nodes until it no
2172
closestPoint := PointSegmentClosestPoint(FLocation, aEdge.NodeA.FLocation, aEdge.NodeB.FLocation);
2174
// Find the distance between the two
2175
VectorSubtract(closestPoint, Location, delta);
2177
deltaLength := VectorLength(delta);
2179
if deltaLength<Radius then begin
2180
if deltaLength>0 then begin
2181
contactNormal := VectorScale(delta, 1/deltaLength);
2182
aEdge.NodeA.ApplyFriction(FFrictionRatio, Radius-Abs(DeltaLength), contactNormal);
2183
aEdge.NodeB.ApplyFriction(FFrictionRatio, Radius-Abs(DeltaLength), contactNormal);
2186
// Move it outside the sphere!
2187
diff:=(Radius-deltaLength)/deltaLength;
2188
VectorScale(delta, diff, move);
2190
AddVector(aEdge.NodeA.FLocation, move);
2191
AddVector(aEdge.NodeB.FLocation, move);
2193
// Add the force to the kickback
2195
// a = move / deltatime
2198
VectorScale(move, -(aEdge.NodeA.FWeight + aEdge.NodeB.FWeight) * Owner.FInvCurrentDeltaTime));
2200
aEdge.NodeA.FChangedOnStep := Owner.CurrentStepCount;
2201
aEdge.NodeB.FChangedOnStep := Owner.CurrentStepCount;
2205
// SatisfyConstraintForNode
2207
procedure TVCSphere.SatisfyConstraintForNode(const aNode : TVerletNode;
2208
const iteration, maxIterations : Integer);
2210
delta, move, contactNormal : TAffineVector;
2211
deltaLength, diff : Single;
2213
// Find the distance between the two
2214
VectorSubtract(aNode.Location, Location, delta);
2216
// Is it inside the sphere?
2217
deltaLength:=VectorLength(delta)-aNode.Radius;
2218
if Abs(deltaLength)<Radius then begin
2219
if deltaLength>0 then begin
2220
contactNormal := VectorScale(delta, 1/deltaLength);
2221
aNode.ApplyFriction(FFrictionRatio, Radius-Abs(DeltaLength), contactNormal);
2224
// Slow it down - this part should not be fired
2225
aNode.OldApplyFriction(FFrictionRatio, Radius-Abs(DeltaLength));
2227
// Move it outside the sphere!
2228
diff:=(Radius-deltaLength)/deltaLength;
2229
VectorScale(delta, diff, move);
2231
AddVector(aNode.FLocation, move);
2232
aNode.FChangedOnStep := Owner.CurrentStepCount;
2234
// Add the force to the kickback
2236
// a = move / deltatime
2239
VectorScale(move, -aNode.FWeight * Owner.FInvCurrentDeltaTime));
2243
// ------------------
2244
// ------------------ TVCCylinder ------------------
2245
// ------------------
2249
procedure TVCCylinder.SetRadius(const val : Single);
2255
// SatisfyConstraintForNode
2257
procedure TVCCylinder.SatisfyConstraintForNode(const aNode : TVerletNode;
2258
const iteration, maxIterations : Integer);
2260
proj, newLocation, move : TAffineVector;
2261
f, dist2, penetrationDepth : Single;
2263
// Compute projection of node position on the axis
2264
f:=PointProject(aNode.Location, FLocation, FAxis);
2265
proj:=VectorCombine(FLocation, FAxis, 1, f);
2268
dist2:=VectorDistance2(proj, aNode.Location);
2269
if dist2<FRadius2 then begin
2270
// move out of the cylinder
2271
VectorLerp(proj, aNode.Location, FRadius*RSqrt(dist2), newLocation);
2273
move := VectorSubtract(aNode.FLocation, newLocation);
2275
penetrationDepth := VectorLength(Move);
2277
aNode.ApplyFriction(FFrictionRatio, penetrationDepth, VectorScale(move, 1/penetrationDepth));
2279
aNode.FLocation := newLocation;
2280
aNode.FChangedOnStep := Owner.CurrentStepCount;
2284
// ------------------
2285
// ------------------ TVCCube ------------------
2286
// ------------------
2288
function TVCCube.GetAABB:TAABB;
2290
VectorAdd(FLocation, FHalfSides, result.max);
2291
VectorSubtract(FLocation, FHalfSides, result.min);
2294
// BROKEN AND VERY SLOW!
2295
procedure TVCCube.SatisfyConstraintForEdge(const aEdge: TGLVerletEdge;
2296
const iteration, maxIterations: Integer);
2298
Corners : array[0..7] of TAffineVector;
2299
EdgeRelative : array[0..1] of TAffineVector;
2301
shortestMove{, contactNormal} : TAffineVector;2302
shortestDeltaLength : Single;
2304
procedure AddCorner(CornerID : Integer; x,y,z : Single);
2309
MakeVector(Corners[CornerID], FHalfSides.V[0]*x, FHalfSides.V[1]*y, FHalfSides.V[2]*z);
2310
AddVector(Corners[CornerID], FLocation);
2313
procedure TryEdge(Corner0, Corner1 : Integer);
2315
CubeEdgeClosest, aEdgeClosest : TAffineVector;
2316
CenteraEdge, move : TAffineVector;
2317
deltaLength : Single;
2319
SegmentSegmentClosestPoint(
2322
aEdge.NodeA.FLocation,
2323
aEdge.NodeB.FLocation,
2327
CenteraEdge := VectorSubtract(aEdgeClosest, FLocation);
2329
if (abs(CenteraEdge.V[0])<FHalfSides.V[0]) and
2330
(abs(CenteraEdge.V[1])<FHalfSides.V[1]) and
2331
(abs(CenteraEdge.V[2])<FHalfSides.V[2]) then
2333
// The distance to move the edge is the difference between CenterCubeEdge and
2335
move := VectorSubtract(CubeEdgeClosest, aEdgeClosest);
2337
deltaLength := VectorLength(move);
2339
if (deltaLength>0) and (deltaLength<shortestDeltaLength) then
2341
shortestDeltaLength := deltaLength;
2342
shortestMove := move;
2351
EdgeRelative[0] := VectorSubtract(aEdge.FNodeA.FLocation, FLocation);
2352
EdgeRelative[1] := VectorSubtract(aEdge.FNodeB.FLocation, FLocation);
2354
// If both edges are on the same side of _any_ box side, the edge can't
2356
if ((EdgeRelative[0].V[0]> FHalfSides.V[0]) and (EdgeRelative[1].V[0] >FHalfSides.V[0])) or
2357
((EdgeRelative[0].V[0]<-FHalfSides.V[0]) and (EdgeRelative[1].V[0]<-FHalfSides.V[0])) or
2359
((EdgeRelative[0].V[1]> FHalfSides.V[1]) and (EdgeRelative[1].V[1]> FHalfSides.V[1])) or
2360
((EdgeRelative[0].V[1]<-FHalfSides.V[1]) and (EdgeRelative[1].V[1]<-FHalfSides.V[1])) or
2362
((EdgeRelative[0].V[2]> FHalfSides.V[2]) and (EdgeRelative[1].V[2]> FHalfSides.V[2])) or
2363
((EdgeRelative[0].V[2]<-FHalfSides.V[2]) and (EdgeRelative[1].V[2]<-FHalfSides.V[2])) then
2368
// For each cube edge:
2369
// find closest positions between CubeEdge and aEdge
2370
// if aEdgeClosestPosition within cube then
2371
// move nodes until closest position is outside cube
2373
AddCorner(0, 0, 0, 0);
2374
AddCorner(1, 1, 0, 0);
2375
AddCorner(2, 1, 1, 0);
2376
AddCorner(3, 0, 1, 0);
2378
AddCorner(4, 0, 0, 1);
2379
AddCorner(5, 1, 0, 1);
2380
AddCorner(6, 1, 1, 1);
2381
AddCorner(7, 0, 1, 1);
2383
shortestDeltaLength := 10e30;
2400
if shortestDeltaLength<10e8 then
2402
//contactNormal := VectorScale(shortestMove, 1/shortestDeltaLength);
2404
{aEdge.NodeA.ApplyFriction(FFrictionRatio, shortestDeltaLength, contactNormal);2405
aEdge.NodeB.ApplyFriction(FFrictionRatio, shortestDeltaLength, contactNormal);//}
2407
AddVector(aEdge.NodeA.FLocation, shortestMove);
2408
AddVector(aEdge.NodeB.FLocation, shortestMove);//}
2410
aEdge.NodeA.Changed;
2411
aEdge.NodeB.Changed;
2413
aEdge.NodeA.FChangedOnStep := Owner.CurrentStepCount;
2414
aEdge.NodeB.FChangedOnStep := Owner.CurrentStepCount;
2418
procedure TVCCube.SatisfyConstraintForNode(const aNode: TVerletNode;
2419
const iteration, maxIterations: Integer);
2421
p, absP, contactNormal : TAffineVector;
2423
smallestSide : Integer;
2425
// TODO: Direction of Cube should be used to rotate the nodes location, as it
2426
// stands, the cube can only face in one direction.
2428
p:=VectorSubtract(aNode.FLocation, FLocation);
2430
absP.V[0]:=FHalfSides.V[0]-Abs(p.V[0]);
2431
absP.V[1]:=FHalfSides.V[1]-Abs(p.V[1]);
2432
absP.V[2]:=FHalfSides.V[2]-Abs(p.V[2]);
2434
if (PInteger(@absP.V[0])^<=0) or (PInteger(@absP.V[1])^<=0) or(PInteger(@absP.V[2])^<=0) then
2437
if absP.V[0]<absP.V[1] then
2438
if absP.V[0]<absP.V[2] then
2440
else smallestSide:=2
2441
else if absP.V[1]<absP.V[2] then
2443
else smallestSide:=2;
2445
contactNormal:=NullVector;
2447
// Only move along the "shortest" axis
2448
if PInteger(@p.V[smallestSide])^>=0 then begin
2449
dp:=absP.V[smallestSide];
2450
contactNormal.V[smallestSide]:=1;
2451
aNode.ApplyFriction(FFrictionRatio, dp, contactNormal);
2452
aNode.FLocation.V[smallestSide]:=aNode.FLocation.V[smallestSide]+dp;
2454
dp:=absP.V[smallestSide];
2455
contactNormal.V[smallestSide]:=-1;
2456
aNode.ApplyFriction(FFrictionRatio, dp, contactNormal);
2457
aNode.FLocation.V[smallestSide]:=aNode.FLocation.V[smallestSide]-dp;
2460
aNode.FChangedOnStep:=Owner.CurrentStepCount;
2463
procedure TVCCube.SetSides(const Value: TAffineVector);
2466
FHalfSides := VectorScale(Sides, 0.5);
2470
// ------------------
2471
// ------------------ TVCCapsule ------------------
2472
// ------------------
2477
procedure TVCCapsule.SetAxis(const val : TAffineVector);
2479
FAxis:=VectorNormalize(val);
2480
UpdateCachedBSphere;
2485
procedure TVCCapsule.SetLength(const val : Single);
2488
FLengthDiv2:=val*0.5;
2489
UpdateCachedBSphere;
2494
procedure TVCCapsule.SetRadius(const val : Single);
2498
UpdateCachedBSphere;
2503
function TVCCapsule.GetBSphere: TBSphere;
2505
result.Center := FLocation;
2506
result.Radius := Length + Radius;
2509
// SatisfyConstraintForNode
2511
procedure TVCCapsule.SatisfyConstraintForNode(const aNode : TVerletNode;
2512
const iteration, maxIterations : Integer);
2514
p, n2, penetrationDepth : Single;
2515
closest, v : TAffineVector;
2516
newLocation, move : TAffineVector;
2519
// Find the closest point to location on the capsule axis
2520
p:=ClampValue(PointProject(aNode.Location, FLocation, FAxis),
2521
-FLengthDiv2, FLengthDiv2);
2522
closest:=VectorCombine(FLocation, FAxis, 1, p);
2524
// vector from closest to location
2525
VectorSubtract(aNode.Location, closest, v);
2527
// should it be altered?
2532
newLocation := VectorCombine(closest, v, 1, Sqrt(FRadius2/n2));
2534
// Do friction calculations
2535
move := VectorSubtract(newLocation,aNode.FLocation);
2536
penetrationDepth := VectorLength(move);
2538
//aNode.OldApplyFriction(FFrictionRatio, penetrationDepth);
2539
aNode.ApplyFriction(FFrictionRatio, penetrationDepth, VectorScale(move, 1/penetrationDepth));
2541
aNode.FLocation:=newLocation;
2542
aNode.FChangedOnStep := Owner.CurrentStepCount;
2546
VectorScale(move, -aNode.FWeight * Owner.FInvCurrentDeltaTime));
2551
procedure TVCCapsule.SatisfyConstraintForEdge(const aEdge: TGLVerletEdge;
2552
const iteration, maxIterations: Integer);
2554
sphereLocation, closestPoint, dummy, delta, move, contactNormal : TAffineVector;
2555
Ax0, Ax1 : TAffineVector;
2556
deltaLength, diff, penetrationDepth : Single;
2558
VectorScale(FAxis, FLengthDiv2, Ax0);
2559
AddVector(Ax0, FLocation);
2560
VectorScale(FAxis, -FLengthDiv2, Ax1);
2561
AddVector(Ax1, FLocation);
2563
SegmentSegmentClosestPoint(
2564
aEdge.NodeA.FLocation,
2565
aEdge.NodeB.FLocation,
2571
// If the edge penetrates the sphere, try pushing the nodes until it no
2573
closestPoint := PointSegmentClosestPoint(sphereLocation, aEdge.NodeA.FLocation, aEdge.NodeB.FLocation);
2575
// Find the distance between the two
2576
VectorSubtract(closestPoint, sphereLocation, delta);
2578
deltaLength := VectorLength(delta);
2580
if deltaLength<Radius then begin
2581
// Move it outside the sphere!
2582
diff:=(Radius-deltaLength)/deltaLength;
2583
VectorScale(delta, diff, move);
2585
penetrationDepth := VectorLength(move);
2586
contactNormal := VectorScale(move, 1/penetrationDepth);
2587
aEdge.NodeA.ApplyFriction(FFrictionRatio, penetrationDepth, contactNormal);
2588
aEdge.NodeB.ApplyFriction(FFrictionRatio, penetrationDepth, contactNormal);
2590
AddVector(aEdge.NodeA.FLocation, move);
2591
AddVector(aEdge.NodeB.FLocation, move);
2593
aEdge.NodeA.FChangedOnStep := Owner.CurrentStepCount;
2594
aEdge.NodeB.FChangedOnStep := Owner.CurrentStepCount;
2598
VectorScale(move, -(aEdge.NodeA.FWeight + aEdge.NodeB.FWeight) * Owner.FInvCurrentDeltaTime));
2602
// ------------------
2603
// ------------------ TGLVerletEdge ------------------
2604
// ------------------
2608
constructor TGLVerletEdge.CreateEdgeOwned(const aNodeA, aNodeB: TVerletNode);
2613
inherited CreateOwned(aNodeA.Owner.SpacePartition);
2616
// ------------------
2617
// ------------------ TGLVerletEdgeList ------------------
2618
// ------------------
2620
procedure TGLVerletEdge.UpdateCachedAABBAndBSphere;
2622
FCachedAABB.min := FNodeA.FLocation;
2623
FCachedAABB.max := FNodeA.FLocation;
2625
AABBInclude(FCachedAABB, FNodeB.FLocation);
2627
AABBToBSphere(FCachedAABB, FCachedBSphere);
2630
{ TGLVerletEdgeList }2632
function TGLVerletEdgeList.GetItems(i: Integer): TGLVerletEdge;
2637
procedure TGLVerletEdgeList.SetItems(i: Integer; const Value: TGLVerletEdge);
2642
procedure TVerletNode.UpdateCachedAABBAndBSphere;
2644
FCachedAABB.min := FLocation;
2645
FCachedAABB.max := FLocation;
2646
FCachedBSphere.Center := FLocation;
2647
FCachedBSphere.Radius := 0;
2650
procedure TVerletNode.SetLocation(const Value: TAffineVector);
2653
FChangedOnStep := Owner.CurrentStepCount;
2656
procedure TGLVerletWorld.CreateOctree(const OctreeMin,
2657
OctreeMax: TAffineVector; const LeafThreshold, MaxTreeDepth: Integer);
2659
Octree : TOctreeSpacePartition;
2661
Assert(FNodes.Count=0,'You can only create an octree while the world is empty!');
2663
FreeAndNil(FSpacePartition);
2665
Octree := TOctreeSpacePartition.Create;
2667
Octree.SetSize(OctreeMin, OctreeMax);
2668
Octree.MaxTreeDepth := MaxTreeDepth;
2669
Octree.LeafThreshold := LeafThreshold;
2670
Octree.CullingMode := cmGrossCulling;
2672
FSpacePartition := Octree;
2674
if FUpdateSpacePartion = uspNever then
2675
FUpdateSpacePartion := uspEveryFrame;
2678
procedure TGLVerletWorld.PauseInertia(const IterationSteps: Integer);
2680
FInertaPauseSteps := IterationSteps+1;
2684
{ TGLVerletGlobalFrictionConstraintBox }2686
procedure TGLVerletGlobalFrictionConstraintBox.PerformSpaceQuery;
2688
Owner.SpacePartition.QueryAABB(FCachedAABB);
2691
procedure TGLVerletGlobalFrictionConstraintBox.SetLocation(
2692
const Value: TAffineVector);
2699
procedure TGLVerletGlobalFrictionConstraintBox.UpdateCachedAABB;
2701
FCachedAABB := GetAABB;
2704
{ TGLVerletGlobalFrictionConstraintSphere }2706
procedure TGLVerletGlobalFrictionConstraintSphere.PerformSpaceQuery;
2708
Owner.SpacePartition.QueryBSphere(FCachedBSphere);
2711
procedure TGLVerletGlobalFrictionConstraintSphere.SetLocation(
2712
const Value: TAffineVector);
2715
UpdateCachedBSphere;
2718
procedure TGLVerletGlobalFrictionConstraintSphere.UpdateCachedBSphere;
2720
FCachedBSphere := GetBSphere;
2723
constructor TGLVerletGlobalConstraint.Create(const aOwner: TGLVerletWorld);
2726
if Assigned(aOwner) then
2727
aOwner.ConstraintsWithBeforeIterations.Add(self);
2730
destructor TGLVerletGlobalConstraint.Destroy;
2732
if Assigned(Owner) then
2733
Owner.ConstraintsWithBeforeIterations.Remove(self);