public static Matrix4x4 operator *(Matrix4x4 lhs, Matrix4x4 rhs)
{
var result = new Matrix4x4();
result.I00 = lhs.I00 * rhs.I00 + lhs.I01 * rhs.I10 + lhs.I02 * rhs.I20;
result.I01 = lhs.I00 * rhs.I01 + lhs.I01 * rhs.I11 + lhs.I02 * rhs.I21;
result.I02 = lhs.I00 * rhs.I02 + lhs.I01 * rhs.I12 + lhs.I02 * rhs.I22;
result.I03 = lhs.I00 * rhs.I03 + lhs.I01 * rhs.I13 + lhs.I02 * rhs.I23 + lhs.I03;
result.I10 = lhs.I10 * rhs.I00 + lhs.I11 * rhs.I10 + lhs.I12 * rhs.I20;
result.I11 = lhs.I10 * rhs.I01 + lhs.I11 * rhs.I11 + lhs.I12 * rhs.I21;
result.I12 = lhs.I10 * rhs.I02 + lhs.I11 * rhs.I12 + lhs.I12 * rhs.I22;
result.I13 = lhs.I10 * rhs.I03 + lhs.I11 * rhs.I13 + lhs.I12 * rhs.I23 + lhs.I13;
result.I20 = lhs.I20 * rhs.I00 + lhs.I21 * rhs.I10 + lhs.I22 * rhs.I20;
result.I21 = lhs.I20 * rhs.I01 + lhs.I21 * rhs.I11 + lhs.I22 * rhs.I21;
result.I22 = lhs.I20 * rhs.I02 + lhs.I21 * rhs.I12 + lhs.I22 * rhs.I22;
result.I23 = lhs.I20 * rhs.I03 + lhs.I21 * rhs.I13 + lhs.I22 * rhs.I23 + lhs.I23;
result.I30 = lhs.I30 * rhs.I00 + lhs.I31 * rhs.I10 + lhs.I32 * rhs.I20;
result.I31 = lhs.I30 * rhs.I01 + lhs.I31 * rhs.I11 + lhs.I32 * rhs.I21;
result.I32 = lhs.I30 * rhs.I02 + lhs.I31 * rhs.I12 + lhs.I32 * rhs.I22;
result.I33 = lhs.I30 * rhs.I03 + lhs.I31 * rhs.I13 + lhs.I32 * rhs.I23 + lhs.I33;
return result;
}
public static Vector3 DoForwardKinematics(
double baseAngle,
double shoulderAngle,
double elbowAngle,
double wristTiltAngle,
double wristRotationAngle,
Vector3 endVector)
{
DHFrameInfo[] frameInfos = GetFrameInfos(baseAngle, shoulderAngle, elbowAngle, wristTiltAngle, wristRotationAngle);
Matrix4x4[] transformations = new Matrix4x4[frameInfos.Length];
Matrix4x4 completeTransformation = Matrix4x4.Identity;
for (int i = frameInfos.Length - 1; i >= 0; i--)
{
transformations[i] = DHTransformation.ToPreviousFrame(frameInfos[i]);
completeTransformation = transformations[i] * completeTransformation;
}
Vector4 endVector4 = new Vector4(endVector);
Vector4 transformedVector = completeTransformation * endVector4;
return new Vector3(
transformedVector.V0,
transformedVector.V1,
transformedVector.V2
);
}
public static Matrix4x4 Scale(double x, double y, double z)
{
var result = new Matrix4x4();
result.I00 = x;
result.I11 = y;
result.I22 = z;
return result;
}
public static Matrix4x4 Translation(double x, double y, double z)
{
var result = new Matrix4x4();
result.I03 = x;
result.I13 = y;
result.I23 = z;
return result;
}
public static Matrix4x4 RotationY(double radiants)
{
var result = new Matrix4x4();
var cos = Math.Cos(radiants);
var sin = Math.Sin(radiants);
result.I00 = cos;
result.I02 = -sin;
result.I20 = sin;
result.I22 = cos;
return result;
}
public Matrix4x4(Matrix4x4 matrix4x4)
: this(
matrix4x4.I00, matrix4x4.I01, matrix4x4.I02, matrix4x4.I03,
matrix4x4.I10, matrix4x4.I11, matrix4x4.I12, matrix4x4.I13,
matrix4x4.I20, matrix4x4.I21, matrix4x4.I22, matrix4x4.I23,
matrix4x4.I30, matrix4x4.I31, matrix4x4.I32, matrix4x4.I33
) { }
public static Matrix4x4 operator +(Matrix4x4 lhs, Matrix4x4 rhs)
{
var result = new Matrix4x4();
for (int i = 0; i < 15; ++i)
result[i] = lhs[i] + rhs[i];
return result;
}