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(); }
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 }
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()); }
/// <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"); } } } }
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(); } }
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(); }
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()); }
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); }
/// <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; }
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); }
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; }
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; } } } }
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; }
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)); }
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; } } } }
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); }
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; } } } }
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; } } } }
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; }