public void AddContactConstraint(MyRBElementInteraction interaction, MySmallCollPointInfo[] pointInfos, int numCollPts)
{
lock (m_Locker)
{
if (m_FreeCc.Count == 0)
{
m_FreeCc.Push(new MyRBContactConstraint());
}
MyRBContactConstraint cc = m_FreeCc.Pop();
cc.Init(interaction, pointInfos, numCollPts);
m_ActiveContactConstrains.Add(cc);
}
}
public void Fill(MyRBContactConstraint cc)
{
m_RigidBody1 = cc.GetRBElementInteraction().GetRigidBody1();
m_RigidBody2 = cc.GetRBElementInteraction().GetRigidBody2();
m_Element1 = cc.GetRBElementInteraction().RBElement1;
m_Element2 = cc.GetRBElementInteraction().RBElement2;
m_ContactNormal = cc.m_PointInfo[0].m_Info.m_Normal;
m_ContactPoint = cc.m_PointInfo[0].m_Info.m_WorldPosition;
m_Velocity1 = cc.m_PointInfo[0].m_Info.m_V0;
m_Velocity2 = cc.m_PointInfo[0].m_Info.m_V1;
m_Guid = cc.GUID;
}
public void Fill(MyRBContactConstraint cc)
{
m_RigidBody1 = cc.GetRBElementInteraction().GetRigidBody1();
m_RigidBody2 = cc.GetRBElementInteraction().GetRigidBody2();
m_Element1 = cc.GetRBElementInteraction().RBElement1;
m_Element2 = cc.GetRBElementInteraction().RBElement2;
m_ContactNormal = cc.m_PointInfo[0].m_Info.m_Normal;
m_ContactPoint = cc.m_PointInfo[0].m_Info.m_WorldPosition;
m_Velocity1 = cc.m_PointInfo[0].m_Info.m_V0;
m_Velocity2 = cc.m_PointInfo[0].m_Info.m_V1;
m_Guid = cc.GUID;
}
/// <summary>
/// Collects all constraints
/// </summary>
private void PrepareConstraints()
{
var cl = MyPhysics.physicsSystem.GetContactConstraintModule().GetActiveRBContactConstraints();
for (int i = 0; i < cl.Count; i++)
{
MyRBContactConstraint cc = cl[i];
MyRigidBody testRbo = cc.GetRBElementInteraction().GetRigidBody1();
if (testRbo.IsStatic() || testRbo.IsKinematic())
{
testRbo = cc.GetRBElementInteraction().GetRigidBody2();
}
for (int j = 0; j < m_Island.GetRigids().Count; j++)
{
MyRigidBody rbo = m_Island.GetRigids()[j];
if (rbo == testRbo)
{
AddConstraint(cc);
break;
}
}
}
}
/// <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>
/// 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);
}
}
private void FreeRBContactConstraint(MyRBContactConstraint info)
{
info.Destroy();
m_FreeCc.Push(info);
}
private void FreeRBContactConstraint(MyRBContactConstraint info)
{
info.Destroy();
m_FreeCc.Push(info);
}
