public TransformationMatrix3D(Vector3D trans) : base(4) { for (var i = 0; i < 4; i++) { base[i, i] = 1.0; } Translation = trans; }
public TransformationMatrix3D(Vector3D trans, RotationMatrix3D rot) : base(4) { for (var i = 0; i < 4; i++) { base[i, i] = 1.0; } Rotation = rot; Translation = trans; }
public Point3D(Point3D point) { _position = point._position; }
public static Vector3D Cross(Vector vec1, Vector vec2) { var vector3D = new Vector3D(); vector3D[0] = vec1[1]*vec2[2] - vec1[2]*vec2[1]; vector3D[1] = vec1[2]*vec2[0] - vec1[0]*vec2[2]; vector3D[2] = vec1[0]*vec2[1] - vec1[1]*vec2[0]; return vector3D; }
public static RotationMatrix3D RotateAroundVector(Vector3D vector, double angle) { return (RotationMatrix3D) Quaternion.FromAxisAngle(vector, angle); }
public Plane3D FitPlaneToPoints(Collection<Point3D> points) { if (points == null) { throw new NullReferenceException(); } if (points.Count < 3) { throw new MatrixException("Not enough points to fit a plane"); } var point3D = Centroid(points); var num = 0.0; var num2 = 0.0; var num3 = 0.0; var num4 = 0.0; var num5 = 0.0; var num6 = 0.0; foreach (var current in points) { var num7 = current.X - point3D.X; var num8 = current.Y - point3D.Y; var num9 = current.Z - point3D.Z; num += num7*num7; num2 += num8*num8; num3 += num9*num9; num4 += num7*num8; num5 += num8*num9; num6 += num7*num9; } var mat = new SquareMatrix(3, new[] { num, num4, num6, num4, num2, num5, num6, num5, num3 }); var sVD = new SVD(mat); var normal = new Vector3D(sVD.U.GetColumn(sVD.SmallestSingularIndex)); var plane3D = new Plane3D(point3D, normal); CalculateErrors(points, plane3D); return plane3D; }
public Quaternion(Vector3D vector, double scalar) { Vector = vector; Scalar = scalar; }
public static Point3D operator *(RotationMatrix3D mat, Point3D pt) { var vec = new Vector3D(pt.X, pt.Y, pt.Z); return new Point3D(new Vector3D(mat*vec)); }
public Plane3D(Point3D point, Vector3D normal) { Point = point; Normal = normal; }
public static Point3D ToPoint3D(Vector3D vec) { return new Point3D(vec); }
public static Vector3D Subtract(Vector3D v1, Vector3D v2) { return new Vector3D(v1 - v2); }
public static Vector3D Negate(Vector3D vec) { return new Vector3D(-vec.X, -vec.Y, -vec.Z); }
public static Vector3D Multiply(Vector3D vec, double scalar) { return new Vector3D(vec*scalar); }
public static Vector3D Divide(Vector3D vec, double scalar) { return new Vector3D(vec/scalar); }
public Point3D(Vector3D position) { _position = position; }
public Point3D(double x, double y, double z) { _position = new Vector3D(x, y, z); }
private void WriteValues(Vector3D result, double scalar, bool isScalar) { switch (isScalar) { case false: OutputItems.V1 = result.X; OutputItems.V2 = result.Y; OutputItems.V3 = result.Z; break; case true: OutputItems.V1 = scalar; OutputItems.V2 = result.X; OutputItems.V3 = result.Y; OutputItems.V4 = result.Z; break; } }
public static Quaternion Multiply(Quaternion q1, Quaternion q2) { var quaternion = new Quaternion(); var vector3D = new Vector3D(q1.X, q1.Y, q1.Z); var vector3D2 = new Vector3D(q2.X, q2.Y, q2.Z); var w = q1.W; var w2 = q2.W; var vector3D3 = (Vector3D) (w*vector3D2 + w2*vector3D) + Vector3D.Cross(vector3D, vector3D2); quaternion.X = vector3D3[0]; quaternion.Y = vector3D3[1]; quaternion.Z = vector3D3[2]; quaternion.W = w*w2 - Classes.Vector.Dot(vector3D, vector3D2); return quaternion; }
public void Convert() { if (!(InputItems == null | OutputItems == null)) { if (!_isConverting) { _isConverting = true; var result = new Vector3D(); var num = 0.0; var quaternion = new Quaternion(); switch (InputItems.SelectedItem) { case CartesianEnum.ABB_Quaternion: quaternion = new Quaternion(InputItems.V1, InputItems.V2, InputItems.V3, InputItems.V4); break; case CartesianEnum.Roll_Pitch_Yaw: _rotationMatrix = RotationMatrix3D.FromRPY(InputItems.V1, InputItems.V2, InputItems.V3); break; case CartesianEnum.Axis_Angle: _rotationMatrix = RotationMatrix3D.RotateAroundVector( new Vector3D(InputItems.V1, InputItems.V2, InputItems.V3), InputItems.V4); break; case CartesianEnum.Kuka_ABC: _rotationMatrix = RotationMatrix3D.FromABC(InputItems.V1, InputItems.V2, InputItems.V3); break; case CartesianEnum.Euler_ZYZ: _rotationMatrix = RotationMatrix3D.FromEulerZYZ(InputItems.V1, InputItems.V2, InputItems.V3); break; } switch (OutputItems.SelectedItem) { case CartesianEnum.ABB_Quaternion: quaternion = (Quaternion) _rotationMatrix; result = quaternion.Vector; num = quaternion.Scalar; break; case CartesianEnum.Roll_Pitch_Yaw: result = _rotationMatrix.RPY; break; case CartesianEnum.Axis_Angle: result = _rotationMatrix.RotationAxis(); num = _rotationMatrix.RotationAngle(); break; case CartesianEnum.Kuka_ABC: result = _rotationMatrix.ABC; break; case CartesianEnum.Euler_ZYZ: result = _rotationMatrix.EulerZYZ; break; case CartesianEnum.Alpha_Beta_Gamma: result = _rotationMatrix.ABG; break; } var text = quaternion.ToString(); // var text = quaternion.ToString("F3"); if (Matrix != null && Matrix != text) { Matrix = text; } WriteValues(result, 0.0, false); if (OutputItems.SelectedItem == CartesianEnum.ABB_Quaternion) { WriteValues(result, num, true); } if (OutputItems.SelectedItem == CartesianEnum.Axis_Angle) { OutputItems.V4 = num; } _isConverting = false; } } }
public Line3D FitLineToPoints(Collection<Point3D> points) { if (points == null) { throw new MatrixNullReference(); } var point3D = Centroid(points); var num = 0.0; var num2 = 0.0; var num3 = 0.0; var num4 = 0.0; var num5 = 0.0; var num6 = 0.0; foreach (var current in points) { var num7 = current.X - point3D.X; var num8 = current.Y - point3D.Y; var num9 = current.Z - point3D.Z; num += num7*num7; num2 += num8*num8; num3 += num9*num9; num4 += num7*num8; num5 += num8*num9; num6 += num7*num9; } var mat = new SquareMatrix(3, new[] { num2 + num3, -num4, -num6, -num4, num3 + num, -num5, -num6, -num5, num + num2 }); var sVD = new SVD(mat); var direction = new Vector3D(sVD.U.GetColumn(sVD.SmallestSingularIndex)); var line3D = new Line3D(point3D, direction); CalculateErrors(points, line3D); return line3D; }
public Line3D(Point3D origin, Vector3D direction) { Origin = origin; Direction = direction; Direction.Normalise(); }
private Vector Circle3DErrorFunction(Vector vec) { var vector = new Vector(_solver.NumEquations); var index = 0; var point3D = new Point3D(vec[0], vec[1], vec[2]); var plane = new Plane3D(point3D, new Vector3D(vec[3], vec[4], vec[5])); foreach (var current in _measuredPoints) { var p = Project3D.PointOntoPlane(plane, current); var vector3D = point3D - p; vector3D.Normalise(); var point3D2 = new Point3D(); vector[index++] = current.X - point3D2.X; vector[index++] = current.Y - point3D2.Y; vector[index++] = current.Z - point3D2.Z; } vector[index] = new Vector3D(vec[3], vec[4], vec[5]).Length() - 1.0; return vector; }
public Point3D GetPoint(double u) { var vec = new Vector3D(u*Direction); return Origin + vec; }
public static Point3D operator *(TransformationMatrix3D mat, Point3D pt) { var vec = new Vector3D(pt.X, pt.Y, pt.Z); var vector3D = mat*vec; return new Point3D(vector3D.X, vector3D.Y, vector3D.Z); }
public static Point3D Add(Point3D point, Vector3D vec) { return new Point3D { X = point.X + vec.X, Y = point.Y + vec.Y, Z = point.Z + vec.Z }; }
public Point3D() { _position = new Vector3D(); }
public static Vector3D Add(Vector3D v1, Vector3D v2) { return new Vector3D(v1 + v2); }