public MyRBElement()
{
m_RBMaterial = new MyRBMaterial(0.5f, 0.5f, 0.7f, 0);
m_RigidBody = null;
m_ElementInteractions = new List <MyRBElementInteraction>(2);
Flags = MyElementFlag.EF_RB_ELEMENT | MyElementFlag.EF_AABB_DIRTY;
m_GroupMask = new MyGroupMask();
}
public MyRBElement()
{
m_RBMaterial = new MyRBMaterial(0.5f, 0.5f, 0.7f, 0);
m_RigidBody = null;
m_ElementInteractions = new List<MyRBElementInteraction>(2);
Flags = MyElementFlag.EF_RB_ELEMENT | MyElementFlag.EF_AABB_DIRTY;
m_GroupMask = new MyGroupMask();
}
public virtual void SetToDefault()
{
m_Matrix = Matrix.Identity;
m_CollisionLayer = 0;
m_RBMaterial = null;
}
/// <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);
}
}
public virtual void SetToDefault()
{
m_Matrix = Matrix.Identity;
m_CollisionLayer = 0;
m_RBMaterial = null;
}
