/// <inheritdoc />
public void Apply(ReadOnlyArrayView <Body> bodies)
{
for (int i = 0; i < _count; i++)
{
bodies[_indices[i]].Position.AddForce(_deltas[i]);
}
}
public void indexer_shouldGetReadOnlyRefToElement()
{
int[] arr = new int[100];
var view = new ReadOnlyArrayView <int>(arr);
Assert.True(Unsafe.AreSame(ref arr[0], ref Unsafe.AsRef(in view[0])));
/// <summary>
///
/// </summary>
public static void UnitizeColumns(ReadOnlyArrayView <Vector2d> matrix, ArrayView <Vector2d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = matrix[i].Unit;
}
}
/// <summary>
///
/// </summary>
private void CalculateImpl(ReadOnlyArrayView <Body> bodies, ReadOnlyArrayView <int> indices, ArrayView <Vector3d> deltas)
{
var diam = _radius * 2.0;
var diamSqr = diam * diam;
for (int i = 0; i < indices.Count; i++)
{
var p0 = bodies[indices[i]].Position.Current;
var sum = Vector3d.Zero;
var count = 0;
foreach (var p1 in _grid.Search(new Interval3d(p0, diam)))
{
var d = p1 - p0;
var m = d.SquareLength;
if (m < diamSqr && m > 0.0)
{
sum += d * (1.0 - diam / Math.Sqrt(m));
count++;
}
}
deltas[i] = count > 0 ? sum * (Strength / count) : Vector3d.Zero; // average for stability
}
}
/// <summary>
///
/// </summary>
public static void DividePointwise(ReadOnlyArrayView <Vector2d> m0, ReadOnlyArrayView <Vector2d> m1, ArrayView <Vector2d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i] / m1[i];
}
}
/// <summary>
/// result = m0 + (m1 - m2) * t
/// Note that t is assumed to represent the diagonal elements of a square matrix.
/// </summary>
public static void AddScaledDelta(ReadOnlyArrayView <Vector2d> m0, ReadOnlyArrayView <Vector2d> m1, ReadOnlyArrayView <Vector2d> m2, ReadOnlyArrayView <double> t, ArrayView <Vector2d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i] + (m1[i] - m2[i]) * t[i];
}
}
/// <summary>
/// Note that the given vector is assumed to represent the diagonal elements of a square matrix.
/// </summary>
public static void Multiply(ReadOnlyArrayView <Vector3d> matrix, ReadOnlyArrayView <double> vector, ArrayView <Vector3d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = matrix[i] * vector[i];
}
}
/// <summary>
///
/// </summary>
public static void Multiply(ReadOnlyArrayView <Vector2d> matrix, double scalar, ArrayView <Vector2d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = matrix[i] * scalar;
}
}
/// <inheritdoc />
public void Apply(ReadOnlyArrayView <Body> bodies)
{
if (_apply)
{
bodies[_index].Position.AddDelta(_delta, Weight);
}
}
/// <summary>
///
/// </summary>
public static void Add(ReadOnlyArrayView <Vector3d> m0, ReadOnlyArrayView <Vector3d> m1, ArrayView <Vector3d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i] + m1[i];
}
}
/// <inheritdoc />
protected override bool Calculate(ReadOnlyArrayView <Body> bodies, ReadOnlyArrayView <int> indices, ArrayView <Vector3d> deltas)
{
int n = indices.Count;
// check if sufficient number of bodies
if (n < 3)
{
deltas.Clear();
return(false);
}
var sum = new Vector3d();
for (int i = 1; i < n; i++)
{
sum += bodies[indices[i]].Position.Current;
}
var t = 1.0 / (n - 1);
var d = (sum * t - bodies[indices[0]].Position.Current) * 0.5;
// apply projection to center
deltas[0] = d;
d *= -t;
// distribute reverse among 1 ring
for (int i = 1; i < n; i++)
{
deltas[i] = d;
}
return(true);
}
/// <inheritdoc />
public void Calculate(ReadOnlyArrayView <Body> bodies)
{
Vector3d p0 = bodies[_i0].Position.Current;
Vector3d p1 = bodies[_i1].Position.Current;
Vector3d p2 = bodies[_i2].Position.Current;
Vector3d p3 = bodies[_i3].Position.Current;
var rotation = AxisAngle3d.Identity;
rotation.Axis = p1 - p0;
var d2 = p2 - p0;
var d3 = p3 - p0;
var angle = Geometry.GetDihedralAngle(rotation.Axis, Vector3d.Cross(rotation.Axis, d2), Vector3d.Cross(rotation.Axis, -d3)) + Math.PI;
rotation.Angle = GetMinAngleDifference(_target, angle) * 0.5;
// calculate deltas as diff bw current and rotated
_d2 = (rotation.Inverse.Apply(d2) - d2) * 0.5;
_d3 = (rotation.Apply(d3) - d3) * 0.5;
// distribute reverse projection among hinge bodies
_d0 = _d1 = (_d2 + _d3) * -0.5;
}
public void asSpan_shouldGetReadOnlySpanOverView()
{
int[] arr = new int[100];
Assert.True((new ReadOnlyArrayView <int>(arr)).asSpan() == arr.AsSpan());
Assert.True((new ReadOnlyArrayView <int>(arr)).asSpan(10) == arr.AsSpan(10));
Assert.True((new ReadOnlyArrayView <int>(arr)).asSpan(10, 30) == arr.AsSpan(10, 30));
Assert.True((new ReadOnlyArrayView <int>(arr)).asSpan(100).IsEmpty);
Assert.True((new ReadOnlyArrayView <int>(arr)).asSpan(100, 0).IsEmpty);
var subView = new ReadOnlyArrayView <int>(arr, 10, 30);
Assert.True(subView.asSpan() == arr.AsSpan(10, 30));
Assert.True(subView.asSpan(10) == arr.AsSpan(20, 20));
Assert.True(subView.asSpan(10, 15) == arr.AsSpan(20, 15));
Assert.True(subView.asSpan(30).IsEmpty);
Assert.True(subView.asSpan(30, 0).IsEmpty);
var subSubView = new ReadOnlyArrayView <int>(subView, 5, 20);
Assert.True(subSubView.asSpan() == arr.AsSpan(15, 20));
Assert.True(subSubView.asSpan(10) == arr.AsSpan(25, 10));
Assert.True(subSubView.asSpan(10, 5) == arr.AsSpan(25, 5));
Assert.True(subSubView.asSpan(20).IsEmpty);
Assert.True(subSubView.asSpan(20, 0).IsEmpty);
}
/// <summary>
///
/// </summary>
public static void Add(ReadOnlyArrayView <Vector2d> matrix, double scalar, ArrayView <Vector2d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = matrix[i] + new Vector2d(scalar);
}
}
/// <summary>
///
/// </summary>
public static void MultiplyPointwise(ReadOnlyArrayView <Vector3d> m0, ReadOnlyArrayView <Vector3d> m1, ArrayView <Vector3d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i] * m1[i];
}
}
/// <summary>
///
/// </summary>
public static void Subtract(ReadOnlyArrayView <Vector2d> m0, ReadOnlyArrayView <Vector2d> m1, ArrayView <Vector2d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i] - m1[i];
}
}
/// <summary>
/// result = m0 * t0 + m1 * t1
/// </summary>
public static void AddScaled(ReadOnlyArrayView <Vector3d> m0, double t0, ReadOnlyArrayView <Vector3d> m1, double t1, ArrayView <Vector3d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i] * t0 + m1[i] * t1;
}
}
/// <summary>
/// Note that the given vector is assumed to represent the diagonal elements of a square matrix.
/// </summary>
public static void Divide(ReadOnlyArrayView <Vector2d> matrix, ReadOnlyArrayView <double> vector, ArrayView <Vector2d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = matrix[i] / vector[i];
}
}
/// <summary>
///
/// </summary>
public static void Lerp(ReadOnlyArrayView <Vector3d> m0, ReadOnlyArrayView <Vector3d> m1, double t, ArrayView <Vector3d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i].LerpTo(m1[i], t);
}
}
/// <summary>
/// result = m0 * t0 + m1 * t1
/// Note that t0 and t1 are assumed to represent the diagonal elements of a square matrix.
/// </summary>
public static void AddScaled(ReadOnlyArrayView <Vector2d> m0, ReadOnlyArrayView <double> t0, ReadOnlyArrayView <Vector2d> m1, ReadOnlyArrayView <double> t1, ArrayView <Vector2d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i] * t0[i] + m1[i] * t1[i];
}
}
/// <summary>
///
/// </summary>
public static void EvaluateColumns(ReadOnlyArrayView <Vector3d> matrix, Interval3d interval, ArrayView <Vector3d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = interval.Evaluate(matrix[i]);
}
}
/// <summary>
/// result = m0 + (m1 - m0) * t
/// Note that t is assumed to represent the diagonal elements of a square matrix.
/// </summary>
public static void LerpColumns(ReadOnlyArrayView <Vector2d> m0, ReadOnlyArrayView <Vector2d> m1, ReadOnlyArrayView <double> t, ArrayView <Vector2d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = m0[i].LerpTo(m1[i], t[i]);
}
}
/// <summary>
///
/// </summary>
public static void RemapColumns(ReadOnlyArrayView <Vector3d> matrix, Interval3d from, Interval3d to, ArrayView <Vector3d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = Interval3d.Remap(matrix[i], from, to);
}
}
/// <summary>
///
/// </summary>
public static void NormalizeColumns(ReadOnlyArrayView <Vector2d> matrix, Interval2d interval, ArrayView <Vector2d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = interval.Normalize(matrix[i]);
}
}
/// <summary>
///
/// </summary>
public static void Abs(ReadOnlyArrayView <Vector2d> matrix, ArrayView <Vector2d> result)
{
for (int i = 0; i < matrix.Count; i++)
{
result[i] = Vector2d.Abs(matrix[i]);
}
}
/// <inheritdoc />
protected override void Calculate(ReadOnlyArrayView <Body> bodies, ReadOnlyArrayView <int> indices, ArrayView <Vector3d> deltas)
{
if (_grid == null)
{
_grid = new HashGrid3d <Vector3d>(indices.Count);
}
// update grid
_grid.Scale = Radius * _radiusToGridScale;
// insert body positions
for (int i = 0; i < indices.Count; i++)
{
var p = bodies[indices[i]].Position.Current;
_grid.Insert(p, p);
}
// search from each body position
if (_parallel)
{
ForEach(Partitioner.Create(0, indices.Count), range =>
{
var i = range.Item1;
var n = range.Item2 - i;
CalculateImpl(bodies, indices.Subview(i, n), deltas.Subview(i, n));
});
}
else
{
CalculateImpl(bodies, indices, deltas);
}
_grid.Clear();
}
/// <summary>
///
/// </summary>
public static void Min(ReadOnlyArrayView <Vector2d> m0, ReadOnlyArrayView <Vector2d> m1, ArrayView <Vector2d> result)
{
for (int i = 0; i < m0.Count; i++)
{
result[i] = Vector2d.Min(m0[i], m1[i]);
}
}
/// <inheritdoc />
public void Apply(ReadOnlyArrayView <Body> bodies)
{
bodies[_i0].Position.AddDelta(_d0, Weight);
bodies[_i1].Position.AddDelta(_d1, Weight);
bodies[_i2].Position.AddDelta(_d2, Weight);
bodies[_i3].Position.AddDelta(_d3, Weight);
}
/// <summary>
///
/// </summary>
private void UpdateBodies(ReadOnlyArrayView <Body> bodies)
{
_maxLinearSpeedSqr = _maxAngularSpeedSqr = 0.0;
var timeStep = _settings.TimeStep;
var linDamp = _settings.LinearDamping;
var angDamp = _settings.AngularDamping;
for (int i = 0; i < bodies.Count; i++)
{
(var bp, var br) = bodies[i];
if (bp != null)
{
bp.Update(timeStep, linDamp);
_maxLinearSpeedSqr = Math.Max(bp.Velocity.SquareLength, _maxLinearSpeedSqr);
}
if (br != null)
{
br.Update(timeStep, angDamp);
_maxAngularSpeedSqr = Math.Max(br.Velocity.SquareLength, _maxAngularSpeedSqr);
}
}
}
