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);
}
