public static void Cross(FP s, ref FPVector2 a, out FPVector2 b)
{
b = new FPVector2(-s * a.y, s * a.x);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 d = cB + rB - cA - rA;
FPVector2 axis = MathUtils.Mul(qA, LocalXAxis);
FP a1 = MathUtils.Cross(d + rA, axis);
FP a2 = MathUtils.Cross(rB, axis);
FPVector2 perp = MathUtils.Mul(qA, _localYAxisA);
FP s1 = MathUtils.Cross(d + rA, perp);
FP s2 = MathUtils.Cross(rB, perp);
FPVector impulse;
FPVector2 C1 = new FPVector2();
C1.x = FPVector2.Dot(perp, d);
C1.y = aB - aA - ReferenceAngle;
FP linearError = FP.Abs(C1.x);
FP angularError = FP.Abs(C1.y);
bool active = false;
FP C2 = 0.0f;
if (_enableLimit)
{
FP translation = FPVector2.Dot(axis, d);
if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = KBEngine.FPMath.Max(linearError, FP.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = KBEngine.FPMath.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = KBEngine.FPMath.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
FP k12 = iA * s1 + iB * s2;
FP k13 = iA * s1 * a1 + iB * s2 * a2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
FP k23 = iA * a1 + iB * a2;
FP k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new FPVector(k11, k12, k13);
K.ey = new FPVector(k12, k22, k23);
K.ez = new FPVector(k13, k23, k33);
FPVector C = new FPVector();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = K.Solve33(C * -1);
}
else
{
FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
FP k12 = iA * s1 + iB * s2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new FPVector2(k11, k12);
K.ey = new FPVector2(k12, k22);
FPVector2 impulse1 = K.Solve(-C1);
impulse = new FPVector();
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
FPVector2 P = impulse.x * perp + impulse.z * axis;
FP LA = impulse.x * s1 + impulse.y + impulse.z * a1;
FP LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invIA = BodyA._invI;
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
Rot qA = new Rot(aA);
FP mass = BodyA.Mass;
// Frequency
FP omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
FP d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
FP k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
FP h = data.step.dt;
//Debug.Assert(d + h * k > Settings.Epsilon);
_gamma = h * (d + h * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
// 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]
Mat22 K = new Mat22();
K.ex.x = _invMassA + _invIA * _rA.y * _rA.y + _gamma;
K.ex.y = -_invIA * _rA.x * _rA.y;
K.ey.x = K.ex.y;
K.ey.y = _invMassA + _invIA * _rA.x * _rA.x + _gamma;
_mass = K.Inverse;
_C = cA + _rA - _worldAnchor;
_C *= _beta;
// Cheat with some damping
wA *= 0.98f;
if (Settings.EnableWarmstarting)
{
_impulse *= data.step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.Cross(_rA, _impulse);
}
else
{
_impulse = FPVector2.zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 cC = data.positions[_indexC].c;
FP aC = data.positions[_indexC].a;
FPVector2 cD = data.positions[_indexD].c;
FP aD = data.positions[_indexD].a;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
FP linearError = 0.0f;
FP coordinateA, coordinateB;
FPVector2 JvAC, JvBD;
FP JwA, JwB, JwC, JwD;
FP mass = 0.0f;
if (_typeA == JointType.Revolute)
{
JvAC = FPVector2.zero;
JwA = 1.0f;
JwC = 1.0f;
mass += _iA + _iC;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
FPVector2 u = MathUtils.Mul(qC, _localAxisC);
FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
JvAC = u;
JwC = MathUtils.Cross(rC, u);
JwA = MathUtils.Cross(rA, u);
mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;
FPVector2 pC = _localAnchorC - _lcC;
FPVector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
}
if (_typeB == JointType.Revolute)
{
JvBD = FPVector2.zero;
JwB = _ratio;
JwD = _ratio;
mass += _ratio * _ratio * (_iB + _iD);
coordinateB = aB - aD - _referenceAngleB;
}
else
{
FPVector2 u = MathUtils.Mul(qD, _localAxisD);
FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
JvBD = _ratio * u;
JwD = _ratio * MathUtils.Cross(rD, u);
JwB = _ratio * MathUtils.Cross(rB, u);
mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;
FPVector2 pD = _localAnchorD - _lcD;
FPVector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
}
FP C = (coordinateA + _ratio * coordinateB) - _constant;
FP impulse = 0.0f;
if (mass > 0.0f)
{
impulse = -C / mass;
}
cA += _mA * impulse * JvAC;
aA += _iA * impulse * JwA;
cB += _mB * impulse * JvBD;
aB += _iB * impulse * JwB;
cC -= _mC * impulse * JvAC;
aC -= _iC * impulse * JwC;
cD -= _mD * impulse * JvBD;
aD -= _iD * impulse * JwD;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
data.positions[_indexC].c = cC;
data.positions[_indexC].a = aC;
data.positions[_indexD].c = cD;
data.positions[_indexD].a = aD;
// TODO_ERIN not implemented
return(linearError < Settings.LinearSlop);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 d = (cB - cA) + rB - rA;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Mul(qA, LocalXAxis);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
FP k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
FP k12 = iA * _s1 + iB * _s2;
FP k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
FP k23 = iA * _a1 + iB * _a2;
FP k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_K.ex = new FPVector(k11, k12, k13);
_K.ey = new FPVector(k12, k22, k23);
_K.ez = new FPVector(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
FP jointTranslation = FPVector2.Dot(_axis, d);
if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_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 (_enableMotor == false)
{
MotorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
MotorImpulse *= data.step.dtRatio;
FPVector2 P = _impulse.x * _perp + (MotorImpulse + _impulse.z) * _axis;
FP LA = _impulse.x * _s1 + _impulse.y + (MotorImpulse + _impulse.z) * _a1;
FP LB = _impulse.x * _s2 + _impulse.y + (MotorImpulse + _impulse.z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse = FPVector.zero;
MotorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public static Vector3 ToVector(this FPVector2 jVector)
{
return(new Vector3((float)jVector.x, (float)jVector.y, 0));
}
public override FPVector2 GetReactionForce(FP invDt)
{
FPVector2 P = _impulse * _JvAC;
return(invDt * P);
}
public static FPVector2 Cross(FP s, FPVector2 a)
{
return(new FPVector2(-s * a.y, s * a.x));
}
public static FPVector2 Abs(FPVector2 v)
{
return(new FPVector2(FP.Abs(v.x), FP.Abs(v.y)));
}
public static FPVector2 Cross(FPVector2 a, FP s)
{
return(new FPVector2(s * a.y, -s * a.x));
}
/// <summary>
/// Initialize this matrix using columns.
/// </summary>
/// <param name="c1">The c1.</param>
/// <param name="c2">The c2.</param>
public void Set(FPVector2 c1, FPVector2 c2)
{
ex = c1;
ey = c2;
}
/// <summary>
/// Construct this matrix using scalars.
/// </summary>
/// <param name="a11">The a11.</param>
/// <param name="a12">The a12.</param>
/// <param name="a21">The a21.</param>
/// <param name="a22">The a22.</param>
public Mat22(FP a11, FP a12, FP a21, FP a22)
{
ex = new FPVector2(a11, a21);
ey = new FPVector2(a12, a22);
}
/// <summary>
/// Construct this matrix using columns.
/// </summary>
/// <param name="c1">The c1.</param>
/// <param name="c2">The c2.</param>
public Mat22(FPVector2 c1, FPVector2 c2)
{
ex = c1;
ey = c2;
}
public static FPVector2 MulT(ref Rot rot, FPVector2 axis)
{
return(MulT(rot, axis));
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat33 K = new Mat33();
K.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
K.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
K.ez.x = -_rA.y * iA - _rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
K.ez.y = _rA.x * iA + _rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
K.GetInverse22(ref _mass);
FP invM = iA + iB;
FP m = invM > 0.0f ? 1.0f / invM : 0.0f;
FP C = aB - aA - ReferenceAngle;
// Frequency
FP omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
FP d = 2.0f * m * DampingRatio * omega;
// Spring stiffness
FP k = m * omega * omega;
// magic formulas
FP h = data.step.dt;
_gamma = h * (d + h * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * h * k * _gamma;
invM += _gamma;
_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
}
else
{
K.GetSymInverse33(ref _mass);
_gamma = 0.0f;
_bias = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
FPVector2 P = new FPVector2(_impulse.x, _impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _impulse.z);
}
else
{
_impulse = FPVector.zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public static FPVector2 Mul(ref Mat22 A, ref FPVector2 v)
{
return(new FPVector2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y));
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FP positionError, angularError;
Mat33 K = new Mat33();
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
K.ez.x = -rA.y * iA - rB.y * iB;
K.ex.y = K.ey.x;
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
K.ez.y = rA.x * iA + rB.x * iB;
K.ex.z = K.ez.x;
K.ey.z = K.ez.y;
K.ez.z = iA + iB;
if (FrequencyHz > 0.0f)
{
FPVector2 C1 = cB + rB - cA - rA;
positionError = C1.magnitude;
angularError = 0.0f;
FPVector2 P = -K.Solve22(C1);
cA -= mA * P;
aA -= iA * MathUtils.Cross(rA, P);
cB += mB * P;
aB += iB * MathUtils.Cross(rB, P);
}
else
{
FPVector2 C1 = cB + rB - cA - rA;
FP C2 = aB - aA - ReferenceAngle;
positionError = C1.magnitude;
angularError = FP.Abs(C2);
FPVector C = new FPVector(C1.x, C1.y, C2);
FPVector impulse = K.Solve33(C) * -1;
FPVector2 P = new FPVector2(impulse.x, impulse.y);
cA -= mA * P;
aA -= iA * (MathUtils.Cross(rA, P) + impulse.z);
cB += mB * P;
aB += iB * (MathUtils.Cross(rB, P) + impulse.z);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="rotation">The r.</param>
public Transform(ref FPVector2 position, ref Rot rotation)
{
p = position;
q = rotation;
}
/// <summary>
/// Requires two existing revolute or prismatic joints (any combination will work).
/// The provided joints must attach a dynamic body to a static body.
/// </summary>
/// <param name="jointA">The first joint.</param>
/// <param name="jointB">The second joint.</param>
/// <param name="ratio">The ratio.</param>
/// <param name="bodyA">The first body</param>
/// <param name="bodyB">The second body</param>
// TS - public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio = 1f)
public GearJoint(Body bodyA, Body bodyB, Joint2D jointA, Joint2D jointB, FP ratio)
{
JointType = JointType.Gear;
BodyA = bodyA;
BodyB = bodyB;
JointA = jointA;
JointB = jointB;
Ratio = ratio;
_typeA = jointA.JointType;
_typeB = jointB.JointType;
Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);
FP coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = JointA.BodyA;
_bodyA = JointA.BodyB;
// Get geometry of joint1
Transform xfA = _bodyA._xf;
FP aA = _bodyA._sweep.A;
Transform xfC = _bodyC._xf;
FP aC = _bodyC._sweep.A;
if (_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointA;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = FPVector2.zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointA;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxis;
FPVector2 pC = _localAnchorC;
FPVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
}
_bodyD = JointB.BodyA;
_bodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = _bodyB._xf;
FP aB = _bodyB._sweep.A;
Transform xfD = _bodyD._xf;
FP aD = _bodyD._sweep.A;
if (_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointB;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = FPVector2.zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointB;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxis;
FPVector2 pD = _localAnchorD;
FPVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
/// <summary>
/// Set this to the identity transform.
/// </summary>
public void SetIdentity()
{
p = FPVector2.zero;
q.SetIdentity();
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = _bodyA.IslandIndex;
_indexB = _bodyB.IslandIndex;
_indexC = _bodyC.IslandIndex;
_indexD = _bodyD.IslandIndex;
_lcA = _bodyA._sweep.LocalCenter;
_lcB = _bodyB._sweep.LocalCenter;
_lcC = _bodyC._sweep.LocalCenter;
_lcD = _bodyD._sweep.LocalCenter;
_mA = _bodyA._invMass;
_mB = _bodyB._invMass;
_mC = _bodyC._invMass;
_mD = _bodyD._invMass;
_iA = _bodyA._invI;
_iB = _bodyB._invI;
_iC = _bodyC._invI;
_iD = _bodyD._invI;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FP aC = data.positions[_indexC].a;
FPVector2 vC = data.velocities[_indexC].v;
FP wC = data.velocities[_indexC].w;
FP aD = data.positions[_indexD].a;
FPVector2 vD = data.velocities[_indexD].v;
FP wD = data.velocities[_indexD].w;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
_mass = 0.0f;
if (_typeA == JointType.Revolute)
{
_JvAC = FPVector2.zero;
_JwA = 1.0f;
_JwC = 1.0f;
_mass += _iA + _iC;
}
else
{
FPVector2 u = MathUtils.Mul(qC, _localAxisC);
FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
_JvAC = u;
_JwC = MathUtils.Cross(rC, u);
_JwA = MathUtils.Cross(rA, u);
_mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA;
}
if (_typeB == JointType.Revolute)
{
_JvBD = FPVector2.zero;
_JwB = _ratio;
_JwD = _ratio;
_mass += _ratio * _ratio * (_iB + _iD);
}
else
{
FPVector2 u = MathUtils.Mul(qD, _localAxisD);
FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
_JvBD = _ratio * u;
_JwD = _ratio * MathUtils.Cross(rD, u);
_JwB = _ratio * MathUtils.Cross(rB, u);
_mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB;
}
// Compute effective mass.
_mass = _mass > 0.0f ? 1.0f / _mass : 0.0f;
if (Settings.EnableWarmstarting)
{
vA += (_mA * _impulse) * _JvAC;
wA += _iA * _impulse * _JwA;
vB += (_mB * _impulse) * _JvBD;
wB += _iB * _impulse * _JwB;
vC -= (_mC * _impulse) * _JvAC;
wC -= _iC * _impulse * _JwC;
vD -= (_mD * _impulse) * _JvBD;
wD -= _iD * _impulse * _JwD;
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
data.velocities[_indexC].v = vC;
data.velocities[_indexC].w = wC;
data.velocities[_indexD].v = vD;
data.velocities[_indexD].w = wD;
}
/// <summary>
/// Set this based on the position and angle.
/// </summary>
/// <param name="position">The position.</param>
/// <param name="angle">The angle.</param>
public void Set(FPVector2 position, FP angle)
{
p = position;
q.Set(angle);
}
public PrismaticJoint(Body bodyA, Body bodyB, FPVector2 anchor, FPVector2 axis, bool useWorldCoordinates = false)
: base(bodyA, bodyB)
{
Initialize(anchor, anchor, axis, useWorldCoordinates);
}
public static FPVector2 MulT(ref Mat22 A, FPVector2 v)
{
return(MulT(ref A, ref v));
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
FP Cdot = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
FP impulse = _motorMass * (_motorSpeed - Cdot);
FP oldImpulse = MotorImpulse;
FP maxImpulse = data.step.dt * _maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
FPVector2 P = impulse * _axis;
FP LA = impulse * _a1;
FP LB = impulse * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
FPVector2 Cdot1 = new FPVector2();
Cdot1.x = FPVector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA;
Cdot1.y = wB - wA;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
FP Cdot2;
Cdot2 = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
FPVector Cdot = new FPVector(Cdot1.x, Cdot1.y, Cdot2);
FPVector f1 = _impulse;
FPVector df = _K.Solve33(Cdot * -1);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.z = KBEngine.FPMath.Max(_impulse.z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.z = KBEngine.FPMath.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)
FPVector2 b = -Cdot1 - (_impulse.z - f1.z) * new FPVector2(_K.ez.x, _K.ez.y);
FPVector2 f2r = _K.Solve22(b) + new FPVector2(f1.x, f1.y);
_impulse.x = f2r.x;
_impulse.y = f2r.y;
df = _impulse - f1;
FPVector2 P = df.x * _perp + df.z * _axis;
FP LA = df.x * _s1 + df.y + df.z * _a1;
FP LB = df.x * _s2 + df.y + df.z * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
FPVector2 df = _K.Solve22(-Cdot1);
_impulse.x += df.x;
_impulse.y += df.y;
FPVector2 P = df.x * _perp;
FP LA = df.x * _s1 + df.y;
FP LB = df.x * _s2 + df.y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public static FPVector2 MulT(ref Mat22 A, ref FPVector2 v)
{
return(new FPVector2(v.x * A.ex.x + v.y * A.ex.y, v.x * A.ey.x + v.y * A.ey.y));
}
/**
* @brief Instantiates a new prefab in a deterministic way.
*
* @param prefab GameObject's prefab to instantiate.
* @param position Position to place the new GameObject.
* @param rotation Rotation to set in the new GameObject.
**/
public static GameObject SyncedInstantiate(GameObject prefab, FPVector2 position, FPQuaternion rotation)
{
return(SyncedInstantiate(prefab, new FPVector(position.x, position.y, 0), rotation));
}
public static FPVector2 MulT(ref Transform T, FPVector2 v)
{
return(MulT(ref T, ref v));
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.x = mA + mB + iA * _rA.y * _rA.y + iB * _rB.y * _rB.y;
K.ex.y = -iA * _rA.x * _rA.y - iB * _rB.x * _rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * _rA.x * _rA.x + iB * _rB.x * _rB.x;
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
_angularImpulse *= data.step.dtRatio;
FPVector2 P = new FPVector2(_linearImpulse.x, _linearImpulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = FPVector2.zero;
_angularImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
/// <summary>
/// Determines if three vertices are collinear (ie. on a straight line)
/// </summary>
/// <param name="a">First vertex</param>
/// <param name="b">Second vertex</param>
/// <param name="c">Third vertex</param>
/// <param name="tolerance">The tolerance</param>
/// <returns></returns>
// FP - public static bool IsCollinear(ref Vector2 a, ref Vector2 b, ref Vector2 c, FP tolerance = 0)
public static bool IsCollinear(ref FPVector2 a, ref FPVector2 b, ref FPVector2 c, FP tolerance)
{
return(FPInRange(Area(ref a, ref b, ref c), -tolerance, tolerance));
}