Example #1
0
        private void оптимизироватьToolStripMenuItem_Click(object sender, EventArgs e)
        {
            try
            {
                DowndateGrid();
            }
            catch
            {
                return;
            }

            Fraction F;

            Fraction[]    solution;
            SimplexSolver solver = new SimplexSolver();

            solver.DebugNewSimplexTable += new SimplexSolver.DebugSimplexTableHandler(solver_DebugNewSimplexTable);
            formTables = null;

            for (int i = 0; i < m; i++)
            {
                solver.AddLimtation(A[i], S[i], B[i]);
            }
            solver.SetTargetFunctionCoefficients(C);

            // решаем
            solution = solver.Solve();

            F = 0;
            for (int i = 0; i < C.Length; i++)
            {
                F += C[i] * solution[i];
            }

            formTables.AddLine("Решение (max):", solution);
            formTables.AddLine("Значение Fmax:", new Fraction[] { F });

            // переделываем под минимизацию
            for (int i = 0; i < C.Length; i++)
            {
                C[i] = -C[i];
            }
            solver.SetTargetFunctionCoefficients(C);

            formTables.ResetIterationCounter();

            // снова решаем
            solution = solver.Solve();

            F = 0;
            for (int i = 0; i < C.Length; i++)
            {
                F -= C[i] * solution[i];
            }

            formTables.AddLine("Решение (min):", solution);
            formTables.AddLine("Значение Fmin:", new Fraction[] { F });

            formTables.UpdateGrid();
        }
Example #2
0
        public void CalculateGoals(ObjectiveType objective, params ConstraintType[] constraints)
        {
            var simplex = new SimplexSolver();

            var goalEarned       = GoalEarned;
            var objCoefficients  = new Dictionary <object, double>();
            var consCoefficients = new Dictionary <object, double>();

            foreach (var section in sections)
            {
                var staticEarned = section.Assignments.Where(a => !a.GoalSelected).Sum(a => a.Earned);
                var totalWorth   = section.Assignments.Sum(a => a.Worth);
                goalEarned -= section.Weight * 100 * (staticEarned / totalWorth);

                foreach (var assignment in section.Assignments)
                {
                    if (assignment.GoalSelected)
                    {
                        var objCoefficient = objective == ObjectiveType.Equal ? 1 : section.Weight;
                        objCoefficients.Add(assignment.Id, objCoefficient);

                        var consCoefficient = section.Weight * 100 / totalWorth;
                        consCoefficients.Add(assignment.Id, consCoefficient);

                        var maxEarnedCoeff = new Dictionary <object, double>();
                        maxEarnedCoeff.Add(assignment.Id, 1);
                        simplex.AddConstraint(maxEarnedCoeff, Relationship.LessThanOrEqual, assignment.GoalEarned);
                    }
                }
            }
            simplex.SetObjective(Optimization.Min, objCoefficients);
            simplex.AddConstraint(consCoefficients, Relationship.GreaterThanOrEqual, goalEarned);

            simplex.Solve(out IDictionary <object, double> solution); // 100, 100, 36.923
        }
Example #3
0
        public string Calculate(
            int a_need,
            int b_need,
            int c_need,
            int d_need,
            int e_need,
            int f_need,
            int g_need,
            int h_need)
        {
            solver.SetBounds(a, a_need, Rational.PositiveInfinity);
            solver.SetBounds(b, b_need, Rational.PositiveInfinity);
            solver.SetBounds(c, c_need, Rational.PositiveInfinity);
            solver.SetBounds(d, d_need, Rational.PositiveInfinity);
            solver.SetBounds(e, e_need, Rational.PositiveInfinity);
            solver.SetBounds(f, f_need, Rational.PositiveInfinity);
            solver.SetBounds(g, g_need, Rational.PositiveInfinity);
            solver.SetBounds(h, h_need, Rational.PositiveInfinity);

            solver.Solve(new SimplexSolverParams());

            StringBuilder output = new StringBuilder();

            int total_a = 0, total_b = 0, total_c = 0, total_d = 0, total_e = 0, total_f = 0, total_g = 0, total_h = 0;

            foreach (var kv in oreDictionary.ORE_Dictionary)
            {
                if (useFullORE || kv.Value.isMain)
                {
                    int number = (int)Math.Ceiling(solver.GetValue(kv.Value.id).ToDouble());

                    if (number != 0)
                    {
                        output.Append($"矿物名称:{kv.Key},购买总量:{AmountFormat(number)},总金额:{MoneyFormat(number * kv.Value.price)}\n");
                        total_a += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["三钛合金"]) * number;
                        total_b += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["类晶体胶矿"]) * number;
                        total_c += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["类银超金属"]) * number;
                        total_d += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["同位聚合体"]) * number;
                        total_e += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["超星诺克石"]) * number;
                        total_f += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["晶状石英岩"]) * number;
                        total_g += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["超噬矿"]) * number;
                        total_h += (int)Math.Round(kv.Value.ratio * kv.Value.mineral["莫尔石"]) * number;
                    }
                }
            }

            output.Append($"三钛合金,需求:{a_need},化矿:{total_a},结余 :{total_a - a_need}\n");
            output.Append($"类晶体胶矿,需求:{b_need},化矿:{total_b},结余 :{total_b - b_need}\n");
            output.Append($"类银超金属,需求:{c_need},化矿:{total_c},结余 :{total_c - c_need}\n");
            output.Append($"同位聚合体,需求:{d_need},化矿:{total_d},结余 :{total_d - d_need}\n");
            output.Append($"超星诺克石,需求:{e_need},化矿:{total_e},结余 :{total_e - e_need}\n");
            output.Append($"晶状石英岩,需求:{f_need},化矿:{total_f},结余 :{total_f - f_need}\n");
            output.Append($"超噬矿,需求:{g_need},化矿:{total_g},结余 :{total_g - g_need}\n");
            output.Append($"莫尔石,需求:{h_need},化矿:{total_h},结余 :{total_h - h_need}\n");

            output.Append($"总金额:{MoneyFormat((int)solver.GetValue(cost).ToDouble())}\n");

            return(output.ToString());
        }
Example #4
0
        /// <summary>
        /// Konstruktor okna z wynikami
        /// </summary>
        /// <param name="solver">Solver do rozwiązania</param>
        /// <param name="alloys">Lista stopów w procesie</param>
        /// <param name="smelt">Jednoelementowa lista wytopów</param>
        /// <param name="isWeightFun">typ optymalizacji</param>
        public ProcessResults(SimplexSolver solver, List <Alloy> alloys, List <Smelt> smelt, bool isWeightFun)
        {
            InitializeComponent();

            //rozwiązanie układu
            solver.Solve(new SimplexSolverParams());

            //inicjalizacja danych
            this.solver  = solver;
            this.alloys  = alloys;
            this.smelt   = smelt;
            isWeightType = isWeightFun;
            total        = 0;

            //jezeli typ optymalizacji masowy, odpowiedni komunikat i stworzenie listy z rozwiązaniami
            if (isWeightFun)
            {
                lblintro.Text = "Minimalna ilość składników potrzebnych do wytopienia " + smelt.ElementAt(0).Weight.ToString() + " [g] wytopu " + smelt.ElementAt(0).name.ToString() + " spełniających normy:";
                this.solutionView.ItemsSource = solut = createSolution(alloys, solver);
                foreach (Solution s in solut)
                {
                    if (double.IsNaN(s.solNum))
                    {
                        btn_raport.IsEnabled = false;
                        lblTotal.Text        = "Brak rozwiązań - niepoprawne dane wejściowe";
                    }
                    else
                    {
                        btn_raport.IsEnabled = true;
                        //suma wszystkich rozwiązań
                        lblTotal.Text = total.ToString("0.00");
                    }
                }
            }
            else
            {
                lblintro.Text = "Minimalna ilość składników potrzebnych do wytopienia " + smelt.ElementAt(0).Weight.ToString() + " [g] wytopu " + smelt.ElementAt(0).name.ToString() + " przy możliwie najniższych kosztach:";
                this.solutionView.ItemsSource = solut = createSolution(alloys, solver);
                foreach (Solution s in solut)
                {
                    if (double.IsNaN(s.solNum))
                    {
                        btn_raport.IsEnabled = false;
                        lblTotal.Text        = "Brak rozwiązań - niepoprawne dane wejściowe";
                    }
                    else
                    {
                        btn_raport.IsEnabled = true;
                        //suma wszystkich rozwiązań
                        lblTotal.Text = total.ToString("0.00");
                    }
                }
            }
        }
Example #5
0
        public void Generate()
        {
            // TODO: Int32 is overloading
            SimplexSolver solver = new SimplexSolver();

            mealPortions = new List <int>();
            int Calories, cost, Proteins, Carbohydrates, Fats;

            solver.AddRow("calories", out Calories);
            solver.SetIntegrality(Calories, true);
            solver.AddRow("proteins", out Proteins);
            solver.SetIntegrality(Calories, true);
            solver.AddRow("carbohydrates", out Carbohydrates);
            solver.SetIntegrality(Calories, true);
            solver.AddRow("fats", out Fats);
            solver.SetIntegrality(Fats, true);
            solver.AddRow("cost", out cost);

            Random rand = new Random();

            foreach (Meal m in this.meals)
            {
                int portions = new int();
                solver.AddVariable(m.Name, out portions);
                solver.SetBounds(portions, 0, 5);
                solver.SetIntegrality(portions, true);
                solver.SetCoefficient(Calories, portions, (int)m.Calories_Per_Portions);
                int prots = ((int)(m.Proteins * m.Portion_Size / 100) - this.proteins) * (-1);
                solver.SetCoefficient(Proteins, portions, prots);
                int carbs = ((int)(m.Carbohydrates * m.Portion_Size / 100) - this.carbohydrates) * (-1);
                solver.SetCoefficient(Carbohydrates, portions, carbs);
                int fa = ((int)(m.Fats * m.Portion_Size / 100) - this.fats) * (-1);
                solver.SetCoefficient(Fats, portions, fa);
                solver.SetCoefficient(cost, portions, rand.Next(0, 100));
                mealPortions.Add(portions);
            }

            solver.SetBounds(Calories, this.calories, this.calories);
            //solver.SetBounds(Proteins, 0, int.MaxValue);
            //solver.SetBounds(Carbohydrates, 0, int.MaxValue);
            //solver.SetBounds(Fats, 0, int.MaxValue);
            solver.AddGoal(cost, 1, true);
            solver.Solve(new SimplexSolverParams());
            for (int i = 0; i < this.mealPortions.Count; i++)
            {
                this.mealPortions[i] = (int)solver.GetValue(this.mealPortions[i]).ToDouble();
            }
        }
Example #6
0
        static void Main(string[] args)
        {
            SimplexSolver solver = new SimplexSolver();
            int           savid, vzvid;

            solver.AddVariable("Saudi", out savid);
            solver.AddVariable("Ven", out vzvid);
            solver.SetBounds(savid, 0, 9000);
            solver.SetBounds(vzvid, 0, 6000);

            int c1rid, c2rid, c3rid, goalrid;

            solver.AddRow("c1", out c1rid);
            solver.AddRow("c2", out c2rid);
            solver.AddRow("c3", out c3rid);
            solver.AddRow("goal", out goalrid);

            // add coefficients to constraint rows
            solver.SetCoefficient(c1rid, savid, 0.3);
            solver.SetCoefficient(c1rid, vzvid, 0.4);
            solver.SetBounds(c1rid, 2000, Rational.PositiveInfinity);
            solver.SetCoefficient(c2rid, savid, 0.4);
            solver.SetCoefficient(c2rid, vzvid, 0.2);
            solver.SetBounds(c2rid, 1500, Rational.PositiveInfinity);
            solver.SetCoefficient(c3rid, savid, 0.2);
            solver.SetCoefficient(c3rid, vzvid, 0.3);
            solver.SetBounds(c3rid, 500, Rational.PositiveInfinity);

            // add objective (goal) to model and specify minimization (==true)
            solver.SetCoefficient(goalrid, savid, 20);
            solver.SetCoefficient(goalrid, vzvid, 15);
            solver.AddGoal(goalrid, 1, true);

            solver.Solve(new SimplexSolverParams());

            Console.WriteLine("SA {0}, VZ {1}, C1 {2}, C2 {3}, C3 {4}, Goal {5}",
                              solver.GetValue(savid).ToDouble(), solver.GetValue(vzvid).ToDouble(),
                              solver.GetValue(c1rid).ToDouble(), solver.GetValue(c2rid).ToDouble(),
                              solver.GetValue(c3rid).ToDouble(), solver.GetValue(goalrid).ToDouble());

            Console.ReadLine();
        }
Example #7
0
File: Otm.cs Project: falreis/tcc
        public void Get()
        {
            SimplexSolver solver = new SimplexSolver();

            double[] estimatedProfitOfProjectX  = new double[] { 1, 1.8, 1.6, 0.8, 1.4 };
            double[] capitalRequiredForProjectX = new double[] { 6, 12, 10, 4, 8 };
            double   availableCapital           = 20;

            int[] chooseProjectX = new int[5];

            int profit;

            solver.AddRow("profit", out profit);
            solver.AddGoal(profit, 0, false);

            int expenditure;

            solver.AddRow("expenditure", out expenditure);
            solver.SetBounds(expenditure, 0, availableCapital);

            for (int i = 0; i < 5; i++)
            {
                solver.AddVariable(string.Format("project{0}", i), out chooseProjectX[i]);
                solver.SetBounds(chooseProjectX[i], 0, 1);
                solver.SetIntegrality(chooseProjectX[i], true);
                solver.SetCoefficient(profit, chooseProjectX[i], estimatedProfitOfProjectX[i]);
                solver.SetCoefficient(expenditure, chooseProjectX[i], capitalRequiredForProjectX[i]);
            }

            SimplexSolverParams param = new SimplexSolverParams();

            param.MixedIntegerGenerateCuts = true;
            solver.Solve(param);

            Console.WriteLine(solver.MipResult);
            for (int i = 0; i < 5; i++)
            {
                Console.WriteLine("Project {0} is {1} selected.", i, solver.GetValue(chooseProjectX[i]) == 1 ? "" : "not ");
            }
            Console.WriteLine("The estimated total profit is: ${0} million.", (double)solver.GetValue(profit).ToDouble());
            Console.WriteLine("The total expenditure is: ${0} million.", solver.GetValue(expenditure).ToDouble());
        }
Example #8
0
        public void Test1()
        {
            var s = new SimplexSolver();

            var coeff = new Dictionary<object, double>
            {
                { 1, .35 },
                { 2, .65 },
                { 3, .65 }
            };
            s.SetObjective(Optimization.Min, coeff);

            coeff = new Dictionary<object, double>
            {
                { 1, .116667 },
                { 2, .216667 },
                { 3, .216667 }
            };
            s.AddConstraint(coeff, Relationship.GreaterThanOrEqual, 43.5);

            coeff = new Dictionary<object, double> { { 1, 1 } };
            s.AddConstraint(coeff, Relationship.LessThanOrEqual, 95);

            coeff = new Dictionary<object, double> { { 2, 1 } };
            s.AddConstraint(coeff, Relationship.LessThanOrEqual, 95);

            coeff = new Dictionary<object, double> { { 3, 1 } };
            s.AddConstraint(coeff, Relationship.LessThanOrEqual, 90);

            coeff = new Dictionary<object, double>
            {
                { 2, 1 },
                { 3, -1 }
            };
            s.AddConstraint(coeff, Relationship.Equal, 0);

            s.Solve(out IDictionary<object, double> solution);
        }
Example #9
0
        /// <summary>
        /// Solves a sample (randomly generated?) cutting stock problem.
        /// Given a bolt of cloth of fixed width, and demand for cut strips of the cloth, determine the min "loss" cut patterns to use and how many
        /// of them.
        /// Loss is defined as the scrap thrown away.
        /// It is acceptable to have extra cut widths made.  They do not contribute to the cost.  (this may be unrealistic in the real world)
        /// Solver runs by 1st creating an enumeration of possible cut patterns using a CspSolver, then choosing between the patterns and selecting a qty of the patterns such that the
        /// amount of scrap is minimized and all demand is met using the SimplexSolver MIP code.
        ///
        /// In an industrial case, there would likely be more constraints in the generation of the cut patterns.  There can be other restrictions such as "these can't be done together"
        /// or "these MUST be done together (matching pattern or color?)".  This can easily be added to the CspSolver model.
        /// Also, there are likely other characteristics of the cuts or the master problem which would need adaptations.
        ///
        /// Further, the limit on the columns generated is implemented in a very arbitrary order.  It is more likely that some ordering of the
        /// value of the columns is needed.  In most industrial occurances, the dual variables from the LP relaxation would likely be used to
        /// guide the generation of columns in an interative fasion rather than a one-time shot at the beginning.
        ///
        /// YMMV
        /// </summary>
        public static void ShortCuttingStock()
        {
            Console.WriteLine("*** Short Cutting Stock ***");
            int    NumItems    = 5;     // how many cut widths to generate
            int    ClothWidth  = 40;    // width of the stock to cut the widths from
            double efficiency  = 0.7;   // reject cut patterns less than this % used of the clothwidth
            int    maxPatterns = 100;   // max # of patterns to generate
            bool   verbose     = true;  // set this to true if you want some (useful?) output
            bool   saveMpsFile = false; // set this to true if you want it to save an mps file in c:\\temp\\cutstock.mps

            int itemSizeMin   = 5;      // minimum size for random generation of cut
            int itemSizeMax   = 10;     // maximum size for random generation of cut
            int itemDemandMin = 10;     // minimum random demand for each cut
            int itemDemandMax = 40;     // maximum random demand for each cut

            int seed = 12447;           // use System.DateTime.Now.Millisecond; instead if you want a random problem.

            if (verbose)
            {
                System.Console.WriteLine(String.Format("Random seed={0}\tmaxWidth={1}", seed, ClothWidth));
            }

            Random rand = new Random(seed);

            int[] cuts   = new int[NumItems];
            int[] demand = new int[NumItems];
            // item weights and demands
            for (int cnt = 0; cnt < NumItems; cnt++)
            {
                cuts[cnt]   = rand.Next(itemSizeMin, itemSizeMax);;
                demand[cnt] = rand.Next(itemDemandMin, itemDemandMax);
                if (verbose)
                {
                    System.Console.WriteLine(String.Format("item[{0}]\tweight={1}\tdemand={2}", cnt, cuts[cnt], demand[cnt]));
                }
            }
            List <int[]> patterns;

            SolveKnapsack(maxPatterns, cuts, ClothWidth, efficiency, out patterns);
            SimplexSolver solver2 = new SimplexSolver();
            int           vId     = 0;

            int[] usage = new int[patterns.Count];
            // construct rows that make sure that the demand is met for each kind of cut
            for (int cnt = 0; cnt < NumItems; cnt++)
            {
                solver2.AddRow(String.Format("item{0}", cnt), out vId);
                solver2.SetBounds(vId, demand[cnt], Rational.PositiveInfinity);
            }
            int patCnt = 0;

            if (verbose)
            {
                System.Console.WriteLine(String.Format("Generated {0} patterns", patterns.Count));
            }
            // set usage coeffs (A matrix entries) -- put the patterns as columns in the MIP.
            Dictionary <int, int> patIdForCol = new Dictionary <int, int>();

            foreach (int[] pattern in patterns)
            {
                int    pId     = 0;
                String varName = String.Format("Pattern{0}", patCnt);
                solver2.AddVariable(varName, out pId);
                patIdForCol[pId] = patCnt;
                solver2.SetIntegrality(pId, true);
                solver2.SetBounds(pId, 0, Rational.PositiveInfinity);
                for (int cnt = 0; cnt < NumItems; cnt++)
                {
                    solver2.SetCoefficient(cnt, pId, pattern[cnt]); // set the coefficient in the matrix
                                                                    // accumulate the quantity used for this pattern.  It will be used to figure out the scrap later.
                    usage[patCnt] += pattern[cnt] * cuts[cnt];
                }
                patCnt++;
            }
            // set objective coeffs.  --- the cost is the scrap
            solver2.AddRow("Scrap", out vId);
            for (int cnt = 0; cnt < patterns.Count; cnt++)
            {
                int colId = solver2.GetIndexFromKey(String.Format("Pattern{0}", cnt));
                solver2.SetCoefficient(vId, colId, (ClothWidth - usage[cnt]));
            }
            solver2.AddGoal(vId, 0, true);
            // invoke the IP solver.
            SimplexSolverParams parms = new SimplexSolverParams();

            parms.MixedIntegerGenerateCuts = true;
            parms.MixedIntegerPresolve     = true;

            if (saveMpsFile)
            {
                MpsWriter writer = new MpsWriter(solver2);
                using (TextWriter textWriter = new StreamWriter(File.OpenWrite("c:\\temp\\cutstock.mps")))
                {
                    writer.WriteMps(textWriter, true);
                }
            }
            solver2.Solve(parms);
            if (solver2.LpResult == LinearResult.Optimal &&
                solver2.MipResult == LinearResult.Optimal)
            {
                //Rational[] solutionVals = solver2.GetValues();
                int goalIndex = 0;
                // output if desired.
                if (verbose)
                {
                    System.Console.WriteLine("Solver complete, printing cut plan.");
                    foreach (int cnt in solver2.VariableIndices)
                    {
                        Rational val = solver2.GetValue(cnt);
                        if (val != 0)
                        {
                            if (solver2.IsGoal(cnt))
                            {
                                goalIndex = cnt;
                                System.Console.WriteLine(String.Format("Goal:{0}\t:   {1}\t", val, solver2.GetKeyFromIndex(cnt)));
                            }
                            else if (solver2.IsRow(cnt))
                            {
                                System.Console.WriteLine(String.Format("{0}:\tValue=   {1}\t", solver2.GetKeyFromIndex(cnt), val));
                            }
                            else
                            {
                                System.Console.Write(String.Format("{0}\tQuantity={1}:\t", solver2.GetKeyFromIndex(cnt), val));
                                for (int cnt2 = 0; cnt2 < NumItems; cnt2++)
                                {
                                    System.Console.Write(String.Format("{0} ", patterns[patIdForCol[cnt]][cnt2]));
                                }
                                System.Console.WriteLine(String.Format("\tUsage:{0} / {2} efficiency={1}%", usage[cnt - NumItems], (int)(100 * (double)usage[cnt - NumItems] / (double)ClothWidth), ClothWidth));
                            }
                        }
                    }
                    System.Console.WriteLine(String.Format("Total scrap={0}", solver2.GetSolutionValue(goalIndex)));
                }
            }
            else
            {
                System.Console.WriteLine("Generated problem is infeasible.  It is likely that more generated columns are needed.");
            }

            Console.WriteLine();
        }
    public static unsafe void CheckSupport(CharacterControllerStepInput stepInput,
                                           RigidTransform transform, float maxSlope, MaxHitsCollector <DistanceHit> distanceHitsCollector,
                                           ref NativeArray <SurfaceConstraintInfo> constraints, out CharacterSupportState characterState)
    {
        // If no hits, proclaim unsupported state
        if (distanceHitsCollector.NumHits == 0)
        {
            characterState = CharacterSupportState.Unsupported;
            return;
        }

        // Downwards direction must be normalized
        float3 downwardsDirection = -stepInput.Up;

        Assert.IsTrue(Math.IsNormalized(downwardsDirection));

        // Iterate over distance hits and create constraints from them
        for (int i = 0; i < distanceHitsCollector.NumHits; i++)
        {
            DistanceHit hit = distanceHitsCollector.AllHits[i];
            CreateConstraintFromHit(stepInput.World, float3.zero, stepInput.DeltaTime, hit.RigidBodyIndex, hit.ColliderKey,
                                    hit.Position, hit.SurfaceNormal, hit.Distance, true, out SurfaceConstraintInfo constraint);
            constraints[i] = constraint;
        }

        // Solve downwards (don't respect min delta time, try to solve full step)
        float3 outVelocity = downwardsDirection;
        float3 outPosition = transform.pos;

        SimplexSolver.Solve(stepInput.World, stepInput.DeltaTime, stepInput.Up, distanceHitsCollector.NumHits,
                            ref constraints, ref outPosition, ref outVelocity, out float integratedTime, false);

        // Check support state
        {
            if (math.lengthsq(downwardsDirection - outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
            {
                // If velocity hasn't changed significantly, declare unsupported state
                characterState = CharacterSupportState.Unsupported;
            }
            else if (math.lengthsq(outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
            {
                // If velocity is very small, declare supported state
                characterState = CharacterSupportState.Supported;
            }
            else
            {
                // Check if sliding or supported
                outVelocity = math.normalize(outVelocity);
                float slopeAngleSin   = math.dot(outVelocity, downwardsDirection);
                float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                float maxSlopeCosine  = math.cos(maxSlope);
                if (slopeAngleCosSq < maxSlopeCosine * maxSlopeCosine - SimplexSolver.c_SimplexSolverEpsilon)
                {
                    characterState = CharacterSupportState.Sliding;
                }
                else
                {
                    characterState = CharacterSupportState.Supported;
                }
            }
        }
    }
    public static unsafe void CollideAndIntegrate(
        CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
        MaxHitsCollector <DistanceHit> distanceHitsCollector, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
        ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
    {
        // Copy parameters
        float        deltaTime = stepInput.DeltaTime;
        float3       gravity   = stepInput.Gravity;
        float3       up        = stepInput.Up;
        PhysicsWorld world     = stepInput.World;

        float  remainingTime    = deltaTime;
        float3 lastDisplacement = linearVelocity * remainingTime;

        float3     newPosition = transform.pos;
        quaternion orientation = transform.rot;
        float3     newVelocity = linearVelocity;

        float maxSlopeCos = math.cos(stepInput.MaxSlope);

        // Iterate over hits and create constraints from them
        int numDistanceConstraints = 0;

        for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
        {
            DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
            CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
                                    hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);

            // Check if max slope plane is required
            float verticalComponent = math.dot(constraint.Plane.Normal, up);
            bool  shouldAddPlane    = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos;
            if (shouldAddPlane)
            {
                AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numDistanceConstraints);
            }

            // Add original constraint to the list
            constraints[numDistanceConstraints++] = constraint;
        }

        const float timeEpsilon = 0.000001f;

        for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
        {
            int    numConstraints  = numDistanceConstraints;
            float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;

            // Then do a collider cast (but not in first iteration)
            if (i > 0)
            {
                float3 displacement = lastDisplacement + gravityMovement;
                MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
                ColliderCastInput input = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Start       = newPosition,
                    End         = newPosition + displacement,
                };
                world.CastCollider(input, ref collector);

                // Iterate over hits and create constraints from them
                for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = collector.AllHits[hitIndex];

                    bool found = false;
                    for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
                    {
                        DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
                        if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
                            dHit.ColliderKey.Equals(hit.ColliderKey))
                        {
                            found = true;
                            break;
                        }
                    }

                    // Skip duplicate hits
                    if (!found)
                    {
                        CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
                                                hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);

                        // Check if max slope plane is required
                        float verticalComponent = math.dot(constraint.Plane.Normal, up);
                        bool  shouldAddPlane    = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos;
                        if (shouldAddPlane)
                        {
                            AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numConstraints);
                        }

                        // Add original constraint to the list
                        constraints[numConstraints++] = constraint;
                    }
                }
            }

            // Solve
            float3 prevVelocity = newVelocity;
            float3 prevPosition = newPosition;
            SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);

            // Apply impulses to hit bodies
            if (affectBodies)
            {
                CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
            }

            float3 newDisplacement = newPosition - prevPosition;

            // Check if we can walk to the position simplex solver has suggested
            MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
            int newContactIndex = -1;

            // If simplex solver moved the character we need to re-cast to make sure it can move to new position
            if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
            {
                float3            displacement = newDisplacement + gravityMovement;
                ColliderCastInput input        = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Start       = prevPosition,
                    End         = prevPosition + displacement
                };

                world.CastCollider(input, ref newCollector);

                for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = newCollector.AllHits[hitIndex];

                    bool found = false;
                    for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
                    {
                        SurfaceConstraintInfo constraint = constraints[constraintIndex];
                        if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
                            constraint.ColliderKey.Equals(hit.ColliderKey))
                        {
                            found = true;
                            break;
                        }
                    }

                    if (!found)
                    {
                        newContactIndex = hitIndex;
                        break;
                    }
                }
            }

            // Move character along the newDisplacement direction until it reaches this new contact
            if (newContactIndex >= 0)
            {
                ColliderCastHit newContact = newCollector.AllHits[newContactIndex];

                Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f);

                integratedTime *= newContact.Fraction;
                newPosition     = prevPosition + newDisplacement * newContact.Fraction;
            }

            remainingTime -= integratedTime;

            // Remember last displacement for next iteration
            lastDisplacement = newVelocity * remainingTime;
        }

        // Write back position and velocity
        transform.pos  = newPosition;
        linearVelocity = newVelocity;
    }
Example #12
0
        public List <Bag.Item> LinearProgramming(List <Item> items)
        {
            List <Bag.Item> choosenItems = new List <Bag.Item>();
            int             itemsCount   = items.Count;
            double          capacity     = MaxWeightAllowed;

            SimplexSolver solver = new SimplexSolver();

            //double[] estimatedProfitOfProjectX = new double[] { 1, 1.8, 1.6, 0.8, 1.4 };
            //double[] capitalRequiredForProjectX = new double[] { 6, 12, 10, 4, 8 };
            //double availableCapital = 20;
            //int[] chooseProjectX = new int[5];

            double[] values  = new double[itemsCount];
            double[] weights = new double[itemsCount];

            for (int i = 0; i < itemsCount; i++)
            {
                values[i]  = items[i].Value;
                weights[i] = items[i].Weight;
            }

            int[] chosenItems = new int[itemsCount];

            //int profit;
            //solver.AddRow("profit", out profit);
            //solver.AddGoal(profit, 0, false);

            int MaximumValue;

            solver.AddRow("MaximumValue", out MaximumValue);
            solver.AddGoal(MaximumValue, 0, false);

            //int expenditure;
            //solver.AddRow("expenditure", out expenditure);
            //solver.SetBounds(expenditure, 0, availableCapital);

            int MaximumWeight;

            solver.AddRow("MaximumWeight", out MaximumWeight);
            solver.SetBounds(MaximumWeight, 0, capacity);

            //for (int i = 0; i < 5; i++)
            //{
            //    solver.AddVariable(string.Format("project{0}", i),
            //                        out chooseProjectX[i]);
            //    solver.SetBounds(chooseProjectX[i], 0, 1);
            //    solver.SetIntegrality(chooseProjectX[i], true);
            //    solver.SetCoefficient(profit, chooseProjectX[i],
            //                           estimatedProfitOfProjectX[i]);
            //    solver.SetCoefficient(expenditure, chooseProjectX[i],
            //                           capitalRequiredForProjectX[i]);
            //}

            for (int i = 0; i < itemsCount; i++)
            {
                solver.AddVariable(string.Format("Item{0}", i),
                                   out chosenItems[i]);
                solver.SetBounds(chosenItems[i], 0, 1);
                solver.SetIntegrality(chosenItems[i], true);
                solver.SetCoefficient(MaximumValue, chosenItems[i],
                                      values[i]);
                solver.SetCoefficient(MaximumWeight, chosenItems[i],
                                      weights[i]);
            }

            //SimplexSolverParams param = new SimplexSolverParams();
            //param.MixedIntegerGenerateCuts = true;
            //solver.Solve(param);

            SimplexSolverParams param = new SimplexSolverParams();

            param.MixedIntegerGenerateCuts = true;
            solver.Solve(param);

            //Console.WriteLine(solver.MipResult);
            //for (int i = 0; i < 5; i++)
            //{
            //    Console.WriteLine("Project {0} is {1} selected.", i,
            //                    solver.GetValue(chooseProjectX[i]) == 1 ? "" : "not ");
            //}
            //Console.WriteLine("The estimated total profit is: ${0} million.",
            //                  (double)solver.GetValue(profit).ToDouble());
            //Console.WriteLine("The total expenditure is: ${0} million.",
            //                   solver.GetValue(expenditure).ToDouble());

            //string result = "";
            for (int i = 0; i < itemsCount; i++)
            {
                if (solver.GetValue(chosenItems[i]) == 1)
                {
                    //result += "Selected Item " + i + " ";
                    choosenItems.Add(items[i]);
                }
            }

            //result += " value "+(double)solver.GetValue(MaximumValue).ToDouble()+ " weight "+ solver.GetValue(MaximumWeight).ToDouble();


            return(choosenItems);
        }
Example #13
0
    public static unsafe void CheckSupport(PhysicsWorld world, float deltaTime, RigidTransform transform,
                                           float3 downwardsDirection, float maxSlope, float contactTolerance, Collider *collider, ref NativeArray <SurfaceConstraintInfo> constraints,
                                           ref NativeArray <DistanceHit> checkSupportHits, out CharacterSupportState characterState)
    {
        // Downwards direction must be normalized
        Assert.IsTrue(Math.IsNormalized(downwardsDirection));

        // "Broad phase"
        MaxHitsCollector <DistanceHit> collector = new MaxHitsCollector <DistanceHit>(contactTolerance, ref checkSupportHits);

        {
            ColliderDistanceInput input = new ColliderDistanceInput()
            {
                MaxDistance = contactTolerance,
                Transform   = transform,
                Collider    = collider
            };
            world.CalculateDistance(input, ref collector);
        }

        // Iterate over hits and create constraints from them
        for (int i = 0; i < collector.NumHits; i++)
        {
            DistanceHit hit = collector.AllHits[i];
            CreateConstraintFromHit(world, float3.zero, deltaTime, hit.RigidBodyIndex, hit.ColliderKey,
                                    hit.Position, hit.SurfaceNormal, hit.Distance, true, out SurfaceConstraintInfo constraint);
            constraints[i] = constraint;
        }

        // Solve downwards
        float3 outVelocity = downwardsDirection;
        float3 outPosition = transform.pos;

        SimplexSolver.Solve(world, deltaTime, -downwardsDirection, collector.NumHits, ref constraints, ref outPosition, ref outVelocity, out float integratedTime);

        // If no hits, proclaim unsupported state
        if (collector.NumHits == 0)
        {
            characterState = CharacterSupportState.Unsupported;
        }
        else
        {
            if (math.lengthsq(downwardsDirection - outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
            {
                // If velocity hasn't changed significantly, declare unsupported state
                characterState = CharacterSupportState.Unsupported;
            }
            else if (math.lengthsq(outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
            {
                // If velocity is very small, declare supported state
                characterState = CharacterSupportState.Supported;
            }
            else
            {
                // Check if sliding or supported
                outVelocity = math.normalize(outVelocity);
                float slopeAngleSin   = math.dot(outVelocity, downwardsDirection);
                float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                float maxSlopeCosine  = math.cos(maxSlope);
                if (slopeAngleCosSq < maxSlopeCosine * maxSlopeCosine)
                {
                    characterState = CharacterSupportState.Sliding;
                }
                else
                {
                    characterState = CharacterSupportState.Supported;
                }
            }
        }
    }
    public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime,
                                                  int maxIterations, float3 up, float3 gravity,
                                                  float characterMass, float tau, float damping, bool affectBodies, Collider *collider,
                                                  ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
                                                  ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
    {
        float  remainingTime    = deltaTime;
        float3 lastDisplacement = linearVelocity * remainingTime;

        float3     newPosition = transform.pos;
        quaternion orientation = transform.rot;
        float3     newVelocity = linearVelocity;

        const float timeEpsilon = 0.000001f;

        for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
        {
            // First do distance query for penetration recovery
            MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits);
            int numConstraints = 0;
            {
                ColliderDistanceInput input = new ColliderDistanceInput()
                {
                    MaxDistance = 0.0f,
                    Transform   = new RigidTransform
                    {
                        pos = newPosition,
                        rot = orientation,
                    },
                    Collider = collider
                };
                world.CalculateDistance(input, ref distanceHitsCollector);

                // Iterate over hits and create constraints from them
                for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
                {
                    DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
                    CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
                                            hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
                    constraints[numConstraints++] = constraint;
                }
            }

            // Then do a collider cast
            {
                float3 displacement = lastDisplacement - up * timeEpsilon;
                float3 endPosition  = newPosition + displacement;
                MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
                ColliderCastInput input = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Position    = newPosition,
                    Direction   = displacement
                };
                world.CastCollider(input, ref collector);

                // Iterate over hits and create constraints from them
                for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = collector.AllHits[hitIndex];

                    bool found = false;
                    for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
                    {
                        DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
                        if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
                            dHit.ColliderKey.Equals(hit.ColliderKey))
                        {
                            found = true;
                            break;
                        }
                    }

                    // Skip duplicate hits
                    if (!found)
                    {
                        CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
                                                hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
                        constraints[numConstraints++] = constraint;
                    }
                }
            }

            // petarm.todo: Add max slope plane to avoid climbing the not allowed slopes

            // Solve
            float3 prevVelocity = newVelocity;
            SimplexSolver.Solve(world, deltaTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);

            remainingTime   -= integratedTime;
            lastDisplacement = newVelocity * remainingTime;

            // Apply impulses to hit bodies
            if (affectBodies)
            {
                ResolveContacts(world, deltaTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
            }
        }

        // Write back position and velocity
        transform.pos  = newPosition;
        linearVelocity = newVelocity;
    }
Example #15
0
    public static unsafe void CheckSupport(
        ref PhysicsWorld world, Collider *collider, CharacterControllerStepInput stepInput, float3 groundProbeVector, RigidTransform transform, float maxSlope,
        ref NativeList <SurfaceConstraintInfo> constraints, ref NativeList <ColliderCastHit> castHits,
        out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity)
    {
        surfaceNormal   = float3.zero;
        surfaceVelocity = float3.zero;

        // Query the world
        var displacement = groundProbeVector;
        SelfFilteringAllHitsCollector <ColliderCastHit> hitCollector = new SelfFilteringAllHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
        ColliderCastInput input = new ColliderCastInput()
        {
            Collider    = collider,
            Orientation = transform.rot,
            Start       = transform.pos,
            End         = transform.pos + displacement
        };

        world.CastCollider(input, ref hitCollector);

        // If no hits, proclaim unsupported state
        if (hitCollector.NumHits == 0)
        {
            characterState = CharacterSupportState.Unsupported;
            return;
        }

        // Downwards direction must be normalized
        float3 downwardsDirection = -stepInput.Up;

        Assert.IsTrue(Unity.Physics.Math.IsNormalized(downwardsDirection));

        float maxSlopeCos = math.cos(maxSlope);

        // Iterate over distance hits and create constraints from them
        for (int i = 0; i < hitCollector.NumHits; i++)
        {
            var hit = hitCollector.AllHits[i];
            CreateConstraint(stepInput.World, stepInput.Up,
                             hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(displacement),
                             stepInput.SkinWidth, maxSlopeCos, ref constraints);
        }

        float3 initialVelocity = groundProbeVector * (1.0f / stepInput.DeltaTime);

        // Solve downwards (don't use min delta time, try to solve full step)
        float3 outVelocity = initialVelocity;
        float3 outPosition = transform.pos;

        SimplexSolver.Solve(stepInput.World, stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed,
                            constraints, ref outPosition, ref outVelocity, out float integratedTime, false);

        // Get info on surface
        {
            int numSupportingPlanes = 0;
            for (int j = 0; j < constraints.Length; j++)
            {
                var constraint = constraints[j];
                if (constraint.Touched && !constraint.IsTooSteep)
                {
                    numSupportingPlanes++;
                    surfaceNormal   += constraint.Plane.Normal;
                    surfaceVelocity += constraint.Velocity;
                }
            }

            if (numSupportingPlanes > 0)
            {
                float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
                surfaceNormal   *= invNumSupportingPlanes;
                surfaceVelocity *= invNumSupportingPlanes;

                surfaceNormal = math.normalize(surfaceNormal);
            }
        }

        // Check support state
        {
            if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
            {
                // If velocity hasn't changed significantly, declare unsupported state
                characterState = CharacterSupportState.Unsupported;
            }
            else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq)
            {
                // If velocity is very small, declare supported state
                characterState = CharacterSupportState.Supported;
            }
            else
            {
                // Check if sliding or supported
                outVelocity = math.normalize(outVelocity);
                float slopeAngleSin   = math.max(0.0f, math.dot(outVelocity, downwardsDirection));
                float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                if (slopeAngleCosSq < maxSlopeCos * maxSlopeCos)
                {
                    characterState = CharacterSupportState.Sliding;
                }
                else
                {
                    characterState = CharacterSupportState.Supported;
                }
            }
        }
    }
    public static unsafe void CollideAndIntegrate(
        CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
        ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints, int numConstraints,
        ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
    {
        // Copy parameters
        float        deltaTime = stepInput.DeltaTime;
        float3       gravity   = stepInput.Gravity;
        float3       up        = stepInput.Up;
        PhysicsWorld world     = stepInput.World;

        float  remainingTime    = deltaTime;
        float3 lastDisplacement = linearVelocity * remainingTime;

        float3     newPosition = transform.pos;
        quaternion orientation = transform.rot;
        float3     newVelocity = linearVelocity;

        float maxSlopeCos = math.cos(stepInput.MaxSlope);

        const float timeEpsilon = 0.000001f;

        for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
        {
            float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;

            // Then do a collider cast (but not in first iteration)
            if (i > 0)
            {
                int numCastConstraints = 0;

                float3 displacement = lastDisplacement + gravityMovement;
                MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
                ColliderCastInput input = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Start       = newPosition,
                    End         = newPosition + displacement * (1.0f + stepInput.ContactTolerance),
                };
                world.CastCollider(input, ref collector);

                // Iterate over hits and create constraints from them
                for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = collector.AllHits[hitIndex];
                    CreateConstraint(stepInput.World, stepInput.Up,
                                     hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(lastDisplacement),
                                     stepInput.SkinWidth, maxSlopeCos, ref constraints, ref numCastConstraints);
                }

                numConstraints = numCastConstraints;
            }

            // Min delta time for solver to break
            float minDeltaTime = 0.0f;
            if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq)
            {
                // Min delta time to travel at least 1cm
                minDeltaTime = 0.01f / math.length(newVelocity);
            }

            // Solve
            float3 prevVelocity = newVelocity;
            float3 prevPosition = newPosition;
            SimplexSolver.Solve(world, remainingTime, minDeltaTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);

            // Apply impulses to hit bodies
            if (affectBodies)
            {
                CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
            }

            float3 newDisplacement = newPosition - prevPosition;

            // Check if we can walk to the position simplex solver has suggested
            MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
            int newContactIndex = -1;

            // If simplex solver moved the character we need to re-cast to make sure it can move to new position
            if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon)
            {
                float3            displacement = newDisplacement + gravityMovement;
                ColliderCastInput input        = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Start       = prevPosition,
                    End         = prevPosition + displacement * (1.0f + stepInput.ContactTolerance)
                };

                world.CastCollider(input, ref newCollector);
                float minFraction = float.MaxValue;

                for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = newCollector.AllHits[hitIndex];
                    if (hit.Fraction < minFraction)
                    {
                        bool found = false;
                        for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
                        {
                            SurfaceConstraintInfo constraint = constraints[constraintIndex];
                            if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
                                constraint.ColliderKey.Equals(hit.ColliderKey))
                            {
                                found = true;
                                break;
                            }
                        }

                        if (!found)
                        {
                            minFraction     = hit.Fraction;
                            newContactIndex = hitIndex;
                        }
                    }
                }
            }

            // Move character along the newDisplacement direction until it reaches this new contact
            if (newContactIndex >= 0)
            {
                ColliderCastHit newContact = newCollector.AllHits[newContactIndex];

                Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f);

                integratedTime *= newContact.Fraction;
                newPosition     = prevPosition + newDisplacement * newContact.Fraction;
            }

            remainingTime -= integratedTime;

            // Remember last displacement for next iteration
            lastDisplacement = newVelocity * remainingTime;
        }

        // Write back position and velocity
        transform.pos  = newPosition;
        linearVelocity = newVelocity;
    }
    public static unsafe void CheckSupport(CharacterControllerStepInput stepInput,
                                           RigidTransform transform, float maxSlope, MaxHitsCollector <DistanceHit> distanceHitsCollector,
                                           ref NativeArray <SurfaceConstraintInfo> constraints, out int numConstraints, out CharacterSupportState characterState,
                                           out float3 surfaceNormal, out float3 surfaceVelocity)
    {
        surfaceNormal   = float3.zero;
        surfaceVelocity = float3.zero;

        // If no hits, proclaim unsupported state
        if (distanceHitsCollector.NumHits == 0)
        {
            characterState = CharacterSupportState.Unsupported;
            numConstraints = 0;
            return;
        }

        // Downwards direction must be normalized
        float3 downwardsDirection = -stepInput.Up;

        Assert.IsTrue(Unity.Physics.Math.IsNormalized(downwardsDirection));

        float maxSlopeCos = math.cos(maxSlope);

        // Iterate over distance hits and create constraints from them
        numConstraints = 0;
        for (int i = 0; i < distanceHitsCollector.NumHits; i++)
        {
            DistanceHit hit = distanceHitsCollector.AllHits[i];
            CreateConstraint(stepInput.World, stepInput.Up,
                             hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
                             stepInput.SkinWidth, maxSlopeCos, ref constraints, ref numConstraints);
        }

        float3 initialVelocity;
        {
            float velAlongDownwardsDir = math.dot(stepInput.CurrentVelocity, downwardsDirection);
            bool  velocityIsAlongDownwardsDirection = velAlongDownwardsDir > 0.0f;
            if (velocityIsAlongDownwardsDirection)
            {
                float3 downwardsVelocity = velAlongDownwardsDir * downwardsDirection;
                initialVelocity =
                    math.select(downwardsVelocity, downwardsDirection, math.abs(velAlongDownwardsDir) > 1.0f) +
                    stepInput.Gravity * stepInput.DeltaTime;
            }
            else
            {
                initialVelocity = downwardsDirection;
            }
        }

        // Solve downwards (don't use min delta time, try to solve full step)
        float3 outVelocity = initialVelocity;
        float3 outPosition = transform.pos;

        SimplexSolver.Solve(stepInput.World, stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, numConstraints,
                            ref constraints, ref outPosition, ref outVelocity, out float integratedTime, false);

        // Reset touched state of constraints and get info on surface
        {
            int numSupportingPlanes = 0;
            for (int j = 0; j < numConstraints; j++)
            {
                var constraint = constraints[j];
                if (constraint.Touched)
                {
                    numSupportingPlanes++;
                    surfaceNormal   += constraint.Plane.Normal;
                    surfaceVelocity += constraint.Velocity;

                    constraint.Touched = false;
                    constraints[j]     = constraint;
                }
            }

            if (numSupportingPlanes > 0)
            {
                float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
                surfaceNormal   *= invNumSupportingPlanes;
                surfaceVelocity *= invNumSupportingPlanes;

                surfaceNormal = math.normalize(surfaceNormal);
            }
        }

        // Check support state
        {
            if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
            {
                // If velocity hasn't changed significantly, declare unsupported state
                characterState = CharacterSupportState.Unsupported;
            }
            else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq)
            {
                // If velocity is very small, declare supported state
                characterState = CharacterSupportState.Supported;
            }
            else
            {
                // Check if sliding or supported
                outVelocity = math.normalize(outVelocity);
                float slopeAngleSin   = math.max(0.0f, math.dot(outVelocity, downwardsDirection) - k_SimplexSolverEpsilon);
                float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                if (slopeAngleCosSq < maxSlopeCos * maxSlopeCos)
                {
                    characterState = CharacterSupportState.Sliding;
                }
                else
                {
                    characterState = CharacterSupportState.Supported;
                }
            }
        }
    }
Example #18
0
        public static unsafe void CollideAndIntegrate(
            CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
            ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter)
        {
            // Copy parameters
            var deltaTime = stepInput.DeltaTime;
            var up        = stepInput.Up;
            var world     = stepInput.World;

            var remainingTime = deltaTime;

            var newPosition = transform.pos;
            var orientation = transform.rot;
            var newVelocity = linearVelocity;

            var maxSlopeCos = math.cos(stepInput.MaxSlope);

            const float timeEpsilon = 0.000001f;

            for (var i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
            {
                var constraints = new NativeList <SurfaceConstraintInfo>(KDefaultConstraintsCapacity, Allocator.Temp);

                // Do a collider cast
                {
                    var displacement = newVelocity * remainingTime;
                    var castHits     = new NativeList <ColliderCastHit>(KDefaultQueryHitsCapacity, Allocator.Temp);
                    var collector    =
                        new SelfFilteringAllHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f,
                                                                            ref castHits);
                    var input = new ColliderCastInput
                    {
                        Collider    = collider,
                        Orientation = orientation,
                        Start       = newPosition,
                        End         = newPosition + displacement
                    };
                    world.CastCollider(input, ref collector);

                    // Iterate over hits and create constraints from them
                    for (var hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
                    {
                        var hit = collector.AllHits[hitIndex];
                        CreateConstraint(stepInput.World, stepInput.Up,
                                         hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
                                         hit.Fraction * math.length(displacement),
                                         stepInput.SkinWidth, maxSlopeCos, ref constraints);
                    }
                }

                // Then do a collider distance for penetration recovery,
                // but only fix up penetrating hits
                {
                    // Collider distance query
                    var distanceHits          = new NativeList <DistanceHit>(KDefaultQueryHitsCapacity, Allocator.Temp);
                    var distanceHitsCollector = new SelfFilteringAllHitsCollector <DistanceHit>(
                        stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits);
                    {
                        var input = new ColliderDistanceInput
                        {
                            MaxDistance = stepInput.ContactTolerance,
                            Transform   = transform,
                            Collider    = collider
                        };
                        world.CalculateDistance(input, ref distanceHitsCollector);
                    }

                    // Iterate over penetrating hits and fix up distance and normal
                    var numConstraints = constraints.Length;
                    for (var hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
                    {
                        var hit = distanceHitsCollector.AllHits[hitIndex];
                        if (hit.Distance < stepInput.SkinWidth)
                        {
                            var found = false;

                            // Iterate backwards to locate the original constraint before the max slope constraint
                            for (var constraintIndex = numConstraints - 1; constraintIndex >= 0; constraintIndex--)
                            {
                                var constraint = constraints[constraintIndex];
                                if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
                                    constraint.ColliderKey.Equals(hit.ColliderKey))
                                {
                                    // Fix up the constraint (normal, distance)
                                    {
                                        // Create new constraint
                                        CreateConstraintFromHit(world, hit.RigidBodyIndex, hit.ColliderKey,
                                                                hit.Position, hit.SurfaceNormal, hit.Distance,
                                                                stepInput.SkinWidth, out var newConstraint);

                                        // Resolve its penetration
                                        ResolveConstraintPenetration(ref newConstraint);

                                        // Write back
                                        constraints[constraintIndex] = newConstraint;
                                    }

                                    found = true;
                                    break;
                                }
                            }

                            // Add penetrating hit not caught by collider cast
                            if (!found)
                            {
                                CreateConstraint(stepInput.World, stepInput.Up,
                                                 hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
                                                 stepInput.SkinWidth, maxSlopeCos, ref constraints);
                            }
                        }
                    }
                }

                // Min delta time for solver to break
                var minDeltaTime = 0.0f;
                if (math.lengthsq(newVelocity) > KSimplexSolverEpsilonSq)
                {
                    // Min delta time to travel at least 1cm
                    minDeltaTime = 0.01f / math.length(newVelocity);
                }

                // Solve
                var prevVelocity = newVelocity;
                var prevPosition = newPosition;
                // FSLog.Info($"begin newVelocity£º{newVelocity} ");
                SimplexSolver.Solve(remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints,
                                    ref newPosition, ref newVelocity, out var integratedTime);
                //  FSLog.Info($"constraints.Length£º{constraints.Length} ");
                // Apply impulses to hit bodies
                if (affectBodies)
                {
                    CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, ref constraints,
                                                      ref deferredImpulseWriter);
                }

                // Calculate new displacement
                var newDisplacement = newPosition - prevPosition;

                // If simplex solver moved the character we need to re-cast to make sure it can move to new position
                if (math.lengthsq(newDisplacement) > KSimplexSolverEpsilon)
                {
                    // Check if we can walk to the position simplex solver has suggested
                    var newCollector =
                        new SelfFilteringClosestHitCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f);

                    var input = new ColliderCastInput
                    {
                        Collider    = collider,
                        Orientation = orientation,
                        Start       = prevPosition,
                        End         = prevPosition + newDisplacement
                    };

                    world.CastCollider(input, ref newCollector);

                    if (newCollector.NumHits > 0)
                    {
                        var hit = newCollector.ClosestHit;

                        var found = false;
                        for (var constraintIndex = 0; constraintIndex < constraints.Length; constraintIndex++)
                        {
                            var constraint = constraints[constraintIndex];
                            if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
                                constraint.ColliderKey.Equals(hit.ColliderKey))
                            {
                                found = true;
                                break;
                            }
                        }

                        // Move character along the newDisplacement direction until it reaches this new contact
                        if (!found)
                        {
                            Assert.IsTrue(hit.Fraction >= 0.0f && hit.Fraction <= 1.0f);

                            integratedTime *= hit.Fraction;
                            newPosition     = prevPosition + newDisplacement * hit.Fraction;
                        }
                    }
                }

                // Reduce remaining time
                remainingTime -= integratedTime;
            }

            // Write back position and velocity
            transform.pos  = newPosition;
            linearVelocity = newVelocity;
        }
Example #19
0
        public static RecipeGraph FromLibrary(Library library, IEnumerable <Item> inputs, IEnumerable <ItemAmount> outputs, Func <Item, double> costFunction)
        {
            if (library == null)
            {
                throw new ArgumentNullException("library");
            }
            if (inputs == null)
            {
                throw new ArgumentNullException("inputs");
            }
            if (outputs == null)
            {
                throw new ArgumentNullException("outputs");
            }
            if (costFunction == null)
            {
                throw new ArgumentNullException("costFunction");
            }

            var solver     = new SimplexSolver();
            var itemRows   = new Dictionary <Item, int>();
            var recipeVars = new Dictionary <Recipe, int>();
            var wasteGoals = new Dictionary <Item, ILinearGoal>();

            foreach (var item in library.Items)
            {
                int id;
                solver.AddRow(item, out id);
                itemRows.Add(item, id);
                solver.SetBounds(id, 0, Rational.PositiveInfinity);
                if (!inputs.Contains(item))
                {
                    wasteGoals.Add(item, solver.AddGoal(id, 1, true));
                }
            }

            // Bound output to requested values
            foreach (var itemAmount in outputs)
            {
                var item   = itemAmount.Item;
                var amount = itemAmount.Amount;
                solver.SetBounds(itemRows[item], amount, amount);
            }

            foreach (var recipe in library.Recipes)
            {
                int id;
                solver.AddVariable(recipe, out id);
                recipeVars.Add(recipe, id);
                solver.SetBounds(id, 0, Rational.PositiveInfinity);

                foreach (var input in recipe.Ingredients)
                {
                    solver.SetCoefficient(itemRows[input.Item], id, -input.Amount);
                }
                foreach (var output in recipe.Results)
                {
                    solver.SetCoefficient(itemRows[output.Item], id, output.Amount);
                }
            }

            // Add input minimize goals
            foreach (var item in inputs)
            {
                var row = itemRows[item];
                solver.SetBounds(row, Rational.NegativeInfinity, 0);
                solver.AddGoal(row, (int)(10000 * costFunction(item)), false);
            }

            solver.Solve(new SimplexSolverParams());

            if (solver.SolutionQuality != Microsoft.SolverFoundation.Services.LinearSolutionQuality.Exact)
            {
                throw new InvalidOperationException("Cannot solve problem");
            }

            List <Tuple <Recipe, double> > usedRecipes = new List <Tuple <Recipe, double> >();

            foreach (var recipe in library.Recipes)
            {
                var value = (double)solver.GetValue(recipeVars[recipe]);
                if (value > 0)
                {
                    usedRecipes.Add(new Tuple <Recipe, double>(recipe, value));
                }
            }

            var sortedRecipes = usedRecipes.SortTopological((a, b) => a.Item1.Results.Select((i) => i.Item).Intersect(b.Item1.Ingredients.Select((i) => i.Item)).Any(), true);
            List <SourceStep>           inputSteps     = new List <SourceStep>();
            List <SinkStep>             outputSteps    = new List <SinkStep>();
            List <SinkStep>             wasteSteps     = new List <SinkStep>();
            Dictionary <Item, FlowStep> itemSteps      = new Dictionary <Item, FlowStep>();
            List <FlowStep>             flowSteps      = new List <FlowStep>();
            List <TransformStep>        transformSteps = new List <TransformStep>();

            foreach (var item in library.Items)
            {
                var value = (double)solver.GetValue(itemRows[item]);
                if (value > 0)
                {
                    var sink = new SinkStep(new ItemAmount(item, value / 2));
                    if (outputs.Select((o) => o.Item).Contains(item))
                    {
                        outputSteps.Add(sink);
                    }
                    else
                    {
                        wasteSteps.Add(sink);
                    }
                    itemSteps.Add(item, sink);
                }
                else if (value < 0)
                {
                    var source = new SourceStep(new ItemAmount(item, -value));
                    inputSteps.Add(source);
                    itemSteps.Add(item, source);
                }
            }

            foreach (var recipe in sortedRecipes)
            {
                foreach (var result in recipe.Item1.Results)
                {
                    var item = result.Item;
                    if (!itemSteps.ContainsKey(item))
                    {
                        var flowstep = new FlowStep(new ItemAmount(item, 0));
                        itemSteps.Add(item, flowstep);
                        flowSteps.Add(flowstep);
                    }
                }
            }

            foreach (var recipe in sortedRecipes)
            {
                var previous = recipe.Item1.Ingredients.Select((i) => i.Item);
                var step     = new TransformStep(recipe.Item1, recipe.Item2);
                foreach (var item in previous)
                {
                    var prev = itemSteps[item];
                    step.Previous.Add(prev);
                }

                foreach (var amount in recipe.Item1.Results)
                {
                    var item = amount.Item;

                    itemSteps[item].Previous.Add(step);
                    itemSteps[item].Item += amount * recipe.Item2;
                }
                transformSteps.Add(step);
            }

            return(new RecipeGraph(wasteSteps, inputSteps, outputSteps, flowSteps, transformSteps));
        }
Example #20
0
    public static unsafe void CheckSupport(
        ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform,
        out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity,
        NativeList <StatefulCollisionEvent> collisionEvents = default)
    {
        surfaceNormal   = float3.zero;
        surfaceVelocity = float3.zero;

        // Up direction must be normalized
        Assert.IsTrue(Unity.Physics.Math.IsNormalized(stepInput.Up));

        // Query the world
        NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
        CharacterControllerAllHitsCollector <ColliderCastHit> castHitsCollector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
            stepInput.RigidBodyIndex, 1.0f, ref castHits, world);
        var maxDisplacement = -stepInput.ContactTolerance * stepInput.Up;

        {
            ColliderCastInput input = new ColliderCastInput()
            {
                Collider    = collider.ColliderPtr,
                Orientation = transform.rot,
                Start       = transform.pos,
                End         = transform.pos + maxDisplacement
            };

            world.CastCollider(input, ref castHitsCollector);
        }

        // If no hits, proclaim unsupported state
        if (castHitsCollector.NumHits == 0)
        {
            characterState = CharacterSupportState.Unsupported;
            return;
        }

        float maxSlopeCos = math.cos(stepInput.MaxSlope);

        // Iterate over distance hits and create constraints from them
        NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
        float maxDisplacementLength = math.length(maxDisplacement);

        for (int i = 0; i < castHitsCollector.NumHits; i++)
        {
            ColliderCastHit hit = castHitsCollector.AllHits[i];
            CreateConstraint(stepInput.World, stepInput.Up,
                             hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * maxDisplacementLength,
                             stepInput.SkinWidth, maxSlopeCos, ref constraints);
        }

        // Velocity for support checking
        float3 initialVelocity = maxDisplacement / stepInput.DeltaTime;

        // Solve downwards (don't use min delta time, try to solve full step)
        float3 outVelocity = initialVelocity;
        float3 outPosition = transform.pos;

        SimplexSolver.Solve(stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed,
                            constraints, ref outPosition, ref outVelocity, out float integratedTime, false);

        // Get info on surface
        int numSupportingPlanes = 0;
        {
            for (int j = 0; j < constraints.Length; j++)
            {
                var constraint = constraints[j];
                if (constraint.Touched && !constraint.IsTooSteep && !constraint.IsMaxSlope)
                {
                    numSupportingPlanes++;
                    surfaceNormal   += constraint.Plane.Normal;
                    surfaceVelocity += constraint.Velocity;

                    // Add supporting planes to collision events
                    if (collisionEvents.IsCreated)
                    {
                        var collisionEvent = new StatefulCollisionEvent(stepInput.World.Bodies[stepInput.RigidBodyIndex].Entity,
                                                                        stepInput.World.Bodies[constraint.RigidBodyIndex].Entity, stepInput.RigidBodyIndex, constraint.RigidBodyIndex,
                                                                        ColliderKey.Empty, constraint.ColliderKey, constraint.Plane.Normal);
                        collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(1, 0, constraint.HitPosition);
                        collisionEvents.Add(collisionEvent);
                    }
                }
            }

            if (numSupportingPlanes > 0)
            {
                float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
                surfaceNormal   *= invNumSupportingPlanes;
                surfaceVelocity *= invNumSupportingPlanes;

                surfaceNormal = math.normalize(surfaceNormal);
            }
        }

        // Check support state
        {
            if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
            {
                // If velocity hasn't changed significantly, declare unsupported state
                characterState = CharacterSupportState.Unsupported;
            }
            else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq && numSupportingPlanes > 0)
            {
                // If velocity is very small, declare supported state
                characterState = CharacterSupportState.Supported;
            }
            else
            {
                // Check if sliding
                outVelocity = math.normalize(outVelocity);
                float slopeAngleSin   = math.max(0.0f, math.dot(outVelocity, -stepInput.Up));
                float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                if (slopeAngleCosSq <= maxSlopeCos * maxSlopeCos)
                {
                    characterState = CharacterSupportState.Sliding;
                }
                else if (numSupportingPlanes > 0)
                {
                    characterState = CharacterSupportState.Supported;
                }
                else
                {
                    // If numSupportingPlanes is 0, surface normal is invalid, so state is unsupported
                    characterState = CharacterSupportState.Unsupported;
                }
            }
        }
    }
Example #21
0
        public double[] AccelDirection(Vector2 dir, bool allowOrtho, bool allowRotate, float moveWeight, float rotateWeight)
        {
            dir = dir.normalized;
            int numVars = thrusters.Count;
            int numEqs  = thrusters.Count; // each thruster < 1

            if (!allowOrtho)
            {
                numEqs += 2;              // 0 < l,r < 0
            }
            if (!allowRotate)
            {
                numEqs += 2;               // 0 < torque l,r < 0
            }
            float[,] lhs = new float[numEqs, numVars];
            float[] rhs       = new float[numEqs];
            float[] objective = new float[numVars];

            int eq = 0;

            // Each thruster activation is below 1
            for (int i = 0; i < thrusters.Count; i++)
            {
                lhs[eq, i] = 1;
                rhs[eq]    = 1;
                eq        += 1;
            }

            if (!allowOrtho)
            {
                // Orthogonal force is 0
                for (int i = 0; i < thrusters.Count; i++)
                {
                    lhs[eq, i]     = PerpDot(thrusters[i].transform.up, dir) * thrusters[i].force; // r < 0
                    lhs[eq + 1, i] = -lhs[eq, i];                                                  // l < 0
                }
                rhs[eq]     = E;
                rhs[eq + 1] = E;
                eq         += 2;
            }

            if (!allowRotate)
            {
                // Torque is 0
                for (int i = 0; i < thrusters.Count; i++)
                {
                    lhs[eq, i]     = GetTorque(thrusters[i]); // r < 0
                    lhs[eq + 1, i] = -lhs[eq, i];             // l < 0
                }
                rhs[eq]     = rotationTolerance;
                rhs[eq + 1] = rotationTolerance;
                eq         += 2;
            }

            // Set objective (maximum parallel force)

            //print("---");
            for (int i = 0; i < thrusters.Count; i++)
            {
                objective[i] = 0;
                float movement = Vector2.Dot(thrusters[i].transform.up, dir) * thrusters[i].force * moveWeight;
                //Debug.Log(i + " " + movement);
                if (Mathf.Abs(movement) > 0.1f)
                {
                    objective[i] += movement;
                }
                float torque = GetTorque(thrusters[i]) * rotateWeight;
                if (Mathf.Abs(torque) > 0.1f)
                {
                    objective[i] += torque;
                }
            }

            SimplexSolver solver = new SimplexSolver(lhs, rhs, objective);

            double[] result = solver.Solve();
            return(result);
        }
Example #22
0
    public static unsafe void CollideAndIntegrate(
        CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Unity.Physics.Collider *collider,
        ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter,
        NativeList <StatefulCollisionEvent> collisionEvents = default, NativeList <StatefulTriggerEvent> triggerEvents = default)
    {
        // Copy parameters
        float        deltaTime = stepInput.DeltaTime;
        float3       up        = stepInput.Up;
        PhysicsWorld world     = stepInput.World;

        float remainingTime = deltaTime;

        float3     newPosition = transform.pos;
        quaternion orientation = transform.rot;
        float3     newVelocity = linearVelocity;

        float maxSlopeCos = math.cos(stepInput.MaxSlope);

        const float timeEpsilon = 0.000001f;

        for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
        {
            NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);

            // Do a collider cast
            {
                float3 displacement = newVelocity * remainingTime;
                NativeList <ColliderCastHit> triggerHits = default;
                if (triggerEvents.IsCreated)
                {
                    triggerHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity / 4, Allocator.Temp);
                }
                NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
                CharacterControllerAllHitsCollector <ColliderCastHit> collector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
                    stepInput.RigidBodyIndex, 1.0f, ref castHits, world, triggerHits);
                ColliderCastInput input = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Start       = newPosition,
                    End         = newPosition + displacement
                };
                world.CastCollider(input, ref collector);

                // Iterate over hits and create constraints from them
                for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = collector.AllHits[hitIndex];
                    CreateConstraint(stepInput.World, stepInput.Up,
                                     hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, math.dot(-hit.SurfaceNormal, hit.Fraction * displacement),
                                     stepInput.SkinWidth, maxSlopeCos, ref constraints);
                }

                // Update trigger events
                if (triggerEvents.IsCreated)
                {
                    UpdateTriggersSeen(stepInput, triggerHits, triggerEvents, collector.MinHitFraction);
                }
            }

            // Then do a collider distance for penetration recovery,
            // but only fix up penetrating hits
            {
                // Collider distance query
                NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
                CharacterControllerAllHitsCollector <DistanceHit> distanceHitsCollector = new CharacterControllerAllHitsCollector <DistanceHit>(
                    stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits, world);
                {
                    ColliderDistanceInput input = new ColliderDistanceInput()
                    {
                        MaxDistance = stepInput.ContactTolerance,
                        Transform   = transform,
                        Collider    = collider
                    };
                    world.CalculateDistance(input, ref distanceHitsCollector);
                }

                // Iterate over penetrating hits and fix up distance and normal
                int numConstraints = constraints.Length;
                for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
                {
                    DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
                    if (hit.Distance < stepInput.SkinWidth)
                    {
                        bool found = false;

                        // Iterate backwards to locate the original constraint before the max slope constraint
                        for (int constraintIndex = numConstraints - 1; constraintIndex >= 0; constraintIndex--)
                        {
                            SurfaceConstraintInfo constraint = constraints[constraintIndex];
                            if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
                                constraint.ColliderKey.Equals(hit.ColliderKey))
                            {
                                // Fix up the constraint (normal, distance)
                                {
                                    // Create new constraint
                                    CreateConstraintFromHit(world, hit.RigidBodyIndex, hit.ColliderKey,
                                                            hit.Position, hit.SurfaceNormal, hit.Distance,
                                                            stepInput.SkinWidth, out SurfaceConstraintInfo newConstraint);

                                    // Resolve its penetration
                                    ResolveConstraintPenetration(ref newConstraint);

                                    // Write back
                                    constraints[constraintIndex] = newConstraint;
                                }

                                found = true;
                                break;
                            }
                        }

                        // Add penetrating hit not caught by collider cast
                        if (!found)
                        {
                            CreateConstraint(stepInput.World, stepInput.Up,
                                             hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
                                             stepInput.SkinWidth, maxSlopeCos, ref constraints);
                        }
                    }
                }
            }

            // Min delta time for solver to break
            float minDeltaTime = 0.0f;
            if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq)
            {
                // Min delta time to travel at least 1cm
                minDeltaTime = 0.01f / math.length(newVelocity);
            }

            // Solve
            float3 prevVelocity = newVelocity;
            float3 prevPosition = newPosition;
            SimplexSolver.Solve(remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints, ref newPosition, ref newVelocity, out float integratedTime);

            // Apply impulses to hit bodies and store collision events
            if (affectBodies || collisionEvents.IsCreated)
            {
                CalculateAndStoreDeferredImpulsesAndCollisionEvents(stepInput, affectBodies, characterMass,
                                                                    prevVelocity, constraints, ref deferredImpulseWriter, collisionEvents);
            }

            // Calculate new displacement
            float3 newDisplacement = newPosition - prevPosition;

            // If simplex solver moved the character we need to re-cast to make sure it can move to new position
            if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon)
            {
                // Check if we can walk to the position simplex solver has suggested
                var newCollector = new CharacterControllerClosestHitCollector <ColliderCastHit>(constraints, world, stepInput.RigidBodyIndex, 1.0f);

                ColliderCastInput input = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Start       = prevPosition,
                    End         = prevPosition + newDisplacement
                };

                world.CastCollider(input, ref newCollector);

                if (newCollector.NumHits > 0)
                {
                    ColliderCastHit hit = newCollector.ClosestHit;

                    // Move character along the newDisplacement direction until it reaches this new contact
                    {
                        Assert.IsTrue(hit.Fraction >= 0.0f && hit.Fraction <= 1.0f);

                        integratedTime *= hit.Fraction;
                        newPosition     = prevPosition + newDisplacement * hit.Fraction;
                    }
                }
            }

            // Reduce remaining time
            remainingTime -= integratedTime;

            // Write back position so that the distance query will update results
            transform.pos = newPosition;
        }

        // Write back final velocity
        linearVelocity = newVelocity;
    }
    public static unsafe void CheckSupport(
        ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform, float maxSlope,
        out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity)
    {
        surfaceNormal   = float3.zero;
        surfaceVelocity = float3.zero;

        // Query the world
        NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
        SelfFilteringAllHitsCollector <DistanceHit> distanceHitsCollector = new SelfFilteringAllHitsCollector <DistanceHit>(
            stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits);

        {
            ColliderDistanceInput input = new ColliderDistanceInput()
            {
                MaxDistance = stepInput.ContactTolerance,
                Transform   = transform,
                Collider    = collider.ColliderPtr
            };
            world.CalculateDistance(input, ref distanceHitsCollector);
        }

        // If no hits, proclaim unsupported state
        if (distanceHitsCollector.NumHits == 0)
        {
            characterState = CharacterSupportState.Unsupported;
            return;
        }

        // Downwards direction must be normalized
        float3 downwardsDirection = -stepInput.Up;

        Assert.IsTrue(Math.IsNormalized(downwardsDirection));

        float maxSlopeCos = math.cos(maxSlope);

        // Iterate over distance hits and create constraints from them
        NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);

        for (int i = 0; i < distanceHitsCollector.NumHits; i++)
        {
            DistanceHit hit = distanceHitsCollector.AllHits[i];
            if (ColliderUtils.IsTrigger(world.Bodies[hit.RigidBodyIndex].Collider, hit.ColliderKey))
            {
                continue;
            }
            CreateConstraint(stepInput.World, stepInput.Up,
                             hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
                             stepInput.SkinWidth, maxSlopeCos, ref constraints);
        }

        float3 initialVelocity;
        {
            float velAlongDownwardsDir = math.dot(stepInput.CurrentVelocity, downwardsDirection);
            bool  velocityIsAlongDownwardsDirection = velAlongDownwardsDir > 0.0f;
            if (velocityIsAlongDownwardsDirection)
            {
                float3 downwardsVelocity = velAlongDownwardsDir * downwardsDirection;
                initialVelocity =
                    math.select(downwardsVelocity, downwardsDirection, math.abs(velAlongDownwardsDir) > 1.0f) +
                    stepInput.Gravity * stepInput.DeltaTime;
            }
            else
            {
                initialVelocity = downwardsDirection;
            }
        }

        // Solve downwards (don't use min delta time, try to solve full step)
        float3 outVelocity = initialVelocity;
        float3 outPosition = transform.pos;

        SimplexSolver.Solve(stepInput.World, stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed,
                            constraints, ref outPosition, ref outVelocity, out float integratedTime, false);

        // Get info on surface
        {
            int numSupportingPlanes = 0;
            for (int j = 0; j < constraints.Length; j++)
            {
                var constraint = constraints[j];
                if (constraint.Touched && !constraint.IsTooSteep)
                {
                    numSupportingPlanes++;
                    surfaceNormal   += constraint.Plane.Normal;
                    surfaceVelocity += constraint.Velocity;
                }
            }

            if (numSupportingPlanes > 0)
            {
                float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
                surfaceNormal   *= invNumSupportingPlanes;
                surfaceVelocity *= invNumSupportingPlanes;

                surfaceNormal = math.normalize(surfaceNormal);
            }
        }

        // Check support state
        {
            if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
            {
                // If velocity hasn't changed significantly, declare unsupported state
                characterState = CharacterSupportState.Unsupported;
            }
            else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq)
            {
                // If velocity is very small, declare supported state
                characterState = CharacterSupportState.Supported;
            }
            else
            {
                // Check if sliding or supported
                outVelocity = math.normalize(outVelocity);
                float slopeAngleSin   = math.max(0.0f, math.dot(outVelocity, downwardsDirection));
                float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                if (slopeAngleCosSq < maxSlopeCos * maxSlopeCos)
                {
                    characterState = CharacterSupportState.Sliding;
                }
                else
                {
                    characterState = CharacterSupportState.Supported;
                }
            }
        }
    }
Example #24
0
    public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime,
                                                  int maxIterations, float3 up, float3 gravity,
                                                  float characterMass, float tau, float damping, float maxSlope, bool affectBodies, Collider *collider,
                                                  ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
                                                  ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
    {
        float  remainingTime    = deltaTime;
        float3 lastDisplacement = linearVelocity * remainingTime;

        float3     newPosition = transform.pos;
        quaternion orientation = transform.rot;
        float3     newVelocity = linearVelocity;

        float maxSlopeCos = math.cos(maxSlope);

        const float timeEpsilon = 0.000001f;

        for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
        {
            // First do distance query for penetration recovery
            MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits);
            int numConstraints = 0;
            {
                ColliderDistanceInput input = new ColliderDistanceInput()
                {
                    MaxDistance = 0.0f,
                    Transform   = new RigidTransform
                    {
                        pos = newPosition,
                        rot = orientation,
                    },
                    Collider = collider
                };
                world.CalculateDistance(input, ref distanceHitsCollector);

                // Iterate over hits and create constraints from them
                for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
                {
                    DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
                    CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
                                            hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);

                    // Potentially add a max slope constraint
                    AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints);

                    // Add original constraint to the list
                    constraints[numConstraints++] = constraint;
                }
            }

            float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;

            // Then do a collider cast
            {
                float3 displacement = lastDisplacement + gravityMovement;
                float3 endPosition  = newPosition + displacement;
                MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
                ColliderCastInput input = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Position    = newPosition,
                    Direction   = displacement
                };
                world.CastCollider(input, ref collector);

                // Iterate over hits and create constraints from them
                for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = collector.AllHits[hitIndex];

                    bool found = false;
                    for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
                    {
                        DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
                        if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
                            dHit.ColliderKey.Equals(hit.ColliderKey))
                        {
                            found = true;
                            break;
                        }
                    }

                    // Skip duplicate hits
                    if (!found)
                    {
                        CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
                                                hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);

                        // Potentially add a max slope constraint
                        AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints);

                        // Add original constraint to the list
                        constraints[numConstraints++] = constraint;
                    }
                }
            }

            // Solve
            float3 prevVelocity = newVelocity;
            float3 prevPosition = newPosition;
            SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);

            // Apply impulses to hit bodies
            if (affectBodies)
            {
                ResolveContacts(world, remainingTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
            }

            float3 newDisplacement = newPosition - prevPosition;

            // Check if we can walk to the position simplex solver has suggested
            MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
            int newContactIndex = -1;

            // If simplex solver moved the character we need to re-cast to make sure it can move to new position
            if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
            {
                float3            displacement = newDisplacement + gravityMovement;
                float3            endPosition  = prevPosition + displacement;
                ColliderCastInput input        = new ColliderCastInput()
                {
                    Collider    = collider,
                    Orientation = orientation,
                    Position    = prevPosition,
                    Direction   = displacement
                };

                world.CastCollider(input, ref newCollector);

                for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
                {
                    ColliderCastHit hit = newCollector.AllHits[hitIndex];

                    bool found = false;
                    for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
                    {
                        SurfaceConstraintInfo constraint = constraints[constraintIndex];
                        if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
                            constraint.ColliderKey.Equals(hit.ColliderKey))
                        {
                            found = true;
                            break;
                        }
                    }

                    if (!found)
                    {
                        newContactIndex = hitIndex;
                        break;
                    }
                }
            }

            // Move character along the newDisplacement direction until it reaches this new contact
            if (newContactIndex >= 0)
            {
                ColliderCastHit newContact = newCollector.AllHits[newContactIndex];

                float fraction = newContact.Fraction / math.length(newDisplacement);
                integratedTime *= fraction;

                float3 displacement = newDisplacement * fraction;
                newPosition = prevPosition + displacement;
            }

            remainingTime -= integratedTime;

            // Remember last displacement for next iteration
            lastDisplacement = newVelocity * remainingTime;
        }

        // Write back position and velocity
        transform.pos  = newPosition;
        linearVelocity = newVelocity;
    }
            public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
            {
                NativeArray <CharacterController> chunkCharacterControllers = chunk.GetNativeArray(CharacterControllerType);
                NativeArray <PhysicsCollider>     chunkPhysicsColliders     = chunk.GetNativeArray(PhysicsColliderType);
                NativeArray <Translation>         chunkTranslations         = chunk.GetNativeArray(TranslationType);
                NativeArray <Rotation>            chunkRotations            = chunk.GetNativeArray(RotationType);

                for (int i = 0; i < chunk.Count; i++)
                {
                    CharacterController controller  = chunkCharacterControllers[i];
                    PhysicsCollider     collider    = chunkPhysicsColliders[i];
                    Translation         translation = chunkTranslations[i];
                    Rotation            rotation    = chunkRotations[i];

                    RigidTransform transform = new RigidTransform
                    {
                        pos = translation.Value,
                        rot = rotation.Value
                    };

                    unsafe
                    {
                        Collider *queryCollider;
                        {
                            Collider *colliderPtr = collider.ColliderPtr;

                            byte *copiedColliderMemory = stackalloc byte[colliderPtr->MemorySize];
                            queryCollider = (Collider *)(copiedColliderMemory);
                            UnsafeUtility.MemCpy(queryCollider, colliderPtr, colliderPtr->MemorySize);
                            queryCollider->Filter = CollisionFilter.Default;
                        }

                        KinematicMotorUtilities.MaxHitCollector <DistanceHit> distanceHitCollector = new KinematicMotorUtilities.MaxHitCollector <DistanceHit>(controller.GroundTollerance, ref DistanceHits);
                        {
                            ColliderDistanceInput input = new ColliderDistanceInput
                            {
                                MaxDistance = controller.GroundTollerance,
                                Transform   = transform,
                                Collider    = queryCollider
                            };
                            World.CalculateDistance(input, ref distanceHitCollector);
                        }

                        for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
                        {
                            DistanceHit hit = distanceHitCollector.AllHits[hitIndex];
                            KinematicMotorUtilities.CreateConstraintFromHit(World, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, float3.zero, hit.SurfaceNormal, hit.Distance, DeltaTime, out SurfaceConstraintInfo constraint);
                            SurfaceConstraintInfos[hitIndex] = constraint;
                        }

                        float3 outPosition = transform.pos;
                        float3 outVelocity = -math.up();
                        SimplexSolver.Solve(World, DeltaTime, math.up(), distanceHitCollector.NumHits, ref SurfaceConstraintInfos, ref outPosition, ref outVelocity, out float integratedTime);

                        if (distanceHitCollector.NumHits == 0)
                        {
                            controller.State = CharacterControllerState.NONE;
                        }
                        else
                        {
                            outVelocity = math.normalize(outVelocity);
                            float slopeAngleSin   = math.dot(outVelocity, -math.up());
                            float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                            float maxSlopeCos     = math.cos(controller.MaxSlope);

                            controller.State = CharacterControllerState.GROUNDED;
                        }
                    }

                    // Apply data back to chunk
                    {
                        chunkCharacterControllers[i] = controller;
                    }
                }
            }
    public static unsafe void CheckSupport(
        ref PhysicsWorld world, Collider *collider, CharacterControllerStepInput stepInput, float3 groundProbeVector, RigidTransform transform, float maxSlope,
        ref NativeList <SurfaceConstraintInfo> constraints, ref NativeList <ColliderCastHit> castHits,
        out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity)
    {
        surfaceNormal   = float3.zero;
        surfaceVelocity = float3.zero;

        //查询物理世界
        var displacement = groundProbeVector;
        SelfFilteringAllHitsCollector <ColliderCastHit> hitCollector = new SelfFilteringAllHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
        ColliderCastInput input = new ColliderCastInput()
        {
            Collider    = collider,
            Orientation = transform.rot,
            Start       = transform.pos,
            End         = transform.pos + displacement
        };

        world.CastCollider(input, ref hitCollector);
        //如果没检测到任何碰撞体,那么可以肯定是非支持状态
        if (hitCollector.NumHits == 0)
        {
            characterState = CharacterSupportState.Unsupported;
            return;
        }

        float3 downwardsDirection = -stepInput.Up;

        Assert.IsTrue(Unity.Physics.Math.IsNormalized(downwardsDirection));

        float maxSlopCos = math.cos(maxSlope);

        for (int i = 0; i < hitCollector.NumHits; i++)
        {
            var hit = hitCollector.AllHits[i];
            CreateConstraint(stepInput.World, stepInput.Up,
                             hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(displacement),
                             stepInput.SkinWidth, maxSlopCos, ref constraints);
        }

        float3 initialVelocity = groundProbeVector * (1.0f / stepInput.DeltaTime);//初速度(期望在1秒内到达目标)

        float3 outVelocity = initialVelocity;
        float3 outPosition = transform.pos;

        SimplexSolver.Solve(stepInput.World, stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed,
                            constraints, ref outPosition, ref outVelocity, out float integratedTime, false);
        {
            int numSupportingPlanes = 0;
            for (int i = 0; i < constraints.Length; i++)
            {
                var constraint = constraints[i];
                if (constraint.Touched && !constraint.IsTooSteep)
                {
                    numSupportingPlanes++;
                    surfaceNormal   += constraint.Plane.Normal;
                    surfaceVelocity += constraint.Velocity;
                }
            }

            if (numSupportingPlanes > 0)
            {
                float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
                surfaceNormal   *= invNumSupportingPlanes;
                surfaceVelocity *= invNumSupportingPlanes;

                surfaceNormal = math.normalize(surfaceNormal);
            }
        }

        {
            if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
            {
                //如果速度没有显著改变,那么肯定是未支持状态?
                characterState = CharacterSupportState.Unsupported;
            }
            else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq)
            {
                //如果速度非常小,那么肯定是支持状态
                characterState = CharacterSupportState.Supported;
            }
            else
            {
                //滑行和支持状态
                outVelocity = math.normalize(outVelocity);
                float slopeAngleSin   = math.max(0.0f, math.dot(outVelocity, downwardsDirection));
                float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
                if (slopeAngleCosSq < maxSlopCos * maxSlopCos)
                {
                    characterState = CharacterSupportState.Sliding;
                }
                else
                {
                    characterState = CharacterSupportState.Supported;
                }
            }
        }
    }
Example #27
0
        public static unsafe void SolveCollisionConstraints(PhysicsWorld world, float deltaTime, int maxIterations, float skinWidth, float maxSlope, Collider *collider, ref RigidTransform transform, ref float3 velocity, ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> colliderHits, ref NativeArray <SurfaceConstraintInfo> surfaceConstraints)
        {
            float  remainingTime        = deltaTime;
            float3 previousDisplacement = velocity * remainingTime;

            float3 outPosition = transform.pos;
            float3 outVelocity = velocity;

            quaternion orientation = transform.rot;

            const float timeEpsilon = 0.000001f;

            for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
            {
                MaxHitCollector <DistanceHit> distanceHitCollector = new MaxHitCollector <DistanceHit>(skinWidth, ref distanceHits);

                int constraintCount = 0;

                // Handle distance checks
                {
                    ColliderDistanceInput input = new ColliderDistanceInput
                    {
                        Collider    = collider,
                        MaxDistance = skinWidth,
                        Transform   = new RigidTransform
                        {
                            pos = outPosition,
                            rot = orientation
                        }
                    };
                    world.CalculateDistance(input, ref distanceHitCollector);

                    for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
                    {
                        DistanceHit hit = distanceHitCollector.AllHits[hitIndex];
                        CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, float3.zero, hit.SurfaceNormal, hit.Distance, deltaTime, out SurfaceConstraintInfo constraint);
                        CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount);
                        surfaceConstraints[constraintCount++] = constraint;
                    }
                }

                // Handle Collider
                {
                    float3 displacement = previousDisplacement;
                    MaxHitCollector <ColliderCastHit> colliderHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits);

                    ColliderCastInput input = new ColliderCastInput
                    {
                        Collider    = collider,
                        Position    = outPosition,
                        Direction   = velocity,
                        Orientation = orientation
                    };
                    world.CastCollider(input, ref colliderHitCollector);

                    for (int hitIndex = 0; hitIndex < colliderHitCollector.NumHits; hitIndex++)
                    {
                        ColliderCastHit hit = colliderHitCollector.AllHits[hitIndex];

                        bool duplicate = false;
                        for (int distanceHitIndex = 0; distanceHitIndex < distanceHitCollector.NumHits; distanceHitIndex++)
                        {
                            DistanceHit distanceHit = distanceHitCollector.AllHits[distanceHitIndex];
                            if (distanceHit.RigidBodyIndex == hit.RigidBodyIndex && distanceHit.ColliderKey.Equals(hit.ColliderKey))
                            {
                                duplicate = true;
                                break;
                            }
                        }

                        if (!duplicate)
                        {
                            CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, outVelocity, hit.SurfaceNormal, hit.Fraction * math.length(previousDisplacement), deltaTime, out SurfaceConstraintInfo constraint);
                            CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount);
                            surfaceConstraints[constraintCount++] = constraint;
                        }
                    }
                }

                float3 previousPosition = outPosition;
                float3 previousVelocity = outVelocity;

                SimplexSolver.Solve(world, remainingTime, math.up(), constraintCount, ref surfaceConstraints, ref outPosition, ref outVelocity, out float integratedTime);

                float3 currentDisplacement = outPosition - previousPosition;

                MaxHitCollector <ColliderCastHit> displacementHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits);
                int displacementContactIndex = -1;

                if (math.lengthsq(currentDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
                {
                    ColliderCastInput input = new ColliderCastInput
                    {
                        Collider    = collider,
                        Position    = previousPosition,
                        Direction   = currentDisplacement,
                        Orientation = orientation
                    };
                    world.CastCollider(input, ref displacementHitCollector);

                    for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
                    {
                        ColliderCastHit hit = displacementHitCollector.AllHits[hitIndex];

                        bool duplicate = false;
                        for (int constrainIndex = 0; constrainIndex < constraintCount; constrainIndex++)
                        {
                            SurfaceConstraintInfo constraint = surfaceConstraints[constrainIndex];
                            if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey))
                            {
                                duplicate = true;
                                break;
                            }

                            if (!duplicate)
                            {
                                displacementContactIndex = hitIndex;
                                break;
                            }
                        }
                    }

                    if (displacementContactIndex >= 0)
                    {
                        ColliderCastHit newContact = displacementHitCollector.AllHits[displacementContactIndex];

                        float fraction = newContact.Fraction / math.length(currentDisplacement);
                        integratedTime *= fraction;

                        float3 displacement = currentDisplacement * fraction;
                        outPosition = previousPosition + displacement;
                    }
                }

                remainingTime -= integratedTime;

                previousDisplacement = outVelocity * remainingTime;
            }

            transform.pos = outPosition;
            velocity      = outVelocity;
        }