public void Write(M44d x)
{
Write(x.M00); Write(x.M01); Write(x.M02); Write(x.M03);
Write(x.M10); Write(x.M11); Write(x.M12); Write(x.M13);
Write(x.M20); Write(x.M21); Write(x.M22); Write(x.M23);
Write(x.M30); Write(x.M31); Write(x.M32); Write(x.M33);
}
/// <summary>
/// Projects outline of a box from given position to a plane.
/// Returns null if position is inside the box.
/// </summary>
/// <param name="box"></param>
/// <param name="fromPosition"></param>
/// <param name="box2plane">Transformation from world/box-space to plane z=0</param>
public static V2d[] GetOutlineProjected(this Box3d box, V3d fromPosition, M44d box2plane)
{
var ps = box.GetOutlineCornersCW(fromPosition);
if (ps == null)
{
return(null);
}
var qs = ps.Map(p => box2plane.TransformPosProj(p));
var behindPositionCount = 0;
for (var i = 0; i < qs.Length; i++)
{
if (qs[i].Z < 0.0)
{
behindPositionCount++;
}
}
if (behindPositionCount == qs.Length)
{
return(new V2d[0]);
}
if (behindPositionCount > 0)
{
return(null);
}
return(qs.Map(p => p.XY));
}
static Box3d Transform3(Box3d box, M44d trafo)
{
var min = new V3d(trafo.M03, trafo.M13, trafo.M23);
var max = min;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
var a = trafo[i, j];
var av = a * box.Min[j];
var bv = a * box.Max[j];
if (av < bv)
{
min[i] += av;
max[i] += bv;
}
else
{
min[i] += bv;
max[i] += av;
}
}
}
return(new Box3d(min, max));
}
public static void WriteMat(M44d mat)
{
Report.Line("[ {0},", mat.R0.ToString("g4"));
Report.Line(" {0},", mat.R1.ToString("g4"));
Report.Line(" {0},", mat.R2.ToString("g4"));
Report.Line(" {0} ]", mat.R3.ToString("g4"));
}
public static STransformationMatrix ToSTransfMatrix(this M44d self)
{
return(new STransformationMatrix()
{
m_adElement = self.Elements.ToArray()
});
}
/// <summary>
/// Calculates the inverse Matrix to A using Householder-Transformations
/// </summary>
public static M44d QrInverse2(this M44d mat)
{
double[,] qr = (double[, ])mat;
double[] diag = qr.QrFactorize();
double[,] inv = new double[4, 4];
qr.QrInverse(diag, inv);
return((M44d)inv);
}
static Ray3d GetCameraRay(V2d uv, M44d invViewProj)
{
var deviceCoord = new V2d(uv.X * 2 - 1, -uv.Y * 2 + 1);
var nearPoint = invViewProj.TransformPosProj(deviceCoord.XYO);
var farPoint = invViewProj.TransformPosProj(deviceCoord.XYI);
return(new Ray3d(nearPoint, (farPoint - nearPoint).Normalized));
}
public void M44d_Multiply()
{
var mat = M44d.RotationZ(1);
var local = arr44;
for (int i = 0; i < local.Length; i++)
{
arr44[i] = mat * local[i];
}
}
public void M34d_Multiply_M44d()
{
var mat = M44d.RotationZ(1);
var local = arr34;
for (int i = 0; i < local.Length; i++)
{
local[i] = local[i] * mat;
}
}
public void BoxTransformTest()
{
var cnt = 10000;
var rnd = new RandomSystem(1);
var trafos = new M44d[cnt].SetByIndex(i => new M44d(rnd.CreateUniformDoubleArray(16)));
//var trafos = new M44d[cnt].SetByIndex(i => M44d.FromBasis(rnd.UniformV3d(), rnd.UniformV3d(), rnd.UniformV3d(), rnd.UniformV3d()));
//var trafos = new M44d[cnt].SetByIndex(i => M44d.Translation(rnd.UniformV3d()));
//var trafos = new M44d[cnt].SetByIndex(i => M44d.Rotation(rnd.UniformV3d()) * M44d.Translation(rnd.UniformV3d()));
var boxes = new Box3d[cnt].SetByIndex(i => Box3d.FromCenterAndSize(rnd.UniformV3d(), rnd.UniformV3d()));
var refBoxes = boxes.Map((b, i) => Transform1(b, trafos[i]));
for (int j = 0; j < 10; j++)
{
Report.BeginTimed("Transform Boxes Aardvark");
for (int i = 0; i < trafos.Length; i++)
{
var test = boxes[i].Transformed(trafos[i]);
Assert.IsTrue(test.Min.ApproximateEquals(refBoxes[i].Min, 1e-7) && test.Max.ApproximateEquals(refBoxes[i].Max, 1e-7));
}
Report.End();
Report.BeginTimed("Transform Boxes1");
for (int i = 0; i < trafos.Length; i++)
{
var test = Transform1(boxes[i], trafos[i]);
Assert.IsTrue(test.Min.ApproximateEquals(refBoxes[i].Min, 1e-7) && test.Max.ApproximateEquals(refBoxes[i].Max, 1e-7));
}
Report.End();
Report.BeginTimed("Transform Boxes2");
for (int i = 0; i < trafos.Length; i++)
{
var test = Transform2(boxes[i], trafos[i]);
Assert.IsTrue(test.Min.ApproximateEquals(refBoxes[i].Min, 1e-7) && test.Max.ApproximateEquals(refBoxes[i].Max, 1e-7));
}
Report.End();
Report.BeginTimed("Transform Boxes3");
for (int i = 0; i < trafos.Length; i++)
{
var test = Transform3(boxes[i], trafos[i]);
Assert.IsTrue(test.Min.ApproximateEquals(refBoxes[i].Min, 1e-7) && test.Max.ApproximateEquals(refBoxes[i].Max, 1e-7));
}
Report.End();
Report.BeginTimed("Transform Boxes4");
for (int i = 0; i < trafos.Length; i++)
{
var test = Transform4(boxes[i], trafos[i]);
Assert.IsTrue(test.Min.ApproximateEquals(refBoxes[i].Min, 1e-7) && test.Max.ApproximateEquals(refBoxes[i].Max, 1e-7));
}
Report.End();
}
}
public void InPlaceTransposeTest()
{
var rand = new RandomSystem();
var m = new M44d(rand.CreateUniformDoubleArray(16));
var transposed = m.Transposed;
Mat.Transpose(ref m);
Assert.IsTrue(Fun.ApproximateEquals(m, transposed, 0.0001));
}
public static M44d LuInverseV(this M44d m)
{
var lu = new Matrix <double>((double[])m, 0, s_luSize, s_luDelta);
var p = lu.LuFactorize();
var inv = new M44d();
var c0 = lu.LuSolve(p, s_c0); inv.M00 = c0.Data[0]; inv.M10 = c0.Data[1]; inv.M20 = c0.Data[2]; inv.M30 = c0.Data[3];
var c1 = lu.LuSolve(p, s_c1); inv.M01 = c1.Data[0]; inv.M11 = c1.Data[1]; inv.M21 = c1.Data[2]; inv.M31 = c1.Data[3];
var c2 = lu.LuSolve(p, s_c2); inv.M02 = c2.Data[0]; inv.M12 = c2.Data[1]; inv.M22 = c2.Data[2]; inv.M32 = c2.Data[3];
var c3 = lu.LuSolve(p, s_c3); inv.M03 = c3.Data[0]; inv.M13 = c3.Data[1]; inv.M23 = c3.Data[2]; inv.M33 = c3.Data[3];
return(inv);
}
public void MatrixMultiplicationTest()
{
using (Report.JobTimed("Matrix multiplication tests"))
{
var rand = new RandomSystem();
Test.Begin("Row vector with matrix");
var m = new M44d(rand.CreateUniformDoubleArray(16));
var v = new V4d(rand.CreateUniformDoubleArray(4));
Test.IsTrue(v * m == m.Transposed * v);
Test.End();
}
}
static Box3d Transform2(Box3d box, M44d trafo)
{
var xa = trafo.C0.XYZ * box.Min.X;
var xb = trafo.C0.XYZ * box.Max.X;
var ya = trafo.C1.XYZ * box.Min.Y;
var yb = trafo.C1.XYZ * box.Max.Y;
var za = trafo.C2.XYZ * box.Min.Z;
var zb = trafo.C2.XYZ * box.Max.Z;
return(new Box3d(
Fun.Min(xa, xb) + Fun.Min(ya, yb) + Fun.Min(za, zb) + trafo.C3.XYZ,
Fun.Max(xa, xb) + Fun.Max(ya, yb) + Fun.Max(za, zb) + trafo.C3.XYZ));
}
public void TransformedInfinite()
{
var rnd = new RandomSystem(123);
for (int i = 0; i < 1000; i++)
{
var mat = new M44d(rnd.CreateUniformDoubleArray(16));
var test = Box3d.Infinite.Transformed(mat);
if (test.Min.AnyInfinity || test.Max.AnyInfinity)
{
Report.Line("Max -> Infinite"); // NOTE: some max values will be infinite
}
Assert.False(test.Max.AnyNaN || test.Min.AnyNaN || test.IsEmpty);
}
}
public void TransformedEmpty()
{
var rnd = new RandomSystem(123);
for (int i = 0; i < 1000; i++)
{
var mat = new M44d(rnd.CreateUniformDoubleArray(16));
var test = Box3d.Invalid.Transformed(mat);
Assert.True(test.IsEmpty);
Assert.False(test.Max.AnyNaN || test.Min.AnyNaN || test.IsNonEmpty);
if (test.Min.AnyInfinity || test.Max.AnyInfinity)
{
Report.Line("Max -> Infinite");
}
}
}
public static M33d Version3(M44d m, int row, int column)
{
M33d rs = new M33d();
for (int k = 0; k < 9; k++)
{
var i = k / 3;
var j = k % 3;
var ii = (i < row) ? i : i + 1;
var jj = (j < column) ? j : j + 1;
rs[k] = m[ii * 4 + jj];
}
return(rs);
}
public static void ConsistentWithMatrixRotationAndShiftTest()
=> TrafoTesting.GenericTest(rnd =>
{
var trans = rnd.UniformV3d() * 10;
var axis = rnd.UniformV3dDirection();
var angle = rnd.UniformDouble() * Constant.PiTimesTwo;
var m = M44d.Translation(trans) * M44d.Rotation(axis, angle);
var e = new Euclidean3d(Rot3d.Rotation(axis, angle), trans);
var p = rnd.UniformV3d() * rnd.UniformInt(1000);
var res = m.TransformPos(p);
var res2 = e.TransformPos(p);
TrafoTesting.AreEqual(res, res2);
});
static Box3d Transform1(Box3d box, M44d trafo)
{
var res = Box3d.Invalid;
if (!box.IsInvalid)
{
res.ExtendBy(trafo.TransformPos((V3d)box.Min));
res.ExtendBy(trafo.TransformPos((V3d) new V3d(box.Max.X, box.Min.Y, box.Min.Z)));
res.ExtendBy(trafo.TransformPos((V3d) new V3d(box.Min.X, box.Max.Y, box.Min.Z)));
res.ExtendBy(trafo.TransformPos((V3d) new V3d(box.Max.X, box.Max.Y, box.Min.Z)));
res.ExtendBy(trafo.TransformPos((V3d) new V3d(box.Min.X, box.Min.Y, box.Max.Z)));
res.ExtendBy(trafo.TransformPos((V3d) new V3d(box.Max.X, box.Min.Y, box.Max.Z)));
res.ExtendBy(trafo.TransformPos((V3d) new V3d(box.Min.X, box.Max.Y, box.Max.Z)));
res.ExtendBy(trafo.TransformPos((V3d)box.Max));
}
return(res);
}
public static void FromM33d()
{
TrafoTesting.GenericTest(rnd =>
{
var rot = rnd.UniformV3dFull() * Constant.PiTimesFour - Constant.PiTimesTwo;
var mat = M44d.RotationEuler(rot);
var mat2 = (M44d)Rot3d.FromM33d((M33d)mat);
Assert.IsFalse(mat.Elements.Any(x => x.IsNaN()), "NaN");
if (!Fun.ApproximateEquals(mat, mat2, 1e-9))
{
Assert.Fail("FAIL");
}
});
}
/// <summary>
/// Returns points inside view frustum (defined by viewProjection and canonicalViewVolume).
/// </summary>
public static IEnumerable <Chunk> QueryPointsInViewFrustum(
this PointSet self, M44d viewProjection, Box3d canonicalViewVolume
)
{
var t = viewProjection.Inverse;
var cs = canonicalViewVolume.ComputeCorners().Map(t.TransformPosProj);
var hull = new Hull3d(new[]
{
new Plane3d(cs[0], cs[2], cs[1]), // near
new Plane3d(cs[5], cs[7], cs[4]), // far
new Plane3d(cs[0], cs[1], cs[4]), // bottom
new Plane3d(cs[1], cs[3], cs[5]), // left
new Plane3d(cs[4], cs[6], cs[0]), // right
new Plane3d(cs[3], cs[2], cs[7]), // top
});
return(QueryPointsInsideConvexHull(self, hull));
}
public static M33d Version2(M44d m, int row, int column)
{
M33d rs = new M33d();
var idx = 0;
for (int k = 0; k < 16; k++)
{
var i = k / 4;
var j = k % 4;
if (i != row && j != column)
{
rs[idx++] = m[k];
}
}
return(rs);
}
public static void YawPitchRoll()
{
TrafoTesting.GenericTest(rnd =>
{
var yaw = rnd.UniformDouble() * Constant.PiTimesFour - Constant.PiTimesTwo;
var pitch = rnd.UniformDouble() * Constant.PiTimesFour - Constant.PiTimesTwo;
var roll = rnd.UniformDouble() * Constant.PiTimesFour - Constant.PiTimesTwo;
// Aardvark euler angles: roll (X), pitch (Y), yaw (Z). Ther are applied in reversed order.
var mat = (M33d)(M44d.RotationZ(yaw) * M44d.RotationY(pitch) * M44d.RotationX(roll));
var mat2 = (M33d)M44d.RotationEuler(roll, pitch, yaw);
var mat3 = (M33d)(Rot3d.RotationZ(yaw) * Rot3d.RotationY(pitch) * Rot3d.RotationX(roll));
var mat4 = (M33d)Rot3d.RotationEuler(roll, pitch, yaw);
Assert.IsTrue(Fun.ApproximateEquals(mat, mat2, 1e-7));
Assert.IsTrue(Fun.ApproximateEquals(mat, mat3, 1e-7));
Assert.IsTrue(Fun.ApproximateEquals(mat, mat4, 1e-7));
});
}
public AngleBetweenDouble()
{
var rnd = new RandomSystem(1);
A.SetByIndex(i => rnd.UniformV3dDirection());
angles.SetByIndex(i => rnd.UniformDouble() * (double)Constant.Pi);
B.SetByIndex(i =>
{
V3d v;
do
{
v = rnd.UniformV3dDirection();
}while (v.Dot(A[i]).IsTiny());
V3d axis = v.Cross(A[i]).Normalized;
return(M44d.Rotation(axis, angles[i]).TransformDir(A[i]));
});
}
/// <summary>
/// Loads positions from aara file and returns a TriangleSet (a regular grid for triangulation is assumed).
/// Further a translation offset can be applied to the position data
/// </summary>
public static TriangleSet GetTriangleSetFromPath(string posPath, M44d affine)
{
//create empty vg
var vg = new VertexGeometry(GeometryMode.TriangleList);
//read grid from aara file
var grid = AaraHelpers.PointSampleGrid2dFromAaraFile(posPath);
//apply affine with double prec
//var transPos = grid.Positions
// .Select(x => (V3f)(affine.TransformPos(x)))
// .ToArray();
var positions = grid.Positions.ToArray();
//get indices while handling invalid positions
var invPoints = OpcIndices.GetInvalidPositions(positions);
var indices = IndexArrayCache.GetIndexArray(grid.Resolution, invPoints.ToList());
if (indices.Length == 0)
{
indices = new int[] { 0, 0, 0 }
}
;
vg.Positions = positions;
vg.TransformV3d(affine);
vg.Indices = indices;
//build up triangle set
var triangles = vg.Triangles
.Where(x => !x.Point0.Position.IsNaN &&
!x.Point1.Position.IsNaN &&
!x.Point2.Position.IsNaN)
.Select(x => x.ToTriangle3d());
return(new TriangleSet(triangles));
}
}
/// <summary>
/// Build a geometry transformation from the given parameters as specified in Transform
/// http://gun.teipir.gr/VRML-amgem/spec/part1/nodesRef.html#Transform
/// </summary>
public static Trafo3d BuildVrmlGeometryTrafo(V3d center, V4d rotation, V3d scale, V4d scaleOrientation, V3d translation)
{
// create composite trafo (naming taken from vrml97 spec)
M44d C = M44d.Translation(center), Ci = M44d.Translation(-center);
M44d SR = M44d.Rotation(scaleOrientation.XYZ, scaleOrientation.W),
SRi = M44d.Rotation(scaleOrientation.XYZ, -scaleOrientation.W);
M44d T = M44d.Translation(translation), Ti = M44d.Translation(-translation);
//if (m_aveCompatibilityMode) r.W = -r.W;
M44d R = M44d.Rotation(rotation.XYZ, rotation.W),
Ri = M44d.Rotation(rotation.XYZ, -rotation.W);
// in case some axis scales by 0 the best thing for the inverse scale is also 0
var si = new V3d(scale.X.IsTiny() ? 0 : 1 / scale.X,
scale.Y.IsTiny() ? 0 : 1 / scale.Y,
scale.Z.IsTiny() ? 0 : 1 / scale.Z);
M44d S = M44d.Scale(scale), Si = M44d.Scale(si);
return(new Trafo3d(
T * C * R * SR * S * SRi * Ci,
C * SR * Si * SRi * Ri * Ci * Ti));
}
public static M33d Version1(M44d m, int rowToDelete, int columnToDelete)
{
M33d result = new M33d();
int checked_row = 0;
for (int actual_row = 0; actual_row < 4; actual_row++)
{
int checked_column = 0;
if (actual_row != rowToDelete)
{
for (int actual_column = 0; actual_column < 4; actual_column++)
{
if (actual_column != columnToDelete)
{
result[checked_row, checked_column] = m[actual_row, actual_column];
checked_column++;
}
}
checked_row++;
}
}
return(result);
}
public ISg SetParameters(AbstractTraversal t)
{
if (!m_isInitialized)
{
var info = m_instance.PatchFileInfo;
var bb = m_instance.PositionsType == PositionsType.V3dPositions ?
info.LocalBoundingBox : info.LocalBoundingBox2d;
var patch = new StreamingJob(
() =>
{
//TaskCombinators.Delay(100);
//do importer logic here
var patchVg = m_instance.LoadingStrategy.Load(m_instance.PatchFileInfo, m_instance.PatchFilePath, m_instance.PositionsType,
true, true, true, m_instance.MaxTriangleSize);
if (patchVg == null)
{
return(EmptyLeaf.Singleton);
}
var lodColor = Patch.GetLodColor(m_instance.Level, m_instance.MaxLevel);
patchVg["LodColor"] = lodColor;
for (int i = 0; i < patchVg.Textures.Count; i++)
{
var key = patchVg.Textures.Keys.ToList()[i];
var source = patchVg.Textures[key].Convertible;
Convertible target = null;
if (t.Renderer is SlimDx9Renderer)
{
target = SlimDx9TextureConvertible.Create(
new SlimDx9TextureConvertible.SlimDx9TextureParameters()
{
//SlimDx9Format = SlimDX.Direct3D9.Format., // .Dxt5,
Pool = SlimDX.Direct3D9.Pool.Default
});
source.ConvertInto(target);
}
else
{
// nothing todo in slimdx10renderer (this just loads the texture on demand)
// fix this if you are fed up with framerate hick ups
target = source;
}
patchVg.Textures[key] = new Texture(target);
}
lock (asyncLock)
{
m_asyncTextures = patchVg.Textures;
}
return(patchVg.ToVertexGeometrySet());
},
bb, m_instance.MaxLevel - m_instance.Level, true);
patch.DestructSideEffects = DisposeSideEffects;
var placeHolder = Primitives.WireBox(bb, C4b.Red).ToVertexGeometrySet();
m_returnISg = new AsyncStreamingNode(patch, placeHolder)
{
DebugName = Path.GetFileName(m_instance.PatchFilePath),
};
var local2Global = m_instance.PatchFileInfo.GetLocal2Global(m_instance.PositionsType);
if (m_instance.InvertZ)
{
local2Global = M44d.Scale(1, 1, -1) * local2Global;
}
var global2Local = local2Global.Inverse;
m_returnISg = Rsg.Apply(Rsg.Attribute.PushTrafo3d(new Trafo3d(local2Global, global2Local)), m_returnISg);
m_isInitialized = true;
}
return(m_returnISg);
}
public static M44d LuInverseM(this M44d m)
{
var lu = new Matrix <double>((double[])m, 0, s_luSize, s_luDelta);
return((M44d)(lu.LuSolve(lu.LuFactorize(), s_unit).Data));
}
/// <summary>
/// Calculates the inverse using gauss elemination.
/// This is a more accurate calculation of the inverse (but slower).
/// This method returns the inverse of the matrix to a new object.
/// </summary>
/// <returns>Returns the inverse of the matrix.</returns>
public static M44d NumericallyInstableGjInverse2(this M44d mat)
{
int i, j, k;
var work = (double[, ])mat;
var result = (double[, ])M44d.Identity;
for (i = 0; i < 3; i++)
{
int pivot = i;
double pivotsize = System.Math.Abs(work[i, i]);
for (j = i + 1; j < 4; j++)
{
double r = work[j, i];
if (r < 0)
{
r = -r;
}
if (r > pivotsize)
{
pivot = j;
pivotsize = r;
}
}
if (pivotsize == 0.0)
{
throw new ArgumentException(
"cannot invert singular matrix");
}
if (pivot != i)
{
for (j = 0; j < 4; j++)
{
double r;
r = work[i, j];
work[i, j] = work[pivot, j];
work[pivot, j] = r;
r = result[i, j];
result[i, j] = result[pivot, j];
result[pivot, j] = r;
}
}
for (j = i + 1; j < 4; j++)
{
double f = work[j, i] / work[i, i];
for (k = 0; k < 4; k++)
{
work[j, k] -= f * work[i, k];
result[j, k] -= f * result[i, k];
}
}
}
//backward substitution
for (i = 3; i >= 0; --i)
{
double f;
if ((f = work[i, i]) == 0)
{
throw new ArgumentException("cannot invert singular matrix");
}
for (j = 0; j < 4; j++)
{
work[i, j] /= f;
result[i, j] /= f;
}
for (j = 0; j < i; j++)
{
f = work[j, i];
for (k = 0; k < 4; k++)
{
work[j, k] -= f * work[i, k];
result[j, k] -= f * result[i, k];
}
}
}
return((M44d)result);
}
