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