private static double[] GaussSeidelIterate(Vec3Matrix S, Vec3Vector lambda, Vec3Vector B, IterationDirection direction) { Vec3Vector lambda_old = lambda.Clone(); double sigma = 0; var deltaLambda = new Vec3(); var sum = new Vec3(); if (direction == IterationDirection.Forward) { for (int i = 0; i < lambda.Length; i++) { sum.SetZero(); for (int j = 0; j < lambda.Length; j++) { sum += S [i, j] * lambda [j]; } deltaLambda.Set((1.0 / S [i, i]) * (B [i] - sum)); sigma += deltaLambda.SqLength; lambda [i] += deltaLambda; } } else { for (int i = lambda.Length - 1; i >= 0; i--) { sum.SetZero(); for (int j = lambda.Length - 1; j >= 0; j--) { sum += S [i, j] * lambda [j]; } deltaLambda.Set((1.0 / S [i, i]) * (B [i] - sum)); sigma += deltaLambda.SqLength; lambda [i] += deltaLambda; } } var error = new double[2] { (lambda - lambda_old).Norm, // Absolute Error sigma / lambda.Length // Sigma }; return(error); }
/* AABB intersect data */ public static Vec3 PointNormalAABB(Vec3 p, AABB b) { Vec3 normal = new Vec3(); double minDistance = double.MaxValue; if (Math.Abs(b.min.x - p.x) < minDistance) { normal.Set(-1, 0, 0); minDistance = Math.Abs(b.min.x - p.x); } if (Math.Abs(b.max.x - p.x) < minDistance) { normal.Set(1, 0, 0); minDistance = Math.Abs(b.max.x - p.x); } if (Math.Abs(b.min.y - p.y) < minDistance) { normal.Set(0, -1, 0); minDistance = Math.Abs(b.min.y - p.y); } if (Math.Abs(b.max.y - p.y) < minDistance) { normal.Set(0, 1, 0); minDistance = Math.Abs(b.max.y - p.y); } if (Math.Abs(b.min.z - p.z) < minDistance) { normal.Set(0, 0, -1); minDistance = Math.Abs(b.min.z - p.z); } if (Math.Abs(b.max.z - p.z) < minDistance) { normal.Set(0, 0, 1); minDistance = Math.Abs(b.max.z - p.z); } return(normal); }