protected IEnumerator NewDate()
{
while (true)
{
yield return(new WaitForSeconds(2f));
jointGraf.SetJoint6((Mathf.Sin(Time.time * 10) + 1) * maxInputSignal / 2,
(Mathf.Sin(Time.time * 9) + 1) * maxInputSignal / 2,
(Mathf.Sin(Time.time * 8) + 1) * maxInputSignal / 2,
(Mathf.Sin(Time.time * 7) + 1) * maxInputSignal / 2,
(Mathf.Sin(Time.time * 6) + 1) * maxInputSignal / 2,
(Mathf.Sin(Time.time * 5) + 1) * maxInputSignal / 2);
}
}
private void InverseKinematic()
{
// Inp x,y,z, a,b,c
// Out J1, J2 ...
Vector3 InpVector3 = inpVector.GetVector3();
float atan = Mathf.Atan2(InpVector3.x, InpVector3.z);
Vector3 orientVector = new Vector3(l4 * Mathf.Cos(inpVector.A * Mathf.Deg2Rad) * Mathf.Sin(atan),
l4 * Mathf.Sin(inpVector.A * Mathf.Deg2Rad),
l4 * Mathf.Cos(inpVector.A * Mathf.Deg2Rad) * Mathf.Cos(atan)); // вектор подхода n
//if (InpVector3.z < 0)
// orientVector = new Vector3(-1 * orientVector.x, 1 * orientVector.y, -1 * orientVector.z);
Vector3 XYZ_1 = InpVector3 - orientVector;
float q1 = Mathf.Atan2(XYZ_1.x, XYZ_1.z); // ??? Mathf.Atan2(XYZ_1.X, XYZ_1.Z)
float q2_acos = Mathf.Acos((Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.y - l1, 2) + Mathf.Pow(XYZ_1.z, 2) + Mathf.Pow(l2, 2) - Mathf.Pow(l3, 2))
/ (2 * Mathf.Sqrt(Mathf.Abs(Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.y - l1, 2) + Mathf.Pow(XYZ_1.z, 2))) * l2));
float q2 = Mathf.PI / 2 - Mathf.Atan2(XYZ_1.y - l1, Mathf.Sqrt(Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.z, 2)));
if (!float.IsNaN(q2_acos))
{
q2 = Mathf.PI / 2 - Mathf.Atan2(XYZ_1.y - l1, Mathf.Sqrt(Mathf.Pow(XYZ_1.x, 2) + Mathf.Pow(XYZ_1.z, 2))) - q2_acos;
}
float q3_acos = (-Mathf.Pow(XYZ_1.x, 2) - Mathf.Pow(XYZ_1.y - l1, 2) - Mathf.Pow(XYZ_1.z, 2) + Mathf.Pow(l2, 2) + Mathf.Pow(l3, 2)) / (2 * l3 * l2);
float q3 = Mathf.PI - Mathf.Acos(q3_acos);
if (q3_acos > 1)
{
q3 = Mathf.PI;
}
if (q3_acos < -1 || float.IsNaN(q3))
{
q3 = 0;
}
Vector3 orientVector_1 = new Vector3(Mathf.Sin(q2 + q3) * Mathf.Sin(q1),
Mathf.Cos(q2 + q3),
Mathf.Sin(q2 + q3) * Mathf.Cos(q1)); // вектор подхода n-1
float q4 = Vector3.SignedAngle(orientVector_1, orientVector, Vector3.Cross(orientVector, orientVector_1)) * Mathf.Deg2Rad;
if (Mathf.Abs(Quaternion.LookRotation(orientVector_1, orientVector).eulerAngles.z) > 60)
{
q4 *= -1;
}
SUPER.transform.position = XYZ_1;
//testSfear1.transform.localPosition = orientVector_1 * l4;
//testSfear2.transform.localPosition = orientVector;
testSfear1.transform.localPosition = Vector3.zero;
testSfear2.transform.localPosition = Vector3.zero;
float q5 = inpVector.B * Mathf.Deg2Rad;
//Debug.Log(q1.ToString() +" "+ q2.ToString() + " " + q3.ToString());
//inpJoint = new Joint6(q1 * Mathf.Rad2Deg, q2 * Mathf.Rad2Deg, q3 * Mathf.Rad2Deg, q4 * Mathf.Rad2Deg, q5 * Mathf.Rad2Deg, inpVector.S);
inpJoint.SetJoint6(q1 * Mathf.Rad2Deg, q2 * Mathf.Rad2Deg, q3 * Mathf.Rad2Deg, q4 * Mathf.Rad2Deg, q5 * Mathf.Rad2Deg, inpVector.S);
}
