public TEntity NearestBelow(IEntity target, Vector2 minDist = default(Vector2), Vector2 maxDist = default(Vector2))
{
return(this.entities.OrderBy(e => Vector2.Distance(LowerLeft(target), LowerLeft(e)))
.FirstOrDefault(e => InRange(MathUtils.Abs(LowerLeft(target) - LowerLeft(e)), minDist, maxDist)));
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
float wA = c.BodyA.AngularVelocityInternal;
float wB = c.BodyB.AngularVelocityInternal;
float tangentx = c.Normal.Y;
float tangenty = -c.Normal.X;
float friction = c.Friction;
Debug.Assert(c.PointCount == 1 || c.PointCount == 2);
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
float lambda = ccp.TangentMass *
-((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) -
c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * tangentx +
(c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) -
c.BodyA.LinearVelocityInternal.Y - (wA * ccp.rA.X)) * tangenty);
// MathUtils.Clamp the accumulated force
float maxFriction = friction * ccp.NormalImpulse;
float newImpulse = Math.Max(-maxFriction, Math.Min(ccp.TangentImpulse + lambda, maxFriction));
lambda = newImpulse - ccp.TangentImpulse;
// Apply contact impulse
float px = lambda * tangentx;
float py = lambda * tangenty;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
ccp.TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
// Relative velocity at contact
// Compute normal impulse
float lambda = -ccp.NormalMass *
((c.BodyB.LinearVelocityInternal.X + (-wB * ccp.rB.Y) -
c.BodyA.LinearVelocityInternal.X - (-wA * ccp.rA.Y)) * c.Normal.X +
(c.BodyB.LinearVelocityInternal.Y + (wB * ccp.rB.X) -
c.BodyA.LinearVelocityInternal.Y -
(wA * ccp.rA.X)) * c.Normal.Y - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
float px = lambda * c.Normal.X;
float py = lambda * c.Normal.Y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * px;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * py;
wA -= c.BodyA.InvI * (ccp.rA.X * py - ccp.rA.Y * px);
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * px;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * py;
wB += c.BodyB.InvI * (ccp.rB.X * py - ccp.rB.Y * px);
ccp.NormalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
ContactConstraintPoint cp1 = c.Points[0];
ContactConstraintPoint cp2 = c.Points[1];
float ax = cp1.NormalImpulse;
float ay = cp2.NormalImpulse;
Debug.Assert(ax >= 0.0f && ay >= 0.0f);
// Relative velocity at contact
// Compute normal velocity
float vn1 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp1.rB.Y) - c.BodyA.LinearVelocityInternal.X -
(-wA * cp1.rA.Y)) * c.Normal.X +
(c.BodyB.LinearVelocityInternal.Y + (wB * cp1.rB.X) - c.BodyA.LinearVelocityInternal.Y -
(wA * cp1.rA.X)) * c.Normal.Y;
float vn2 = (c.BodyB.LinearVelocityInternal.X + (-wB * cp2.rB.Y) - c.BodyA.LinearVelocityInternal.X -
(-wA * cp2.rA.Y)) * c.Normal.X +
(c.BodyB.LinearVelocityInternal.Y + (wB * cp2.rB.X) - c.BodyA.LinearVelocityInternal.Y -
(wA * cp2.rA.X)) * c.Normal.Y;
float bx = vn1 - cp1.VelocityBias - (c.K.Col1.X * ax + c.K.Col2.X * ay);
float by = vn2 - cp2.VelocityBias - (c.K.Col1.Y * ax + c.K.Col2.Y * ay);
float xx = -(c.NormalMass.Col1.X * bx + c.NormalMass.Col2.X * by);
float xy = -(c.NormalMass.Col1.Y * bx + c.NormalMass.Col2.Y * by);
while (true)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
if (xx >= 0.0f && xy >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
float k_errorTol = 1e-3f;
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
xx = -cp1.NormalMass * bx;
xy = 0.0f;
vn1 = 0.0f;
vn2 = c.K.Col1.Y * xx + by;
if (xx >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
xx = 0.0f;
xy = -cp2.NormalMass * by;
vn1 = c.K.Col2.X * xy + bx;
vn2 = 0.0f;
if (xy >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
xx = 0.0f;
xy = 0.0f;
vn1 = bx;
vn2 = by;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
float dx = xx - ax;
float dy = xy - ay;
// Apply incremental impulse
float p1x = dx * c.Normal.X;
float p1y = dx * c.Normal.Y;
float p2x = dy * c.Normal.X;
float p2y = dy * c.Normal.Y;
float p12x = p1x + p2x;
float p12y = p1y + p2y;
c.BodyA.LinearVelocityInternal.X -= c.BodyA.InvMass * p12x;
c.BodyA.LinearVelocityInternal.Y -= c.BodyA.InvMass * p12y;
wA -= c.BodyA.InvI * ((cp1.rA.X * p1y - cp1.rA.Y * p1x) + (cp2.rA.X * p2y - cp2.rA.Y * p2x));
c.BodyB.LinearVelocityInternal.X += c.BodyB.InvMass * p12x;
c.BodyB.LinearVelocityInternal.Y += c.BodyB.InvMass * p12y;
wB += c.BodyB.InvI * ((cp1.rB.X * p1y - cp1.rB.Y * p1x) + (cp2.rB.X * p2y - cp2.rB.Y * p2x));
// Accumulate
cp1.NormalImpulse = xx;
cp2.NormalImpulse = xy;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
c.BodyA.AngularVelocityInternal = wA;
c.BodyB.AngularVelocityInternal = wB;
}
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// </summary>
/// <param slotName="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param slotName="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func <RayCastInput, int, float> callback, ref RayCastInput input)
{
Vector2 p1 = input.Point1;
Vector2 p2 = input.Point2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_raycastStack.Clear();
_raycastStack.Push(_root);
while (_raycastStack.Count > 0)
{
int nodeId = _raycastStack.Pop();
if (nodeId == NullNode)
{
continue;
}
TreeNode <T> node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = node.AABB.Center;
Vector2 h = node.AABB.Extents;
float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
Vector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = Vector2.Min(p1, t);
segmentAABB.UpperBound = Vector2.Max(p1, t);
}
}
else
{
_raycastStack.Push(node.Child1);
_raycastStack.Push(node.Child2);
}
}
}
public override bool SolvePositionConstraints(SolverData data)
{
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 rA = Pool.PopVec2();
Vec2 rB = Pool.PopVec2();
Vec2 uA = Pool.PopVec2();
Vec2 uB = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
Vec2 PA = Pool.PopVec2();
Vec2 PB = Pool.PopVec2();
Vec2 cA = data.Positions[IndexA].C;
float aA = data.Positions[IndexA].A;
Vec2 cB = data.Positions[IndexB].C;
float aB = data.Positions[IndexB].A;
qA.Set(aA);
qB.Set(aB);
Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), rA);
Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), rB);
uA.Set(cA).AddLocal(rA).SubLocal(m_groundAnchorA);
uB.Set(cB).AddLocal(rB).SubLocal(m_groundAnchorB);
float lengthA = uA.Length();
float lengthB = uB.Length();
if (lengthA > 10.0f * Settings.LINEAR_SLOP)
{
uA.MulLocal(1.0f / lengthA);
}
else
{
uA.SetZero();
}
if (lengthB > 10.0f * Settings.LINEAR_SLOP)
{
uB.MulLocal(1.0f / lengthB);
}
else
{
uB.SetZero();
}
// Compute effective mass.
float ruA = Vec2.Cross(rA, uA);
float ruB = Vec2.Cross(rB, uB);
float mA = InvMassA + InvIA * ruA * ruA;
float mB = InvMassB + InvIB * ruB * ruB;
float mass = mA + m_ratio * m_ratio * mB;
if (mass > 0.0f)
{
mass = 1.0f / mass;
}
float C = m_constant - lengthA - m_ratio * lengthB;
float linearError = MathUtils.Abs(C);
float impulse = (-mass) * C;
PA.Set(uA).MulLocal(-impulse);
PB.Set(uB).MulLocal((-m_ratio) * impulse);
cA.X += InvMassA * PA.X;
cA.Y += InvMassA * PA.Y;
aA += InvIA * Vec2.Cross(rA, PA);
cB.X += InvMassB * PB.X;
cB.Y += InvMassB * PB.Y;
aB += InvIB * Vec2.Cross(rB, PB);
data.Positions[IndexA].C.Set(cA);
data.Positions[IndexA].A = aA;
data.Positions[IndexB].C.Set(cB);
data.Positions[IndexB].A = aB;
Pool.PushRot(2);
Pool.PushVec2(7);
return(linearError < Settings.LINEAR_SLOP);
}
private void OnUpdateControlDot(float x, float y)
{
if (x == 0 && y == 0)
{
Position(0, 0, Config.EMPTY);
return;
}
if (this.allowDiagonal)
{
float angle = MathUtils.RadToDeg(MathUtils.Atan2(x, y)) + 180;
if (this.CheckAngle(0, angle) || this.CheckAngle(360, angle))
{
Position(0, -SIDE, Config.TUP);
}
else if (this.CheckAngle(45, angle))
{
Position(-DIAGONAL, -DIAGONAL, Config.LEFT);
}
else if (this.CheckAngle(90, angle))
{
Position(-SIDE, 0, Config.TLEFT);
}
else if (this.CheckAngle(135, angle))
{
Position(-DIAGONAL, DIAGONAL, Config.DOWN);
}
else if (this.CheckAngle(180, angle))
{
Position(0, SIDE, Config.TDOWN);
}
else if (this.CheckAngle(225, angle))
{
Position(DIAGONAL, DIAGONAL, Config.RIGHT);
}
else if (this.CheckAngle(270, angle))
{
Position(SIDE, 0, Config.TRIGHT);
}
else if (this.CheckAngle(315, angle))
{
Position(DIAGONAL, -DIAGONAL, Config.UP);
}
else
{
Position(0, 0, Config.EMPTY);
}
}
else
{
if (MathUtils.Abs(x) > MathUtils.Abs(y))
{
if (x > 0)
{
Position(SIDE, 0, Config.RIGHT);
}
else if (x < 0)
{
Position(-SIDE, 0, Config.LEFT);
}
else if (x == 0)
{
Position(0, 0, Config.EMPTY);
}
}
else
{
if (y > 0)
{
Position(0, SIDE, Config.DOWN);
}
else if (y < 0)
{
Position(0, -SIDE, Config.UP);
}
else if (y == 0)
{
Position(0, 0, Config.EMPTY);
}
}
}
}
public Vector2f Abs()
{
return(new Vector2f(MathUtils.Abs(x), MathUtils.Abs(y)));
}
public static Vector2f Abs(Vector2f a)
{
return(new Vector2f(MathUtils.Abs(a.x), MathUtils.Abs(a.y)));
}
public void Solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep)
{
// Console.WriteLine("Solving Island");
float h = step.Dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
Vec2 c = b.Sweep.C;
float a = b.Sweep.A;
Vec2 v = b.LinearVelocity;
float w = b.AngularVelocity;
// Store positions for continuous collision.
b.Sweep.C0.Set(b.Sweep.C);
b.Sweep.A0 = b.Sweep.A;
if (b.Type == BodyType.Dynamic)
{
// Integrate velocities.
// v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force);
v.X += h * (b.GravityScale * gravity.X + b.InvMass * b.Force.X);
v.Y += h * (b.GravityScale * gravity.Y + b.InvMass * b.Force.Y);
w += h * b.InvI * b.Torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v *
// exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v.MulLocal(MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f));
w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
//Debug.Assert (v.x == 0);
Positions[i].C.Set(c);
Positions[i].A = a;
Velocities[i].V.Set(v);
Velocities[i].W = w;
}
timer.Reset();
// Solver data
solverData.Step = step;
solverData.Positions = Positions;
solverData.Velocities = Velocities;
// Initialize velocity constraints.
solverDef.Step = step;
solverDef.Contacts = Contacts;
solverDef.Count = ContactCount;
solverDef.Positions = Positions;
solverDef.Velocities = Velocities;
contactSolver.Init(solverDef);
//Console.WriteLine("island init vel");
contactSolver.InitializeVelocityConstraints();
if (step.WarmStarting)
{
//Console.WriteLine("island warm start");
contactSolver.WarmStart();
}
for (int i = 0; i < JointCount; ++i)
{
Joints[i].InitVelocityConstraints(solverData);
}
profile.SolveInit = timer.Milliseconds;
// Solve velocity constraints
timer.Reset();
//Console.WriteLine("island solving velocities");
for (int i = 0; i < step.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
profile.SolveVelocity = timer.Milliseconds;
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
Vec2 c = Positions[i].C;
float a = Positions[i].A;
Vec2 v = Velocities[i].V;
float w = Velocities[i].W;
// Check for large velocities
translation.X = v.X * h;
translation.Y = v.Y * h;
if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED)
{
float ratio = Settings.MAX_TRANSLATION / translation.Length();
v.X *= ratio;
v.Y *= ratio;
}
float rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation);
w *= ratio;
}
// Integrate
c.X += h * v.X;
c.Y += h * v.Y;
a += h * w;
Positions[i].A = a;
Velocities[i].W = w;
}
// Solve position constraints
timer.Reset();
bool positionSolved = false;
for (int i = 0; i < step.PositionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
bool jointOkay = Joints[j].SolvePositionConstraints(solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body.Sweep.C.Set(Positions[i].C);
body.Sweep.A = Positions[i].A;
body.LinearVelocity.Set(Velocities[i].V);
body.AngularVelocity = Velocities[i].W;
body.SynchronizeTransform();
}
profile.SolvePosition = timer.Milliseconds;
Report(contactSolver.VelocityConstraints);
if (allowSleep)
{
float minSleepTime = Single.MaxValue;
const float linTolSqr = Settings.LINEAR_SLEEP_TOLERANCE * Settings.LINEAR_SLEEP_TOLERANCE;
float angTolSqr = Settings.ANGULAR_SLEEP_TOLERANCE * Settings.ANGULAR_SLEEP_TOLERANCE;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.Type == BodyType.Static)
{
continue;
}
if ((b.Flags & Body.TypeFlags.AutoSleep) == 0 || b.AngularVelocity * b.AngularVelocity > angTolSqr || Vec2.Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = MathUtils.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TIME_TO_SLEEP && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
public void SolveToi(TimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < BodyCount);
Debug.Assert(toiIndexB < BodyCount);
// Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
Positions[i].C.Set(b.Sweep.C);
Positions[i].A = b.Sweep.A;
Velocities[i].V.Set(b.LinearVelocity);
Velocities[i].W = b.AngularVelocity;
}
toiSolverDef.Contacts = Contacts;
toiSolverDef.Count = ContactCount;
toiSolverDef.Step = subStep;
toiSolverDef.Positions = Positions;
toiSolverDef.Velocities = Velocities;
toiContactSolver.Init(toiSolverDef);
// Solve position constraints.
for (int i = 0; i < subStep.PositionIterations; ++i)
{
bool contactsOkay = toiContactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
// #if 0
// // Is the new position really safe?
// for (int i = 0; i < m_contactCount; ++i)
// {
// Contact* c = m_contacts[i];
// Fixture* fA = c.GetFixtureA();
// Fixture* fB = c.GetFixtureB();
//
// Body bA = fA.GetBody();
// Body bB = fB.GetBody();
//
// int indexA = c.GetChildIndexA();
// int indexB = c.GetChildIndexB();
//
// DistanceInput input;
// input.proxyA.Set(fA.GetShape(), indexA);
// input.proxyB.Set(fB.GetShape(), indexB);
// input.transformA = bA.GetTransform();
// input.transformB = bB.GetTransform();
// input.useRadii = false;
//
// DistanceOutput output;
// SimplexCache cache;
// cache.count = 0;
// Distance(&output, &cache, &input);
//
// if (output.distance == 0 || cache.count == 3)
// {
// cache.count += 0;
// }
// }
// #endif
// Leap of faith to new safe state.
Bodies[toiIndexA].Sweep.C0.Set(Positions[toiIndexA].C);
Bodies[toiIndexA].Sweep.A0 = Positions[toiIndexA].A;
Bodies[toiIndexB].Sweep.C0.Set(Positions[toiIndexB].C);
Bodies[toiIndexB].Sweep.A0 = Positions[toiIndexB].A;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
toiContactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < subStep.VelocityIterations; ++i)
{
toiContactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float h = subStep.Dt;
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
Vec2 c = Positions[i].C;
float a = Positions[i].A;
Vec2 v = Velocities[i].V;
float w = Velocities[i].W;
// Check for large velocities
translation.Set(v).MulLocal(h);
if (Vec2.Dot(translation, translation) > Settings.MAX_TRANSLATION_SQUARED)
{
float ratio = Settings.MAX_TRANSLATION / translation.Length();
v.MulLocal(ratio);
}
float rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MAX_ROTATION / MathUtils.Abs(rotation);
w *= ratio;
}
// Integrate
c.X += v.X * h;
c.Y += v.Y * h;
a += h * w;
Positions[i].C.Set(c);
Positions[i].A = a;
Velocities[i].V.Set(v);
Velocities[i].W = w;
// Sync bodies
Body body = Bodies[i];
body.Sweep.C.Set(c);
body.Sweep.A = a;
body.LinearVelocity.Set(v);
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(toiContactSolver.VelocityConstraints);
}
public float LenManhattan()
{
return(MathUtils.Abs(this.x) + MathUtils.Abs(this.y));
}
public static void AbsToOut(Vector2f a, Vector2f outs)
{
outs.x = MathUtils.Abs(a.x);
outs.y = MathUtils.Abs(a.y);
}
public bool IsUnit(float margin)
{
return(MathUtils.Abs(Len2() - 1f) < margin);
}
protected internal override void ProcessTouchPressed()
{
float x = MathUtils.BringToBounds(0, baseWidth, Touch.GetX()
- GetScreenX())
/ baseWidth - 0.5f;
float y = MathUtils.BringToBounds(0, baseHeight, Touch.GetY()
- GetScreenY())
/ baseHeight - 0.5f;
if (x == 0 && y == 0)
{
return;
}
if (MathUtils.Abs(x) > MathUtils.Abs(y))
{
if (x > 0)
{
this.isRight = true;
this.isClick = true;
this.centerX = offsetX + x + (baseWidth - dotWidth) / 2
+ dotWidth * 0.75f;
this.centerY = offsetY + y + (baseHeight - dotHeight) / 2;
if (listener != null)
{
listener.Right();
}
}
else if (x < 0)
{
this.isLeft = true;
this.isClick = true;
this.centerX = offsetX + x + (baseWidth - dotWidth) / 2
- dotWidth * 0.75f;
this.centerY = offsetY + y + (baseHeight - dotHeight) / 2;
if (listener != null)
{
listener.Left();
}
}
else if (x == 0)
{
FreeClick();
}
}
else
{
if (y > 0)
{
this.isDown = true;
this.isClick = true;
this.centerX = offsetX + x + (baseWidth - dotWidth) / 2 - 1;
this.centerY = offsetY + y + (baseHeight - dotHeight) / 2
+ dotHeight * 0.75f;
if (listener != null)
{
listener.Down();
}
}
else if (y < 0)
{
this.isUp = true;
this.isClick = true;
this.centerX = offsetX + x + (baseWidth - dotWidth) / 2 - 1;
this.centerY = offsetY + y + (baseHeight - dotHeight) / 2
- dotHeight * 0.75f;
if (listener != null)
{
listener.Up();
}
}
else if (y == 0)
{
FreeClick();
}
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _constraintCount; ++i)
{
ContactConstraint c = Constraints[i];
Body bodyA = c.BodyA;
Body bodyB = c.BodyB;
float wA = bodyA.AngularVelocityInternal;
float wB = bodyB.AngularVelocityInternal;
Vector2 vA = bodyA.LinearVelocityInternal;
Vector2 vB = bodyB.LinearVelocityInternal;
float invMassA = bodyA.InvMass;
float invIA = bodyA.InvI;
float invMassB = bodyB.InvMass;
float invIB = bodyB.InvI;
Vector2 normal = c.Normal;
#if MATH_OVERLOADS
Vector2 tangent = MathUtils.Cross(normal, 1.0f);
#else
Vector2 tangent = new Vector2(normal.Y, -normal.X);
#endif
float friction = c.Friction;
Debug.Assert(c.PointCount == 1 || c.PointCount == 2);
// Solve tangent constraints
for (int j = 0; j < c.PointCount; ++j)
{
ContactConstraintPoint ccp = c.Points[j];
#if MATH_OVERLOADS
// Relative velocity at contact
Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA);
// Compute tangent force
float vt = Vector2.Dot(dv, tangent);
#else
// Relative velocity at contact
Vector2 dv = new Vector2(vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y),
vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X));
// Compute tangent force
float vt = dv.X * tangent.X + dv.Y * tangent.Y;
#endif
float lambda = ccp.TangentMass * (-vt);
// MathUtils.Clamp the accumulated force
float maxFriction = friction * ccp.NormalImpulse;
float newImpulse = MathUtils.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - ccp.TangentImpulse;
#if MATH_OVERLOADS
// Apply contact impulse
Vector2 P = lambda * tangent;
vA -= invMassA * P;
wA -= invIA * MathUtils.Cross(ccp.rA, P);
vB += invMassB * P;
wB += invIB * MathUtils.Cross(ccp.rB, P);
#else
// Apply contact impulse
Vector2 P = new Vector2(lambda * tangent.X, lambda * tangent.Y);
vA.X -= invMassA * P.X;
vA.Y -= invMassA * P.Y;
wA -= invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X);
vB.X += invMassB * P.X;
vB.Y += invMassB * P.Y;
wB += invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X);
#endif
ccp.TangentImpulse = newImpulse;
}
// Solve normal constraints
if (c.PointCount == 1)
{
ContactConstraintPoint ccp = c.Points[0];
#if MATH_OVERLOADS
// Relative velocity at contact
Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA);
// Compute normal impulse
float vn = Vector2.Dot(dv, normal);
float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
// MathUtils.Clamp the accumulated impulse
float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
Vector2 P = lambda * normal;
vA -= invMassA * P;
wA -= invIA * MathUtils.Cross(ccp.rA, P);
vB += invMassB * P;
wB += invIB * MathUtils.Cross(ccp.rB, P);
#else
// Relative velocity at contact
Vector2 dv = new Vector2(vB.X + (-wB * ccp.rB.Y) - vA.X - (-wA * ccp.rA.Y),
vB.Y + (wB * ccp.rB.X) - vA.Y - (wA * ccp.rA.X));
// Compute normal impulse
float vn = dv.X * normal.X + dv.Y * normal.Y;
float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);
// Clamp the accumulated impulse
float newImpulse = Math.Max(ccp.NormalImpulse + lambda, 0.0f);
lambda = newImpulse - ccp.NormalImpulse;
// Apply contact impulse
Vector2 P = new Vector2(lambda * normal.X, lambda * normal.Y);
vA.X -= invMassA * P.X;
vA.Y -= invMassA * P.Y;
wA -= invIA * (ccp.rA.X * P.Y - ccp.rA.Y * P.X);
vB.X += invMassB * P.X;
vB.Y += invMassB * P.Y;
wB += invIB * (ccp.rB.X * P.Y - ccp.rB.Y * P.X);
#endif
ccp.NormalImpulse = newImpulse;
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = x' - a
//
// Plug into above equation:
//
// vn = A * x + b
// = A * (x' - a) + b
// = A * x' + b - A * a
// = A * x' + b'
// b' = b - A * a;
ContactConstraintPoint cp1 = c.Points[0];
ContactConstraintPoint cp2 = c.Points[1];
Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
#if MATH_OVERLOADS
// Relative velocity at contact
Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
float vn1 = Vector2.Dot(dv1, normal);
float vn2 = Vector2.Dot(dv2, normal);
Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias);
b -= MathUtils.Multiply(ref c.K, a);
#else
// Relative velocity at contact
Vector2 dv1 = new Vector2(vB.X + (-wB * cp1.rB.Y) - vA.X - (-wA * cp1.rA.Y),
vB.Y + (wB * cp1.rB.X) - vA.Y - (wA * cp1.rA.X));
Vector2 dv2 = new Vector2(vB.X + (-wB * cp2.rB.Y) - vA.X - (-wA * cp2.rA.Y),
vB.Y + (wB * cp2.rB.X) - vA.Y - (wA * cp2.rA.X));
// Compute normal velocity
float vn1 = dv1.X * normal.X + dv1.Y * normal.Y;
float vn2 = dv2.X * normal.X + dv2.Y * normal.Y;
Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias);
b -= MathUtils.Multiply(ref c.K, ref a); // Inlining didn't help for the multiply.
#endif
while (true)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
Vector2 x = -MathUtils.Multiply(ref c.NormalMass, ref b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
#if MATH_OVERLOADS
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
// Resubstitute for the incremental impulse
Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);
// Apply incremental impulse
Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);
vA.X -= invMassA * P12.X;
vA.Y -= invMassA * P12.Y;
wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));
vB.X += invMassB * P12.X;
vB.Y += invMassB * P12.Y;
wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
float k_errorTol = 1e-3f;
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + b2'
//
x.X = -cp1.NormalMass * b.X;
x.Y = 0.0f;
vn1 = 0.0f;
vn2 = c.K.Col1.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
#if MATH_OVERLOADS
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
// Resubstitute for the incremental impulse
Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);
// Apply incremental impulse
Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);
vA.X -= invMassA * P12.X;
vA.Y -= invMassA * P12.Y;
wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));
vB.X += invMassB * P12.X;
vB.Y += invMassB * P12.Y;
wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
// Compute normal velocity
vn1 = Vector2.Dot(dv1, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + b2'
//
x.X = 0.0f;
x.Y = -cp2.NormalMass * b.Y;
vn1 = c.K.Col2.X * x.Y + b.X;
vn2 = 0.0f;
if (x.Y >= 0.0f && vn1 >= 0.0f)
{
#if MATH_OVERLOADS
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
// Resubstitute for the incremental impulse
Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);
// Apply incremental impulse
Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);
vA.X -= invMassA * P12.X;
vA.Y -= invMassA * P12.Y;
wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));
vB.X += invMassB * P12.X;
vB.Y += invMassB * P12.Y;
wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn2 = Vector2.Dot(dv2, normal);
Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.X = 0.0f;
x.Y = 0.0f;
vn1 = b.X;
vn2 = b.Y;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
#if MATH_OVERLOADS
// Resubstitute for the incremental impulse
Vector2 d = x - a;
// Apply incremental impulse
Vector2 P1 = d.X * normal;
Vector2 P2 = d.Y * normal;
vA -= invMassA * (P1 + P2);
wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += invMassB * (P1 + P2);
wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
// Resubstitute for the incremental impulse
Vector2 d = new Vector2(x.X - a.X, x.Y - a.Y);
// Apply incremental impulse
Vector2 P1 = new Vector2(d.X * normal.X, d.X * normal.Y);
Vector2 P2 = new Vector2(d.Y * normal.X, d.Y * normal.Y);
Vector2 P12 = new Vector2(P1.X + P2.X, P1.Y + P2.Y);
vA.X -= invMassA * P12.X;
vA.Y -= invMassA * P12.Y;
wA -= invIA * ((cp1.rA.X * P1.Y - cp1.rA.Y * P1.X) + (cp2.rA.X * P2.Y - cp2.rA.Y * P2.X));
vB.X += invMassB * P12.X;
vB.Y += invMassB * P12.Y;
wB += invIB * ((cp1.rB.X * P1.Y - cp1.rB.Y * P1.X) + (cp2.rB.X * P2.Y - cp2.rB.Y * P2.X));
#endif
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
bodyA.LinearVelocityInternal = vA;
bodyA.AngularVelocityInternal = wA;
bodyB.LinearVelocityInternal = vB;
bodyB.AngularVelocityInternal = wB;
}
}
public static Vec2 Abs(Vec2 v)
{
return(new Vec2(MathUtils.Abs(v.x), MathUtils.Abs(v.y)));
}
public override float GetScore(float sx, float sy, float tx, float ty)
{
return(MathUtils.Abs(sx - tx) + MathUtils.Abs(sy - ty));
}
public static Vec4 Abs(Vec4 v)
{
return(new Vec4(MathUtils.Abs(v.x), MathUtils.Abs(v.y), MathUtils.Abs(v.z), MathUtils.Abs(v.w)));
}
private Matrix4x4 GetRandomTransformation(ref ItemReferenceData itemReference, ref ItemData itemData, System.Random rnd)
{
Matrix4x4 matrix = itemReference.transform;
if (itemReference.randomOrientation)
{
matrix *= GetRandomOrientationMatrix(rnd, itemReference.anchorPlane);
}
Vector3 randPositionAmplitude = itemReference.randomPositionAmplitude;
if (itemReference.procedural)
{
Vector3 refAnchorPosition = GetAnchorPosition(itemReference.anchorPlane, itemReference.availableProceduralVolume);
Vector3 itemAnchorPosition = GetAnchorPosition(itemData.anchorPlane, itemData.itemBounds);
matrix *= Matrix4x4.TRS(refAnchorPosition - itemAnchorPosition, Quaternion.identity, Vector3.one);
// Add random amplitude based on the possible volume
Vector3 anchorDirection = GetAnchorDirection(itemReference.anchorPlane);
randPositionAmplitude += Vector3.Scale(itemReference.availableProceduralVolume.size, Vector3.one - MathUtils.Abs(anchorDirection));
Vector3 position = GetRandomPositionOffset(randPositionAmplitude, rnd);
Quaternion rotation = Quaternion.Euler(GetRandomRotationOffset(itemReference.randomRotationAmplitude, rnd));
Vector3 scale = GetRandomScaleOffset(itemReference.randomScaleAmplitude, itemReference.uniformScale, rnd);
return(matrix * Matrix4x4.TRS(position, rotation, scale));
}
else
{
Vector3 position = GetRandomPositionOffset(randPositionAmplitude, rnd);
Quaternion rotation = Quaternion.Euler(GetRandomRotationOffset(itemReference.randomRotationAmplitude, rnd));
Vector3 scale = GetRandomScaleOffset(itemReference.randomScaleAmplitude, itemReference.uniformScale, rnd);
return(matrix * Matrix4x4.TRS(position, rotation, scale));
}
}
public void AbsLocal()
{
x = MathUtils.Abs(x);
y = MathUtils.Abs(y);
}
public bool Intersects(Bounds boundingBox, out float result)
{
// X
if (MathUtils.Abs(this.direction.x) < MathUtils.EPSILON &&
(this.origin.x < boundingBox.min.x || this.origin.x > boundingBox.max.x))
{
//If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it can't be intersecting.
result = 0;
return(false);
}
float tmin = 0, tmax = float.MaxValue;
float inverseDirection = 1 / this.direction.x;
float t1 = (boundingBox.min.x - this.origin.x) * inverseDirection;
float t2 = (boundingBox.max.x - this.origin.x) * inverseDirection;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
}
tmin = MathUtils.Max(tmin, t1);
tmax = MathUtils.Min(tmax, t2);
if (tmin > tmax)
{
result = 0;
return(false);
}
// Y
if (MathUtils.Abs(this.direction.y) < MathUtils.EPSILON &&
(this.origin.y < boundingBox.min.y || this.origin.y > boundingBox.max.y))
{
//If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it can't be intersecting.
result = 0;
return(false);
}
inverseDirection = 1 / this.direction.y;
t1 = (boundingBox.min.y - this.origin.y) * inverseDirection;
t2 = (boundingBox.max.y - this.origin.y) * inverseDirection;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
}
tmin = MathUtils.Max(tmin, t1);
tmax = MathUtils.Min(tmax, t2);
if (tmin > tmax)
{
result = 0;
return(false);
}
// Z
if (MathUtils.Abs(this.direction.z) < MathUtils.EPSILON &&
(this.origin.z < boundingBox.min.z || this.origin.z > boundingBox.max.z))
{
//If the ray isn't pointing along the axis at all, and is outside of the box's interval, then it can't be intersecting.
result = 0;
return(false);
}
inverseDirection = 1 / this.direction.z;
t1 = (boundingBox.min.z - this.origin.z) * inverseDirection;
t2 = (boundingBox.max.z - this.origin.z) * inverseDirection;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
}
tmin = MathUtils.Max(tmin, t1);
tmax = MathUtils.Min(tmax, t2);
if (tmin > tmax)
{
result = 0;
return(false);
}
result = tmin;
return(true);
}
public static void AbsToOut(Vector2f a, Vector2f xout)
{
xout.x = MathUtils.Abs(a.x);
xout.y = MathUtils.Abs(a.y);
}
public override void InitVelocityConstraints(SolverData data)
{
IndexA = BodyA.IslandIndex;
IndexB = BodyB.IslandIndex;
LocalCenterA.Set(BodyA.Sweep.LocalCenter);
LocalCenterB.Set(BodyB.Sweep.LocalCenter);
InvMassA = BodyA.InvMass;
InvMassB = BodyB.InvMass;
InvIA = BodyA.InvI;
InvIB = BodyB.InvI;
Vec2 cA = data.Positions[IndexA].C;
float aA = data.Positions[IndexA].A;
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 cB = data.Positions[IndexB].C;
float aB = data.Positions[IndexB].A;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 d = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
Vec2 rA = Pool.PopVec2();
Vec2 rB = Pool.PopVec2();
qA.Set(aA);
qB.Set(aB);
// Compute the effective masses.
Rot.MulToOutUnsafe(qA, d.Set(LocalAnchorA).SubLocal(LocalCenterA), rA);
Rot.MulToOutUnsafe(qB, d.Set(LocalAnchorB).SubLocal(LocalCenterB), rB);
d.Set(cB).SubLocal(cA).AddLocal(rB).SubLocal(rA);
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
// Compute motor Jacobian and effective mass.
{
Rot.MulToOutUnsafe(qA, LocalXAxisA, Axis);
temp.Set(d).AddLocal(rA);
A1 = Vec2.Cross(temp, Axis);
A2 = Vec2.Cross(rB, Axis);
MotorMass = mA + mB + iA * A1 * A1 + iB * A2 * A2;
if (MotorMass > 0.0f)
{
MotorMass = 1.0f / MotorMass;
}
}
// Prismatic constraint.
{
Rot.MulToOutUnsafe(qA, LocalYAxisA, Perp);
temp.Set(d).AddLocal(rA);
S1 = Vec2.Cross(temp, Perp);
S2 = Vec2.Cross(rB, Perp);
float k11 = mA + mB + iA * S1 * S1 + iB * S2 * S2;
float k12 = iA * S1 + iB * S2;
float k13 = iA * S1 * A1 + iB * S2 * A2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * A1 + iB * A2;
float k33 = mA + mB + iA * A1 * A1 + iB * A2 * A2;
K.Ex.Set(k11, k12, k13);
K.Ey.Set(k12, k22, k23);
K.Ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_limitEnabled)
{
float jointTranslation = Vec2.Dot(Axis, d);
if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP)
{
LimitState = LimitState.Equal;
}
else if (jointTranslation <= LowerTranslation)
{
if (LimitState != LimitState.AtLower)
{
LimitState = LimitState.AtLower;
Impulse.Z = 0.0f;
}
}
else if (jointTranslation >= UpperTranslation)
{
if (LimitState != LimitState.AtUpper)
{
LimitState = LimitState.AtUpper;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.Inactive;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.Inactive;
Impulse.Z = 0.0f;
}
if (m_motorEnabled == false)
{
MotorImpulse = 0.0f;
}
if (data.Step.WarmStarting)
{
// Account for variable time step.
Impulse.MulLocal(data.Step.DtRatio);
MotorImpulse *= data.Step.DtRatio;
Vec2 P = Pool.PopVec2();
temp.Set(Axis).MulLocal(MotorImpulse + Impulse.Z);
P.Set(Perp).MulLocal(Impulse.X).AddLocal(temp);
float LA = Impulse.X * S1 + Impulse.Y + (MotorImpulse + Impulse.Z) * A1;
float LB = Impulse.X * S2 + Impulse.Y + (MotorImpulse + Impulse.Z) * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(1);
}
else
{
Impulse.SetZero();
MotorImpulse = 0.0f;
}
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushRot(2);
Pool.PushVec2(4);
}
/// <summary>
/// Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction
/// between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function again.
/// Note: use Distance to compute the contact point and normal at the time of impact.
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
public void GetTimeOfImpact(TOIOutput output, TOIInput input)
{
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
++ToiCalls;
output.State = TOIOutputState.Unknown;
output.T = input.tMax;
Distance.DistanceProxy proxyA = input.ProxyA;
Distance.DistanceProxy proxyB = input.ProxyB;
sweepA.Set(input.SweepA);
sweepB.Set(input.SweepB);
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float tMax = input.tMax;
float totalRadius = proxyA.Radius + proxyB.Radius;
// djm: whats with all these constants?
float target = MathUtils.Max(Settings.LINEAR_SLOP, totalRadius - 3.0f * Settings.LINEAR_SLOP);
const float tolerance = 0.25f * Settings.LINEAR_SLOP;
Debug.Assert(target > tolerance);
float t1 = 0f;
int iter = 0;
cache.Count = 0;
distanceInput.ProxyA = input.ProxyA;
distanceInput.ProxyB = input.ProxyB;
distanceInput.UseRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for (; ;)
{
sweepA.GetTransform(xfA, t1);
sweepB.GetTransform(xfB, t1);
// System.out.printf("sweepA: %f, %f, sweepB: %f, %f\n",
// sweepA.c.x, sweepA.c.y, sweepB.c.x, sweepB.c.y);
// Get the distance between shapes. We can also use the results
// to get a separating axis
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
pool.GetDistance().GetDistance(distanceOutput, cache, distanceInput);
// System.out.printf("Dist: %f at points %f, %f and %f, %f. %d iterations\n",
// distanceOutput.distance, distanceOutput.pointA.x, distanceOutput.pointA.y,
// distanceOutput.pointB.x, distanceOutput.pointB.y,
// distanceOutput.iterations);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.Distance <= 0f)
{
// System.out.println("failure, overlapped");
// Failure!
output.State = TOIOutputState.Overlapped;
output.T = 0f;
break;
}
if (distanceOutput.Distance < target + tolerance)
{
// System.out.println("touching, victory");
// Victory!
output.State = TOIOutputState.Touching;
output.T = t1;
break;
}
// Initialize the separating axis.
fcn.Initialize(cache, proxyA, sweepA, proxyB, sweepB, t1);
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of
// vertices.
bool done = false;
float t2 = tMax;
int pushBackIter = 0;
for (; ;)
{
// Find the deepest point at t2. Store the witness point indices.
float s2 = fcn.FindMinSeparation(indexes, t2);
// System.out.printf("s2: %f\n", s2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
// System.out.println("separated");
output.State = TOIOutputState.Separated;
output.T = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// System.out.println("advancing");
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float s1 = fcn.Evaluate(indexes[0], indexes[1], t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
// System.out.printf("s1: %f, target: %f, tolerance: %f\n", s1, target,
// tolerance);
if (s1 < target - tolerance)
{
// System.out.println("failed?");
output.State = TOIOutputState.Failed;
output.T = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// System.out.println("touching?");
// Victory! t1 should hold the TOI (could be 0.0).
output.State = TOIOutputState.Touching;
output.T = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int rootIterCount = 0;
float a1 = t1, a2 = t2;
for (; ;)
{
// Use a mix of the secant rule and bisection.
float t;
if ((rootIterCount & 1) == 1)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
float s = fcn.Evaluate(indexes[0], indexes[1], t);
if (MathUtils.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
++rootIterCount;
++ToiRootIters;
// djm: whats with this? put in settings?
if (rootIterCount == 50)
{
break;
}
}
ToiMaxRootIters = MathUtils.Max(ToiMaxRootIters, rootIterCount);
++pushBackIter;
if (pushBackIter == Settings.MAX_POLYGON_VERTICES)
{
break;
}
}
++iter;
++ToiIters;
if (done)
{
// System.out.println("done");
break;
}
if (iter == MAX_ITERATIONS)
{
// System.out.println("failed, root finder stuck");
// Root finder got stuck. Semi-victory.
output.State = TOIOutputState.Failed;
output.T = t1;
break;
}
}
// System.out.printf("final sweeps: %f, %f, %f; %f, %f, %f", input.s)
ToiMaxIters = MathUtils.Max(ToiMaxIters, iter);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
Vec2 temp = Pool.PopVec2();
// Solve linear motor constraint.
if (m_motorEnabled && LimitState != LimitState.Equal)
{
temp.Set(vB).SubLocal(vA);
float Cdot = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA;
float impulse = MotorMass * (m_motorSpeed - Cdot);
float oldImpulse = MotorImpulse;
float maxImpulse = data.Step.Dt * m_maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
Vec2 P = Pool.PopVec2();
P.Set(Axis).MulLocal(impulse);
float LA = impulse * A1;
float LB = impulse * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(1);
}
Vec2 Cdot1 = Pool.PopVec2();
temp.Set(vB).SubLocal(vA);
Cdot1.X = Vec2.Dot(Perp, temp) + S2 * wB - S1 * wA;
Cdot1.Y = wB - wA;
// System.out.println(Cdot1);
if (m_limitEnabled && LimitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
temp.Set(vB).SubLocal(vA);
Cdot2 = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA;
Vec3 Cdot = Pool.PopVec3();
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Cdot.NegateLocal();
Vec3 f1 = Pool.PopVec3();
Vec3 df = Pool.PopVec3();
f1.Set(Impulse);
K.Solve33ToOut(Cdot.NegateLocal(), df);
//Cdot.negateLocal(); not used anymore
Impulse.AddLocal(df);
if (LimitState == LimitState.AtLower)
{
Impulse.Z = MathUtils.Max(Impulse.Z, 0.0f);
}
else if (LimitState == LimitState.AtUpper)
{
Impulse.Z = MathUtils.Min(Impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
// f1(1:2)
Vec2 b = Pool.PopVec2();
Vec2 f2r = Pool.PopVec2();
temp.Set(K.Ez.X, K.Ez.Y).MulLocal(Impulse.Z - f1.Z);
b.Set(Cdot1).NegateLocal().SubLocal(temp);
temp.Set(f1.X, f1.Y);
K.Solve22ToOut(b, f2r);
f2r.AddLocal(temp);
Impulse.X = f2r.X;
Impulse.Y = f2r.Y;
df.Set(Impulse).SubLocal(f1);
Vec2 P = Pool.PopVec2();
temp.Set(Axis).MulLocal(df.Z);
P.Set(Perp).MulLocal(df.X).AddLocal(temp);
float LA = df.X * S1 + df.Y + df.Z * A1;
float LB = df.X * S2 + df.Y + df.Z * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(3);
Pool.PushVec3(3);
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vec2 df = Pool.PopVec2();
K.Solve22ToOut(Cdot1.NegateLocal(), df);
Cdot1.NegateLocal();
Impulse.X += df.X;
Impulse.Y += df.Y;
Vec2 P = Pool.PopVec2();
P.Set(Perp).MulLocal(df.X);
float LA = df.X * S1 + df.Y;
float LB = df.X * S2 + df.Y;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Vec2 Cdot10 = Pool.PopVec2();
Cdot10.Set(Cdot1);
Cdot1.X = Vec2.Dot(Perp, temp.Set(vB).SubLocal(vA)) + S2 * wB - S1 * wA;
Cdot1.Y = wB - wA;
if (MathUtils.Abs(Cdot1.X) > 0.01f || MathUtils.Abs(Cdot1.Y) > 0.01f)
{
// djm note: what's happening here?
Mat33.Mul22ToOutUnsafe(K, df, temp);
Cdot1.X += 0.0f;
}
Pool.PushVec2(3);
}
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(2);
}
public override Vec3 Steer()
{
Entity self = this._behaviors.owner;
float detectRadius = MIN_DETECTION_BOX_LENGTH * (1 + self.property.speed / self.maxSpeed);
EntityUtils.GetEntitiesInCircle(self.battle.GetEntities(), self.property.position, detectRadius, ref this._temp);
this.DebugDrawDetectRadius(detectRadius);
Bio closestIntersectingObstacle = null;
float distToClosestIp = float.MaxValue;
Vec3 localPosOfClosestObstacle = Vec3.zero;
int count = this._temp.Count;
for (int i = 0; i < count; i++)
{
Bio bio = this._temp[i] as Bio;
if (bio == null ||
bio == self ||
bio.isDead ||
!bio.volumetric ||
bio.property.dashing > 0 ||
bio.property.ignoreVolumetric > 0)
{
continue;
}
Vec3 localPos = self.PointToLocal(bio.property.position);
if (localPos.x >= 0)
{
float expandedRadius = bio.size.z + self.size.z;
if (MathUtils.Abs(localPos.z) < expandedRadius)
{
float cX = localPos.x;
float cZ = localPos.z;
float sqrtPart = MathUtils.Sqrt(expandedRadius * expandedRadius - cZ * cZ);
float ip = cX - sqrtPart;
if (ip <= 0.0f)
{
ip = cX + sqrtPart;
}
if (ip < distToClosestIp)
{
distToClosestIp = ip;
closestIntersectingObstacle = bio;
localPosOfClosestObstacle = localPos;
}
}
}
}
this._temp.Clear();
if (closestIntersectingObstacle != null)
{
Vec3 steeringForce = Vec3.zero;
const float minLocalXDistance = .3f; //限定最小的x轴距离,值越小,下面得到的x轴因子越大,侧向力就越大
float multiplier = 1.5f / MathUtils.Min(minLocalXDistance, localPosOfClosestObstacle.x); //侧向力和障碍物的x距离成反比,越近x轴的因子越大,侧向力越大
steeringForce.z = -closestIntersectingObstacle.size.z * multiplier / localPosOfClosestObstacle.z; //侧向力和障碍物的半径成正比,z轴距离成反比
const float brakingWeight = .2f; //制动力因子,值越大,速度减少越快
steeringForce.x = (closestIntersectingObstacle.size.x -
localPosOfClosestObstacle.x) *
brakingWeight;
steeringForce = self.VectorToWorld(steeringForce);
this.DebugDrawForce(steeringForce * 3);
return(steeringForce);
}
return(Vec3.zero);
}
public override bool SolvePositionConstraints(SolverData data)
{
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 rA = Pool.PopVec2();
Vec2 rB = Pool.PopVec2();
Vec2 d = Pool.PopVec2();
Vec2 axis = Pool.PopVec2();
Vec2 perp = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
Vec2 C1 = Pool.PopVec2();
Vec3 impulse = Pool.PopVec3();
Vec2 cA = data.Positions[IndexA].C;
float aA = data.Positions[IndexA].A;
Vec2 cB = data.Positions[IndexB].C;
float aB = data.Positions[IndexB].A;
qA.Set(aA);
qB.Set(aB);
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
// Compute fresh Jacobians
Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(LocalCenterA), rA);
Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(LocalCenterB), rB);
d.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA);
Rot.MulToOutUnsafe(qA, LocalXAxisA, axis);
float a1 = Vec2.Cross(temp.Set(d).AddLocal(rA), axis);
float a2 = Vec2.Cross(rB, axis);
Rot.MulToOutUnsafe(qA, LocalYAxisA, perp);
float s1 = Vec2.Cross(temp.Set(d).AddLocal(rA), perp);
float s2 = Vec2.Cross(rB, perp);
C1.X = Vec2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = MathUtils.Abs(C1.X);
float angularError = MathUtils.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (m_limitEnabled)
{
float translation = Vec2.Dot(axis, d);
if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MAX_LINEAR_CORRECTION, Settings.MAX_LINEAR_CORRECTION);
linearError = MathUtils.Max(linearError, MathUtils.Abs(translation));
active = true;
}
else if (translation <= LowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - LowerTranslation + Settings.LINEAR_SLOP, -Settings.MAX_LINEAR_CORRECTION, 0.0f);
linearError = MathUtils.Max(linearError, LowerTranslation - translation);
active = true;
}
else if (translation >= UpperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - UpperTranslation - Settings.LINEAR_SLOP, 0.0f, Settings.MAX_LINEAR_CORRECTION);
linearError = MathUtils.Max(linearError, translation - UpperTranslation);
active = true;
}
}
if (active)
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k13 = iA * s1 * a1 + iB * s2 * a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = Pool.PopMat33();
K.Ex.Set(k11, k12, k13);
K.Ey.Set(k12, k22, k23);
K.Ez.Set(k13, k23, k33);
Vec3 C = Pool.PopVec3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
K.Solve33ToOut(C.NegateLocal(), impulse);
Pool.PushVec3(1);
Pool.PushMat33(1);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = Pool.PopMat22();
K.Ex.Set(k11, k12);
K.Ey.Set(k12, k22);
// temp is impulse1
K.SolveToOut(C1.NegateLocal(), temp);
C1.NegateLocal();
impulse.X = temp.X;
impulse.Y = temp.Y;
impulse.Z = 0.0f;
Pool.PushMat22(1);
}
float Px = impulse.X * perp.X + impulse.Z * axis.X;
float Py = impulse.X * perp.Y + impulse.Z * axis.Y;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA.X -= mA * Px;
cA.Y -= mA * Py;
aA -= iA * LA;
cB.X += mB * Px;
cB.Y += mB * Py;
aB += iB * LB;
data.Positions[IndexA].C.Set(cA);
data.Positions[IndexA].A = aA;
data.Positions[IndexB].C.Set(cB);
data.Positions[IndexB].A = aB;
Pool.PushVec2(7);
Pool.PushVec3(1);
Pool.PushRot(2);
return(linearError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP);
}
/// <summary>
/// Raycast against this AABB using the specified points and maxfraction (found in input)
/// </summary>
/// <param name="output">The results of the raycast.</param>
/// <param name="input">The parameters for the raycast.</param>
/// <returns>True if the ray intersects the AABB</returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
{
// From Real-time Collision Detection, p179.
output = new RayCastOutput();
float tmin = -Settings.MaxFloat;
float tmax = Settings.MaxFloat;
Vector2 p = input.Point1;
Vector2 d = input.Point2 - input.Point1;
Vector2 absD = MathUtils.Abs(d);
Vector2 normal = Vector2.Zero;
for (int i = 0; i < 2; ++i)
{
float absD_i = i == 0 ? absD.X : absD.Y;
float lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y;
float upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y;
float p_i = i == 0 ? p.X : p.Y;
if (absD_i < Settings.Epsilon)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return(false);
}
}
else
{
float d_i = i == 0 ? d.X : d.Y;
float inv_d = 1.0f / d_i;
float t1 = (lowerBound_i - p_i) * inv_d;
float t2 = (upperBound_i - p_i) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
if (i == 0)
{
normal.X = s;
}
else
{
normal.Y = s;
}
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin))
{
return(false);
}
// Intersection.
output.Fraction = tmin;
output.Normal = normal;
return(true);
}
public bool AproxEqualsBox(Vec2 vector, float tolerance)
{
return
((MathUtils.Abs(this.x - vector.x) <= tolerance) &&
(MathUtils.Abs(this.y - vector.y) <= tolerance));
}
public static Vec3 Abs(Vec3 v)
{
return(new Vec3(MathUtils.Abs(v.x), MathUtils.Abs(v.y), MathUtils.Abs(v.z)));
}
public void Update()
{
if (gameOver || !hasStarted)
{
return;
}
currentTick++;
// increasing the difficulty
if (currentTick % 100 == 0 && speed < 0.2f)
{
speed += 0.01f;
}
if (up)
{
if (right)
{
ball.x = ball.x + MathUtils.Cos(bounceAngle) * speed;
ball.y = ball.y + MathUtils.Sin(bounceAngle) * speed;
}
else
{
ball.x = ball.x - MathUtils.Sin(bounceAngle) * speed;
ball.y = ball.y + MathUtils.Cos(bounceAngle) * speed;
}
}
else
{
if (right)
{
ball.x = ball.x + MathUtils.Cos(bounceAngle) * speed;
ball.y = ball.y - MathUtils.Sin(bounceAngle) * speed;
}
else
{
ball.x = ball.x - MathUtils.Sin(bounceAngle) * speed;
ball.y = ball.y - MathUtils.Cos(bounceAngle) * speed;
}
}
// verifies if it hits the walls
if (ball.x > higherBoundGridX)
{
right = false;
}
else if (ball.x < lowerBoundGridX)
{
right = true;
}
// verifies if it hit the paddles
if (MathUtils.Abs(ball.y, paddle2.y) <= 0.2f && HasCollided(paddle2.x, ball.x))
{
up = false;
bounceAngle = GetAngle();
}
else if (MathUtils.Abs(ball.y, paddle1.y) <= 0.2f && HasCollided(paddle1.x, ball.x))
{
up = true;
bounceAngle = GetAngle();
}
// pause to show the goal sign
if (isGoalPause && currentTick > goalUnpauseTick)
{
isGoalPause = false;
speed = START_SPEED;
// reset the game depending on who scored
if (ball.y < lowerBoundGridY)
{
// player 1 starts
ball.x = paddle1.x;
ball.y = lowerBoundGridY;
up = true;
right = true;
}
else if (ball.y > higherBoundGridY)
{
ball.x = paddle2.x;
ball.y = higherBoundGridY;
up = false;
right = false;
}
}
else if (ball.y < (lowerBoundGridY - 1) && !isGoalPause)
{
isGoalPause = true;
// 2 seconds to show the Goal sign
goalUnpauseTick = currentTick + FPS * 2;
player2Score++;
}
else if (ball.y > (higherBoundGridY + 1) && !isGoalPause)
{
isGoalPause = true;
// 2 seconds to show the Goal sign
goalUnpauseTick = currentTick + FPS * 2;
player1Score++;
}
}
