把这段matlab代码改成c语言的函数。

Question

function [angle_theta1,angle_theta2,angle_theta3] = inver(px,py,pz) %11,0,0为零点

syms theta1 theta2 theta3 l a1 a2 a3
pi = 3.1415926;
a1 = 4.9;
a2 = 6.5;
a3 = 15.5;
l = 12.8;
    
theta1 = atan2(py,px);

theta2 = -2*atan((((8*(a2^2*a3*l - a2^2*a3*pz))/((a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)*(a1^2 - 2*a1*a2 - 2*a1*px + a2^2 + 2*a2*px - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)) - (4*a2*a3*((- a1^2 + 2*a1*px + a2^2 + 2*a2*a3 + a3^2 - l^2 + 2*l*pz - px^2 - pz^2)*(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2))^(1/2))/((a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)*(a1^2 - 2*a1*a2 - 2*a1*px + a2^2 + 2*a2*px - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)))*(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2))/(4*a2*a3));

theta3 = -2*atan(((- a1^2 + 2*a1*px + a2^2 + 2*a2*a3 + a3^2 - l^2 + 2*l*pz - px^2 - pz^2)*(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2))^(1/2)/(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2));






angle_theta1 = theta1/pi*180;
angle_theta2 = theta2/pi*180;
angle_theta3 = theta3/pi*180;

end

最后的代码是matlab上的运行成果，转换成c语言后算出来的角度angle_theta也需要一致，需要验证答案正确性。

把这段matlab代码改成c语言的函数。

这段matlab代码是计算逆运动学三个角度的，由于其中的^2平方形式无法用在c语言上，所以需要转换成pow的方式，比较难。其实只要将所有的^2平方格式的转化成pow格式即可。函数的解释是：输入三个参数px py pz，为尖端的三个坐标，然后利用这三个坐标计算theta123，然后弧度制转角度制，输出。

记得先验证答案正确再回答，感谢大家!

Answer 1

根据要求，编写了以下示例代码

#include<stdlib.h>
#include<stdio.h>
#include <math.h>
float pi = 3.1415926;
float  a1 = 4.9;
float a2 = 6.5;
float a3 = 15.5;
float l = 12.8;
float theta1=0;
float theta2=0;
float theta3=0;
float angle_theta1=0;
float angle_theta2=0;
float angle_theta3=0;
float x[3]={0,0,0};
void Inver(float px,float py,float pz,float *angle)
{

    
    theta1 = atan2(py,px);
 
    theta2 = -2*atan((((8*(a2*a2*a3*l - a2*a2*a3*pz))/((a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)*(a1*a1 - 2*a1*a2 - 2*a1*px + a2*a2 + 2*a2*px - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)) - (4*a2*a3*sqrt((- a1*a1 + 2*a1*px + a2*a2 + 2*a2*a3 + a3*a3 - l*l + 2*l*pz - px*px - pz*pz)*(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)))/((a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)*(a1*a1 - 2*a1*a2 - 2*a1*px + a2*a2 + 2*a2*px - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)))*(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz))/(4*a2*a3));
     
    theta3 = -2*atan(sqrt((- a1*a1 + 2*a1*px + a2*a2 + 2*a2*a3 + a3*a3 - l*l + 2*l*pz - px*px - pz*pz)*(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz))/(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz));
     
     angle[0] = theta1/pi*180;
    angle[1] = theta2/pi*180;
    angle[2] = theta3/pi*180;
 
}

int main(void)
{
    Inver(11,0,0,x);
    printf("%f\n",x[0]);
    printf("%f\n",x[1]);
    printf("%f\n",x[2]);
    
}

 

运行结果为：

与题目中的相同

结果满足要求
如果问题得到解决请点 采纳~~