public static void CreateChain(KinematicsSystem system, int boxCount, float boxSize, Vector2 particlePosition)
{
for (int i = 0; i < boxCount; i++)
{
particlePosition += new Vector2(boxSize, 0);
system.AddParticles(
new PhysicalParticle
{
Position = particlePosition,
InertialMoment = boxSize * boxSize / 12f,
}
);
if (i > 0)
{
PhysicalParticle last = system.Particles[i - 1];
PhysicalParticle current = system.Particles[i];
// Use the sign to alternate which corner is attached. This allows the chain to spread out diagonally.
int sign = 2 * (i % 2) - 1;
system.AddConstraints(new JointConstraint(
last, new Vector2(boxSize * .5f, sign * boxSize * .5f),
current, new Vector2(-boxSize * .5f, sign * boxSize * .5f)));
}
}
}
Dictionary <int, ParticleAggregate> PartitionParticles(PhysicalParticle[] particles, Vector3 cell_size)
{
Dictionary <int, ParticleAggregate> spatial_hash = new Dictionary <int, ParticleAggregate>();
for (int i = 0; i < particles.Length; i++)
{
PhysicalParticle p = particles[i];
int x_ind = Mathf.FloorToInt(p.position.x / cell_size.x);
int y_ind = Mathf.FloorToInt(p.position.y / cell_size.y);
int z_ind = Mathf.FloorToInt(p.position.z / cell_size.z);
int hash = x_ind * 113 + y_ind * 12769 + z_ind * 1442897;
ParticleAggregate bucket;
if (!spatial_hash.TryGetValue(hash, out bucket))
{
bucket = new ParticleAggregate();
spatial_hash[hash] = bucket;
}
bucket.particles.Add(p);
bucket.center_of_mass += p.position * p.mass;
bucket.total_mass += p.mass;
}
foreach (KeyValuePair <int, ParticleAggregate> pair in spatial_hash)
{
pair.Value.center_of_mass /= pair.Value.total_mass;
}
return(spatial_hash);
}
public ConstraintDerivative Derivative(PhysicalParticle particle)
{
return(new ConstraintDerivative(
particle.Position.X - circlePosition.X,
particle.Position.Y - circlePosition.Y,
0));
}
private void TwoBodyJointConstraint(Func <KinematicsSystem, IConstraintSolver> solver)
{
var a = new PhysicalParticle();
var b = new PhysicalParticle();
var c = new JointConstraint(a, Vector2.UnitX, b, -Vector2.UnitX);
b.Position = 2 * Vector2.UnitX;
b.Velocity.X = 10;
system.AddParticles(a, b);
system.AddConstraints(c);
var constraintSolver = solver(system);
var integrator = new KinematicsIntegrator(system, constraintSolver)
{
MaximumTimeStep = 0.01f,
MaxStepsPerFrame = 500
};
integrator.Integrate(1);
a.Position.X.Should().BeApproximately(5, tolerance);
b.Position.X.Should().BeApproximately(7, tolerance);
a.Velocity.X.Should().BeApproximately(5, tolerance);
b.Velocity.X.Should().BeApproximately(5, tolerance);
}
public KinematicsSystem Initialize(Size area)
{
system = new KinematicsSystem();
const int boxSize = 20;
for (int i = 0; i < rows; i++)
{
double y = area.Height - boxSize * (i + 0.5);
for (int j = 0; j < rows - i; j++)
{
int x = (area.Width - boxSize * 2 * rows) / 2 + boxSize * 2 * j + boxSize * i;
var particle = new PhysicalParticle
{
Position = new Vector2(x, y),
Polygon = new Rectangle(-boxSize, -boxSize / 2, boxSize * 2, boxSize).ToPolygon(),
InertialMoment = boxSize * boxSize
};
system.AddParticles(particle);
}
}
system.AddForceField(new ConstantGravityField());
system.AddConstraints(new TwoBodyConstraint(new CollisionConstraint(), new AllPairs()));
system.AddConstraints(new RectangleBoundaryConstraint(new Rectangle(Point.Zero, area)));
return(system);
}
private void PrintStateOfBox(StringBuilder b, PhysicalParticle box)
{
const double RadsToDegress = 180 / (double)Math.PI;
b.AppendLine($" X: {box.Position.X}");
b.AppendLine($" Y: {box.Position.Y}");
b.AppendLine($" A: {box.Angle}\n");
if (debugInfoPage == 0)
{
b.AppendLine($" v_x: {box.Velocity.X}");
b.AppendLine($" v_y: {box.Velocity.Y}");
b.AppendLine($" w: {box.AngularVelocity}");
b.AppendLine($"\n F_x: {box.Force.X}");
b.AppendLine($" F_y: {box.Force.Y}");
b.AppendLine($" T: {box.Torque}");
b.AppendLine($"\nFC_x: {box.ConstraintForce.X}");
b.AppendLine($"FC_y: {box.ConstraintForce.Y}");
b.AppendLine($" TC: {box.ConstraintTorque}");
var angle = RadsToDegress * Vector2.DotProduct(box.ConstraintForce, box.Velocity) /
(box.ConstraintForce.Magnitude * box.Velocity.Magnitude);
b.AppendLine($"\n DOT: {angle}");
}
else if (debugInfoPage == 1)
{
solver.DebugInfo(b, debugInfoPage, box);
}
}
public ConstraintDerivative Derivative(PhysicalParticle particle)
{
return(new ConstraintDerivative(
ConstrainedPointPosition.X,
ConstrainedPointPosition.Y,
Vector2.Dot(ConstrainedPointPosition, OffsetDerivative)));
}
public CirclePerimeterOffcenterConstraint(PhysicalParticle particle, Vector2 circlePosition, float circleRadius, Vector2 offset)
{
this.particle = particle;
this.circlePosition = circlePosition;
this.circleRadius = circleRadius;
this.offset = offset;
}
public JointConstraint(PhysicalParticle particle1, Vector2 point1, PhysicalParticle particle2, Vector2 point2)
{
this.particle1 = particle1;
this.particle2 = particle2;
this.point1 = point1;
this.point2 = point2;
}
// Update is called once per frame
void Update()
{
if (_isDestroyed)
{
return;
}
if (TimeDestroy < MySpace.MyTime)
{
MyParticle.Stop();
GameObject g = Resources.Load <GameObject>("PhysicalParticle");
for (int i = 0; i < MyParticles.Length; i++)
{
GameObject inst = Instantiate(g);
PhysicalParticle p = inst.GetComponent <PhysicalParticle>();
p.MySpace = MySpace;
p.StartPos = MyParticle.CurPos;
float r = Random.Range(0f, 1f);
p.StartSpeed = Quaternion.Euler(0, 0, 360 * r) * SpeedDestroyParticle;
MyParticles[i] = p;
}
for (int i = 0; i < MyParticles.Length; i++)
{
MyParticles[i].IsStart = true;
}
_isDestroyed = true;
}
}
// Update is called once per frame
void Update()
{
if (Input.GetKeyUp(KeyCode.Space))
{
PhysicalParticle p = new PhysicalParticle();
p.position = new Vector3(Random.value, Random.value, Random.value);
p.size = 1f;
p.color = new Color(1, 0, 0, 0.2f);
manager.AddParticles(new PhysicalParticle[] { p }, 0, 1);
}
}
public void DebugInfo(StringBuilder b, int debugPage, PhysicalParticle particle)
{
if (debugPage == 1)
{
b.AppendLine($"Constraint Forces:\n{TotalConstraintForces?.Transpose().ToMatrixString()}");
b.AppendLine($"Lagrange Multipliers:\n{LagrangeMultipliers?.Transpose()?.ToMatrixString()}");
b.AppendLine($"Jacobian:\n{Jacobian?.ToMatrixString()}");
b.AppendLine($"Velocity:\n{Velocity?.Transpose()?.ToMatrixString()}");
b.AppendLine($"Coefficient Matrix: (det {CoefficientMatrix?.Determinant()})\n{CoefficientMatrix?.ToMatrixString()}");
b.AppendLine($"Equation Constants:\n{EquationConstants?.ToMatrixString()}");
}
}
public bool AppliesTo(PhysicalParticle obj)
{
if (particle1 == obj)
{
return(true);
}
if (particle2 == obj)
{
return(true);
}
return(false);
}
public ConstraintDerivative Derivative(PhysicalParticle particle)
{
ConstraintDerivative result = new ConstraintDerivative();
foreach (var pair in pairs)
{
if (pair.Item1 == particle || pair.Item2 == particle)
{
result += constraint.Derivative(particle, pair);
}
}
return(result);
}
public ConstraintDerivative Derivative(PhysicalParticle particle)
{
int sign = particle == particle1 ? 1 : -1;
var point = particle == particle1 ? point1 : point2;
var B = Displacement;
var dA = PointDerivativeAngle(particle, point);
var dot = Vector2.Dot(B, dA);
return(sign * new ConstraintDerivative(
B.X,
B.Y,
dot));
}
private PhysicalParticle GenerateParticle(Vector2 startPoint, Vector2 startVelocity)
{
var size = random.Next(15, 80);
var innerSize = random.NextDouble() * 0.7 * size + 1;
var particle = new PhysicalParticle
{
Polygon = GeneratePolygon(size, innerSize),
Position = startPoint,
Velocity = startVelocity,
AngularVelocity = random.NextDouble(),
InertialMoment = 0.5 * size * size,
};
return(particle);
}
public KinematicsSystem Initialize(Size area)
{
boxes.Clear();
system = new KinematicsSystem();
for (int i = 0; i < BoxCount; i++)
{
boxes.Add(new PhysicalParticle
{
Polygon = new Rectangle(-boxSize / 2, -boxSize / 2, boxSize, boxSize).ToPolygon(),
Position = new Vector2(
area.Width * 0.5 + (BoxCount / 2 - i) * boxSize,
area.Height * 0.3
),
Velocity = new Vector2(0, (BoxCount / 2 - i) * 100)
});
if (i > 0)
{
var constraint = new JointConstraint(boxes[i - 1], new Vector2(-boxSize * 0.5, 0),
boxes[i], new Vector2(boxSize * 0.5, 0));
system.AddConstraints(constraint);
}
}
sphere = new PhysicalParticle
{
Position = new Vector2(area.Width * 0.5, area.Height * 0.25)
};
var constraintBox = boxes[boxes.Count / 2];
system.AddConstraints(
new CirclePerimeterOffcenterConstraint(constraintBox,
sphere.Position, 40, constraintBox.Polygon[0]));
system.AddParticles(boxes.ToArray());
system.AddForceField(new ConstantGravityField());
return(system);
}
public ConstraintDerivative Derivative(PhysicalParticle particle, Tuple <PhysicalParticle, PhysicalParticle> pair)
{
var contactPoint = collider.FindConvexContactPoint(pair.Item1.TransformedPolygon, pair.Item2.TransformedPolygon);
if (contactPoint.Contact == false)
{
return(new ConstraintDerivative());
}
var firstParticle = particle == pair.Item1;
var sign = firstParticle ? -1 : 1;
var force = sign * contactPoint.FirstPolygonNormal;
var r = firstParticle ? contactPoint.FirstPolygonContactPoint : contactPoint.SecondPolygonContactPoint;
var torque = -Vector2X.Cross(r, force); // minus sign because r points from the center to the contact point instead of the other way.
return(new ConstraintDerivative(force.X, force.Y, torque));
}
public KinematicsSystem Initialize(Size area)
{
system = new KinematicsSystem();
var planet = new PhysicalParticle
{
Position = (Vector2)area / 2,
Polygon = new RegularPolygonBuilder().BuildPolygon(6, 60),
Mass = 10,
InertialMoment = 5 * 60 * 60,
};
system.AddParticles(planet);
GenerateParticles(planet);
system.AddConstraints(new TwoBodyConstraint(new CollisionConstraint(), new AllPairs()));
system.AddForceField(new PlanetGravity(planet, 4000000));
return(system);
}
Vector3 NaiveForceOnParticle(uint index)
{
Vector3 force = new Vector3();
PhysicalParticle p_i = physical_particles[index];
for (uint j = 0; j < index; ++j)
{
Vector3 position_difference = physical_particles[j].position - p_i.position;
float distance = position_difference.magnitude;
float pd_cubed = distance * distance * distance;
force += p_i.mass * physical_particles[j].mass / pd_cubed * position_difference;
}
for (uint j = index + 1; j < num_particles; ++j)
{
Vector3 position_difference = physical_particles[j].position - p_i.position;
float distance = position_difference.magnitude;
float pd_cubed = distance * distance * distance;
force += p_i.mass * physical_particles[j].mass / pd_cubed * position_difference;
}
return(force * GRAVITATIONAL_CONSTANT);
}
public ConstraintDerivative Derivative(PhysicalParticle particle)
{
float dx = 0;
float dy = 0;
float dphi = 0;
int pointCount = 0;
foreach (var point in particle.TransformedPolygon)
{
var magnitude = (point - particle.Position).Length();
var angle = (point - particle.Position).Angle();
//if (rotatedPoint.X < Bounds.Left)
//{
// dx -= 1;
// dphi += -Math.Sin(particle.Angle + angle);
//}
//if (rotatedPoint.X > Bounds.Right)
//{
// dx += 1;
// dphi += -Math.Sin(particle.Angle + angle);
//}
//if (rotatedPoint.Y < Bounds.Top)
//{
// dy -= 1;
// dphi += Math.Cos(particle.Angle + angle);
//}
if (point.Y > Bounds.Bottom)
{
pointCount++;
dy += 1;
dphi -= magnitude * (float)Math.Cos(angle);
}
}
return(new ConstraintDerivative(dx, dy, dphi));
}
private void ThreeBodyJointConstraint(Func <KinematicsSystem, IConstraintSolver> solver)
{
var a1 = new PhysicalParticle();
var a2 = new PhysicalParticle();
var a3 = new PhysicalParticle();
var c1 = new JointConstraint(a1, Vector2.UnitX, a2, -Vector2.UnitX);
var c2 = new JointConstraint(a2, Vector2.UnitX, a3, -Vector2.UnitX);
a2.Position = 2 * Vector2.UnitX;
a3.Position = 4 * Vector2.UnitX;
a2.Velocity.X = 15;
c1.Value(new List <PhysicalParticle> {
a1, a2
}).Should().Be(0);
c2.Value(new List <PhysicalParticle> {
a2, a3
}).Should().Be(0);
system.AddParticles(a1, a2, a3);
system.AddConstraints(c1, c2);
var constraintSolver = solver(system);
var integrator = new KinematicsIntegrator(system, constraintSolver)
{
MaximumTimeStep = 0.005f,
MaxStepsPerFrame = 1000
};
integrator.Integrate(1);
a1.Position.X.Should().BeApproximately(5, tolerance);
a2.Position.X.Should().BeApproximately(7, tolerance);
a3.Position.X.Should().BeApproximately(9, tolerance);
a1.Velocity.X.Should().BeApproximately(5, tolerance);
a2.Velocity.X.Should().BeApproximately(5, tolerance);
a3.Velocity.X.Should().BeApproximately(5, tolerance);
}
/// <summary>
/// Calculates the velocity of the particle's point, taking into account angular motion.
/// </summary>
/// <param name="particle"></param>
/// <param name="point"></param>
/// <remarks>This value is \f$\dot{\vec{R}}\f$ in the math.</remarks>
/// <returns></returns>
private Vector2 PointVelocity(PhysicalParticle particle, Vector2 point)
{
return(particle.Velocity + particle.AngularVelocity * PointDerivativeAngle(particle, point));
}
public CirclePerimeterConstraint(PhysicalParticle particle, Vector2 circlePosition, float circleRadius)
{
this.particle = particle;
this.circlePosition = circlePosition;
this.circleRadius = circleRadius;
}
public bool AppliesTo(PhysicalParticle particle)
{
throw new NotImplementedException();
}
private void GenerateParticles(PhysicalParticle planet)
{
system.AddParticles(new ParticleGenerator().Generate(particles,
i => planet.Position + Vector2.FromPolar(300, 2 * Math.PI * i / particles),
i => Vector2.FromPolar(50, 2 * Math.PI * i / particles + Math.PI * 0.5)));
}
public bool AppliesTo(PhysicalParticle particle)
{
return(ReferenceEquals(this.particle, particle));
}
public PlanetGravity(PhysicalParticle source, double gravity)
{
this.source = source;
this.gravity = gravity;
}
Vector3 HashingForce(Dictionary <int, ParticleAggregate> spatial_hash, ParticleAggregate curr_bucket, PhysicalParticle particle, float deltaT)
{
ParticleAggregate other_bucket;
Vector3 total_force = Vector3.zero;
foreach (KeyValuePair <int, ParticleAggregate> other_cell in spatial_hash)
{
other_bucket = other_cell.Value;
if (curr_bucket == other_bucket)
{
continue;
}
Vector3 position_difference = other_bucket.center_of_mass - particle.position;
float distance = position_difference.magnitude;
if (distance < MIN_DISTANCE)
{
continue;
}
float pd_cubed = distance * distance * distance;
total_force += other_bucket.total_mass * particle.mass / pd_cubed * position_difference;
}
foreach (PhysicalParticle other_particle in curr_bucket.particles)
{
if (other_particle == particle)
{
continue;
}
Vector3 position_difference = other_particle.position - particle.position;
float distance = position_difference.magnitude;
if (distance < MIN_DISTANCE)
{
continue;
}
float pd_cubed = distance * distance * distance;
total_force += other_particle.mass * particle.mass / pd_cubed * position_difference;
}
total_force *= GRAVITATIONAL_CONSTANT;
//Debug.Log(total_force);
return(total_force);
}
void ApplyForce(PhysicalParticle particle, Vector3 total_force, float dt)
{
particle.position += particle.velocity * dt;
particle.velocity += total_force / particle.mass * dt;
}
