private void Rotate(RobotLink part, int deg)
{
float c = Mathf.Cos(deg * Mathf.PI / 180);
float s = Mathf.Sin(deg * Mathf.PI / 180);
part.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
if (part == arm1)
{
Text text = (Text)GameObject.Find("TextArm1").GetComponent(typeof(Text));
int d = int.Parse(text.text.Substring(0, text.text.Length - 1));
d = (d + deg + 360) % 360;
d = d > 180 ? d - 360 : d;
text.text = d + "°";
}
else if (part == arm2)
{
Text text = (Text)GameObject.Find("TextArm2").GetComponent(typeof(Text));
int d = int.Parse(text.text.Substring(0, text.text.Length - 1));
d = (d + deg + 360) % 360;
d = d > 180 ? d - 360 : d;
text.text = d + "°";
}
else if (part == arm3)
{
Text text = (Text)GameObject.Find("TextArm3").GetComponent(typeof(Text));
int d = int.Parse(text.text.Substring(0, text.text.Length - 1));
d = (d + deg + 360) % 360;
d = d > 180 ? d - 360 : d;
text.text = d + "°";
}
}
private void Move()
{
float c, s;
c = Mathf.Cos(-t1 * Mathf.PI / 180);
s = Mathf.Sin(-t1 * Mathf.PI / 180);
arm1.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
c = Mathf.Cos(-t2 * Mathf.PI / 180);
s = Mathf.Sin(-t2 * Mathf.PI / 180);
arm2.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
c = Mathf.Cos(-t3 * Mathf.PI / 180);
s = Mathf.Sin(-t3 * Mathf.PI / 180);
pointer.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
float l1 = 16.65f;
float l2 = 15.25f;
float l3 = 8.33f;
Vector2 p = l1 * new Vector2(Mathf.Sin(t1 * Mathf.PI / 180), Mathf.Cos(t1 * Mathf.PI / 180));
Vector2 q = p + l2 * new Vector2(Mathf.Sin((t1 + t2) * Mathf.PI / 180), Mathf.Cos((t1 + t2) * Mathf.PI / 180));
Vector2 r = q + l3 * new Vector2(Mathf.Sin((t1 + t2 + t3) * Mathf.PI / 180), Mathf.Cos((t1 + t2 + t3) * Mathf.PI / 180));
Vector2 rp = r + 0.1f * moveDir;
Vector2 qp = q + 0.1f * moveDir;
if (rp.sqrMagnitude > Mathf.Pow(l1 + l2 + l3, 2))
{
rp = rp.normalized * (l1 + l2 + l3);
}
//rp = new Vector2 (l2 + l3, l1);
//qp = rp - new Vector2 (l3, 0);
if (qp.sqrMagnitude <= Mathf.Pow(l1 + l2, 2) && qp.sqrMagnitude >= Mathf.Pow(l1 - l2, 2))
{
float gamma = Mathf.Acos((qp.sqrMagnitude + l1 * l1 - l2 * l2) / (2 * l1 * qp.magnitude)) * 180 / Mathf.PI;
float beta = Mathf.Atan2(qp.y, qp.x) * 180 / Mathf.PI;
float t1p1, t1p2, t2p1, t2p2;
t1p1 = 90 - (beta + gamma);
t2p1 = Mathf.Acos((Mathf.Pow(qp.x, 2) + Mathf.Pow(qp.y, 2) - l1 * l1 - l2 * l2) / (2 * l1 * l2)) * 180 / Mathf.PI;
t1p2 = 90 - (beta - gamma);
t2p2 = -Mathf.Acos((Mathf.Pow(qp.x, 2) + Mathf.Pow(qp.y, 2) - l1 * l1 - l2 * l2) / (2 * l1 * l2)) * 180 / Mathf.PI;
float t1p, t2p;
if (float.IsNaN(t1p1))
{
t1p = t1p2;
}
else
{
t1p = 180 - Mathf.Abs(Mathf.Abs(t1p1 - t1) - 180) < 180 - Mathf.Abs(Mathf.Abs(t1p2 - t1) - 180) ? t1p1 : t1p2;
}
if (float.IsNaN(t2p1))
{
t2p = t2p2;
}
else
{
t2p = 180 - Mathf.Abs(Mathf.Abs(t2p1 - t2) - 180) < 180 - Mathf.Abs(Mathf.Abs(t2p2 - t2) - 180) ? t2p1 : t2p2;
}
if (!float.IsNaN(t1p) && !float.IsNaN(t2p))
{
t3 -= (t1p + t2p) - (t1 + t2);
t1 = t1p;
t2 = t2p;
}
}
else
{
//Debug.Break ();
rp -= p;
float gamma = Mathf.Acos((rp.sqrMagnitude + l2 * l2 - l3 * l3) / (2 * l2 * rp.magnitude)) * 180 / Mathf.PI;
float beta = Mathf.Atan2(rp.y, rp.x) * 180 / Mathf.PI + t1;
float t2p1, t2p2, t3p1, t3p2;
t2p1 = 90 - (beta - gamma);
t3p1 = -Mathf.Acos((Mathf.Pow(rp.x, 2) + Mathf.Pow(rp.y, 2) - l2 * l2 - l3 * l3) / (2 * l2 * l3)) * 180 / Mathf.PI;
t2p2 = 90 - (beta + gamma);
t3p2 = Mathf.Acos((Mathf.Pow(rp.x, 2) + Mathf.Pow(rp.y, 2) - l2 * l2 - l3 * l3) / (2 * l2 * l3)) * 180 / Mathf.PI;
float t2p, t3p;
if (float.IsNaN(t2p1))
{
t2p = t2p2;
}
else
{
t2p = 180 - Mathf.Abs(Mathf.Abs(t2p1 - t2) - 180) < 180 - Mathf.Abs(Mathf.Abs(t2p2 - t2) - 180) ? t2p1 : t2p2;
}
if (float.IsNaN(t3p1))
{
t3p = t3p2;
}
else
{
t3p = 180 - Mathf.Abs(Mathf.Abs(t3p1 - t3) - 180) < 180 - Mathf.Abs(Mathf.Abs(t3p2 - t3) - 180) ? t3p1 : t3p2;
}
if (!float.IsNaN(t2p) && !float.IsNaN(t3p))
{
t2 = t2p;
t3 = t3p;
}
}
c = Mathf.Cos(t1 * Mathf.PI / 180);
s = Mathf.Sin(t1 * Mathf.PI / 180);
arm1.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
c = Mathf.Cos(t2 * Mathf.PI / 180);
s = Mathf.Sin(t2 * Mathf.PI / 180);
arm2.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
c = Mathf.Cos(t3 * Mathf.PI / 180);
s = Mathf.Sin(t3 * Mathf.PI / 180);
pointer.Multiply(new float[4, 4] {
{ 1, 0, 0, 0 }, { 0, c, s, 0 }, { 0, -s, c, 0 }, { 0, 0, 0, 1 }
});
UpdateText();
}