Ejemplo n.º 1
0
        /// <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);
        }
Ejemplo n.º 2
0
        /// <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;
                }
            }
        }
Ejemplo n.º 3
0
        /// <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);
            }
        }
Ejemplo n.º 4
0
        /// <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;
                }
            }
        }