static void DrawVO(Vector2 circleCenter, float radius, Vector2 origin) { float alpha = Mathf.Atan2((origin - circleCenter).y, (origin - circleCenter).x); float gamma = radius / (origin - circleCenter).magnitude; float delta = gamma <= 1.0f ? Mathf.Abs(Mathf.Acos(gamma)) : 0; Draw.Debug.CircleXZ(FromXZ(circleCenter), radius, Color.black, alpha - delta, alpha + delta); Vector2 p1 = new Vector2(Mathf.Cos(alpha - delta), Mathf.Sin(alpha - delta)) * radius; Vector2 p2 = new Vector2(Mathf.Cos(alpha + delta), Mathf.Sin(alpha + delta)) * radius; Vector2 p1t = -new Vector2(-p1.y, p1.x); Vector2 p2t = new Vector2(-p2.y, p2.x); p1 += circleCenter; p2 += circleCenter; Debug.DrawRay(FromXZ(p1), FromXZ(p1t).normalized *100, Color.black); Debug.DrawRay(FromXZ(p2), FromXZ(p2t).normalized *100, Color.black); }
/** Creates a VO for avoiding another agent. * \param center The position of the other agent relative to this agent. * \param offset Offset of the velocity obstacle. For example to account for the agents' relative velocities. * \param radius Combined radius of the two agents (radius1 + radius2). * \param inverseDt 1 divided by the local avoidance time horizon (e.g avoid agents that we will hit within the next 2 seconds). * \param inverseDeltaTime 1 divided by the time step length. */ public VO(Vector2 center, Vector2 offset, float radius, float inverseDt, float inverseDeltaTime) { // Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor this.weightFactor = 1; weightBonus = 0; //this.radius = radius; Vector2 globalCenter; circleCenter = center * inverseDt + offset; this.weightFactor = 4 * Mathf.Exp(-Sqr(center.sqrMagnitude / (radius * radius))) + 1; // Collision? if (center.magnitude < radius) { colliding = true; // 0.001 is there to make sure lin1.magnitude is not so small that the normalization // below will return Vector2.zero as that will make the VO invalid and it will be ignored. line1 = center.normalized * (center.magnitude - radius - 0.001f) * 0.3f * inverseDeltaTime; dir1 = new Vector2(line1.y, -line1.x).normalized; line1 += offset; cutoffDir = Vector2.zero; cutoffLine = Vector2.zero; dir2 = Vector2.zero; line2 = Vector2.zero; this.radius = 0; } else { colliding = false; center *= inverseDt; radius *= inverseDt; globalCenter = center + offset; // 0.001 is there to make sure cutoffDistance is not so small that the normalization // below will return Vector2.zero as that will make the VO invalid and it will be ignored. var cutoffDistance = center.magnitude - radius + 0.001f; cutoffLine = center.normalized * cutoffDistance; cutoffDir = new Vector2(-cutoffLine.y, cutoffLine.x).normalized; cutoffLine += offset; float alpha = Mathf.Atan2(-center.y, -center.x); float delta = Mathf.Abs(Mathf.Acos(radius / center.magnitude)); this.radius = radius; // Bounding Lines // Point on circle line1 = new Vector2(Mathf.Cos(alpha + delta), Mathf.Sin(alpha + delta)); // Vector tangent to circle which is the correct line tangent // Note that this vector is normalized dir1 = new Vector2(line1.y, -line1.x); // Point on circle line2 = new Vector2(Mathf.Cos(alpha - delta), Mathf.Sin(alpha - delta)); // Vector tangent to circle which is the correct line tangent // Note that this vector is normalized dir2 = new Vector2(line2.y, -line2.x); line1 = line1 * radius + globalCenter; line2 = line2 * radius + globalCenter; } segmentStart = Vector2.zero; segmentEnd = Vector2.zero; segment = false; }
// // Static Methods // public static float Angle(Vector2 from, Vector2 to) { return(Mathf.Acos(Mathf.Clamp(Vector2.Dot(from.normalized, to.normalized), -1f, 1f)) * 57.29578f); }
// Returns the angle in degrees between /from/ and /to/. public static float Angle(Vector2 from, Vector2 to) { return(Mathf.Acos(Mathf.Clamp(Dot(from.normalized, to.normalized), -1F, 1F)) * Mathf.Rad2Deg); }
public static float AngleBetween(Vector3 from, Vector3 to) { return(Mathf.Acos(Mathf.Clamp(Vector3.Dot(from.normalized, to.normalized), -1f, 1f))); }
public static float Angle(Vector3RightHand from, Vector3RightHand to) { return((float)Mathf.Acos(Mathf.Clamp(Vector3RightHand.Dot(from.normalized, to.normalized), -1f, 1f)) * 57.29578f); }
/// <summary> /// <para>Returns the angle in degrees between two rotations a and b.</para> /// </summary> /// <param name="a"></param> /// <param name="b"></param> public static float Angle(Quaternion a, Quaternion b) { float f = Quaternion.Dot(a, b); return(Mathf.Acos(Mathf.Min(Mathf.Abs(f), 1f)) * 2f * 57.29578f); }