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