static void DrawVO(TSVector2 circleCenter, FP radius, TSVector2 origin)
{
FP alpha = TSMath.Atan2((origin - circleCenter).y, (origin - circleCenter).x);
FP gamma = radius / (origin - circleCenter).magnitude;
FP delta = gamma <= FP.One ? TSMath.Abs(TSMath.Acos(gamma)) : 0;
// Draw.Debug.CircleXZ(FromXZ(circleCenter), radius, Color.black, alpha - delta, alpha + delta);
TSVector2 p1 = new TSVector2(TSMath.Cos(alpha - delta), TSMath.Sin(alpha - delta)) * radius;
TSVector2 p2 = new TSVector2(TSMath.Cos(alpha + delta), TSMath.Sin(alpha + delta)) * radius;
TSVector2 p1t = -new TSVector2(-p1.y, p1.x);
TSVector2 p2t = new TSVector2(-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(TSVector2 center, TSVector2 offset, FP radius, FP inverseDt, FP inverseDeltaTime)
{
// Adjusted so that a parameter weightFactor of 1 will be the default ("natural") weight factor
this.weightFactor = 1;
weightBonus = 0;
//this.radius = radius;
TSVector2 globalCenter;
circleCenter = center * inverseDt + offset;
FP tmp = Sqr(center.LengthSquared() / (radius * radius));//exp(-tmp),
// tmp = 1 +tmp+tmp*tmp/2+tmp*tmp*tmp/6; //simple use Taylor's Formula
this.weightFactor = 4 * System.Math.Exp(-tmp.AsFloat()) + 1;// 4 /tmp + 1;//exp()
// 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 TSVector2.zero as that will make the VO invalid and it will be ignored.
line1 = center.normalized * (center.magnitude - radius - FP.One / 1000) * 3 / 10 * inverseDeltaTime;
dir1 = new TSVector2(line1.y, -line1.x).normalized;
line1 += offset;
cutoffDir = TSVector2.zero;
cutoffLine = TSVector2.zero;
dir2 = TSVector2.zero;
line2 = TSVector2.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 TSVector2.zero as that will make the VO invalid and it will be ignored.
var cutoffDistance = center.magnitude - radius + FP.One / 1000;
cutoffLine = center.normalized * cutoffDistance;
cutoffDir = new TSVector2(-cutoffLine.y, cutoffLine.x).normalized;
cutoffLine += offset;
FP alpha = TSMath.Atan2(-center.y, -center.x);
FP delta = TSMath.Abs(TSMath.Acos(radius / center.magnitude));
this.radius = radius;
// Bounding Lines
// Point on circle
line1 = new TSVector2(TSMath.Cos(alpha + delta), TSMath.Sin(alpha + delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir1 = new TSVector2(line1.y, -line1.x);
// Point on circle
line2 = new TSVector2(TSMath.Cos(alpha - delta), TSMath.Sin(alpha - delta));
// Vector tangent to circle which is the correct line tangent
// Note that this vector is normalized
dir2 = new TSVector2(line2.y, -line2.x);
line1 = line1 * radius + globalCenter;
line2 = line2 * radius + globalCenter;
}
segmentStart = TSVector2.zero;
segmentEnd = TSVector2.zero;
segment = false;
}