/// <summary>
/// Creates a solver body from a rbo and prepares solver data
/// </summary>
private MyRBSolverBody AddRigidBody(MyRigidBody rbo)
{
MyRBSolverBody sb = m_SolverBodiesPool.Allocate();
sb.Clear();
sb.m_RigidBody = rbo;
sb.m_Matrix = rbo.Matrix;
sb.m_LinearVelocity = rbo.LinearVelocity;
sb.m_AngularVelocity = rbo.AngularVelocity;
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(rbo.AngularVelocity);
sb.m_LinearAcceleration = rbo.ExternalLinearAcceleration;
sb.m_AngularAcceleration = rbo.ExternalAngularAcceleration;
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(rbo.ExternalAngularAcceleration);
sb.m_MaxAngularVelocity = rbo.MaxAngularVelocity;
sb.m_MaxLinearVelocity = rbo.MaxLinearVelocity;
if (rbo.IsStatic() || rbo.IsKinematic())
{
sb.m_OneOverMass = 0;
sb.m_InertiaTensor = MyPhysicsUtils.ZeroInertiaTensor;
sb.m_InvertedInertiaTensor = MyPhysicsUtils.ZeroInertiaTensor;
if (rbo.IsStatic())
{
sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Static;
}
else
{
sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Kinematic;
}
}
else
{
sb.m_OneOverMass = rbo.GetOneOverMass();
Matrix matrix = rbo.Matrix;
matrix.Translation = Vector3.Zero;
sb.m_InertiaTensor = Matrix.Transpose(matrix) * rbo.InertiaTensor * matrix;
// not sure if I can use directly inverted
sb.m_InvertedInertiaTensor = Matrix.Transpose(matrix) * rbo.InvertInertiaTensor * matrix;
sb.m_State = MyRBSolverBody.SolverBodyState.SBS_Dynamic;
}
m_SolverBodies.Add(rbo, sb);
return(sb);
}
/// <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;
}
}
}
/// <summary>
/// creates and prepare the solver constraint
/// </summary>
private void AddConstraint(MyRBContactConstraint rbc)
{
if (rbc.GetRBElementInteraction().GetRigidBody1().IsStatic() && rbc.GetRBElementInteraction().GetRigidBody2().IsStatic())
{
return;
}
for (int i = 0; i < rbc.m_NumCollPts; i++)
{
MyRBSolverConstraint rbsc = m_SolverConstaintPool.Allocate();
rbsc.Clear();
MyCollPointInfo pointInfo = rbc.m_PointInfo[i];
MyRBSolverBody body1 = null;
MyRBSolverBody body2 = null;
MyRigidBody rbo1 = rbc.GetRBElementInteraction().GetRigidBody1();
MyRigidBody rbo2 = rbc.GetRBElementInteraction().GetRigidBody2();
if (!m_SolverBodies.TryGetValue(rbo1, out body1))
{
body1 = AddRigidBody(rbo1);
}
if (!m_SolverBodies.TryGetValue(rbo2, out body2))
{
body2 = AddRigidBody(rbo2);
}
if ((rbo1.GetFlags() & RigidBodyFlag.RBF_DISABLE_COLLISION_RESPONCE) > 0)
{
continue;
}
if ((rbo2.GetFlags() & RigidBodyFlag.RBF_DISABLE_COLLISION_RESPONCE) > 0)
{
continue;
}
rbsc.m_SolverBody1 = body1;
rbsc.m_SolverBody2 = body2;
MyRBMaterial material1 = rbc.GetRBElementInteraction().RBElement1.RBElementMaterial;
MyRBMaterial material2 = rbc.GetRBElementInteraction().RBElement2.RBElementMaterial;
float restitution = material1.NominalRestitution * material2.NominalRestitution;
rbsc.m_RBConstraint = rbc;
rbsc.m_Magnitude = rbc.Magnitude * m_SolutionDamping;
//This is a contact constraint
Vector3 globalVel1 = new Vector3();
Vector3 globalVel2 = new Vector3();
rbc.GetRBElementInteraction().GetRigidBody1().GetGlobalPointVelocity(ref pointInfo.m_Info.m_WorldPosition, out globalVel1);
rbc.GetRBElementInteraction().GetRigidBody2().GetGlobalPointVelocity(ref pointInfo.m_Info.m_WorldPosition, out globalVel2);
Vector3 relativeVelocity = globalVel2 - globalVel1;
float aRelVel = Vector3.Dot(pointInfo.m_Info.m_Normal, relativeVelocity);
float diff = aRelVel;
rbsc.m_Target = diff >= 0.0f ? 0.0f : (-diff * restitution);
if (pointInfo.m_Info.m_InitialPenetration < -0.01f)
{
rbsc.m_Target -= pointInfo.m_Info.m_InitialPenetration * m_DepenetrationCoeficient;
}
rbsc.m_Restitution = restitution;
rbsc.m_Normal = pointInfo.m_Info.m_Normal;
rbsc.m_ContactPoint = pointInfo.m_Info.m_WorldPosition;
rbsc.m_Body1LocalPoint = pointInfo.m_Info.m_R0;
rbsc.m_Body2LocalPoint = pointInfo.m_Info.m_R1;
Vector3 cross1 = Vector3.Cross(pointInfo.m_Info.m_R0, pointInfo.m_Info.m_Normal);
Vector3 cross2 = Vector3.Cross(pointInfo.m_Info.m_R1, pointInfo.m_Info.m_Normal);
rbsc.m_Body1LocalPointCrossedNormal = Vector3.Transform(cross1, body1.m_InvertedInertiaTensor);
rbsc.m_Body2LocalPointCrossedNormal = Vector3.Transform(cross2, body2.m_InvertedInertiaTensor);
rbsc.m_StaticFriction = material1.NominalStaticFriction * material2.NominalStaticFriction;
rbsc.m_DynamicFriction = material1.NominalDynamicFriction * material2.NominalDynamicFriction;
rbsc.m_Affection = 0.0f;
if (body1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbsc.m_Affection += Vector3.Dot(rbsc.m_Normal, (rbsc.m_Normal * body1.m_OneOverMass + Vector3.Cross(rbsc.m_Body1LocalPointCrossedNormal, rbsc.m_Body1LocalPoint)));
body1.m_LinearVelocity -= rbsc.m_Normal * (body1.m_OneOverMass * rbsc.m_Magnitude);
body1.m_AngularVelocity -= rbsc.m_Body1LocalPointCrossedNormal * (rbsc.m_Magnitude);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body1.m_AngularVelocity);
}
if (body2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbsc.m_Affection += Vector3.Dot(rbsc.m_Normal, (rbsc.m_Normal * body2.m_OneOverMass + Vector3.Cross(rbsc.m_Body2LocalPointCrossedNormal, rbsc.m_Body2LocalPoint)));
body2.m_LinearVelocity -= rbsc.m_Normal * (body2.m_OneOverMass * rbsc.m_Magnitude);
body2.m_AngularVelocity -= rbsc.m_Body2LocalPointCrossedNormal * (rbsc.m_Magnitude);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body2.m_AngularVelocity);
}
if (rbo1.ContactModifyNotificationHandler != null)
{
if (!rbo1.ContactModifyNotificationHandler.OnContact(ref rbsc))
{
m_SolverConstaintPool.Deallocate(rbsc);
continue;
}
}
if (rbo2.ContactModifyNotificationHandler != null)
{
if (!rbo2.ContactModifyNotificationHandler.OnContact(ref rbsc))
{
m_SolverConstaintPool.Deallocate(rbsc);
continue;
}
}
m_SolverConstraints.Add(rbsc);
}
}
/// <summary>
/// Updates rigid body position after solver has computed new velocities
/// </summary>
private void UpdatePositions(float dt)
{
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
MyRigidBody rbo = kvp.Key;
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbo.LinearAcceleration = rbo.ExternalLinearAcceleration;
rbo.AngularAcceleration = rbo.ExternalAngularAcceleration;
rbo.ExternalAngularAcceleration = Vector3.Zero;
rbo.ExternalLinearAcceleration = Vector3.Zero;
#if PHYSICS_CHECK
MyCommonDebugUtils.AssertDebug(float.IsNaN(body.m_LinearVelocity.X) == false);
MyCommonDebugUtils.AssertDebug(float.IsNaN(body.m_Matrix.Translation.X) == false);
#endif
rbo.LinearVelocity = body.m_LinearVelocity;
rbo.AngularVelocity = body.m_AngularVelocity;
/*Vector3 pos, scale, scale2;Quaternion rot;
* rbo.Matrix.Decompose(out scale, out rot, out pos);
* body.m_Matrix.Decompose(out scale2, out rot, out pos);
* rbo.Matrix = Matrix.CreateScale(scale) * Matrix.CreateFromQuaternion(rot) * Matrix.CreateTranslation(pos);*/
rbo.Matrix = body.m_Matrix;
rbo.ApplyDamping(dt);
foreach (var el in rbo.GetRBElementList())
{
el.UpdateAABB();
}
}
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Kinematic)
{
rbo.LinearAcceleration = Vector3.Zero;
rbo.AngularAcceleration = Vector3.Zero;
rbo.ExternalAngularAcceleration = Vector3.Zero;
rbo.ExternalLinearAcceleration = Vector3.Zero;
rbo.LinearVelocity = body.m_LinearVelocity;
rbo.AngularVelocity = body.m_AngularVelocity;
rbo.SetMatrix(body.m_Matrix);
}
}
for (int i = 0; i < m_SolverConstraints.Count; i++)
{
MyRBSolverConstraint sc = m_SolverConstraints[i];
if (sc.m_Magnitude != 0.0f)
{
sc.m_RBConstraint.m_Magnitude = sc.m_Magnitude;
}
}
}
