剑指 Offer 笔记: T57 - II. 和为 s 的连续正数序列

本文介绍了一种解法,用于解决给定正整数target,找出所有和为目标值的连续正整数序列。通过滑动窗口技巧,算法的时间复杂度为O(target),空间复杂度为O(1),并在LeetCode上展示了Java实现。

T57 - II. 和为 s 的连续正数序列

输入一个正整数 target ,输出所有和为 target 的连续正整数序列(至少含有两个数)。

序列内的数字由小到大排列,不同序列按照首个数字从小到大排列。

示例 1:

输入:target = 9
输出:[[2,3,4],[4,5]]
示例 2:

输入:target = 15
输出:[[1,2,3,4,5],[4,5,6],[7,8]]

限制:

1 <= target <= 10^5

解法 1

没做出来

解法 2

class Solution {
    public int[][] findContinuousSequence(int target) {
        int left = 1;
        int right = 2;
        List<int[]> res = new ArrayList<>();

        while (left < right) {
            int sum = (left + right) * (right - left + 1) / 2;
            if (sum == target){
                int[] arr = new int[right - left + 1];
                for (int k = left; k <= right; k++) {
                    arr[k - left] = k;
                }
                res.add(arr);
                left++;
            }
            else if (sum < target) {
                right++;
            }
            else {
                left++;
            }
        }
        return res.toArray(new int[res.size()][]);
    }
}

执行用时:4 ms, 在所有 Java 提交中击败了**56.28%**的用户

内存消耗:36.4 MB, 在所有 Java 提交中击败了**86.99%**的用户

时间复杂度:O(target)O(target)。滑动窗口最多移动 target/2 次
空间复杂度:O(1)O(1)。排除必要的存储结果数组之外,只需要保存左右指针

作者:画手大鹏
链接:https://leetcode-cn.com/leetbook/read/illustrate-lcof/5017gs/
来源:力扣(LeetCode)
著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。

using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks; namespace RobotCalcForm { //Puma560六轴机械臂-逆解计算 public class Puma560IK { private double[,] _mdh; // 6x3矩阵 [d, a, alpha] public Puma560IK(double[,] mdhParameters) { if (mdhParameters == null) throw new ArgumentNullException("mdhParameters"); if (mdhParameters.GetLength(0) != 6 || mdhParameters.GetLength(1) != 3) throw new ArgumentException("MDH参数必须是6x3矩阵"); _mdh = (double[,])mdhParameters.Clone(); } public Puma560IK() { // 标准PUMA560 MDH参数 [d, a, alpha] _mdh = new double[,] { { 0, 0, 0 }, { 0, 0.180, -Math.PI/2 }, { 0, 0.600, 0 }, { 0.630, 0.130, -Math.PI/2 }, { 0, 0, Math.PI/2 }, { 0, 0, -Math.PI/2 } }; } public double[,] ComputeIK(double[,] Tbe) { // 提取目标位姿矩阵元素 double nx = Tbe[0, 0], ny = Tbe[1, 0], nz = Tbe[2, 0]; double ox = Tbe[0, 1], oy = Tbe[1, 1], oz = Tbe[2, 1]; double ax = Tbe[0, 2], ay = Tbe[1, 2], az = Tbe[2, 2]; double px = Tbe[0, 3], py = Tbe[1, 3], pz = Tbe[2, 3]; // 提取MDH参数 double d4 = _mdh[3, 0]; // 第4个关节的d参数 double a1 = _mdh[1, 1]; // 第2个关节的a参数 double a2 = _mdh[2, 1]; // 第3个关节的a参数 double a3 = _mdh[3, 1]; // 第4个关节的a参数 // 固定关节参数 double d2 = 0, d3 = 0; double f1 = -Math.PI / 2; // alpha1 double f3 = -Math.PI / 2; // alpha3 double f4 = Math.PI / 2; // alpha4 double f5 = -Math.PI / 2; // alpha5 // ===== 计算t1的两个可能解 ===== double denom1 = Math.Pow(px * Math.Sin(f1), 2) + Math.Pow(py * Math.Sin(f1), 2) - Math.Pow(d2 - d3, 2); if (denom1 < 0) denom1 = 0; double t11 = -Math.Atan2(-py, px) + Math.Atan2((d2 - d3) / Math.Sin(f1), Math.Sqrt(denom1)); double t12 = -Math.Atan2(-py, px) + Math.Atan2((d2 - d3) / Math.Sin(f1), -Math.Sqrt(denom1)); // ===== 计算中间变量 ===== double m3_1 = pz * Math.Sin(f1); double n3_1 = a1 - px * Math.Cos(t11) - py * Math.Sin(t11); double m3_2 = pz * Math.Sin(f1); double n3_2 = a1 - px * Math.Cos(t12) - py * Math.Sin(t12); // ===== 计算t3的四个可能解 ===== double expr3_1 = Math.Pow(m3_1, 2) + Math.Pow(n3_1, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2); double denom3_1 = Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) - Math.Pow(expr3_1, 2); if (denom3_1 < 0) denom3_1 = 0; double t31 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) + Math.Atan2(expr3_1 / Math.Sin(f3), Math.Sqrt(denom3_1)); double t32 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) + Math.Atan2(expr3_1 / Math.Sin(f3), -Math.Sqrt(denom3_1)); double expr3_2 = Math.Pow(m3_2, 2) + Math.Pow(n3_2, 2) - Math.Pow(a2, 2) - Math.Pow(a3, 2) - Math.Pow(d4, 2); double denom3_2 = Math.Pow(2 * a2 * d4 * Math.Sin(f3), 2) + Math.Pow(2 * a2 * a3, 2) - Math.Pow(expr3_2, 2); if (denom3_2 < 0) denom3_2 = 0; double t33 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) + Math.Atan2(expr3_2 / Math.Sin(f3), Math.Sqrt(denom3_2)); double t34 = -Math.Atan2(a2 * a3 / Math.Sin(f3), a2 * d4) + Math.Atan2(expr3_2 / Math.Sin(f3), -Math.Sqrt(denom3_2)); // ===== 计算t2的四个可能解 ===== double m2_1 = a2 + a3 * Math.Cos(t31) + d4 * Math.Sin(f3) * Math.Sin(t31); double n2_1 = a3 * Math.Sin(t31) - d4 * Math.Sin(f3) * Math.Cos(t31); double t21 = Math.Atan2(m3_1 * m2_1 + n2_1 * n3_1, m3_1 * n2_1 - m2_1 * n3_1); double m2_2 = a2 + a3 * Math.Cos(t32) + d4 * Math.Sin(f3) * Math.Sin(t32); double n2_2 = a3 * Math.Sin(t32) - d4 * Math.Sin(f3) * Math.Cos(t32); double t22 = Math.Atan2(m3_1 * m2_2 + n2_2 * n3_1, m3_1 * n2_2 - m2_2 * n3_1); double m2_3 = a2 + a3 * Math.Cos(t33) + d4 * Math.Sin(f3) * Math.Sin(t33); double n2_3 = a3 * Math.Sin(t33) - d4 * Math.Sin(f3) * Math.Cos(t33); double t23 = Math.Atan2(m3_2 * m2_3 + n2_3 * n3_2, m3_2 * n2_3 - m2_3 * n3_2); double m2_4 = a2 + a3 * Math.Cos(t34) + d4 * Math.Sin(f3) * Math.Sin(t34); double n2_4 = a3 * Math.Sin(t34) - d4 * Math.Sin(f3) * Math.Cos(t34); double t24 = Math.Atan2(m3_2 * m2_4 + n2_4 * n3_2, m3_2 * n2_4 - m2_4 * n3_2); // ===== 计算t5的八个可能解 ===== // 第一组 double m5_1 = -Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Cos(t21) + ay * Math.Sin(t11) * Math.Cos(t21) + az * Math.Sin(f1) * Math.Sin(t21)); double n5_1 = Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Sin(t21) + ay * Math.Sin(t11) * Math.Sin(t21) - az * Math.Sin(f1) * Math.Cos(t21)); double t51 = Math.Atan2( Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_1 * Math.Cos(t31) + n5_1 * Math.Sin(t31), 2)), (m5_1 * Math.Sin(t31) - n5_1 * Math.Cos(t31)) / (Math.Sin(f3) * Math.Sin(f4))); double t52 = Math.Atan2( -Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_1 * Math.Cos(t31) + n5_1 * Math.Sin(t31), 2)), (m5_1 * Math.Sin(t31) - n5_1 * Math.Cos(t31)) / (Math.Sin(f3) * Math.Sin(f4))); // 第二组 double m5_2 = -Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Cos(t22) + ay * Math.Sin(t11) * Math.Cos(t22) + az * Math.Sin(f1) * Math.Sin(t22)); double n5_2 = Math.Sin(f5) * (ax * Math.Cos(t11) * Math.Sin(t22) + ay * Math.Sin(t11) * Math.Sin(t22) - az * Math.Sin(f1) * Math.Cos(t22)); double t53 = Math.Atan2( Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_2 * Math.Cos(t32) + n5_2 * Math.Sin(t32), 2)), (m5_2 * Math.Sin(t32) - n5_2 * Math.Cos(t32)) / (Math.Sin(f3) * Math.Sin(f4))); double t54 = Math.Atan2( -Math.Sqrt(Math.Pow(ay * Math.Cos(t11) - ax * Math.Sin(t11), 2) + Math.Pow(m5_2 * Math.Cos(t32) + n5_2 * Math.Sin(t32), 2)), (m5_2 * Math.Sin(t32) - n5_2 * Math.Cos(t32)) / (Math.Sin(f3) * Math.Sin(f4))); // 第三组 double m5_3 = -Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Cos(t23) + ay * Math.Sin(t12) * Math.Cos(t23) + az * Math.Sin(f1) * Math.Sin(t23)); double n5_3 = Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Sin(t23) + ay * Math.Sin(t12) * Math.Sin(t23) - az * Math.Sin(f1) * Math.Cos(t23)); double t55 = Math.Atan2( Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_3 * Math.Cos(t33) + n5_3 * Math.Sin(t33), 2)), (m5_3 * Math.Sin(t33) - n5_3 * Math.Cos(t33)) / (Math.Sin(f3) * Math.Sin(f4))); double t56 = Math.Atan2( -Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_3 * Math.Cos(t33) + n5_3 * Math.Sin(t33), 2)), (m5_3 * Math.Sin(t33) - n5_3 * Math.Cos(t33)) / (Math.Sin(f3) * Math.Sin(f4))); // 第四组 double m5_4 = -Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Cos(t24) + ay * Math.Sin(t12) * Math.Cos(t24) + az * Math.Sin(f1) * Math.Sin(t24)); double n5_4 = Math.Sin(f5) * (ax * Math.Cos(t12) * Math.Sin(t24) + ay * Math.Sin(t12) * Math.Sin(t24) - az * Math.Sin(f1) * Math.Cos(t24)); double t57 = Math.Atan2( Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_4 * Math.Cos(t34) + n5_4 * Math.Sin(t34), 2)), (m5_4 * Math.Sin(t34) - n5_4 * Math.Cos(t34)) / (Math.Sin(f3) * Math.Sin(f4))); double t58 = Math.Atan2( -Math.Sqrt(Math.Pow(ay * Math.Cos(t12) - ax * Math.Sin(t12), 2) + Math.Pow(m5_4 * Math.Cos(t34) + n5_4 * Math.Sin(t34), 2)), (m5_4 * Math.Sin(t34) - n5_4 * Math.Cos(t34)) / (Math.Sin(f3) * Math.Sin(f4))); // ===== 计算t4的八个可能解 ===== double t41 = CalculateT4(t51, t11, t31, ax, ay, f1, f3, f5, m5_1, n5_1); double t42 = CalculateT4(t52, t11, t31, ax, ay, f1, f3, f5, m5_1, n5_1); double t43 = CalculateT4(t53, t11, t32, ax, ay, f1, f3, f5, m5_2, n5_2); double t44 = CalculateT4(t54, t11, t32, ax, ay, f1, f3, f5, m5_2, n5_2); double t45 = CalculateT4(t55, t12, t33, ax, ay, f1, f3, f5, m5_3, n5_3); double t46 = CalculateT4(t56, t12, t33, ax, ay, f1, f3, f5, m5_3, n5_3); double t47 = CalculateT4(t57, t12, t34, ax, ay, f1, f3, f5, m5_4, n5_4); double t48 = CalculateT4(t58, t12, t34, ax, ay, f1, f3, f5, m5_4, n5_4); // ===== 计算t6的八个可能解 ===== double e1 = nx * Math.Sin(t11) - ny * Math.Cos(t11); double f1_val = ox * Math.Sin(t11) - oy * Math.Cos(t11); double t61 = Math.Atan2( Math.Cos(t41) * e1 - Math.Cos(t51) * Math.Sin(t41) * f1_val, Math.Cos(t41) * f1_val + Math.Cos(t51) * Math.Sin(t41) * e1); double t62 = Math.Atan2( Math.Cos(t42) * e1 - Math.Cos(t52) * Math.Sin(t42) * f1_val, Math.Cos(t42) * f1_val + Math.Cos(t52) * Math.Sin(t42) * e1); double t63 = Math.Atan2( Math.Cos(t43) * e1 - Math.Cos(t53) * Math.Sin(t43) * f1_val, Math.Cos(t43) * f1_val + Math.Cos(t53) * Math.Sin(t43) * e1); double t64 = Math.Atan2( Math.Cos(t44) * e1 - Math.Cos(t54) * Math.Sin(t44) * f1_val, Math.Cos(t44) * f1_val + Math.Cos(t54) * Math.Sin(t44) * e1); double e2 = nx * Math.Sin(t12) - ny * Math.Cos(t12); double f2_val = ox * Math.Sin(t12) - oy * Math.Cos(t12); double t65 = Math.Atan2( Math.Cos(t45) * e2 - Math.Cos(t55) * Math.Sin(t45) * f2_val, Math.Cos(t45) * f2_val + Math.Cos(t55) * Math.Sin(t45) * e2); double t66 = Math.Atan2( Math.Cos(t46) * e2 - Math.Cos(t56) * Math.Sin(t46) * f2_val, Math.Cos(t46) * f2_val + Math.Cos(t56) * Math.Sin(t46) * e2); double t67 = Math.Atan2( Math.Cos(t47) * e2 - Math.Cos(t57) * Math.Sin(t47) * f2_val, Math.Cos(t47) * f2_val + Math.Cos(t57) * Math.Sin(t47) * e2); double t68 = Math.Atan2( Math.Cos(t48) * e2 - Math.Cos(t58) * Math.Sin(t48) * f2_val, Math.Cos(t48) * f2_val + Math.Cos(t58) * Math.Sin(t48) * e2); // ===== 组合八组解 ===== double[,] ikine_t = new double[8, 6]; // 第一组解 ikine_t[0, 0] = t11; ikine_t[0, 1] = t21; ikine_t[0, 2] = t31; ikine_t[0, 3] = t41; ikine_t[0, 4] = t51; ikine_t[0, 5] = t61; // 第二组解 ikine_t[1, 0] = t11; ikine_t[1, 1] = t21; ikine_t[1, 2] = t31; ikine_t[1, 3] = t42; ikine_t[1, 4] = t52; ikine_t[1, 5] = t62; // 第三组解 ikine_t[2, 0] = t11; ikine_t[2, 1] = t22; ikine_t[2, 2] = t32; ikine_t[2, 3] = t43; ikine_t[2, 4] = t53; ikine_t[2, 5] = t63; // 第四组解 ikine_t[3, 0] = t11; ikine_t[3, 1] = t22; ikine_t[3, 2] = t32; ikine_t[3, 3] = t44; ikine_t[3, 4] = t54; ikine_t[3, 5] = t64; // 第五组解 ikine_t[4, 0] = t12; ikine_t[4, 1] = t23; ikine_t[4, 2] = t33; ikine_t[4, 3] = t45; ikine_t[4, 4] = t55; ikine_t[4, 5] = t65; // 第六组解 ikine_t[5, 0] = t12; ikine_t[5, 1] = t23; ikine_t[5, 2] = t33; ikine_t[5, 3] = t46; ikine_t[5, 4] = t56; ikine_t[5, 5] = t66; // 第七组解 ikine_t[6, 0] = t12; ikine_t[6, 1] = t24; ikine_t[6, 2] = t34; ikine_t[6, 3] = t47; ikine_t[6, 4] = t57; ikine_t[6, 5] = t67; // 第八组解 ikine_t[7, 0] = t12; ikine_t[7, 1] = t24; ikine_t[7, 2] = t34; ikine_t[7, 3] = t48; ikine_t[7, 4] = t58; ikine_t[7, 5] = t68; return ikine_t; } private double CalculateT4(double t5, double t1, double t3, double ax, double ay, double f1, double f3, double f5, double m5, double n5) { if (Math.Abs(Math.Sin(t5)) < 1e-10) { return 0; } double numerator = (ay * Math.Cos(t1) - ax * Math.Sin(t1)) * Math.Sin(f1) * Math.Sin(f5); numerator /= (-Math.Sin(t5) * Math.Sin(f3)); double denominator = (-m5 * Math.Cos(t3) - n5 * Math.Sin(t3)) / Math.Sin(t5); return Math.Atan2(numerator, denominator); } // 辅助方法:创建齐次变换矩阵 public static double[,] CreateTransformMatrix(double x, double y, double z, double roll, double pitch, double yaw) { double[,] T = new double[4, 4]; double cr = Math.Cos(roll), sr = Math.Sin(roll); double cp = Math.Cos(pitch), sp = Math.Sin(pitch); double cy = Math.Cos(yaw), sy = Math.Sin(yaw); T[0, 0] = cy * cp; T[0, 1] = cy * sp * sr - sy * cr; T[0, 2] = cy * sp * cr + sy * sr; T[1, 0] = sy * cp; T[1, 1] = sy * sp * sr + cy * cr; T[1, 2] = sy * sp * cr - cy * sr; T[2, 0] = -sp; T[2, 1] = cp * sr; T[2, 2] = cp * cr; T[0, 3] = x; T[1, 3] = y; T[2, 3] = z; T[3, 0] = 0; T[3, 1] = 0; T[3, 2] = 0; T[3, 3] = 1; return T; } // 获取当前MDH参数 public double[,] GetMDHParameters() { return (double[,])_mdh.Clone(); } // 设置新的MDH参数 public void SetMDHParameters(double[,] newMdh) { if (newMdh == null) throw new ArgumentNullException("newMdh"); if (newMdh.GetLength(0) != 6 || newMdh.GetLength(1) != 3) throw new ArgumentException("MDH参数必须是6x3矩阵"); _mdh = (double[,])newMdh.Clone(); } } } using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading.Tasks; namespace RobotCalcForm { //Puma560六轴机械臂-正解计算 public class Puma560FK { // MDH参数表:d, a, alpha(每个关节3个参数) private readonly double[,] _mdhParams; /// <summary> /// 构造函数,初始化MDH参数 /// </summary> /// <param name="mdhParameters">6x3的MDH参数数组 [d, a, alpha]</param> public Puma560FK(double[,] mdhParameters) { if (mdhParameters == null) throw new ArgumentNullException("mdhParameters", "MDH参数不能为空"); if (mdhParameters.GetLength(0) != 6 || mdhParameters.GetLength(1) != 3) throw new ArgumentException("MDH参数必须是6x3的数组"); _mdhParams = mdhParameters; } /// <summary> /// 使用默认MDH参数的构造函数 /// </summary> public Puma560FK() { // 默认PUMA560参数 _mdhParams = new double[6, 3] { // d, a, alpha] { 0, 0, 0 }, // Joint 1 { 0, 0.180, -Math.PI/2 }, // Joint 2 { 0, 0.600, 0 }, // Joint 3 { 0.630, 0.130, -Math.PI/2 }, // Joint 4 { 0, 0, Math.PI/2 }, // Joint 5 { 0, 0, -Math.PI/2 } // Joint 6 }; } /// <summary> /// 计算机器人末端位姿(齐次变换矩阵) /// </summary> /// <param name="theta">6个关节角度数组(弧度)</param> /// <returns>4x4齐次变换矩阵</returns> public double[,] ForwardKinematics(double[] theta) { if (theta == null || theta.Length != 6) throw new ArgumentException("需要6个关节角度参数"); // 计算各关节变换矩阵 double[,] T01 = CalculateTransform(theta[0] * Math.PI / 180.0f, _mdhParams[0, 0], _mdhParams[0, 1], _mdhParams[0, 2]); double[,] T12 = CalculateTransform(theta[1] * Math.PI / 180.0f, _mdhParams[1, 0], _mdhParams[1, 1], _mdhParams[1, 2]); double[,] T23 = CalculateTransform(theta[2] * Math.PI / 180.0f, _mdhParams[2, 0], _mdhParams[2, 1], _mdhParams[2, 2]); double[,] T34 = CalculateTransform(theta[3] * Math.PI / 180.0f, _mdhParams[3, 0], _mdhParams[3, 1], _mdhParams[3, 2]); double[,] T45 = CalculateTransform(theta[4] * Math.PI / 180.0f, _mdhParams[4, 0], _mdhParams[4, 1], _mdhParams[4, 2]); double[,] T56 = CalculateTransform(theta[5] * Math.PI / 180.0f, _mdhParams[5, 0], _mdhParams[5, 1], _mdhParams[5, 2]); // 级联变换矩阵 double[,] T02 = MatrixMultiply(T01, T12); double[,] T03 = MatrixMultiply(T02, T23); double[,] T04 = MatrixMultiply(T03, T34); double[,] T05 = MatrixMultiply(T04, T45); double[,] T06 = MatrixMultiply(T05, T56); return T06; } /// <summary> /// 获取当前MDH参数 /// </summary> public double[,] GetMDHParameters() { return (double[,])_mdhParams.Clone(); } /// <summary> /// 设置单个关节的MDH参数 /// </summary> /// <param name="jointIndex">关节索引(0-5)</param> /// <param name="d">连杆偏移</param> /// <param name="a">连杆长度</param> /// <param name="alpha">连杆扭角</param> public void SetJointParameters(int jointIndex, double d, double a, double alpha) { if (jointIndex < 0 || jointIndex > 5) throw new ArgumentOutOfRangeException("jointIndex", "关节索引必须在0-5之间"); _mdhParams[jointIndex, 0] = d; _mdhParams[jointIndex, 1] = a; _mdhParams[jointIndex, 2] = alpha; } /// <summary> /// 计算单个关节的变换矩阵 /// </summary> private double[,] CalculateTransform(double theta, double d, double a, double alpha) { double cosT = Math.Cos(theta); double sinT = Math.Sin(theta); double cosA = Math.Cos(alpha); double sinA = Math.Sin(alpha); return new double[4, 4] { { cosT, -sinT, 0, a }, { cosA * sinT, cosA * cosT, -sinA, -d * sinA }, { sinA * sinT, sinA * cosT, cosA, d * cosA }, { 0, 0, 0, 1 } }; } /// <summary> /// 4x4矩阵乘法 /// </summary> private double[,] MatrixMultiply(double[,] a, double[,] b) { double[,] result = new double[4, 4]; for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { result[i, j] = 0; for (int k = 0; k < 4; k++) { result[i, j] += a[i, k] * b[k, j]; } } } return result; } /// <summary> /// 从变换矩阵中提取位置向量 /// </summary> public double[] GetPosition(double[,] matrix) { return new double[3] { matrix[0, 3], matrix[1, 3], matrix[2, 3] }; } /// <summary> /// 从变换矩阵中提取欧拉角(ZYX顺序) /// </summary> public double[] GetEulerAngles(double[,] matrix) { double sy = Math.Sqrt(matrix[0, 0] * matrix[0, 0] + matrix[1, 0] * matrix[1, 0]); bool singular = sy < 1e-6; double x, y, z; if (!singular) { x = Math.Atan2(matrix[2, 1], matrix[2, 2]); y = Math.Atan2(-matrix[2, 0], sy); z = Math.Atan2(matrix[1, 0], matrix[0, 0]); } else { x = Math.Atan2(-matrix[1, 2], matrix[1, 1]); y = Math.Atan2(-matrix[2, 0], sy); z = 0; } return new double[3] { x, y, z }; } /// <summary> /// 打印齐次变换矩阵 /// </summary> public void PrintMatrix(double[,] matrix) { Console.WriteLine("Homogeneous Transformation Matrix:"); for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { Console.Write(string.Format("{0,12:F6}", matrix[i, j])); } Console.WriteLine(); } } } } 上述计算六轴机械臂封装的正解逆解的代码,两个计算出来的结果无法对应起来,查找哪里计算有问题,修改好重新输出
09-02
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值