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