/** 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;
}
private static float GuassianFunc(float x) => Mathf.Exp((-(x * x)) / 2F) / Mathf.Sqrt(2F * Mathf.PI);