/// <summary>
/// Create a new rotation matrix based on a rotation axis and an angle.
/// </summary>
/// <param name="axis">Axis around which the rotation is executed</param>
/// <param name="angle">Angle of the clockwise rotation.</param>
public RotationMatrix3d(Cartesian3dCoordinate axis, double angle)
{
// Angle is reverse to provide a clockwise rotation.
double c = Trigonometry.Cos(-angle);
double s = Trigonometry.Sin(-angle);
double t = 1.0 - c;
// Need to use normalised vector.
axis = axis.Normalize();
// Create the matrix
m_Matrix = new double[3, 3];
m_Matrix[0, 0] = c + (axis.X * axis.X * t);
m_Matrix[1, 1] = c + (axis.Y * axis.Y * t);
m_Matrix[2, 2] = c + (axis.Z * axis.Z * t);
double part1 = axis.X * axis.Y * t;
double part2 = axis.Z * s;
m_Matrix[1, 0] = part1 + part2;
m_Matrix[0, 1] = part1 - part2;
part1 = axis.X * axis.Z * t;
part2 = axis.Y * s;
m_Matrix[2, 0] = part1 - part2;
m_Matrix[0, 2] = part1 + part2;
part1 = axis.Y * axis.Z * t;
part2 = axis.X * s;
m_Matrix[2, 1] = part1 + part2;
m_Matrix[1, 2] = part1 - part2;
}
public void Plane_GetIntersectionFromPlaneAndCheckResultProperties_Expected(string planeCoordinate, string secondPlaneCoordinate, string expectedCoordinate)
{
// 1. Prepare
ParametricLine expected = new ParametricLine(expectedCoordinate);
Cartesian3dCoordinate expectedVector = expected.Vector.Normalize();
Plane p1 = new Plane(planeCoordinate);
Plane p2 = new Plane(secondPlaneCoordinate);
// 2. Execute
ParametricLine l = p1.GetIntersection(p2);
Cartesian3dCoordinate vector = l.Vector.Normalize();
// Warning ! thoses tests are dependent of other methods, in case of errors, checks the UnitsTests for those methods used for validation.
bool isOnP1 = p1.IsOnPlane(l.Point);
bool isOnP2 = p2.IsOnPlane(l.Point);
bool isFurtherOnP1 = p1.IsOnPlane(l.Point + l.Vector);
bool isFurtherOnP2 = p2.IsOnPlane(l.Point + l.Vector);
bool isParrallelToP1 = new ParametricLine(l.Vector).IsParallelTo(p1);
bool isParrallelToP2 = new ParametricLine(l.Vector).IsParallelTo(p2);
// 3. Verify
Assert.True(isOnP1);
Assert.True(isOnP2);
Assert.True(isFurtherOnP1);
Assert.True(isFurtherOnP2);
Assert.True(isParrallelToP1);
Assert.True(isParrallelToP2);
Assert.Equal(expected.X, l.X, PRECISION_DOUBLE);
Assert.Equal(expected.Y, l.Y, PRECISION_DOUBLE);
Assert.Equal(expected.Z, l.Z, PRECISION_DOUBLE);
Assert.Equal(expectedVector.X, vector.X, PRECISION_DOUBLE);
Assert.Equal(expectedVector.Y, vector.Y, PRECISION_DOUBLE);
Assert.Equal(expectedVector.Z, vector.Z, PRECISION_DOUBLE);
}
/// <summary>
/// Rotate the provided vector based using this rotation.
/// </summary>
/// <param name="cc">Cartesian COordiante of the point to rotate.</param>
/// <returns>Rotated value.</returns>
public Cartesian3dCoordinate Rotate(Cartesian3dCoordinate cc)
{
return(new Cartesian3dCoordinate(
(cc.X * m_Matrix[0, 0]) + (cc.Y * m_Matrix[0, 1]) + (cc.Z * m_Matrix[0, 2]),
(cc.X * m_Matrix[1, 0]) + (cc.Y * m_Matrix[1, 1]) + (cc.Z * m_Matrix[1, 2]),
(cc.X * m_Matrix[2, 0]) + (cc.Y * m_Matrix[2, 1]) + (cc.Z * m_Matrix[2, 2])));
}
/// <summary>
/// Perform the switch of positions of blocks and their rotation on themselve.
/// </summary>
/// <typeparam name="T">Type of blocks available in the collection to switch and rotate.</typeparam>
/// <param name="blocks">Sorted collection for which position will be switched. Each block will take the position of the next one.</param>
/// <param name="rotationAxis">Axis used for the rotation of the blocks.</param>
/// <param name="theta">Angle in radians of the rotations to execute on each blocks.</param>
protected void SwitchAndRotate <T>(IList <T> blocks, Cartesian3dCoordinate rotationAxis, double theta)
where T : Block
{
// No switch to perform if their is not at least 2 blocks.
if (blocks.Count > 1)
{
// Store position of the first block that need to be set to the last one.
var firstPosition = blocks[0].Position;
// Update all intermediate blocks positions.
for (int i = 0; i < blocks.Count - 1; ++i)
{
blocks[i].Position = blocks[i + 1].Position;
}
// Finalize the switch by replacing the position of the last one with the one from the first one.
blocks[blocks.Count - 1].Position = firstPosition;
}
if (!theta.IsZero())
{
// Rotate the block aroung themselve.
foreach (var b in blocks)
{
b.RotateAround(rotationAxis, theta);
}
}
}
/// <summary>
/// Create a standard Cube Edge.
/// </summary>
/// <param name="initialPosition">Initial position vector of the block in the cube.</param>
/// <param name="face1">First Visible face of the block.</param>
/// <param name="face2">Second Visible face of the block.</param>
public EdgeBlock(Cartesian3dCoordinate initialPosition, BlockFace face1, BlockFace face2)
: base(new BlockFace[] { face1, face2 })
{
base.Position = initialPosition;
// Each Edge block is unique in the cube and be identified by his face.
Id = $"E_{face1.Id}{face2.Id}";
}
/// <summary>
/// Create a standard Cube Corner.
/// </summary>
/// <param name="initialPosition">Initial position vector of the block in the cube.</param>
/// <param name="face1">First visible face of the block.</param>
/// <param name="face2">Second visible face of the block.</param>
/// <param name="face3">Third visible face of the block.</param>
public CornerBlock(Cartesian3dCoordinate initialPosition, BlockFace face1, BlockFace face2, BlockFace face3)
: base(new BlockFace[] { face1, face2, face3 })
{
base.Position = initialPosition;
// Each corner block is unique in the cube and be identified by the combination of his 3 faces.
m_Id = $"C{face1.Id}{face2.Id}{face3.Id}";
}
/// <summary>
/// Create a standard Cube Corner.
/// </summary>
/// <param name="initialPosition">Initial position vector of the block in the cube.</param>
/// <param name="face">Visible face of the block.</param>
public CenterBlock(Cartesian3dCoordinate initialPosition, BlockFace face)
: base(face)
{
base.Position = initialPosition;
// Each corner block is unique in the cube and be identified by his face.
Id = $"CF_{face.Id}";
}
public void Plane_CheckPointIsAbovePlane_Expected(Cartesian3dCoordinate planeNormal, Cartesian3dCoordinate planePoint, Cartesian3dCoordinate point, bool expected)
{
// 1. Prepare
Plane p = new Plane(planeNormal, planePoint);
// 2. Execute
bool isAbovePlane = p.IsAbovePlane(point);
// 3. Verify
Assert.Equal(expected, isAbovePlane);
}
public void Plane_CheckCreationPointIsOnPlane_True(Cartesian3dCoordinate normal, Cartesian3dCoordinate point, double expectedA, double expectedB, double expectedC, double expectedD)
{
// 1. Prepare
Plane p = new Plane(normal, point);
// 2. Execute
bool isOnPlane = p.IsOnPlane(point);
// 3. Verify
Assert.True(isOnPlane);
}
public void CoordinateConverter_ConvertFromCartesianToSpherical_BeExpected(double phi, double theta, double x, double y, double z)
{
// 1. Prepare
Cartesian3dCoordinate cc = new Cartesian3dCoordinate(x, y, z);
// 2. Execute
var sc = CoordinateConverter.ConvertToSpherical(cc);
// 3. Verify
Assert.Equal(phi, sc.Phi);
Assert.Equal(theta, sc.Theta);
}
public void CoordinateConverter_ConvertFromHomogeneousToCartesian_BeExpected(double hx, double hy, double hz, double hw, double x, double y, double z)
{
// 1. Prepare
HomogeneousCoordinate hc = new HomogeneousCoordinate(hx, hy, hz, hw);
// 2. Execute
Cartesian3dCoordinate cc = CoordinateConverter.ConvertToCartesian(hc);
// 3. Verify
Assert.Equal(x, cc.X, PRECISION_DOUBLE);
Assert.Equal(y, cc.Y, PRECISION_DOUBLE);
Assert.Equal(z, cc.Z, PRECISION_DOUBLE);
}
public void CoordinateConverter_ConvertFromSphericalToCartesian_BeExpected(double phi, double theta, double x, double y, double z)
{
// 1. Prepare
SphericalVector sc = new SphericalVector(phi, theta);
// 2. Execute
Cartesian3dCoordinate cc = CoordinateConverter.ConvertToCartesian(sc);
// 3. Verify
Assert.Equal(x, cc.X, PRECISION_DOUBLE);
Assert.Equal(y, cc.Y, PRECISION_DOUBLE);
Assert.Equal(z, cc.Z, PRECISION_DOUBLE);
}
public void Cartesian3dCoordinate_CreateFromString_BeExpected(string pointCoordinates, double expectedX, double expectedY, double expectedZ)
{
// 1. Prepare
// Nothing to prepare
// 2. Execute
Cartesian3dCoordinate c = new Cartesian3dCoordinate(pointCoordinates);
// 3. Verify
Assert.Equal(expectedX, c.X, PRECISION_DOUBLE);
Assert.Equal(expectedY, c.Y, PRECISION_DOUBLE);
Assert.Equal(expectedZ, c.Z, PRECISION_DOUBLE);
}
public void Cartesian3dCoordinate_RotateAroundVector_BeExpected(Cartesian3dCoordinate source, double theta, Cartesian3dCoordinate k, Cartesian3dCoordinate expected)
{
// 1. Prepare
// Nothing to prepare.
// 2. Execute
Cartesian3dCoordinate r = source.RotateAroundVector(k, theta);
// 3. Verify
Assert.Equal(expected.X, r.X, PRECISION_DOUBLE);
Assert.Equal(expected.Y, r.Y, PRECISION_DOUBLE);
Assert.Equal(expected.Z, r.Z, PRECISION_DOUBLE);
}
public void CoordinateConverter_ConvertFromCartesianToHomogeneous_BeExpected(double hx, double hy, double hz, double hw, double x, double y, double z)
{
// 1. Prepare
Cartesian3dCoordinate cc = new Cartesian3dCoordinate(x, y, z);
// 2. Execute
var hc = CoordinateConverter.ConvertToHomogeneous(cc);
// 3. Verify
Assert.Equal(hx, hc.X);
Assert.Equal(hy, hc.Y);
Assert.Equal(hz, hc.Z);
Assert.Equal(hw, hc.W);
}
public void Cartesian3dCoordinate_IsSameVector_False(string cc1, string cc2)
{
// 1. Prepare
Cartesian3dCoordinate c1 = new Cartesian3dCoordinate(cc1);
Cartesian3dCoordinate c2 = new Cartesian3dCoordinate(cc2);
// 2. Execute
bool r1 = c1.IsSameVector(c2);
bool r2 = c2.IsSameVector(c1);
// 3. Verify
Assert.False(r1);
Assert.False(r2);
}
public void Cartesian3dCoordinate_NormalizeValues_ShouldMatch(string coord, string expected)
{
// 1. Prepare
Cartesian3dCoordinate cc = new Cartesian3dCoordinate(coord);
Cartesian3dCoordinate expectedCoordinates = new Cartesian3dCoordinate(expected);
// 2. Execute
Cartesian3dCoordinate r = cc.Normalize();
// 3. Verify
Assert.Equal(expectedCoordinates.X, r.X, PRECISION_DOUBLE);
Assert.Equal(expectedCoordinates.Y, r.Y, PRECISION_DOUBLE);
Assert.Equal(expectedCoordinates.Z, r.Z, PRECISION_DOUBLE);
}
public void ParametricLine_IsParallelToPlane_BeExpected(string planeCoordinates, string ccCoordinate1, string ccCoordinate2, bool result)
{
// 1. Prepare
Plane p = new Plane(planeCoordinates);
Cartesian3dCoordinate p1 = new Cartesian3dCoordinate(ccCoordinate1);
Cartesian3dCoordinate p2 = new Cartesian3dCoordinate(ccCoordinate2);
ParametricLine line = ParametricLine.FromTwoPoints(p1, p2);
// 2. Execute
bool b = line.IsParallelTo(p);
// 3. Verify
Assert.Equal(result, b);
}
public void Plane_CreateFromNormalAndPoint_BeExpected(Cartesian3dCoordinate normal, Cartesian3dCoordinate point, double expectedA, double expectedB, double expectedC, double expectedD)
{
// 1. Prepare
// Nothing to prepare
// 2. Execute
Plane p = new Plane(normal, point);
// 3. Verify
Assert.Equal(expectedA, p.A, PRECISION_DOUBLE);
Assert.Equal(expectedB, p.B, PRECISION_DOUBLE);
Assert.Equal(expectedC, p.C, PRECISION_DOUBLE);
Assert.Equal(expectedD, p.D, PRECISION_DOUBLE);
}
public void Cartesian3dCoordinate_CalculateThetaBetweenVector_ReturnExpected(string c1Cc, string c2Cc, double expected)
{
// 1. Prepare
Cartesian3dCoordinate c1 = new Cartesian3dCoordinate(c1Cc);
Cartesian3dCoordinate c2 = new Cartesian3dCoordinate(c2Cc);
// 2. Execute
double r1 = c1.GetThetaTo(c2);
double r2 = c2.GetThetaTo(c1);
// 3. Verify
Assert.Equal(expected, r1, PRECISION_DOUBLE);
Assert.Equal(expected, r2, PRECISION_DOUBLE);
}
public void Cartesian3dCoordinate_SubstractVector_BeExpected(string sourceCc, string substractedCc, string expectedCc)
{
// 1. Prepare
Cartesian3dCoordinate source = new Cartesian3dCoordinate(sourceCc);
Cartesian3dCoordinate substracted = new Cartesian3dCoordinate(substractedCc);
Cartesian3dCoordinate expected = new Cartesian3dCoordinate(expectedCc);
// 2. Execute
Cartesian3dCoordinate r = source - substracted;
// 3. Verify
Assert.Equal(expected.X, r.X, PRECISION_DOUBLE);
Assert.Equal(expected.Y, r.Y, PRECISION_DOUBLE);
Assert.Equal(expected.Z, r.Z, PRECISION_DOUBLE);
}
public void Cartesian3dCoordinate_TransposeFromReferential_Expected(string originCoordinate, string vectorCoordinate, string expectedCoordiante)
{
// 1. Prepare
Cartesian3dCoordinate origin = new Cartesian3dCoordinate(originCoordinate);
Cartesian3dCoordinate vector = new Cartesian3dCoordinate(vectorCoordinate);
Cartesian3dCoordinate expected = new Cartesian3dCoordinate(expectedCoordiante);
// 2. Execute
Cartesian3dCoordinate r = vector.TransposeFromReferential(origin);
// 3. Verify
Assert.Equal(expected.X, r.X, PRECISION_DOUBLE);
Assert.Equal(expected.Y, r.Y, PRECISION_DOUBLE);
Assert.Equal(expected.Z, r.Z, PRECISION_DOUBLE);
}
public void Plane_GetIntersectionFromCartesian3dCoordinate_Expected(string planeCoordinate, string Cartesian3dCoordinate, string expectedCoordinate)
{
// 1. Prepare
Cartesian3dCoordinate expected = new Cartesian3dCoordinate(expectedCoordinate);
Cartesian3dCoordinate cc = new Cartesian3dCoordinate(Cartesian3dCoordinate);
Plane p = new Plane(planeCoordinate);
// 2. Execute
Cartesian3dCoordinate r = p.GetIntersection(cc);
// 3. Verify
Assert.Equal(expected.X, r.X, PRECISION_DOUBLE);
Assert.Equal(expected.Y, r.Y, PRECISION_DOUBLE);
Assert.Equal(expected.Z, r.Z, PRECISION_DOUBLE);
}
/// <summary>
/// Create a new RotationAxis object.
/// </summary>
/// <param name="id">Id of the RotationAxis.</param>
/// <param name="axis">Rotation Axis coordinate starting from the core center.</param>
/// <exception cref="System.ArgumentNullException">Axis id is mandatory.</exception>
/// <exception cref="System.ArgumentException">Axis id cannot be an empty or a white string.</exception>
public RotationAxis(string id, Cartesian3dCoordinate axis)
{
if (id == null)
{
throw new ArgumentNullException(nameof(id), "Axis id is mandatory.");
}
if (string.IsNullOrWhiteSpace(id))
{
throw new ArgumentException("Axis id cannot be an empty or a white string.", nameof(id));
}
this.Id = id;
this.Vector = axis;
}
// Rotated Conversions
public void Cartesian3dCoordinatesConverter_Convert3dTo2d_BeExpected(string cPlane, string c3d, string c2d)
{
// 1. Prepare
Plane p = new Plane(cPlane);
CartesianCoordinatesConverter converter = new CartesianCoordinatesConverter(p);
Cartesian2dCoordinate expected = new Cartesian2dCoordinate(c2d);
Cartesian3dCoordinate source = new Cartesian3dCoordinate(c3d);
// 2. Execute
Cartesian2dCoordinate result = converter.ConvertTo2d(source);
// 3. Verify
Assert.Equal(expected.X, result.X, PRECISION_DOUBLE);
Assert.Equal(expected.Y, result.Y, PRECISION_DOUBLE);
}
public void Block_RotateAndGetFace_FindFace(string rotationAxisCc, double theta, string faceCc, string expectedCc)
{
// 1. Prepare
Cartesian3dCoordinate axis = new Cartesian3dCoordinate(rotationAxisCc);
Cartesian3dCoordinate faceOrientation = new Cartesian3dCoordinate(faceCc);
Cartesian3dCoordinate expectedOrientation = new Cartesian3dCoordinate(expectedCc);
Block b = new TestBlock(new BlockFace("test", faceOrientation));
// 2. Execute
b.RotateAround(axis, theta);
BlockFace f = b.GetBlockFace(expectedOrientation);
// 3. Verify
Assert.NotNull(f);
}
public void Block_CreateRotateAndGetFaceWithOriginalOrientation_ReturnNull(string axisCc, string originalFace, double theta)
{
// 1. Prepare
Cartesian3dCoordinate axis = new Cartesian3dCoordinate(axisCc);
Cartesian3dCoordinate faceOrientation = new Cartesian3dCoordinate(originalFace);
Block b = new TestBlock(new BlockFace("test", faceOrientation));
b.RotateAround(axis, theta);
// 2. Execute
BlockFace f = b.GetBlockFace(faceOrientation);
// 3. Verify
Assert.Null(f);
}
private void CircularVectorComparer_Compare3dVector_BeExpected(string xCoord, string yCoord, string plane, int expected)
{
// 1. Prepare
Cartesian3dCoordinate x = new Cartesian3dCoordinate(xCoord);
Cartesian3dCoordinate y = new Cartesian3dCoordinate(yCoord);
Plane p = new Plane(plane);
var comparer = new CircularVectorComparer(p);
// 2. Execute
int result = comparer.Compare(x, y);
int reverse = comparer.Compare(y, x);
// 3. Verify
Assert.Equal(expected, result);
Assert.Equal(-expected, reverse);
}
public void ParametricLine_CreateFromTwoPoints_BeExpected(Cartesian3dCoordinate p1, Cartesian3dCoordinate p2, double x, double xt, double y, double yt, double z, double zt)
{
// 1. Prepare
// Nothing to prepare.
// 2. Execute
ParametricLine o = ParametricLine.FromTwoPoints(p1, p2);
// 3. Verify
Assert.Equal(x, o.X, PRECISION_DOUBLE);
Assert.Equal(xt, o.A, PRECISION_DOUBLE);
Assert.Equal(y, o.Y, PRECISION_DOUBLE);
Assert.Equal(yt, o.B, PRECISION_DOUBLE);
Assert.Equal(z, o.Z, PRECISION_DOUBLE);
Assert.Equal(zt, o.C, PRECISION_DOUBLE);
}
public void RotationMatrix3d_RotateRotateVector_Expected(string origin, string axis, double angle, string expected)
{
// Prepare
Cartesian3dCoordinate o = new Cartesian3dCoordinate(origin);
Cartesian3dCoordinate a = new Cartesian3dCoordinate(axis);
Cartesian3dCoordinate e = new Cartesian3dCoordinate(expected);
RotationMatrix3d m = new RotationMatrix3d(a, angle);
// Execute
Cartesian3dCoordinate r = m.Rotate(o);
// Verify
Assert.Equal(e.X, r.X, PRECISION_DOUBLE);
Assert.Equal(e.Y, r.Y, PRECISION_DOUBLE);
Assert.Equal(e.Z, r.Z, PRECISION_DOUBLE);
}
