/// <summary>
/// Calculates the gravitational force that exists between this body and another gravitational body.
/// </summary>
/// <returns>Force (Vector) directed toward this body, i.e. if the other body has higher gravitational strength,
/// the force will be negative (directed away from this body)</returns>
public Vector GravitationalForceToward(GravitationalBody b)
{
double distance = Math.Abs(Position.Abs() - b.Position.Abs());
if (distance < Radius || distance < b.Radius)
{
throw new OverLappingRadiusException("Bodies must not be connected!");
}
Vector direction = b.Position - Position;
double r_squared = direction.Abs() * direction.Abs();
return(direction.Normalised() * Constants.Common.G * Mass * b.Mass / r_squared);
}
/// <summary>
/// Ctor.
/// </summary>
/// <param name="GravityDirection">Unit vector for spatial direction of gravity.</param>
/// <param name="SpatialComponent">Spatial component of source.</param>
/// <param name="Froude">Dimensionless Froude number.</param>
/// <param name="physicsMode"></param>
/// <param name="EoS">Equation of state for calculating density.</param>
public Buoyancy(Vector GravityDirection, int SpatialComponent, double Froude, PhysicsMode physicsMode, MaterialLaw EoS)
{
// Check direction
if ((GravityDirection.Abs() - 1.0) > 1.0e-13)
{
throw new ArgumentException("Length of GravityDirection vector has to be 1.0");
}
// Initialize
this.GravityDirection = GravityDirection;
this.SpatialComponent = SpatialComponent;
this.Froude = Froude;
this.EoS = EoS;
this.physicsMode = physicsMode;
switch (physicsMode)
{
case PhysicsMode.LowMach:
this.m_ParameterOrdering = new string[] { VariableNames.Temperature0 };
break;
case PhysicsMode.Combustion:
this.m_ParameterOrdering = new string[] { VariableNames.Temperature0, VariableNames.MassFraction0_0, VariableNames.MassFraction1_0, VariableNames.MassFraction2_0, VariableNames.MassFraction3_0 };
break;
default:
throw new ApplicationException("wrong physicsmode");
}
}
public static int VectorExp(Vector <T> x, T checkValue, T epsilon, T allowableError)
{
Vector <T> sum = Vector <T> .One;
Vector <T> count = Vector <T> .One;
Vector <T> term = x;
Vector <T> epsilonVec = new Vector <T>(epsilon);
do
{
if (Vector.LessThanOrEqualAll <T>(Vector.Abs(term), epsilonVec))
{
break;
}
sum = sum + term;
count = count + Vector <T> .One;
term = term * (x / count);
}while (true);
if (Vector.LessThanOrEqualAll <T>((Vector.Abs(sum) - new Vector <T>(checkValue)), new Vector <T>(allowableError)))
{
return(Pass);
}
else
{
Console.WriteLine("Failed " + typeof(T).Name);
VectorPrint(" sum: ", sum);
return(Fail);
}
}
public bool AlmostEquals(Matrix otherMatrix, double precision)
{
if ((this.rows != otherMatrix.rows) || (this.columns != otherMatrix.columns))
{
return(false);
}
for (int i = 0; i < this.rows; i++)
{
int j = 0;
for (; j < this.columns - Vector <double> .Count; j += Vector <double> .Count)
{
var diff = Vector.Abs <double>((new Vector <double>(this.matrix[i], j) - new Vector <double>(otherMatrix.matrix[i], j)));
for (int k = 0; k < Vector <double> .Count; k++)
{
if (diff[k] > precision)
{
return(false);
}
}
}
for (; j < this.columns; j++)
{
if (System.Math.Abs(this.matrix[i][j] - otherMatrix.matrix[i][j]) > precision)
{
return(false);
}
}
}
return(true);
}
private static Vector <double> ImplicitTrapezoid(double tmax, double T, Vector <double> start,
Vector <double> prediction,
Matrix <double> A,
Matrix <double> B, Func <double, double> r, bool verbose = false)
{
Console.WriteLine(
$"\nIMPLICIT TRAPEZOID(tmax: {tmax}, T: {T}, prediction: [{string.Join(' ', prediction.ToArray())}])");
var x = VectorBuilder.Dense(prediction.Count);
var cumulativeError = VectorBuilder.Dense(A.RowCount);
for (var t = T; t <= tmax; t += T)
{
var pendulum = Generator.GeneratePendulum(start, t);
x += 0.5 * T * (A * x + B * VectorBuilder.Dense(new[] { r(t - T), r(t - T) }) + A * prediction +
B * VectorBuilder.Dense(new[] { r(t), r(t) }));
cumulativeError += Vector <double> .Abs(x - pendulum);
if (verbose)
{
Console.WriteLine(
$"t: {t:F2} -- x: [{string.Join(' ', x.ToArray())}] -- pendulum: {string.Join(' ', pendulum.ToArray())}]");
}
}
Console.WriteLine($"Cumulative error: [{string.Join(' ', cumulativeError.ToArray())}]");
return(x);
}
public static float AbsDiffMax(float[] first, int firstIndex, float[] second, int secondIndex, int length)
{
var remainderMax = 0.0f;
var vectorMax = Vector <float> .Zero;
if ((firstIndex | secondIndex) == 0)
{
int highestForVector = length - Vector <float> .Count;
for (int i = 0; i <= highestForVector; i += Vector <float> .Count)
{
vectorMax = Vector.Max(Vector.Abs(new Vector <float>(first, i) - new Vector <float>(second, i)), vectorMax);
}
}
else
{
int highestForVector = length - Vector <float> .Count + firstIndex;
int s = secondIndex;
for (int f = 0; f <= highestForVector; f += Vector <float> .Count)
{
vectorMax = Vector.Max(Vector.Abs(new Vector <float>(first, f) - new Vector <float>(second, s)), vectorMax);
s += Vector <float> .Count;
}
}
// copy the remainder
for (int i = length - (length % Vector <float> .Count); i < length; i++)
{
remainderMax = Math.Max(remainderMax, Math.Abs(first[i + firstIndex] - second[i + secondIndex]));
}
float[] temp = new float[Vector <float> .Count];
for (int i = 0; i < temp.Length; i++)
{
remainderMax = Math.Max(temp[i], remainderMax);
}
return(remainderMax);
}
public void Fn(double[] v, double[] w, int startIdx, int endIdx)
{
// Init constants.
var vecTwo = new Vector <double>(2.0);
var vecFifth = new Vector <double>(0.2);
var vecHalf = new Vector <double>(0.5);
int width = Vector <double> .Count;
int i = startIdx;
for (; i <= endIdx - width; i += width)
{
// Load values into a vector.
var vec = new Vector <double>(v, i);
// Calc the softsign sigmoid.
vec = vecHalf + (vec / (vecTwo * (vecFifth + Vector.Abs(vec))));
// Copy the final result into w.
vec.CopyTo(w, i);
}
// Handle vectors with lengths not an exact multiple of vector width.
for (; i < endIdx; i++)
{
w[i] = Fn(v[i]);
}
}
private List <PointTempExtension> findConnectedPoints(ICollection <Line> Line, Point original, Line originalLine)
{
List <PointTempExtension> result = new List <PointTempExtension>();
foreach (Line l in Line)
{
if (l.Type >= 3 && l.Type <= 10 && (l.A.Id == original.Id || l.B.Id == original.Id || l.O.Id == original.Id))
{
result.Add(new PointTempExtension(l.A));
result.Add(new PointTempExtension(l.B));
result.Add(new PointTempExtension(l.O));
}
else if (l.Type == (int)LineType.Point && originalLine.Type >= 3 && originalLine.Type <= 6)
{
Vector mid = Vector.Middle(new Vector(originalLine.A.longitude, originalLine.A.latitude, originalLine.A.altitude), new Vector(originalLine.B.longitude, originalLine.B.latitude, originalLine.B.altitude));
if (Vector.Abs(mid - new Vector(l.A.longitude, l.A.latitude, l.A.altitude)) < 0.01)
{
result.Add(new PointTempExtension(l.A));
result.Add(new PointTempExtension(l.B));
result.Add(new PointTempExtension(l.O));
result.AddRange(findGluePoints(Line, new PointTempExtension(l.O)));
}
}
}
return(result);
}
public void Fn(double[] v, double[] w, int startIdx, int endIdx)
{
// Constants.
Vector <double> vec_4_9 = new Vector <double>(4.9);
Vector <double> vec_0_555 = new Vector <double>(0.555);
Vector <double> vec_0_143 = new Vector <double>(0.143);
int width = Vector <double> .Count;
int i = startIdx;
for (; i <= endIdx - width; i += width)
{
// Load values into a vector.
var vec = new Vector <double>(v, i);
vec *= vec_4_9;
var vec_x2 = vec * vec;
var vec_e = Vector <double> .One + Vector.Abs(vec) + (vec_x2 * vec_0_555) + (vec_x2 * vec_x2 * vec_0_143);
var vec_e_recip = Vector <double> .One / vec_e;
var vec_f_select = Vector.GreaterThan(vec, Vector <double> .Zero);
var vec_f = Vector.ConditionalSelect(vec_f_select, vec_e_recip, vec_e);
var vec_result = Vector <double> .One / (Vector <double> .One + vec_f);
// Copy the result back into arr.
vec_result.CopyTo(w, i);
}
// Handle vectors with lengths not an exact multiple of vector width.
for (; i < endIdx; i++)
{
w[i] = Fn(v[i]);
}
}
/// <summary>
/// Computes the quaternion rotation between two normalized vectors.
/// </summary>
/// <param name="v1">First unit-length vector.</param>
/// <param name="v2">Second unit-length vector.</param>
/// <param name="q">Quaternion representing the rotation from v1 to v2.</param>
public static void GetQuaternionBetweenNormalizedVectors(ref Vector3Wide v1, ref Vector3Wide v2, out QuaternionWide q)
{
Vector3Wide.Dot(ref v1, ref v2, out var dot);
//For non-normal vectors, the multiplying the axes length squared would be necessary:
//float w = dot + Sqrt(v1.LengthSquared() * v2.LengthSquared());
//There exists an ambiguity at dot == -1. If the directions point away from each other, there are an infinite number of shortest paths.
//One must be chosen arbitrarily. Here, we choose one by projecting onto the plane whose normal is associated with the smallest magnitude.
//Since this is a SIMD operation, the special case is always executed and its result is conditionally selected.
Vector3Wide.CrossWithoutOverlap(ref v1, ref v2, out var cross);
var useNormalCase = Vector.GreaterThan(dot, new Vector <float>(-0.999999f));
var absX = Vector.Abs(v1.X);
var absY = Vector.Abs(v1.Y);
var absZ = Vector.Abs(v1.Z);
var xIsSmallest = Vector.BitwiseAnd(Vector.LessThan(absX, absY), Vector.LessThan(absX, absZ));
var yIsSmaller = Vector.LessThan(absY, absZ);
q.X = Vector.ConditionalSelect(useNormalCase, cross.X, Vector.ConditionalSelect(xIsSmallest, Vector <float> .Zero, Vector.ConditionalSelect(yIsSmaller, -v1.Z, -v1.Y)));
q.Y = Vector.ConditionalSelect(useNormalCase, cross.Y, Vector.ConditionalSelect(xIsSmallest, -v1.Z, Vector.ConditionalSelect(yIsSmaller, Vector <float> .Zero, v1.X)));
q.Z = Vector.ConditionalSelect(useNormalCase, cross.Z, Vector.ConditionalSelect(xIsSmallest, v1.Y, Vector.ConditionalSelect(yIsSmaller, v1.X, Vector <float> .Zero)));
q.W = Vector.ConditionalSelect(useNormalCase, dot + Vector <float> .One, Vector <float> .Zero);
Normalize(ref q, out q);
}
static void TestEndpointNormal(ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength, ref Vector3Wide endpoint,
ref BoxWide box, out Vector <float> depth, out Vector3Wide normal)
{
Vector3Wide clamped;
clamped.X = Vector.Min(box.HalfWidth, Vector.Max(-box.HalfWidth, endpoint.X));
clamped.Y = Vector.Min(box.HalfHeight, Vector.Max(-box.HalfHeight, endpoint.Y));
clamped.Z = Vector.Min(box.HalfLength, Vector.Max(-box.HalfLength, endpoint.Z));
Vector3Wide.Subtract(endpoint, clamped, out normal);
Vector3Wide.Length(normal, out var length);
var inverseLength = Vector <float> .One / length;
Vector3Wide.Scale(normal, inverseLength, out normal);
//The dot between the offset from B to A and the normal gives us the center offset.
//The dot between the capsule axis and normal gives us the (unscaled) extent of the capsule along the normal.
//The depth is (boxExtentAlongNormal + capsuleExtentAlongNormal) - separationAlongNormal.
Vector3Wide.Dot(offsetA, normal, out var baN);
Vector3Wide.Dot(capsuleAxis, normal, out var daN);
depth =
Vector.Abs(normal.X) * box.HalfWidth + Vector.Abs(normal.Y) * box.HalfHeight + Vector.Abs(normal.Z) * box.HalfLength +
Vector.Abs(daN * capsuleHalfLength) -
Vector.Abs(baN);
//If the endpoint doesn't generate a valid normal due to containment, ignore the depth result.
depth = Vector.ConditionalSelect(Vector.GreaterThan(length, new Vector <float>(1e-10f)), depth, new Vector <float>(float.MaxValue));
}
//---------------------------------------------------------------------
//[Benchmark]
public double Simd()
{
double delta = 0;
double[] tmp = _values;
double avg = this.Mean;
int i = 0;
if (Vector.IsHardwareAccelerated && this.Count >= Vector<double>.Count * 2)
{
var avgVec = new Vector<double>(avg);
var deltaVec = new Vector<double>(0);
for (; i < tmp.Length - 2 * Vector<double>.Count; i += Vector<double>.Count)
{
var tmpVec = new Vector<double>(tmp, i);
deltaVec += Vector.Abs(tmpVec - avgVec);
i += Vector<double>.Count;
tmpVec = new Vector<double>(tmp, i);
deltaVec += Vector.Abs(tmpVec - avgVec);
}
for (int j = 0; j < Vector<double>.Count; ++j)
delta += deltaVec[j];
}
for (; i < tmp.Length; ++i)
delta += Math.Abs(tmp[i] - avg);
return delta / tmp.Length;
}
private static double SumForMeanDeviationToDoubleSseInner(this float[] arrayToSum, int l, int r, double average)
{
var averageVector = new Vector <double>(average);
var sumVectorLower = new Vector <double>();
var sumVectorUpper = new Vector <double>();
int sseIndexEnd = l + ((r - l + 1) / Vector <float> .Count) * Vector <float> .Count;
int i;
for (i = l; i < sseIndexEnd; i += Vector <float> .Count)
{
var inVector = new Vector <float>(arrayToSum, i);
Vector.Widen(inVector, out Vector <double> doubleLower, out Vector <double> doubleUpper);
sumVectorLower += Vector.Abs(doubleLower - averageVector);
sumVectorUpper += Vector.Abs(doubleLower - averageVector);
}
double overallSum = 0;
for (; i <= r; i++)
{
overallSum += Math.Abs(arrayToSum[i] - average);
}
sumVectorLower += sumVectorUpper;
for (i = 0; i < Vector <float> .Count; i++)
{
overallSum += sumVectorLower[i];
}
return(overallSum);
}
// For text version
public int[] MoveCommand(Vector move)
{
Vector diff = Vector.Subtract(move, Position);
int dir = diff.AngleDeg() - Direction;
int steps = diff.Abs().Sum();
switch (Utils.To360Deg(dir))
{
case 90:
dir = 1;
break;
case 180:
dir = 3;
break;
case 270:
dir = 2;
break;
default:
dir = 0;
break;
}
return(new int[] { steps, dir });
}
static void TestBoxFace(ref Vector <float> offsetAZ,
ref Vector <float> capsuleAxisZ, ref Vector <float> capsuleHalfLength,
ref Vector <float> boxHalfLength,
out Vector <float> depth, out Vector <float> normalSign)
{
normalSign = Vector.ConditionalSelect(Vector.GreaterThan(offsetAZ, Vector <float> .Zero), Vector <float> .One, new Vector <float>(-1f));
depth = boxHalfLength + Vector.Abs(capsuleAxisZ) * capsuleHalfLength - normalSign * offsetAZ;
}
static void TestFace(ref Vector <float> halfLengthA,
ref Vector <float> halfWidthB, ref Vector <float> halfHeightB, ref Vector <float> halfLengthB,
ref Vector <float> offsetBZ,
ref Vector3Wide rBX, ref Vector3Wide rBY, ref Vector3Wide rBZ,
out Vector <float> depth)
{
depth = halfLengthA + halfWidthB * Vector.Abs(rBX.Z) + halfHeightB * Vector.Abs(rBY.Z) + halfLengthB * Vector.Abs(rBZ.Z) - Vector.Abs(offsetBZ);
}
public void Abs()
{
var vector = new Vector(1, 2, 3);
var expected = Math.Sqrt(14);
var actual = vector.Abs();
Assert.AreEqual(expected, actual, 0.000001);
}
public static Vector <float> SelectIfFiniteAndLessThan(Vector <float> baseValues, Vector <float> alternateValues, Vector <float> minimumV)
{
//If it is greater than the maximum value it is infinite, if it is not equal to itself it is NaN
return(Vector.ConditionalSelect(
Vector.BitwiseAnd(Vector.BitwiseAnd(Vector.LessThanOrEqual(Vector.Abs(baseValues), MaxFloat),
Vector.GreaterThanOrEqual(baseValues, baseValues)), Vector.GreaterThanOrEqual(baseValues, minimumV)),
baseValues, alternateValues
));
}
public static Vector <float> SelectIfFinite(Vector <float> baseValues, Vector <float> alternateValues)
{
Vector <float> max = new Vector <float>(float.MaxValue);
//If it is greater than the maximum value it is infinite, if it is not equal to itself it is NaN
return(Vector.ConditionalSelect(
Vector.BitwiseAnd(Vector.LessThanOrEqual(Vector.Abs(baseValues), max), Vector.GreaterThanOrEqual(baseValues, baseValues)),
baseValues, alternateValues
));
}
private Vector <float> FastTanh(Vector <float> x)
{
var ax = Vector.Abs <float>(x);
var x2 = x * x;
return(x * (new Vector <float>(2.45550750702956f) + new Vector <float>(2.45550750702956f) * ax +
(new Vector <float>(0.893229853513558f) + new Vector <float>(0.821226666969744f) * ax) * x2) /
(new Vector <float>(2.44506634652299f) + (new Vector <float>(2.44506634652299f) + x2) *
Vector.Abs <float>(x + new Vector <float>(0.814642734961073f) * x * ax)));
}
public unsafe double ParallelizedUnsafeSimd()
{
double delta = 0;
var sync = new object();
Partitioner<Tuple<int, int>> partitioner = Partitioner.Create(0, _values.Length);
#if SEQUENTIAL
var range = Tuple.Create(0, _values.Length);
#else
Parallel.ForEach(
partitioner,
range =>
{
#endif
double localDelta = 0;
double avg = this.Mean;
int n = range.Item2;
fixed (double* pArray = _values)
{
int i = range.Item1;
//double* arr = &pArray[i];
double* arr = pArray + i; // is the same as above
if (Vector.IsHardwareAccelerated && (n - i) >= Vector<double>.Count * 2)
{
var avgVec = new Vector<double>(avg);
var deltaVec = new Vector<double>(0);
for (; i < n - 2 * Vector<double>.Count; i += 2 * Vector<double>.Count)
{
Vector<double> vec = Unsafe.Read<Vector<double>>(arr);
deltaVec += Vector.Abs(vec - avgVec);
arr += Vector<double>.Count;
vec = Unsafe.Read<Vector<double>>(arr);
deltaVec += Vector.Abs(vec - avgVec);
arr += Vector<double>.Count;
}
for (int j = 0; j < Vector<double>.Count; ++j)
localDelta += deltaVec[j];
}
for (; i < n; ++i)
localDelta += Math.Abs(pArray[i] - avg);
}
lock (sync) delta += localDelta;
#if !SEQUENTIAL
}
);
#endif
return delta / _values.Length;
}
private static double SumForDeviationMostAccurateSseInner(this double[] arrayToSum, int l, int r, double average)
{
var averageVector = new Vector <double>(average);
var sumVector = new Vector <double>();
var cVector = new Vector <double>();
int sseIndexEnd = l + ((r - l + 1) / Vector <double> .Count) * Vector <double> .Count;
int i;
for (i = l; i < sseIndexEnd; i += Vector <double> .Count)
{
var inVector = new Vector <double>(arrayToSum, i);
var subVector = inVector - averageVector;
var subVectorSqrd = subVector * subVector;
var tVector = sumVector + subVectorSqrd;
Vector <long> gteMask = Vector.GreaterThanOrEqual(Vector.Abs(sumVector), Vector.Abs(subVectorSqrd)); // if true then 0xFFFFFFFFFFFFFFFFL else 0L at each element of the Vector<long>
cVector += Vector.ConditionalSelect(gteMask, sumVector, subVectorSqrd) - tVector + Vector.ConditionalSelect(gteMask, subVectorSqrd, sumVector);
sumVector = tVector;
}
int iLast = i;
// At this point we have sumVector and cVector, which have Vector<double>.Count number of sum's and c's
// Reduce these Vector's to a single sum and a single c
double sum = 0.0;
double c = 0.0;
for (i = 0; i < Vector <double> .Count; i++)
{
double t = sum + sumVector[i];
if (Math.Abs(sum) >= Math.Abs(sumVector[i]))
{
c += (sum - t) + sumVector[i]; // If sum is bigger, low-order digits of input[i] are lost.
}
else
{
c += (sumVector[i] - t) + sum; // Else low-order digits of sum are lost
}
sum = t;
c += cVector[i];
}
for (i = iLast; i <= r; i++)
{
double inValueSubSqrd = (arrayToSum[i] - average) * (arrayToSum[i] - average);
double t = sum + inValueSubSqrd;
if (Math.Abs(sum) >= Math.Abs(inValueSubSqrd))
{
c += (sum - t) + inValueSubSqrd; // If sum is bigger, low-order digits of input[i] are lost.
}
else
{
c += (inValueSubSqrd - t) + sum; // Else low-order digits of sum are lost
}
sum = t;
}
return(sum + c);
}
static void TestBoxFace(ref Vector <float> offsetAZ,
ref Vector <float> capsuleAxisZ, ref Vector <float> capsuleHalfLength,
ref Vector <float> tCandidateMinX, ref Vector <float> tCandidateMaxX, ref Vector <float> tCandidateMinY, ref Vector <float> tCandidateMaxY,
ref Vector <float> boxHalfLength,
out Vector <float> depth, out Vector <float> taMin, out Vector <float> taMax, out Vector <float> normalSign)
{
taMin = Vector.Max(tCandidateMinX, tCandidateMinY);
taMax = Vector.Min(tCandidateMaxX, tCandidateMaxY);
normalSign = Vector.ConditionalSelect(Vector.GreaterThan(offsetAZ, Vector <float> .Zero), Vector <float> .One, new Vector <float>(-1f));
depth = boxHalfLength + Vector.Abs(capsuleAxisZ) * capsuleHalfLength - normalSign * offsetAZ;
}
private static unsafe void ISP1Core2(double *arr, int offset, Vector <double> avgVec, ref Vector <double> deltaVec0, ref Vector <double> deltaVec1, double *end)
{
Vector <double> vec0 = VectorHelper.GetVectorUnaligned(arr + offset + 0 * Vector <double> .Count);
Vector <double> vec1 = VectorHelper.GetVectorUnaligned(arr + offset + 1 * Vector <double> .Count);
vec0 -= avgVec;
vec1 -= avgVec;
deltaVec0 += Vector.Abs(vec0);
deltaVec1 += Vector.Abs(vec1);
}
/// <summary>
///
/// </summary>
public static void Abs(this ISampledField <double> f0, ISampledField <double> result, bool parallel = false)
{
if (parallel)
{
Vector.Parallel.Abs(f0.Values, result.Values);
}
else
{
Vector.Abs(f0.Values, result.Values);
}
}
public int SIMDSorted()
{
var length = Vector <int> .Count;
for (var i = 0; i < sorted.Length; i += length)
{
var span = new Span <int>(sorted, i, length);
Vector.Abs(new Vector <int>(span)).CopyTo(output, i);
}
return(sorted.Length);
}
static void GetEdgeClosestPoint(ref Vector3Wide normal, ref Vector <int> edgeDirectionIndex,
ref BoxWide box,
ref Vector3Wide offsetA, ref Vector3Wide capsuleAxis, ref Vector <float> capsuleHalfLength, out Vector3Wide closestPointFromEdge)
{
Vector3Wide.Dot(normal, offsetA, out var calibrationDot);
var flipNormal = Vector.LessThan(calibrationDot, Vector <float> .Zero);
normal.X = Vector.ConditionalSelect(flipNormal, -normal.X, normal.X);
normal.Y = Vector.ConditionalSelect(flipNormal, -normal.Y, normal.Y);
normal.Z = Vector.ConditionalSelect(flipNormal, -normal.Z, normal.Z);
Vector3Wide boxEdgeCenter;
//Note that this can result in a closest point in the middle of the box when the capsule is parallel to a face. That's fine.
boxEdgeCenter.X = Vector.ConditionalSelect(Vector.Equals(normal.X, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.X, Vector <float> .Zero), box.HalfWidth, -box.HalfWidth));
boxEdgeCenter.Y = Vector.ConditionalSelect(Vector.Equals(normal.Y, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Y, Vector <float> .Zero), box.HalfHeight, -box.HalfHeight));
boxEdgeCenter.Z = Vector.ConditionalSelect(Vector.Equals(normal.Z, Vector <float> .Zero), Vector <float> .Zero,
Vector.ConditionalSelect(Vector.GreaterThan(normal.Z, Vector <float> .Zero), box.HalfLength, -box.HalfLength));
Vector3Wide boxEdgeDirection;
var useEdgeX = Vector.Equals(edgeDirectionIndex, Vector <int> .Zero);
var useEdgeY = Vector.Equals(edgeDirectionIndex, Vector <int> .One);
var useEdgeZ = Vector.Equals(edgeDirectionIndex, new Vector <int>(2));
boxEdgeDirection.X = Vector.ConditionalSelect(useEdgeX, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Y = Vector.ConditionalSelect(useEdgeY, Vector <float> .One, Vector <float> .Zero);
boxEdgeDirection.Z = Vector.ConditionalSelect(useEdgeZ, Vector <float> .One, Vector <float> .Zero);
//From CapsulePairCollisionTask, point of closest approach along the capsule axis, unbounded:
//ta = (da * (b - a) + (db * (a - b)) * (da * db)) / (1 - ((da * db) * (da * db))
//where da = capsuleAxis, db = boxEdgeDirection, a = offsetA, b = boxEdgeCenter
Vector3Wide.Subtract(boxEdgeCenter, offsetA, out var ab);
Vector3Wide.Dot(ab, capsuleAxis, out var abda);
Vector3Wide.Dot(ab, boxEdgeDirection, out var abdb);
Vector3Wide.Dot(capsuleAxis, boxEdgeDirection, out var dadb);
//Note division by zero guard.
var ta = (abda - abdb * dadb) / Vector.Max(new Vector <float>(1e-15f), (Vector <float> .One - dadb * dadb));
//In some cases, ta won't be in a useful location. Need to constrain it to the projection of the box edge onto the capsule edge.
//B onto A: +-BHalfExtent * (da * db) + da * ab
var bHalfExtent = Vector.ConditionalSelect(useEdgeX, box.HalfWidth, Vector.ConditionalSelect(useEdgeY, box.HalfHeight, box.HalfLength));
var bOntoAOffset = bHalfExtent * Vector.Abs(dadb);
var taMin = Vector.Max(-capsuleHalfLength, Vector.Min(capsuleHalfLength, abda - bOntoAOffset));
var taMax = Vector.Min(capsuleHalfLength, Vector.Max(-capsuleHalfLength, abda + bOntoAOffset));
ta = Vector.Min(Vector.Max(ta, taMin), taMax);
Vector3Wide.Scale(capsuleAxis, ta, out var offsetAlongCapsule);
Vector3Wide.Add(offsetA, offsetAlongCapsule, out closestPointFromEdge);
}
public int SIMD()
{
var length = Vector <int> .Count;
for (var i = 0; i < data.Length; i += length)
{
var span = new Span <int>(data, i, length);
Vector.Abs(new Vector <int>(span)).CopyTo(output, i);
}
return(data.Length);
}
//---------------------------------------------------------------------
//[Benchmark]
public double ParallelizedSimd()
{
double delta = 0;
var sync = new object();
Partitioner<Tuple<int, int>> partitioner = Partitioner.Create(0, _values.Length);
#if SEQUENTIAL
var range = Tuple.Create(0, _values.Length);
#else
Parallel.ForEach(
partitioner,
range =>
{
#endif
double localDelta = 0;
double[] arr = _values;
double avg = this.Mean;
int i = range.Item1;
// RCE
if ((uint)range.Item1 >= arr.Length || (uint)range.Item2 > arr.Length)
ThrowHelper.ThrowArgumentOutOfRange(ThrowHelper.ExceptionArgument.range);
if (Vector.IsHardwareAccelerated && (range.Item2 - range.Item1) >= Vector<double>.Count * 2)
{
var avgVec = new Vector<double>(avg);
var deltaVec = new Vector<double>(0);
for (; i < range.Item2 - 2 * Vector<double>.Count; i += Vector<double>.Count)
{
var arrVec = new Vector<double>(arr, i);
deltaVec += Vector.Abs(arrVec - avgVec);
i += Vector<double>.Count;
arrVec = new Vector<double>(arr, i);
deltaVec += Vector.Abs(arrVec - avgVec);
}
for (int j = 0; j < Vector<double>.Count; ++j)
localDelta += deltaVec[j];
}
for (; i < range.Item2; ++i)
localDelta += Math.Abs(arr[i] - avg);
lock (sync) delta += localDelta;
#if !SEQUENTIAL
}
);
#endif
return delta / _values.Length;
}
/// <summary>
/// Evaluates the configured flux function (see
/// <see cref="BoundaryConditionSourceFromINonlinearFlux.BoundaryConditionSourceFromINonlinearFlux(CNSControl, ISpeciesMap, BoundaryCondition, INonlinearFlux)"/>
/// using the present flow state <paramref name="U"/> and the boundary
/// value provided by the configured boundary condition.
/// </summary>
/// <param name="time"></param>
/// <param name="x"></param>
/// <param name="U"></param>
/// <param name="IndexOffset"></param>
/// <param name="FirstCellInd"></param>
/// <param name="Lenght"></param>
/// <param name="Output"></param>
public void Source(double time, MultidimensionalArray x, MultidimensionalArray[] U, int IndexOffset, int FirstCellInd, int Lenght, MultidimensionalArray Output)
{
int D = CompressibleEnvironment.NumberOfDimensions;
int noOfNodes = x.GetLength(1);
int noOfVariables = D + 2;
MultidimensionalArray normal = MultidimensionalArray.Create(Lenght, noOfNodes, D);
MultidimensionalArray[] Uout = new MultidimensionalArray[noOfVariables];
for (int i = 0; i < noOfVariables; i++)
{
Uout[i] = MultidimensionalArray.Create(Lenght, noOfNodes);
}
double[] xLocal = new double[D];
Material material = speciesMap.GetMaterial(double.NaN);
for (int i = 0; i < Lenght; i++)
{
for (int j = 0; j < noOfNodes; j++)
{
StateVector stateIn = new StateVector(material, U, i, j, CompressibleEnvironment.NumberOfDimensions);
Vector levelSetNormal = new Vector(CompressibleEnvironment.NumberOfDimensions);
int offset = CompressibleEnvironment.NumberOfDimensions + 2;
for (int d = 0; d < CompressibleEnvironment.NumberOfDimensions; d++)
{
levelSetNormal[d] = U[offset + d][i + IndexOffset, j];
}
levelSetNormal.Normalize();
Debug.Assert(Math.Abs(levelSetNormal.Abs() - 1.0) < 1e-13, "Abnormal normal vector");
for (int d = 0; d < D; d++)
{
xLocal[d] = x[i + IndexOffset, j, d];
normal[i, j, d] = levelSetNormal[d];
}
StateVector stateOut = boundaryCondition.GetBoundaryState(time, xLocal, levelSetNormal, stateIn);
Debug.Assert(stateOut.IsValid, "Invalid boundary state");
Uout[0][i, j] = stateOut.Density;
for (int d = 0; d < D; d++)
{
Uout[d + 1][i, j] = stateOut.Momentum[d];
}
Uout[D + 1][i, j] = stateOut.Energy;
}
}
fluxFunction.InnerEdgeFlux(time, -1, x, normal, U, Uout, IndexOffset, Lenght, Output);
}
