/// <summary>
/// Builds a hull from the given view-projection transformation (left, right, top, bottom, near, far).
/// The normals of the hull planes point to the inside.
/// A point inside the visual hull will have positive height to all planes.
/// </summary>
public static Hull3d GetVisualHull(this Trafo3d vpTrafo)
{
var frustumCorners = new Box3d(-V3d.IIO, V3d.III).ComputeCorners();
//Min, 0 near left bottom
//new V3d(Max.X, Min.Y, Min.Z), 1 near right bottom
//new V3d(Min.X, Max.Y, Min.Z), 2 near left top
//new V3d(Max.X, Max.Y, Min.Z), 3 near right top
//new V3d(Min.X, Min.Y, Max.Z), 4 far left bottom
//new V3d(Max.X, Min.Y, Max.Z), 5 far right bottom
//new V3d(Min.X, Max.Y, Max.Z), 6 far left top
//Max 7 far right top
// use inverse view-projection to get vertices in world space
frustumCorners.Apply(c => vpTrafo.Backward.TransformPosProj(c));
// hull planes should point inward, assume right-handed transformation to build planes
var hull = new Hull3d(new[]
{
new Plane3d(frustumCorners[0], frustumCorners[4], frustumCorners[2]), // left
new Plane3d(frustumCorners[1], frustumCorners[3], frustumCorners[5]), // right
new Plane3d(frustumCorners[2], frustumCorners[6], frustumCorners[3]), // top
new Plane3d(frustumCorners[0], frustumCorners[1], frustumCorners[4]), // bottom
new Plane3d(frustumCorners[0], frustumCorners[2], frustumCorners[1]), // near
new Plane3d(frustumCorners[4], frustumCorners[5], frustumCorners[6]), // far
});
return(hull);
}
/// <summary>
/// Decomposes a transformation into a scale, rotation and translation component.
/// NOTE: The input is assumed to be a valid affine transformation.
/// The rotation output is in Euler-Angles (yaw, pitch, roll).
/// </summary>
public static void Decompose(this Trafo3d trafo, out V3d scale, out V3d rotation, out V3d translation)
{
translation = trafo.GetModelOrigin();
var rt = trafo.GetOrthoNormalOrientation();
if (rt.Forward.Det.IsTiny())
{
rotation = V3d.Zero;
}
else
{
var rot = Rot3d.FromFrame(rt.Forward.C0.XYZ, rt.Forward.C1.XYZ, rt.Forward.C2.XYZ);
rotation = rot.GetEulerAngles();
}
scale = trafo.GetScaleVector();
// if matrix is left-handed there must be some negative scale
// since rotation remains the x-axis, the y-axis must be flipped
if (trafo.Forward.Det < 0)
{
scale.Y = -scale.Y;
}
}
private void UpdateProjectionTrafo()
{
var l = m_box.Min.X; var r = m_box.Max.X;
var b = m_box.Min.Y; var t = m_box.Max.Y;
var n = m_box.Min.Z; var f = m_box.Max.Z;
m_trafo = Trafo3d.PerspectiveProjectionRH(l, r, b, t, n, f);
m_trafoChanges.Emit(m_trafo);
}
/// <summary>
/// Builds an ortho-normal orientation transformation form the given transform.
/// Scale and Translation will be removed and basis vectors will be ortho-normalized.
/// NOTE: The X-Axis is untouched and Y/Z are forced to a normal-angle.
/// </summary>
public static Trafo3d GetOrthoNormalOrientation(this Trafo3d trafo)
{
var x = trafo.Forward.C0.XYZ.Normalized; // TransformDir(V3d.XAxis)
var y = trafo.Forward.C1.XYZ.Normalized; // TransformDir(V3d.YAxis)
var z = trafo.Forward.C2.XYZ.Normalized; // TransformDir(V3d.ZAxis)
y = z.Cross(x).Normalized;
z = x.Cross(y).Normalized;
return(Trafo3d.FromBasis(x, y, z, V3d.Zero));
}
public FastHull3d Transformed(Trafo3d trafo)
{
var newFastHull = new FastHull3d()
{
Hull = this.Hull.Transformed(trafo)
};
newFastHull.MinCornerIndexArray =
ComputeMinCornerIndexArray(newFastHull.Hull.PlaneArray);
return(newFastHull);
}
public void TransformInto(Trafo3d trafo, ref Hull3d hull)
{
int count = PlaneCount;
var invTr = trafo.Backward.Transposed;
for (int i = 0; i < count; i++)
{
hull.PlaneArray[i]
= new Plane3d(
invTr.TransformDir(PlaneArray[i].Normal).Normalized,
trafo.Forward.TransformPos(PlaneArray[i].Point));
}
}
public Hull3d Transformed(Trafo3d trafo)
{
int count = PlaneCount;
var hull = new Hull3d(count);
var invTr = trafo.Backward.Transposed;
for (int i = 0; i < count; i++)
{
hull.PlaneArray[i]
= new Plane3d(
invTr.TransformDir(PlaneArray[i].Normal).Normalized,
trafo.Forward.TransformPos(PlaneArray[i].Point));
}
return(hull);
}
/// <summary>
/// Creates a transformation from the specified coordinate system
/// to the aardvark coordinate system (Meters, Right-Handed, Z-Up).
/// </summary>
public static Trafo3d ToAardvark(double scale, Handedness hand, Axis up)
{
var t = scale != 1 ? Trafo3d.Scale(scale) : Trafo3d.Identity;
if (hand != Handedness.Right)
{
t *= SwapHand;
}
if (up != Axis.Z)
{
t *= FromToRH(up, Axis.Z);
}
return(t);
}
/// <summary>
/// Builds transformation from one coordinate system to another.
/// </summary>
public static Trafo3d FromTo(Info from, Info to)
{
var t = Trafo3d.Identity;
if (from.UnitScale != to.UnitScale)
{
t = Trafo3d.Scale(to.UnitScale / from.UnitScale);
}
if (from.Handedness != to.Handedness)
{
t *= SwapHand;
}
if (from.UpVector != to.UpVector)
{
t *= FromToRH(from.UpVector, to.UpVector);
}
return(t);
}
private void UpdateProjectionTrafo()
{
var l = m_box.Min.X; var r = m_box.Max.X;
var b = m_box.Min.Y; var t = m_box.Max.Y;
var n = m_box.Min.Z; var f = m_box.Max.Z;
m_trafo = new Trafo3d(
new M44d(
2 / (r - l), 0, 0, (l + r) / (l - r),
0, 2 / (t - b), 0, (b + t) / (b - t),
0, 0, 1 / (n - f), n / (n - f),
0, 0, 0, 1
),
new M44d(
(r - l) / 2, 0, 0, (l + r) / 2,
0, (t - b) / 2, 0, (b + t) / 2,
0, 0, n - f, -n,
0, 0, 0, 1
)
);
m_trafoChanges.Emit(m_trafo);
}
/// <summary>
/// Approximates the uniform scale value of the given transformation (average length of basis vectors).
/// </summary>
public static double GetScale(this Trafo3d trafo)
{
return(trafo.Forward.GetScale());
}
/// <summary>
/// Creates a left-handed view trafo, where z-positive points into the scene.
/// </summary>
public static Trafo3d ViewTrafoLH(V3d location, V3d up, V3d forward)
{
return(Trafo3d.ViewTrafo(location, up.Cross(forward), up, forward));
}
/// <summary>
/// Creates a right-handed view trafo, where z-negative points into the scene.
/// </summary>
public static Trafo3d ViewTrafoRH(V3d location, V3d up, V3d forward)
{
return(Trafo3d.ViewTrafo(location, forward.Cross(up), up, -forward));
}
public Trafo3d(Trafo3d trafo)
{
Forward = trafo.Forward;
Backward = trafo.Backward;
}
/// <summary>
/// Returns the trafo that transforms from the coordinate system
/// specified by the basis into the world coordinate system.
/// </summary>
public static Trafo3d FromBasis(V3f xAxis, V3f yAxis, V3f zAxis, V3f orign)
{
return(Trafo3d.FromBasis((V3d)xAxis, (V3d)yAxis, (V3d)zAxis, (V3d)orign));
}
/// <summary>
/// Builds a transformation matrix using the scale, rotation and translation componets.
/// NOTE: Uses the Scale * Rotation * Translation notion.
/// The rotation is in Euler-Angles (yaw, pitch, roll).
/// </summary>
public static Trafo3d FromComponents(V3d scale, V3d rotation, V3d translation)
{
return(Trafo3d.Scale(scale) * Trafo3d.Rotation(rotation) * Trafo3d.Translation(translation));
}
/// <summary> /// Returns a transformation of an orthonormal basis in Plane- to WorldSpace. /// </summary> public static Trafo3d GetPlaneSpaceTransform(this Plane3d self) => Trafo3d.FromNormalFrame(self.Point, self.Normal);
/// <summary>
/// Extracts the Z-Axis from the given transformation.
/// NOTE: A right-handed coordinates system transformation is expected, where
/// the view-space z-axis points opposit the forward vector.
/// </summary>
public static V3d GetViewDirectionRH(this Trafo3d trafo)
{
return(-trafo.Forward.R2.XYZ.Normalized);
}
/// <summary>
/// Extracts a scale vector from the given matrix by calculating the lengths of the basis vectors.
/// </summary>
public static V3d GetScaleVector(this Trafo3d trafo)
{
return(trafo.Forward.GetScaleVector());
}
/// <summary> /// Extracts the Z-Axis from the given transformation. /// NOTE: A left-handed coordinates system transformation is expected, /// where the view-space z-axis points in forward direction. /// </summary> public static V3d GetViewDirectionLH(this Trafo3d trafo) => trafo.Forward.R2.XYZ.Normalized;
/// <summary> /// Extracts the inverse/backward translation component of the given transformation, which when given /// a view transformation represents the location of the camera in world space. /// </summary> public static V3d GetViewPosition(this Trafo3d trafo) => trafo.Backward.C3.XYZ;
/// <summary> /// Extracts a scale vector from the given matrix by calculating the lengths of the basis vectors. /// </summary> public static V3d GetScaleVector(this Trafo3d trafo) => trafo.Forward.GetScaleVector();
/// <summary> /// Approximates the uniform scale value of the given transformation (average length of basis vectors). /// </summary> public static double GetScale(this Trafo3d trafo) => trafo.Forward.GetScale();
/// <summary>
/// Creates a rigid transformation from a trafo <paramref name="trafo"/>.
/// </summary>
public __e3t__(Trafo3d trafo, __ft__ epsilon = __eps__)
: this((M4__s4f__)trafo.Forward, epsilon)
{
}
/// <summary>
/// Extracts the inverse/backward translation component of the given transformation, which when given
/// a view transformation represents the location of the camera in world space.
/// </summary>
public static V3d GetViewPosition(this Trafo3d trafo)
{
return(trafo.Backward.C3.XYZ);
}
/// <summary>
/// Sets location and axes in a single transaction.
/// </summary>
public void Set(V3d location, V3d right, V3d up, V3d forward)
{
m_trafo = Trafo3d.ViewTrafo(location, right, up, -forward);
m_trafoChanges.Emit(m_trafo);
}
/// <summary>
/// Extracts the translation component of the given transformation, which when given
/// a model transformation represents the model origin in world position.
/// </summary>
public static V3d GetModelOrigin(this Trafo3d trafo)
{
return(trafo.Forward.C3.XYZ);
}
/// <summary>
/// Transforms the plane with a given trafo using the inverse
/// transposed matrix.
/// </summary>
public Plane3d Transformed(Trafo3d trafo) => new Plane3d(
trafo.Backward.TransposedTransformDir(Normal),
trafo.Forward.TransformPos(Point));
/// <summary> /// Extracts the translation component of the given transformation, which when given /// a model transformation represents the model origin in world position. /// </summary> public static V3d GetModelOrigin(this Trafo3d trafo) => trafo.Forward.C3.XYZ;
public static Hull3d GetVisualHull(this Trafo3d viewProj)
{
return(viewProj.Forward.GetVisualHull());
}
