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); }