public static void NifStream(Matrix22 val, OStream s, NifInfo info)
{
for (var c = 0; c < 2; ++c)
{
for (var r = 0; r < 2; ++r)
{
WriteFloat(val[r][c], s);
}
}
}
//Matrix22
public static void NifStream(out Matrix22 val, IStream s, NifInfo info)
{
val = new Matrix22();
for (var c = 0; c < 2; ++c)
{
for (var r = 0; r < 2; ++r)
{
val[r][c] = ReadFloat(s);
}
}
}
public Matrix22 MultiplyTo(Matrix22 other)
{
var result = new Matrix22(0, 0, 0, 0);
result.matrix[0, 0] = this.matrix[0, 0] * other.matrix[0, 0]
+ this.matrix[0, 1] * other.matrix[1, 0];
result.matrix[0, 1] = this.matrix[0, 0] * other.matrix[0, 1]
+ this.matrix[0, 1] * other.matrix[1, 1];
result.matrix[1, 0] = this.matrix[1, 0] * other.matrix[0, 0]
+ this.matrix[1, 1] * other.matrix[1, 0];
result.matrix[1, 1] = this.matrix[1, 0] * other.matrix[0, 1]
+ this.matrix[1, 1] * other.matrix[1, 1];
return(result);
}
internal override bool SolvePositionConstraints(SolverData data)
{
var cA = data.Positions[_indexA];
float aA = data.Angles[_indexA];
var cB = data.Positions[_indexB];
float aB = data.Angles[_indexB];
Quaternion2D qA = new(aA), qB = new(aB);
float angularError = 0.0f;
float positionError = 0.0f;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint
if (EnableLimit && fixedRotation == false)
{
float angle = aB - aA - ReferenceAngle;
float C = 0.0f;
if (Math.Abs(UpperAngle - LowerAngle) < 2.0f * data.AngularSlop)
{
// Prevent large angular corrections
C = Math.Clamp(angle - LowerAngle, -data.MaxAngularCorrection, data.MaxAngularCorrection);
}
else if (angle <= LowerAngle)
{
// Prevent large angular corrections and allow some slop.
C = Math.Clamp(angle - LowerAngle + data.AngularSlop, -data.MaxAngularCorrection, 0.0f);
}
else if (angle >= UpperAngle)
{
// Prevent large angular corrections and allow some slop.
C = Math.Clamp(angle - UpperAngle - data.AngularSlop, 0.0f, data.MaxAngularCorrection);
}
float limitImpulse = -_axialMass * C;
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
angularError = Math.Abs(C);
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
var rA = Transform.Mul(qA, LocalAnchorA - _localCenterA);
var rB = Transform.Mul(qB, LocalAnchorB - _localCenterB);
var C = cB + rB - cA - rA;
positionError = C.Length;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
var K = new Matrix22(
mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y,
-iA * rA.X * rA.Y - iB * rB.X * rB.Y,
0f,
mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X);
K.EY.X = K.EX.Y;
var impulse = -K.Solve(C);
cA -= impulse * mA;
aA -= iA * Vector2.Cross(rA, impulse);
cB += impulse * mB;
aB += iB * Vector2.Cross(rB, impulse);
}
data.Positions[_indexA] = cA;
data.Angles[_indexA] = aA;
data.Positions[_indexB] = cB;
data.Angles[_indexB] = aB;
return(positionError <= data.LinearSlop && angularError <= data.AngularSlop);
}
