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); } }