public void RotateAt(Point3D pt, Quaternion q)
{
foreach (var cuboid in cuboids)
{
cuboid.RotateAt(pt, q);
}
}
/// <summary>
/// Constructor which sets the initial values to bound the two points provided.
/// </summary>
/// <param name="point1">First point.</param>
/// <param name="point2">Second point.</param>
internal Rect3D(Point3D point1, Point3D point2)
{
_x = Math.Min(point1._x, point2._x);
_y = Math.Min(point1._y, point2._y);
_z = Math.Min(point1._z, point2._z);
_sizeX = Math.Max(point1._x, point2._x) - _x;
_sizeY = Math.Max(point1._y, point2._y) - _y;
_sizeZ = Math.Max(point1._z, point2._z) - _z;
}
// Returns the interpolated 3D point from the given positions and barycentric coordinate.
//
// NOTE: v0-v2 and barycentric are passed by ref for performance. They are not modified.
//
internal static Point3D Interpolate(ref Point3D v0, ref Point3D v1, ref Point3D v2, ref Point barycentric)
{
double v = barycentric.X;
double w = barycentric.Y;
double u = 1 - v - w;
return new Point3D(u * v0.X + v * v1.X + w * v2.X,
u * v0.Y + v * v1.Y + w * v2.Y,
u * v0.Z + v * v1.Z + w * v2.Z);
}
public RubikCube2(double a, double b, double c, int rank)
{
DrawingLine = true;
FillingFace = true;
Rank = rank;
center = new Point3D(a / 2, b / 2, c / 2);
points[0] = new Point3D(0, 0, 0);
points[1] = new Point3D(a, 0, 0);
points[2] = new Point3D(a, b, 0);
points[3] = new Point3D(0, b, 0);
points[4] = new Point3D(0, 0, c);
points[5] = new Point3D(a, 0, c);
points[6] = new Point3D(a, b, c);
points[7] = new Point3D(0, b, c);
}
public Cuboid(double a, double b, double c)
{
DrawingLine = false;
FillingFace = true;
DrawingImage = false;
center = new Point3D(a / 2, b / 2, c / 2);
points[0] = new Point3D(0, 0, 0);
points[1] = new Point3D(a, 0, 0);
points[2] = new Point3D(a, b, 0);
points[3] = new Point3D(0, b, 0);
points[4] = new Point3D(0, 0, c);
points[5] = new Point3D(a, 0, c);
points[6] = new Point3D(a, b, c);
points[7] = new Point3D(0, b, c);
}
public RubikCube(int rank_num = 3, int edge = 150)
{
rank = rank_num;
double temp = rank_num * edge / 2.0;
// initial cuboids
cuboids = new Cuboid[rank, rank, rank];
ForEach((i, j, k) =>
{
var cub = new Cuboid(edge, edge, edge);
cub.MoveBy(i * edge - temp, j * edge - temp, k * edge - temp);
cub.DrawingLine = true;
cub.FillingFace = false;
cuboids[i, j, k] = cub;
});
// default center is 0,0,0
center = new Point3D(0, 0, 0);
}
//------------------------------------------------------
//
// Constructors
//
//------------------------------------------------------
/// <summary>
/// Constructor which sets the initial values to the values of the parameters.
/// </summary>
/// <param name="location">Location of the new rectangle.</param>
/// <param name="size">Size of the new rectangle.</param>
public Rect3D(Point3D location, Size3D size)
{
if (size.IsEmpty)
{
this = s_empty;
}
else
{
_x = location._x;
_y = location._y;
_z = location._z;
_sizeX = size._x;
_sizeY = size._y;
_sizeZ = size._z;
}
Debug.Assert(size.IsEmpty == IsEmpty);
}
// Helper method for compiting the bounds of a set of points. The given point
// is added to the bounds of the given Rect3D. The point/bounds are both passed
// by reference for perf. Only the bounds may be modified.
private static void AddPointToBounds(ref Point3D point, ref Rect3D bounds)
{
Debug.Assert(!bounds.IsEmpty, "Caller should construct the Rect3D from the first point before calling this method.");
if (point.X < bounds.X)
{
bounds.SizeX += (bounds.X - point.X);
bounds.X = point.X;
}
else if (point.X > (bounds.X + bounds.SizeX))
{
bounds.SizeX = point.X - bounds.X;
}
if (point.Y < bounds.Y)
{
bounds.SizeY += (bounds.Y - point.Y);
bounds.Y = point.Y;
}
else if (point.Y > (bounds.Y + bounds.SizeY))
{
bounds.SizeY = point.Y - bounds.Y;
}
if (point.Z < bounds.Z)
{
bounds.SizeZ += (bounds.Z - point.Z);
bounds.Z = point.Z;
}
else if (point.Z > (bounds.Z + bounds.SizeZ))
{
bounds.SizeZ = point.Z - bounds.Z;
}
#if NEVER
// Because we do not store rectangles as TLRB (+ another dimension in 3D)
// we need to compute SizeX/Y/Z which involves subtraction and introduces
// cancelation so this assert isn't accurate.
Debug.Assert(bounds.Contains(point),
"Error detect - bounds did not contain point on exit.");
#endif
}
//------------------------------------------------------
//
// Transformation Services
//
//------------------------------------------------------
/// <summary>
/// Transforms the given Point3D by this matrix, projecting the
/// result back into the W=1 plane.
/// </summary>
/// <param name="point">Point to transform.</param>
/// <returns>Transformed point.</returns>
public Point3D Transform(Point3D point)
{
MultiplyPoint(ref point);
return point;
}
/// <summary>
/// Prepends scale transform to the current matrix.
/// </summary>
/// <param name="scale">Scaling vector for transformation.</param>
/// <param name="center">Point around which to scale.</param>
public void ScaleAtPrepend(Vector3D scale, Point3D center)
{
if (IsDistinguishedIdentity)
{
SetScaleMatrix(ref scale, ref center);
}
else
{
double csx = center.X - center.X * scale.X;
double csy = center.Y - center.Y * scale.Y;
double csz = center.Z - center.Z * scale.Z;
// We have to set the bottom row first because it depends
// on values that will change
_offsetX += _m11 * csx + _m21 * csy + _m31 * csz;
_offsetY += _m12 * csx + _m22 * csy + _m32 * csz;
_offsetZ += _m13 * csx + _m23 * csy + _m33 * csz;
_m44 += _m14 * csx + _m24 * csy + _m34 * csz;
_m11 *= scale.X; _m12 *= scale.X; _m13 *= scale.X; _m14 *= scale.X;
_m21 *= scale.Y; _m22 *= scale.Y; _m23 *= scale.Y; _m24 *= scale.Y;
_m31 *= scale.Z; _m32 *= scale.Z; _m33 *= scale.Z; _m34 *= scale.Z;
}
}
/// <summary>
/// Appends scale transform to the current matrix.
/// </summary>
/// <param name="scale">Scaling vector for transformation.</param>
/// <param name="center">Point around which to scale.</param>
public void ScaleAt(Vector3D scale, Point3D center)
{
if (IsDistinguishedIdentity)
{
SetScaleMatrix(ref scale, ref center);
}
else
{
double tmp = _m14 * center.X;
_m11 = tmp + scale.X * (_m11 - tmp);
tmp = _m14 * center.Y;
_m12 = tmp + scale.Y * (_m12 - tmp);
tmp = _m14 * center.Z;
_m13 = tmp + scale.Z * (_m13 - tmp);
tmp = _m24 * center.X;
_m21 = tmp + scale.X * (_m21 - tmp);
tmp = _m24 * center.Y;
_m22 = tmp + scale.Y * (_m22 - tmp);
tmp = _m24 * center.Z;
_m23 = tmp + scale.Z * (_m23 - tmp);
tmp = _m34 * center.X;
_m31 = tmp + scale.X * (_m31 - tmp);
tmp = _m34 * center.Y;
_m32 = tmp + scale.Y * (_m32 - tmp);
tmp = _m34 * center.Z;
_m33 = tmp + scale.Z * (_m33 - tmp);
tmp = _m44 * center.X;
_offsetX = tmp + scale.X * (_offsetX - tmp);
tmp = _m44 * center.Y;
_offsetY = tmp + scale.Y * (_offsetY - tmp);
tmp = _m44 * center.Z;
_offsetZ = tmp + scale.Z * (_offsetZ - tmp);
}
}
/// <summary>
/// Prepends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
/// <param name="center">Center to rotate around.</param>
public void RotateAtPrepend(Quaternion quaternion, Point3D center)
{
this = CreateRotationMatrix(ref quaternion, ref center) * this;
}
/// <summary>
/// Appends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
/// <param name="center">Center to rotate around.</param>
public void RotateAt(Quaternion quaternion, Point3D center)
{
this *= CreateRotationMatrix(ref quaternion, ref center);
}
// Creates a rotation matrix given a quaternion and center.
//
// Quaternion and center are passed by reference for performance
// only and are not modified.
//
internal static Matrix3D CreateRotationMatrix(ref Quaternion quaternion, ref Point3D center)
{
Matrix3D matrix = s_identity;
matrix.IsDistinguishedIdentity = false; // Will be using direct member access
double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
x2 = quaternion.X + quaternion.X;
y2 = quaternion.Y + quaternion.Y;
z2 = quaternion.Z + quaternion.Z;
xx = quaternion.X * x2;
xy = quaternion.X * y2;
xz = quaternion.X * z2;
yy = quaternion.Y * y2;
yz = quaternion.Y * z2;
zz = quaternion.Z * z2;
wx = quaternion.W * x2;
wy = quaternion.W * y2;
wz = quaternion.W * z2;
matrix._m11 = 1.0 - (yy + zz);
matrix._m12 = xy + wz;
matrix._m13 = xz - wy;
matrix._m21 = xy - wz;
matrix._m22 = 1.0 - (xx + zz);
matrix._m23 = yz + wx;
matrix._m31 = xz + wy;
matrix._m32 = yz - wx;
matrix._m33 = 1.0 - (xx + yy);
if (center.X != 0 || center.Y != 0 || center.Z != 0)
{
matrix._offsetX = -center.X * matrix._m11 - center.Y * matrix._m21 - center.Z * matrix._m31 + center.X;
matrix._offsetY = -center.X * matrix._m12 - center.Y * matrix._m22 - center.Z * matrix._m32 + center.Y;
matrix._offsetZ = -center.X * matrix._m13 - center.Y * matrix._m23 - center.Z * matrix._m33 + center.Z;
}
return matrix;
}
// Multiplies the given Point3D by this matrix, projecting the
// result back into the W=1 plane.
//
// The point is modified in place for performance.
//
internal void MultiplyPoint(ref Point3D point)
{
if (IsDistinguishedIdentity)
return;
double x = point.X;
double y = point.Y;
double z = point.Z;
point.X = x * _m11 + y * _m21 + z * _m31 + _offsetX;
point.Y = x * _m12 + y * _m22 + z * _m32 + _offsetY;
point.Z = x * _m13 + y * _m23 + z * _m33 + _offsetZ;
if (!IsAffine)
{
double w = x * _m14 + y * _m24 + z * _m34 + _m44;
point.X /= w;
point.Y /= w;
point.Z /= w;
}
}
// CTAABB for non-affine transformations
internal static Rect3D ComputeTransformedAxisAlignedBoundingBoxNonAffine(/* IN */ ref Rect3D originalBox, /* IN */ ref Matrix3D matrix)
{
Debug.Assert(!matrix.IsAffine);
double x1 = originalBox.X;
double y1 = originalBox.Y;
double z1 = originalBox.Z;
double x2 = originalBox.X + originalBox.SizeX;
double y2 = originalBox.Y + originalBox.SizeY;
double z2 = originalBox.Z + originalBox.SizeZ;
Point3D[] points = new Point3D[] {
new Point3D(x1, y1, z1),
new Point3D(x1, y1, z2),
new Point3D(x1, y2, z1),
new Point3D(x1, y2, z2),
new Point3D(x2, y1, z1),
new Point3D(x2, y1, z2),
new Point3D(x2, y2, z1),
new Point3D(x2, y2, z2),
};
matrix.Transform(points);
Point3D p = points[0];
Rect3D newBounds = new Rect3D(p.X, p.Y, p.Z, 0, 0, 0);
// Traverse the entire mesh and compute bounding box.
for (int i = 1; i < points.Length; i++)
{
p = points[i];
AddPointToBounds(ref p, ref newBounds);
}
return newBounds;
}
/// <summary>
/// Return the result of the union of rect and point.
/// </summary>
/// <param name="rect">Rectangle.</param>
/// <param name="point">Point.</param>
/// <returns>The result of the union of rect and point.</returns>
public static Rect3D Union(Rect3D rect, Point3D point)
{
rect.Union(new Rect3D(point, point));
return rect;
}
/// <summary>
/// Update this rectangle to be the union of this and point.
/// </summary>
/// <param name="point">Point.</param>
public void Union(Point3D point)
{
Union(new Rect3D(point, point));
}
/// <summary>
/// Returns true if the point is within the rectangle, inclusive of the edges.
/// Returns false otherwise.
/// </summary>
/// <param name="point">The point which is being tested.</param>
/// <returns>True if the point is within the rectangle. False otherwise</returns>
public bool Contains(Point3D point)
{
return Contains(point._x, point._y, point._z);
}
/// <summary>
/// Transforms the given Point3Ds by this matrix, projecting the
/// results back into the W=1 plane.
/// </summary>
/// <param name="points">Points to transform.</param>
public void Transform(Point3D[] points)
{
if (points != null)
{
for (int i = 0; i < points.Length; i++)
{
MultiplyPoint(ref points[i]);
}
}
}
//------------------------------------------------------
//
// Rotate
//
//------------------------------------------------------
/// <summary>
/// Appends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
public void Rotate(Quaternion quaternion)
{
Point3D center = new Point3D();
this *= CreateRotationMatrix(ref quaternion, ref center);
}
internal void SetScaleMatrix(ref Vector3D scale, ref Point3D center)
{
Debug.Assert(IsDistinguishedIdentity);
_m11 = scale.X;
_m22 = scale.Y;
_m33 = scale.Z;
_m44 = 1.0;
_offsetX = center.X - center.X * scale.X;
_offsetY = center.Y - center.Y * scale.Y;
_offsetZ = center.Z - center.Z * scale.Z;
IsDistinguishedIdentity = false;
}
/// <summary>
/// Prepends rotation transform to the current matrix.
/// </summary>
/// <param name="quaternion">Quaternion representing rotation.</param>
public void RotatePrepend(Quaternion quaternion)
{
Point3D center = new Point3D();
this = CreateRotationMatrix(ref quaternion, ref center) * this;
}
/// <summary>
/// Constructor which sets the initial values to bound the point provided and the point
/// which results from point + vector.
/// </summary>
/// <param name="point">Location of the rectangle.</param>
/// <param name="vector">Vector extending the rectangle from the location.</param>
internal Rect3D(Point3D point, Vector3D vector)
: this(point, point + vector)
{
}
/// <summary>
/// Function tests to see if the given texture coordinate point p is contained within the
/// given triangle. If it is it returns the 3D point corresponding to that intersection.
/// </summary>
/// <param name="p">The point to test</param>
/// <param name="triUVVertices">The texture coordinates of the triangle</param>
/// <param name="tri3DVertices">The 3D coordinates of the triangle</param>
/// <param name="inters3DPoint">The 3D point of intersection</param>
/// <returns>True if the point is in the triangle, false otherwise</returns>
internal static bool IsPointInTriangle(Point p, Point[] triUVVertices, Point3D[] tri3DVertices, out Point3D inters3DPoint)
{
double denom = 0.0;
inters3DPoint = new Point3D();
//
// get the barycentric coordinates and then use these to test if the point is in the triangle
// any standard math reference on barycentric coordinates will give the derivation for the below
// parameters.
//
double A = triUVVertices[0].X - triUVVertices[2].X;
double B = triUVVertices[1].X - triUVVertices[2].X;
double C = triUVVertices[2].X - p.X;
double D = triUVVertices[0].Y - triUVVertices[2].Y;
double E = triUVVertices[1].Y - triUVVertices[2].Y;
double F = triUVVertices[2].Y - p.Y;
denom = (A * E - B * D);
if (denom == 0)
{
return false;
}
double lambda1 = (B * F - C * E) / denom;
denom = (B * D - A * E);
if (denom == 0)
{
return false;
}
double lambda2 = (A * F - C * D) / denom;
if (lambda1 < 0 || lambda1 > 1 ||
lambda2 < 0 || lambda2 > 1 ||
(lambda1 + lambda2) > 1)
{
return false;
}
inters3DPoint = (Point3D)(lambda1 * (Vector3D)tri3DVertices[0] +
lambda2 * (Vector3D)tri3DVertices[1] +
(1.0f - lambda1 - lambda2) * (Vector3D)tri3DVertices[2]);
return true;
}