void FixedUpdate()
{
dtheta0_dt = (float)Mathf.Cos(timer) * 0.05f;
dtheta1_dt = (float)Mathf.Cos(timer * 0.6f) * (-0.05f);
dtheta2_dt = (float)Mathf.Cos(timer * 1.2f) * 0.05f;
var dthetai_dt_vec = new Vector3(dtheta0_dt, dtheta1_dt, dtheta2_dt);
dthetai_dt_array = vector_to_array(dthetai_dt_vec);
//foreach(var dthetai_dt in dthetai_dt_array){Debug.Log(dthetai_dt.ToString());}
//update the framework according to dthetai_dt
theta0 += dtheta0_dt;
theta1 += dtheta1_dt;
theta2 += dtheta2_dt;
//ai are centres of the spheres
a0 = findEndPoint_ai(theta0, s0);
a1 = findEndPoint_ai(theta1, s1);
a2 = findEndPoint_ai(theta2, s2);
elb0 = findEndPoint_elbi(theta0, s0);
elb1 = findEndPoint_elbi(theta1, s1);
elb2 = findEndPoint_elbi(theta2, s2);
// For visualisation purpose
elbow0.Point3D = elb0; elbow0.UpdateGameObject();
elbow1.Point3D = elb1; elbow1.UpdateGameObject();
elbow2.Point3D = elb2; elbow2.UpdateGameObject();
Sigma0.Centre = a0; Sigma0.FindSphere5DbyCandRou();
Sigma1.Centre = a1; Sigma1.FindSphere5DbyCandRou();
Sigma2.Centre = a2; Sigma2.FindSphere5DbyCandRou();
C0 = (!Sigma1.Sphere5D) ^ (!Sigma2.Sphere5D);
C1 = (!Sigma0.Sphere5D) ^ (!Sigma2.Sphere5D);
C2 = (!Sigma1.Sphere5D) ^ (!Sigma0.Sphere5D);
T = get_grade_2(!(!Sigma0.Sphere5D ^ !Sigma1.Sphere5D ^ !Sigma2.Sphere5D));
var result = find_P(T);
P = result.Item1; P_d = result.Item2;
Y = ExtractPntBfromPntPairs(T); //extract one point from the point pair
var y_old = y;
y = pnt_to_vector(down(Y));
var dy_dt_real = y - y_old;
point_y_another.Point3D = y;
//compute the derivatives
dSigma0_dtheta0 = find_dSigmai_dthetai(a0, theta0, s0);
dSigma1_dtheta1 = find_dSigmai_dthetai(a1, theta1, s1);
dSigma2_dtheta2 = find_dSigmai_dthetai(a2, theta2, s2);
dT_dtheta0 = find_dT_dthetai(dSigma0_dtheta0, C0, !Sigma0.Sphere5D);
dT_dtheta1 = find_dT_dthetai(dSigma1_dtheta1, C1, !Sigma1.Sphere5D);
dT_dtheta2 = find_dT_dthetai(dSigma2_dtheta2, C2, !Sigma2.Sphere5D);
dP_dtheta0 = find_dP_dthetai(T, dT_dtheta0);
dP_dtheta1 = find_dP_dthetai(T, dT_dtheta1);
dP_dtheta2 = find_dP_dthetai(T, dT_dtheta2);
dY_dtheta0 = find_dY_dthetai(~T, ~P, ~P_d, ~dP_dtheta0, -1f * dT_dtheta0);
dY_dtheta1 = find_dY_dthetai(~T, ~P, ~P_d, ~dP_dtheta1, -1f * dT_dtheta1);
dY_dtheta2 = find_dY_dthetai(~T, ~P, ~P_d, ~dP_dtheta2, -1f * dT_dtheta2);
// check ExtractPntAfromPntPairs!!!!
CGA.CGA unnormedY = ExtractPntBfromPntPairs(T, false);
dy_dtheta0 = -1f * pnt_to_vector(find_dy_dthetai(dY_dtheta0, unnormedY));
dy_dtheta1 = pnt_to_vector(find_dy_dthetai(dY_dtheta1, unnormedY));
dy_dtheta2 = pnt_to_vector(find_dy_dthetai(dY_dtheta2, unnormedY));
// double[] dy_dtheta0_arr = vector_to_array(dy_dtheta0);
// double[] dy_dtheta1_arr = vector_to_array(dy_dtheta1);
// double[] dy_dtheta2_arr = vector_to_array(dy_dtheta2);
// mat[0]=dy_dtheta0_arr;mat[1]=dy_dtheta1_arr;mat[2]=dy_dtheta2_arr;
var dy_dt_1 = dy_dtheta0[0] * dthetai_dt_vec[0] + dy_dtheta1[0] * dthetai_dt_vec[1] + dy_dtheta2[0] * dthetai_dt_vec[2];
var dy_dt_2 = dy_dtheta0[1] * dthetai_dt_vec[0] + dy_dtheta1[1] * dthetai_dt_vec[1] + dy_dtheta2[1] * dthetai_dt_vec[2];
var dy_dt_3 = dy_dtheta0[2] * dthetai_dt_vec[0] + dy_dtheta1[2] * dthetai_dt_vec[1] + dy_dtheta2[2] * dthetai_dt_vec[2];
var dy_dt_cal = new Vector3(dy_dt_1, dy_dt_2, dy_dt_3);
point_y.Point3D += dy_dt_cal;
// Debug.Log("dy_dt_error");
// Debug.Log(dy_dt_cal-dy_dt_real);
point_y.UpdateGameObject();
point_y_another.UpdateGameObject();
timer += Mathf.PI * 0.05f;
}
// Update is called once per frame
void FixedUpdate()
{
//update the framework according to dy_dt
dy_dt = new Vector3((float)Mathf.Cos(timer) * 0.05f, (float)Mathf.Cos(timer * 0.6f) * (-0.1f), (float)Mathf.Cos(timer * 1.2f) * 0.05f);
// dy_dt=new Vector3(0,0,(float) Mathf.Cos(timer)*0.05f);
// dy_dt=new Vector3(0,0,0);
y += dy_dt;
x0 = y + re * s0;
x1 = y + re * s1;
x2 = y + re * s2;
PointX0.Point3D = x0; PointX0.UpdateGameObject();
PointX1.Point3D = x1; PointX1.UpdateGameObject();
PointX2.Point3D = x2; PointX2.UpdateGameObject();
Sigma0.Centre = x0; Sigma0.FindSphere5DbyCandRou();
Sigma1.Centre = x1; Sigma1.FindSphere5DbyCandRou();
Sigma2.Centre = x2; Sigma2.FindSphere5DbyCandRou();
T0 = find_Ti(Sigma0.Sphere5D, C0); T1 = find_Ti(Sigma1.Sphere5D, C1); T2 = find_Ti(Sigma2.Sphere5D, C2);
A0 = ExtractPntBfromPntPairs(T0); A1 = ExtractPntBfromPntPairs(T1); A2 = ExtractPntBfromPntPairs(T2);
a0 = pnt_to_vector(down(A0)); a1 = pnt_to_vector(down(A1)); a2 = pnt_to_vector(down(A2));
z0 = find_zi(vector_to_pnt(a0), s0); z1 = find_zi(vector_to_pnt(a1), s1); z2 = find_zi(vector_to_pnt(a2), s2);
theta0 = find_thetai(z0, s0); theta1 = find_thetai(z1, s1); theta2 = find_thetai(z2, s2);
dSigma0_dalphax = find_dSigmai_dalpha(dx0_dalphax, x0);
dSigma1_dalphax = find_dSigmai_dalpha(dx1_dalphax, x1);
dSigma2_dalphax = find_dSigmai_dalpha(dx2_dalphax, x2);
dSigma0_dalphay = find_dSigmai_dalpha(dx0_dalphay, x0);
dSigma1_dalphay = find_dSigmai_dalpha(dx1_dalphay, x1);
dSigma2_dalphay = find_dSigmai_dalpha(dx2_dalphay, x2);
dSigma0_dalphaz = find_dSigmai_dalpha(dx0_dalphaz, x0);
dSigma1_dalphaz = find_dSigmai_dalpha(dx1_dalphaz, x1);
dSigma2_dalphaz = find_dSigmai_dalpha(dx2_dalphaz, x2);
dT0_dalphax = find_dTi_dalpha(dSigma0_dalphax, C0);
dT0_dalphay = find_dTi_dalpha(dSigma0_dalphay, C0);
dT0_dalphaz = find_dTi_dalpha(dSigma0_dalphaz, C0);
dT1_dalphax = find_dTi_dalpha(dSigma1_dalphax, C1);
dT1_dalphay = find_dTi_dalpha(dSigma1_dalphay, C1);
dT1_dalphaz = find_dTi_dalpha(dSigma1_dalphaz, C1);
dT2_dalphax = find_dTi_dalpha(dSigma2_dalphax, C2);
dT2_dalphay = find_dTi_dalpha(dSigma2_dalphay, C2);
dT2_dalphaz = find_dTi_dalpha(dSigma2_dalphaz, C2);
var result = find_P(T0);
var P = result.Item1; var P_d = result.Item2;
var dP0_dalphax = find_dP_dthetai(T0, dT0_dalphax);
var dP0_dalphay = find_dP_dthetai(T0, dT0_dalphay);
var dP0_dalphaz = find_dP_dthetai(T0, dT0_dalphaz);
dA0_dalphax = find_dAi_dalpha(~T0, ~P, ~P_d, ~dP0_dalphax, -1f * dT0_dalphax);
dA0_dalphay = find_dAi_dalpha(~T0, ~P, ~P_d, ~dP0_dalphay, -1f * dT0_dalphay);
dA0_dalphaz = find_dAi_dalpha(~T0, ~P, ~P_d, ~dP0_dalphaz, -1f * dT0_dalphaz);
result = find_P(T1);
P = result.Item1; P_d = result.Item2;
var dP1_dalphax = find_dP_dthetai(T1, dT1_dalphax);
var dP1_dalphay = find_dP_dthetai(T1, dT1_dalphay);
var dP1_dalphaz = find_dP_dthetai(T1, dT1_dalphaz);
dA1_dalphax = find_dAi_dalpha(~T1, ~P, ~P_d, ~dP1_dalphax, -1f * dT1_dalphax);
dA1_dalphay = find_dAi_dalpha(~T1, ~P, ~P_d, ~dP1_dalphay, -1f * dT1_dalphay);
dA1_dalphaz = find_dAi_dalpha(~T1, ~P, ~P_d, ~dP1_dalphaz, -1f * dT1_dalphaz);
result = find_P(T2);
P = result.Item1; P_d = result.Item2;
var dP2_dalphax = find_dP_dthetai(T2, dT2_dalphax);
var dP2_dalphay = find_dP_dthetai(T2, dT2_dalphay);
var dP2_dalphaz = find_dP_dthetai(T2, dT2_dalphaz);
dA2_dalphax = find_dAi_dalpha(~T2, ~P, ~P_d, ~dP2_dalphax, -1f * dT2_dalphax);
dA2_dalphay = find_dAi_dalpha(~T2, ~P, ~P_d, ~dP2_dalphay, -1f * dT2_dalphay);
dA2_dalphaz = find_dAi_dalpha(~T2, ~P, ~P_d, ~dP2_dalphaz, -1f * dT2_dalphaz);
CGA.CGA unnormedA0 = ExtractPntBfromPntPairs(T0, false);
CGA.CGA unnormedA1 = ExtractPntBfromPntPairs(T1, false);
CGA.CGA unnormedA2 = ExtractPntBfromPntPairs(T2, false);
da0_dalphax = -1f * find_dai_dalpha(dA0_dalphax, unnormedA0);
da0_dalphay = find_dai_dalpha(dA0_dalphay, unnormedA0);
da0_dalphaz = -1f * find_dai_dalpha(dA0_dalphaz, unnormedA0);
da1_dalphax = -1f * find_dai_dalpha(dA1_dalphax, unnormedA1);
da1_dalphay = find_dai_dalpha(dA1_dalphay, unnormedA1);
da1_dalphaz = -1f * find_dai_dalpha(dA1_dalphaz, unnormedA1);
da2_dalphax = -1f * find_dai_dalpha(dA2_dalphax, unnormedA2);
da2_dalphay = find_dai_dalpha(dA2_dalphay, unnormedA2);
da2_dalphaz = -1f * find_dai_dalpha(dA2_dalphaz, unnormedA2);
dtheta0_dalphax = find_dthetai_dalpha(z0, da0_dalphax, s0);
dtheta0_dalphay = find_dthetai_dalpha(z0, da0_dalphay, s0);
dtheta0_dalphaz = find_dthetai_dalpha(z0, da0_dalphaz, s0);
dtheta1_dalphax = find_dthetai_dalpha(z1, da1_dalphax, s1);
dtheta1_dalphay = find_dthetai_dalpha(z1, da1_dalphay, s1);
dtheta1_dalphaz = find_dthetai_dalpha(z1, da1_dalphaz, s1);
dtheta2_dalphax = find_dthetai_dalpha(z2, da2_dalphax, s2);
dtheta2_dalphay = find_dthetai_dalpha(z2, da2_dalphay, s2);
dtheta2_dalphaz = find_dthetai_dalpha(z2, da2_dalphaz, s2);
dtheta0_dt = dtheta0_dalphax * dy_dt.x + dtheta0_dalphay * dy_dt.y + dtheta0_dalphaz * dy_dt.z;
dtheta1_dt = dtheta1_dalphax * dy_dt.x + dtheta1_dalphay * dy_dt.y + dtheta1_dalphaz * dy_dt.z;
dtheta2_dt = dtheta2_dalphax * dy_dt.x + dtheta2_dalphay * dy_dt.y + dtheta2_dalphaz * dy_dt.z;
theta0_ += dtheta0_dt;
theta1_ += dtheta1_dt;
theta2_ += dtheta2_dt;
Elbow0.Point3D = a0 + re * s0; Elbow0.UpdateGameObject();
Elbow1.Point3D = a1 + re * s1; Elbow1.UpdateGameObject();
Elbow2.Point3D = a2 + re * s2; Elbow2.UpdateGameObject();
Elbow0_.Point3D = find_elbow_3d(theta0_, s0); Elbow0_.UpdateGameObject();
Elbow1_.Point3D = find_elbow_3d(theta1_, s1); Elbow1_.UpdateGameObject();
Elbow2_.Point3D = find_elbow_3d(theta2_, s2); Elbow2_.UpdateGameObject();
timer += Mathf.PI * 0.05f;
}
