public float[] inverse_kinematics()
{
Vector3[] si_l = { this.s0, this.s1, this.s2 };
float rb = this.rb; float re = this.re; float rou = this.rou; float l = this.l;
Vector3 y = this.end_point;
CGA.CGA[] C_l = new CGA.CGA[3];
CGA.CGA[] A_l = new CGA.CGA[3];
CGA.CGA[] S_l = new CGA.CGA[3]; CGA.CGA[] T_l = new CGA.CGA[3];
CGA.CGA[] z_l = new CGA.CGA[3];
float[] joint_angle_l = new float[3];
CGA.CGA Y = up_v(y);
for (int i = 0; i < 3; i++)
{
C_l[i] = find_Ci(si_l[i]);
S_l[i] = !(up_v(y + re * si_l[i]) + (-0.5f) * rou * rou * ei);
T_l[i] = find_Ti(S_l[i], C_l[i]);
}
CGA.CGA C0 = C_l[0]; CGA.CGA C1 = C_l[1]; CGA.CGA C2 = C_l[2];
bool reachable = ((T_l[0] * T_l[0])[0] > 0f) && ((T_l[1] * T_l[1])[0] > 0f) && ((T_l[2] * T_l[2])[0] > 0f);
if (reachable)
{
for (int i = 0; i < 3; i++)
{
A_l[i] = ExtractPntfromPntPairs(T_l[i], true, true);
z_l[i] = find_zi(down(A_l[i]), si_l[i]);
joint_angle_l[i] = Mathf.Atan2((z_l[i] | (-1f * e2))[0], (z_l[i] | vector_to_pnt(si_l[i]))[0]);
}
}
this.theta_l = joint_angle_l;
return(joint_angle_l);
}
public CGA.CGA find_zi(CGA.CGA ai, Vector3 si)
{
var si_cga = vector_to_pnt(si);
var zi = ai - rb * si_cga;
return(zi);
}
public Vector3 forward_kinematics()
{
Vector3[] si_l = { this.s0, this.s1, this.s2 };
CGA.CGA[] si_l_cga = { vector_to_pnt(this.s0),
vector_to_pnt(this.s1),
vector_to_pnt(this.s2) };
float rb = this.rb; float re = this.re; float rou = this.rou; float l = this.l;
float[] joint_angle_l = this.theta_l;
CGA.CGA[] elbow_l = new CGA.CGA[3]; CGA.CGA[] S_l = new CGA.CGA[3];
CGA.CGA[] a_l = new CGA.CGA[3]; CGA.CGA[] A_l = new CGA.CGA[3];
for (int i = 0; i < 3; i++)
{
elbow_l[i] = (rb + l * Mathf.Cos(theta_l[i])) * si_l_cga[i] + l * Mathf.Sin(joint_angle_l[i]) * (-1f * e2);
a_l[i] = elbow_l[i] - re * si_l_cga[i]; A_l[i] = up_v(pnt_to_vector(a_l[i]));
elbow_l[i] = up_v(pnt_to_vector(elbow_l[i]));
CGA.CGA Centre5D = up_v(pnt_to_vector(a_l[i]));
S_l[i] = !(Centre5D + (-0.5f) * rou * rou * ei);
}
CGA.CGA T = get_grade_2(!(!S_l[0] ^ !S_l[1] ^ !S_l[2]));
var result = find_P(T);
var P = result.Item1; var P_d = result.Item2;
CGA.CGA Y = ExtractPntfromPntPairs(T, true, true); Vector3 y = pnt_to_vector(down(Y));
this.end_point = y;
return(y);
}
public CGA.CGA find_zi(CGA.CGA a, Vector3 si)
{
var si_cga = vector_to_pnt(si);
var z = a - rb * si_cga;
return(z);
}
// member function or method
public void CreateCircle5DusingThreePoints() //or should it be called "Update"?
{
CGA.CGA A = up(PointA.x, PointA.y, PointA.z);
CGA.CGA B = up(PointB.x, PointB.y, PointB.z);
CGA.CGA C = up(PointC.x, PointC.y, PointC.z);
Circle5D = A ^ B ^ C;
}
public Vector3 findEndPoint_elbi(float thetai, Vector3 si)
{
var si_cga = vector_to_pnt(si);
CGA.CGA elbi = (rb + l * Mathf.Cos(thetai)) * si_cga + l * Mathf.Sin(thetai) * (-1f * e2);
return(pnt_to_vector(elbi));
}
public void FindSphere5Dfrom4Points(Vector3 a, Vector3 b, Vector3 c, Vector3 d)
{
PointA = a; PointB = b; PointC = c; PointD = d;
var A = up(a.x, a.y, a.z); var B = up(b.x, b.y, b.z); var C = up(c.x, c.y, c.z); var D = up(d.x, d.y, d.z);
Sphere5D = A ^ B ^ C ^ D;
}
public void findSphereRadius()
{
CGA.CGA Sphere5D_nD = normalise_pnt_minus_one(!Sphere5D);
float SphereRadiusSqr = (Sphere5D_nD * Sphere5D_nD)[0];
Radius = Mathf.Sqrt(SphereRadiusSqr);
}
// Update is called once per frame
void Update()
{
//take the position of the wrist as the centre of a Sphere class object
Wrist.UpdatePoint3DfromObject();
PlatformVertexE.Point3D = Wrist.Point3D + new Vector3(-2.5f / (float)Math.Sqrt(3f), 0, -2.5f);
PlatformVertexF.Point3D = Wrist.Point3D + new Vector3(-2.5f / (float)Math.Sqrt(3f), 0, 2.5f);
PlatformVertexG.Point3D = Wrist.Point3D + new Vector3(5f / (float)Math.Sqrt(3f), 0, 0);
PlatformVertexE.UpdateGameObject();
PlatformVertexF.UpdateGameObject();
PlatformVertexG.UpdateGameObject();
LowerArmReach.Centre = PlatformVertexE.Point3D;
LowerArmReach.FindSphere5DbyCandRou(); //update the 5d expression of the sphere obj
LowerArmReach2.Centre = PlatformVertexF.Point3D;
LowerArmReach2.FindSphere5DbyCandRou();
LowerArmReach3.Centre = PlatformVertexG.Point3D;
LowerArmReach3.FindSphere5DbyCandRou();
CSIntersection = Intersection5D(PivotReach.Circle5D, LowerArmReach.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersection * CSIntersection) > 0)//two point intersection
{
IntersectPoint1.Shape5D = ExtractPntAfromPntPairs(CSIntersection);
IntersectPoint1.FindPoint3Dfrom5D();
IntersectPoint1.UpdateGameObject();
IntersectPoint2.Shape5D = ExtractPntBfromPntPairs(CSIntersection);
IntersectPoint2.FindPoint3Dfrom5D();
IntersectPoint2.UpdateGameObject();
ElbowJoint.GameObj.transform.position = IntersectPoint2.GameObj.transform.position;
}
CSIntersectionB = Intersection5D(PivotReachB.Circle5D, LowerArmReach2.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionB * CSIntersectionB) > 0)//two point intersection
{
IntersectPoint1B.Shape5D = ExtractPntAfromPntPairs(CSIntersectionB);
IntersectPoint1B.FindPoint3Dfrom5D();
IntersectPoint1B.UpdateGameObject();
IntersectPoint2B.Shape5D = ExtractPntBfromPntPairs(CSIntersectionB);
IntersectPoint2B.FindPoint3Dfrom5D();
IntersectPoint2B.UpdateGameObject();
ElbowJointB.GameObj.transform.position = IntersectPoint2B.GameObj.transform.position;
}
CSIntersectionC = Intersection5D(PivotReachC.Circle5D, LowerArmReach3.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionC * CSIntersectionC) > 0)//two point intersection
{
IntersectPoint1C.Shape5D = ExtractPntAfromPntPairs(CSIntersectionC);
IntersectPoint1C.FindPoint3Dfrom5D();
IntersectPoint1C.UpdateGameObject();
IntersectPoint2C.Shape5D = ExtractPntBfromPntPairs(CSIntersectionC);
IntersectPoint2C.FindPoint3Dfrom5D();
IntersectPoint2C.UpdateGameObject();
ElbowJointC.GameObj.transform.position = IntersectPoint2C.GameObj.transform.position;
}
Debug.Log(IntersectPoint2C.GameObj.transform.position);
}
public float find_dtheta_dalpha(CGA.CGA zi, CGA.CGA dz_dt, Vector3 si)
{
var si_cga = vector_to_pnt(si);
var denom = (zi | zi)[0];
var nume = (zi | si_cga)[0] * (dz_dt | (-1f * e2))[0] - (zi | (-1f * e2))[0] * (dz_dt | si_cga)[0];
return(nume / denom);
}
public CGA.CGA find_dE_prime_dalpha(CGA.CGA T, CGA.CGA P, CGA.CGA P_d, CGA.CGA dP_dthetai, CGA.CGA dT_dthetai, bool is_dY_dtheta = false)
{
var dP_d_dthetai = ~dP_dthetai;
CGA.CGA part1 = -1f * dP_d_dthetai * (T | ei) * P;
CGA.CGA part2 = -1f * P_d * (dT_dthetai | ei) * P;
CGA.CGA part3 = -1f * P_d * (T | ei) * dP_dthetai;
return(get_grade_1(part1 + part2 + part3));
}
public CGA.CGA find_Ci(Vector3 si)
{
var si_cga = vector_to_pnt(si);
var res = up_v(rb * si);
CGA.CGA temp_Sphere_dual = res - 0.5f * l * l * ei;
CGA.CGA Ci = temp_Sphere_dual ^ (I3 * ((-1f * e2) ^ si_cga));
return(Ci);
}
public CGA.CGA find_dY_dthetai(CGA.CGA T, CGA.CGA P, CGA.CGA P_d, CGA.CGA dP_dthetai, CGA.CGA dT_dthetai)
{
var dP_d_dthetai = ~dP_dthetai;
CGA.CGA part1 = -1f * dP_d_dthetai * (T | ei) * P;
CGA.CGA part2 = -1f * P_d * (dT_dthetai | ei) * P;
CGA.CGA part3 = -1f * P_d * (T | ei) * dP_dthetai;
return(get_grade_1(part1 + part2 + part3));
}
public void UpdateCircleandCircle5DfromCircleObj()
{
Centre = PlaneForCircle.GameObj.transform.position;
Radius = PlaneForCircle.GameObj.transform.localScale.x; // need to make sure same x,y,z values for localScale
PlaneForCircle.GameObjToShape5D();
Ic = PlaneForCircle.Shape5D;
CGA.CGA SpherebyCircle = Generate5DSpherebyCandRou(Centre, Radius);
Circle5D = Intersection5D(SpherebyCircle, Ic);
}
public CGA.CGA find_dP_dthetai(CGA.CGA T, CGA.CGA dT_dthetai)
{
float beta = Mathf.Sqrt((T | T)[0]);
CGA.CGA dbeta_dthetai = (dT_dthetai | T) * (1f / beta);
CGA.CGA dP_dthetai = (dT_dthetai | T) * (1f / beta);
dP_dthetai = 0.5f * (beta * dT_dthetai - T * dbeta_dthetai) * (1f / (beta * beta));
return(dP_dthetai);
}
public (CGA.CGA, CGA.CGA) find_P(CGA.CGA T) //T is a point pair
{
var beta = (float)Mathf.Sqrt((T * T)[0]);
var F = 1.0f / beta * T;
var P = 0.5f * (one + F);
var P_d = 0.5f * (one - F);
return(P, P_d);
}
public CGA.CGA find_dy_dthetai(CGA.CGA dY_dthetai, CGA.CGA Y)
{
float denom = (Y | ei)[0] * (Y | ei)[0];
CGA.CGA left = one - one; //zero vector
CGA.CGA right = one - one; //zero vector
CGA.CGA[] basis_array = { e1, e2, e3 };
foreach (CGA.CGA b in basis_array)
{
left += -1f * (dY_dthetai | b) * b * (Y | ei);
right += (Y | b) * b * (dY_dthetai | ei);
}
return((left + right) * (1f / denom));
}
public CGA.CGA find_de_prime_dalpha(CGA.CGA dAi_dt, CGA.CGA Ai, bool isY = false)
{
float denom = (Ai | ei)[0] * (Ai | ei)[0];
CGA.CGA left = one - one; //zero vector
CGA.CGA right = one - one; //zero vector
CGA.CGA[] basis_array = { e1, e2, e3 };
foreach (CGA.CGA b in basis_array)
{
left += -1f * (dAi_dt | b) * b * (Ai | ei);
right += (Ai | b) * b * (dAi_dt | ei);
}
return((left + right) * (1f / denom));
}
public override void UpdateGameObj()
{
//rotation from old plane normal (0,1,0) to norm
//rotation angle = the angle between (0,1,0) and (A,B,C)
float scale_of_norm = Mathf.Sqrt(norm[0] * norm[0] + norm[1] * norm[1] + norm[2] * norm[2]);
float theta = (float)Math.Acos(norm[1] / scale_of_norm);
//rotation plane= the plane spaned by (0,1,0) and (A,B,C)
var rot_plane = (e2 ^ (norm[0] * e1 + norm[1] * e2 + norm[2] * e3)).normalized();
CGA.CGA R = GenerateRotationRotor(theta, rot_plane);
//above: previous 'SetRotParamforPlane'
this.GameObj.transform.rotation = RotorToQuat(R);
// centrePoint=a;
this.GameObj.transform.position = centrePoint;
}
/// <summary>
/// CGA.Dual : res = !a
/// Poincare duality operator.
/// </summary>
public static CGA operator !(CGA a)
{
CGA res = new CGA();
res[0] = -a[31];
res[1] = -a[30];
res[2] = a[29];
res[3] = -a[28];
res[4] = a[27];
res[5] = a[26];
res[6] = a[25];
res[7] = -a[24];
res[8] = a[23];
res[9] = a[22];
res[10] = a[21];
res[11] = -a[20];
res[12] = -a[19];
res[13] = a[18];
res[14] = a[17];
res[15] = -a[16];
res[16] = a[15];
res[17] = -a[14];
res[18] = -a[13];
res[19] = a[12];
res[20] = a[11];
res[21] = -a[10];
res[22] = -a[9];
res[23] = -a[8];
res[24] = a[7];
res[25] = -a[6];
res[26] = -a[5];
res[27] = -a[4];
res[28] = a[3];
res[29] = -a[2];
res[30] = a[1];
res[31] = a[0];
return(res);
}
/// <summary>
/// CGA.muls : res = a * b
/// multivector/scalar multiplication
/// </summary>
public static CGA operator *(CGA a, float b)
{
CGA res = new CGA();
res[0] = a[0] * b;
res[1] = a[1] * b;
res[2] = a[2] * b;
res[3] = a[3] * b;
res[4] = a[4] * b;
res[5] = a[5] * b;
res[6] = a[6] * b;
res[7] = a[7] * b;
res[8] = a[8] * b;
res[9] = a[9] * b;
res[10] = a[10] * b;
res[11] = a[11] * b;
res[12] = a[12] * b;
res[13] = a[13] * b;
res[14] = a[14] * b;
res[15] = a[15] * b;
res[16] = a[16] * b;
res[17] = a[17] * b;
res[18] = a[18] * b;
res[19] = a[19] * b;
res[20] = a[20] * b;
res[21] = a[21] * b;
res[22] = a[22] * b;
res[23] = a[23] * b;
res[24] = a[24] * b;
res[25] = a[25] * b;
res[26] = a[26] * b;
res[27] = a[27] * b;
res[28] = a[28] * b;
res[29] = a[29] * b;
res[30] = a[30] * b;
res[31] = a[31] * b;
return(res);
}
/// <summary>
/// CGA.sadd : res = a + b
/// scalar/multivector addition
/// </summary>
public static CGA operator +(float a, CGA b)
{
CGA res = new CGA();
res[0] = a + b[0];
res[1] = b[1];
res[2] = b[2];
res[3] = b[3];
res[4] = b[4];
res[5] = b[5];
res[6] = b[6];
res[7] = b[7];
res[8] = b[8];
res[9] = b[9];
res[10] = b[10];
res[11] = b[11];
res[12] = b[12];
res[13] = b[13];
res[14] = b[14];
res[15] = b[15];
res[16] = b[16];
res[17] = b[17];
res[18] = b[18];
res[19] = b[19];
res[20] = b[20];
res[21] = b[21];
res[22] = b[22];
res[23] = b[23];
res[24] = b[24];
res[25] = b[25];
res[26] = b[26];
res[27] = b[27];
res[28] = b[28];
res[29] = b[29];
res[30] = b[30];
res[31] = b[31];
return(res);
}
/// <summary>
/// CGA.Sub : res = a - b
/// Multivector subtraction
/// </summary>
public static CGA operator -(CGA a, CGA b)
{
CGA res = new CGA();
res[0] = a[0] - b[0];
res[1] = a[1] - b[1];
res[2] = a[2] - b[2];
res[3] = a[3] - b[3];
res[4] = a[4] - b[4];
res[5] = a[5] - b[5];
res[6] = a[6] - b[6];
res[7] = a[7] - b[7];
res[8] = a[8] - b[8];
res[9] = a[9] - b[9];
res[10] = a[10] - b[10];
res[11] = a[11] - b[11];
res[12] = a[12] - b[12];
res[13] = a[13] - b[13];
res[14] = a[14] - b[14];
res[15] = a[15] - b[15];
res[16] = a[16] - b[16];
res[17] = a[17] - b[17];
res[18] = a[18] - b[18];
res[19] = a[19] - b[19];
res[20] = a[20] - b[20];
res[21] = a[21] - b[21];
res[22] = a[22] - b[22];
res[23] = a[23] - b[23];
res[24] = a[24] - b[24];
res[25] = a[25] - b[25];
res[26] = a[26] - b[26];
res[27] = a[27] - b[27];
res[28] = a[28] - b[28];
res[29] = a[29] - b[29];
res[30] = a[30] - b[30];
res[31] = a[31] - b[31];
return(res);
}
/// <summary>
/// CGA.Involute : res = a.Involute()
/// Main involution
/// </summary>
public CGA Involute()
{
CGA res = new CGA();
res[0] = this[0];
res[1] = -this[1];
res[2] = -this[2];
res[3] = -this[3];
res[4] = -this[4];
res[5] = -this[5];
res[6] = this[6];
res[7] = this[7];
res[8] = this[8];
res[9] = this[9];
res[10] = this[10];
res[11] = this[11];
res[12] = this[12];
res[13] = this[13];
res[14] = this[14];
res[15] = this[15];
res[16] = -this[16];
res[17] = -this[17];
res[18] = -this[18];
res[19] = -this[19];
res[20] = -this[20];
res[21] = -this[21];
res[22] = -this[22];
res[23] = -this[23];
res[24] = -this[24];
res[25] = -this[25];
res[26] = this[26];
res[27] = this[27];
res[28] = this[28];
res[29] = this[29];
res[30] = this[30];
res[31] = -this[31];
return(res);
}
/// <summary>
/// CGA.Dot : res = a | b
/// The inner product.
/// </summary>
public static CGA operator |(CGA a, CGA b)
{
CGA res = new CGA();
res[0] = b[0] * a[0] + b[1] * a[1] + b[2] * a[2] + b[3] * a[3] + b[4] * a[4] - b[5] * a[5] - b[6] * a[6] - b[7] * a[7] - b[8] * a[8] + b[9] * a[9] - b[10] * a[10] - b[11] * a[11] + b[12] * a[12] - b[13] * a[13] + b[14] * a[14] + b[15] * a[15] - b[16] * a[16] - b[17] * a[17] + b[18] * a[18] - b[19] * a[19] + b[20] * a[20] + b[21] * a[21] - b[22] * a[22] + b[23] * a[23] + b[24] * a[24] + b[25] * a[25] + b[26] * a[26] - b[27] * a[27] - b[28] * a[28] - b[29] * a[29] - b[30] * a[30] - b[31] * a[31];
res[1] = b[1] * a[0] + b[0] * a[1] - b[6] * a[2] - b[7] * a[3] - b[8] * a[4] + b[9] * a[5] + b[2] * a[6] + b[3] * a[7] + b[4] * a[8] - b[5] * a[9] - b[16] * a[10] - b[17] * a[11] + b[18] * a[12] - b[19] * a[13] + b[20] * a[14] + b[21] * a[15] - b[10] * a[16] - b[11] * a[17] + b[12] * a[18] - b[13] * a[19] + b[14] * a[20] + b[15] * a[21] + b[26] * a[22] - b[27] * a[23] - b[28] * a[24] - b[29] * a[25] - b[22] * a[26] + b[23] * a[27] + b[24] * a[28] + b[25] * a[29] - b[31] * a[30] - b[30] * a[31];
res[2] = b[2] * a[0] + b[6] * a[1] + b[0] * a[2] - b[10] * a[3] - b[11] * a[4] + b[12] * a[5] - b[1] * a[6] + b[16] * a[7] + b[17] * a[8] - b[18] * a[9] + b[3] * a[10] + b[4] * a[11] - b[5] * a[12] - b[22] * a[13] + b[23] * a[14] + b[24] * a[15] + b[7] * a[16] + b[8] * a[17] - b[9] * a[18] - b[26] * a[19] + b[27] * a[20] + b[28] * a[21] - b[13] * a[22] + b[14] * a[23] + b[15] * a[24] - b[30] * a[25] + b[19] * a[26] - b[20] * a[27] - b[21] * a[28] + b[31] * a[29] + b[25] * a[30] + b[29] * a[31];
res[3] = b[3] * a[0] + b[7] * a[1] + b[10] * a[2] + b[0] * a[3] - b[13] * a[4] + b[14] * a[5] - b[16] * a[6] - b[1] * a[7] + b[19] * a[8] - b[20] * a[9] - b[2] * a[10] + b[22] * a[11] - b[23] * a[12] + b[4] * a[13] - b[5] * a[14] + b[25] * a[15] - b[6] * a[16] + b[26] * a[17] - b[27] * a[18] + b[8] * a[19] - b[9] * a[20] + b[29] * a[21] + b[11] * a[22] - b[12] * a[23] + b[30] * a[24] + b[15] * a[25] - b[17] * a[26] + b[18] * a[27] - b[31] * a[28] - b[21] * a[29] - b[24] * a[30] - b[28] * a[31];
res[4] = b[4] * a[0] + b[8] * a[1] + b[11] * a[2] + b[13] * a[3] + b[0] * a[4] + b[15] * a[5] - b[17] * a[6] - b[19] * a[7] - b[1] * a[8] - b[21] * a[9] - b[22] * a[10] - b[2] * a[11] - b[24] * a[12] - b[3] * a[13] - b[25] * a[14] - b[5] * a[15] - b[26] * a[16] - b[6] * a[17] - b[28] * a[18] - b[7] * a[19] - b[29] * a[20] - b[9] * a[21] - b[10] * a[22] - b[30] * a[23] - b[12] * a[24] - b[14] * a[25] + b[16] * a[26] + b[31] * a[27] + b[18] * a[28] + b[20] * a[29] + b[23] * a[30] + b[27] * a[31];
res[5] = b[5] * a[0] + b[9] * a[1] + b[12] * a[2] + b[14] * a[3] + b[15] * a[4] + b[0] * a[5] - b[18] * a[6] - b[20] * a[7] - b[21] * a[8] - b[1] * a[9] - b[23] * a[10] - b[24] * a[11] - b[2] * a[12] - b[25] * a[13] - b[3] * a[14] - b[4] * a[15] - b[27] * a[16] - b[28] * a[17] - b[6] * a[18] - b[29] * a[19] - b[7] * a[20] - b[8] * a[21] - b[30] * a[22] - b[10] * a[23] - b[11] * a[24] - b[13] * a[25] + b[31] * a[26] + b[16] * a[27] + b[17] * a[28] + b[19] * a[29] + b[22] * a[30] + b[26] * a[31];
res[6] = b[6] * a[0] + b[16] * a[3] + b[17] * a[4] - b[18] * a[5] + b[0] * a[6] - b[26] * a[13] + b[27] * a[14] + b[28] * a[15] + b[3] * a[16] + b[4] * a[17] - b[5] * a[18] + b[31] * a[25] - b[13] * a[26] + b[14] * a[27] + b[15] * a[28] + b[25] * a[31];
res[7] = b[7] * a[0] - b[16] * a[2] + b[19] * a[4] - b[20] * a[5] + b[0] * a[7] + b[26] * a[11] - b[27] * a[12] + b[29] * a[15] - b[2] * a[16] + b[4] * a[19] - b[5] * a[20] - b[31] * a[24] + b[11] * a[26] - b[12] * a[27] + b[15] * a[29] - b[24] * a[31];
res[8] = b[8] * a[0] - b[17] * a[2] - b[19] * a[3] - b[21] * a[5] + b[0] * a[8] - b[26] * a[10] - b[28] * a[12] - b[29] * a[14] - b[2] * a[17] - b[3] * a[19] - b[5] * a[21] + b[31] * a[23] - b[10] * a[26] - b[12] * a[28] - b[14] * a[29] + b[23] * a[31];
res[9] = b[9] * a[0] - b[18] * a[2] - b[20] * a[3] - b[21] * a[4] + b[0] * a[9] - b[27] * a[10] - b[28] * a[11] - b[29] * a[13] - b[2] * a[18] - b[3] * a[20] - b[4] * a[21] + b[31] * a[22] - b[10] * a[27] - b[11] * a[28] - b[13] * a[29] + b[22] * a[31];
res[10] = b[10] * a[0] + b[16] * a[1] + b[22] * a[4] - b[23] * a[5] - b[26] * a[8] + b[27] * a[9] + b[0] * a[10] + b[30] * a[15] + b[1] * a[16] + b[31] * a[21] + b[4] * a[22] - b[5] * a[23] - b[8] * a[26] + b[9] * a[27] + b[15] * a[30] + b[21] * a[31];
res[11] = b[11] * a[0] + b[17] * a[1] - b[22] * a[3] - b[24] * a[5] + b[26] * a[7] + b[28] * a[9] + b[0] * a[11] - b[30] * a[14] + b[1] * a[17] - b[31] * a[20] - b[3] * a[22] - b[5] * a[24] + b[7] * a[26] + b[9] * a[28] - b[14] * a[30] - b[20] * a[31];
res[12] = b[12] * a[0] + b[18] * a[1] - b[23] * a[3] - b[24] * a[4] + b[27] * a[7] + b[28] * a[8] + b[0] * a[12] - b[30] * a[13] + b[1] * a[18] - b[31] * a[19] - b[3] * a[23] - b[4] * a[24] + b[7] * a[27] + b[8] * a[28] - b[13] * a[30] - b[19] * a[31];
res[13] = b[13] * a[0] + b[19] * a[1] + b[22] * a[2] - b[25] * a[5] - b[26] * a[6] + b[29] * a[9] + b[30] * a[12] + b[0] * a[13] + b[31] * a[18] + b[1] * a[19] + b[2] * a[22] - b[5] * a[25] - b[6] * a[26] + b[9] * a[29] + b[12] * a[30] + b[18] * a[31];
res[14] = b[14] * a[0] + b[20] * a[1] + b[23] * a[2] - b[25] * a[4] - b[27] * a[6] + b[29] * a[8] + b[30] * a[11] + b[0] * a[14] + b[31] * a[17] + b[1] * a[20] + b[2] * a[23] - b[4] * a[25] - b[6] * a[27] + b[8] * a[29] + b[11] * a[30] + b[17] * a[31];
res[15] = b[15] * a[0] + b[21] * a[1] + b[24] * a[2] + b[25] * a[3] - b[28] * a[6] - b[29] * a[7] - b[30] * a[10] + b[0] * a[15] - b[31] * a[16] + b[1] * a[21] + b[2] * a[24] + b[3] * a[25] - b[6] * a[28] - b[7] * a[29] - b[10] * a[30] - b[16] * a[31];
res[16] = b[16] * a[0] - b[26] * a[4] + b[27] * a[5] + b[31] * a[15] + b[0] * a[16] + b[4] * a[26] - b[5] * a[27] + b[15] * a[31];
res[17] = b[17] * a[0] + b[26] * a[3] + b[28] * a[5] - b[31] * a[14] + b[0] * a[17] - b[3] * a[26] - b[5] * a[28] - b[14] * a[31];
res[18] = b[18] * a[0] + b[27] * a[3] + b[28] * a[4] - b[31] * a[13] + b[0] * a[18] - b[3] * a[27] - b[4] * a[28] - b[13] * a[31];
res[19] = b[19] * a[0] - b[26] * a[2] + b[29] * a[5] + b[31] * a[12] + b[0] * a[19] + b[2] * a[26] - b[5] * a[29] + b[12] * a[31];
res[20] = b[20] * a[0] - b[27] * a[2] + b[29] * a[4] + b[31] * a[11] + b[0] * a[20] + b[2] * a[27] - b[4] * a[29] + b[11] * a[31];
res[21] = b[21] * a[0] - b[28] * a[2] - b[29] * a[3] - b[31] * a[10] + b[0] * a[21] + b[2] * a[28] + b[3] * a[29] - b[10] * a[31];
res[22] = b[22] * a[0] + b[26] * a[1] + b[30] * a[5] - b[31] * a[9] + b[0] * a[22] - b[1] * a[26] - b[5] * a[30] - b[9] * a[31];
res[23] = b[23] * a[0] + b[27] * a[1] + b[30] * a[4] - b[31] * a[8] + b[0] * a[23] - b[1] * a[27] - b[4] * a[30] - b[8] * a[31];
res[24] = b[24] * a[0] + b[28] * a[1] - b[30] * a[3] + b[31] * a[7] + b[0] * a[24] - b[1] * a[28] + b[3] * a[30] + b[7] * a[31];
res[25] = b[25] * a[0] + b[29] * a[1] + b[30] * a[2] - b[31] * a[6] + b[0] * a[25] - b[1] * a[29] - b[2] * a[30] - b[6] * a[31];
res[26] = b[26] * a[0] - b[31] * a[5] + b[0] * a[26] - b[5] * a[31];
res[27] = b[27] * a[0] - b[31] * a[4] + b[0] * a[27] - b[4] * a[31];
res[28] = b[28] * a[0] + b[31] * a[3] + b[0] * a[28] + b[3] * a[31];
res[29] = b[29] * a[0] - b[31] * a[2] + b[0] * a[29] - b[2] * a[31];
res[30] = b[30] * a[0] + b[31] * a[1] + b[0] * a[30] + b[1] * a[31];
res[31] = b[31] * a[0] + b[0] * a[31];
return(res);
}
// Update is called once per frame
void Update()
{
//take the position of the wrist as the centre of a Sphere class object
Wrist.UpdatePoint3DfromObject();
PlatformVertexD.Point3D = Wrist.Point3D + new Vector3((3f - 10 * (float)Math.Sqrt(3f)) / 6f, 0, 0) * scale_factor;
PlatformVertexE.Point3D = Wrist.Point3D + new Vector3((-3f + 10 * (float)Math.Sqrt(3f)) / 12f, 0, (10f - (float)Math.Sqrt(3f)) / 4f) * scale_factor;
PlatformVertexF.Point3D = Wrist.Point3D + new Vector3((-3f + 10 * (float)Math.Sqrt(3f)) / 12f, 0, -1f * (10f - (float)Math.Sqrt(3f)) / 4f) * scale_factor;
PlatformVertexD.UpdateGameObject();
PlatformVertexE.UpdateGameObject();
PlatformVertexF.UpdateGameObject();
LowerArmReachD.Centre = PlatformVertexD.Point3D;
LowerArmReachD.FindSphere5DbyCandRou(); //update the 5d expression of the sphere obj
LowerArmReachE.Centre = PlatformVertexE.Point3D;
LowerArmReachE.FindSphere5DbyCandRou();
LowerArmReachF.Centre = PlatformVertexF.Point3D;
LowerArmReachF.FindSphere5DbyCandRou();
CSIntersectionDA1 = Intersection5D(PivotReachA1.Circle5D, LowerArmReachD.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionDA1 * CSIntersectionDA1) > 0)//two point intersection
{
IntersectPointDA1_1.Shape5D = ExtractPntAfromPntPairs(CSIntersectionDA1);
IntersectPointDA1_1.FindPoint3Dfrom5D();
IntersectPointDA1_1.UpdateGameObject();
IntersectPointDA1_2.Shape5D = ExtractPntBfromPntPairs(CSIntersectionDA1);
IntersectPointDA1_2.FindPoint3Dfrom5D();
IntersectPointDA1_2.UpdateGameObject();
ElbowJointA1.GameObj.transform.position = IntersectPointDA1_2.GameObj.transform.position;
}
else
{
Debug.Log("Cannot reach.");
}
CSIntersectionDB2 = Intersection5D(PivotReachB2.Circle5D, LowerArmReachD.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionDB2 * CSIntersectionDB2) > 0)
{
IntersectPointDB2_1.Shape5D = ExtractPntAfromPntPairs(CSIntersectionDB2);
IntersectPointDB2_1.FindPoint3Dfrom5D();
IntersectPointDB2_1.UpdateGameObject();
IntersectPointDB2_2.Shape5D = ExtractPntBfromPntPairs(CSIntersectionDB2);
IntersectPointDB2_2.FindPoint3Dfrom5D();
IntersectPointDB2_2.UpdateGameObject();
ElbowJointB2.GameObj.transform.position = IntersectPointDB2_2.GameObj.transform.position;
}
else
{
Debug.Log("Cannot reach.");
}
CSIntersectionEB1 = Intersection5D(PivotReachB1.Circle5D, LowerArmReachE.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionEB1 * CSIntersectionEB1) > 0)
{
IntersectPointEB1_1.Shape5D = ExtractPntAfromPntPairs(CSIntersectionEB1);
IntersectPointEB1_1.FindPoint3Dfrom5D();
IntersectPointEB1_1.UpdateGameObject();
IntersectPointEB1_2.Shape5D = ExtractPntBfromPntPairs(CSIntersectionEB1);
IntersectPointEB1_2.FindPoint3Dfrom5D();
IntersectPointEB1_2.UpdateGameObject();
ElbowJointB1.GameObj.transform.position = IntersectPointEB1_2.GameObj.transform.position;
}
else
{
Debug.Log("Cannot reach.");
}
CSIntersectionEC2 = Intersection5D(PivotReachC2.Circle5D, LowerArmReachE.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionEC2 * CSIntersectionEC2) > 0)
{
IntersectPointEC2_1.Shape5D = ExtractPntAfromPntPairs(CSIntersectionEC2);
IntersectPointEC2_1.FindPoint3Dfrom5D();
IntersectPointEC2_1.UpdateGameObject();
IntersectPointEC2_2.Shape5D = ExtractPntBfromPntPairs(CSIntersectionEC2);
IntersectPointEC2_2.FindPoint3Dfrom5D();
IntersectPointEC2_2.UpdateGameObject();
ElbowJointC2.GameObj.transform.position = IntersectPointEC2_2.GameObj.transform.position;
}
else
{
Debug.Log("Cannot reach.");
}
CSIntersectionFC1 = Intersection5D(PivotReachC1.Circle5D, LowerArmReachF.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionFC1 * CSIntersectionFC1) > 0)
{
IntersectPointFC1_1.Shape5D = ExtractPntAfromPntPairs(CSIntersectionFC1);
IntersectPointFC1_1.FindPoint3Dfrom5D();
IntersectPointFC1_1.UpdateGameObject();
IntersectPointFC1_2.Shape5D = ExtractPntBfromPntPairs(CSIntersectionFC1);
IntersectPointFC1_2.FindPoint3Dfrom5D();
IntersectPointFC1_2.UpdateGameObject();
ElbowJointC1.GameObj.transform.position = IntersectPointFC1_2.GameObj.transform.position;
}
else
{
Debug.Log("Cannot reach.");
}
CSIntersectionFA2 = Intersection5D(PivotReachA2.Circle5D, LowerArmReachF.Sphere5D);
if (pnt_to_scalar_pnt(CSIntersectionFA2 * CSIntersectionFA2) > 0)
{
IntersectPointFA2_1.Shape5D = ExtractPntAfromPntPairs(CSIntersectionFA2);
IntersectPointFA2_1.FindPoint3Dfrom5D();
IntersectPointFA2_1.UpdateGameObject();
IntersectPointFA2_2.Shape5D = ExtractPntBfromPntPairs(CSIntersectionFA2);
IntersectPointFA2_2.FindPoint3Dfrom5D();
IntersectPointFA2_2.UpdateGameObject();
ElbowJointA2.GameObj.transform.position = IntersectPointFA2_2.GameObj.transform.position;
}
else
{
Debug.Log("<color=red>Error: Cannot reach");
}
ElbowJointA.GameObj.transform.position = (ElbowJointA1.GameObj.transform.position + ElbowJointA2.GameObj.transform.position) / 2;
ElbowJointB.GameObj.transform.position = (ElbowJointB1.GameObj.transform.position + ElbowJointB2.GameObj.transform.position) / 2;
ElbowJointC.GameObj.transform.position = (ElbowJointC1.GameObj.transform.position + ElbowJointC2.GameObj.transform.position) / 2;
Vector3 planeNormal = new Vector3(0, 1f, 0);
//Find the angle for the two Vectors
Angle_A = Vector3.Angle(Vector3.ProjectOnPlane(ElbowJointA.GameObj.transform.position - Pivot.Point3D, planeNormal), ElbowJointA.GameObj.transform.position - Pivot.Point3D);
//Debug.Log(Angle_A);
Angle_B = Vector3.Angle(Vector3.ProjectOnPlane(ElbowJointB.GameObj.transform.position - PivotB.Point3D, planeNormal), ElbowJointB.GameObj.transform.position - PivotB.Point3D);
//Debug.Log(Angle_B);
Angle_C = Vector3.Angle(Vector3.ProjectOnPlane(ElbowJointC.GameObj.transform.position - PivotC.Point3D, planeNormal), ElbowJointC.GameObj.transform.position - PivotC.Point3D);
//Debug.Log(Angle_C);
}
public override void GameObjToShape5D()
{
CGA.CGA norm5D = vector_to_pnt(this.GameObj.transform.up);
DistToOrigin = (vector_to_pnt(this.GameObj.transform.position) | norm5D)[0];
this.Shape5D = !(norm5D + DistToOrigin * ei);
}
public void FindSphere5DbyCandRou()
{
CGA.CGA Centre5D = up(Centre.x, Centre.y, Centre.z);
Sphere5D = !(Centre5D + (-0.5f) * Radius * Radius * ei);
}
public void GetPlaneNormal()
{
CGA.CGA n_roof = (!(this.Shape5D.normalized())) - ((!this.Shape5D.normalized()) | eo) * ei;
norm = pnt_to_vector(n_roof);
}
public void findSphereCentrefromSphere5D()
{
CGA.CGA CGAVector = Sphere5D * ei * Sphere5D;
CGA.CGA CGAVector2 = down(CGAVector);
Centre = new Vector3(CGAVector2[1], CGAVector2[2], CGAVector2[3]);
}
