public static Vector128 <int>[,] Procedure(int[,] A, int[,] B, int a, Vector128 <int> x, Vector128 <int> y)
{
unsafe
{
int length = A.GetLength(0);
int length2 = B.GetLength(1);
int length3 = 0;
int length4 = 0;
try
{
length3 = length;
length4 = length2 / 4;
}
catch (Exception e)
{
Console.WriteLine("Error");
}
Vector128 <int>[,] MatrixA = new Vector128 <int> [length3, length4];
Vector128 <int>[,] MatrixB = new Vector128 <int> [length3, length4];
Vector128 <int>[,] Matrix = new Vector128 <int> [length3, length4];
int c = 0;
fixed(int *ptr = A)
{
fixed(int *ptr2 = B)
{
for (int i = 0; i < length3; i++)
{
for (int j = 0; j < length4; j++)
{
var v = Sse41.LoadVector128(ptr + c);
var v2 = Sse41.LoadVector128(ptr2 + c);
c += 4;
MatrixA[i, j] = Sse41.MultiplyLow(v, x);
MatrixA[i, j] = Sse41.MultiplyLow(MatrixA[i, j], Vector128.Create(a));
MatrixB[i, j] = Sse41.MultiplyLow(v2, y);
Matrix[i, j] = Sse41.Add(MatrixA[i, j], MatrixB[i, j]);
}
}
}
}
return(Matrix);
}
}
/// <summary>
/// Calculate "distance" of cloud at determined pose
/// TODO - It's actually slower than SISD. Need more parallelism.
/// </summary>
/// <param name="cloud">Cloud of points</param>
/// <param name="pose">Pose of cloud</param>
/// <returns></returns>
private int CalculateDistanceSSE41(ScanCloud cloud, Vector3 pose)
{
int nb_points = 0;
long sum = 0;
float px = pose.X * HoleMap.Scale;
float py = pose.Y * HoleMap.Scale;
float c = MathF.Cos(pose.Z) * HoleMap.Scale;
float s = MathF.Sin(pose.Z) * HoleMap.Scale;
Vector128 <float> sincos = Vector128.Create(c, -s, s, c);
Vector128 <float> posxy = Vector128.Create(px, py, px, py);
// Translate and rotate scan to robot position and compute the "distance"
for (int i = 0; i < cloud.Points.Count; i++)
{
Vector128 <float> xy = Vector128.Create(cloud.Points[i].X, cloud.Points[i].Y, cloud.Points[i].X, cloud.Points[i].Y);
xy = Sse41.Multiply(sincos, xy);
xy = Sse41.HorizontalAdd(xy, xy);
xy = Sse41.Add(xy, posxy);
xy = Sse41.RoundToNearestInteger(xy);
int x = (int)xy.GetElement(0);
int y = (int)xy.GetElement(1);
// Check boundaries
if ((x >= 0) && (x < HoleMap.Size) && (y >= 0) && (y < HoleMap.Size))
{
sum += HoleMap.Pixels[y * HoleMap.Size + x];
nb_points++;
}
}
if (nb_points > 0)
{
return((int)((sum * 1024) / cloud.Points.Count));
}
else
{
return(int.MaxValue);
}
}
