Delphi/CppBuilder

开发平台：

Delphi

UPhysics2D.pas：源码内容

// Find support vertex on poly2 for -normal.
index := 0;
minDot := FLT_MAX;
for i := 0 to poly2.GetVertexCount - 1 do
begin
dot := b2Dot(poly2.m_vertices[i], normal1);
if dot < minDot then
begin
minDot := dot;
index := i;
end;
end;
v1 := b2Mul(xf1, poly1.m_vertices[edge1]);
v2 := b2Mul(xf2, poly2.m_vertices[index]);
{$IFDEF OP_OVERLOAD}
Result := b2Dot(v2 - v1, normal1World);
{$ELSE}
Result := b2Dot(Subtract(v2, v1), normal1World);
{$ENDIF}
end;
// Find the max separation between poly1 and poly2 using edge normals from poly1.
function FindMaxSeparation(var edgeIndex: Int32;
poly1, poly2: Tb2PolygonShape; const xf1, xf2: Tb2XForm): Float;
var
i: Integer;
edge, prevEdge, nextEdge, bestEdge, increment: Int32;
d, dLocal1: TVector2;
maxDot, dot, s, sPrev, sNext, bestSeparation: Float;
begin
// Vector pointing from the centroid of poly1 to the centroid of poly2.
{$IFDEF OP_OVERLOAD}
d := b2Mul(xf2, poly2.m_centroid) - b2Mul(xf1, poly1.m_centroid);
{$ELSE}
d := Subtract(b2Mul(xf2, poly2.m_centroid), b2Mul(xf1, poly1.m_centroid));
{$ENDIF}
dLocal1 := b2MulT(xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
edge := 0;
maxDot := -FLT_MAX;
for i := 0 to poly1.GetVertexCount - 1 do
begin
dot := b2Dot(poly1.m_normals[i], dLocal1);
if dot > maxDot then
begin
maxDot := dot;
edge := i;
end;
end;
// Get the separation for the edge normal.
s := EdgeSeparation(poly1, poly2, xf1, xf2, edge);
if s > 0.0 then
begin
Result := s;
Exit;
end;
// Check the separation for the previous edge normal.
if edge - 1 >= 0 then
prevEdge := edge - 1
else
prevEdge := poly1.GetVertexCount - 1;
sPrev := EdgeSeparation(poly1, poly2, xf1, xf2, prevEdge);
if sPrev > 0.0 then
begin
Result := sPrev;
Exit;
end;
// Check the separation for the next edge normal.
if edge + 1 < poly1.GetVertexCount then
nextEdge := edge + 1
else
nextEdge := 0;
sNext := EdgeSeparation(poly1, poly2, xf1, xf2, nextEdge);
if sNext > 0.0 then
begin
Result := sNext;
Exit;
end;
// Find the best edge and the search direction.
if (sPrev > s) and (sPrev > sNext) then
begin
increment := -1;
bestEdge := prevEdge;
bestSeparation := sPrev;
end
else if sNext > s then
begin
increment := 1;
bestEdge := nextEdge;
bestSeparation := sNext;
end
else
begin
edgeIndex := edge;
Result := s;
Exit;
end;
// Perform a local search for the best edge normal.
while True do
begin
if increment = -1 then
begin
if bestEdge - 1 >= 0 then
edge := bestEdge - 1
else
edge := poly1.GetVertexCount - 1;
end
else
if bestEdge + 1 < poly1.GetVertexCount then
edge := bestEdge + 1
else
edge := 0;
s := EdgeSeparation(poly1, poly2, xf1, xf2, edge);
if s > 0.0 then
begin
Result := s;
Exit;
end;
if s > bestSeparation then
begin
bestEdge := edge;
bestSeparation := s;
end
else
Break;
end;
edgeIndex := bestEdge;
Result := bestSeparation;
end;
procedure FindIncidentEdge(c: PClipVertices; poly1, poly2: Tb2PolygonShape;
const xf1, xf2: Tb2XForm; edge1: Int32);
var
i: Integer;
index, i1, i2: Int32;
normal1: TVector2;
minDot, dot: Float;
begin
//b2Assert(0 <= edge1 && edge1 < poly1.GetVertexCount);
// Get the normal of the reference edge in poly2's frame.
normal1 := b2MulT(xf2.R, b2Mul(xf1.R, poly1.m_normals[edge1]));
// Find the incident edge on poly2.
index := 0;
minDot := FLT_MAX;
for i := 0 to poly2.GetVertexCount - 1 do
begin
dot := b2Dot(normal1, poly2.m_normals[i]);
if dot < minDot then
begin
minDot := dot;
index := i;
end;
end;
// Build the clip vertices for the incident edge.
i1 := index;
if i1 + 1 < poly2.GetVertexCount then
i2 := i1 + 1
else
i2 := 0;
c^[0].v := b2Mul(xf2, poly2.m_vertices[i1]);
c^[0].id.referenceEdge := edge1;
c^[0].id.incidentEdge := i1;
c^[0].id.incidentVertex := 0;
c^[1].v := b2Mul(xf2, poly2.m_vertices[i2]);
c^[1].id.referenceEdge := edge1;
c^[1].id.incidentEdge := i2;
c^[1].id.incidentVertex := 1;
end;
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
procedure b2CollidePolygons(var manifold: Tb2Manifold;
polyA, polyB: Tb2PolygonShape; xfA, xfB: Tb2XForm);
var
i: Integer;
edgeA, edgeB: Int32;
edge1: Int32; // reference edge
flip: UInt8;
separationA, separationB: Float;
poly1, poly2: Tb2PolygonShape; // reference poly and incident poly
xf1, xf2: Tb2XForm;
k_relativeTol, k_absoluteTol: Float;
incidentEdge, clipPoints1, clipPoints2: TClipVertices;
v11, v12, sideNormal, frontNormal: TVector2;
frontOffset, sideOffset1, sideOffset2: Float;
np: Int32; // Clip incident edge against extruded edge1 side edges.
pointCount: Int32;
_separation: Float;
begin
manifold.pointCount := 0;
separationA := FindMaxSeparation(edgeA, polyA, polyB, xfA, xfB);
if separationA > 0.0 then
Exit;
edgeB := 0;
separationB := FindMaxSeparation(edgeB, polyB, polyA, xfB, xfA);
if separationB > 0.0 then
Exit;
k_relativeTol := 0.98;
k_absoluteTol := 0.001;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if separationB > k_relativeTol * separationA + k_absoluteTol then
begin
poly1 := polyB;
poly2 := polyA;
xf1 := xfB;
xf2 := xfA;
edge1 := edgeB;
flip := 1;
end
else
begin
poly1 := polyA;
poly2 := polyB;
xf1 := xfA;
xf2 := xfB;
edge1 := edgeA;
flip := 0;
end;
FindIncidentEdge(@incidentEdge, poly1, poly2, xf1, xf2, edge1);
v11 := poly1.m_vertices[edge1];
if edge1 + 1 < poly1.GetVertexCount then
v12 := poly1.m_vertices[edge1+1]
else
v12 := poly1.m_vertices[0];
{$IFDEF OP_OVERLOAD}
sideNormal := b2Mul(xf1.R, v12 - v11);
sideNormal.Normalize;
{$ELSE}
sideNormal := b2Mul(xf1.R, Subtract(v12, v11));
Normalize(sideNormal);
{$ENDIF}
frontNormal := b2Cross(sideNormal, 1.0);
v11 := b2Mul(xf1, v11);
v12 := b2Mul(xf1, v12);
frontOffset := b2Dot(frontNormal, v11);
sideOffset1 := -b2Dot(sideNormal, v11);
sideOffset2 := b2Dot(sideNormal, v12);
// Clip to box side 1
{$IFDEF OP_OVERLOAD}
np := ClipSegmentToLine(@clipPoints1, @incidentEdge, -sideNormal, sideOffset1);
{$ELSE}
np := ClipSegmentToLine(@clipPoints1, @incidentEdge, Negative(sideNormal), sideOffset1);
{$ENDIF}
if np < 2 then
Exit;
// Clip to negative box side 1
np := ClipSegmentToLine(@clipPoints2, @clipPoints1, sideNormal, sideOffset2);
if np < 2 then
Exit;
// Now clipPoints2 contains the clipped points.
if flip <> 0 then
{$IFDEF OP_OVERLOAD}
manifold.normal := -frontNormal
{$ELSE}
manifold.normal := Negative(frontNormal)
{$ENDIF}
else
manifold.normal := frontNormal;
pointCount := 0;
for i := 0 to b2_maxManifoldPoints - 1 do
begin
_separation := b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
if _separation <= 0.0 then
with manifold.points[pointCount] do
begin
separation := _separation;
localPoint1 := b2MulT(xfA, clipPoints2[i].v);
localPoint2 := b2MulT(xfB, clipPoints2[i].v);
id := clipPoints2[i].id;
id.flip := flip;
Inc(pointCount);
end;
end;
manifold.pointCount := pointCount;
end;
{ Tb2CircleDef }
constructor Tb2CircleDef.Create;
begin
inherited Create;
ShapeType := e_circleShape;
SetZero(localPosition);
radius := 1.0;
end;
{ Tb2CircleShape }
constructor Tb2CircleShape.Create(def: Tb2ShapeDef);
var
circleDef: Tb2CircleDef;
begin
inherited Create(def);
//b2Assert(def->type == e_circleShape);
circleDef := Tb2CircleDef(def);
m_type := e_circleShape;
m_localPosition := circleDef.localPosition;
m_radius := circleDef.radius;
end;
procedure Tb2CircleShape.UpdateSweepRadius(const center: TVector2);
begin
// Update the sweep radius (maximum radius) as measured from a local center point.
{$IFDEF OP_OVERLOAD}
m_sweepRadius := (m_localPosition - center).Length() + m_radius - b2_toiSlop;
{$ELSE}
m_sweepRadius := Length(Subtract(m_localPosition, center)) + m_radius - b2_toiSlop;
{$ENDIF}
end;
function Tb2CircleShape.TestPoint(const transform: Tb2XForm; const p: TVector2): Boolean;
var
center, d: TVector2;
begin
{$IFDEF OP_OVERLOAD}
center := transform.position + b2Mul(transform.R, m_localPosition);
d := p - center;
{$ELSE}
center := Add(transform.position, b2Mul(transform.R, m_localPosition));
d := Subtract(p, center);
{$ENDIF}
Result := b2Dot(d, d) <= m_radius * m_radius;
end;
function Tb2CircleShape.TestSegment(const xf: Tb2XForm; var lambda: Float;
var normal: TVector2; const segment: Tb2Segment; maxLambda: Float): Boolean;
var
position, s, r: TVector2;
b, c, rr, sigma, a: Float;
begin
{$IFDEF OP_OVERLOAD}
position := xf.position + b2Mul(xf.R, m_localPosition);
s := segment.p1 - position;
{$ELSE}
position := Add(xf.position, b2Mul(xf.R, m_localPosition));
s := Subtract(segment.p1, position);
{$ENDIF}
b := b2Dot(s, s) - m_radius * m_radius;
// Does the segment start inside the circle?
if b < 0.0 then
begin
Result := False;
Exit;
end;
// Solve quadratic equation.
{$IFDEF OP_OVERLOAD}
r := segment.p2 - segment.p1;
{$ELSE}
r := Subtract(segment.p2, segment.p1);
{$ENDIF}
c := b2Dot(s, r);
rr := b2Dot(r, r);
sigma := c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0) or (rr < FLT_EPSILON) then
begin
Result := False;
Exit;
end;
// Find the point of intersection of the line with the circle.
a := -(c + Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0 <= a) and (a <= maxLambda * rr) then
begin
a := a / rr;
lambda := a;
{$IFDEF OP_OVERLOAD}
normal := s + a * r;
normal.Normalize;
{$ELSE}
normal := Add(s, Multiply(r, a));
Normalize(normal);
{$ENDIF}
Result := True;
end
else
Result := False;
end;
procedure Tb2CircleShape.ComputeAABB(var aabb: Tb2AABB; const xf: Tb2XForm);
var
p: TVector2;
begin
{$IFDEF OP_OVERLOAD}
p := xf.position + b2Mul(xf.R, m_localPosition);
aabb.lowerBound.SetValue(p.x - m_radius, p.y - m_radius);
aabb.upperBound.SetValue(p.x + m_radius, p.y + m_radius);
{$ELSE}
p := Add(xf.position, b2Mul(xf.R, m_localPosition));
SetValue(aabb.lowerBound, p.x - m_radius, p.y - m_radius);
SetValue(aabb.upperBound, p.x + m_radius, p.y + m_radius);
{$ENDIF}
end;
procedure Tb2CircleShape.ComputeSweptAABB(var aabb: Tb2AABB; const xf1, xf2: Tb2XForm);
var
p1, p2, lower, upper: TVector2;
begin
{$IFDEF OP_OVERLOAD}
p1 := xf1.position + b2Mul(xf1.R, m_localPosition);
p2 := xf2.position + b2Mul(xf2.R, m_localPosition);
{$ELSE}
p1 := Add(xf1.position, b2Mul(xf1.R, m_localPosition));
p2 := Add(xf2.position, b2Mul(xf2.R, m_localPosition));
{$ENDIF}
lower := b2Min(p1, p2);
upper := b2Max(p1, p2);
{$IFDEF OP_OVERLOAD}
aabb.lowerBound.SetValue(lower.x - m_radius, lower.y - m_radius);
aabb.upperBound.SetValue(upper.x + m_radius, upper.y + m_radius);
{$ELSE}
SetValue(aabb.lowerBound, lower.x - m_radius, lower.y - m_radius);
SetValue(aabb.upperBound, upper.x + m_radius, upper.y + m_radius);
{$ENDIF}
end;
procedure Tb2CircleShape.ComputeMass(var massData: Tb2MassData);
begin
massData.mass := m_density * Pi * m_radius * m_radius;
massData.center := m_localPosition;
// inertia about the local origin
massData.I := massData.mass * (0.5 * m_radius * m_radius +
b2Dot(m_localPosition, m_localPosition));
end;
{ Tb2PolygonDef }
constructor Tb2PolygonDef.Create;
begin
inherited Create;
ShapeType := e_polygonShape;
vertexCount := 0;
end;
procedure Tb2PolygonDef.SetAsBox(hx, hy: Float);
begin
vertexCount := 4;
{$IFDEF OP_OVERLOAD}
vertices[0].SetValue(-hx, -hy);
vertices[1].SetValue(hx, -hy);
vertices[2].SetValue(hx, hy);
vertices[3].SetValue(-hx, hy);
{$ELSE}
SetValue(vertices[0], -hx, -hy);
SetValue(vertices[1], hx, -hy);
SetValue(vertices[2], hx, hy);
SetValue(vertices[3], -hx, hy);
{$ENDIF}
end;
procedure Tb2PolygonDef.SetAsBox(hx, hy: Float; const center: TVector2; angle: Float);
var
i: Integer;
xf: Tb2XForm;
begin
SetAsBox(hx, hy);
xf.position := center;
{$IFDEF OP_OVERLOAD}
xf.R.SetValue(angle);
{$ELSE}
SetValue(xf.R, angle);
{$ENDIF}
for i := 0 to vertexCount - 1 do
vertices[i] := b2Mul(xf, vertices[i]);
end;
{ Tb2PolygonShape }
constructor Tb2PolygonShape.Create(const def: Tb2ShapeDef);
var
i: Integer;
i2: Int32;
poly: Tb2PolygonDef;
edge, n1, n2, v, d: TVector2;
A: TMatrix22;
begin
inherited Create(def);
pm_vertices := @m_vertices[0];
pm_normals := @m_normals[0];
pm_coreVertices := @m_coreVertices[0];
//b2Assert(def.type == e_polygonShape);
m_type := e_polygonShape;
poly := Tb2PolygonDef(def);
// Get the vertices transformed into the body frame.
m_vertexCount := poly.vertexCount;
//b2Assert(3 <= m_vertexCount && m_vertexCount <= b2_maxPolygonVertices);
// Copy vertices.
m_vertices := poly.vertices;
// Compute normals. Ensure the edges have non-zero length.
for i := 0 to m_vertexCount - 1 do
begin
if i + 1 < m_vertexCount then
i2 := i + 1
else
i2 := 0;
{$IFDEF OP_OVERLOAD}
edge := m_vertices[i2] - m_vertices[i];
//b2Assert(edge.LengthSquared() > B2_FLT_EPSILON * B2_FLT_EPSILON);
m_normals[i] := b2Cross(edge, 1.0);
m_normals[i].Normalize;
{$ELSE}
edge := Subtract(m_vertices[i2], m_vertices[i]);
//b2Assert(edge.LengthSquared() > B2_FLT_EPSILON * B2_FLT_EPSILON);
m_normals[i] := b2Cross(edge, 1.0);
Normalize(m_normals[i]);
{$ENDIF}
end;
// Compute the polygon centroid.
m_centroid := ComputeCentroid(poly.vertices, poly.vertexCount);
// Compute the oriented bounding box.
ComputeOBB(m_obb, m_vertices, m_vertexCount);
// Create core polygon shape by shifting edges inward.
// Also compute the min/max radius for CCD.
for i := 0 to m_vertexCount - 1 do
begin
if i - 1 >= 0 then
i2 := i - 1
else
i2 := m_vertexCount - 1;
n1 := m_normals[i2];
n2 := m_normals[i];
{$IFDEF OP_OVERLOAD}
v := m_vertices[i] - m_centroid;
{$ELSE}
v := Subtract(m_vertices[i], m_centroid);
{$ENDIF}
d.x := b2Dot(n1, v) - b2_toiSlop;
d.y := b2Dot(n2, v) - b2_toiSlop;
// Shifting the edge inward by b2_toiSlop should
// not cause the plane to pass the centroid.
// Your shape has a radius/extent less than b2_toiSlop.
//b2Assert(d.x >= 0.0f);
//b2Assert(d.y >= 0.0f);
A.col1.x := n1.x;
A.col2.x := n1.y;
A.col1.y := n2.x;
A.col2.y := n2.y;
{$IFDEF OP_OVERLOAD}
m_coreVertices[i] := A.Solve(d) + m_centroid;
{$ELSE}
m_coreVertices[i] := Add(Solve(A, d), m_centroid);
{$ENDIF}
end;
end;
procedure Tb2PolygonShape.UpdateSweepRadius(const center: TVector2);
var
i: Integer;
d: TVector2;
begin
// Update the sweep radius (maximum radius) as measured from a local center point.
m_sweepRadius := 0.0;
for i := 0 to m_vertexCount - 1 do
begin
{$IFDEF OP_OVERLOAD}
d := m_coreVertices[i] - center;
m_sweepRadius := b2Max(m_sweepRadius, d.Length());
{$ELSE}
d := Subtract(m_coreVertices[i], center);
m_sweepRadius := b2Max(m_sweepRadius, Length(d));
{$ENDIF}
end;
end;
function Tb2PolygonShape.TestPoint(const transform: Tb2XForm;
const p: TVector2): Boolean;
var
i: Integer;
pLocal: TVector2;
begin
{$IFDEF OP_OVERLOAD}
pLocal := b2MulT(transform.R, p - transform.position);
{$ELSE}
pLocal := b2MulT(transform.R, Subtract(p, transform.position));
{$ENDIF}
for i := 0 to m_vertexCount - 1 do
{$IFDEF OP_OVERLOAD}
if b2Dot(m_normals[i], pLocal - m_vertices[i]) > 0.0 then
{$ELSE}
if b2Dot(m_normals[i], Subtract(pLocal, m_vertices[i])) > 0.0 then
{$ENDIF}
begin
Result := False;
Exit;
end;
Result := True;
end;
function Tb2PolygonShape.TestSegment(const xf: Tb2XForm; var lambda: Float;
var normal: TVector2; const segment: Tb2Segment; maxLambda: Float): Boolean;
var
lower, upper: Float;
p1, p2, d: TVector2;
index: Int32;
i: Integer;
numerator, denominator: Float;
begin
lower := 0.0;
upper := maxLambda;
{$IFDEF OP_OVERLOAD}
p1 := b2MulT(xf.R, segment.p1 - xf.position);
p2 := b2MulT(xf.R, segment.p2 - xf.position);
d := p2 - p1;
{$ELSE}
p1 := b2MulT(xf.R, Subtract(segment.p1, xf.position));
p2 := b2MulT(xf.R, Subtract(segment.p2, xf.position));
d := Subtract(p2, p1);
{$ENDIF}
index := -1;
for i := 0 to m_vertexCount - 1 do
begin
// p := p1 + a * d
// dot(normal, p - v) := 0
// dot(normal, p1 - v) + a * dot(normal, d) := 0
{$IFDEF OP_OVERLOAD}
numerator := b2Dot(m_normals[i], m_vertices[i] - p1);
{$ELSE}
numerator := b2Dot(m_normals[i], Subtract(m_vertices[i], p1));
{$ENDIF}
denominator := b2Dot(m_normals[i], d);
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0) and (numerator < lower * denominator) then
begin
// Increase lower.
// The segment enters this half-space.
lower := numerator / denominator;
index := i;
end
else if (denominator > 0.0) and (numerator < upper * denominator) then
begin
// Decrease upper.
// The segment exits this half-space.
upper := numerator / denominator;
end;
if upper < lower then
begin
Result := False;
Exit;
end;
end;
//b2Assert(0.0f <= lower && lower <= maxLambda);
if index >= 0 then
begin
lambda := lower;
normal := b2Mul(xf.R, m_normals[index]);
Result := True;
end
else
Result := False;
end;
procedure Tb2PolygonShape.ComputeAABB(var aabb: Tb2AABB; const xf: Tb2XForm);
var
R, absR: TMatrix22;
h, position: TVector2;
begin
R := b2Mul(xf.R, m_obb.R);
absR := b2Abs(R);
h := b2Mul(absR, m_obb.extents);
{$IFDEF OP_OVERLOAD}
position := xf.position + b2Mul(xf.R, m_obb.center);
aabb.lowerBound := position - h;
aabb.upperBound := position + h;
{$ELSE}
position := Add(xf.position, b2Mul(xf.R, m_obb.center));
aabb.lowerBound := Subtract(position, h);
aabb.upperBound := Add(position, h);
{$ENDIF}
end;
procedure Tb2PolygonShape.ComputeMass(var massData: Tb2MassData);
const
k_inv3 = 1.0 / 3.0;
var
i: Integer;
center, pRef, p1, p2, p3, e1, e2: TVector2;
area, inertia: Float;
D, triangleArea: Float;
px, py, ex1, ey1, ex2, ey2: Float;
intx2, inty2: Float;
begin
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
//b2Assert(m_vertexCount >= 3);
SetZero(center);
area := 0.0;
inertia := 0.0;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
SetZero(pRef);
for i := 0 to m_vertexCount - 1 do
begin
// Triangle vertices.
p1 := pRef;
p2 := m_vertices[i];
if (i + 1 < m_vertexCount) then
p3 := m_vertices[i + 1]
else
p3 := m_vertices[0];
{$IFDEF OP_OVERLOAD}
e1 := p2 - p1;
e2 := p3 - p1;
{$ELSE}
e1 := Subtract(p2, p1);
e2 := Subtract(p3, p1);
{$ENDIF}
D := b2Cross(e1, e2);
triangleArea := 0.5 * D;
area := area + triangleArea;
// Area weighted centroid
{$IFDEF OP_OVERLOAD}
center.AddBy(triangleArea * k_inv3 * (p1 + p2 + p3));
{$ELSE}
AddBy(center, Multiply(Add(p1, p2, p3), triangleArea * k_inv3));
{$ENDIF}
px := p1.x;
py := p1.y;
ex1 := e1.x;
ey1 := e1.y;
ex2 := e2.x;
ey2 := e2.y;
intx2 := k_inv3 * (0.25 * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) +
(px * ex1 + px * ex2)) + 0.5 * px * px;
inty2 := k_inv3 * (0.25 * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) +
(py * ey1 + py * ey2)) + 0.5 * py * py;
inertia := inertia + D * (intx2 + inty2);
end;
// Total mass
massData.mass := m_density * area;
// Center of mass
//b2Assert(area > B2_FLT_EPSILON);
{$IFDEF OP_OVERLOAD}
center.DivideBy(area);
{$ELSE}
DivideBy(center, area);
{$ENDIF}
massData.center := center;
// Inertia tensor relative to the local origin.
massData.I := m_density * inertia;
end;
procedure Tb2PolygonShape.ComputeSweptAABB(var aabb: Tb2AABB; const xf1,
xf2: Tb2XForm);
var
aabb1, aabb2: Tb2AABB;
begin
ComputeAABB(aabb1, xf1);
ComputeAABB(aabb2, xf2);
aabb.lowerBound := b2Min(aabb1.lowerBound, aabb2.lowerBound);
aabb.upperBound := b2Max(aabb1.upperBound, aabb2.upperBound);
end;
function Tb2PolygonShape.GetFirstVertex(const xf: Tb2XForm): TVector2;
begin
Result := b2Mul(xf, m_coreVertices[0]);
end;
function Tb2PolygonShape.Centroid(const xf: Tb2XForm): TVector2;
begin
Result := b2Mul(xf, m_centroid);
end;
function Tb2PolygonShape.Support(const xf: Tb2XForm; const d: TVector2): TVector2;
var
i: Integer;
dLocal: TVector2;
bestIndex: Int32;
bestValue, value: Float;
begin
dLocal := b2MulT(xf.R, d);
bestIndex := 0;
bestValue := b2Dot(m_coreVertices[0], dLocal);
for i := 1 to m_vertexCount - 1 do
begin
value := b2Dot(m_coreVertices[i], dLocal);
if value > bestValue then
begin
bestIndex := i;
bestValue := value;
end;
end;
Result := b2Mul(xf, m_coreVertices[bestIndex]);
end;
{ Tb2CircleContact }
constructor Tb2CircleContact.Create(shape1, shape2: Tb2Shape);
begin
inherited Create(shape1, shape2);
//b2Assert(m_shape1->GetType() == e_circleShape);
//b2Assert(m_shape2->GetType() == e_circleShape);
m_manifold.pointCount := 0;
m_manifold.points[0].normalImpulse := 0.0;
m_manifold.points[0].tangentImpulse := 0.0;
end;
procedure Tb2CircleContact.Evaluate(listener: Tb2ContactListener);
var
b1, b2: Tb2Body;
m0: Tb2Manifold;
cp: Tb2ContactPoint;
v1, v2: TVector2;
begin
b1 := m_shape1.GetBody;
b2 := m_shape2.GetBody;
m0 := m_manifold;
b2CollideCircles(m_manifold, Tb2CircleShape(m_shape1), Tb2CircleShape(m_shape2),
b1.m_xf, b2.m_xf);
cp.shape1 := m_shape1;
cp.shape2 := m_shape2;
cp.friction := m_friction;
cp.restitution := m_restitution;
if m_manifold.pointCount > 0 then
begin
m_manifoldCount := 1;
with m_manifold.points[0] do
begin
if m0.pointCount = 0 then
begin
normalImpulse := 0.0;
tangentImpulse := 0.0;
if Assigned(listener) then
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m_manifold.normal;
cp.separation := separation;
cp.id := id;
listener.Add(cp);
end;
end
else
begin
normalImpulse := m0.points[0].normalImpulse;
tangentImpulse := m0.points[0].tangentImpulse;
if Assigned(listener) then
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m_manifold.normal;
cp.separation := separation;
cp.id := id;
listener.Persist(cp);
end;
end;
end;
end
else
begin
m_manifoldCount := 0;
if (m0.pointCount > 0) and Assigned(listener) then
begin
with m0.points[0] do
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m0.normal;
cp.separation := separation;
cp.id := id;
listener.Remove(cp);
end;
end;
end;
end;
function Tb2CircleContact.GetManifolds: Pb2Manifold;
begin
Result := @m_manifold;
end;
{ Tb2PolyAndCircleContact }
constructor Tb2PolyAndCircleContact.Create(shape1, shape2: Tb2Shape);
begin
inherited Create(shape1, shape2);
//b2Assert(m_shape1->GetType() == e_polygonShape);
//b2Assert(m_shape2->GetType() == e_circleShape);
m_manifold.pointCount := 0;
m_manifold.points[0].normalImpulse := 0.0;
m_manifold.points[0].tangentImpulse := 0.0;
end;
procedure Tb2PolyAndCircleContact.Evaluate(listener: Tb2ContactListener);
var
i, j: Integer;
b1, b2: Tb2Body;
m0: Tb2Manifold;
persisted: array[0..b2_maxManifoldPoints - 1] of Boolean;
cp: Tb2ContactPoint;
found: Boolean;
mp0: Pb2ManifoldPoint;
local_id: Tb2ContactID;
v1, v2: TVector2;
begin
b1 := m_shape1.GetBody;
b2 := m_shape2.GetBody;
m0 := m_manifold;
b2CollidePolygonAndCircle(m_manifold, Tb2PolygonShape(m_shape1),
Tb2CircleShape(m_shape2), b1.m_xf, b2.m_xf);
persisted[0] := False;
persisted[1] := False;
cp.shape1 := m_shape1;
cp.shape2 := m_shape2;
cp.friction := m_friction;
cp.restitution := m_restitution;
// Match contact ids to facilitate warm starting.
if m_manifold.pointCount > 0 then
begin
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for i := 0 to m_manifold.pointCount - 1 do
with m_manifold.points[i] do
begin
normalImpulse := 0.0;
tangentImpulse := 0.0;
found := False;
local_id := id;
for j := 0 to m0.pointCount - 1 do
begin
if persisted[j] then
Continue;
mp0 := @m0.points[j];
if mp0.id.key = local_id.key then
begin
persisted[j] := True;
normalImpulse := mp0.normalImpulse;
tangentImpulse := mp0.tangentImpulse;
// A persistent point.
found := True;
// Report persistent point.
if Assigned(listener) then
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m_manifold.normal;
cp.separation := separation;
cp.id := local_id;
listener.Persist(cp);
end;
Break;
end;
end;
// Report added point.
if (not found) and Assigned(listener) then
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m_manifold.normal;
cp.separation := separation;
cp.id := local_id;
listener.Add(cp);
end;
end;
m_manifoldCount := 1;
end
else
m_manifoldCount := 0;
if not Assigned(listener) then
Exit;
// Report removed points.
for i := 0 to m0.pointCount - 1 do
begin
if persisted[i] then
Continue;
with m0.points[i] do
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m0.normal;
cp.separation := separation;
cp.id := id;
listener.Remove(cp);
end;
end;
end;
function Tb2PolyAndCircleContact.GetManifolds: Pb2Manifold;
begin
Result := @m_manifold;
end;
{ Tb2PolygonContact }
constructor Tb2PolygonContact.Create(shape1, shape2: Tb2Shape);
begin
inherited Create(shape1, shape2);
//b2Assert(m_shape1->GetType() == e_polygonShape);
//b2Assert(m_shape2->GetType() == e_polygonShape);
m_manifold.pointCount := 0;
end;
procedure Tb2PolygonContact.Evaluate(listener: Tb2ContactListener);
var
i, j: Integer;
b1, b2: Tb2Body;
m0: Tb2Manifold;
persisted: array[0..b2_maxManifoldPoints - 1] of Boolean;
cp: Tb2ContactPoint;
v1, v2: TVector2;
found: Boolean;
local_id: Tb2ContactID;
mp0: Pb2ManifoldPoint;
begin
b1 := m_shape1.GetBody;
b2 := m_shape2.GetBody;
m0 := m_manifold;
b2CollidePolygons(m_manifold, Tb2PolygonShape(m_shape1), Tb2PolygonShape(m_shape2),
b1.m_xf, b2.m_xf);
persisted[0] := False;
persisted[1] := False;
cp.shape1 := m_shape1;
cp.shape2 := m_shape2;
cp.friction := m_friction;
cp.restitution := m_restitution;
// Match contact ids to facilitate warm starting.
if m_manifold.pointCount > 0 then
begin
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for i := 0 to m_manifold.pointCount - 1 do
with m_manifold.points[i] do
begin
normalImpulse := 0.0;
tangentImpulse := 0.0;
found := False;
local_id := id;
for j := 0 to m0.pointCount - 1 do
begin
if persisted[j] then
Continue;
mp0 := @m0.points[j];
if mp0.id.key = id.key then
begin
persisted[j] := True;
normalImpulse := mp0.normalImpulse;
tangentImpulse := mp0.tangentImpulse;
// A persistent point.
found := True;
// Report persistent point.
if Assigned(listener) then
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m_manifold.normal;
cp.separation := separation;
cp.id := local_id;
listener.Persist(cp);
end;
Break;
end;
end;
// Report added point.
if (not found) and Assigned(listener) then
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m_manifold.normal;
cp.separation := separation;
cp.id := local_id;
listener.Add(cp);
end;
end;
m_manifoldCount := 1;
end
else
m_manifoldCount := 0;
if not Assigned(listener) then
Exit;
// Report removed points.
for i := 0 to m0.pointCount - 1 do
begin
if persisted[i] then
Continue;
with m0.points[i] do
begin
cp.position := b1.GetWorldPoint(localPoint1);
v1 := b1.GetLinearVelocityFromLocalPoint(localPoint1);
v2 := b2.GetLinearVelocityFromLocalPoint(localPoint2);
{$IFDEF OP_OVERLOAD}
cp.velocity := v2 - v1;
{$ELSE}
cp.velocity := Subtract(v2, v1);
{$ENDIF}
cp.normal := m0.normal;
cp.separation := separation;
cp.id := id;
listener.Remove(cp);
end;
end;
end;
function Tb2PolygonContact.GetManifolds: Pb2Manifold;
begin
Result := @m_manifold;
end;
{ Tb2DistanceJointDef }
// 1-D constrained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
constructor Tb2DistanceJointDef.Create;
begin
JointType := e_distanceJoint;
SetZero(localAnchor1);
SetZero(localAnchor2);
length := 1.0;
frequencyHz := 0.0;
dampingRatio := 0.0;
end;
procedure Tb2DistanceJointDef.Initialize(body1, body2: Tb2Body; const anchor1,
anchor2: TVector2);
begin
Self.body1 := body1;
Self.body2 := body2;
localAnchor1 := body1.GetLocalPoint(anchor1);
localAnchor2 := body2.GetLocalPoint(anchor2);
{$IFDEF OP_OVERLOAD}
length := (anchor2 - anchor1).Length;
{$ELSE}
length := UPhysics2DTypes.Length(Subtract(anchor2, anchor1));
{$ENDIF}
end;
{ Tb2DistanceJoint }
constructor Tb2DistanceJoint.Create(def: Tb2DistanceJointDef);
begin
inherited Create(def);
m_localAnchor1 := def.localAnchor1;
m_localAnchor2 := def.localAnchor2;
m_length := def.length;
m_frequencyHz := def.frequencyHz;
m_dampingRatio := def.dampingRatio;
m_impulse := 0.0;
m_gamma := 0.0;
m_bias := 0.0;
m_inv_dt := 0.0;
end;
function Tb2DistanceJoint.GetAnchor1: TVector2;
begin
Result := m_body1.GetWorldPoint(m_localAnchor1);
end;
function Tb2DistanceJoint.GetAnchor2: TVector2;
begin
Result := m_body2.GetWorldPoint(m_localAnchor2);
end;
function Tb2DistanceJoint.GetReactionForce: TVector2;
begin
{$IFDEF OP_OVERLOAD}
Result := (m_inv_dt * m_impulse) * m_u;
{$ELSE}
Result := Multiply(m_u, m_inv_dt * m_impulse);
{$ENDIF}
end;
function Tb2DistanceJoint.GetReactionTorque: Float;
begin
Result := 0.0;
end;
procedure Tb2DistanceJoint.InitVelocityConstraints(const step: Tb2TimeStep);
var
r1, r2, P: TVector2;
length, cr1u, cr2u, invMass: Float;
C, omega, d, k: Float;
begin
m_inv_dt := step.inv_dt;
// Compute the effective mass matrix.
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
m_u := m_body2.m_sweep.c + r2 - m_body1.m_sweep.c - r1;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
m_u := Subtract(r2, r1);
AddBy(m_u, m_body2.m_sweep.c);
SubtractBy(m_u, m_body1.m_sweep.c);
{$ENDIF}
// Handle singularity.
{$IFDEF OP_OVERLOAD}
length := m_u.Length;
if length > b2_linearSlop then
m_u := m_u / length
else
m_u.SetZero;
{$ELSE}
length := UPhysics2DTypes.Length(m_u);
if length > b2_linearSlop then
DivideBy(m_u, length)
else
SetZero(m_u);
{$ENDIF}
cr1u := b2Cross(r1, m_u);
cr2u := b2Cross(r2, m_u);
invMass := m_body1.m_invMass + m_body1.m_invI * cr1u * cr1u +
m_body2.m_invMass + m_body2.m_invI * cr2u * cr2u;
//b2Assert(invMass > B2_FLT_EPSILON);
m_mass := 1.0 / invMass;
if m_frequencyHz > 0.0 then
begin
C := length - m_length;
omega := 2.0 * Pi * m_frequencyHz; // Frequency
d := 2.0 * m_mass * m_dampingRatio * omega; // Damping coefficient
k := m_mass * omega * omega; // Spring stiffness
m_gamma := 1.0 / (step.dt * (d + step.dt * k)); // magic formulas
m_bias := C * step.dt * k * m_gamma;
m_mass := 1.0 / (invMass + m_gamma);
end;
if step.warmStarting then
begin
m_impulse := m_impulse * step.dtRatio;
{$IFDEF OP_OVERLOAD}
P := m_impulse * m_u;
m_body1.m_linearVelocity.SubtractBy(m_body1.m_invMass * P);
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P);
{$ELSE}
P := Multiply(m_u, m_impulse);
SubtractBy(m_body1.m_linearVelocity, Multiply(P, m_body1.m_invMass));
AddBy(m_body2.m_linearVelocity, Multiply(P, m_body2.m_invMass));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity - m_body1.m_invI * b2Cross(r1, P);
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * b2Cross(r2, P);
end
else
m_impulse := 0.0;
end;
procedure Tb2DistanceJoint.SolveVelocityConstraints(const step: Tb2TimeStep);
var
r1, r2, v1, v2, P: TVector2;
Cdot, impulse: Float;
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
// Cdot = dot(u, v + cross(w, r))
v1 := m_body1.m_linearVelocity + b2Cross(m_body1.m_angularVelocity, r1);
v2 := m_body2.m_linearVelocity + b2Cross(m_body2.m_angularVelocity, r2);
Cdot := b2Dot(m_u, v2 - v1);
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
// Cdot = dot(u, v + cross(w, r))
v1 := Add(m_body1.m_linearVelocity, b2Cross(m_body1.m_angularVelocity, r1));
v2 := Add(m_body2.m_linearVelocity, b2Cross(m_body2.m_angularVelocity, r2));
Cdot := b2Dot(m_u, Subtract(v2, v1));
{$ENDIF}
impulse := -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
m_impulse := m_impulse + impulse;
{$IFDEF OP_OVERLOAD}
P := impulse * m_u;
m_body1.m_linearVelocity.SubtractBy(m_body1.m_invMass * P);
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P);
{$ELSE}
P := Multiply(m_u, impulse);
SubtractBy(m_body1.m_linearVelocity, Multiply(P, m_body1.m_invMass));
AddBy(m_body2.m_linearVelocity, Multiply(P, m_body2.m_invMass));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity - m_body1.m_invI * b2Cross(r1, P);
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * b2Cross(r2, P);
end;
function Tb2DistanceJoint.SolvePositionConstraints: Boolean;
var
r1, r2, d, P: TVector2;
C, impulse: Float;
begin
if m_frequencyHz > 0.0 then
begin
Result := True;
Exit;
end;
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
d := m_body2.m_sweep.c + r2 - m_body1.m_sweep.c - r1;
C := d.Normalize() - m_length;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
d := Subtract(r2, r1);
AddBy(d, m_body2.m_sweep.c);
SubtractBy(d, m_body1.m_sweep.c);
C := Normalize(d) - m_length;
{$ENDIF}
C := b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
impulse := -m_mass * C;
m_u := d;
{$IFDEF OP_OVERLOAD}
P := impulse * m_u;
m_body1.m_sweep.c.SubtractBy(m_body1.m_invMass * P);
m_body2.m_sweep.c.AddBy(m_body2.m_invMass * P);
{$ELSE}
P := Multiply(m_u, impulse);
SubtractBy(m_body1.m_sweep.c, Multiply(P, m_body1.m_invMass));
AddBy(m_body2.m_sweep.c, Multiply(P, m_body2.m_invMass));
{$ENDIF}
m_body1.m_sweep.a := m_body1.m_sweep.a - m_body1.m_invI * b2Cross(r1, P);
m_body2.m_sweep.a := m_body2.m_sweep.a + m_body2.m_invI * b2Cross(r2, P);
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
Result := Abs(C) < b2_linearSlop;
end;
{ Tb2PrismaticJointDef }
// Linear constraint (point-to-line)
// d = p2 - p1 = x2 + r2 - x1 - r1
// C = dot(ay1, d)
// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
//
// Angular constraint
// C = a2 - a1 + a_initial
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// Motor/Limit linear constraint
// C = dot(ax1, d)
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
constructor Tb2PrismaticJointDef.Create;
begin
JointType := e_prismaticJoint;
SetZero(localAnchor1);
SetZero(localAnchor2);
{$IFDEF OP_OVERLOAD}
localAxis1.SetValue(1.0, 0.0);
{$ELSE}
SetValue(localAxis1, 1.0, 0.0);
{$ENDIF}
referenceAngle := 0.0;
enableLimit := False;
lowerTranslation := 0.0;
upperTranslation := 0.0;
enableMotor := False;
maxMotorForce := 0.0;
motorSpeed := 0.0;
end;
procedure Tb2PrismaticJointDef.Initialize(body1, body2: Tb2Body; const anchor,
axis: TVector2);
begin
Self.body1 := body1;
Self.body2 := body2;
localAnchor1 := body1.GetLocalPoint(anchor);
localAnchor2 := body2.GetLocalPoint(anchor);
localAxis1 := body1.GetLocalVector(axis);
referenceAngle := body2.GetAngle - body1.GetAngle;
end;
{ Tb2PrismaticJoint }
constructor Tb2PrismaticJoint.Create(def: Tb2PrismaticJointDef);
begin
inherited Create(def);
m_localAnchor1 := def.localAnchor1;
m_localAnchor2 := def.localAnchor2;
m_localXAxis1 := def.localAxis1;
m_localYAxis1 := b2Cross(1.0, m_localXAxis1);
m_refAngle := def.referenceAngle;
{$IFDEF OP_OVERLOAD}
m_linearJacobian.SetZero;
m_motorJacobian.SetZero;
{$ELSE}
SetZero(m_linearJacobian);
SetZero(m_motorJacobian);
{$ENDIF}
m_linearMass := 0.0;
m_force := 0.0;
m_angularMass := 0.0;
m_torque := 0.0;
m_motorMass := 0.0;
m_motorForce := 0.0;
m_limitForce := 0.0;
m_limitPositionImpulse := 0.0;
m_lowerTranslation := def.lowerTranslation;
m_upperTranslation := def.upperTranslation;
m_maxMotorForce := def.maxMotorForce;
m_motorSpeed := def.motorSpeed;
m_enableLimit := def.enableLimit;
m_enableMotor := def.enableMotor;
end;
function Tb2PrismaticJoint.GetAnchor1: TVector2;
begin
Result := m_body1.GetWorldPoint(m_localAnchor1);
end;
function Tb2PrismaticJoint.GetAnchor2: TVector2;
begin
Result := m_body2.GetWorldPoint(m_localAnchor2);
end;
function Tb2PrismaticJoint.GetReactionForce: TVector2;
var
ax1, ay1: TVector2;
begin
ax1 := b2Mul(m_body1.m_xf.R, m_localXAxis1);
ay1 := b2Mul(m_body1.m_xf.R, m_localYAxis1);
{$IFDEF OP_OVERLOAD}
Result := m_limitForce * ax1 + m_force * ay1;
{$ELSE}
Result := Add(Multiply(ax1, m_limitForce), Multiply(ay1, m_force));
{$ENDIF}
end;
function Tb2PrismaticJoint.GetReactionTorque: Float;
begin
Result := m_torque;
end;
procedure Tb2PrismaticJoint.InitVelocityConstraints(const step: Tb2TimeStep);
var
r1, r2, ay1, e, ax1, d: TVector2;
invMass1, invMass2, invI1, invI2, jointTranslation, L1, L2: Float;
begin
// Compute the effective masses.
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
{$ENDIF}
invMass1 := m_body1.m_invMass;
invMass2 := m_body2.m_invMass;
invI1 := m_body1.m_invI;
invI2 := m_body2.m_invI;
// Compute point to line constraint effective mass.
// J := [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
ay1 := b2Mul(m_body1.m_xf.R, m_localYAxis1);
{$IFDEF OP_OVERLOAD}
e := m_body2.m_sweep.c + r2 - m_body1.m_sweep.c; // e := d + r1
m_linearJacobian.SetValue(-ay1, ay1, -b2Cross(e, ay1), b2Cross(r2, ay1));
{$ELSE}
e := Subtract(m_body2.m_sweep.c, m_body1.m_sweep.c);
AddBy(e, r2);
SetValue(m_linearJacobian, Negative(ay1), ay1, -b2Cross(e, ay1), b2Cross(r2, ay1));
{$ENDIF}
m_linearMass := invMass1 + invI1 * m_linearJacobian.angular1 *
m_linearJacobian.angular1 + invMass2 + invI2 *
m_linearJacobian.angular2 * m_linearJacobian.angular2;
//b2Assert(m_linearMass > B2_FLT_EPSILON);
m_linearMass := 1.0 / m_linearMass;
// Compute angular constraint effective mass.
m_angularMass := invI1 + invI2;
if m_angularMass > FLT_EPSILON then
m_angularMass := 1.0 / m_angularMass;
// Compute motor and limit terms.
if m_enableLimit or m_enableMotor then
begin
// The motor and limit share a Jacobian and effective mass.
ax1 := b2Mul(m_body1.m_xf.R, m_localXAxis1);
{$IFDEF OP_OVERLOAD}
m_motorJacobian.SetValue(-ax1, ax1, -b2Cross(e, ax1), b2Cross(r2, ax1));
{$ELSE}
SetValue(m_motorJacobian, Negative(ax1), ax1, -b2Cross(e, ax1), b2Cross(r2, ax1));
{$ENDIF}
m_motorMass := invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 +
invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
//b2Assert(m_motorMass > B2_FLT_EPSILON);
m_motorMass := 1.0 / m_motorMass;
if m_enableLimit then
begin
{$IFDEF OP_OVERLOAD}
d := e - r1; // p2 - p1
{$ELSE}
d := Subtract(e, r1); // p2 - p1
{$ENDIF}
jointTranslation := b2Dot(ax1, d);
if Abs(m_upperTranslation - m_lowerTranslation) < 2.0 * b2_linearSlop then
m_limitState := e_equalLimits
else if jointTranslation <= m_lowerTranslation then
begin
if m_limitState <> e_atLowerLimit then
m_limitForce := 0.0;
m_limitState := e_atLowerLimit;
end
else if jointTranslation >= m_upperTranslation then
begin
if m_limitState <> e_atUpperLimit then
m_limitForce := 0.0;
m_limitState := e_atUpperLimit;
end
else
begin
m_limitState := e_inactiveLimit;
m_limitForce := 0.0;
end;
end;
end;
if not m_enableMotor then
m_motorForce := 0.0;
if not m_enableLimit then
m_limitForce := 0.0;
if step.warmStarting then
begin
{$IFDEF OP_OVERLOAD}
r1 := step.dt * (m_force * m_linearJacobian.linear1 + (m_motorForce +
m_limitForce) * m_motorJacobian.linear1);
r2 := step.dt * (m_force * m_linearJacobian.linear2 + (m_motorForce +
m_limitForce) * m_motorJacobian.linear2);
{$ELSE}
r1 := Multiply(m_motorJacobian.linear1, m_motorForce + m_limitForce);
AddBy(r1, Multiply(m_linearJacobian.linear1, m_force));
MultiplyBy(r1, step.dt);
r2 := Multiply(m_motorJacobian.linear2, m_motorForce + m_limitForce);
AddBy(r2, Multiply(m_linearJacobian.linear2, m_force));
MultiplyBy(r2, step.dt);
{$ENDIF}
L1 := step.dt * (m_force * m_linearJacobian.angular1 - m_torque +
(m_motorForce + m_limitForce) * m_motorJacobian.angular1);
L2 := step.dt * (m_force * m_linearJacobian.angular2 + m_torque +
(m_motorForce + m_limitForce) * m_motorJacobian.angular2);
{$IFDEF OP_OVERLOAD}
m_body1.m_linearVelocity.AddBy(invMass1 * r1);
m_body2.m_linearVelocity.AddBy(invMass2 * r2);
{$ELSE}
AddBy(m_body1.m_linearVelocity, Multiply(r1, invMass1));
AddBy(m_body2.m_linearVelocity, Multiply(r2, invMass2));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + invI1 * L1;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + invI2 * L2;
end
else
begin
m_force := 0.0;
m_torque := 0.0;
m_limitForce := 0.0;
m_motorForce := 0.0;
end;
m_limitPositionImpulse := 0.0;
end;
procedure Tb2PrismaticJoint.SolveVelocityConstraints(const step: Tb2TimeStep);
var
invMass1, invMass2, invI1, invI2: Float;
linearCdot, force, P, angularCdot, torque, L: Float;
motorCdot, motorForce, oldMotorForce: Float;
limitCdot, limitForce, oldLimitForce: Float;
begin
invMass1 := m_body1.m_invMass;
invMass2 := m_body2.m_invMass;
invI1 := m_body1.m_invI;
invI2 := m_body2.m_invI;
// Solve linear constraint.
{$IFDEF OP_OVERLOAD}
linearCdot := m_linearJacobian.Compute(m_body1.m_linearVelocity,
m_body2.m_linearVelocity, m_body1.m_angularVelocity, m_body2.m_angularVelocity);
{$ELSE}
linearCdot := Compute(m_linearJacobian, m_body1.m_linearVelocity,
m_body2.m_linearVelocity, m_body1.m_angularVelocity, m_body2.m_angularVelocity);
{$ENDIF}
force := -step.inv_dt * m_linearMass * linearCdot;
m_force := m_force + force;
P := step.dt * force;
{$IFDEF OP_OVERLOAD}
m_body1.m_linearVelocity.AddBy((invMass1 * P) * m_linearJacobian.linear1);
m_body2.m_linearVelocity.AddBy((invMass2 * P) * m_linearJacobian.linear2);
{$ELSE}
AddBy(m_body1.m_linearVelocity, Multiply(m_linearJacobian.linear1, invMass1 * P));
AddBy(m_body2.m_linearVelocity, Multiply(m_linearJacobian.linear2, invMass2 * P));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + invI1 * P * m_linearJacobian.angular1;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + invI2 * P * m_linearJacobian.angular2;
// Solve angular constraint.
angularCdot := m_body2.m_angularVelocity - m_body1.m_angularVelocity;
torque := -step.inv_dt * m_angularMass * angularCdot;
m_torque := m_torque + torque;
L := step.dt * torque;
m_body1.m_angularVelocity := m_body1.m_angularVelocity - invI1 * L;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + invI2 * L;
// Solve linear motor constraint.
if m_enableMotor and (m_limitState <> e_equalLimits) then
begin
{$IFDEF OP_OVERLOAD}
motorCdot := m_motorJacobian.Compute(m_body1.m_linearVelocity,
m_body2.m_linearVelocity, m_body1.m_angularVelocity,
m_body2.m_angularVelocity) - m_motorSpeed;
{$ELSE}
motorCdot := Compute(m_motorJacobian, m_body1.m_linearVelocity,
m_body2.m_linearVelocity, m_body1.m_angularVelocity,
m_body2.m_angularVelocity) - m_motorSpeed;
{$ENDIF}
motorForce := -step.inv_dt * m_motorMass * motorCdot;
oldMotorForce := m_motorForce;
m_motorForce := b2Clamp(m_motorForce + motorForce, -m_maxMotorForce, m_maxMotorForce);
motorForce := m_motorForce - oldMotorForce;
P := step.dt * motorForce;
{$IFDEF OP_OVERLOAD}
m_body1.m_linearVelocity.AddBy((invMass1 * P) * m_motorJacobian.linear1);
m_body2.m_linearVelocity.AddBy((invMass2 * P) * m_motorJacobian.linear2);
{$ELSE}
AddBy(m_body1.m_linearVelocity, Multiply(m_motorJacobian.linear1, invMass1 * P));
AddBy(m_body2.m_linearVelocity, Multiply(m_motorJacobian.linear2, invMass2 * P));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + invI1 * P * m_motorJacobian.angular1;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + invI2 * P * m_motorJacobian.angular2;
end;
// Solve linear limit constraint.
if m_enableLimit and (m_limitState <> e_inactiveLimit) then
begin
{$IFDEF OP_OVERLOAD}
limitCdot := m_motorJacobian.Compute(m_body1.m_linearVelocity,
m_body2.m_linearVelocity, m_body1.m_angularVelocity, m_body2.m_angularVelocity);
{$ELSE}
limitCdot := Compute(m_motorJacobian, m_body1.m_linearVelocity,
m_body2.m_linearVelocity, m_body1.m_angularVelocity, m_body2.m_angularVelocity);
{$ENDIF}
limitForce := -step.inv_dt * m_motorMass * limitCdot;
case m_limitState of
e_equalLimits: m_limitForce := m_limitForce + limitForce;
e_atLowerLimit:
begin
oldLimitForce := m_limitForce;
m_limitForce := b2Max(m_limitForce + limitForce, 0.0);
limitForce := m_limitForce - oldLimitForce;
end;
e_atUpperLimit:
begin
oldLimitForce := m_limitForce;
m_limitForce := b2Min(m_limitForce + limitForce, 0.0);
limitForce := m_limitForce - oldLimitForce;
end;
end;
P := step.dt * limitForce;
{$IFDEF OP_OVERLOAD}
m_body1.m_linearVelocity.AddBy((invMass1 * P) * m_motorJacobian.linear1);
m_body2.m_linearVelocity.AddBy((invMass2 * P) * m_motorJacobian.linear2);
{$ELSE}
AddBy(m_body1.m_linearVelocity, Multiply(m_motorJacobian.linear1, invMass1 * P));
AddBy(m_body2.m_linearVelocity, Multiply(m_motorJacobian.linear2, invMass2 * P));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + invI1 * P * m_motorJacobian.angular1;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + invI2 * P * m_motorJacobian.angular2;
end;
end;
function Tb2PrismaticJoint.SolvePositionConstraints: Boolean;
var
invMass1, invMass2, invI1, invI2: Float;
r1, r2, p1, p2, d, ay1, ax1: TVector2;
linearC, linearImpulse, positionError, angularC, angularImpulse, angularError: Float;
translation, limitImpulse, limitC, oldLimitImpulse: Float;
begin
invMass1 := m_body1.m_invMass;
invMass2 := m_body2.m_invMass;
invI1 := m_body1.m_invI;
invI2 := m_body2.m_invI;
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
p1 := m_body1.m_sweep.c + r1;
p2 := m_body2.m_sweep.c + r2;
d := p2 - p1;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
p1 := Add(m_body1.m_sweep.c, r1);
p2 := Add(m_body2.m_sweep.c, r2);
d := Subtract(p2, p1);
{$ENDIF}
ay1 := b2Mul(m_body1.m_xf.R, m_localYAxis1);
// Solve linear (point-to-line) constraint.
linearC := b2Dot(ay1, d);
// Prevent overly large corrections.
linearC := b2Clamp(linearC, -b2_maxLinearCorrection, b2_maxLinearCorrection);
linearImpulse := -m_linearMass * linearC;
{$IFDEF OP_OVERLOAD}
m_body1.m_sweep.c.AddBy((invMass1 * linearImpulse) * m_linearJacobian.linear1);
m_body2.m_sweep.c.AddBy((invMass2 * linearImpulse) * m_linearJacobian.linear2);
{$ELSE}
AddBy(m_body1.m_sweep.c, Multiply(m_linearJacobian.linear1, invMass1 * linearImpulse));
AddBy(m_body2.m_sweep.c, Multiply(m_linearJacobian.linear2, invMass2 * linearImpulse));
{$ENDIF}
m_body1.m_sweep.a := m_body1.m_sweep.a + invI1 * linearImpulse * m_linearJacobian.angular1;
//m_body1.SynchronizeTransform; // updated by angular constraint
m_body2.m_sweep.a := m_body2.m_sweep.a + invI2 * linearImpulse * m_linearJacobian.angular2;
//m_body2.SynchronizeTransform; // updated by angular constraint
positionError := Abs(linearC);
// Solve angular constraint.
angularC := m_body2.m_sweep.a - m_body1.m_sweep.a - m_refAngle;
// Prevent overly large corrections.
angularC := b2Clamp(angularC, -b2_maxAngularCorrection, b2_maxAngularCorrection);
angularImpulse := -m_angularMass * angularC;
m_body1.m_sweep.a := m_body1.m_sweep.a - m_body1.m_invI * angularImpulse;
m_body2.m_sweep.a := m_body2.m_sweep.a + m_body2.m_invI * angularImpulse;
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
angularError := Abs(angularC);
// Solve linear limit constraint.
if m_enableLimit and (m_limitState <> e_inactiveLimit) then
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
p1 := m_body1.m_sweep.c + r1;
p2 := m_body2.m_sweep.c + r2;
d := p2 - p1;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
p1 := Add(m_body1.m_sweep.c, r1);
p2 := Add(m_body2.m_sweep.c, r2);
d := Subtract(p2, p1);
{$ENDIF}
ax1 := b2Mul(m_body1.m_xf.R, m_localXAxis1);
translation := b2Dot(ax1, d);
limitImpulse := 0.0;
if m_limitState = e_equalLimits then
begin
// Prevent large angular corrections
limitC := b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
limitImpulse := -m_motorMass * limitC;
positionError := b2Max(positionError, Abs(angularC));
end
else if m_limitState = e_atLowerLimit then
begin
limitC := translation - m_lowerTranslation;
positionError := b2Max(positionError, -limitC);
// Prevent large linear corrections and allow some slop.
limitC := b2Clamp(limitC + b2_linearSlop, -b2_maxLinearCorrection, 0.0);
limitImpulse := -m_motorMass * limitC;
oldLimitImpulse := m_limitPositionImpulse;
m_limitPositionImpulse := b2Max(m_limitPositionImpulse + limitImpulse, 0.0);
limitImpulse := m_limitPositionImpulse - oldLimitImpulse;
end
else if m_limitState = e_atUpperLimit then
begin
limitC := translation - m_upperTranslation;
positionError := b2Max(positionError, limitC);
// Prevent large linear corrections and allow some slop.
limitC := b2Clamp(limitC - b2_linearSlop, 0.0, b2_maxLinearCorrection);
limitImpulse := -m_motorMass * limitC;
oldLimitImpulse := m_limitPositionImpulse;
m_limitPositionImpulse := b2Min(m_limitPositionImpulse + limitImpulse, 0.0);
limitImpulse := m_limitPositionImpulse - oldLimitImpulse;
end;
{$IFDEF OP_OVERLOAD}
m_body1.m_sweep.c.AddBy((invMass1 * limitImpulse) * m_motorJacobian.linear1);
m_body2.m_sweep.c.AddBy((invMass2 * limitImpulse) * m_motorJacobian.linear2);
{$ELSE}
AddBy(m_body1.m_sweep.c, Multiply(m_motorJacobian.linear1, invMass1 * limitImpulse));
AddBy(m_body2.m_sweep.c, Multiply(m_motorJacobian.linear2, invMass2 * limitImpulse));
{$ENDIF}
m_body1.m_sweep.a := m_body1.m_sweep.a + invI1 * limitImpulse * m_motorJacobian.angular1;
m_body2.m_sweep.a := m_body2.m_sweep.a + invI2 * limitImpulse * m_motorJacobian.angular2;
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
end;
Result := (positionError <= b2_linearSlop) and (angularError <= b2_angularSlop);
end;
function Tb2PrismaticJoint.GetJointTranslation: Float;
var
d, axis: TVector2;
begin
{$IFDEF OP_OVERLOAD}
d := m_body2.GetWorldPoint(m_localAnchor2) - m_body1.GetWorldPoint(m_localAnchor1);
{$ELSE}
d := Subtract(m_body2.GetWorldPoint(m_localAnchor2), m_body1.GetWorldPoint(m_localAnchor1));
{$ENDIF}
axis := m_body1.GetWorldVector(m_localXAxis1);
Result := b2Dot(d, axis);
end;
function Tb2PrismaticJoint.GetJointSpeed: Float;
var
r1, r2, d, axis: TVector2;
w1: Float;
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
d := (m_body2.m_sweep.c + r2) - (m_body1.m_sweep.c + r1);
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
d := Subtract(Add(m_body2.m_sweep.c, r2), Add(m_body1.m_sweep.c, r1));
{$ENDIF}
axis := m_body1.GetWorldVector(m_localXAxis1);
w1 := m_body1.m_angularVelocity;
{$IFDEF OP_OVERLOAD}
Result := b2Dot(d, b2Cross(w1, axis)) +
b2Dot(axis, m_body2.m_linearVelocity + b2Cross(m_body2.m_angularVelocity, r2) -
m_body1.m_linearVelocity - b2Cross(w1, r1));
{$ELSE}
Result := b2Dot(d, b2Cross(w1, axis)) +
b2Dot(axis, Subtract(Add(m_body2.m_linearVelocity, b2Cross(m_body2.m_angularVelocity, r2)),
Add(m_body1.m_linearVelocity, b2Cross(w1, r1))));
{$ENDIF}
end;
procedure Tb2PrismaticJoint.SetLimits(lower, upper: Float);
begin
//b2Assert(lower <= upper);
m_lowerTranslation := lower;
m_upperTranslation := upper;
end;
{ Tb2MouseJointDef }
// p = attached point, m = mouse point
// C = p - m
// Cdot = v
// = v + cross(w, r)
// J = [I r_skew]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
constructor Tb2MouseJointDef.Create;
begin
JointType := e_mouseJoint;
SetZero(target);
maxForce := 0.0;
frequencyHz := 5.0;
dampingRatio := 0.7;
timeStep := 1.0 / 60.0;
end;
{ Tb2MouseJoint }
constructor Tb2MouseJoint.Create(def: Tb2MouseJointDef);
var
omega, d, k: Float;
begin
inherited Create(def);
m_target := def.target;
m_localAnchor := b2MulT(m_body2.m_xf, m_target);
m_maxForce := def.maxForce;
SetZero(m_impulse);
omega := 2.0 * Pi * def.frequencyHz; // Frequency
d := 2.0 * m_body2.m_mass * def.dampingRatio * omega; // Damping coefficient
k := (def.timeStep * m_body2.m_mass) * (omega * omega); // Spring stiffness
//b2Assert(d + k > B2_FLT_EPSILON);
m_gamma := 1.0 / (d + k);
m_beta := k / (d + k);
end;
function Tb2MouseJoint.GetAnchor1: TVector2;
begin
Result := m_target;
end;
function Tb2MouseJoint.GetAnchor2: TVector2;
begin
Result := m_body2.GetWorldPoint(m_localAnchor);
end;
function Tb2MouseJoint.GetReactionForce: TVector2;
begin
Result := m_impulse;
end;
function Tb2MouseJoint.GetReactionTorque: Float;
begin
Result := 0.0;
end;
procedure Tb2MouseJoint.InitVelocityConstraints(const step: Tb2TimeStep);
var
r, P: TVector2;
invMass, invI: Float;
K1, K2, K: TMatrix22;
begin
// Compute the effective mass matrix.
{$IFDEF OP_OVERLOAD}
r := b2Mul(m_body2.m_xf.R, m_localAnchor - m_body2.GetLocalCenter);
{$ELSE}
r := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor, m_body2.GetLocalCenter));
{$ENDIF}
// K := [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// := [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
invMass := m_body2.m_invMass;
invI := m_body2.m_invI;
K1.col1.x := invMass;
K1.col2.x := 0.0;
K1.col1.y := 0.0;
K1.col2.y := invMass;
K2.col1.x := invI * r.y * r.y;
K2.col2.x := -invI * r.x * r.y;
K2.col1.y := K2.col2.x;
K2.col2.y := invI * r.x * r.x;
{$IFDEF OP_OVERLOAD}
K := K1 + K2;
{$ELSE}
K := Add(K1, K2);
{$ENDIF}
K.col1.x := K.col1.x + m_gamma;
K.col2.y := K.col2.y + m_gamma;
{$IFDEF OP_OVERLOAD}
m_mass := K.Invert;
m_C := m_body2.m_sweep.c + r - m_target;
{$ELSE}
m_mass := Invert(K);
m_C := Add(m_body2.m_sweep.c, r);
SubtractBy(m_C, m_target);
{$ENDIF}
m_body2.m_angularVelocity := m_body2.m_angularVelocity * 0.98; // Cheat with some damping
// Warm starting.
{$IFDEF OP_OVERLOAD}
P := step.dt * m_impulse;
m_body2.m_linearVelocity.AddBy(invMass * P);
{$ELSE}
P := Multiply(m_impulse, step.dt);
AddBy(m_body2.m_linearVelocity, Multiply(P, invMass));
{$ENDIF}
m_body2.m_angularVelocity := m_body2.m_angularVelocity + invI * b2Cross(r, P);
end;
procedure Tb2MouseJoint.SolveVelocityConstraints(const step: Tb2TimeStep);
var
r, Cdot, force, oldForce, P: TVector2;
forceMagnitude: Float;
begin
oldForce := m_impulse;
// Cdot := v + cross(w, r)
{$IFDEF OP_OVERLOAD}
r := b2Mul(m_body2.m_xf.R, m_localAnchor - m_body2.GetLocalCenter);
Cdot := m_body2.m_linearVelocity + b2Cross(m_body2.m_angularVelocity, r);
force := -step.inv_dt * b2Mul(m_mass, Cdot + (m_beta * step.inv_dt) * m_C +
step.dt * (m_gamma * m_impulse));
m_impulse.AddBy(force);
forceMagnitude := m_impulse.Length;
if forceMagnitude > m_maxForce then
m_impulse.MultiplyBy(m_maxForce / forceMagnitude);
force := m_impulse - oldForce;
P := step.dt * force;
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P);
{$ELSE}
r := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor, m_body2.GetLocalCenter));
Cdot := Add(m_body2.m_linearVelocity, b2Cross(m_body2.m_angularVelocity, r));
force := Multiply(b2Mul(m_mass, Add(Cdot, Multiply(m_C, m_beta * step.inv_dt),
Multiply(m_impulse, step.dt * m_gamma))), -step.inv_dt);
AddBy(m_impulse, force);
forceMagnitude := Length(m_impulse);
if forceMagnitude > m_maxForce then
MultiplyBy(m_impulse, m_maxForce / forceMagnitude);
force := Subtract(m_impulse, oldForce);
P := Multiply(force, step.dt);
AddBy(m_body2.m_linearVelocity, Multiply(P, m_body2.m_invMass));
{$ENDIF}
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * b2Cross(r, P);
end;
function Tb2MouseJoint.SolvePositionConstraints: Boolean;
begin
Result := True;
end;
procedure Tb2MouseJoint.SetTarget(const target: TVector2);
begin
if m_body2.IsSleeping() then
m_body2.WakeUp;
m_target := target;
end;
{ Tb2PulleyJointDef }
// Pulley:
// length1 = norm(p1 - s1)
// length2 = norm(p2 - s2)
// C0 = (length1 + ratio * length2)_initial
// C = C0 - (length1 + ratio * length2) >= 0
// u1 = (p1 - s1) / norm(p1 - s1)
// u2 = (p2 - s2) / norm(p2 - s2)
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
//
// Limit:
// C = maxLength - length
// u = (p - s) / norm(p - s)
// Cdot = -dot(u, v + cross(w, r))
// K = invMass + invI * cross(r, u)^2
// 0 <= impulse
const
b2_minPulleyLength = 2.0;
constructor Tb2PulleyJointDef.Create;
begin
JointType := e_pulleyJoint;
{$IFDEF OP_OVERLOAD}
groundAnchor1.SetValue(-1.0, 1.0);
groundAnchor2.SetValue(1.0, 1.0);
localAnchor1.SetValue(-1.0, 0.0);
localAnchor2.SetValue(1.0, 0.0);
{$ELSE}
SetValue(groundAnchor1, -1.0, 1.0);
SetValue(groundAnchor2, 1.0, 1.0);
SetValue(localAnchor1, -1.0, 0.0);
SetValue(localAnchor2, 1.0, 0.0);
{$ENDIF}
length1 := 0.0;
maxLength1 := 0.0;
length2 := 0.0;
maxLength2 := 0.0;
ratio := 1.0;
collideConnected := True;
end;
procedure Tb2PulleyJointDef.Initialize(body1, body2: Tb2Body; const groundAnchor1,
groundAnchor2, anchor1, anchor2: TVector2; ratio: Float);
var
C: Float;
begin
Self.body1 := body1;
Self.body2 := body2;
Self.groundAnchor1 := groundAnchor1;
Self.groundAnchor2 := groundAnchor2;
Self.localAnchor1 := body1.GetLocalPoint(anchor1);
Self.localAnchor2 := body2.GetLocalPoint(anchor2);
{$IFDEF OP_OVERLOAD}
length1 := (anchor1 - groundAnchor1).Length;
length2 := (anchor2 - groundAnchor2).Length;
{$ELSE}
length1 := Length(Subtract(anchor1, groundAnchor1));
length2 := Length(Subtract(anchor2, groundAnchor2));
{$ENDIF}
Self.ratio := ratio;
//b2Assert(ratio > B2_FLT_EPSILON);
C := length1 + ratio * length2;
maxLength1 := C - ratio * b2_minPulleyLength;
maxLength2 := (C - b2_minPulleyLength) / ratio;
end;
{ Tb2PulleyJoint }
constructor Tb2PulleyJoint.Create(def: Tb2PulleyJointDef);
begin
inherited Create(def);
m_ground := m_body1.GetWorld.GetGroundBody;
{$IFDEF OP_OVERLOAD}
m_groundAnchor1 := def.groundAnchor1 - m_ground.m_xf.position;
m_groundAnchor2 := def.groundAnchor2 - m_ground.m_xf.position;
{$ELSE}
m_groundAnchor1 := Subtract(def.groundAnchor1, m_ground.m_xf.position);
m_groundAnchor2 := Subtract(def.groundAnchor2, m_ground.m_xf.position);
{$ENDIF}
m_localAnchor1 := def.localAnchor1;
m_localAnchor2 := def.localAnchor2;
//b2Assert(def.ratio != 0.0);
m_ratio := def.ratio;
m_constant := def.length1 + m_ratio * def.length2;
m_maxLength1 := b2Min(def.maxLength1, m_constant - m_ratio * b2_minPulleyLength);
m_maxLength2 := b2Min(def.maxLength2, (m_constant - b2_minPulleyLength) / m_ratio);
m_force := 0.0;
m_limitForce1 := 0.0;
m_limitForce2 := 0.0;
end;
function Tb2PulleyJoint.GetAnchor1: TVector2;
begin
Result := m_body1.GetWorldPoint(m_localAnchor1);
end;
function Tb2PulleyJoint.GetAnchor2: TVector2;
begin
Result := m_body2.GetWorldPoint(m_localAnchor2);
end;
function Tb2PulleyJoint.GetReactionForce: TVector2;
begin
{$IFDEF OP_OVERLOAD}
Result := m_force * m_u2;
{$ELSE}
Result := Multiply(m_u2, m_force);
{$ENDIF}
end;
function Tb2PulleyJoint.GetReactionTorque: Float;
begin
Result := 0.0;
end;
procedure Tb2PulleyJoint.InitVelocityConstraints(const step: Tb2TimeStep);
var
r1, r2, P1, P2: TVector2;
length1, length2, C, cr1u1, cr2u2: Float;
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
// Get the pulley axes.
m_u1 := m_body1.m_sweep.c + r1 - (m_ground.m_xf.position + m_groundAnchor1);
m_u2 := m_body2.m_sweep.c + r2 - (m_ground.m_xf.position + m_groundAnchor2);
length1 := m_u1.Length;
length2 := m_u2.Length;
if length1 > b2_linearSlop then
m_u1.DivideBy(length1)
else
m_u1.SetZero;
if length2 > b2_linearSlop then
m_u2.DivideBy(length2)
else
m_u2.SetZero;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
// Get the pulley axes.
m_u1 := Subtract(Add(m_body1.m_sweep.c, r1), Add(m_ground.m_xf.position, m_groundAnchor1));
m_u2 := Subtract(Add(m_body2.m_sweep.c, r2), Add(m_ground.m_xf.position, m_groundAnchor2));
length1 := Length(m_u1);
length2 := Length(m_u2);
if length1 > b2_linearSlop then
DivideBy(m_u1, length1)
else
SetZero(m_u1);
if length2 > b2_linearSlop then
DivideBy(m_u2, length2)
else
SetZero(m_u2);
{$ENDIF}
C := m_constant - length1 - m_ratio * length2;
if C > 0.0 then
begin
m_state := e_inactiveLimit;
m_force := 0.0;
end
else
begin
m_state := e_atUpperLimit;
m_positionImpulse := 0.0;
end;
if length1 < m_maxLength1 then
begin
m_limitState1 := e_inactiveLimit;
m_limitForce1 := 0.0;
end
else
begin
m_limitState1 := e_atUpperLimit;
m_limitPositionImpulse1 := 0.0;
end;
if length2 < m_maxLength2 then
begin
m_limitState2 := e_inactiveLimit;
m_limitForce2 := 0.0;
end
else
begin
m_limitState2 := e_atUpperLimit;
m_limitPositionImpulse2 := 0.0;
end;
// Compute effective mass.
cr1u1 := b2Cross(r1, m_u1);
cr2u2 := b2Cross(r2, m_u2);
m_limitMass1 := m_body1.m_invMass + m_body1.m_invI * cr1u1 * cr1u1;
m_limitMass2 := m_body2.m_invMass + m_body2.m_invI * cr2u2 * cr2u2;
m_pulleyMass := m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
//b2Assert(m_limitMass1 > B2_FLT_EPSILON);
//b2Assert(m_limitMass2 > B2_FLT_EPSILON);
//b2Assert(m_pulleyMass > B2_FLT_EPSILON);
m_limitMass1 := 1.0 / m_limitMass1;
m_limitMass2 := 1.0 / m_limitMass2;
m_pulleyMass := 1.0 / m_pulleyMass;
if step.warmStarting then
begin
// Warm starting.
{$IFDEF OP_OVERLOAD}
P1 := step.dt * (-m_force - m_limitForce1) * m_u1;
P2 := step.dt * (-m_ratio * m_force - m_limitForce2) * m_u2;
m_body1.m_linearVelocity.AddBy(m_body1.m_invMass * P1);
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P2);
{$ELSE}
P1 := Multiply(m_u1, -step.dt * (m_force + m_limitForce1));
P2 := Multiply(m_u2, -step.dt * (m_ratio * m_force + m_limitForce2));
AddBy(m_body1.m_linearVelocity, Multiply(P1, m_body1.m_invMass));
AddBy(m_body2.m_linearVelocity, Multiply(P2, m_body2.m_invMass));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + m_body1.m_invI * b2Cross(r1, P1);
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * b2Cross(r2, P2);
end
else
begin
m_force := 0.0;
m_limitForce1 := 0.0;
m_limitForce2 := 0.0;
end;
end;
procedure Tb2PulleyJoint.SolveVelocityConstraints(const step: Tb2TimeStep);
var
r1, r2, v1, v2, P1, P2: TVector2;
Cdot, force, oldForce: Float;
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
{$ENDIF}
if m_state = e_atUpperLimit then
begin
{$IFDEF OP_OVERLOAD}
v1 := m_body1.m_linearVelocity + b2Cross(m_body1.m_angularVelocity, r1);
v2 := m_body2.m_linearVelocity + b2Cross(m_body2.m_angularVelocity, r2);
{$ELSE}
v1 := Add(m_body1.m_linearVelocity, b2Cross(m_body1.m_angularVelocity, r1));
v2 := Add(m_body2.m_linearVelocity, b2Cross(m_body2.m_angularVelocity, r2));
{$ENDIF}
Cdot := -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
force := -step.inv_dt * m_pulleyMass * Cdot;
oldForce := m_force;
m_force := b2Max(0.0, m_force + force);
force := m_force - oldForce;
{$IFDEF OP_OVERLOAD}
P1 := -step.dt * force * m_u1;
P2 := -step.dt * m_ratio * force * m_u2;
m_body1.m_linearVelocity.AddBy(m_body1.m_invMass * P1);
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P2);
{$ELSE}
P1 := Multiply(m_u1, -step.dt * force);
P2 := Multiply(m_u2, -step.dt * m_ratio * force);
AddBy(m_body1.m_linearVelocity, Multiply(P1, m_body1.m_invMass));
AddBy(m_body2.m_linearVelocity, Multiply(P2, m_body2.m_invMass));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + m_body1.m_invI * b2Cross(r1, P1);
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * b2Cross(r2, P2);
end;
if m_limitState1 = e_atUpperLimit then
begin
{$IFDEF OP_OVERLOAD}
v1 := m_body1.m_linearVelocity + b2Cross(m_body1.m_angularVelocity, r1);
{$ELSE}
v1 := Add(m_body1.m_linearVelocity, b2Cross(m_body1.m_angularVelocity, r1));
{$ENDIF}
Cdot := -b2Dot(m_u1, v1);
force := -step.inv_dt * m_limitMass1 * Cdot;
oldForce := m_limitForce1;
m_limitForce1 := b2Max(0.0, m_limitForce1 + force);
force := m_limitForce1 - oldForce;
{$IFDEF OP_OVERLOAD}
P1 := -step.dt * force * m_u1;
m_body1.m_linearVelocity.AddBy(m_body1.m_invMass * P1);
{$ELSE}
P1 := Multiply(m_u1, -step.dt * force);
AddBy(m_body1.m_linearVelocity, Multiply(P1, m_body1.m_invMass));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + m_body1.m_invI * b2Cross(r1, P1);
end;
if m_limitState2 = e_atUpperLimit then
begin
{$IFDEF OP_OVERLOAD}
v2 := m_body2.m_linearVelocity + b2Cross(m_body2.m_angularVelocity, r2);
{$ELSE}
v2 := Add(m_body2.m_linearVelocity, b2Cross(m_body2.m_angularVelocity, r2));
{$ENDIF}
Cdot := -b2Dot(m_u2, v2);
force := -step.inv_dt * m_limitMass2 * Cdot;
oldForce := m_limitForce2;
m_limitForce2 := b2Max(0.0, m_limitForce2 + force);
force := m_limitForce2 - oldForce;
{$IFDEF OP_OVERLOAD}
P2 := -step.dt * force * m_u2;
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P2);
{$ELSE}
P2 := Multiply(m_u2, -step.dt * force);
AddBy(m_body2.m_linearVelocity, Multiply(P2, m_body2.m_invMass));
{$ENDIF}
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * b2Cross(r2, P2);
end;
end;
function Tb2PulleyJoint.SolvePositionConstraints: Boolean;
var
s1, s2, r1, r2, P1, P2: TVector2;
linearError, length1, length2, C, impulse, oldImpulse, oldLimitPositionImpulse: Float;
begin
{$IFDEF OP_OVERLOAD}
s1 := m_ground.m_xf.position + m_groundAnchor1;
s2 := m_ground.m_xf.position + m_groundAnchor2;
{$ELSE}
s1 := Add(m_ground.m_xf.position, m_groundAnchor1);
s2 := Add(m_ground.m_xf.position, m_groundAnchor2);
{$ENDIF}
linearError := 0.0;
if m_state = e_atUpperLimit then
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
// Get the pulley axes.
m_u1 := m_body1.m_sweep.c + r1 - s1;
m_u2 := m_body2.m_sweep.c + r2 - s2;
length1 := m_u1.Length;
length2 := m_u2.Length;
if length1 > b2_linearSlop then
m_u1.DivideBy(length1)
else
m_u1.SetZero;
if length2 > b2_linearSlop then
m_u2.DivideBy(length2)
else
m_u2.SetZero;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
// Get the pulley axes.
m_u1 := Subtract(Add(m_body1.m_sweep.c, r1), s1);
m_u2 := Subtract(Add(m_body2.m_sweep.c, r2), s2);
length1 := Length(m_u1);
length2 := Length(m_u2);
if length1 > b2_linearSlop then
DivideBy(m_u1, length1)
else
SetZero(m_u1);
if length2 > b2_linearSlop then
DivideBy(m_u2, length2)
else
SetZero(m_u2);
{$ENDIF}
C := m_constant - length1 - m_ratio * length2;
linearError := b2Max(linearError, -C);
C := b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0);
impulse := -m_pulleyMass * C;
oldImpulse := m_positionImpulse;
m_positionImpulse := b2Max(0.0, m_positionImpulse + impulse);
impulse := m_positionImpulse - oldImpulse;
{$IFDEF OP_OVERLOAD}
P1 := -impulse * m_u1;
P2 := -m_ratio * impulse * m_u2;
m_body1.m_sweep.c.AddBy(m_body1.m_invMass * P1);
m_body2.m_sweep.c.AddBy(m_body2.m_invMass * P2);
{$ELSE}
P1 := Multiply(m_u1, -impulse);
P2 := Multiply(m_u2, -m_ratio * impulse);
AddBy(m_body1.m_sweep.c, Multiply(P1, m_body1.m_invMass));
AddBy(m_body2.m_sweep.c, Multiply(P2, m_body2.m_invMass));
{$ENDIF}
m_body1.m_sweep.a := m_body1.m_sweep.a + m_body1.m_invI * b2Cross(r1, P1);
m_body2.m_sweep.a := m_body2.m_sweep.a + m_body2.m_invI * b2Cross(r2, P2);
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
end;
if m_limitState1 = e_atUpperLimit then
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
p1 := m_body1.m_sweep.c + r1;
m_u1 := p1 - s1;
length1 := m_u1.Length;
if length1 > b2_linearSlop then
m_u1.DivideBy(length1)
else
m_u1.SetZero;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
p1 := Add(m_body1.m_sweep.c, r1);
m_u1 := Subtract(p1, s1);
length1 := Length(m_u1);
if length1 > b2_linearSlop then
DivideBy(m_u1, length1)
else
SetZero(m_u1);
{$ENDIF}
C := m_maxLength1 - length1;
linearError := b2Max(linearError, -C);
C := b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0);
impulse := -m_limitMass1 * C;
oldLimitPositionImpulse := m_limitPositionImpulse1;
m_limitPositionImpulse1 := b2Max(0.0, m_limitPositionImpulse1 + impulse);
impulse := m_limitPositionImpulse1 - oldLimitPositionImpulse;
{$IFDEF OP_OVERLOAD}
P1 := -impulse * m_u1;
m_body1.m_sweep.c.AddBy(m_body1.m_invMass * P1);
{$ELSE}
P1 := Multiply(m_u1, -impulse);
AddBy(m_body1.m_sweep.c, Multiply(P1, m_body1.m_invMass));
{$ENDIF}
m_body1.m_sweep.a := m_body1.m_sweep.a + m_body1.m_invI * b2Cross(r1, P1);
m_body1.SynchronizeTransform;
end;
if m_limitState2 = e_atUpperLimit then
begin
{$IFDEF OP_OVERLOAD}
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
p2 := m_body2.m_sweep.c + r2;
m_u2 := p2 - s2;
length2 := m_u2.Length;
if length2 > b2_linearSlop then
m_u2.DivideBy(length2)
else
m_u2.SetZero;
{$ELSE}
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
p2 := Add(m_body2.m_sweep.c, r2);
m_u2 := Subtract(p2, s2);
length2 := Length(m_u2);
if length2 > b2_linearSlop then
DivideBy(m_u2, length2)
else
SetZero(m_u2);
{$ENDIF}
C := m_maxLength2 - length2;
linearError := b2Max(linearError, -C);
C := b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0);
impulse := -m_limitMass2 * C;
oldLimitPositionImpulse := m_limitPositionImpulse2;
m_limitPositionImpulse2 := b2Max(0.0, m_limitPositionImpulse2 + impulse);
impulse := m_limitPositionImpulse2 - oldLimitPositionImpulse;
{$IFDEF OP_OVERLOAD}
P2 := -impulse * m_u2;
m_body2.m_sweep.c.AddBy(m_body2.m_invMass * P2);
{$ELSE}
P2 := Multiply(m_u2, -impulse);
AddBy(m_body2.m_sweep.c, Multiply(P2, m_body2.m_invMass));
{$ENDIF}
m_body2.m_sweep.a := m_body2.m_sweep.a + m_body2.m_invI * b2Cross(r2, P2);
m_body2.SynchronizeTransform;
end;
Result := linearError < b2_linearSlop;
end;
function Tb2PulleyJoint.GetLength1: Float;
begin
{$IFDEF OP_OVERLOAD}
Result := (m_body1.GetWorldPoint(m_localAnchor1) - (m_ground.m_xf.position +
m_groundAnchor1)).Length;
{$ELSE}
Result := Length(Subtract(m_body1.GetWorldPoint(m_localAnchor1),
Add(m_ground.m_xf.position, m_groundAnchor1)));
{$ENDIF}
end;
function Tb2PulleyJoint.GetLength2: Float;
begin
{$IFDEF OP_OVERLOAD}
Result := (m_body2.GetWorldPoint(m_localAnchor2) - (m_ground.m_xf.position +
m_groundAnchor2)).Length;
{$ELSE}
Result := Length(Subtract(m_body2.GetWorldPoint(m_localAnchor2),
Add(m_ground.m_xf.position, m_groundAnchor2)));
{$ENDIF}
end;
function Tb2PulleyJoint.GetGroundAnchor1: TVector2;
begin
{$IFDEF OP_OVERLOAD}
Result := m_ground.m_xf.position + m_groundAnchor1;
{$ELSE}
Result := Add(m_ground.m_xf.position, m_groundAnchor1);
{$ENDIF}
end;
function Tb2PulleyJoint.GetGroundAnchor2: TVector2;
begin
{$IFDEF OP_OVERLOAD}
Result := m_ground.m_xf.position + m_groundAnchor2;
{$ELSE}
Result := Add(m_ground.m_xf.position, m_groundAnchor2);
{$ENDIF}
end;
{ Tb2RevoluteJointDef }
// Point-to-point constraint
// C = p2 - p1
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Motor constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
constructor Tb2RevoluteJointDef.Create;
begin
JointType := e_revoluteJoint;
SetZero(localAnchor1);
SetZero(localAnchor2);
referenceAngle := 0.0;
lowerAngle := 0.0;
upperAngle := 0.0;
maxMotorTorque := 0.0;
motorSpeed := 0.0;
enableLimit := False;
enableMotor := False;
end;
procedure Tb2RevoluteJointDef.Initialize(body1, body2: Tb2Body; const anchor: TVector2);
begin
Self.body1 := body1;
Self.body2 := body2;
localAnchor1 := body1.GetLocalPoint(anchor);
localAnchor2 := body2.GetLocalPoint(anchor);
referenceAngle := body2.GetAngle - body1.GetAngle;
end;
{ Tb2RevoluteJoint }
constructor Tb2RevoluteJoint.Create(def: Tb2RevoluteJointDef);
begin
inherited Create(def);
m_localAnchor1 := def.localAnchor1;
m_localAnchor2 := def.localAnchor2;
m_referenceAngle := def.referenceAngle;
SetZero(m_pivotForce);
m_motorForce := 0.0;
m_limitForce := 0.0;
m_limitPositionImpulse := 0.0;
m_lowerAngle := def.lowerAngle;
m_upperAngle := def.upperAngle;
m_maxMotorTorque := def.maxMotorTorque;
m_motorSpeed := def.motorSpeed;
m_enableLimit := def.enableLimit;
m_enableMotor := def.enableMotor;
end;
function Tb2RevoluteJoint.GetAnchor1: TVector2;
begin
Result := m_body1.GetWorldPoint(m_localAnchor1);
end;
function Tb2RevoluteJoint.GetAnchor2: TVector2;
begin
Result := m_body2.GetWorldPoint(m_localAnchor2);
end;
function Tb2RevoluteJoint.GetReactionForce: TVector2;
begin
Result := m_pivotForce;
end;
function Tb2RevoluteJoint.GetReactionTorque: Float;
begin
Result := m_limitForce;
end;
procedure Tb2RevoluteJoint.InitVelocityConstraints(const step: Tb2TimeStep);
var
r1, r2: TVector2;
K1, K2, K3, K: TMatrix22;
jointAngle: Float;
tmp: Float;
begin
// Compute the effective mass matrix.
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
{$ENDIF}
// K := [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// := [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
K1.col1.x := m_body1.m_invMass + m_body2.m_invMass;
K1.col2.x := 0.0;
K1.col1.y := 0.0;
K1.col2.y := K1.col1.x;
tmp := m_body1.m_invI;
K2.col1.x := tmp * r1.y * r1.y;
tmp := tmp * r1.x;
K2.col2.x := -tmp * r1.y;
K2.col1.y := K2.col2.x;
K2.col2.y := tmp * r1.x;
tmp := m_body2.m_invI;
K3.col1.x := tmp * r2.y * r2.y;
tmp := tmp * r2.x;
K3.col2.x := -tmp * r2.y;
K3.col1.y := K3.col2.x;
K3.col2.y := tmp * r2.x;
{$IFDEF OP_OVERLOAD}
K := K1 + K2 + K3;
m_pivotMass := K.Invert;
{$ELSE}
K := Add(K1, K2, K3);
m_pivotMass := Invert(K);
{$ENDIF}
m_motorMass := 1.0 / (m_body1.m_invI + m_body2.m_invI);
if not m_enableMotor then
m_motorForce := 0.0;
if m_enableLimit then
begin
jointAngle := m_body2.m_sweep.a - m_body1.m_sweep.a - m_referenceAngle;
if Abs(m_upperAngle - m_lowerAngle) < 2.0 * b2_angularSlop then
m_limitState := e_equalLimits
else if jointAngle <= m_lowerAngle then
begin
if m_limitState <> e_atLowerLimit then
m_limitForce := 0.0;
m_limitState := e_atLowerLimit;
end
else if jointAngle >= m_upperAngle then
begin
if m_limitState <> e_atUpperLimit then
m_limitForce := 0.0;
m_limitState := e_atUpperLimit;
end
else
begin
m_limitState := e_inactiveLimit;
m_limitForce := 0.0;
end;
end
else
m_limitForce := 0.0;
if step.warmStarting then
begin
{$IFDEF OP_OVERLOAD}
m_body1.m_linearVelocity.SubtractBy(step.dt * m_body1.m_invMass * m_pivotForce);
m_body2.m_linearVelocity.AddBy(step.dt * m_body2.m_invMass * m_pivotForce);
{$ELSE}
SubtractBy(m_body1.m_linearVelocity, Multiply(m_pivotForce, step.dt * m_body1.m_invMass));
AddBy(m_body2.m_linearVelocity, Multiply(m_pivotForce, step.dt * m_body2.m_invMass));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity - step.dt * m_body1.m_invI *
(b2Cross(r1, m_pivotForce) + m_motorForce + m_limitForce);
m_body2.m_angularVelocity := m_body2.m_angularVelocity + step.dt * m_body2.m_invI *
(b2Cross(r2, m_pivotForce) + m_motorForce + m_limitForce);
end
else
begin
SetZero(m_pivotForce);
m_motorForce := 0.0;
m_limitForce := 0.0;
end;
m_limitPositionImpulse := 0.0;
end;
procedure Tb2RevoluteJoint.SolveVelocityConstraints(const step: Tb2TimeStep);
var
r1, r2, pivotCdot, pivotForce, P: TVector2;
motorCdot, motorForce, oldMotorForce, limitCdot, limitForce, oldLimitForce, tmp: Float;
begin
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
// Solve point-to-point constraint
pivotCdot := m_body2.m_linearVelocity + b2Cross(m_body2.m_angularVelocity, r2) -
m_body1.m_linearVelocity - b2Cross(m_body1.m_angularVelocity, r1);
pivotForce := -step.inv_dt * b2Mul(m_pivotMass, pivotCdot);
m_pivotForce.AddBy(pivotForce);
P := step.dt * pivotForce;
m_body1.m_linearVelocity.SubtractBy(m_body1.m_invMass * P);
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P);
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
// Solve point-to-point constraint
pivotCdot := Subtract(m_body2.m_linearVelocity, m_body1.m_linearVelocity);
AddBy(pivotCdot, b2Cross(m_body2.m_angularVelocity, r2));
SubtractBy(pivotCdot, b2Cross(m_body1.m_angularVelocity, r1));
pivotForce := Multiply(b2Mul(m_pivotMass, pivotCdot), -step.inv_dt);
AddBy(m_pivotForce, pivotForce);
P := Multiply(pivotForce, step.dt);
SubtractBy(m_body1.m_linearVelocity, Multiply(P, m_body1.m_invMass));
AddBy(m_body2.m_linearVelocity, Multiply(P, m_body2.m_invMass));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity - m_body1.m_invI * b2Cross(r1, P);
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * b2Cross(r2, P);
if m_enableMotor and (m_limitState <> e_equalLimits) then
begin
motorCdot := m_body2.m_angularVelocity - m_body1.m_angularVelocity - m_motorSpeed;
motorForce := -step.inv_dt * m_motorMass * motorCdot;
oldMotorForce := m_motorForce;
m_motorForce := b2Clamp(m_motorForce + motorForce, -m_maxMotorTorque, m_maxMotorTorque);
motorForce := m_motorForce - oldMotorForce;
tmp := step.dt * motorForce;
m_body1.m_angularVelocity := m_body1.m_angularVelocity - m_body1.m_invI * tmp;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * tmp;
end;
if m_enableLimit and (m_limitState <> e_inactiveLimit) then
begin
limitCdot := m_body2.m_angularVelocity - m_body1.m_angularVelocity;
limitForce := -step.inv_dt * m_motorMass * limitCdot;
if m_limitState = e_equalLimits then
m_limitForce := m_limitForce + limitForce
else if m_limitState = e_atLowerLimit then
begin
oldLimitForce := m_limitForce;
m_limitForce := b2Max(m_limitForce + limitForce, 0.0);
limitForce := m_limitForce - oldLimitForce;
end
else if m_limitState = e_atUpperLimit then
begin
oldLimitForce := m_limitForce;
m_limitForce := b2Min(m_limitForce + limitForce, 0.0);
limitForce := m_limitForce - oldLimitForce;
end;
tmp := step.dt * limitForce;
m_body1.m_angularVelocity := m_body1.m_angularVelocity - m_body1.m_invI * tmp;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * tmp;
end;
end;
function Tb2RevoluteJoint.SolvePositionConstraints: Boolean;
var
positionError, angularError, angle, limitImpulse, limitC, oldLimitImpulse: Float;
r1, r2, ptpC, impulse: TVector2;
K1, K2, K3, K: TMatrix22;
tmp: Float;
begin
positionError := 0.0;
// Solve point-to-point position error.
{$IFDEF OP_OVERLOAD}
r1 := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
r2 := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
ptpC := m_body2.m_sweep.c + r2 - (m_body1.m_sweep.c + r1);
positionError := ptpC.Length;
{$ELSE}
r1 := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
r2 := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
ptpC := Subtract(Add(m_body2.m_sweep.c, r2), Add(m_body1.m_sweep.c, r1));
positionError := Length(ptpC);
{$ENDIF}
// Prevent overly large corrections.
//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
//ptpC := b2Clamp(ptpC, -dpMax, dpMax);
K1.col1.x := m_body1.m_invMass + m_body2.m_invMass;
K1.col2.x := 0.0;
K1.col1.y := 0.0;
K1.col2.y := K1.col1.x;
tmp := m_body1.m_invI;
K2.col1.x := tmp * r1.y * r1.y;
tmp := tmp * r1.x;
K2.col2.x := -tmp * r1.y;
K2.col1.y := K2.col2.x;
K2.col2.y := tmp * r1.x;
tmp := m_body2.m_invI;
K3.col1.x := tmp * r2.y * r2.y;
tmp := tmp * r2.x;
K3.col2.x := -tmp * r2.y;
K3.col1.y := K3.col2.x;
K3.col2.y := tmp * r2.x;
{$IFDEF OP_OVERLOAD}
K := K1 + K2 + K3;
impulse := K.Solve(-ptpC);
m_body1.m_sweep.c.SubtractBy(m_body1.m_invMass * impulse);
m_body2.m_sweep.c.AddBy(m_body2.m_invMass * impulse);
{$ELSE}
K := Add(K1, K2, K3);
impulse := Solve(K, Negative(ptpC));
SubtractBy(m_body1.m_sweep.c, Multiply(impulse, m_body1.m_invMass));
AddBy(m_body2.m_sweep.c, Multiply(impulse, m_body2.m_invMass));
{$ENDIF}
m_body1.m_sweep.a := m_body1.m_sweep.a - m_body1.m_invI * b2Cross(r1, impulse);
m_body2.m_sweep.a := m_body2.m_sweep.a + m_body2.m_invI * b2Cross(r2, impulse);
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
// Handle limits.
angularError := 0.0;
if m_enableLimit and (m_limitState <> e_inactiveLimit) then
begin
angle := m_body2.m_sweep.a - m_body1.m_sweep.a - m_referenceAngle;
limitImpulse := 0.0;
if m_limitState = e_equalLimits then
begin
// Prevent large angular corrections
limitC := b2Clamp(angle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
limitImpulse := -m_motorMass * limitC;
angularError := Abs(limitC);
end
else if m_limitState = e_atLowerLimit then
begin
limitC := angle - m_lowerAngle;
angularError := b2Max(0.0, -limitC);
// Prevent large angular corrections and allow some slop.
limitC := b2Clamp(limitC + b2_angularSlop, -b2_maxAngularCorrection, 0.0);
limitImpulse := -m_motorMass * limitC;
oldLimitImpulse := m_limitPositionImpulse;
m_limitPositionImpulse := b2Max(m_limitPositionImpulse + limitImpulse, 0.0);
limitImpulse := m_limitPositionImpulse - oldLimitImpulse;
end
else if m_limitState = e_atUpperLimit then
begin
limitC := angle - m_upperAngle;
angularError := b2Max(0.0, limitC);
// Prevent large angular corrections and allow some slop.
limitC := b2Clamp(limitC - b2_angularSlop, 0.0, b2_maxAngularCorrection);
limitImpulse := -m_motorMass * limitC;
oldLimitImpulse := m_limitPositionImpulse;
m_limitPositionImpulse := b2Min(m_limitPositionImpulse + limitImpulse, 0.0);
limitImpulse := m_limitPositionImpulse - oldLimitImpulse;
end;
m_body1.m_sweep.a := m_body1.m_sweep.a - m_body1.m_invI * limitImpulse;
m_body2.m_sweep.a := m_body2.m_sweep.a + m_body2.m_invI * limitImpulse;
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
end;
Result := (positionError <= b2_linearSlop) and (angularError <= b2_angularSlop);
end;
function Tb2RevoluteJoint.GetJointAngle: Float;
begin
Result := m_body2.m_sweep.a - m_body1.m_sweep.a - m_referenceAngle;
end;
function Tb2RevoluteJoint.GetJointSpeed: Float;
begin
Result := m_body2.m_angularVelocity - m_body1.m_angularVelocity;
end;
{ Tb2GearJointDef }
// Gear Joint:
// C0 = (coordinate1 + ratio * coordinate2)_initial
// C = C0 - (cordinate1 + ratio * coordinate2) = 0
// Cdot = -(Cdot1 + ratio * Cdot2)
// J = -[J1 ratio * J2]
// K = J * invM * JT
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
//
// Revolute:
// coordinate = rotation
// Cdot = angularVelocity
// J = [0 0 1]
// K = J * invM * JT = invI
//
// Prismatic:
// coordinate = dot(p - pg, ug)
// Cdot = dot(v + cross(w, r), ug)
// J = [ug cross(r, ug)]
// K = J * invM * JT = invMass + invI * cross(r, ug)^2
constructor Tb2GearJointDef.Create;
begin
JointType := e_gearJoint;
joint1 := nil;
joint2 := nil;
ratio := 1.0;
end;
{ Tb2GearJoint }
constructor Tb2GearJoint.Create(def: Tb2GearJointDef);
var
type1, type2: Tb2JointType;
coordinate1, coordinate2: Float;
begin
inherited Create(def);
type1 := def.joint1.GetType;
type2 := def.joint2.GetType;
//b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
//b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
//b2Assert(def.joint1.GetBody1().IsStatic());
//b2Assert(def.joint2.GetBody1().IsStatic());
m_revolute1 := nil;
m_prismatic1 := nil;
m_revolute2 := nil;
m_prismatic2 := nil;
m_ground1 := def.joint1.GetBody1;
m_body1 := def.joint1.GetBody2;
if type1 = e_revoluteJoint then
begin
m_revolute1 := Tb2RevoluteJoint(def.joint1);
m_groundAnchor1 := m_revolute1.m_localAnchor1;
m_localAnchor1 := m_revolute1.m_localAnchor2;
coordinate1 := m_revolute1.GetJointAngle;
end
else
begin
m_prismatic1 := Tb2PrismaticJoint(def.joint1);
m_groundAnchor1 := m_prismatic1.m_localAnchor1;
m_localAnchor1 := m_prismatic1.m_localAnchor2;
coordinate1 := m_prismatic1.GetJointTranslation;
end;
m_ground2 := def.joint2.GetBody1;
m_body2 := def.joint2.GetBody2;
if type2 = e_revoluteJoint then
begin
m_revolute2 := Tb2RevoluteJoint(def.joint2);
m_groundAnchor2 := m_revolute2.m_localAnchor1;
m_localAnchor2 := m_revolute2.m_localAnchor2;
coordinate2 := m_revolute2.GetJointAngle;
end
else
begin
m_prismatic2 := Tb2PrismaticJoint(def.joint2);
m_groundAnchor2 := m_prismatic2.m_localAnchor1;
m_localAnchor2 := m_prismatic2.m_localAnchor2;
coordinate2 := m_prismatic2.GetJointTranslation;
end;
m_ratio := def.ratio;
m_constant := coordinate1 + m_ratio * coordinate2;
m_force := 0.0;
end;
function Tb2GearJoint.GetAnchor1: TVector2;
begin
Result := m_body1.GetWorldPoint(m_localAnchor1);
end;
function Tb2GearJoint.GetAnchor2: TVector2;
begin
Result := m_body2.GetWorldPoint(m_localAnchor2);
end;
function Tb2GearJoint.GetReactionForce: TVector2;
begin
// TODO_ERIN not tested
{$IFDEF OP_OVERLOAD}
Result := m_force * m_J.linear2;
{$ELSE}
Result := Multiply(m_J.linear2, m_force);
{$ENDIF}
end;
function Tb2GearJoint.GetReactionTorque: Float;
var
r, F: TVector2;
begin
// TODO_ERIN not tested
{$IFDEF OP_OVERLOAD}
r := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
F := m_force * m_J.linear2;
{$ELSE}
r := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
F := Multiply(m_J.linear2, m_force);
{$ENDIF}
Result := m_force * m_J.angular2 - b2Cross(r, F);
end;
procedure Tb2GearJoint.InitVelocityConstraints(const step: Tb2TimeStep);
var
K, crug, P: Float;
ug, r: TVector2;
begin
K := 0.0;
{$IFDEF OP_OVERLOAD}
m_J.SetZero;
{$ELSE}
SetZero(m_J);
{$ENDIF}
if Assigned(m_revolute1) then
begin
m_J.angular1 := -1.0;
K := K + m_body1.m_invI;
end
else
begin
ug := b2Mul(m_ground1.m_xf.R, m_prismatic1.m_localXAxis1);
{$IFDEF OP_OVERLOAD}
r := b2Mul(m_body1.m_xf.R, m_localAnchor1 - m_body1.GetLocalCenter);
m_J.linear1 := -ug;
{$ELSE}
r := b2Mul(m_body1.m_xf.R, Subtract(m_localAnchor1, m_body1.GetLocalCenter));
m_J.linear1 := Negative(ug);
{$ENDIF}
crug := b2Cross(r, ug);
m_J.angular1 := -crug;
K := K + m_body1.m_invMass + m_body1.m_invI * crug * crug;
end;
if Assigned(m_revolute2) then
begin
m_J.angular2 := -m_ratio;
K := K + m_ratio * m_ratio * m_body2.m_invI;
end
else
begin
ug := b2Mul(m_ground2.m_xf.R, m_prismatic2.m_localXAxis1);
{$IFDEF OP_OVERLOAD}
r := b2Mul(m_body2.m_xf.R, m_localAnchor2 - m_body2.GetLocalCenter);
m_J.linear2 := -m_ratio * ug;
{$ELSE}
r := b2Mul(m_body2.m_xf.R, Subtract(m_localAnchor2, m_body2.GetLocalCenter));
m_J.linear2 := Multiply(ug, -m_ratio);
{$ENDIF}
crug := b2Cross(r, ug);
m_J.angular2 := -m_ratio * crug;
K := K + m_ratio * m_ratio * (m_body2.m_invMass + m_body2.m_invI * crug * crug);
end;
// Compute effective mass.
//b2Assert(K > 0.0);
m_mass := 1.0 / K;
if step.warmStarting then
begin
// Warm starting.
P := step.dt * m_force;
{$IFDEF OP_OVERLOAD}
m_body1.m_linearVelocity.AddBy(m_body1.m_invMass * P * m_J.linear1);
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P * m_J.linear2);
{$ELSE}
AddBy(m_body1.m_linearVelocity, Multiply(m_J.linear1, m_body1.m_invMass * P));
AddBy(m_body2.m_linearVelocity, Multiply(m_J.linear2, m_body2.m_invMass * P));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + m_body1.m_invI * P * m_J.angular1;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * P * m_J.angular2;
end
else
m_force := 0.0;
end;
procedure Tb2GearJoint.SolveVelocityConstraints(const step: Tb2TimeStep);
var
Cdot, force, P: Float;
begin
{$IFDEF OP_OVERLOAD}
Cdot := m_J.Compute(m_body1.m_linearVelocity, m_body2.m_linearVelocity,
m_body1.m_angularVelocity, m_body2.m_angularVelocity);
{$ELSE}
Cdot := Compute(m_J, m_body1.m_linearVelocity, m_body2.m_linearVelocity,
m_body1.m_angularVelocity, m_body2.m_angularVelocity);
{$ENDIF}
force := -step.inv_dt * m_mass * Cdot;
m_force := m_force + force;
P := step.dt * force;
{$IFDEF OP_OVERLOAD}
m_body1.m_linearVelocity.AddBy(m_body1.m_invMass * P * m_J.linear1);
m_body2.m_linearVelocity.AddBy(m_body2.m_invMass * P * m_J.linear2);
{$ELSE}
AddBy(m_body1.m_linearVelocity, Multiply(m_J.linear1, m_body1.m_invMass * P));
AddBy(m_body2.m_linearVelocity, Multiply(m_J.linear2, m_body2.m_invMass * P));
{$ENDIF}
m_body1.m_angularVelocity := m_body1.m_angularVelocity + m_body1.m_invI * P * m_J.angular1;
m_body2.m_angularVelocity := m_body2.m_angularVelocity + m_body2.m_invI * P * m_J.angular2;
end;
function Tb2GearJoint.SolvePositionConstraints: Boolean;
var
linearError, coordinate1, coordinate2, C, impulse: Float;
begin
linearError := 0.0;
if Assigned(m_revolute1) then
coordinate1 := m_revolute1.GetJointAngle()
else
coordinate1 := m_prismatic1.GetJointTranslation;
if Assigned(m_revolute2) then
coordinate2 := m_revolute2.GetJointAngle()
else
coordinate2 := m_prismatic2.GetJointTranslation;
C := m_constant - (coordinate1 + m_ratio * coordinate2);
impulse := -m_mass * C;
{$IFDEF OP_OVERLOAD}
m_body1.m_sweep.c.AddBy(m_body1.m_invMass * impulse * m_J.linear1);
m_body2.m_sweep.c.AddBy(m_body2.m_invMass * impulse * m_J.linear2);
{$ELSE}
AddBy(m_body1.m_sweep.c, Multiply(m_J.linear1, m_body1.m_invMass * impulse));
AddBy(m_body2.m_sweep.c, Multiply(m_J.linear2, m_body2.m_invMass * impulse));
{$ENDIF}
m_body1.m_sweep.a := m_body1.m_sweep.a + m_body1.m_invI * impulse * m_J.angular1;
m_body2.m_sweep.a := m_body2.m_sweep.a + m_body2.m_invI * impulse * m_J.angular2;
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
Result := linearError < b2_linearSlop;
end;
{ Tb2FixedJointDef }
constructor Tb2FixedJointDef.Create;
begin
JointType := e_fixedJoint;
end;
procedure Tb2FixedJointDef.Initialize(body1, body2: Tb2Body);
begin
Self.body1 := body1;
Self.body2 := body2;
end;
{ Tb2FixedJoint }
constructor Tb2FixedJoint.Create(def: Tb2FixedJointDef);
begin
inherited Create(def);
// Get initial delta position and angle
{$IFDEF OP_OVERLOAD}
m_dp := b2MulT(m_body1.m_xf.R, m_body2.m_xf.position - m_body1.m_xf.position);
{$ELSE}
m_dp := b2MulT(m_body1.m_xf.R, Subtract(m_body2.m_xf.position, m_body1.m_xf.position));
{$ENDIF}
m_a := m_body2.GetAngle - m_body1.GetAngle;
m_R0 := b2MulT(m_body1.m_xf.R, m_body2.m_xf.R);
// Reset accumulated lambda
m_lambda[0] := 0.0;
m_lambda[1] := 0.0;
m_lambda[2] := 0.0;
end;
procedure Tb2FixedJoint.CalculateMC;
var
invM12, invI1, a, b, c, d, e, f, c1, c2, c3, den: Float;
begin
UPhysics2DTypes.SinCos(m_body1.m_sweep.a, m_s, m_c);
// Calculate vector A w1 := d/dt (R(a1) d)
m_Ax := -m_s * m_d.x - m_c * m_d.y;
m_Ay := m_c * m_d.x - m_s * m_d.y;
// Calculate effective constraint mass: mC := (J M^-1 J^T)^-1
invM12 := m_body1.m_invMass + m_body2.m_invMass;
invI1 := m_body1.m_invI;
a := invM12 + m_Ax * m_Ax * invI1;
b := m_Ax * m_Ay * invI1;
c := m_Ax * invI1;
d := invM12 + m_Ay * m_Ay * invI1;
e := m_Ay * invI1;
f := m_body1.m_invI + invI1;
c1 := d * f - e * e;
c2 := c * e - b * f;
c3 := b * e - c * d;
den := a * c1 + b * c2 + c * c3;
m_mc[0][0] := c1 / den;
m_mc[1][0] := c2 / den;
m_mc[2][0] := c3 / den;
m_mc[0][1] := m_mc[1][0];
m_mc[1][1] := (a * f - c * c ) / den;
m_mc[2][1] := (b * c - a * e ) / den;
m_mc[0][2] := m_mc[2][0];
m_mc[1][2] := m_mc[2][1];
m_mc[2][2] := (a * d - b * b ) / den;
end;
function Tb2FixedJoint.GetAnchor1: TVector2;
begin
// Return arbitrary position (we have to implement this abstract virtual function)
Result := m_body1.GetWorldCenter;
end;
function Tb2FixedJoint.GetAnchor2: TVector2;
begin
// Return arbitrary position (we have to implement this abstract virtual function)
Result := m_body2.GetWorldCenter;
end;
function Tb2FixedJoint.GetReactionForce: TVector2;
begin
{$IFDEF OP_OVERLOAD}
Result := m_inv_dt * MakeVector(m_lambda[0], m_lambda[1]);
{$ELSE}
Result := Multiply(MakeVector(m_lambda[0], m_lambda[1]), m_inv_dt);
{$ENDIF}
end;
function Tb2FixedJoint.GetReactionTorque: Float;
begin
Result := m_inv_dt * m_lambda[2];
end;
procedure Tb2FixedJoint.InitVelocityConstraints(const step: Tb2TimeStep);
begin
// Store step
m_inv_dt := step.inv_dt;
// Get d for this step (transform from delta between positions to delta between center of masses)
{$IFDEF OP_OVERLOAD}
m_d := m_dp - m_body1.m_sweep.localCenter + b2Mul(m_R0, m_body2.m_sweep.localCenter);
{$ELSE}
m_d := Add(Subtract(m_dp, m_body1.m_sweep.localCenter), b2Mul(m_R0, m_body2.m_sweep.localCenter));
{$ENDIF}
// Calculate effective joint mass (stays constant during velocity solving)
CalculateMC;
if step.warmStarting then
begin
// Apply initial impulse
with m_body1 do
begin
m_linearVelocity.x := m_linearVelocity.x - m_invMass * m_lambda[0];
m_linearVelocity.y := m_linearVelocity.y - m_invMass * m_lambda[1];
m_angularVelocity := m_angularVelocity - m_invI * (m_lambda[0] * m_Ax + m_lambda[1] * m_Ay + m_lambda[2]);
end;
with m_body2 do
begin
m_linearVelocity.x := m_linearVelocity.x + m_invMass * m_lambda[0];
m_linearVelocity.y := m_linearVelocity.y + m_invMass * m_lambda[1];
m_angularVelocity := m_angularVelocity + m_invI * m_lambda[2];
end;
end
else
begin
// Reset accumulated lambda
m_lambda[0] := 0.0;
m_lambda[1] := 0.0;
m_lambda[2] := 0.0;
end;
end;
procedure Tb2FixedJoint.SolveVelocityConstraints(const step: Tb2TimeStep);
var
i: Integer;
Cdot, lambda: array[0..2] of Float;
begin
// Assert that angle is still the same so the effective joint mass is still valid
//assert(m_body1.m_sweep.a == m_a1);
// Calculate Cdot
Cdot[0] := m_body2.m_linearVelocity.x - m_body1.m_linearVelocity.x - m_Ax * m_body1.m_angularVelocity;
Cdot[1] := m_body2.m_linearVelocity.y - m_body1.m_linearVelocity.y - m_Ay * m_body1.m_angularVelocity;
Cdot[2] := m_body2.m_angularVelocity - m_body1.m_angularVelocity;
// Calculate lambda
for i := 0 to 2 do
lambda[i] := -(m_mc[i][0] * Cdot[0] + m_mc[i][1] * Cdot[1] + m_mc[i][2] * Cdot[2]);
// Apply impulse
with m_body1 do
begin
m_linearVelocity.x := m_linearVelocity.x - m_invMass * lambda[0];
m_linearVelocity.y := m_linearVelocity.y - m_invMass * lambda[1];
m_angularVelocity := m_angularVelocity - m_invI * (lambda[0] * m_Ax + lambda[1] * m_Ay + lambda[2]);
end;
with m_body2 do
begin
m_linearVelocity.x := m_linearVelocity.x + m_invMass * lambda[0];
m_linearVelocity.y := m_linearVelocity.y + m_invMass * lambda[1];
m_angularVelocity := m_angularVelocity + m_invI * lambda[2];
end;
// Accumulate total lambda
for i := 0 to 2 do
m_lambda[i] := m_lambda[i] + lambda[i];
end;
function Tb2FixedJoint.SolvePositionConstraints: Boolean;
var
i: Integer;
C, lambda: array[0..2] of Float;
begin
// Recalculate effective constraint mass if angle changed enough
if Abs(m_body1.m_sweep.a - m_a1) > 1e-3 then
CalculateMC;
// Calculate C
C[0] := m_body2.m_sweep.c.x - m_body1.m_sweep.c.x - m_c * m_d.x + m_s * m_d.y;
C[1] := m_body2.m_sweep.c.y - m_body1.m_sweep.c.y - m_s * m_d.x - m_c * m_d.y;
C[2] := m_body2.m_sweep.a - m_a1 - m_a;
// Calculate lambda
for i := 0 to 2 do
lambda[i] := -(m_mc[i][0] * C[0] + m_mc[i][1] * C[1] + m_mc[i][2] * C[2]);
// Apply impulse
with m_body1, m_sweep do
begin
c.x := c.x - m_invMass * lambda[0];
c.y := c.y - m_invMass * lambda[1];
a := a - m_invI * (lambda[0] * m_Ax + lambda[1] * m_Ay + lambda[2]);
end;
with m_body2, m_sweep do
begin
c.x := c.x + m_invMass * lambda[0];
c.y := c.y + m_invMass * lambda[1];
a := a + m_invI * lambda[2];
end;
// Push the changes to the transforms
m_body1.SynchronizeTransform;
m_body2.SynchronizeTransform;
// Constraint is satisfied if all constraint equations are nearly zero
Result := (Abs(C[0]) < b2_linearSlop) and (Abs(C[1]) < b2_linearSlop) and (Abs(C[2]) < b2_angularSlop);
end;
initialization
b2_defaultFilter := Tb2ContactFilter.Create;
g_GJK_Iterations := 0;
{$IFDEF CLASSVAR_AVAIL}
Tb2BroadPhase.s_validate := False;
{$ELSE}
b2BroadPhase_s_validate := False;
{$ENDIF}
finalization
b2_defaultFilter.Free;
{$IFDEF DEBUG_LOG}
DEBUG.Free;
{$ENDIF}
end.