// |x A B D| -1 |30*x.x 30*y.x 30*z.x o.x+x.x+y.x+z.x|
// |A y C E| |30*x.y 30*y.y 30*z.y o.y+x.y+y.y+z.y|
// |B C z F| * |30*x.z 30*y.z 30*z.z o.z+x.z+y.z+z.z|
// |D E F 4| |30 30 30 4 |
public static void CalcP()
{
double x, A, B, C, y, D, E, F, z;
x = xais.X * xais.X + yais.X * yais.X + zais.X * zais.X + origin.X * origin.X;
y = xais.Y * xais.Y + yais.Y * yais.Y + zais.Y * zais.Y + origin.Y * origin.Y;
z = xais.Z * xais.Z + yais.Z * yais.Z + zais.Z * zais.Z + origin.Z * origin.Z;
A = origin.X * origin.Y + xais.X * xais.Y + yais.X * yais.Y + zais.X * zais.Y;
B = origin.X * origin.Z + xais.X * xais.Z + yais.X * yais.Z + zais.X * zais.Z;
C = origin.Y * origin.Z + xais.Y * xais.Z + yais.Y * yais.Z + zais.Y * zais.Z;
D = origin.X + xais.X + yais.X + zais.X;
E = yais.Y + xais.Y + zais.Y + origin.Y;
F = yais.Z + xais.Z + zais.Z + origin.Z;
NewTransfer.Pmatrix = new Matrix3D(x, A, B, D,
A, y, C, E,
B, C, z, F,
D, E, F, 4);
NewTransfer.Pmatrix.Invert();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="matrix">Combined matrix</param>
/// <param name="normalize">Normalize?</param>
public Frustum( Matrix3D matrix, bool normalize )
{
// Left clipping plane
planes[ 0 ].Normal = new Vector3D( matrix.M14 + matrix.M11, matrix.M24 + matrix.M21, matrix.M34 + matrix.M31 );
planes[ 0 ].D = matrix.M44 + matrix.OffsetX;
// Right clipping plane
planes[ 1 ].Normal = new Vector3D( matrix.M14 - matrix.M11, matrix.M24 - matrix.M21, matrix.M34 - matrix.M31 );
planes[ 1 ].D = matrix.M44 - matrix.OffsetX;
// Top clipping plane
planes[ 2 ].Normal = new Vector3D( matrix.M14 - matrix.M12, matrix.M24 - matrix.M22, matrix.M34 - matrix.M32 );
planes[ 2 ].D = matrix.M44 - matrix.OffsetY;
// Bottom clipping plane
planes[ 3 ].Normal = new Vector3D( matrix.M14 + matrix.M12, matrix.M24 + matrix.M22, matrix.M34 + matrix.M32 );
planes[ 3 ].D = matrix.M44 + matrix.OffsetY;
// Near clipping plane
planes[ 4 ].Normal = new Vector3D( matrix.M13, matrix.M23, matrix.M33 );
planes[ 4 ].D = matrix.OffsetZ;
// Far clipping plane
planes[ 5 ].Normal = new Vector3D( matrix.M14 - matrix.M13, matrix.M24 - matrix.M23, matrix.M34 - matrix.M33 );
planes[ 5 ].D = matrix.M44 - matrix.OffsetZ;
// Normalize the plane equations, if requested
if ( normalize )
{
for ( int index = 0; index < planes.Length; ++index ) planes[ index ].Normalize();
}
}
public static Matrix3D LookAt( this Point3D position, Point3D target, Vector3D up )
{
var vector = (Vector3D)position - (Vector3D)target;
vector.Normalize();
var vector2 = Vector3D.CrossProduct( up, vector );
vector2.Normalize();
var vector3 = Vector3D.CrossProduct( vector, vector2 );
var matrix = new Matrix3D();
matrix.M11 = vector2.X;
matrix.M12 = vector3.X;
matrix.M13 = vector.X;
matrix.M14 = 0f;
matrix.M21 = vector2.Y;
matrix.M22 = vector3.Y;
matrix.M23 = vector.Y;
matrix.M24 = 0f;
matrix.M31 = vector2.Z;
matrix.M32 = vector3.Z;
matrix.M33 = vector.Z;
matrix.M34 = 0f;
matrix.OffsetX = -Vector3D.DotProduct( vector2, (Vector3D)position );
matrix.OffsetY = -Vector3D.DotProduct( vector3, (Vector3D)position );
matrix.OffsetZ = -Vector3D.DotProduct( vector, (Vector3D)position );
matrix.M44 = 1f;
return matrix;
}
public void TestConstructor()
{
var m = new Matrix3D();
m.SetColumn(0, 0, 0, 0);
m.SetColumn(1, 0, 0, 0);
m.SetColumn(2, 0, 0, 0);
Assert.IsTrue(Matrix3D.AreEqual(m, new Matrix3D()));
m.SetRow(0, 1, 2, 3);
m.SetRow(1, 4, 5, 6);
m.SetRow(2, 7, 8, 9);
Assert.IsTrue(Matrix3D.AreEqual(m, new Matrix3D(new float[,] {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}})));
Assert.IsTrue(Matrix3D.AreEqual(m, new Matrix3D(new Matrix(new float[,] {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}}))));
try
{
new Matrix3D(new float[,] {{1, 2}, {4, 5}, {6, 7}});
Assert.Fail("Expected an exception");
}
catch (ArgumentException) {}
try
{
new Matrix3D(new float[,] {{1, 2, 3}, {4, 5, 6}});
Assert.Fail("Expected an exception");
}
catch (ArgumentException) {}
try
{
new Matrix3D(new Matrix(4, 5));
Assert.Fail("Expected an exception");
}
catch (ArgumentException) {}
}
public static void MatrixInvert(Matrix3D MatrixIn, Matrix3D MatrixOut)
{
var num = MatrixIn.Values[0];
var num3 = MatrixIn.Values[1];
var num5 = MatrixIn.Values[2];
var num7 = MatrixIn.Values[3];
var num9 = MatrixIn.Values[4];
var num11 = MatrixIn.Values[5];
var num13 = MatrixIn.Values[6];
var num15 = MatrixIn.Values[7];
var num17 = MatrixIn.Values[8];
var num2 = (num9 * num17) - (num15 * num11);
var num4 = (num13 * num11) - (num7 * num17);
var num6 = (num7 * num15) - (num13 * num9);
var num8 = (num15 * num5) - (num3 * num17);
var num10 = (num * num17) - (num13 * num5);
var num12 = (num13 * num3) - (num * num15);
var num14 = (num3 * num11) - (num9 * num5);
var num16 = (num7 * num5) - (num * num11);
var num18 = (num * num9) - (num7 * num3);
var num19 = ((num * num2) + (num3 * num4)) + (num5 * num6);
MatrixOut.Values[0] = num2 / num19;
MatrixOut.Values[1] = num8 / num19;
MatrixOut.Values[2] = num14 / num19;
MatrixOut.Values[3] = num4 / num19;
MatrixOut.Values[4] = num10 / num19;
MatrixOut.Values[5] = num16 / num19;
MatrixOut.Values[6] = num6 / num19;
MatrixOut.Values[7] = num12 / num19;
MatrixOut.Values[8] = num18 / num19;
}
internal ModelMesh(Mesh sourceMesh, Device device, VertexBuffer vertexBuffer, int numVertices,
IndexBuffer indexBuffer, int primitiveCount,
Matrix3D world, Material material)
{
SourceMesh = sourceMesh;
_device = device;
_vertexBuffer = vertexBuffer;
_numVertices = numVertices;
_indexBuffer = indexBuffer;
_primitiveCount = primitiveCount;
_effect = new SimpleEffect(device)
{
World = world,
AmbientLightColor = new ColorRgbF(0.1f, 0.1f, 0.1f),
DiffuseColor = material.DiffuseColor,
SpecularColor = material.SpecularColor,
SpecularPower = material.Shininess,
Alpha = material.Transparency
};
if (!string.IsNullOrEmpty(material.DiffuseTextureName))
_effect.DiffuseTexture = Texture.FromFile(device, material.DiffuseTextureName, Usage.None, Pool.Default);
_effect.CurrentTechnique = "RenderScene";
Opaque = (material.Transparency == 1.0f);
}
/// <summary>
/// Appends transform along to the given direction
/// (the object must have original direction along YAxis(!))
/// </summary>
/// <param name="originalMatrix">Matrix</param>
/// <param name="direction">Direction</param>
/// <returns>Transformation</returns>
public static Matrix3D TransformAlongTo(this Matrix3D originalMatrix, Vector3D direction)
{
Matrix3D matrix = new Matrix3D();
if ((!direction.ApproxEqual(new Vector3D(0,+1,0), 0.001)) &&
(!direction.ApproxEqual(new Vector3D(0,-1,0), 0.001)))
{
Vector3D firstVector = Vector3D.CrossProduct(new Vector3D(0,10,0), direction);
firstVector.Normalize();
// Получаем перпендикуляр к полученному ранее перепендикуляру
Vector3D secondVector = Vector3D.CrossProduct(firstVector, direction);
secondVector.Normalize();
// Получаем матрицу поворота
matrix.M11 = firstVector.X; matrix.M12 = firstVector.Y; matrix.M13 = firstVector.Z;
matrix.M21 = direction.X; matrix.M22 = direction.Y; matrix.M23 = direction.Z;
matrix.M31 = secondVector.X; matrix.M32 = secondVector.Y; matrix.M33 = secondVector.Z;
}
else if (direction.ApproxEqual(new Vector3D(0,-1,0), 0.001))
{
matrix.Scale(new Vector3D(1, -1, 1));
}
else return originalMatrix;
// Append transform
originalMatrix.Append(matrix);
return originalMatrix;
}
public static void CalcF()
{
double A, B, C;
A = origin.X + xais.X + yais.X + zais.X;
B = yais.Y + xais.Y + zais.Y + origin.Y;
C = yais.Z + xais.Z + zais.Z + origin.Z;
NewTransfer.Fmatrix = new Matrix3D(30 * xais.X, 30 * yais.X, 30 * zais.X, A,
30 * xais.Y, 30 * yais.Y, 30 * zais.Y, B,
30 * xais.Z, 30 * yais.Z, 30 * zais.Z, C,
30, 30, 30, 4);
}
public static double MatrixAngleToVector(Matrix3D Matrix, XYZDouble Vector)
{
var matrixOut = new Matrix3D();
MatrixInvert(Matrix, matrixOut);
var y = ((matrixOut.Values[0] * Vector.X) + (matrixOut.Values[1] * Vector.Y)) + (matrixOut.Values[2] * Vector.Z);
var x = ((matrixOut.Values[3] * Vector.X) + (matrixOut.Values[4] * Vector.Y)) + (matrixOut.Values[5] * Vector.Z);
var num3 = ((matrixOut.Values[6] * Vector.X) + (matrixOut.Values[7] * Vector.Y)) + (matrixOut.Values[8] * Vector.Z);
var a = Math.Atan2(y, x);
var num5 = (Math.Sin(a) * y) + (Math.Cos(a) * x);
return Math.Atan2(num5, num3);
}
public static double MatrixAngleToMatrix(Matrix3D MatrixA, Matrix3D MatrixB)
{
var matrixOut = new Matrix3D();
MatrixInvert(MatrixA, matrixOut);
var y = ((matrixOut.Values[0] * MatrixB.Values[2]) + (matrixOut.Values[1] * MatrixB.Values[5])) + (matrixOut.Values[2] * MatrixB.Values[8]);
var x = ((matrixOut.Values[3] * MatrixB.Values[2]) + (matrixOut.Values[4] * MatrixB.Values[5])) + (matrixOut.Values[5] * MatrixB.Values[8]);
var num3 = ((matrixOut.Values[6] * MatrixB.Values[2]) + (matrixOut.Values[7] * MatrixB.Values[5])) + (matrixOut.Values[8] * MatrixB.Values[8]);
var a = Math.Atan2(y, x);
var num5 = (Math.Sin(a) * y) + (Math.Cos(a) * x);
return Math.Atan2(num5, num3);
}
public static void MatrixCopy(Matrix3D MatrixIn, Matrix3D MatrixOut)
{
MatrixOut.Values[0] = MatrixIn.Values[0];
MatrixOut.Values[1] = MatrixIn.Values[1];
MatrixOut.Values[2] = MatrixIn.Values[2];
MatrixOut.Values[3] = MatrixIn.Values[3];
MatrixOut.Values[4] = MatrixIn.Values[4];
MatrixOut.Values[5] = MatrixIn.Values[5];
MatrixOut.Values[6] = MatrixIn.Values[6];
MatrixOut.Values[7] = MatrixIn.Values[7];
MatrixOut.Values[8] = MatrixIn.Values[8];
}
public static void MatrixAngleToVector(Matrix3D Matrix, XYZDouble Vector, ref double ResultArcAngle, ref double ResultDirectionAngle)
{
var matrixOut = new Matrix3D();
MatrixInvert(Matrix, matrixOut);
var x = ((matrixOut.Values[0] * Vector.X) + (matrixOut.Values[1] * Vector.Y)) + (matrixOut.Values[2] * Vector.Z);
var y = ((matrixOut.Values[3] * Vector.X) + (matrixOut.Values[4] * Vector.Y)) + (matrixOut.Values[5] * Vector.Z);
var num3 = ((matrixOut.Values[6] * Vector.X) + (matrixOut.Values[7] * Vector.Y)) + (matrixOut.Values[8] * Vector.Z);
ResultDirectionAngle = Math.Atan2(y, x);
var a = Math.Atan2(x, y);
var num4 = (Math.Sin(a) * x) + (Math.Cos(a) * y);
ResultArcAngle = Math.Atan2(num4, num3);
}
public static void MatrixAngleToMatrix(Matrix3D MatrixA, Matrix3D MatrixB, ref double ResultArcAngle, ref double ResultDirectionAngle)
{
var matrixOut = new Matrix3D();
MatrixInvert(MatrixA, matrixOut);
var x = ((matrixOut.Values[0] * MatrixB.Values[2]) + (matrixOut.Values[1] * MatrixB.Values[5])) + (matrixOut.Values[2] * MatrixB.Values[8]);
var y = ((matrixOut.Values[3] * MatrixB.Values[2]) + (matrixOut.Values[4] * MatrixB.Values[5])) + (matrixOut.Values[5] * MatrixB.Values[8]);
var num3 = ((matrixOut.Values[6] * MatrixB.Values[2]) + (matrixOut.Values[7] * MatrixB.Values[5])) + (matrixOut.Values[8] * MatrixB.Values[8]);
ResultDirectionAngle = Math.Atan2(y, x);
var a = Math.Atan2(x, y);
var num4 = (Math.Sin(a) * x) + (Math.Cos(a) * y);
ResultArcAngle = Math.Atan2(num4, num3);
}
public static Plane3D Transform(Plane3D plane, Matrix3D matrix)
{
Matrix3D matrix2 = Matrix3D.Invert(matrix);
float x = plane.Normal.X;
float y = plane.Normal.Y;
float z = plane.Normal.Z;
float d = plane.D;
Plane3D plane2;
plane2.Normal.X = (((x * matrix2.M11) + (y * matrix2.M12)) + (z * matrix2.M13)) + (d * matrix2.M14);
plane2.Normal.Y = (((x * matrix2.M21) + (y * matrix2.M22)) + (z * matrix2.M23)) + (d * matrix2.M24);
plane2.Normal.Z = (((x * matrix2.M31) + (y * matrix2.M32)) + (z * matrix2.M33)) + (d * matrix2.M34);
plane2.D = (((x * matrix2.M41) + (y * matrix2.M42)) + (z * matrix2.M43)) + (d * matrix2.M44);
return plane2;
}
public void TestAreEqual()
{
var m1 = new Matrix3D();
m1.SetColumn(0, 1, 4, 7);
m1.SetColumn(1, 2, 5, 8);
m1.SetColumn(2, 3, 6, 9);
var m2 = new Matrix3D();
m2.SetRow(0, 1, 2, 3);
m2.SetRow(1, 4, 5, 6);
m2.SetRow(2, 7, 8, 9);
Assert.IsTrue(Matrix3D.AreEqual(m1, m2));
m2[1, 1] = 0;
Assert.IsFalse(Matrix3D.AreEqual(m1, m2));
}
/// <summary>
/// Perform an Isometric projection of the vector.
/// </summary>
/// <param name="alpha">The first angle of projection.</param>
/// <param name="beta">The second angle of projection.</param>
/// <returns></returns>
public Point Project(double alpha, double beta)
{
Matrix3D alphaMat = new Matrix3D();
Matrix3D betaMat = new Matrix3D();
alphaMat.X1 = 1; alphaMat.X2 = 0; alphaMat.X3 = 0;
alphaMat.Y1 = 0; alphaMat.Y2 = Math.Cos(alpha); alphaMat.Y3 = Math.Sin(alpha);
alphaMat.Z1 = 0; alphaMat.Z2 = Math.Sin(alpha) * -1; alphaMat.Z3 = Math.Cos(alpha);
betaMat.X1 = Math.Cos(beta); betaMat.X2 = 0; betaMat.X3 = Math.Sin(beta) * -1;
betaMat.Y1 = 0; betaMat.Y2 = 1; betaMat.Y3 = 0;
betaMat.Z1 = Math.Sin(beta); betaMat.Z2 = 0; betaMat.Z3 = Math.Cos(beta);
Matrix3D projectionMat = alphaMat.Multiply3D(betaMat);
Vector projectionVector = projectionMat.MultiplyVector3(this);
return new Point(projectionVector.X, projectionVector.Y);
}
public void OnBeginDrawModel(Model model, RenderSettings renderSettings, IEnumerable<IDecorator> decorators)
{
if (_creatingShadowMap)
return;
_creatingShadowMap = true;
_cameraFrustum.Matrix = renderSettings.ViewMatrix * renderSettings.ProjectionMatrix;
// Update the lights ViewProjection matrix based on the
// current camera frustum
_lightViewProjection = CreateLightViewProjectionMatrix(model, renderSettings.Parameters.LightDirection);
// Save the current back buffer.
_savedBackBuffer = _device.GetRenderTarget(0);
// Set our render target to our floating point render target
_device.SetRenderTarget(0, _shadowRenderTarget.GetSurfaceLevel(0));
// Save the current stencil buffer
_savedDepthBuffer = _device.DepthStencilSurface;
// Set the graphics device to use the shadow depth stencil buffer
_device.DepthStencilSurface = _shadowDepthBuffer;
// Clear the render target to white or all 1's
// We set the clear to white since that represents the
// furthest the object could be away
_device.Clear(ClearFlags.Target | ClearFlags.ZBuffer, new SlimDX.Color4(1, 1, 1), 1, 0);
model.DrawInternal(renderSettings, decorators);
_creatingShadowMap = false;
// Now blur shadow map.
//_blur.InputTexture = _shadowRenderTarget;
//_blur.Draw();
//_shadowRenderTarget = _blur.OutputTexture;
// Set render target back to the back buffer
_device.SetRenderTarget(0, _savedBackBuffer);
// Reset the depth buffer
_device.DepthStencilSurface = _savedDepthBuffer;
//_shadowRenderTarget.GenerateMipSublevels();
}
public static Matrix ToSharpDXMatrix(Matrix3D matrix)
{
return new Matrix
{
M11 = matrix.M11,
M12 = matrix.M12,
M13 = matrix.M13,
M14 = matrix.M14,
M21 = matrix.M21,
M22 = matrix.M22,
M23 = matrix.M23,
M24 = matrix.M24,
M31 = matrix.M31,
M32 = matrix.M32,
M33 = matrix.M33,
M34 = matrix.M34,
M41 = matrix.M41,
M42 = matrix.M42,
M43 = matrix.M43,
M44 = matrix.M44,
};
}
/// <summary>
/// Transforms the axis-aligned bounding box 'bounds' by 'transform'
/// </summary>
/// <param name="bounds">The AABB to transform</param>
/// <returns>Transformed AABB</returns>
public static Rect3D Transform(this Rect3D bounds, Matrix3D transform)
{
double x1 = bounds.X;
double y1 = bounds.Y;
double z1 = bounds.Z;
double x2 = bounds.X + bounds.SizeX;
double y2 = bounds.Y + bounds.SizeY;
double z2 = bounds.Z + bounds.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),
};
transform.Transform(points);
// reuse the 1 and 2 variables to stand for smallest and largest
Point3D p = points[0];
x1 = x2 = p.X;
y1 = y2 = p.Y;
z1 = z2 = p.Z;
for (int i = 1; i < points.Length; i++)
{
p = points[i];
x1 = Math.Min(x1, p.X); y1 = Math.Min(y1, p.Y); z1 = Math.Min(z1, p.Z);
x2 = Math.Max(x2, p.X); y2 = Math.Max(y2, p.Y); z2 = Math.Max(z2, p.Z);
}
return new Rect3D(x1, y1, z1, x2 - x1, y2 - y1, z2 - z1);
}
internal ModelMesh(Mesh sourceMesh, Device device, VertexBuffer vertexBuffer, int numVertices,
IndexBuffer indexBuffer, int primitiveCount,
Matrix3D world, Material material)
{
SourceMesh = sourceMesh;
_device = device;
_vertexBuffer = vertexBuffer;
_numVertices = numVertices;
_indexBuffer = indexBuffer;
_primitiveCount = primitiveCount;
_effect = new SimpleEffect(device)
{
World = world,
AmbientLightColor = new ColorRgbF(0.1f, 0.1f, 0.1f),
DiffuseColor = material.DiffuseColor,
SpecularColor = material.SpecularColor,
SpecularPower = material.Shininess,
Alpha = material.Transparency
};
_effect.CurrentTechnique = "RenderScene";
Opaque = (material.Transparency == 1.0f);
}
public Renderer(Device device, Model model, int width, int height, Transform3D cameraTransform)
{
_device = device;
_model = model;
_cameraTransform = cameraTransform;
const float fov = MathUtility.PI_OVER_4;
AxisAlignedBox3D bounds = _model.SourceScene.Bounds;
Vector3D max = bounds.Size;
float radius = System.Math.Max(max.X, System.Math.Max(max.Y, max.Z));
_projection = Matrix3D.CreatePerspectiveFieldOfView(
fov,
width / (float) height,
1.0f, radius * 10);
float dist = radius / MathUtility.Sin(fov / 2);
_view = Matrix3D.CreateLookAt(
bounds.Center + Vector3D.Backward * dist,
Vector3D.Forward,
Vector3D.Up);
}
public void Update(Point3D center, Vector3D xAxis, Vector3D yAxis, double xStabilityDir, double yStabilityDir)
{
this.center = center;
this.xAxis = xAxis;
this.yAxis = yAxis;
// Console.WriteLine("xAxis = " + xAxis);
// Console.WriteLine("yAxis = " + yAxis);
Matrix3D mat = new Matrix3D();
Vector3D zAxis = Vector3D.CrossProduct(xAxis, yAxis);
mat.M11 = xAxis.X;
mat.M21 = yAxis.X;
mat.M31 = zAxis.X;
mat.M12 = xAxis.Y;
mat.M22 = yAxis.Y;
mat.M32 = zAxis.Y;
mat.M13 = xAxis.Z;
mat.M23 = yAxis.Z;
mat.M33 = zAxis.Z;
mat.OffsetX = center.X;
mat.OffsetY = center.Y;
mat.OffsetZ = center.Z;
Matrix3D yRotate = new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), 90)).Value;
double yScaleVal = 3 * Math.Max(Math.Min(Math.Abs(yStabilityDir) * 10, 1.0), 0.3);
double xScaleVal = 3 * Math.Max(Math.Min(Math.Abs(xStabilityDir) * 10, 1.0), 0.3);
//Matrix3D yTransMatrix3D = Matrix3D.Multiply(yRotate, mat);
//Matrix3D yTransMatrix3D1 = yTransMatrix3D;
Matrix3D yScale = new ScaleTransform3D(new Vector3D(yScaleVal, yScaleVal, yScaleVal)).Value;
Matrix3D xScale = new ScaleTransform3D(new Vector3D(xScaleVal, xScaleVal, xScaleVal)).Value;
Matrix3D xScale_center = Matrix3D.Multiply(Matrix3D.Multiply((new TranslateTransform3D(-30, 0, -3)).Value, xScale), (new TranslateTransform3D(30, 0, 3)).Value);
Matrix3D yScale_center = Matrix3D.Multiply(Matrix3D.Multiply((new TranslateTransform3D(-30, 0, -3)).Value, yScale), (new TranslateTransform3D(30, 0, 3)).Value);
Matrix3D yTransMatrix3DArrow = Matrix3D.Multiply(yScale_center, Matrix3D.Multiply(yRotate, mat));
Matrix3D xTransMatrix3DArrow = Matrix3D.Multiply(xScale_center, mat);
Matrix3D yTransMatrix3D = Matrix3D.Multiply(yRotate, mat);
Matrix3D xTransMatrix3D = mat;
containerY.Transform = new MatrixTransform3D(yTransMatrix3D);
containerX.Transform = new MatrixTransform3D(xTransMatrix3D);
containerY1.Transform = new MatrixTransform3D(yTransMatrix3DArrow);
containerX1.Transform = new MatrixTransform3D(xTransMatrix3DArrow);
containerY2.Transform = new MatrixTransform3D(yTransMatrix3DArrow);
containerX2.Transform = new MatrixTransform3D(xTransMatrix3DArrow);
const double epsilon = 0;
if (Math.Abs(xStabilityDir) <= epsilon)
{
setArrowVisible(containerX1, false);
setArrowVisible(containerX2, false);
}
else if (xStabilityDir > epsilon)
{
//turn off positive pointing arrow
// turn on negative pointing arrow
setArrowVisible(containerX1, false);
setArrowVisible(containerX2, true);
}
else
{
setArrowVisible(containerX1, true);
setArrowVisible(containerX2, false);
}
// For y direction gizmo
if (Math.Abs(yStabilityDir) <= epsilon)
{
setArrowVisible(containerY1, false);
setArrowVisible(containerY2, false);
}
else if (yStabilityDir > epsilon)
{
//turn off positive pointing arrow
// turn on negative pointing arrow
setArrowVisible(containerY1, false);
setArrowVisible(containerY2, true);
}
else
{
setArrowVisible(containerY1, true);
setArrowVisible(containerY2, false);
}
}
/// <summary>
/// Transforms a Point3D by the given Matrix.
/// </summary>
/// <param name="position">The source Point3D.</param>
/// <param name="matrix">The transformation matrix.</param>
/// <returns>The HomogeneousPoint3D resulting from the transformation.</returns>
public static Point4D Transform(Point3D position, Matrix3D matrix)
{
Point4D result;
result.X = (((position.X * matrix.M11) + (position.Y * matrix.M21)) + (position.Z * matrix.M31)) + matrix.M41;
result.Y = (((position.X * matrix.M12) + (position.Y * matrix.M22)) + (position.Z * matrix.M32)) + matrix.M42;
result.Z = (((position.X * matrix.M13) + (position.Y * matrix.M23)) + (position.Z * matrix.M33)) + matrix.M43;
result.W = (((position.X * matrix.M14) + (position.Y * matrix.M24)) + (position.Z * matrix.M34)) + matrix.M44;
return result;
}
public static Matrix3D compose(Vector3D position, Quaternion quaternion, Vector3D scale, Matrix3D matrix)
{
double x = quaternion.X;
double y = quaternion.Y;
double z = quaternion.Z;
double w = quaternion.W;
double x2 = x + x;
double y2 = y + y;
double z2 = z + z;
double xx = x * x2;
double xy = x * y2;
double xz = x * z2;
double yy = y * y2;
double yz = y * z2;
double zz = z * z2;
double wx = w * x2;
double wy = w * y2;
double wz = w * z2;
double sx = scale.X;
double sy = scale.Y;
double sz = scale.Z;
matrix.M11 = (1 - (yy + zz)) * sx;
matrix.M12 = (xy + wz) * sx;
matrix.M13 = (xz - wy) * sx;
matrix.M14 = 0;
matrix.M21 = (xy - wz) * sy;
matrix.M22 = (1 - (xx + zz)) * sy;
matrix.M23 = (yz + wx) * sy;
matrix.M24 = 0;
matrix.M31 = (xz + wy) * sz;
matrix.M32 = (yz - wx) * sz;
matrix.M33 = (1 - (xx + yy)) * sz;
matrix.M34 = 0;
/*matrix.M41 = position.X;
* matrix.M42 = position.Y;
* matrix.M43 = position.Z;*/
matrix.M44 = 1;
return(matrix);
}
//<SnippetMil3dVectorSample3DN1>
private void PerformOperation(object sender, RoutedEventArgs e)
{
RadioButton li = (sender as RadioButton);
// Strings used to display results
String syntaxString, resultType, operationString;
///The local variables point1, point2, vector2, etc are defined in each
///case block for readability reasons. Each variable is contained within
///the scope of each case statement.
//</SnippetMil3dVectorSample3DN1>
switch (li.Name)
{ //begin switch
case "rb1":
{
//<SnippetMil3dVectorSample3DN2>
// Translates a Point3D by a Vector3D using the overloaded + operator.
// Returns a Point3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D point1 = new Point3D(10, 5, 1);
Point3D pointResult = new Point3D();
pointResult = point1 + vector1;
// vectorResult is equal to (30, 35, 41)
//</SnippetMil3dVectorSample3DN2>
// Displaying Results
syntaxString = "pointResult = point1 + vector1;";
resultType = "Point3D";
operationString = "Adding a Vector3D to a Vector3D";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb2":
{
//<SnippetMil3dVectorSample3DN3>
// Adds a Vector3D to a Vector3D using the overloaded + operator.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Vector3D vectorResult = new Vector3D();
vectorResult = vector1 + vector2;
// vectorResult is equal to (65, 100, 120)
//</SnippetMil3dVectorSample3DN3>
// Displaying Results
syntaxString = "vectorResult = vector1 + vector2;";
resultType = "Vector3D";
operationString = "Adding a Vector3D to a Vector3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb3":
{
//<SnippetMil3dVectorSample3DN4>
// Translates a Point3D by a Vector3D using the static Add method.
// Returns a Point3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D point1 = new Point3D(10, 5, 1);
Point3D pointResult = new Point3D();
pointResult = Vector3D.Add(vector1, point1);
// vectorResult is equal to (30, 35, 41)
//</SnippetMil3dVectorSample3DN4>
// Displaying Results
syntaxString = " pointResult = Vector3D.Add(vector1, point1);";
resultType = "Point3D";
operationString = "Adding a Vector3D to a Vector3D";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb4":
{
//<SnippetMil3dVectorSample3DN5>
// Adds a Vector3D to a Vector3D using the static Add method.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Vector3D vectorResult = new Vector3D();
vectorResult = Vector3D.Add(vector1, vector2);
// vectorResult is equal to (65, 100, 120)
//</SnippetMil3dVectorSample3DN5>
// Displaying Results
syntaxString = "vectorResult = Vector3D.Add(vector1, vector2);";
resultType = "Vector3D";
operationString = "Adding a Vector3D to a Vector3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb5":
{
//<SnippetMil3dVectorSample3DN6>
// Subtracts a Vector3D from a Vector3D using the overloaded - operator.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Vector3D vectorResult = new Vector3D();
vectorResult = vector1 - vector2;
// vector Result is equal to (-25, -40, -40)
//</SnippetMil3dVectorSample3DN6>
// Displaying Results
syntaxString = "vectorResult = vector1 - vector2;";
resultType = "Vector3D";
operationString = "Subtracting a Vector3D from a Vector3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb6":
{
//<SnippetMil3dVectorSample3DN7>
// Subtracts a Vector3D from a Vector3D using the static Subtract method.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Vector3D vectorResult = new Vector3D();
vectorResult = Vector3D.Subtract(vector1, vector2);
// vector Result is equal to (-25, -40, -40)
//</SnippetMil3dVectorSample3DN7>
// Displaying Results
syntaxString = "vectorResult = Vector3D.Subtract(vector1, vector2);";
resultType = "Vector3D";
operationString = "Subtracting a Vector3D from a Vector3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb7":
{
//<SnippetMil3dVectorSample3DN8>
// Subtracts a Vector3D from a Point3D using the overloaded - operator.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D pointResult = new Point3D();
// Subtracting the vector from the point
pointResult = vector1 - point1;
// pointResult is equal to (10, 25, 39)
//</SnippetMil3dVectorSample3DN8>
// Displaying Results
syntaxString = " pointResult = point1 - vector1;";
resultType = "Point3D";
operationString = "Subtracting a Vector3D from a Point3D";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb8":
{
//<SnippetMil3dVectorSample3DN9>
// Subtracts a Vector3D from a Point3D using the static Subtract method.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D pointResult = new Point3D();
pointResult = Vector3D.Subtract(vector1, point1);
// pointResult is equal to (10, 25, 39)
//</SnippetMil3dVectorSample3DN9>
// Displaying Results
syntaxString = "pointResult = Vector3D.Subtract(vector1, point1);";
resultType = "Point3D";
operationString = "Subtracting a Vector3D from a Point3D";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb9":
{
//<SnippetMil3dVectorSample3DN10>
// Multiplies a Vector3D by a Scalar using the overloaded * operator.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Double scalar1 = 75;
Vector3D vectorResult = new Vector3D();
vectorResult = vector1 * scalar1;
// vectorResult is equal to (1500, 2250, 3000)
//</SnippetMil3dVectorSample3DN10>
// Displaying Results
syntaxString = "vectorResult = vector1 * scalar1;";
resultType = "Vector3D";
operationString = "Multiplies a Vector3D by a Scalar";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb10":
{
//<SnippetMil3dVectorSample3DN11>
// Multiplies a Scalar by a Vector3D using the overloaded * operator.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Double scalar1 = 75;
Vector3D vectorResult = new Vector3D();
vectorResult = scalar1 * vector1;
// vectorResult is equal to (1500, 2250, 3000)
//</SnippetMil3dVectorSample3DN11>
// Displaying Results
syntaxString = "vectorResult = scalar1 * vector1;";
resultType = "Vector3D";
operationString = "Multiplies a Scalar by a Vector3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb11":
{
//<SnippetMil3dVectorSample3DN12>
// Multiplies a Vector3D by a Matrix3D using the overloaded * operator.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Matrix3D matrix1 = new Matrix3D(10, 10, 10, 0, 20, 20, 20, 0, 30, 30, 30, 0, 5, 10, 15, 1);
Vector3D vectorResult = new Vector3D();
vectorResult = vector1 * matrix1;
// vector Result is equal to (2000, 2000, 2000)
//</SnippetMil3dVectorSample3DN12>
// Displaying Results
syntaxString = "vectorResult = vector1 * matrix1;";
resultType = "Vector3D";
operationString = "Multiplies a Vector3D by a Matrix3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb12":
{
//<SnippetMil3dVectorSample3DN13>
// Multiplies a Vector3D by a Scalar using the static Multiply method.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Double scalar1 = 75;
Vector3D vectorResult = new Vector3D();
vectorResult = Vector3D.Multiply(vector1, scalar1);
// vectorResult is equal to (1500, 2250, 3000)
//</SnippetMil3dVectorSample3DN13>
// Displaying Results
syntaxString = "vectorResult = Vector3D.Multiply(vector1, scalar1);";
resultType = "Vector3D";
operationString = "Multiplies a Vector3D by a Scalar";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb13":
{
//<SnippetMil3dVectorSample3DN14>
// Multiplies a Scalar by a Vector3D using the static Multiply method.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Double scalar1 = 75;
Vector3D vectorResult = new Vector3D();
vectorResult = Vector3D.Multiply(scalar1, vector1);
// vectorResult is equal to (1500, 2250, 3000)
//</SnippetMil3dVectorSample3DN14>
// Displaying Results
syntaxString = "vectorResult = Vector3D.Multiply(scalar1, vector1);";
resultType = "Vector3D";
operationString = "Multiplies a Scalar by a Vector3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb14":
{
//<SnippetMil3dVectorSample3DN15>
// Multiplies a Vector3D by a Matrix3D using the static Multiply method.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Matrix3D matrix1 = new Matrix3D(10, 10, 10, 0, 20, 20, 20, 0, 30, 30, 30, 0, 5, 10, 15, 1);
Vector3D vectorResult = new Vector3D();
vectorResult = Vector3D.Multiply(vector1, matrix1);
// vector Result is equal to (2000, 2000, 2000)
//</SnippetMil3dVectorSample3DN15>
// Displaying Results
syntaxString = "vectorResult = Vector3D.Multiply(vector1,matrix1);";
resultType = "Vector3D";
operationString = "Multiplies a Vector3D by a Matrix3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb15":
{
//<SnippetMil3dVectorSample3DN16>
// Divides a Vector3D by a Scalar using the overloaded / operator.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vectorResult = new Vector3D();
Double scalar1 = 75;
vectorResult = vector1 / scalar1;
// vectorResult is approximately equal to (0.26667, 0.4, 0.53333)
//</SnippetMil3dVectorSample3DN16>
// Displaying Results
syntaxString = "vectorResult = vector1 / scalar1;";
resultType = "Vector3D";
operationString = "Dividing a Vector3D by a Scalar";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb16":
{
//<SnippetMil3dVectorSample3DN17>
// Divides a Vector3D by a Double using the static Divide method.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vectorResult = new Vector3D();
Double scalar1 = 75;
vectorResult = Vector3D.Divide(vector1, scalar1);
// vectorResult is approximately equal to (0.26667, 0.4, 0.53333)
//</SnippetMil3dVectorSample3DN17>
// Displaying Results
syntaxString = "vectorResult = Vector3D.Divide(vector1, scalar1);";
resultType = "Vector3D";
operationString = "Dividing a Vector3D by a Scalar";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb17":
{
//<SnippetMil3dVectorSample3DN18>
// Unary Negate a Vector3D using the - unary operator.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vectorResult = new Vector3D();
vectorResult = -vector1;
// vectorResult is equal to (-20, -30, -40)
//</SnippetMil3dVectorSample3DN18>
//Displaying Results
syntaxString = "vectorResult = -vector1;";
resultType = "Vector3D";
operationString = "Unary Negate a Vector3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb18":
{
//<SnippetMil3dVectorSample3DN19>
// Gets the length of a Vector3D.
// Returns a Double.
Vector3D vector1 = new Vector3D(20, 30, 40);
Double length;
length = vector1.Length;
// length is approximately equal to 53.85165
//</SnippetMil3dVectorSample3DN19>
// Displaying Results
syntaxString = "length = vector1.Length();";
resultType = "Double";
operationString = "Getting the length of a Vector3D";
ShowResults(length.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb19":
{
//<SnippetMil3dVectorSample3DN20>
// Gets the square of the length of a Vector3D.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
Double lengthSq;
lengthSq = vector1.LengthSquared;
// lengthSq is equal to 2900
//</SnippetMil3dVectorSample3DN20>
// Displaying Results
syntaxString = "lengthSq = vector1.LengthSquared;";
resultType = "Double";
operationString = "Getting the length square of a Vector3D";
ShowResults(lengthSq.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb20":
{
//<SnippetMil3dVectorSample3DN21>
// Normalizes a Vector3D using the Normalize method.
// Returns a Vector3D.
Vector3D vector1 = new Vector3D(20, 30, 40);
vector1.Normalize();
// vector1 is approximately equal to (0.37139, 0.55709, 0.74278)
//</SnippetMil3dVectorSample3DN21>
// Displaying Results
syntaxString = "vector1.Normalize();";
resultType = "Void";
operationString = "Normalizing a Vector3D";
ShowResults(vector1.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb21":
{
//<SnippetMil3dVectorSample3DN22>
// Calculates the angle between two Vector3Ds using the static AngleBetween method.
// Returns a Double.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Double angleBetween;
angleBetween = Vector3D.AngleBetween(vector1, vector2);
// angleBetween is approximately equal to 4.15129
//</SnippetMil3dVectorSample3DN22>
// Displaying Results
syntaxString = "angleBetween = Vector3D.AngleBetween(vector1, vector2);";
resultType = "Double";
operationString = "Calculating the angle between two Vector3Ds";
ShowResults(angleBetween.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb22":
{
//<SnippetMil3dVectorSample3DN23>
// Calculates the cross product of two Vector3D structures
// using the static CrossProduct method.
// Returns a Double.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Vector3D crossProduct = new Vector3D();
crossProduct = Vector3D.CrossProduct(vector1, vector2);
// crossProduct is equal to (-400, 200, 50)
//</SnippetMil3dVectorSample3DN23>
// Displaying Results
syntaxString = "crossProduct = Vector3D.CrossProduct(vector1,vector2);";
resultType = "Vector3D";
operationString = "Calculating the crossproduct of two Vector3Ds";
ShowResults(crossProduct.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb23":
{
//<SnippetMil3dVectorSample3DN24>
// Calculates the Dot Product of two Vectors.
// Declaring vector1 and initializing x,y,z values
Vector3D vector1 = new Vector3D(20, 30, 40);
// Declaring vector2 without initializing x,y,z values
Vector3D vector2 = new Vector3D();
// A Double to hold the result of the operation
Double dotProduct;
// Assigning values to vector2
vector2.X = 45;
vector2.Y = 70;
vector2.Z = 80;
// Calculating the dot product of vector1 and vector2
dotProduct = Vector3D.DotProduct(vector1, vector2);
// vectorResult is equal to (6200)
//</SnippetMil3dVectorSample3DN24>
// Displaying Results
syntaxString = "dotProduct = Vector3D.DotProduct(vector1, vector2);";
resultType = "Vector3D";
operationString = "Calculating the dot product of vector1 and vector2";
ShowResults(dotProduct.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb24":
{
//<SnippetMil3dVectorSample3DN25>
// Checks if two Vector3D structures are equal using the overloaded equality operator.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Boolean areEqual;
areEqual = (vector1 == vector2);
// areEqual is False
//</SnippetMil3dVectorSample3DN25>
// Displaying Results
syntaxString = "areEqual = (vector1 == vector2);";
resultType = "Boolean";
operationString = "Checking if two vectors are equal";
ShowResults(areEqual.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb25":
{
//<SnippetMil3dVectorSample3DN26>
// Checks if two Vector3D structures are equal using the static Equals method.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Boolean areEqual;
areEqual = Vector3D.Equals(vector1, vector2);
// areEqual is False
//</SnippetMil3dVectorSample3DN26>
// Displaying Results
syntaxString = "areEqual = Vector3D.Equals(vector1, vector2);";
resultType = "Boolean";
operationString = "Checking if two vectors are equal";
ShowResults(areEqual.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb26":
{
//<SnippetMil3dVectorSample3DN27>
// Compares an Object and a Vector3D for equality using the non-static Equals method.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Boolean areEqual;
areEqual = vector1.Equals(vector2);
// areEqual is False
//</SnippetMil3dVectorSample3DN27>
// Displaying Results
syntaxString = "areEqual = vector1.Equals(vector2);";
resultType = "Boolean";
operationString = "Checking if two vectors are equal";
ShowResults(areEqual.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb27":
{
//<SnippetMil3dVectorSample3DN28>
// Converts a string representation of a vector into a Vector3D structure
Vector3D vectorResult = new Vector3D();
vectorResult = Vector3D.Parse("1,3,5");
// vectorResult is equal to (1, 3, 5)
//</SnippetMil3dVectorSample3DN28>
// Displaying Results
syntaxString = "vectorResult = Vector3D.Parse(\"1,3,5\");";
resultType = "Boolean";
operationString = "Checking if two points are not equal";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb28":
{
//<SnippetMil3dVectorSample3DN29>
// Checks if two Vector3D structures are not equal using the overloaded inequality operator.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vector2 = new Vector3D(45, 70, 80);
Boolean areNotEqual;
areNotEqual = (vector1 != vector2);
// areNotEqual is True
//</SnippetMil3dVectorSample3DN29>
// Displaying Results
syntaxString = "areNotEqual = (vector1 != vector2);";
resultType = "Boolean";
operationString = "Checking if two points are not equal";
ShowResults(areNotEqual.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb29":
{
//<SnippetMil3dVectorSample3DN30>
// Negates a Vector3D using the Negate method.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vectorResult = new Vector3D();
vector1.Negate();
// vector1 is equal to (-20, -30, -40)
//</SnippetMil3dVectorSample3DN30>
// Displaying Results
syntaxString = "vector1.Negate(vector1);";
resultType = "void";
operationString = "Negating a vector";
ShowResults(vector1.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb30":
{
//<SnippetMil3dVectorSample3DN31>
// Negates a Vector3D using the overloaded unary negation operator.
Vector3D vector1 = new Vector3D(20, 30, 40);
Vector3D vectorResult = new Vector3D();
vectorResult = -vector1;
// vectorResult is equal to (-20, -30, -40)
//</SnippetMil3dVectorSample3DN31>
// Displaying Results
syntaxString = "vectorResult = -vector1;";
resultType = "Vector3D";
operationString = "Negating a vector";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb31":
{
//<SnippetMil3dVectorSample3DN32>
// Gets a string representation of the structure
Vector3D vector1 = new Vector3D(20, 30, 40);
String vectorString;
vectorString = vector1.ToString();
// vectorString is equal to 20, 30, 40
//</SnippetMil3dVectorSample3DN32>
// Displaying Results
syntaxString = "vectorString = vector1.ToString();";
resultType = "String";
operationString = "Getting the string representation of a Vector3D";
ShowResults(vectorString.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb32":
{
//<SnippetMil3dVectorSample3DN33>
// Gets the hashcode of a Vector3D structure
Vector3D vector1 = new Vector3D(20, 30, 40);
int vectorHashCode;
vectorHashCode = vector1.GetHashCode();
//</SnippetMil3dVectorSample3DN33>
// Displaying Results
syntaxString = "vectorHashCode = vector1.GetHashCode();";
resultType = "int";
operationString = "Getting the hashcode of Vector3D";
ShowResults(vectorHashCode.ToString(), syntaxString, resultType, operationString);
break;
}
case "rb50":
{
//<SnippetMil3dVectorSample3DN34>
// Subtracts two 3D Vectors using the Subtract method and -
// Declaring vector1 and initializing x,y,z values
Vector3D vector1 = new Vector3D(20, 30, 40);
// Declaring vector2 without initializing x,y,z values
Vector3D vector2 = new Vector3D();
// Assigning values to vector2
vector2.X = 45;
vector2.Y = 70;
vector2.Z = 80;
// subtracted Vectors using overload - operator
vector1 = vector1 - vector2;
// vector1 is now equal to (-25, -40, -40)
// Subtracting vectors using static Subtract method
vector1 = Vector3D.Subtract(vector1, vector2);
// vector1 is now equal to (-70, -110, -120)
//</SnippetMil3dVectorSample3DN34>
// Displaying Results
syntaxString = "vector1 = vector1 - vector2;";
resultType = "Vector3D";
operationString = "Negating a vector";
ShowResults(vector1.ToString(), syntaxString, resultType, operationString);
break;
}
default:
break;
} // end switch
}
private void UpdateRotation(object sender, EventArgs e)
{
//Clear prior values
//find a cleaner way to do this--loop?
QuaternionWText.Clear();
QuaternionXText.Clear();
QuaternionYText.Clear();
QuaternionZText.Clear();
M11Text.Clear();
M21Text.Clear();
M31Text.Clear();
OffsetXText.Clear();
M12Text.Clear();
M22Text.Clear();
M32Text.Clear();
OffsetYText.Clear();
M13Text.Clear();
M23Text.Clear();
M33Text.Clear();
OffsetZText.Clear();
M14Text.Clear();
M24Text.Clear();
M34Text.Clear();
M44Text.Clear();
//<SnippetMatrixTransform3DView3DN2>
Double axisX = System.Convert.ToDouble(RotationAxisXText.Text);
Double axisY = System.Convert.ToDouble(RotationAxisYText.Text);
Double axisZ = System.Convert.ToDouble(RotationAxisZText.Text);
Double angle = System.Convert.ToDouble(RotationAngleText.Text);
Vector3D axis = new Vector3D(axisX, axisY, axisZ);
try
{
myAxisAngleRotation3D = new AxisAngleRotation3D(axis, angle);
}
catch
{
MessageBox.Show("Set non-null values for the axis Vector3D.");
}
myRotateTransform3D.Rotation = myAxisAngleRotation3D;
//</SnippetMatrixTransform3DView3DN2>
//update matrix display
//<SnippetMatrixTransform3DView3DN13>
rotationMatrix3D = myRotateTransform3D.Value;
M11Text.Text = rotationMatrix3D.M11.ToString();
//</SnippetMatrixTransform3DView3DN13>
M21Text.Text = rotationMatrix3D.M21.ToString();
M31Text.Text = rotationMatrix3D.M31.ToString();
OffsetXText.Text = rotationMatrix3D.OffsetX.ToString();
M12Text.Text = rotationMatrix3D.M12.ToString();
M22Text.Text = rotationMatrix3D.M22.ToString();
M32Text.Text = rotationMatrix3D.M32.ToString();
OffsetYText.Text = rotationMatrix3D.OffsetY.ToString();
M13Text.Text = rotationMatrix3D.M13.ToString();
M23Text.Text = rotationMatrix3D.M23.ToString();
M33Text.Text = rotationMatrix3D.M33.ToString();
OffsetZText.Text = rotationMatrix3D.OffsetZ.ToString();
M14Text.Text = rotationMatrix3D.M14.ToString();
M24Text.Text = rotationMatrix3D.M24.ToString();
M34Text.Text = rotationMatrix3D.M34.ToString();
M44Text.Text = rotationMatrix3D.M44.ToString();
myprocTransformGroup.Children.Clear();
myprocTransformGroup.Children.Add(myRotateTransform3D);
topModelVisual3D.Transform = myprocTransformGroup;
//<SnippetMatrixTransform3DView3DN3>
//convert to quaternion and update display
try
{
Quaternion tempQuaternion = new Quaternion(axis, angle);
QuaternionWText.Text = tempQuaternion.W.ToString();
QuaternionXText.Text = tempQuaternion.X.ToString();
QuaternionYText.Text = tempQuaternion.Y.ToString();
QuaternionZText.Text = tempQuaternion.Z.ToString();
}
catch
{
MessageBox.Show("Set non-null values for the axis Vector3D.");
}
//</SnippetMatrixTransform3DView3DN3>
}
public Matrix <double> GetMatrix()
{
return(Matrix3D.RotationAroundArbitraryVector(RotationAxis, Angle));
}
public bool Intesects(GenericObject o, List <GenericObject> objects, Vector3D d, Point3D e)
{
foreach (GenericObject obj in objects.Where(x => x != o))
{
// We need the inverse of the transformation matrix
Matrix3D m = obj.M;
m.Invert();
// The starting point of the ray transformed by M^-1
// Treat it as a vector to do the following math
Vector3D E = (Vector3D)(e * m);
// The direction ray tranformed by m^-1
Vector3D D = d * m;
// The constant t to multiply the direction vector d
double t = 0;
if (obj is Sphere)
{
// Get the parameters for the quadratic equation
double a = D.LengthSquared;
double b = Vector3D.DotProduct(E, D);
double c = E.LengthSquared - 1;
// The discriminent will indicate how many solutions there are
double discriminent = b * b - a * c;
if (discriminent < 0) // discrim < 0 --> No intersection
{
continue;
}
else if (discriminent == 0) // discrim = 0 --> One intersection
{
t = -b / a;
}
else if (discriminent > 0) // discrim > 0 --> Two intersections
{
double t1 = -(b + Math.Sqrt(discriminent)) / a;
double t2 = -(b - Math.Sqrt(discriminent)) / a;
t = Math.Min(t1, t2);
}
if (t <= 0)
{
continue; // We're only concerned with positive t values
}
return(true);
}
else if (obj is Plane)
{
if (E.Z == 0)
{
continue; // The plane is parallel to the viewing direction.
}
t = -E.Z / D.Z;
if (t <= 0)
{
continue; // We're only concerned with positiv t values
}
return(true);
}
else if (obj is Cylinder)
{
double a = Math.Pow(D.X, 2) + Math.Pow(D.Y, 2);
double b = E.X * D.X + E.Y * D.Y;
double c = Math.Pow(E.X, 2) + Math.Pow(E.Y, 2) - 1;
// The discriminent will indicate how many solutions there are
double discriminent = b * b - a * c;
if (discriminent < 0) // discrim < 0 --> No intersection
{
continue;
}
else if (discriminent == 0) // discrim = 0 --> One intersection
{
t = -b / a;
}
else if (discriminent > 0) // discrim > 0 --> Two intersections
{
double t1 = -(b + Math.Sqrt(discriminent)) / a;
double t2 = -(b - Math.Sqrt(discriminent)) / a;
if (t1 > 0 && t2 > 0)
{
t = Math.Min(t1, t2);
}
else if (t1 < 0 && t2 > 0)
{
t = t2;
}
else
{
continue;
}
}
if (t <= 0)
{
continue;
}
Point3D int1 = (Point3D)(E + D * t);
if (int1.Z < 1 && int1.Z > -1)
{
return(true);
}
Matrix3D topCap = Matrix3D.Identity; topCap.OffsetZ = 1;
topCap.Invert();
Matrix3D bottomCap = Matrix3D.Identity; bottomCap.OffsetZ = -1;
bottomCap.Invert();
Vector3D E_top = (Vector3D)(((Point3D)E) * topCap);
Vector3D D_top = D * topCap;
if (D_top.Z != 0)
{
double t_top = -E_top.Z / D_top.Z;
if (t_top > 0)
{
Point3D int_top = (Point3D)(E_top + D_top * t_top);
if (Math.Pow(int_top.X, 2) + Math.Pow(int_top.Y, 2) <= 1)
{
return(true);
}
}
}
Vector3D E_bottom = (Vector3D)(((Point3D)E) * bottomCap);
Vector3D D_bottom = D * bottomCap;
if (D_bottom.Z != 0)
{
double t_bottom = -E_bottom.Z / D_bottom.Z;
if (t_bottom > 0)
{
Point3D int_bottom = (Point3D)(E_bottom + D_bottom * t_bottom);
if (Math.Pow(int_bottom.X, 2) + Math.Pow(int_bottom.Y, 2) <= 1)
{
return(true);
}
}
}
}
else if (obj is Cone)
{
double a = Math.Pow(D.X, 2) + Math.Pow(D.Y, 2) - 0.25 * Math.Pow(D.Z, 2);
double b = E.X * D.X + E.Y * D.Y + D.Z * (1 - E.Z) / 4.0;
double c = Math.Pow(E.X, 2) + Math.Pow(E.Y, 2) - Math.Pow(1.0 - E.Z, 2) / 4.0;
// The discriminent will indicate how many solutions there are
double discriminent = b * b - a * c;
if (discriminent < 0) // discrim < 0 --> No intersection
{
continue;
}
else if (discriminent == 0) // discrim = 0 --> One intersection
{
t = -b / a;
}
else if (discriminent > 0) // discrim > 0 --> Two intersections
{
double t1 = -(b + Math.Sqrt(discriminent)) / a;
double t2 = -(b - Math.Sqrt(discriminent)) / a;
if (t1 > 0 && t2 > 0)
{
t = Math.Min(t1, t2);
}
else if (t1 < 0 && t2 > 0)
{
t = t2;
}
else
{
continue;
}
}
if (t <= 0)
{
continue; // The intersection is behind the view point
}
Point3D int1 = (Point3D)(E + D * t);
if (int1.Z < 1 && int1.Z > -1)
{
return(true);
}
Matrix3D bottomCap = Matrix3D.Identity; bottomCap.OffsetZ = -1;
bottomCap.Invert();
Vector3D E_bottom = (Vector3D)(((Point3D)E) * bottomCap);
Vector3D D_bottom = D * bottomCap;
if (D_bottom.Z != 0)
{
double t_bottom = -E_bottom.Z / D_bottom.Z;
if (t_bottom > 0)
{
Point3D int_bottom = (Point3D)(E_bottom + D_bottom * t_bottom);
if (Math.Pow(int_bottom.X, 2) + Math.Pow(int_bottom.Y, 2) <= 1)
{
return(true);
}
}
}
}
}
return(false);
}
/// <summary>
/// Camera frustum 3D graphics
/// </summary>
/// <param name="viewport">The viewport.</param>
/// <param name="near">The near plane of the frustum.</param>
/// <param name="far">The far plane of the frustum.</param>
/// <param name="depthWidth">The width of the depth image.</param>
/// <param name="depthHeight">The height of the depth image.</param>
/// <param name="color">The color to draw the frustum.</param>
/// <param name="thickness">The line thickness to use when drawing the frustum.</param>
public void CreateFrustum3DGraphics(Viewport3D viewport, float near, float far, float depthWidth, float depthHeight, System.Windows.Media.Color color, int thickness)
{
if (null == viewport)
{
return;
}
this.graphicsViewport = viewport;
// De-normalize default camera params
float px = camParams.PrincipalPointX * depthWidth;
float py = camParams.PrincipalPointY * depthHeight;
float fx = camParams.FocalLengthX * depthWidth;
float fy = camParams.FocalLengthY * depthHeight;
float iflx = 1.0f / fx;
float ifly = 1.0f / fy;
this.CameraFrustum = new ScreenSpaceLines3D();
this.CameraFrustum.Points = new Point3DCollection();
Point3DCollection pts = this.CameraFrustum.Points;
// Near plane rectangle
pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly));
// Far plane rectangle
pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly));
// Connecting lines
pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly));
this.CameraFrustum.Thickness = thickness;
this.CameraFrustum.Color = color;
// Add a fixed rotation around the Y axis to look back down +Z towards origin
Matrix3D fixedRotY180 = new Matrix3D();
fixedRotY180.Rotate(new Quaternion(new Vector3D(0, 1, 0), 180));
this.cumulativeCameraFrustumTransform.Children.Add(new MatrixTransform3D(fixedRotY180));
this.cumulativeCameraFrustumTransform.Children.Add(this.cameraFrustumTransform3D);
this.CameraFrustum.Transform = this.cumulativeCameraFrustumTransform;
}
public void SetRotation(Matrix3D matrix)
{
rotation = new MatrixTransform3D(matrix);
}
private Dictionary <string, Mesh> GenerateMeshes(Table.Table table)
{
var meshes = new Dictionary <string, Mesh>();
var fullMatrix = new Matrix3D();
fullMatrix.RotateZMatrix(MathF.DegToRad(180.0f));
var height = table.GetSurfaceHeight(_data.Surface, _data.Center.X, _data.Center.Y);
var baseRadius = _data.BaseRadius - _data.RubberThickness;
var endRadius = _data.EndRadius - _data.RubberThickness;
// calc angle needed to fix P0 location
var sinAngle = (baseRadius - endRadius) / _data.FlipperRadius;
if (sinAngle > 1.0)
{
sinAngle = 1.0f;
}
if (sinAngle < -1.0)
{
sinAngle = -1.0f;
}
var fixAngle = MathF.Asin(sinAngle);
var fixAngleScale = fixAngle / (float)(System.Math.PI * 0.5); // scale (in relation to 90 deg.)
// fixAngleScale = 0.0; // note: if you force fixAngleScale = 0.0 then all will look as old version
// lambda used to apply fix
void ApplyFix(Vertex3DNoTex2 vert, Vertex2D center, float midAngle, float radius, Vertex2D newCenter)
{
var vAngle = MathF.Atan2(vert.Y - center.Y, vert.X - center.X);
var nAngle = MathF.Atan2(vert.Ny, vert.Nx);
// we want have angles with same sign as midAngle, fix it:
if (midAngle < 0.0)
{
if (vAngle > 0.0)
{
vAngle -= (float)(System.Math.PI * 2.0);
}
if (nAngle > 0.0)
{
nAngle -= (float)(System.Math.PI * 2.0);
}
}
else
{
if (vAngle < 0.0)
{
vAngle += (float)(System.Math.PI * 2.0);
}
if (nAngle < 0.0)
{
nAngle += (float)(System.Math.PI * 2.0);
}
}
nAngle -= (vAngle - midAngle) * fixAngleScale * MathF.Sign(midAngle);
vAngle -= (vAngle - midAngle) * fixAngleScale * MathF.Sign(midAngle);
float nL = new Vertex2D(vert.Nx, vert.Ny).Length();
vert.X = MathF.Cos(vAngle) * radius + newCenter.X;
vert.Y = MathF.Sin(vAngle) * radius + newCenter.Y;
vert.Nx = MathF.Cos(nAngle) * nL;
vert.Ny = MathF.Sin(nAngle) * nL;
}
// base and tip
var baseMesh = FlipperBaseMesh.Clone(Base);
for (var t = 0; t < 13; t++)
{
foreach (var v in baseMesh.Vertices)
{
if (v.X == VertsBaseBottom[t].X && v.Y == VertsBaseBottom[t].Y && v.Z == VertsBaseBottom[t].Z)
{
ApplyFix(v, new Vertex2D(VertsBaseBottom[6].X, VertsBaseBottom[0].Y), (float)-(System.Math.PI * 0.5), baseRadius, new Vertex2D(0, 0));
}
if (v.X == VertsTipBottom[t].X && v.Y == VertsTipBottom[t].Y && v.Z == VertsTipBottom[t].Z)
{
ApplyFix(v, new Vertex2D(VertsTipBottom[6].X, VertsTipBottom[0].Y), (float)(System.Math.PI * 0.5), endRadius, new Vertex2D(0, _data.FlipperRadius));
}
if (v.X == VertsBaseTop[t].X && v.Y == VertsBaseTop[t].Y && v.Z == VertsBaseTop[t].Z)
{
ApplyFix(v, new Vertex2D(VertsBaseBottom[6].X, VertsBaseBottom[0].Y), (float)(-System.Math.PI * 0.5), baseRadius, new Vertex2D(0, 0));
}
if (v.X == VertsTipTop[t].X && v.Y == VertsTipTop[t].Y && v.Z == VertsTipTop[t].Z)
{
ApplyFix(v, new Vertex2D(VertsTipBottom[6].X, VertsTipBottom[0].Y), (float)(System.Math.PI * 0.5), endRadius, new Vertex2D(0, _data.FlipperRadius));
}
}
}
baseMesh.Transform(fullMatrix, null, z => z * _data.Height * table.GetScaleZ() + height);
meshes[Base] = baseMesh;
// rubber
if (_data.RubberThickness > 0.0)
{
var rubberMesh = FlipperBaseMesh.Clone(Rubber);
for (var t = 0; t < 13; t++)
{
foreach (var v in rubberMesh.Vertices)
{
if (v.X == VertsBaseBottom[t].X && v.Y == VertsBaseBottom[t].Y && v.Z == VertsBaseBottom[t].Z)
{
ApplyFix(v, new Vertex2D(VertsBaseBottom[6].X, VertsBaseBottom[0].Y), (float)(-System.Math.PI * 0.5), baseRadius + _data.RubberThickness, new Vertex2D(0, 0));
}
if (v.X == VertsTipBottom[t].X && v.Y == VertsTipBottom[t].Y && v.Z == VertsTipBottom[t].Z)
{
ApplyFix(v, new Vertex2D(VertsTipBottom[6].X, VertsTipBottom[0].Y), (float)(System.Math.PI * 0.5), endRadius + _data.RubberThickness, new Vertex2D(0, _data.FlipperRadius));
}
if (v.X == VertsBaseTop[t].X && v.Y == VertsBaseTop[t].Y && v.Z == VertsBaseTop[t].Z)
{
ApplyFix(v, new Vertex2D(VertsBaseBottom[6].X, VertsBaseBottom[0].Y), (float)(-System.Math.PI * 0.5), baseRadius + _data.RubberThickness, new Vertex2D(0, 0));
}
if (v.X == VertsTipTop[t].X && v.Y == VertsTipTop[t].Y && v.Z == VertsTipTop[t].Z)
{
ApplyFix(v, new Vertex2D(VertsTipBottom[6].X, VertsTipBottom[0].Y), (float)(System.Math.PI * 0.5), endRadius + _data.RubberThickness, new Vertex2D(0, _data.FlipperRadius));
}
}
}
rubberMesh.Transform(fullMatrix, null,
z => z * _data.RubberWidth * table.GetScaleZ() + (height + _data.RubberHeight));
meshes[Rubber] = rubberMesh;
}
return(meshes);
}
public override Matrix3D GetProjectionMatrix(float aspectRatio)
{
return(Matrix3D.CreateOrthographic(Width, Width / aspectRatio,
NearPlaneDistance, FarPlaneDistance));
}
public MeshGeometryRectangleHitTestResultTreeLeaf(RectangleHitTestResultTreeNode parent, DependencyObject objectHit, MeshGeometry3D geometryHit, BoundingVolume transformedFrustum, Ray3D frustumCenterRay, Matrix3D transform, Material frontMaterial, Material backMaterial)
: base(parent, objectHit, transformedFrustum, frustumCenterRay, transform)
{
this.geometryHit = geometryHit;
this.frontMaterial = frontMaterial;
this.backMaterial = backMaterial;
}
// Apply a transformation Matrix3D or transformation class.
public static void ApplyTransformation(this MeshGeometry3D mesh, Matrix3D transformation)
{
Point3D[] points = mesh.Positions.ToArray();
transformation.Transform(points);
mesh.Positions = new Point3DCollection(points);
}
public static Vector3D TransformNormal(Vector3D normal, Matrix3D matrix)
{
float num3 = ((normal.X * matrix.M11) + (normal.Y * matrix.M21)) + (normal.Z * matrix.M31);
float num2 = ((normal.X * matrix.M12) + (normal.Y * matrix.M22)) + (normal.Z * matrix.M32);
float num = ((normal.X * matrix.M13) + (normal.Y * matrix.M23)) + (normal.Z * matrix.M33);
return new Vector3D(num3, num2, num);
}
public Color RayTrace(List <GenericObject> objects, Vector3D d, Point3D e, PointLight pointLight, DirectionalLight directionalLight)
{
Dictionary <GenericObject, double> hitTimes = new Dictionary <GenericObject, double>();
foreach (GenericObject obj in objects)
{
// We need the inverse of the transformation matrix
Matrix3D m = obj.M;
m.Invert();
// The starting point of the ray transformed by M^-1
// Treat it as a vector to do the following math
Vector3D E = (Vector3D)(e * m);
// The direction ray tranformed by m^-1
Vector3D D = d * m;
// Consider this 't' in r(t) = e + d*t
double directionMultiplier = 0;
if (obj is Sphere)
{
#region Sphere
// Get the parameters for the quadratic equation
double a = D.LengthSquared;
double b = Vector3D.DotProduct(E, D);
double c = E.LengthSquared - 1;
// The discriminent will indicate how many solutions there are
double discriminent = b * b - a * c;
if (discriminent < 0) // discrim < 0 --> No intersection
{
continue;
}
else if (discriminent == 0) // discrim = 0 --> One intersection
{
directionMultiplier = -b / a;
}
else if (discriminent > 0) // discrim > 0 --> Two intersections
{
double t1 = -(b + Math.Sqrt(discriminent)) / a;
double t2 = -(b - Math.Sqrt(discriminent)) / a;
directionMultiplier = Math.Min(t1, t2);
}
// We're only concerned with positive t values
if (directionMultiplier <= 0)
{
continue;
}
hitTimes.Add(obj, directionMultiplier);
#endregion
}
else if (obj is Plane)
{
#region Plane
if (D.Z == 0)
{
continue; // The plane is parallel to the viewing direction.
}
directionMultiplier = -E.Z / D.Z;
if (directionMultiplier <= 0)
{
continue; // We're only concerned with positive t values
}
Point3D canonicalIntersection = Camera.S1T1Mp.Transform(Camera.Mv.Transform(e + d * directionMultiplier));
if (canonicalIntersection.Z > 1)
{
continue; // I'm only concerned with those z values that are in the viewing volume
}
hitTimes.Add(obj, directionMultiplier);
#endregion
}
else if (obj is Cylinder)
{
#region Cylinder
#region Rounded side
double a = Math.Pow(D.X, 2) + Math.Pow(D.Y, 2);
double b = E.X * D.X + E.Y * D.Y;
double c = Math.Pow(E.X, 2) + Math.Pow(E.Y, 2) - 1;
// The discriminent will indicate how many solutions there are
double discriminent = b * b - a * c;
if (discriminent < 0) // discrim < 0 --> No intersection
{
continue;
}
else if (discriminent == 0) // discrim = 0 --> One intersection
{
directionMultiplier = -b / a;
}
else if (discriminent > 0) // discrim > 0 --> Two intersections
{
double t1 = -(b + Math.Sqrt(discriminent)) / a;
double t2 = -(b - Math.Sqrt(discriminent)) / a;
directionMultiplier = Math.Min(t1, t2);
}
if (directionMultiplier <= 0)
{
continue;
}
Point3D int1 = (Point3D)(E + D * directionMultiplier);
if (int1.Z < 1 && int1.Z > -1)
{
hitTimes.Add(obj, directionMultiplier);
continue;
}
#endregion
#region Caps
Matrix3D topCap = Matrix3D.Identity; topCap.OffsetZ = 1;
topCap.Invert();
Matrix3D bottomCap = Matrix3D.Identity; bottomCap.OffsetZ = -1;
bottomCap.Invert();
Vector3D E_top = (Vector3D)(((Point3D)E) * topCap);
Vector3D D_top = D * topCap;
bool intersectsTop = false;
double directionMultiplier_top = double.MaxValue;
if (D_top.Z != 0)
{
double t_top = -E_top.Z / D_top.Z;
if (t_top > 0 && Camera.S1T1Mp.Transform(Camera.Mv.Transform(e + d * t_top)).Z < 1)
{
Point3D int_top = (Point3D)(E_top + D_top * t_top);
if (Math.Pow(int_top.X, 2) + Math.Pow(int_top.Y, 2) <= 1)
{
// intersects top cap.
directionMultiplier_top = t_top;
intersectsTop = true;
}
}
}
Vector3D E_bottom = (Vector3D)(((Point3D)E) * bottomCap);
Vector3D D_bottom = D * bottomCap;
bool intersectsBottom = false;
double directionMultiplier_bottom = double.MaxValue;
if (D_bottom.Z != 0)
{
double t_bottom = -E_bottom.Z / D_bottom.Z;
if (t_bottom > 0 && Camera.S1T1Mp.Transform(Camera.Mv.Transform(e + d * t_bottom)).Z < 1)
{
Point3D int_bottom = (Point3D)(E_bottom + D_bottom * t_bottom);
if (Math.Pow(int_bottom.X, 2) + Math.Pow(int_bottom.Y, 2) <= 1)
{
directionMultiplier_bottom = t_bottom;
intersectsBottom = true;
}
}
}
if (intersectsTop || intersectsBottom)
{
directionMultiplier = Math.Min(directionMultiplier_bottom, directionMultiplier_top);
hitTimes.Add(obj, directionMultiplier);
}
#endregion
#endregion
}
else if (obj is Cone)
{
#region Cone
double a = Math.Pow(D.X, 2) + Math.Pow(D.Y, 2) - 0.25 * Math.Pow(D.Z, 2);
double b = E.X * D.X + E.Y * D.Y + D.Z * (1 - E.Z) / 4.0;
double c = Math.Pow(E.X, 2) + Math.Pow(E.Y, 2) - Math.Pow(1.0 - E.Z, 2) / 4.0;
// The discriminent will indicate how many solutions there are
double discriminent = b * b - a * c;
if (discriminent < 0) // discrim < 0 --> No intersection
{
continue;
}
else if (discriminent == 0) // discrim = 0 --> One intersection
{
directionMultiplier = -b / a;
}
else if (discriminent > 0) // discrim > 0 --> Two intersections
{
double t1 = -(b + Math.Sqrt(discriminent)) / a;
double t2 = -(b - Math.Sqrt(discriminent)) / a;
directionMultiplier = Math.Min(t1, t2);
}
if (directionMultiplier <= 0)
{
continue; // The intersection is behind the view point
}
Point3D int1 = (Point3D)(E + D * directionMultiplier);
if (int1.Z < 1 && int1.Z > -1)
{
hitTimes.Add(obj, directionMultiplier);
continue;
}
Matrix3D bottomCap = Matrix3D.Identity; bottomCap.OffsetZ = -1;
bottomCap.Invert(); Vector3D E_bottom = (Vector3D)(((Point3D)E) * bottomCap);
Vector3D D_bottom = D * bottomCap;
if (D_bottom.Z != 0)
{
double t_bottom = -E_bottom.Z / D_bottom.Z;
if (t_bottom > 0 && Camera.S1T1Mp.Transform(Camera.Mv.Transform(e + d * t_bottom)).Z < 1)
{
Point3D int_bottom = (Point3D)(E_bottom + D_bottom * t_bottom);
if (Math.Pow(int_bottom.X, 2) + Math.Pow(int_bottom.Y, 2) <= 1)
{
hitTimes.Add(obj, t_bottom);
continue;
}
}
}
#endregion
}
}
if (hitTimes.Count == 0)
{
return(new Color());
}
KeyValuePair <GenericObject, double> hit = hitTimes.OrderBy(x => x.Value).First();
GenericObject ob = hit.Key;
double t = hit.Value;
// Move everything into generic form
Matrix3D M = ob.M;
M.Invert();
Point3D cameraPos = e * M; // Camera in generic world coords
Point3D lightPos = pointLight.Position * M; // Light position in generic world coords
Point3D intersection = cameraPos + (d * M) * t; // Insersection point in generic world coords
Vector3D n = GetNormal(ob, intersection); // The normal vector in generic world coords
Vector3D s = (Vector3D)(lightPos - intersection); // Direction to the point light source from the intersection, in generic space
Vector3D v = (Vector3D)(cameraPos - intersection); // Direction to the camera from the intersection, in generic space
// Prepare the color values
Color Id = Colors.Black; // Diffuse intensity
Color Is = Colors.Black; // Specular intensity
Color Ia = Colors.Black; // Ambient intensity
Color diffuse = Colors.Black; // diffuse component
Color specular = Colors.Black; // specular component
Color ambient = Colors.Black; // ambient component
bool inShadowOfPointLight = Intesects(ob, objects, (Vector3D)(pointLight.Position - (e + d * t)), e + d * t);
if (!inShadowOfPointLight)
{
Id = LightingModel.GetDiffuseIntensity(ob, s, n, pointLight);
diffuse += Color.FromScRgb(1, Id.ScR * ob.DiffuseColor.ScR, Id.ScG * ob.DiffuseColor.ScG, Id.ScB * ob.DiffuseColor.ScB);
Is = LightingModel.GetSpecularIntensity(ob, s, n, v, pointLight);
specular += Color.FromScRgb(1, Is.ScR * ob.SpecularColor.ScR, Is.ScG * ob.SpecularColor.ScG, Is.ScB * ob.SpecularColor.ScB);
}
bool inShadowOfDirectionalLight = Intesects(ob, objects, directionalLight.Direction * -1.0, e + d * t);
if (!inShadowOfDirectionalLight)
{
Id = LightingModel.GetDiffuseIntensity(ob, directionalLight.Direction * -1.0, n, directionalLight);
diffuse += Color.FromScRgb(1, Id.ScR * ob.DiffuseColor.ScR, Id.ScG * ob.DiffuseColor.ScG, Id.ScB * ob.DiffuseColor.ScB);
Is = LightingModel.GetSpecularIntensity(ob, directionalLight.Direction * -1.0, n, v, pointLight);
specular += Color.FromScRgb(1, Is.ScR * ob.SpecularColor.ScR, Is.ScG * ob.SpecularColor.ScG, Is.ScB * ob.SpecularColor.ScB);
}
// The scene will always have ambient light
Ia = LightingModel.GetAmbientIntensity(ob);
ambient = Color.FromScRgb(1, Ia.ScR * ob.AmbientColor.ScR, Ia.ScG * ob.AmbientColor.ScG, Ia.ScB * ob.AmbientColor.ScB);
return(diffuse + specular + ambient + ob.Color);
}
private void GeometryDirty()
{
// Force next call to UpdateTransforms() to return true.
this.visualToScreen = MathUtils.ZeroMatrix;
}
public List <Point3D> projectedPointList()
{
Vector3D[] majorAxes = identifyMajorAxes();
Point3D centroid = Centroid;
// Modify Axes to be aligned to XY plane, the longest X will be first axis (X), the longest Y will be Y
Vector3D[] modAxes = new Vector3D[3];
// set as default first to be the same
if (Math.Abs(majorAxes[0].X) > Math.Abs(majorAxes[1].X) && Math.Abs(majorAxes[0].X) > Math.Abs(majorAxes[2].X))
{
modAxes[0] = majorAxes[0];
if (Math.Abs(majorAxes[1].Y) > Math.Abs(majorAxes[2].Y))
{
modAxes[1] = majorAxes[1];
modAxes[2] = majorAxes[2];
}
else
{
modAxes[1] = majorAxes[2];
modAxes[2] = majorAxes[1];
}
}
else if (Math.Abs(majorAxes[1].X) > Math.Abs(majorAxes[0].X) && Math.Abs(majorAxes[1].X) > Math.Abs(majorAxes[2].X))
{
modAxes[0] = majorAxes[1];
if (Math.Abs(majorAxes[0].Y) > Math.Abs(majorAxes[2].Y))
{
modAxes[1] = majorAxes[0];
modAxes[2] = majorAxes[2];
}
else
{
modAxes[1] = majorAxes[2];
modAxes[2] = majorAxes[0];
}
}
else if (Math.Abs(majorAxes[2].X) > Math.Abs(majorAxes[0].X) && Math.Abs(majorAxes[2].X) > Math.Abs(majorAxes[1].X))
{
modAxes[0] = majorAxes[2];
if (Math.Abs(majorAxes[1].Y) > Math.Abs(majorAxes[0].Y))
{
modAxes[1] = majorAxes[1];
modAxes[2] = majorAxes[0];
}
else
{
modAxes[1] = majorAxes[0];
modAxes[2] = majorAxes[1];
}
}
else
{
modAxes[0] = majorAxes[0];
if (Math.Abs(majorAxes[1].Y) > Math.Abs(majorAxes[2].Y))
{
modAxes[1] = majorAxes[1];
modAxes[2] = majorAxes[2];
}
else
{
modAxes[1] = majorAxes[2];
modAxes[2] = majorAxes[1];
}
}
// Force axes 1 and 2 (X and Y) to be on the XY plane
modAxes[0].Z = 0;
modAxes[1].Z = 0;
// the third Axis will be +Z
modAxes[2].X = 0;
modAxes[2].Y = 0;
if (modAxes[2].Z == 0)
{
modAxes[2].Z = 1;
}
transformMatrix = new Matrix3D(modAxes[0].X, modAxes[1].X, modAxes[2].X, 0,
modAxes[0].Y, modAxes[1].Y, modAxes[2].Y, 0,
modAxes[0].Z, modAxes[1].Z, modAxes[2].Z, 0,
-centroid.X, -centroid.Y, -centroid.Z, 1);
List <Point3D> transfPoints = transformPointSet();
//BoundingBox3D trOBB = new BoundingBox3D(transfPoints);
//List<Point3D> modOBB = transformBackPointSet(trOBB.BBVertices);
return(transfPoints);
}
private void UpdateQuaternionRotation3D(object sender, EventArgs e)
{
//Clear prior values
//find a cleaner way to do this--loop?
RotationAxisXText.Clear();
RotationAxisYText.Clear();
RotationAxisZText.Clear();
RotationAngleText.Clear();
M11Text.Clear();
M21Text.Clear();
M31Text.Clear();
OffsetXText.Clear();
M12Text.Clear();
M22Text.Clear();
M32Text.Clear();
OffsetYText.Clear();
M13Text.Clear();
M23Text.Clear();
M33Text.Clear();
OffsetZText.Clear();
M14Text.Clear();
M24Text.Clear();
M34Text.Clear();
M44Text.Clear();
//<SnippetMatrixTransform3DView3DN4>
//Read new settings
try
{
Double WValue = System.Convert.ToDouble(QuaternionWText.Text);
Double XValue = System.Convert.ToDouble(QuaternionXText.Text);
Double YValue = System.Convert.ToDouble(QuaternionYText.Text);
Double ZValue = System.Convert.ToDouble(QuaternionZText.Text);
endQuaternion = new Quaternion(XValue, YValue, ZValue, WValue);
}
catch
{
MessageBox.Show("Set non-null values for the quaternion.");
}
myQuaternionRotation3D = new QuaternionRotation3D(endQuaternion);
myRotateTransform3D.Rotation = myQuaternionRotation3D;
//update matrix display
qrotationMatrix3D = myRotateTransform3D.Value;
//</SnippetMatrixTransform3DView3DN4>
M11Text.Text = qrotationMatrix3D.M11.ToString();
M21Text.Text = qrotationMatrix3D.M21.ToString();
M31Text.Text = qrotationMatrix3D.M31.ToString();
OffsetXText.Text = qrotationMatrix3D.OffsetX.ToString();
M12Text.Text = qrotationMatrix3D.M12.ToString();
M22Text.Text = qrotationMatrix3D.M22.ToString();
M32Text.Text = qrotationMatrix3D.M32.ToString();
OffsetYText.Text = qrotationMatrix3D.OffsetY.ToString();
M13Text.Text = qrotationMatrix3D.M13.ToString();
M23Text.Text = qrotationMatrix3D.M23.ToString();
M33Text.Text = qrotationMatrix3D.M33.ToString();
OffsetZText.Text = qrotationMatrix3D.OffsetZ.ToString();
M14Text.Text = qrotationMatrix3D.M14.ToString();
M24Text.Text = qrotationMatrix3D.M24.ToString();
M34Text.Text = qrotationMatrix3D.M34.ToString();
M44Text.Text = qrotationMatrix3D.M44.ToString();
//apply transform
myprocTransformGroup.Children.Clear();
myprocTransformGroup.Children.Add(myRotateTransform3D);
topModelVisual3D.Transform = myprocTransformGroup;
//convert to axis/angle and display results
RotationAxisXText.Text = endQuaternion.Axis.X.ToString();
RotationAxisYText.Text = endQuaternion.Axis.X.ToString();
RotationAxisZText.Text = endQuaternion.Axis.X.ToString();
RotationAngleText.Text = endQuaternion.Angle.ToString();
}
internal static IEnumerable <Profile2D> ElaboratePathData(PathGeometry p1, Point2D startPoint)
{
/*
* La matrice di rotazione serve solamente a correggere il comportamento strano ( testo rovesciato) ottenuto dalla trasformazione in geometria del testo.
*/
var matrix = new Matrix3D();
matrix.RotateAt(new Quaternion(new Vector3D(1, 0, 0), 180),
new System.Windows.Media.Media3D.Point3D(0, startPoint.Y, 0));
var profiles = new List <Profile2D>();
foreach (var figure in p1.Figures)
{
var profile = new Profile2D();
profiles.Add(profile);
var d = figure.Segments;
foreach (var l in d)
{
if (l is PolyLineSegment)
{
var pl = l as PolyLineSegment;
var pts = pl.Points;
foreach (var pnt in pts)
{
profile.AddPnt(new Point2D(pnt.X, pnt.Y), matrix);
}
if (pts.Count > 0)
{
profile.AddPnt(new Point2D(pts[0].X, pts[0].Y), matrix);
}
continue;
}
else if (l is PolyBezierSegment)
{
var pl = l as PolyBezierSegment;
var pts = pl.Points;
foreach (var pnt in pts)
{
profile.AddPnt(new Point2D(pnt.X, pnt.Y), matrix);
}
//if (pts.Count > 0)
// profile.AddPnt(new Point2D(pts[0].X, pts[0].Y));
continue;
}
else if (l is LineSegment)
{
var pl = l as LineSegment;
profile.AddPnt(new Point2D(pl.Point.X, pl.Point.Y), matrix);
continue;
}
else if (l is BezierSegment)
{
var pl = l as BezierSegment;
/*
* todo , calcolare bezier da 3 point
*/
profile.AddPnt(new Point2D(pl.Point1.X, pl.Point1.Y), matrix);
profile.AddPnt(new Point2D(pl.Point2.X, pl.Point3.Y), matrix);
profile.AddPnt(new Point2D(pl.Point2.X, pl.Point3.Y), matrix);
continue;
}
//if (!(l is PolyLineSegment))
//{
Debug.Fail("XamlDataManager.ElaboratePathData");
throw new Exception("XamlDataManager.ElaboratePathData");
//}
}
}
return(profiles);
}
// This method performs the Point3D operations
//<SnippetMil3DPoints3DN1>
private void PerformOperation(object sender, RoutedEventArgs e)
{
RadioButton li = (sender as RadioButton);
// Strings used to display the results
String syntaxString, resultType, operationString;
// The local variables point1, point2, vector2, etc are defined in each
// case block for readability reasons. Each variable is contained within
// the scope of each case statement.
switch (li.Name)
{ //begin switch
//</SnippetMil3DPoints3DN1>
case "rb1":
{
//<SnippetMil3DPoints3DN3>
// Translates a Point3D by a Vector3D using the overloaded + operator.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D pointResult = new Point3D();
pointResult = point1 + vector1;
// point3DResult is equal to (30, 35, 41)
// Displaying Results
syntaxString = "pointResult = point1 + vector1;";
resultType = "Point3D";
operationString = "Adding a 3D Point and a 3D Vector";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN3>
break;
}
case "rb2":
{
//<SnippetMil3DPoints3DN4>
// Translates a Point3D by a Vector3D using the static Add method.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D pointResult = new Point3D();
pointResult = Point3D.Add(point1, vector1);
// pointResult is equal to (30, 35, 41)
// Displaying Results
syntaxString = "pointResult = Point3D.Add(point1, vector1);";
resultType = "Point3D";
operationString = "Adding a 3D Point and a 3D Vector";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN4>
break;
}
case "rb3":
{
//<SnippetMil3DPoints3DN5>
// Subtracts a Vector3D from a Point3D using the overloaded - operator.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D pointResult = new Point3D();
pointResult = point1 - vector1;
// pointResult is equal to (-10, -25, -39)
// Displaying Results
syntaxString = "pointResult = point1 - vector1;";
resultType = "Point3D";
operationString = "Subtracting a Vector3D from a Point3D";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN5>
break;
}
case "rb4":
{
//<SnippetMil3DPoints3DN6>
// Subtracts a Vector3D from a Point3D using the static Subtract method.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D pointResult = new Point3D();
pointResult = Point3D.Subtract(point1, vector1);
// pointResult is equal to (-10, -25, -39)
// Displaying Results
syntaxString = "pointResult = Point3D.Subtract(point1, vector1);";
resultType = "Point3D";
operationString = "Subtracting a Vector3D from a Point3D";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN6>
break;
}
case "rb5":
{
//<SnippetMil3DPoints3DN7>
// Subtracts a Point3D from a Point3D using the overloaded - operator.
// Returns a Vector3D.
Point3D point1 = new Point3D(10, 5, 1);
Point3D point2 = new Point3D(15, 40, 60);
Vector3D vectorResult = new Vector3D();
vectorResult = point1 - point2;
// vectorResult is equal to (-5, -35, -59)
// Displaying Results
syntaxString = " vectorResult = point1 - point2;";
resultType = "Vector3D";
operationString = "Subtracting a Point3D from a Point3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN7>
break;
}
case "rb6":
{
//<SnippetMil3DPoints3DN8>
// Subtracts a Point3D from a Point3D using the static Subtract method.
// Returns a Vector3D.
Point3D point1 = new Point3D(10, 5, 1);
Point3D point2 = new Point3D(15, 40, 60);
Vector3D vectorResult = new Vector3D();
vectorResult = Point3D.Subtract(point1, point2);
// vectorResult is equal to (-5, -35, -59)
// Displaying Results
syntaxString = "vectorResult = Point3D.Subtract(point1, point2);";
resultType = "Vector3D";
operationString = "Subtracting a Point3D from a Point3D";
ShowResults(vectorResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN8>
break;
}
case "rb7":
{
//<SnippetMil3DPoints3DN9>
// Offsets the X, Y and Z values of a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
point1.Offset(20, 30, 40);
// point1 is equal to (30, 35, 41)
// Note: This operation is equivalent to adding a point
// to vector with the corresponding X,Y, Z values.
// Displaying Results
syntaxString = "point1.Offset(20, 30, 40);";
resultType = "Point3D";
operationString = "Offsetting a Point3D";
ShowResults(point1.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN9>
break;
}
case "rb8":
{
//<SnippetMil3DPoints3DN10>
// Multiplies a Point3D by a Matrix.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Point3D pointResult = new Point3D();
Matrix3D matrix1 = new Matrix3D(10, 10, 10, 0, 20, 20, 20, 0, 30, 30, 30, 0, 5, 10, 15, 1);
pointResult = point1 * matrix1;
// pointResult is equal to (235, 240, 245)
// Displaying Results
resultType = "Point3D";
syntaxString = "pointResult = point1 * matrix1;";
operationString = "Multiplying a Point3D by a Matrix3D";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN10>
break;
}
case "rb9":
{
//<SnippetMil3DPoints3DN11>
// Multiplies a Point3D by a Matrix.
// Returns a Point3D.
Point3D point1 = new Point3D(10, 5, 1);
Point3D pointResult = new Point3D();
Matrix3D matrix1 = new Matrix3D(10, 10, 10, 0, 20, 20, 20, 0, 30, 30, 30, 0, 5, 10, 15, 1);
pointResult = Point3D.Multiply(point1, matrix1);
// pointResult is equal to (235, 240, 245)
// Displaying Results
resultType = "Point3D";
syntaxString = "pointResult = Point3D.Multiply(point1, matrix1);";
operationString = "Multiplying a Point3D by a Matrix";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN11>
break;
}
case "rb10":
{
//<SnippetMil3DPoints3DN12>
// Checks if two Point3Ds are equal using the overloaded equality operator.
Point3D point1 = new Point3D(10, 5, 1);
Point3D point2 = new Point3D(15, 40, 60);
Boolean areEqual;
areEqual = (point1 == point2);
// areEqual is False
// Displaying Results
syntaxString = "areEqual = (point1 == point2);";
resultType = "Boolean";
operationString = "Checking if two 3D points are equal";
ShowResults(areEqual.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN12>
break;
}
case "rb11":
{
//<SnippetMil3DPoints3DN13>
// Checks if two Point3D structures are equal using the static Equals method.
Point3D point1 = new Point3D(10, 5, 1);
Point3D point2 = new Point3D(15, 40, 60);
Boolean areEqual;
areEqual = Point3D.Equals(point1, point2);
// areEqual is False
//Displaying Results
syntaxString = "areEqual = Point3D.Equals(point1, point2);";
resultType = "Boolean";
operationString = "Checking if 3D two points are equal";
ShowResults(areEqual.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN13>
break;
}
case "rb12":
{
//<SnippetMil3DPoints3DN14>
// Compares an Object and a Point3D for equality using the non-static Equals method.
Point3D point1 = new Point3D(10, 5, 1);
Point3D point2 = new Point3D(15, 40, 60);
Boolean areEqual;
areEqual = point1.Equals(point2);
// areEqual is False. point2 is a Point3D structure, but it is not equal to point1.
// Displaying Results
syntaxString = "areEqual = point1.Equals(point2);;";
resultType = "Boolean";
operationString = "Checking if two 3D points are equal";
ShowResults(areEqual.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN14>
break;
}
case "rb13":
{
//<SnippetMil3DPoints3DN15>
// Converts a string representation of a 3-D point into a Point3D structure.
Point3D pointResult = new Point3D();
pointResult = Point3D.Parse("1,3,5");
// pointResult is equal to (1,3,5)
// Displaying Results
syntaxString = "ointResult = Point3D.Parse(\"1,3,5\");";
resultType = "Matrix";
operationString = "Converting a string into a Point3D structure.";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN15>
break;
}
case "rb14":
{
//<SnippetMil3DPoints3DN16>
// Checks if two Point3Ds are not equal using the overloaded inequality operator.
Point3D point1 = new Point3D(10, 5, 1);
Point3D point2 = new Point3D(15, 40, 60);
Boolean areNotEqual;
areNotEqual = (point1 != point2);
// areNotEqual is True
// Displaying Results
syntaxString = "areNotEqual = (point1 != point2);";
resultType = "Boolean";
operationString = "Checking if two 3D points are not equal";
ShowResults(areNotEqual.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN16>
break;
}
case "rb15":
{
//<SnippetMil3DPoints3DN17>
// Point3D Subtraction
// instantiate variables
Point3D point1 = new Point3D();
Point3D point2 = new Point3D(15, 40, 60);
Vector3D vector1 = new Vector3D(20, 30, 40);
Point3D pointResult = new Point3D();
Vector3D vectorResult = new Vector3D();
// defining x,y,z of point1
point1.X = 10;
point1.Y = 5;
point1.Z = 1;
vectorResult = Point3D.Subtract(point1, point2);
// vectorResult is equal to (-5, -35, -39)
vectorResult = point2 - point1;
// vectorResult is equal to (5, 35, 59)
//pointResult = Point3D.Subtract(point1, vector1);
// pointResult is equal to (-10, -25, -39)
pointResult = vector1 - point1;
// pointResult is equal to (10, 25, 39)
// Displaying Results
syntaxString = "areNotEqual = (point1 != point2);";
resultType = "Boolean";
operationString = "Checking if two 3D points are not equal";
ShowResults(pointResult.ToString(), syntaxString, resultType, operationString);
//</SnippetMil3DPoints3DN17>
break;
}
default:
break;
//<SnippetMil3DPoints3DN2>
} //end switch
}
/// <summary>
/// Point3D * Matrix3D multiplication.
/// </summary>
/// <param name="point">Point being transformed.</param>
/// <param name="matrix">Transformation matrix applied to the point.</param>
/// <returns>Result of the transformation matrix applied to the point.</returns>
public static Point3D Multiply(Point3D point, Matrix3D matrix)
{
return(matrix.Transform(point));
}
/// <summary>
/// Creates a combined transformation which could be used for the Projection of an UIElement.
/// The arguments are multiplied in that order.
/// </summary>
/// <param name="world">The world matrix.</param>
/// <param name="lookAt">The camera (look-at) matrix.</param>
/// <param name="projection">The projection matrix.</param>
/// <param name="viewport">The final viewport transformation.</param>
/// <returns>A new matrix combined matrix.</returns>
public static Matrix3D CreateViewportProjection(Matrix3D world, Matrix3D lookAt, Matrix3D projection, Matrix3D viewport)
{
return(world * lookAt * projection * viewport);
}
public static GraphicsPath ToGraphicsPath(IShape2D shape, Matrix3D transform)
{
return((GraphicsPath) new GeneralShape2D(shape, transform, true));
}
/// <summary>
/// Creates a combined transformation which could be used for the Projection of an UIElement.
/// The arguments are multiplied in that order.
/// </summary>
/// <param name="world">The world matrix.</param>
/// <param name="lookAt">The camera (look-at) matrix.</param>
/// <param name="projection">The projection matrix.</param>
/// <param name="viewport">The final viewport transformation.</param>
/// <returns>A new matrix combined matrix.</returns>
public static Matrix3D CreateViewportProjection(Matrix3D world, Matrix3D lookAt, Matrix3D projection, Matrix3D viewport)
{
return world * lookAt * projection * viewport;
}
public override void CreateVertices()
{
my_VertexCount = (my_Steps + 1) * (my_Slices + 1);
Matrix3D rm = CalculateRotationMatrix(InitialRotationAxis.X, InitialRotationAxis.Y, InitialRotationAxis.Z);
my_Vertices = new Vector3D[my_VertexCount];
centerPoints = new Vector3D[my_Steps + 1];
int count = 0;
double mu;
double x;
double y;
double z;
Vector3D V;
Vector3D V1 = new Vector3D();
AxisAngleRotation3D Aar;
RotateTransform3D rotT;
//Calculate the knot center coordinates
for (int I = 0; I <= my_Steps; I++)
{
mu = I * (4 * a3 + 2) * Math.PI / my_Steps;
x = my_Size * Math.Cos(mu) * (a1 - Math.Cos(a2 * mu / (2 * a3 + 1)));
y = my_Size * Math.Sin(mu) * (b1 - Math.Cos(b2 * mu / (2 * a3 + 1)));
z = -1 * my_Size * Math.Sin(c1 * mu / (2 * a3 + 1));
centerPoints[I] = new Vector3D(x, y, z);
}
//Calculate the vertex positions at my_Diameter / 2 around the knot center coordinates
for (int I = 0; I < my_Steps; I++)
{
V = centerPoints[I + 1] - centerPoints[I];
Aar = new AxisAngleRotation3D(V, 360.0 / my_Slices);
rotT = new RotateTransform3D(Aar);
if (V.X != 0 | V.Z != 0)
{
V1 = Vector3D.CrossProduct(V, new Vector3D(0, 1, 0));
}
else if (V.Y != 0)
{
V1 = Vector3D.CrossProduct(V, new Vector3D(0, 0, 1));
}
V1.Normalize();
V1 = (my_Diameter / 2.0) * V1;
for (int J = 0; J <= my_Slices; J++)
{
my_Vertices[count] = V1 + centerPoints[I];
V1 = rotT.Transform(V1);
count += 1;
}
}
//Add vertices around the first knot coordinate again to close the knot.
V = centerPoints[1] - centerPoints[0];
Aar = new AxisAngleRotation3D(V, 360.0 / my_Slices);
rotT = new RotateTransform3D(Aar);
if (V.X != 0 | V.Z != 0)
{
V1 = Vector3D.CrossProduct(V, new Vector3D(0, 1, 0));
}
else if (V.Y != 0)
{
V1 = Vector3D.CrossProduct(V, new Vector3D(0, 0, 1));
}
V1.Normalize();
V1 = (my_Diameter / 2.0) * V1;
for (int J = 0; J <= my_Slices; J++)
{
my_Vertices[count] = V1 + centerPoints[0];
V1 = rotT.Transform(V1);
count += 1;
}
//Apply the initial rotation
for (int I = 0; I < my_VertexCount; I++)
{
my_Vertices[I] = rm.Transform(my_Vertices[I]);
}
}
/// <summary>
/// Create a quaternion from a 3D homogeneous transform
/// </summary>
/// <param name="xfrm"></param>
/// <returns></returns>
static public Quaternion FromTransform( Matrix3D xfrm ) {
Quaternion quat = new Quaternion();
// Check the sum of the diagonal
float tr = xfrm[ 0, 0 ] + xfrm[ 1, 1 ] + xfrm[ 2, 2 ];
if(tr > 0.0f) {
// The sum is positive
// 4 muls, 1 div, 6 adds, 1 trig function call
float s = (float) Math.Sqrt( tr + 1.0f );
quat.W = s * 0.5f;
s = 0.5f / s;
quat.X = ( xfrm[ 1, 2 ] - xfrm[ 2, 1 ] ) * s;
quat.Y = ( xfrm[ 2, 0 ] - xfrm[ 0, 2 ] ) * s;
quat.Z = ( xfrm[ 0, 1 ] - xfrm[ 1, 0 ] ) * s;
}
else {
// The sum is negative
// 4 muls, 1 div, 8 adds, 1 trig function call
int[] nIndex = { 1, 2, 0 };
int i, j, k;
i = 0;
if( xfrm[ 1, 1 ] > xfrm[ i, i ] )
i = 1;
if( xfrm[ 2, 2 ] > xfrm[ i, i ] )
i = 2;
j = nIndex[i];
k = nIndex[j];
float s = (float) Math.Sqrt(( xfrm[ i, i ] - ( xfrm[ j, j ] + xfrm[ k, k ] ) ) + 1.0f );
quat[ i ] = s * 0.5f;
if( s != 0.0 ) {
s = 0.5f / s;
}
quat[ j ] = ( xfrm[ i, j ] + xfrm[ j, i ] ) * s;
quat[ k ] = ( xfrm[ i, k ] + xfrm[ k, i ] ) * s;
quat[ 3 ] = ( xfrm[ j, k ] - xfrm[ k, j ] ) * s;
}
return quat;
}
public void SetVoxels(Matrix3D <bool> voxels)
{
SetVoxelsThread(voxels);
SetVoxelsApply();
}
public MatrixTransform(Matrix3D matrix)
{
Matrix = matrix;
}
public void SetVoxelsThread(Matrix3D <bool> voxels)
/// Prepares verts and tris in thread
{
List <Vector3> verts = new List <Vector3>();
List <Vector3> normals = new List <Vector3>();
List <int> tris = new List <int>();
//bottom/top faces
for (int x = voxels.cube.offset.x; x < voxels.cube.offset.x + voxels.cube.size.x; x++)
{
for (int z = voxels.cube.offset.z; z < voxels.cube.offset.z + voxels.cube.size.z; z++)
{
bool prevVal = false;
for (int y = 0; y < voxels.cube.size.y + 1; y++)
{
bool nextVal = y < voxels.cube.size.y ? voxels[x, y + voxels.cube.offset.y, z] : false;
if (nextVal && !prevVal)
{
int vertsCount = verts.Count;
tris.Add(vertsCount + 1);
tris.Add(vertsCount + 2);
tris.Add(vertsCount);
tris.Add(vertsCount + 3);
tris.Add(vertsCount);
tris.Add(vertsCount + 2);
verts.Add(new Vector3(x, y + voxels.cube.offset.y, z));
verts.Add(new Vector3(x + 1, y + voxels.cube.offset.y, z));
verts.Add(new Vector3(x + 1, y + voxels.cube.offset.y, z + 1));
verts.Add(new Vector3(x, y + voxels.cube.offset.y, z + 1));
normals.Add(new Vector3(0, -1, 0));
normals.Add(new Vector3(0, -1, 0));
normals.Add(new Vector3(0, -1, 0));
normals.Add(new Vector3(0, -1, 0));
}
if (!nextVal && prevVal)
{
int vertsCount = verts.Count;
tris.Add(vertsCount + 1);
tris.Add(vertsCount + 2);
tris.Add(vertsCount);
tris.Add(vertsCount + 3);
tris.Add(vertsCount);
tris.Add(vertsCount + 2);
verts.Add(new Vector3(x, y + voxels.cube.offset.y, z));
verts.Add(new Vector3(x, y + voxels.cube.offset.y, z + 1));
verts.Add(new Vector3(x + 1, y + voxels.cube.offset.y, z + 1));
verts.Add(new Vector3(x + 1, y + voxels.cube.offset.y, z));
normals.Add(new Vector3(0, 1, 0));
normals.Add(new Vector3(0, 1, 0));
normals.Add(new Vector3(0, 1, 0));
normals.Add(new Vector3(0, 1, 0));
}
prevVal = nextVal;
}
}
}
//left/right faces
for (int x = voxels.cube.offset.x; x < voxels.cube.offset.x + voxels.cube.size.x; x++)
{
for (int y = voxels.cube.offset.y; y < voxels.cube.offset.y + voxels.cube.size.y; y++)
{
bool prevVal = false;
for (int z = 0; z < voxels.cube.size.z + 1; z++)
{
bool nextVal = z < voxels.cube.size.z ? voxels[x, y, z + voxels.cube.offset.z] : false;
if (nextVal && !prevVal)
{
int vertsCount = verts.Count;
tris.Add(vertsCount + 1);
tris.Add(vertsCount + 2);
tris.Add(vertsCount);
tris.Add(vertsCount + 3);
tris.Add(vertsCount);
tris.Add(vertsCount + 2);
verts.Add(new Vector3(x, y, z + voxels.cube.offset.z));
verts.Add(new Vector3(x, y + 1, z + voxels.cube.offset.z));
verts.Add(new Vector3(x + 1, y + 1, z + voxels.cube.offset.z));
verts.Add(new Vector3(x + 1, y, z + voxels.cube.offset.z));
normals.Add(new Vector3(0, 0, 1));
normals.Add(new Vector3(0, 0, 1));
normals.Add(new Vector3(0, 0, 1));
normals.Add(new Vector3(0, 0, 1));
}
if (!nextVal && prevVal)
{
int vertsCount = verts.Count;
tris.Add(vertsCount + 1);
tris.Add(vertsCount + 2);
tris.Add(vertsCount);
tris.Add(vertsCount + 3);
tris.Add(vertsCount);
tris.Add(vertsCount + 2);
verts.Add(new Vector3(x, y, z + voxels.cube.offset.z));
verts.Add(new Vector3(x + 1, y, z + voxels.cube.offset.z));
verts.Add(new Vector3(x + 1, y + 1, z + voxels.cube.offset.z));
verts.Add(new Vector3(x, y + 1, z + voxels.cube.offset.z));
normals.Add(new Vector3(0, 0, -1));
normals.Add(new Vector3(0, 0, -1));
normals.Add(new Vector3(0, 0, -1));
normals.Add(new Vector3(0, 0, -1));
}
prevVal = nextVal;
}
}
}
//left/right faces
for (int y = voxels.cube.offset.y; y < voxels.cube.offset.y + voxels.cube.size.y; y++)
{
for (int z = voxels.cube.offset.z; z < voxels.cube.offset.z + voxels.cube.size.z; z++)
{
bool prevVal = false;
for (int x = 0; x < voxels.cube.size.x + 1; x++)
{
bool nextVal = x < voxels.cube.size.x ? voxels[x + voxels.cube.offset.x, y, z] : false;
if (nextVal && !prevVal)
{
int vertsCount = verts.Count;
tris.Add(vertsCount + 1);
tris.Add(vertsCount + 2);
tris.Add(vertsCount);
tris.Add(vertsCount + 3);
tris.Add(vertsCount);
tris.Add(vertsCount + 2);
verts.Add(new Vector3(x + voxels.cube.offset.x, y, z));
verts.Add(new Vector3(x + voxels.cube.offset.x, y, z + 1));
verts.Add(new Vector3(x + voxels.cube.offset.x, y + 1, z + 1));
verts.Add(new Vector3(x + voxels.cube.offset.x, y + 1, z));
normals.Add(new Vector3(1, 0, 0));
normals.Add(new Vector3(1, 0, 0));
normals.Add(new Vector3(1, 0, 0));
normals.Add(new Vector3(1, 0, 0));
}
if (!nextVal && prevVal)
{
int vertsCount = verts.Count;
tris.Add(vertsCount + 1);
tris.Add(vertsCount + 2);
tris.Add(vertsCount);
tris.Add(vertsCount + 3);
tris.Add(vertsCount);
tris.Add(vertsCount + 2);
verts.Add(new Vector3(x + voxels.cube.offset.x, y, z));
verts.Add(new Vector3(x + voxels.cube.offset.x, y + 1, z));
verts.Add(new Vector3(x + voxels.cube.offset.x, y + 1, z + 1));
verts.Add(new Vector3(x + voxels.cube.offset.x, y, z + 1));
normals.Add(new Vector3(-1, 0, 0));
normals.Add(new Vector3(-1, 0, 0));
normals.Add(new Vector3(-1, 0, 0));
normals.Add(new Vector3(-1, 0, 0));
}
prevVal = nextVal;
}
}
}
vertices = verts.ToArray();
this.tris = tris.ToArray();
this.normals = normals.ToArray();
}
public static Vector3D Transform(Vector3D position, Matrix3D matrix)
{
Vector3D vector;
float num3 = (((position.X * matrix.M11) + (position.Y * matrix.M21)) + (position.Z * matrix.M31)) + matrix.M41;
float num2 = (((position.X * matrix.M12) + (position.Y * matrix.M22)) + (position.Z * matrix.M32)) + matrix.M42;
float num = (((position.X * matrix.M13) + (position.Y * matrix.M23)) + (position.Z * matrix.M33)) + matrix.M43;
vector.X = num3;
vector.Y = num2;
vector.Z = num;
return vector;
}
private void Load()
{
XDocument doc = LoadXml();
if (null == doc)
{
return;
}
XElement samples = GetSamples(doc);
if (null == samples)
{
return;
}
frameRate = GetSceneInfo(doc, "frameRate");
ticksPerFrame = GetSceneInfo(doc, "ticksPerFrame");
if (frameRate < 1 || ticksPerFrame < 1)
{
return;
}
PositionText = "<Point3DAnimationUsingKeyFrames Storyboard.TargetProperty=\"(ProjectionCamera.Position)\" Storyboard.TargetName=\"camera\">\n";
LookDirectionText = "<Vector3DAnimationUsingKeyFrames Storyboard.TargetProperty=\"(ProjectionCamera.LookDirection)\" Storyboard.TargetName=\"camera\">\n";
lastPosition = new Point3D();
lastLook = new Vector3D();
bool isFirstLoop = true;
bool isPositionChanged = false;
bool isLookDirectionChanged = false;
bool isTruncatedTimeFixed = false;
TimeSpan truncatedTime = TimeSpan.Zero;
foreach (XElement s in samples.Descendants())
{
TimeSpan time = GetTimeSpan(s);
double[] v = s.Attribute("v").Value.Split(' ').Select(x => Double.Parse(x)).ToArray();
Matrix3D vMatrix = GetMatrixFrom(v);
// Get new Position and LookDirection
Point3D position = GetPosition(vMatrix);
Vector3D lookDirection = GetLook(vMatrix);
if (isFirstLoop)
{
// Always append starting values
PositionText += GetXaml(typeof(EasingPoint3DKeyFrame).Name, time, position.ToString());
LookDirectionText += GetXaml(typeof(EasingVector3DKeyFrame).Name, time, lookDirection.ToString());
}
else
{
isPositionChanged = position != lastPosition;
isLookDirectionChanged = lookDirection != lastLook;
// Truncate time if the option is set
if (IsTruncateBlankKeyFrames)
{
// Fix truncatedTime if changes occur in this loop. Else save time to be truncated
if (!isTruncatedTimeFixed)
{
if (isPositionChanged || isLookDirectionChanged)
{
isTruncatedTimeFixed = true;
}
else
{
truncatedTime = time;
}
}
// isTruncatedTimeFixed is set if any change
if (isTruncatedTimeFixed)
{
time -= truncatedTime;
}
}
if (isPositionChanged)
{
PositionText += GetXaml(typeof(EasingPoint3DKeyFrame).Name, time, position.ToString());
}
if (isLookDirectionChanged)
{
LookDirectionText += GetXaml(typeof(EasingVector3DKeyFrame).Name, time, lookDirection.ToString());
}
}
lastPosition = position;
lastLook = lookDirection;
isFirstLoop = false;
}
PositionText += "</Point3DAnimationUsingKeyFrames>";
LookDirectionText += "</Vector3DAnimationUsingKeyFrames>";
}
public int OnMouseMove(Point _currentHit, Viewport3DXext _vp, out Point3D snapPoint,
bool _testEnd, bool _testMid, bool _testInters, int _pixelTolerance = 20)
{
int foundIndex = -1;
snapPoint = new Point3D(0, 0, 0);
if (_currentHit == null || _vp == null)
{
return(foundIndex);
}
// test points priority: END > INTERSECTION > MIDPOINT
if (_testEnd)
{
foundIndex = OcTreeManager.SnapTestPoints(_currentHit, _vp, out snapPoint, _pixelTolerance,
this.PointsVisible_End);
if (foundIndex != -1)
{
this.snapMarker.Geometry = OcTreeManager.EndPoint;
}
}
if (_testInters && foundIndex == -1)
{
foundIndex = OcTreeManager.SnapTestPoints(_currentHit, _vp, out snapPoint, _pixelTolerance,
this.PointsCollision);
if (foundIndex != -1)
{
this.snapMarker.Geometry = OcTreeManager.IntPoint;
}
}
if (_testMid && foundIndex == -1)
{
foundIndex = OcTreeManager.SnapTestPoints(_currentHit, _vp, out snapPoint, _pixelTolerance,
this.PointsVisible_Mid);
if (foundIndex != -1)
{
this.snapMarker.Geometry = OcTreeManager.MidPoint;
}
}
// process display
if (foundIndex != -1)
{
// snap found
Vector3D offset = snapPoint - new Point3D(0, 0, 0);
double scale = Vector3.Distance(snapPoint.ToVector3(), _vp.Camera.Position.ToVector3());
float factOrthoCam = ((_vp.Camera as HelixToolkit.SharpDX.OrthographicCamera) != null) ? SNAP_ORTHOFACT : 1.0f;
scale *= factOrthoCam;
Matrix3D M = Matrix3D.Identity;
M.Scale(new Vector3D(scale, scale, scale));
M.Translate(offset);
this.snapMarker.Transform = new MatrixTransform3D(M);
this.snapMarker.Visibility = Visibility.Visible;
}
else
{
this.snapMarker.Transform = new MatrixTransform3D(Matrix3D.Identity);
this.snapMarker.Visibility = Visibility.Hidden;
}
return(foundIndex);
}
public void setImagePixelSize()
{
AngleResolution = Math.Acos((Matrix3D.SumOfDiagonalCompenent(Rotations[Rotations.Length / 2] * Rotations[Rotations.Length / 2 + 1].Inverse()) - 1) / 2);
ImagePixelSize = FormDiffractionSimulator.CameraLength2 * Math.Tan(AngleResolution);
}
public static void CalcW()
{
NewTransfer.Wmatrix = Matrix3D.Multiply(Pmatrix, Fmatrix);
}
/*
* public Matrix3D transformation_matrix(int a) //4 DOF Lue's calculation
* {
* double LengthUpperArm = 68.58;
* double LengthForearm = 96.393;
* double r = 14;
* // calculate forward kinematics and haptic forces, assuming kineAngle[0] is leftUpperBevel and kineAngle[1] is leftLowerBevel
* double theta1 = ((leftUpperBevel + leftLowerBevel) / 2) * Math.PI / 180; //theta1 = 0;
* double theta2 = ((leftUpperBevel - leftLowerBevel) / 2) * Math.PI / 180; //theta2 = 0;
* double theta3 = leftElbow * Math.PI / 180; //theta3 = 0;
*
* double c1 = Math.Cos(theta1);
* double s1 = Math.Sin(theta1);
* double c2 = Math.Cos(theta2);
* double s2 = Math.Sin(theta2);
* double c3 = Math.Cos(theta3);
* double s3 = Math.Sin(theta3); //twist = 90;
* double theta4 = twist * Math.PI / 180;
* double c4 = Math.Cos(theta4);
* double s4 = Math.Sin(theta4);
* Matrix3D T1 = new Matrix3D(c1, -s1, 0, 0,
* s1, c1, 0, 0,
* 0, 0, 1, 0,
* 0, 0, 0, 1);
* Matrix3D T2 = new Matrix3D(c2, -s2, 0, 0,
* 0, 0, -1, 0,
* s2, c2, 0, 0,
* 0, 0, 0, 1);
* Matrix3D T3 = new Matrix3D(c3, -s3, 0, LengthUpperArm,
* 0, 0, 1, 0,
* -s3, -c3, 0, 0,
* 0, 0, 0, 1);
* Matrix3D T4 = new Matrix3D(1, 0, 0, LengthForearm,
* 0, c4, -s4, 0,
* 0, s4, c4, 0,
* 0, 0, 0, 1);
* Matrix3D T5 = new Matrix3D(1, 0, 0, 0,
* 0, 1, 0, -2*r,
* 0, 0, 1, 0,
* 0, 0, 0, 1);
* Matrix3D T = new Matrix3D();
* switch(a)
* {
* case 4:
* T = Matrix3D.Multiply(T1, Matrix3D.Multiply(T2, Matrix3D.Multiply(T3, T4)));
* break;
* case 5:
* T = Matrix3D.Multiply(T1,Matrix3D.Multiply(T2, Matrix3D.Multiply(T3, Matrix3D.Multiply(T4, T5))));
* break;
* case 45:
* T = T5;
* break;
* default:
* break;
*
* }
* return T;
* }*/
/*public Matrix3D transformation_matrix(int a) //5 DOF my calculation
* {
* double LengthUpperArm = 68.58;
* double LengthForearm = 96.393;
* double r = 14;
* // calculate forward kinematics and haptic forces, assuming kineAngle[0] is leftUpperBevel and kineAngle[1] is leftLowerBevel
* double theta1 = ((leftUpperBevel + leftLowerBevel) / 2) * Math.PI / 180;// theta1 = 0;
* double theta2 = ((leftUpperBevel - leftLowerBevel) / 2) * Math.PI / 180;// theta2 = 0;
* double theta3 = leftElbow * Math.PI / 180 + Math.PI / 2; //theta3 = 0 + Math.PI / 2;
*
* double c1 = Math.Cos(theta1);
* double s1 = Math.Sin(theta1);
* double c2 = Math.Cos(theta2);
* double s2 = Math.Sin(theta2);
* double c3 = Math.Cos(theta3);
* double s3 = Math.Sin(theta3); //twist = 0;
* double theta4 = twist * Math.PI / 180;
* double c4 = Math.Cos(theta4);
* double s4 = Math.Sin(theta4);
* Matrix3D T1 = new Matrix3D(c1, -s1, 0, 0,
* s1, c1, 0, 0,
* 0, 0, 1, 0,
* 0, 0, 0, 1);
* Matrix3D T2 = new Matrix3D(c2, -s2, 0, 0,
* 0, 0, -1, 0,
* s2, c2, 0, 0,
* 0, 0, 0, 1);
* Matrix3D T3 = new Matrix3D(c3, -s3, 0, LengthUpperArm,
* 0, 0, 1, 0,
* -s3, -c3, 0, 0,
* 0, 0, 0, 1);
* Matrix3D T4 = new Matrix3D(c4, -s4, 0, 0,
* 0, 0, -1, -LengthForearm,
* s4, c4, 0, 0,
* 0, 0, 0, 1);
* Matrix3D T5 = new Matrix3D(1, 0, 0, -2 * r,
* 0, 1, 0, 0,
* 0, 0, 1, 0,
* 0, 0, 0, 1);
* Matrix3D T = new Matrix3D();
* switch (a)
* {
* case 4:
* T = Matrix3D.Multiply(T1, Matrix3D.Multiply(T2, Matrix3D.Multiply(T3, T4)));
* break;
* case 5:
* T = Matrix3D.Multiply(T1, Matrix3D.Multiply(T2, Matrix3D.Multiply(T3, Matrix3D.Multiply(T4, T5))));
* break;
* case 35:
* T = Matrix3D.Multiply(T4, T5);
* break;
* case 45:
* T = T5;
* break;
* default:
* break;
*
* }
* return T;
* }*/
public Matrix3D transformation_matrix(int a)
{
double LengthUpperArm = 68.58;
double LengthForearm = 96.393;
double r = 14;
// calculate forward kinematics and haptic forces, assuming kineAngle[0] is leftUpperBevel and kineAngle[1] is leftLowerBevel
double theta1 = ((leftUpperBevel + leftLowerBevel) / 2) * Math.PI / 180; //theta1 = 0;
double theta2 = ((leftUpperBevel - leftLowerBevel) / 2) * Math.PI / 180; //theta2 = 0;
double theta3 = leftElbow * Math.PI / 180 + Math.PI / 2; //theta3 = 0 + Math.PI / 2;
double c1 = Math.Cos(theta1);
double s1 = Math.Sin(theta1);
double c2 = Math.Cos(theta2);
double s2 = Math.Sin(theta2);
double c3 = Math.Cos(theta3);
double s3 = Math.Sin(theta3); //twist = 0;
double theta4 = twist;
double c5 = Math.Cos(theta4);
double s5 = Math.Sin(theta4);
Matrix3D T1 = new Matrix3D(c1, -s1, 0, 0,
s1, c1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix3D T2 = new Matrix3D(c2, -s2, 0, 0,
0, 0, -1, 0,
s2, c2, 0, 0,
0, 0, 0, 1);
Matrix3D T3 = new Matrix3D(c3, -s3, 0, LengthUpperArm,
0, 0, 1, 0,
-s3, -c3, 0, 0,
0, 0, 0, 1);
Matrix3D T4 = new Matrix3D(1, 0, 0, 0,
0, 0, -1, -LengthForearm,
0, 1, 0, 0,
0, 0, 0, 1);
Matrix3D T5 = new Matrix3D(c5, -s5, 0, 0,
s5, c5, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix3D T6 = new Matrix3D(1, 0, 0, -2 * r,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1); // r is negative to place the needle on the right. It's appear on the left since X is inverted
Matrix3D T = new Matrix3D();
switch (a)
{
case 4:
T = Matrix3D.Multiply(T1, Matrix3D.Multiply(T2, Matrix3D.Multiply(T3, T4)));
break;
case 5:
T = Matrix3D.Multiply(T1, Matrix3D.Multiply(T2, Matrix3D.Multiply(T3, Matrix3D.Multiply(T4, T5))));
break;
case 6:
T = Matrix3D.Multiply(T1, Matrix3D.Multiply(T2, Matrix3D.Multiply(T3, Matrix3D.Multiply(T4, Matrix3D.Multiply(T5, T6)))));
break;
case 46:
T = Matrix3D.Multiply(T5, T6);
break;
case 56:
T = T6;
break;
default:
break;
}
return(T);
}
public static Vector3D Multiply( Vector3D vector, Matrix3D matrix )
{
return matrix.Transform( vector );
}
public void Reset()
{
Matrix = new Matrix3D();
}
private Matrix3D RewindMatrix( Matrix3D m, double scale )
{
return new Matrix3D(
( ( m.M11 - 1.0 ) * scale ) + 1.0,
m.M12 * scale,
m.M13 * scale,
m.M14 * scale,
m.M21 * scale,
( ( m.M22 - 1.0 ) * scale ) + 1.0,
m.M23 * scale,
m.M24 * scale,
m.M31 * scale,
m.M32 * scale,
( ( m.M33 - 1.0 ) * scale ) + 1.0,
m.M34 * scale,
m.OffsetX * scale,
m.OffsetY * scale,
m.OffsetZ * scale,
( ( m.M44 - 1.0 ) * scale ) + 1.0 );
}
private IEnumerable <Profile2D> GetTextProfiles()
{
/*
* se scritta in cerchio faccio lettera per volta.
*/
if (WriteInCircle)
{
var charCount = TextToEngrave.Count();
var radianAngleWidth = GeometryHelper.DegreeToRadian(AngleWidth);
var angleIncre = radianAngleWidth / charCount;
/*
* scrivo in senso orario , quindi devo decrementare angolo
*
* devo fare una lettera alla volta con angolo diverso
*
*/
var profiles = new List <Profile2D>();
var radianAngleStart = GeometryHelper.DegreeToRadian(AngleStart);
//if (radianAngleStart < 0)
// radianAngleStart += 2 * Math.PI;
for (int i = 0; i < charCount; i++)
{
var letterProfiles = GetProfiles(TextToEngrave[i].ToString(), FontHeight);
if (letterProfiles == null)
{
continue;
}
var angleCurrent = -((angleIncre * i) + radianAngleStart);
var rotationMatrix = new Matrix3D();
rotationMatrix.TranslatePrepend(new Vector3D(0, CenterY + RadiusCircle, 0));
rotationMatrix.RotateAt(new Quaternion(new Vector3D(0, 0, 1), GeometryHelper.RadianToDegree(angleCurrent)), new Point3D(CenterX, CenterY, 0));
foreach (var profile2D in letterProfiles)
{
profile2D.Multiply(rotationMatrix);
}
profiles.AddRange(letterProfiles);
}
return(profiles);
}
else
{
var profiles = GetProfiles(TextToEngrave, FontHeight);
var matrix = new Matrix3D();
matrix.Translate(new Vector3D(CenterX, CenterY, 0));
foreach (var profile2D in profiles)
{
profile2D.Multiply(matrix);
}
return(profiles);
}
}
