|
@@ -108,7 +108,7 @@ namespace Godot
|
|
public Transform3D AffineInverse()
|
|
public Transform3D AffineInverse()
|
|
{
|
|
{
|
|
Basis basisInv = basis.Inverse();
|
|
Basis basisInv = basis.Inverse();
|
|
- return new Transform3D(basisInv, basisInv.Xform(-origin));
|
|
|
|
|
|
+ return new Transform3D(basisInv, basisInv * -origin);
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// <summary>
|
|
@@ -147,7 +147,7 @@ namespace Godot
|
|
public Transform3D Inverse()
|
|
public Transform3D Inverse()
|
|
{
|
|
{
|
|
Basis basisTr = basis.Transposed();
|
|
Basis basisTr = basis.Transposed();
|
|
- return new Transform3D(basisTr, basisTr.Xform(-origin));
|
|
|
|
|
|
+ return new Transform3D(basisTr, basisTr * -origin);
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// <summary>
|
|
@@ -286,43 +286,6 @@ namespace Godot
|
|
));
|
|
));
|
|
}
|
|
}
|
|
|
|
|
|
- /// <summary>
|
|
|
|
- /// Returns a vector transformed (multiplied) by this transformation matrix.
|
|
|
|
- /// </summary>
|
|
|
|
- /// <seealso cref="XformInv(Vector3)"/>
|
|
|
|
- /// <param name="v">A vector to transform.</param>
|
|
|
|
- /// <returns>The transformed vector.</returns>
|
|
|
|
- public Vector3 Xform(Vector3 v)
|
|
|
|
- {
|
|
|
|
- return new Vector3
|
|
|
|
- (
|
|
|
|
- basis.Row0.Dot(v) + origin.x,
|
|
|
|
- basis.Row1.Dot(v) + origin.y,
|
|
|
|
- basis.Row2.Dot(v) + origin.z
|
|
|
|
- );
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- /// <summary>
|
|
|
|
- /// Returns a vector transformed (multiplied) by the transposed transformation matrix.
|
|
|
|
- ///
|
|
|
|
- /// Note: This results in a multiplication by the inverse of the
|
|
|
|
- /// transformation matrix only if it represents a rotation-reflection.
|
|
|
|
- /// </summary>
|
|
|
|
- /// <seealso cref="Xform(Vector3)"/>
|
|
|
|
- /// <param name="v">A vector to inversely transform.</param>
|
|
|
|
- /// <returns>The inversely transformed vector.</returns>
|
|
|
|
- public Vector3 XformInv(Vector3 v)
|
|
|
|
- {
|
|
|
|
- Vector3 vInv = v - origin;
|
|
|
|
-
|
|
|
|
- return new Vector3
|
|
|
|
- (
|
|
|
|
- (basis.Row0[0] * vInv.x) + (basis.Row1[0] * vInv.y) + (basis.Row2[0] * vInv.z),
|
|
|
|
- (basis.Row0[1] * vInv.x) + (basis.Row1[1] * vInv.y) + (basis.Row2[1] * vInv.z),
|
|
|
|
- (basis.Row0[2] * vInv.x) + (basis.Row1[2] * vInv.y) + (basis.Row2[2] * vInv.z)
|
|
|
|
- );
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
// Constants
|
|
// Constants
|
|
private static readonly Transform3D _identity = new Transform3D(Basis.Identity, Vector3.Zero);
|
|
private static readonly Transform3D _identity = new Transform3D(Basis.Identity, Vector3.Zero);
|
|
private static readonly Transform3D _flipX = new Transform3D(new Basis(-1, 0, 0, 0, 1, 0, 0, 0, 1), Vector3.Zero);
|
|
private static readonly Transform3D _flipX = new Transform3D(new Basis(-1, 0, 0, 0, 1, 0, 0, 0, 1), Vector3.Zero);
|
|
@@ -399,11 +362,187 @@ namespace Godot
|
|
/// <returns>The composed transform.</returns>
|
|
/// <returns>The composed transform.</returns>
|
|
public static Transform3D operator *(Transform3D left, Transform3D right)
|
|
public static Transform3D operator *(Transform3D left, Transform3D right)
|
|
{
|
|
{
|
|
- left.origin = left.Xform(right.origin);
|
|
|
|
|
|
+ left.origin = left * right.origin;
|
|
left.basis *= right.basis;
|
|
left.basis *= right.basis;
|
|
return left;
|
|
return left;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns a Vector3 transformed (multiplied) by the transformation matrix.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <param name="vector">A Vector3 to transform.</param>
|
|
|
|
+ /// <returns>The transformed Vector3.</returns>
|
|
|
|
+ public static Vector3 operator *(Transform3D transform, Vector3 vector)
|
|
|
|
+ {
|
|
|
|
+ return new Vector3
|
|
|
|
+ (
|
|
|
|
+ transform.basis.Row0.Dot(vector) + transform.origin.x,
|
|
|
|
+ transform.basis.Row1.Dot(vector) + transform.origin.y,
|
|
|
|
+ transform.basis.Row2.Dot(vector) + transform.origin.z
|
|
|
|
+ );
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns a Vector3 transformed (multiplied) by the transposed transformation matrix.
|
|
|
|
+ ///
|
|
|
|
+ /// Note: This results in a multiplication by the inverse of the
|
|
|
|
+ /// transformation matrix only if it represents a rotation-reflection.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="vector">A Vector3 to inversely transform.</param>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <returns>The inversely transformed Vector3.</returns>
|
|
|
|
+ public static Vector3 operator *(Vector3 vector, Transform3D transform)
|
|
|
|
+ {
|
|
|
|
+ Vector3 vInv = vector - transform.origin;
|
|
|
|
+
|
|
|
|
+ return new Vector3
|
|
|
|
+ (
|
|
|
|
+ (transform.basis.Row0[0] * vInv.x) + (transform.basis.Row1[0] * vInv.y) + (transform.basis.Row2[0] * vInv.z),
|
|
|
|
+ (transform.basis.Row0[1] * vInv.x) + (transform.basis.Row1[1] * vInv.y) + (transform.basis.Row2[1] * vInv.z),
|
|
|
|
+ (transform.basis.Row0[2] * vInv.x) + (transform.basis.Row1[2] * vInv.y) + (transform.basis.Row2[2] * vInv.z)
|
|
|
|
+ );
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns an AABB transformed (multiplied) by the transformation matrix.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <param name="aabb">An AABB to transform.</param>
|
|
|
|
+ /// <returns>The transformed AABB.</returns>
|
|
|
|
+ public static AABB operator *(Transform3D transform, AABB aabb)
|
|
|
|
+ {
|
|
|
|
+ Vector3 min = aabb.Position;
|
|
|
|
+ Vector3 max = aabb.Position + aabb.Size;
|
|
|
|
+
|
|
|
|
+ Vector3 tmin = transform.origin;
|
|
|
|
+ Vector3 tmax = transform.origin;
|
|
|
|
+ for (int i = 0; i < 3; i++)
|
|
|
|
+ {
|
|
|
|
+ for (int j = 0; j < 3; j++)
|
|
|
|
+ {
|
|
|
|
+ real_t e = transform.basis[i][j] * min[j];
|
|
|
|
+ real_t f = transform.basis[i][j] * max[j];
|
|
|
|
+ if (e < f)
|
|
|
|
+ {
|
|
|
|
+ tmin[i] += e;
|
|
|
|
+ tmax[i] += f;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ tmin[i] += f;
|
|
|
|
+ tmax[i] += e;
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ return new AABB(tmin, tmax - tmin);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns an AABB transformed (multiplied) by the inverse transformation matrix.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="aabb">An AABB to inversely transform.</param>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <returns>The inversely transformed AABB.</returns>
|
|
|
|
+ public static AABB operator *(AABB aabb, Transform3D transform)
|
|
|
|
+ {
|
|
|
|
+ Vector3 pos = new Vector3(aabb.Position.x + aabb.Size.x, aabb.Position.y + aabb.Size.y, aabb.Position.z + aabb.Size.z) * transform;
|
|
|
|
+ Vector3 to1 = new Vector3(aabb.Position.x + aabb.Size.x, aabb.Position.y + aabb.Size.y, aabb.Position.z) * transform;
|
|
|
|
+ Vector3 to2 = new Vector3(aabb.Position.x + aabb.Size.x, aabb.Position.y, aabb.Position.z + aabb.Size.z) * transform;
|
|
|
|
+ Vector3 to3 = new Vector3(aabb.Position.x + aabb.Size.x, aabb.Position.y, aabb.Position.z) * transform;
|
|
|
|
+ Vector3 to4 = new Vector3(aabb.Position.x, aabb.Position.y + aabb.Size.y, aabb.Position.z + aabb.Size.z) * transform;
|
|
|
|
+ Vector3 to5 = new Vector3(aabb.Position.x, aabb.Position.y + aabb.Size.y, aabb.Position.z) * transform;
|
|
|
|
+ Vector3 to6 = new Vector3(aabb.Position.x, aabb.Position.y, aabb.Position.z + aabb.Size.z) * transform;
|
|
|
|
+ Vector3 to7 = new Vector3(aabb.Position.x, aabb.Position.y, aabb.Position.z) * transform;
|
|
|
|
+
|
|
|
|
+ return new AABB(pos, new Vector3()).Expand(to1).Expand(to2).Expand(to3).Expand(to4).Expand(to5).Expand(to6).Expand(to7);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns a Plane transformed (multiplied) by the transformation matrix.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <param name="plane">A Plane to transform.</param>
|
|
|
|
+ /// <returns>The transformed Plane.</returns>
|
|
|
|
+ public static Plane operator *(Transform3D transform, Plane plane)
|
|
|
|
+ {
|
|
|
|
+ Basis bInvTrans = transform.basis.Inverse().Transposed();
|
|
|
|
+
|
|
|
|
+ // Transform a single point on the plane.
|
|
|
|
+ Vector3 point = transform * (plane.Normal * plane.D);
|
|
|
|
+
|
|
|
|
+ // Use inverse transpose for correct normals with non-uniform scaling.
|
|
|
|
+ Vector3 normal = (bInvTrans * plane.Normal).Normalized();
|
|
|
|
+
|
|
|
|
+ real_t d = normal.Dot(point);
|
|
|
|
+ return new Plane(normal, d);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns a Plane transformed (multiplied) by the inverse transformation matrix.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="plane">A Plane to inversely transform.</param>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <returns>The inversely transformed Plane.</returns>
|
|
|
|
+ public static Plane operator *(Plane plane, Transform3D transform)
|
|
|
|
+ {
|
|
|
|
+ Transform3D tInv = transform.AffineInverse();
|
|
|
|
+ Basis bTrans = transform.basis.Transposed();
|
|
|
|
+
|
|
|
|
+ // Transform a single point on the plane.
|
|
|
|
+ Vector3 point = tInv * (plane.Normal * plane.D);
|
|
|
|
+
|
|
|
|
+ // Note that instead of precalculating the transpose, an alternative
|
|
|
|
+ // would be to use the transpose for the basis transform.
|
|
|
|
+ // However that would be less SIMD friendly (requiring a swizzle).
|
|
|
|
+ // So the cost is one extra precalced value in the calling code.
|
|
|
|
+ // This is probably worth it, as this could be used in bottleneck areas. And
|
|
|
|
+ // where it is not a bottleneck, the non-fast method is fine.
|
|
|
|
+
|
|
|
|
+ // Use transpose for correct normals with non-uniform scaling.
|
|
|
|
+ Vector3 normal = (bTrans * plane.Normal).Normalized();
|
|
|
|
+
|
|
|
|
+ real_t d = normal.Dot(point);
|
|
|
|
+ return new Plane(normal, d);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns a copy of the given Vector3[] transformed (multiplied) by the transformation matrix.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <param name="array">A Vector3[] to transform.</param>
|
|
|
|
+ /// <returns>The transformed copy of the Vector3[].</returns>
|
|
|
|
+ public static Vector3[] operator *(Transform3D transform, Vector3[] array)
|
|
|
|
+ {
|
|
|
|
+ Vector3[] newArray = new Vector3[array.Length];
|
|
|
|
+
|
|
|
|
+ for (int i = 0; i < array.Length; i++)
|
|
|
|
+ {
|
|
|
|
+ newArray[i] = transform * array[i];
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ return newArray;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ /// <summary>
|
|
|
|
+ /// Returns a copy of the given Vector3[] transformed (multiplied) by the inverse transformation matrix.
|
|
|
|
+ /// </summary>
|
|
|
|
+ /// <param name="array">A Vector3[] to inversely transform.</param>
|
|
|
|
+ /// <param name="transform">The transformation to apply.</param>
|
|
|
|
+ /// <returns>The inversely transformed copy of the Vector3[].</returns>
|
|
|
|
+ public static Vector3[] operator *(Vector3[] array, Transform3D transform)
|
|
|
|
+ {
|
|
|
|
+ Vector3[] newArray = new Vector3[array.Length];
|
|
|
|
+
|
|
|
|
+ for (int i = 0; i < array.Length; i++)
|
|
|
|
+ {
|
|
|
|
+ newArray[i] = array[i] * transform;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ return newArray;
|
|
|
|
+ }
|
|
|
|
+
|
|
/// <summary>
|
|
/// <summary>
|
|
/// Returns <see langword="true"/> if the transforms are exactly equal.
|
|
/// Returns <see langword="true"/> if the transforms are exactly equal.
|
|
/// Note: Due to floating-point precision errors, consider using
|
|
/// Note: Due to floating-point precision errors, consider using
|