/// <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> /// 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 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> /// Computes the inertia tensor of a rigid body using box inertia definition /// </summary> public static void ComputeIntertiaTensor(MyRigidBody rbo) { MyCommonDebugUtils.AssertDebug(rbo != null); MyCommonDebugUtils.AssertDebug(rbo.GetRBElementList().Count > 0); MyCommonDebugUtils.AssertDebug(rbo.GetMass() > 0); float mass = rbo.GetMass(); BoundingBox box; box.Min = new Vector3(FLT_MAX); box.Max = new Vector3(FLT_MIN); BoundingBox aabb; Matrix infTensor = new Matrix(); infTensor.M11 = FLT_MAX; infTensor.M22 = FLT_MAX; infTensor.M33 = FLT_MAX; infTensor.M44 = 1.0f; if (rbo.IsStatic()) { rbo.InertiaTensor = infTensor; return; } if (rbo.GetRBElementList().Count > 1) { for (int e = 0; e < rbo.GetRBElementList().Count; e++) { MyRBElement el = rbo.GetRBElementList()[e]; switch (el.GetElementType()) { case MyRBElementType.ET_TRIANGLEMESH: { rbo.InertiaTensor = infTensor; return; } break; case MyRBElementType.ET_VOXEL: { rbo.InertiaTensor = infTensor; return; } break; default: { aabb = el.GetWorldSpaceAABB(); box = BoundingBox.CreateMerged(box, aabb); } break; } } Vector3 size = box.Max - box.Min; infTensor.M11 = mass * (size.Y * size.Y + size.Z * size.Z) / 12.0f; infTensor.M22 = mass * (size.X * size.X + size.Z * size.Z) / 12.0f; infTensor.M33 = mass * (size.X * size.X + size.Y * size.Y) / 12.0f; infTensor.M44 = 1.0f; rbo.InertiaTensor = infTensor; rbo.InvertInertiaTensor = Matrix.Invert(infTensor); return; } MyRBElement elem = rbo.GetRBElementList()[0]; switch (elem.GetElementType()) { case MyRBElementType.ET_TRIANGLEMESH: { rbo.InertiaTensor = infTensor; infTensor.M11 = 0.0f; infTensor.M22 = 0.0f; infTensor.M33 = 0.0f; infTensor.M44 = 0.0f; rbo.InvertInertiaTensor = infTensor; return; } break; case MyRBElementType.ET_VOXEL: { rbo.InertiaTensor = infTensor; infTensor.M11 = 0.0f; infTensor.M22 = 0.0f; infTensor.M33 = 0.0f; infTensor.M44 = 0.0f; rbo.InvertInertiaTensor = infTensor; return; } case MyRBElementType.ET_SPHERE: { float radius = ((MyRBSphereElement)elem).Radius; infTensor.M11 = 2.0f / 5.0f * mass * radius * radius; infTensor.M22 = 2.0f / 5.0f * mass * radius * radius; infTensor.M33 = 2.0f / 5.0f * mass * radius * radius; infTensor.M44 = 1.0f; rbo.InertiaTensor = infTensor; //rbo.InvertInertiaTensor = Matrix.Invert(infTensor); return; } break; case MyRBElementType.ET_BOX: { //Vector3 size = ((MyRBBoxElement)elem).Size; //infTensor.M11 = mass * (size.Y * size.Y + size.Z * size.Z) / 12.0f; //infTensor.M22 = mass * (size.X * size.X + size.Z * size.Z) / 12.0f; //infTensor.M33 = mass * (size.X * size.X + size.Y * size.Y) / 12.0f; //infTensor.M44 = 1.0f; //rbo.InertiaTensor = infTensor; //rbo.InvertInertiaTensor = Matrix.Invert(infTensor); // HACK: After speaking with PetrM, computing changed like box is sphere float radius = ((MyRBBoxElement)elem).Size.Length() / 2; infTensor.M11 = 2.0f / 5.0f * mass * radius * radius; infTensor.M22 = 2.0f / 5.0f * mass * radius * radius; infTensor.M33 = 2.0f / 5.0f * mass * radius * radius; infTensor.M44 = 1.0f; rbo.InertiaTensor = infTensor; //rbo.InvertInertiaTensor = Matrix.Invert(infTensor); return; } break; default: MyCommonDebugUtils.AssertDebug(false); break; } }
/// <summary> /// Adding rigid body recursively to check for constraint connections and make sure that its only in 1 island /// </summary> private void AddRigidBody(MyRigidBody rbo, MyRigidBody secondRigidBody, MyRigidBodyIsland addIsland) { if (rbo.IsStatic()) { rbo.PutToSleep(); return; } if (!m_proccesedList.Add(rbo)) { return; } // add rigid bodies to island recursively int numInteractions = 0; for (int j = 0; j < rbo.GetRBElementList().Count; j++) { MyRBElement el = rbo.GetRBElementList()[j]; numInteractions += el.GetRBElementInteractions().Count; for (int k = 0; k < el.GetRBElementInteractions().Count; k++) { if (addIsland == null && !rbo.IsStatic()) { addIsland = m_islandsPool.Allocate(); addIsland.Clear(); addIsland.IterationCount = 0; addIsland.AddRigidBody(rbo); m_islands.Add(addIsland); } else { if (!rbo.IsStatic()) { addIsland.AddRigidBody(rbo); } } MyRBElementInteraction intr = el.GetRBElementInteractions()[k]; if (intr.GetRigidBody1() != rbo && intr.GetRigidBody2() != secondRigidBody) { AddRigidBody(intr.GetRigidBody1(), rbo, addIsland); } if (intr.GetRigidBody2() != rbo && intr.GetRigidBody1() != secondRigidBody) { AddRigidBody(intr.GetRigidBody2(), rbo, addIsland); } } } // isolated rbo if (numInteractions == 0 && !rbo.IsStatic()) { MyRigidBodyIsland island = m_islandsPool.Allocate(); island.Clear(); island.IterationCount = 0; island.AddRigidBody(rbo); m_islands.Add(island); } }
/// <summary> /// Adding rigid body recursively to check for constraint connections and make sure that its only in 1 island /// </summary> private void AddRigidBody(MyRigidBody rbo, MyRigidBody secondRigidBody,MyRigidBodyIsland addIsland) { if(rbo.IsStatic()) { rbo.PutToSleep(); return; } if (!m_proccesedList.Add(rbo)) return; // add rigid bodies to island recursively int numInteractions = 0; for (int j = 0; j < rbo.GetRBElementList().Count; j++) { MyRBElement el = rbo.GetRBElementList()[j]; numInteractions += el.GetRBElementInteractions().Count; for (int k = 0; k < el.GetRBElementInteractions().Count; k++) { if (addIsland == null && !rbo.IsStatic()) { addIsland = m_islandsPool.Allocate(); addIsland.Clear(); addIsland.IterationCount = 0; addIsland.AddRigidBody(rbo); m_islands.Add(addIsland); } else { if(!rbo.IsStatic()) { addIsland.AddRigidBody(rbo); } } MyRBElementInteraction intr = el.GetRBElementInteractions()[k]; if(intr.GetRigidBody1() != rbo && intr.GetRigidBody2() != secondRigidBody) { AddRigidBody(intr.GetRigidBody1(),rbo,addIsland); } if (intr.GetRigidBody2() != rbo && intr.GetRigidBody1() != secondRigidBody) { AddRigidBody(intr.GetRigidBody2(), rbo, addIsland); } } } // isolated rbo if (numInteractions == 0 && !rbo.IsStatic()) { MyRigidBodyIsland island = m_islandsPool.Allocate(); island.Clear(); island.IterationCount = 0; island.AddRigidBody(rbo); m_islands.Add(island); } }
/// <summary> /// Checks the sleep state of rigids and decides if its possible to sleep the whole island /// </summary> private void UpdateSleepState() { //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("GetIslands"); List <MyRigidBodyIsland> islands = MyPhysics.physicsSystem.GetRigidBodyModule().GetRigidBodyIslandGeneration().GetIslands(); float dt = MyPhysics.physicsSystem.GetRigidBodyModule().CurrentTimeStep; //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock(); //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("Loop Islands"); int count = 0; for (int i = 0; i < islands.Count; i++) { MyRigidBodyIsland island = islands[i]; bool canDeactivate = true; //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("Loop Rigids"); for (int j = 0; j < island.GetRigids().Count; j++) { count++; MyRigidBody rbo = island.GetRigids()[j]; if (rbo.IsStatic()) { rbo.PutToSleep(); MyPhysics.physicsSystem.GetRigidBodyModule().RemoveActiveRigid(rbo); } else { //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("MoveVolumeFast"); foreach (var el in rbo.GetRBElementList()) { m_Broadphase.MoveVolumeFast(el); } //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock(); //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("NotifyMotionHandler"); if (rbo.NotifyMotionHandler != null) { rbo.NotifyMotionHandler.OnMotion(rbo, dt); } //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock(); } // dynamic rigids use different approach if (!rbo.CanDeactivate()) { canDeactivate = false; } } //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock(); //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().StartProfilingBlock("canDeactivate"); // check if all are asleep if (canDeactivate) { foreach (var rbo in island.GetRigids()) { rbo.PutToSleep(); MyPhysics.physicsSystem.GetRigidBodyModule().RemoveActiveRigid(rbo); } } //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock(); } //MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().EndProfilingBlock(); MinerWars.AppCode.Game.Render.MyRender.GetRenderProfiler().ProfileCustomValue("UpdateSleepState", count); }