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