开发平台(Platform): (Ex: VC++, GCC, Linux, ...)
VS2008
额外使用到的函数库(Library Used): (Ex: OpenGL, ...)
问题(Question)：
从我的输出中可以看到Xcmd的矩阵，我想请问的是如何将矩阵变成Xcmd'[]的矩阵
其中Xcmd'[]矩阵中的值为后项减前项
喂入的资料(Input)：
预期的正确结果(Expected Output)：
错误结果(Wrong Output)：
程式码(Code)：(请善用置底文网页, 记得排版)
#include <iostream>
using namespace std;
#include <math.h>
void main()
{
int array_x=0;
double RL=10,RS=10,RCX=5,RCY=5,Ta=0,Tc=0,Td=0,T=0,Rem=0,J1=0,J2=0,Xa=0,Xd=0,u=0,pi=3.14159,Fc1=10,Fc2=10,Fc3=10;
double Amax=250,Vmax=40,Ts=0.001;
double v1[2]={RL,0};
double v2[2]={0,RS};
double v3[2]={-RL,0};
double v4[2]={0,-RS};
double vs1=0,ve1=0,vs2=0,ve2=0,vs3=0,ve3=0,vs4=0,ve4=0,L1=0,L2=0,L3=0,L4=0,v_s=0,v_e=0,l=0,the=0;
ve1=vs2=Fc1;
ve2=vs3=Fc2;
ve3=vs4=Fc3;
L1=L3=RL;L2=L4=RS;
double T_X[4]={RL/L1,0,-RL/L3,0};
double T_Y[4]={0,RS/L2,0,-RS/L4};
double vs[4]={ vs1,vs2,vs3,vs4},ve[4]={ve1,ve2,ve3,ve4},L[4]={L1,L2,L3,L4};
double time4[4]={0},time1=0;
double Va1=0,Va2=0,Vd1=0,Vd2=0;
double Xcmd=0,Ycmd=0;
double S=0;
int aaa=0;
for (int aa=0;aa<4;aa++)
{
double a_x=0,b_y=0;
v_s=vs[aa],v_e=ve[aa],l=L[aa];
double V_limit=0;
V_limit=sqrt(0.5*(v_s*v_s+v_e*v_e+2*Amax*l));
if (V_limit<Vmax)
{
Vmax=V_limit;
}
u = (Vmax - v_s) / Amax / Ts;
if (u < 0.0) {
J1 = ceil(u);
} else {
J1 = floor(u);
}
Ta = J1 * Ts;
Vmax = v_s + Amax * Ta;
u = (Vmax - v_e) / Amax / Ts;
if (u < 0.0) {
u = ceil(u);
} else {
u = floor(u);
}
Td = u * Ts;
v_e = Vmax - Amax * Td;
Xa = (v_s + Vmax) * Ta / 2.0;
Xd = (Vmax + v_e) * Td / 2.0;
u = ((l - Xa) - Xd) / Vmax / Ts;
if (u < 0.0) {
J2 = ceil(u);
} else {
J2 = floor(u);
}
Tc = J2 * Ts;
T = (Ta + Tc) + Td;
Rem = l - ((Xa + Xd) + Vmax * Tc);
while(time4[aa]<Ta)
{
Va1=v_s+Amax*time4[aa];
time4[aa]=time4[aa]+Ts;
Va2=v_s+Amax*time4[aa];
if (aaa<J1+2)
{
S=Ts*0.5*(Va1+Va2);
}
else
{
S=Ts*0.5*(Va1+Va2)+(Rem/J2);
}
aaa++;
Xcmd=a_x+S*T_X[aa];
Ycmd=b_y+S*T_Y[aa];
a_x=Xcmd;
b_y=Ycmd;
if (aa == 0)
{
Xcmd+=(RCX-RL/2);
Ycmd+=(RCY-RS/2);
}
else if (aa == 1)
{
Xcmd+=(RCX+RL/2);
Ycmd+=(RCY-RS/2);
}
else if (aa == 2)
{
Xcmd+=(RCX+RL/2);
Ycmd+=(RCY+RS/2);
}
else
{
Xcmd+=(RCX-RL/2);
Ycmd+=(RCY+RS/2);
}
cout<<Xcmd<<",";
time1=time4[0]+time4[1]+time4[2]+time4[3];
}
while(time4[aa]<Ta+Tc)
{
time4[aa]=time4[aa]+Ts;
if (aaa<J1+2)
{
S=Ts*0.5*(Vmax+Vmax);
}
else
{
S=Ts*0.5*(Vmax+Vmax)+(Rem/J2);
}
aaa++;
Xcmd=a_x+S*T_X[aa];
Ycmd=b_y+S*T_Y[aa];
a_x=Xcmd;
b_y=Ycmd;
if (aa == 0)
{
Xcmd+=(RCX-RL/2);
Ycmd+=(RCY-RS/2);
}
else if (aa == 1)
{
Xcmd+=(RCX+RL/2);
Ycmd+=(RCY-RS/2);
}
else if (aa == 2)
{
Xcmd+=(RCX+RL/2);
Ycmd+=(RCY+RS/2);
}
else
{
Xcmd+=(RCX-RL/2);
Ycmd+=(RCY+RS/2);
}
time1=time4[0]+time4[1]+time4[2]+time4[3];
cout<<Xcmd<<",";
}
while(time4[aa]<T)
{
Vd1=Vmax-Amax*(time4[aa]-(Ta+Tc));
time4[aa]=time4[aa]+Ts;
Vd2=Vmax-Amax*(time4[aa]-(Ta+Tc));
if (aaa<J1+2)
{
S=Ts*0.5*(Vd1+Vd2);
}
else
{
S=Ts*0.5*(Vd1+Vd2)+(Rem/J2);
}
aaa++;
Xcmd=a_x+S*T_X[aa];
Ycmd=b_y+S*T_Y[aa];
a_x=Xcmd;
b_y=Ycmd;
if (aa == 0)
{
Xcmd+=(RCX-RL/2);
Ycmd+=(RCY-RS/2);
}
else if (aa == 1)
{
Xcmd+=(RCX+RL/2);
Ycmd+=(RCY-RS/2);
}
else if (aa == 2)
{
Xcmd+=(RCX+RL/2);
Ycmd+=(RCY+RS/2);
}
else
{
Xcmd+=(RCX-RL/2);
Ycmd+=(RCY+RS/2);
}
time1=time4[0]+time4[1]+time4[2]+time4[3];
cout<<Xcmd<<",";
}
//array_x=aaa;
}
}
补充说明(Supplement)：