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
}
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 ORECalculate(bool usefullORE)
{
useFullORE = usefullORE;
oreDictionary = new OREDictionary();
solver = new SimplexSolver();
foreach (var kv in oreDictionary.ORE_Dictionary)
{
if (usefullORE || kv.Value.isMain)
{
int id = 0;
solver.AddVariable(kv.Key, out id);
solver.SetBounds(id, 0, Rational.PositiveInfinity);
kv.Value.id = id;
}
}
solver.AddRow("a", out a);
solver.AddRow("b", out b);
solver.AddRow("c", out c);
solver.AddRow("d", out d);
solver.AddRow("e", out e);
solver.AddRow("f", out f);
solver.AddRow("g", out g);
solver.AddRow("h", out h);
solver.AddRow("cost", out cost);
solver.AddGoal(cost, 1, true);
}
private void gaussProcessSelectedBasesToolStripMenuItem_Click(object sender, EventArgs e)
{
try
{
DowndateGrid();
}
catch
{
MessageBox.Show("Input error!");
return;
}
if (dataGridView1.SelectedCells.Count == 1)
{
int maxJ = 0;
for (int i = 0; i < A.Length; i++)
{
if (A[i].Length > maxJ)
{
maxJ = A[i].Length;
}
}
maxJ++; // A0
Fraction[,] a = new Fraction[A.Length, maxJ];
for (int i = 0; i < A.Length; i++)
{
for (int j = 0; j < A[i].Length; j++)
{
a[i, j] = A[i][j];
}
}
for (int i = 0; i < B.Length; i++)
{
a[i, maxJ - 1] = B[i];
}
uint r, c;
c = (uint)dataGridView1.SelectedCells[0].ColumnIndex;
r = (uint)dataGridView1.SelectedCells[0].RowIndex - 1;
SimplexSolver.GGaussProcess(ref a, r, c);
for (int i = 0; i < a.GetLength(0); i++)
{
for (int j = 0; j < a.GetLength(1) - 1; j++)
{
A[i][j] = a[i, j];
}
}
for (int i = 0; i < B.Length; i++)
{
B[i] = a[i, maxJ - 1];
}
UpdateGrid();
}
}
/// <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");
}
}
}
}
/// <summary>
/// Tworzy listę rozwiązań wraz z nazwami stopów
/// </summary>
/// <param name="all">Lista stopów biorąca udział w procesie</param>
/// <param name="solv">Solver zawierający wyniki</param>
/// <returns>Lista rozwiązań</returns>
private List <Solution> createSolution(List <Alloy> all, SimplexSolver solv)
{
List <Solution> sol = new List <Solution>();
for (int i = 0; i < all.Count(); i++)
{
//Dla kazdego składnika weź wynik i stworz obiekt rozwiązanie
double tmp = solver.GetValue(i).ToDouble();
sol.Add(new Solution(all.ElementAt(i).name, tmp, tmp.ToString("0.00000")));
total += tmp;
}
return(sol);
}
/// <summary>
/// Akcja wykonywana po wcisnieciu przycisku. Sprawdzenie poprawnosci danych i uruchomienie solvera
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private async void btn_count_Clicked(object sender, EventArgs e)
{
if (!(selectedSmelts.Count > 1))
{
double num = 0;
//Spróbuj przeparsować dane
if (Double.TryParse(entWeight.Text, out num))
{
//Inicjalizuj solver i do wytopu wprowadź masę
this.solver = new SimplexSolver();
selectedSmelts.ElementAt(0).Weight = double.Parse(entWeight.Text);
//Do uruchomienia solera potrzebnne są tablice z pierwiastkami (zmniejszenie ilości kodu i poprawienie czytelności)
foreach (Alloy xd in selectedAlloys)
{
xd.createTabOfElements(xd);
}
foreach (Smelt xd in selectedSmelts)
{
xd.createTabofEvoporation(xd);
xd.createTabofMaxNorm(xd);
xd.createTabofMinNorm(xd);
}
//Wybór otymalizacji kosztowej lub masowej
if (SwitchPriceWeight.IsToggled)
{
//optymalizuj mase
calculate_weight(selectedAlloys, selectedSmelts);
}
else
{
//optymalizuj koszt
calculate_price(selectedAlloys, selectedSmelts);
}
}
//Pokaż errory jak coś źle
else
{
await DisplayAlert("Error", "Popraw masę wytopu", "OK");
}
}
else
{
await DisplayAlert("Error", "Ilość wybranych wytopów musi wynosić 1", "OK");
}
}
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();
}
}
private void выделитьБазисМатрицыAToolStripMenuItem_Click(object sender, EventArgs e)
{
try
{
DowndateGrid();
}
catch
{
return;
}
int maxJ = 0;
for (int i = 0; i < A.Length; i++)
{
if (A[i].Length > maxJ)
{
maxJ = A[i].Length;
}
}
Fraction[,] a = new Fraction[A.Length, maxJ];
for (int i = 0; i < A.Length; i++)
{
for (int j = 0; j < A[i].Length; j++)
{
a[i, j] = A[i][j];
}
}
if (formTables == null)
{
formTables = new FormSTables();
}
formTables.AddTable(a);
for (uint i = 0; i < (a.GetLength(0) < a.GetLength(1) ? a.GetLength(0) : a.GetLength(1)); i++)
{
SimplexSolver.GGaussProcess(ref a, i, i);
formTables.AddTable(a);
}
formTables.Show();
}
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());
}
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 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);
}
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,
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;
}
}
}
}
/// <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 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(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 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 SimplexSolver PrepareSimplexSolver(int maxAgents, out Dictionary <Models.Shift, int> shiftsExt, out int vidGoal)
{
var SX = new SimplexSolver();
var maxRq = HalfHourRequirements.Max(x => x.RequiredForce);
var sumRq = HalfHourRequirements.Sum(x => x.RequiredForce);
shiftsExt = null;
////goal: total no of agents
//int ridgoal;
//SX.AddRow(key: "goal", vid: out ridgoal);
//SX.SetBounds(ridgoal, lower: 0, upper: maxAgents); //total agents must range from 0 to max agents available
//SX.AddGoal(vid: ridgoal, pri: 1, fMinimize: true); //minimize total number of agents for that day
//goal: total no of agents, minimize overtime
int ridgoal;
SX.AddRow(key: "goal", vid: out ridgoal);
SX.SetBounds(ridgoal, lower: 0, upper: Rational.PositiveInfinity); // upper: maxAgents); //total agents must range from 0 to max agents available
SX.AddGoal(vid: ridgoal, pri: 2, fMinimize: true); //minimize total number of agents for that day
//goal2: Minimize excess force
int ridexcess;
SX.AddRow(key: "excess", vid: out ridexcess);
SX.SetBounds(ridexcess, lower: 0, upper: sumRq); //allow excess from 0 to sum of all requirements
SX.AddGoal(vid: ridexcess, pri: 1, fMinimize: true); //try to minimize excess time
var shiftsX = new Dictionary <Models.Shift, int>();
int i = 0;
Shifts.ForEach(x => {
int vid;
SX.AddVariable(key: string.Format("{0}. {1} {2:hh}:{2:mm} - {3:hh}:{3:mm}", i + 1, x.Name, x.Start, x.End), vid: out vid);
//the no of agents on each shift may not exceed to max requirements
SX.SetBounds(vid: vid, lower: 0, upper: maxRq);
shiftsX.Add(x, vid);
//SX.SetCoefficient(vidRow: ridgoal, vidVar: vid, num: 1); //normal weight: all shifts are the same
SX.SetCoefficient(vidRow: ridgoal, vidVar: vid, num: x.Duration.Hours <= 8 ? 1 : 10); //weights per shift: overtime = *10 more expensive than normal shifts
i++;
});
//Constraint - Agents from every active shift on every half hour must be >= requirement for that half hour
HalfHourRequirements.ForEach(hh =>
{
List <int> vidShiftsActive = new List <int>();
foreach (var entry in shiftsX)
{
if (entry.Key.IncludesHalfHour(hh.Start))
{
vidShiftsActive.Add(entry.Value);
}
}
//add constraint for sum of force of active shifts on that halfhour
//if we need agents but no shifts exists for a halfhour, do not add a constraint
if (vidShiftsActive.Count > 0)
{
int ridHalf;
SX.AddRow(key: string.Format("{0:hh}:{0:mm} [{1}]", hh.Start, hh.RequiredForce), vid: out ridHalf);
//specify whichs shifts contributes to half hour's total force
vidShiftsActive.ForEach(vidShift =>
{
SX.SetCoefficient(vidRow: ridHalf, vidVar: vidShift, num: 1);
SX.SetCoefficient(vidRow: ridexcess, vidVar: vidShift, num: 1);
});
//each 30' span, must have at least as many required agents
SX.SetBounds(vid: ridHalf, lower: hh.RequiredForce, upper: Rational.PositiveInfinity);
}
});
shiftsExt = shiftsX;
vidGoal = ridgoal;
return(SX);
}
static void Main(string[] args)
{
{
double[][] arr;
arr = new double[3][];
arr[0] = new double[4] {
9, 8, 7, 6
};
arr[1] = new double[4] {
42, 13, 7, 2
};
arr[2] = new double[4] {
101, 102, 103, 104
};
MeshAttributeCLASSES meshComponent2 = MeshAttributeCLASSES.makeDouble4(arr);
double[] readback = meshComponent2.getDouble4Accessor()[1];
int breakPointHere = 42;
}
// test solver for trivial cases
{
double one = 1.0;
double J11 = 1.0, J22 = 1.0, J33 = 1.0, // principal moments of inertia of the body
w1 = 0, w2 = 0, w3 = 0, // angular velocity of spacecraft in spacecraft frame
// result values
q1Dot, q2Dot, q3Dot, q4Dot,
w1Dot, w2Dot, w3Dot,
// coeefficients, taken from the paper
gamma = 0.7,
aCoefficient = 1.25,
d = 7.5, k = 3.0, betaOne = 0.001;
Quaternion spacecraftOrientationError;
// set to identity
/*
* spacecraftOrientationError.i = 0;
* spacecraftOrientationError.j = 0;
* spacecraftOrientationError.k = 0;
* spacecraftOrientationError.scalar = 1;
*/
spacecraftOrientationError = QuaternionUtilities.makeFromAxisAndAngle(new SpatialVectorDouble(new double[] { 1, 0, 0 }), 1.0);
QuaternionFeedbackRegulatorForSpacecraft.calcControl(
J11, J22, J33,
spacecraftOrientationError.i, spacecraftOrientationError.j, spacecraftOrientationError.k, spacecraftOrientationError.scalar,
w1, w2, w3,
out q1Dot, out q2Dot, out q3Dot, out q4Dot,
out w1Dot, out w2Dot, out w3Dot,
aCoefficient,
gamma,
d, k,
betaOne
);
// changes must be zero
Debug.Assert(q1Dot == 0);
Debug.Assert(q2Dot == 0);
Debug.Assert(q3Dot == 0);
Debug.Assert(q4Dot == 0);
Debug.Assert(w1Dot == 0);
Debug.Assert(w2Dot == 0);
Debug.Assert(w3Dot == 0);
int breakPointHere = 42;
}
SimplexSolver simplexSolver = new SimplexSolver();
/* first example from video tutorial, https://www.youtube.com/watch?v=Axg9OhJ4cjg, bottom row is negative unlike in the video
*/
simplexSolver.matrix = Matrix.makeByRowsAndColumns(3, 5);
simplexSolver.matrix[0, 0] = 4;
simplexSolver.matrix[0, 1] = 8;
simplexSolver.matrix[0, 2] = 1;
simplexSolver.matrix[0, 3] = 0;
simplexSolver.matrix[0, 4] = 24;
simplexSolver.matrix[1, 0] = 2;
simplexSolver.matrix[1, 1] = 1;
simplexSolver.matrix[1, 2] = 0;
simplexSolver.matrix[1, 3] = 1;
simplexSolver.matrix[1, 4] = 10;
simplexSolver.matrix[2, 0] = -3;
simplexSolver.matrix[2, 1] = -4;
simplexSolver.matrix[2, 2] = 0;
simplexSolver.matrix[2, 3] = 0;
simplexSolver.matrix[2, 4] = 0;
simplexSolver.iterate();
// result must be ~16.67
Debug.Assert(simplexSolver.matrix[2, 4] > 16.6 && simplexSolver.matrix[2, 4] < 16.8);
// TODO< move into unittest for Matrix >
Matrix toInverseMatrix = new Matrix(2, 2);
toInverseMatrix[0, 0] = 2.0f;
toInverseMatrix[0, 1] = 1.0f;
toInverseMatrix[1, 0] = 2.0f;
toInverseMatrix[1, 1] = 2.0f;
Matrix inversedMatrix = toInverseMatrix.inverse();
Debug.Assert(System.Math.Abs(inversedMatrix[0, 0] - 1.0f) < 0.001f);
Debug.Assert(System.Math.Abs(inversedMatrix[0, 1] - -0.5f) < 0.001f);
Debug.Assert(System.Math.Abs(inversedMatrix[1, 0] - -1.0f) < 0.001f);
Debug.Assert(System.Math.Abs(inversedMatrix[1, 1] - 1.0f) < 0.001f);
// test pid controller
if (false)
{
Pid.Configuration pidConfiguration = new Pid.Configuration();
pidConfiguration.integral = 0;
pidConfiguration.derivative = 0.1;
pidConfiguration.proportial = 0.3;
Pid pid = Pid.makeByTargetAndConfiguration(0, pidConfiguration);
double value = 1.0;
double control;
control = pid.step(value, 0.5);
value += control;
for (int i = 0; i < 10; i++)
{
control = pid.step(value, 0.5);
value += control;
Console.WriteLine("value={0} control={1}", value, control);
}
int breakPointHere0 = 1;
}
EntityManager entityManager = new EntityManager();
SolidResponsibility solidResponsibility;
EffectResponsibility effectResponsibility;
ThrusterResponsibility thrusterResponsibility;
AttitudeAndAccelerationControlResponsibility attitudeAndAccelerationControlResponsibility;
thrusterResponsibility = new ThrusterResponsibility();
attitudeAndAccelerationControlResponsibility = new AttitudeAndAccelerationControlResponsibility(thrusterResponsibility);
effectResponsibility = new EffectResponsibility();
solidResponsibility = new SolidResponsibility();
solidResponsibility.mapping = new PhysicsObjectIdToSolidClusterMapping();
EntityController entityController = new EntityController();
IKeyboardInputHandler keyboardInputHandler = new PlayerShipControlInputHandler(entityController);
KeyboardInputRemapper keyboardInputRemapper = new KeyboardInputRemapper(keyboardInputHandler);
SoftwareRenderer softwareRenderer = new SoftwareRenderer();
PrototypeForm prototypeForm = new PrototypeForm();
prototypeForm.softwareRenderer = softwareRenderer;
IGuiRenderer guiRenderer = prototypeForm.softwareGuiRenderer;
GuiContext guiContext = new GuiContext(guiRenderer);
prototypeForm.guiContext = guiContext;
KeyboardEventRouterOfGui keyboardEventRouterOfGui = new KeyboardEventRouterOfGui(guiContext.selectionTracker);
KeyboardInputRouter keyboardInputRouter = new KeyboardInputRouter(keyboardInputRemapper, keyboardEventRouterOfGui);
prototypeForm.keyboardInputRouter = keyboardInputRouter;
prototypeForm.Show();
PhysicsEngine physicsEngine = new PhysicsEngine();
solidResponsibility.physicsEngine = physicsEngine;
physicsEngine.collisionHandlers.Add(new DefaultCollisionHandler()); // add the default collision handler for absorbing all particles
AttachedForce thruster = new AttachedForce(
new SpatialVectorDouble(new double[] { 0, 1, 0 }), // localLocation
new SpatialVectorDouble(new double[] { 1, 0, 0 }) // localDirection
);
PhysicsComponent physicsComponent;
{
double mass = 1.0;
Matrix inertiaTensor = InertiaHelper.calcInertiaTensorForCube(mass, 1.0, 1.0, 1.0);
physicsComponent = physicsEngine.createPhysicsComponent(new SpatialVectorDouble(new double[] { 0, 0, 10 }), new SpatialVectorDouble(new double[] { 0, 0, 0 }), mass, inertiaTensor);
}
physicsComponent.attachedForces.Add(thruster);
MeshComponent meshComponent = new MeshComponent();
meshComponent.mesh = new MeshWithExplicitFaces();
meshComponent.mesh.faces = new MeshWithExplicitFaces.Face[1];
meshComponent.mesh.faces[0] = new MeshWithExplicitFaces.Face();
meshComponent.mesh.faces[0].verticesIndices = new uint[] { 0, 1, 2 };
{ // create a VerticesWithAttributes with just positions
MutableMeshAttribute positionMeshAttribute = MutableMeshAttribute.makeDouble4ByLength(4);
positionMeshAttribute.getDouble4Accessor()[0] = new double[] { -1, -1, 0, 1 };
positionMeshAttribute.getDouble4Accessor()[1] = new double[] { 1, -1, 0, 1 };
positionMeshAttribute.getDouble4Accessor()[2] = new double[] { 0, 1, 0, 1 };
positionMeshAttribute.getDouble4Accessor()[3] = new double[] { 0, 0, 1, 1 };
VerticesWithAttributes verticesWithAttributes = new VerticesWithAttributes(new AbstractMeshAttribute[] { positionMeshAttribute }, 0);
meshComponent.mesh.verticesWithAttributes = verticesWithAttributes;
}
//TransformedMeshComponent transformedMeshComponentForPhysics = new TransformedMeshComponent();
//transformedMeshComponentForPhysics.meshComponent = meshComponent;
IList <ColliderComponent> colliderComponents = new List <ColliderComponent>();
{
SpatialVectorDouble colliderComponentSize = new SpatialVectorDouble(new double[] { 2, 2, 2 });
SpatialVectorDouble colliderComponentLocalPosition = new SpatialVectorDouble(new double[] { 0, 0, 0 });
SpatialVectorDouble colliderComponentLocalRotation = new SpatialVectorDouble(new double[] { 0, 0, 0 });
ColliderComponent colliderComponent0 = ColliderComponent.makeBox(colliderComponentSize, colliderComponentLocalPosition, colliderComponentLocalRotation);
colliderComponents.Add(colliderComponent0);
}
///physicsEngine.physicsAndMeshPairs.Add(new PhysicsComponentAndCollidersPair(physicsComponent, colliderComponents));
// same mesh for the visualization
TransformedMeshComponent transformedMeshComponentForRendering = new TransformedMeshComponent();
transformedMeshComponentForRendering.meshComponent = meshComponent;
///softwareRenderer.physicsAndMeshPairs.Add(new PhysicsComponentAndMeshPair(physicsComponent, transformedMeshComponentForRendering));
physicsEngine.tick();
// TODO< read object description from json and create physics and graphics objects >
// TODO< read and create solid description from json >
// create and store solids of rocket
// refactored
/*
* if ( true ) {
*
*
* SolidCluster solidClusterOfRocket = new SolidCluster();
*
* // create solid of rocket
* {
* SpatialVectorDouble solidSize = new SpatialVectorDouble(new double[] { 2, 2, 2 });
* double massOfSolidInKilogram = 1.0;
* IList<CompositionFraction> solidCompositionFractions = new List<CompositionFraction>() { new CompositionFraction(new Isotope("Fe56"), massOfSolidInKilogram) };
* Composition solidComposition = new Composition(solidCompositionFractions);
*
* physics.solid.Solid solid = physics.solid.Solid.makeBox(solidComposition, solidSize);
*
* SpatialVectorDouble solidLocalPosition = new SpatialVectorDouble(new double[] { 0, 0, 0 });
* SpatialVectorDouble solidLocalRotation = new SpatialVectorDouble(new double[] { 0, 0, 0 });
*
* solidClusterOfRocket.solids.Add(new SolidCluster.SolidWithPositionAndRotation(solid, solidLocalPosition, solidLocalRotation));
* }
*
* /// TODO, uncommented because it uses the not existing physics object
* /// solidResponsibility.mapping.physicsObjectIdToSolidCluster[rocketPhysicsObject.id] = solidClusterOfRocket;
* }
*/
// add test particle
if (false)
{
SpatialVectorDouble particlePosition = new SpatialVectorDouble(new double[] { 0, 0, 0 });
SpatialVectorDouble particleVelocity = new SpatialVectorDouble(new double[] { 0, 0, 1 });
PhysicsComponent particlePhysicsComponent = physicsEngine.createPhysicsComponent(particlePosition, particleVelocity, 1.0, null);
physicsEngine.addParticle(particlePhysicsComponent);
}
// write game object for TestMissile
if (true)
{
DemoObjectSerializer.seralizeAndWriteShip();
DemoObjectSerializer.serializeAndWriteMissile();
}
GameObjectBuilder gameObjectBuilder = new GameObjectBuilder();
gameObjectBuilder.physicsEngine = physicsEngine;
gameObjectBuilder.softwareRenderer = softwareRenderer;
gameObjectBuilder.solidResponsibility = solidResponsibility;
gameObjectBuilder.effectResponsibility = effectResponsibility;
gameObjectBuilder.thrusterResponsibility = thrusterResponsibility;
gameObjectBuilder.attitudeAndAccelerationControlResponsibility = attitudeAndAccelerationControlResponsibility;
// load missile game object from json and construct it
if (false)
{
GameObjectTemplate deserilizedGameObjectTemplate;
// load
{
List <string> uriParts = new List <string>(AssemblyDirectory.Uri.Segments);
uriParts.RemoveAt(0); // remove first "/"
uriParts.RemoveRange(uriParts.Count - 4, 4);
uriParts.AddRange(new string[] { "gameResources/", "prototypingMissile.json" });
string path = string.Join("", uriParts).Replace('/', '\\').Replace("%20", " ");
string fileContent = File.ReadAllText(path);
deserilizedGameObjectTemplate = GameObjectTemplate.deserialize(fileContent);
}
// build game object from template
Entity createdEntity;
{
SpatialVectorDouble globalPosition = new SpatialVectorDouble(new double[] { 0, 0, 5 });
SpatialVectorDouble globalVelocity = new SpatialVectorDouble(new double[] { 0, 0, 0 });
createdEntity = gameObjectBuilder.buildFromTemplate(deserilizedGameObjectTemplate, globalPosition, globalVelocity);
entityManager.addEntity(createdEntity);
}
// manually add components
{
// TODO< chemical explosive ignition component >
DummyComponent chemicalExplosiveIgnitionComponent = new DummyComponent();
// remap proximityEnter to explode
EventRemapperComponent eventRemapper = new EventRemapperComponent(chemicalExplosiveIgnitionComponent);
eventRemapper.eventTypeMap["proximityEnter"] = "explode";
// TODO< blacklist somehow other missiles >
PhysicsComponent parentPhysicsComponent = createdEntity.getSingleComponentsByType <PhysicsComponent>();
ProximityDetectorComponent proximityDetector = ProximityDetectorComponent.makeSphereDetector(physicsEngine, parentPhysicsComponent, 20.0, eventRemapper);
entityManager.addComponentToEntity(createdEntity, proximityDetector);
}
}
Entity playerShip;
if (true)
{
GameObjectTemplate deserilizedGameObjectTemplate;
// load
{
List <string> uriParts = new List <string>(AssemblyDirectory.Uri.Segments);
uriParts.RemoveAt(0); // remove first "/"
uriParts.RemoveRange(uriParts.Count - 4, 4);
uriParts.AddRange(new string[] { "gameResources/", "prototypingShip.json" });
string path = string.Join("", uriParts).Replace('/', '\\').Replace("%20", " ");
string fileContent = File.ReadAllText(path);
deserilizedGameObjectTemplate = GameObjectTemplate.deserialize(fileContent);
}
// build game object from template
{
SpatialVectorDouble globalPosition = new SpatialVectorDouble(new double[] { 0, 0, 10 });
SpatialVectorDouble globalVelocity = new SpatialVectorDouble(new double[] { 0, 0, 0 });
Entity createdEntity = gameObjectBuilder.buildFromTemplate(deserilizedGameObjectTemplate, globalPosition, globalVelocity);
entityManager.addEntity(createdEntity);
playerShip = createdEntity;
}
}
Entity aiShip = null;
EntityController aiShipController = new EntityController();
VehicleAlignToCommand command = null;
bool withTestAiShip = true;
if (withTestAiShip)
{
GameObjectTemplate deserilizedGameObjectTemplate;
// load
{
List <string> uriParts = new List <string>(AssemblyDirectory.Uri.Segments);
uriParts.RemoveAt(0); // remove first "/"
uriParts.RemoveRange(uriParts.Count - 4, 4);
uriParts.AddRange(new string[] { "gameResources/", "prototypingShip.json" });
string path = string.Join("", uriParts).Replace('/', '\\').Replace("%20", " ");
string fileContent = File.ReadAllText(path);
deserilizedGameObjectTemplate = GameObjectTemplate.deserialize(fileContent);
}
// build game object from template
{
SpatialVectorDouble globalPosition = new SpatialVectorDouble(new double[] { 0, 0, 10 });
SpatialVectorDouble globalVelocity = new SpatialVectorDouble(new double[] { 0, 0, 0 });
Entity createdEntity = gameObjectBuilder.buildFromTemplate(deserilizedGameObjectTemplate, globalPosition, globalVelocity);
entityManager.addEntity(createdEntity);
aiShip = createdEntity;
}
{ // control
aiShip.getSingleComponentsByType <VehicleControllerComponent>().controller = aiShipController;
}
{ // AI initialization
double dt = 1.0 / 60.0;
SpatialVectorDouble targetDirection = new SpatialVectorDouble(new double[] { 1, 0, 0.1 }).normalized();
double targetDerivationDistance = 0.001;
ulong targetPhysicsObjectId = aiShip.getSingleComponentsByType <PhysicsComponent>().id;
command = VehicleAlignToCommand.makeByGettingPidConfigurationFromAttitudeAndAccelerationControlResponsibility(
attitudeAndAccelerationControlResponsibility,
physicsEngine,
dt,
targetDirection,
aiShipController,
targetPhysicsObjectId,
targetDerivationDistance);
}
}
//
{
}
playerShip.getSingleComponentsByType <VehicleControllerComponent>().controller = entityController;
// create test GUI
{
Button button = new subsystems.gui.Button();
button.position = new SpatialVectorDouble(new double[] { 0.2, 0.2 });
button.size = new SpatialVectorDouble(new double[] { 0.3, 0.1 });
button.text = "[Gamesave05-res.save](35)";
button.textScale = new SpatialVectorDouble(new double[] { 0.05, 0.08 });
guiContext.addGuiElement(button);
}
ulong frameCounter = 0;
ulong debugFrameCounter = 0; // used to limit the frames we do for debugging a scenario
bool isFixedScenarioDebugMode = false;
// TODO< use logger >
StreamWriter debugLogFileOfVariables = null; // used to debug variables and coeeficients of the debug scenario
if (isFixedScenarioDebugMode)
{
if (!File.Exists("E:\\myLogShipOrientation.txt"))
{
using (var stream = File.Create("E:\\myLogShipOrientation.txt"));
}
debugLogFileOfVariables = File.AppendText("E:\\myLogShipOrientation.txt");
}
for (;;)
{
Console.WriteLine("frame={0}", frameCounter);
guiContext.tick();
physicsEngine.tick();
if (!isFixedScenarioDebugMode)
{
prototypeForm.Refresh();
Application.DoEvents(); // HACK, EVIL
}
// AI
{
// we just do this small AI because we are testing
if (false && aiShip != null)
{
command.process();
}
}
// AI - attitude control test
// we change the rotation velocity of the spacecraft directly to simulate "perfect" thruster control
{
PhysicsComponent objectOfControlledSpacecraft = aiShip.getSingleComponentsByType <PhysicsComponent>();
double
// principal moments of inertia of the body
// the controller assumes that the body has an inertia tensor like an box, but small derivations are propably fine, too
J11 = objectOfControlledSpacecraft.inertiaTensor[0, 0], J22 = objectOfControlledSpacecraft.inertiaTensor[1, 1], J33 = objectOfControlledSpacecraft.inertiaTensor[2, 2],
// angular velocity of spacecraft in spacecraft frame
w1 = objectOfControlledSpacecraft.eulerLocalAngularVelocity.x,
w2 = objectOfControlledSpacecraft.eulerLocalAngularVelocity.y,
w3 = objectOfControlledSpacecraft.eulerLocalAngularVelocity.z,
// result values
q1Dot, q2Dot, q3Dot, q4Dot,
w1Dot, w2Dot, w3Dot,
// coeefficients, taken from the paper
gamma = 0.7,
aCoefficient = 1.25,
d = 7.5, k = 3.0, betaOne = 0.001;
// calculate the rotation delta to our target orientation from the current orientation
double debugMagnitude = objectOfControlledSpacecraft.rotation.magnitude;
Quaternion spacecraftOrientationError = objectOfControlledSpacecraft.rotation.difference(QuaternionUtilities.makeFromEulerAngles(0, 0.5, 0));
QuaternionFeedbackRegulatorForSpacecraft.calcControl(
J11, J22, J33,
spacecraftOrientationError.i, spacecraftOrientationError.j, spacecraftOrientationError.k, spacecraftOrientationError.scalar,
w1, w2, w3,
out q1Dot, out q2Dot, out q3Dot, out q4Dot,
out w1Dot, out w2Dot, out w3Dot,
aCoefficient,
gamma,
d, k,
betaOne
);
Console.WriteLine(
"w1dot={0}, w2dot={1}, w3dot={2}", w1Dot.ToString("0.0000", System.Globalization.CultureInfo.InvariantCulture), w2Dot.ToString("0.0000", System.Globalization.CultureInfo.InvariantCulture), w3Dot.ToString("0.0000", System.Globalization.CultureInfo.InvariantCulture)
);
if (isFixedScenarioDebugMode)
{
debugLogFileOfVariables.WriteLine("w1dot={0}, w2dot={1}, w3dot={2}", w1Dot.ToString("0.0000", System.Globalization.CultureInfo.InvariantCulture), w2Dot.ToString("0.0000", System.Globalization.CultureInfo.InvariantCulture), w3Dot.ToString("0.0000", System.Globalization.CultureInfo.InvariantCulture));
debugLogFileOfVariables.Flush();
}
// access directly
objectOfControlledSpacecraft.eulerLocalAngularVelocity.x += (w1Dot * (1.0 / 60.0));
objectOfControlledSpacecraft.eulerLocalAngularVelocity.y += (w2Dot * (1.0 / 60.0));
objectOfControlledSpacecraft.eulerLocalAngularVelocity.z += (w3Dot * (1.0 / 60.0));
}
attitudeAndAccelerationControlResponsibility.resetAllThrusters();
entityManager.updateAllEntities();
// order after update is important
attitudeAndAccelerationControlResponsibility.limitAllThrusters();
attitudeAndAccelerationControlResponsibility.transferThrusterForce();
//thruster.forceInNewton = 0.5 * entityController.inputAcceleration;
if (!isFixedScenarioDebugMode)
{
Thread.Sleep((int)((1.0 / 60.0) * 1000.0));
}
if (isFixedScenarioDebugMode)
{
debugLogFileOfVariables.WriteLine("{0},", aiShip.getSingleComponentsByType <PhysicsComponent>().rotation.j.ToString("0.0000", System.Globalization.CultureInfo.InvariantCulture));
debugLogFileOfVariables.Flush();
}
//File.AppendText("E:\\myLogShipOrientation.txt").WriteLine("{0},", aiShip.getSingleComponentsByType<PhysicsComponent>().rotation.y);
if (isFixedScenarioDebugMode && debugFrameCounter >= 8000) // 30000
{
break;
}
debugFrameCounter++;
frameCounter++;
}
}
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 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 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 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 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(
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 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, 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;
}
