/// <summary>
/// Updates matrix from velocities used by old solver
/// </summary>=
public void UpdateMatrix(float dt)
{
m_LinearAcceleration = Vector3.Zero;
m_AngularAcceleration = Vector3.Zero;
m_ExternalAngularAcceleration = Vector3.Zero;
m_ExternalLinearAcceleration = Vector3.Zero;
Vector3 translation = m_Matrix.Translation + m_Velocity * dt;
m_Matrix.Translation = Vector3.Zero;
Vector3 dir = AngularVelocity;
float ang = dir.Length();
if (ang > 0.0f)
{
Vector3.Divide(ref dir, ang, out dir); // dir /= ang;
ang *= dt;
Matrix rot;
Matrix.CreateFromAxisAngle(ref dir, ang, out rot);
Matrix.Multiply(ref m_Matrix, ref rot, out m_Matrix);
}
MyPhysicsUtils.Orthonormalise(ref m_Matrix);
m_Matrix.Translation = translation;
m_LinearAcceleration = Vector3.Zero;
m_AngularAcceleration = Vector3.Zero;
}
/// <summary>
/// Solves the solver rbos and constraints through iterations given by the island. This is a newton based solver.
/// </summary>
private void Solve(float dt)
{
Vector3 v1 = new Vector3();
Vector3 v2 = new Vector3();
for (int iteration = 0; iteration < m_Island.IterationCount; iteration++)
{
for (int i = 0; i < m_SolverConstraints.Count; i++)
{
MyRBSolverConstraint sc = m_SolverConstraints[i];
MyRBSolverBody b1 = sc.m_SolverBody1;
MyRBSolverBody b2 = sc.m_SolverBody2;
// compute relative constraint velocity
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Static)
{
v1 = Vector3.Zero;
}
else
{
v1 = b1.m_LinearVelocity + Vector3.Cross(b1.m_AngularVelocity, sc.m_Body1LocalPoint);
}
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Static)
{
v2 = Vector3.Zero;
}
else
{
v2 = b2.m_LinearVelocity + Vector3.Cross(b2.m_AngularVelocity, sc.m_Body2LocalPoint);
}
float current = Vector3.Dot(v2, sc.m_Normal) - Vector3.Dot(v1, sc.m_Normal);
float diff = sc.m_Target - current;
if (sc.m_Magnitude < sc.m_MinMagnitude || sc.m_Magnitude > sc.m_MaxMagnitude ||
(diff < -m_CollisionEpsilon && sc.m_Magnitude > sc.m_MinMagnitude) ||
(diff > m_CollisionEpsilon && sc.m_Magnitude < sc.m_MaxMagnitude))
{
// Alter magnitude
float alter = (diff / sc.m_Affection) * 0.5f;
sc.m_Magnitude += alter;
//Ensure magnitude is in valid range
if (sc.m_Magnitude > sc.m_MaxMagnitude)
{
alter -= sc.m_Magnitude - sc.m_MaxMagnitude;
sc.m_Magnitude = sc.m_MaxMagnitude;
}
else if (sc.m_Magnitude < sc.m_MinMagnitude)
{
alter -= sc.m_Magnitude - sc.m_MinMagnitude;
sc.m_Magnitude = sc.m_MinMagnitude;
}
//Apply impulse
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
b1.m_LinearVelocity -= sc.m_Normal * (b1.m_OneOverMass * alter);
Vector3 tempV = b1.m_LinearVelocity;
tempV.Normalize();
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
float massRatio = 1.0f / (b1.m_OneOverMass / b2.m_OneOverMass);
//If mass of object1 is more than object2, do not apply restitution at all
float rest = MathHelper.Lerp(sc.m_Restitution, 1, massRatio - 0.5f);
sc.m_Restitution = MathHelper.Clamp(rest, sc.m_Restitution, 1);
}
b1.m_AngularVelocity -= sc.m_Body1LocalPointCrossedNormal * (alter);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(b1.m_AngularVelocity);
}
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
b2.m_LinearVelocity += sc.m_Normal * (b2.m_OneOverMass * alter);
Vector3 tempV = b2.m_LinearVelocity;
tempV.Normalize();
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
float massRatio = 1.0f / (b2.m_OneOverMass / b1.m_OneOverMass);
//If mass of object2 is more than object1, do not apply restitution at all
float rest = MathHelper.Lerp(sc.m_Restitution, 1, massRatio - 0.5f);
sc.m_Restitution = MathHelper.Clamp(rest, sc.m_Restitution, 1);
}
b2.m_AngularVelocity += sc.m_Body2LocalPointCrossedNormal * (alter);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(b2.m_AngularVelocity);
}
}
}
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
float cdt = dt / (float)(iteration + 1);
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
//Integrate velocities
body.m_LinearVelocity += body.m_LinearAcceleration * (cdt);
body.m_AngularVelocity += body.m_AngularAcceleration * (cdt);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body.m_AngularVelocity);
if (body.m_MaxAngularVelocity > 0.0f && body.m_AngularVelocity.Length() > body.m_MaxAngularVelocity)
{
body.m_AngularVelocity = MyMwcUtils.Normalize(body.m_AngularVelocity);
body.m_AngularVelocity *= body.m_MaxAngularVelocity;
}
if (body.m_MaxLinearVelocity > 0.0f && body.m_LinearVelocity.Length() > body.m_MaxLinearVelocity)
{
body.m_LinearVelocity = MyMwcUtils.Normalize(body.m_LinearVelocity);
body.m_LinearVelocity *= body.m_MaxLinearVelocity;
}
}
}
}
//Integrate position and orientation
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
if (body.m_State != MyRBSolverBody.SolverBodyState.SBS_Static)
{
Vector3 transl = body.m_Matrix.Translation + body.m_LinearVelocity * (dt);
Vector3 dir = body.m_AngularVelocity;
float ang = dir.Length();
if (ang > 0.0f)
{
body.m_Matrix.Translation = Vector3.Zero;
Vector3.Divide(ref dir, ang, out dir); // dir /= ang;
ang *= dt;
Matrix rot;
Matrix.CreateFromAxisAngle(ref dir, ang, out rot);
Matrix.Multiply(ref body.m_Matrix, ref rot, out body.m_Matrix);
MyPhysicsUtils.Orthonormalise(ref body.m_Matrix);
}
body.m_Matrix.Translation = transl;
}
}
}