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(); }