// The smaller of the two possible angles between the two vectors is returned, therefore the result will never be greater than 180 degrees or smaller than -180 degrees.
// If you imagine the from and to vectors as lines on a piece of paper, both originating from the same point, then the /axis/ vector would point up out of the paper.
// The measured angle between the two vectors would be positive in a clockwise direction and negative in an anti-clockwise direction.
public static FP SignedAngle(TSVector from, TSVector to, TSVector axis)
{
TSVector fromNorm = from.normalized, toNorm = to.normalized;
FP unsignedAngle = TSMath.Acos(TSMath.Clamp(Dot(fromNorm, toNorm), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg;
FP sign = TSMath.Sign(Dot(axis, Cross(fromNorm, toNorm)));
return(unsignedAngle * sign);
}
/// <summary>
/// is the target infront of ship.
/// </summary>
/// <param name="targetpos">the target to check with</param>
/// <returns></returns>
bool FrontTest()
{
TSVector fwd = transformts.forward;
TSVector vec = localtarget - transformts.position;
vec.Normalize();
FP ang = TSMath.Acos(TSVector.Dot(fwd, vec)) * Mathf.Rad2Deg;
if (ang <= 45.0f)
{
return(true);
}
return(false);
}
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;
}
// Returns the angle in degrees between /from/ and /to/. This is always the smallest
public static FP Angle(TSVector from, TSVector to)
{
return(TSMath.Acos(TSMath.Clamp(Dot(from.normalized, to.normalized), -FP.ONE, FP.ONE)) * TSMath.Rad2Deg);
}